/*!\file:  GetGlobalDofList.cpp
 * \brief create transform matrix for different coordinate systems
 */ 
#include "./elements.h"
#include <math.h>

void CoordinateSystemTransform(IssmDouble** ptransform,Node** nodes,int numnodes,int* cs_array){

	int     i,counter;
	int     numdofs           = 0;
	IssmDouble  norm;
	IssmDouble *transform         = NULL;
	IssmDouble *values            = NULL;
	IssmDouble  coord_system[3][3];

	/*Some checks in debugging mode*/
	_assert_(numnodes && nodes);

	/*Get total number of dofs*/
	for(i=0;i<numnodes;i++){
		switch(cs_array[i]){
			case XYEnum:   numdofs+=2; break;
			case XYZPEnum: numdofs+=4; break;
			default: _error2_("Coordinate system " << EnumToStringx(cs_array[i]) << " not supported yet");
		}
	}

	/*Allocate and initialize transform matrix*/
	transform=xNew<IssmDouble>(numdofs*numdofs);
	for(i=0;i<numdofs*numdofs;i++) transform[i]=0.0;

	/*Create transform matrix for all nodes (x,y for 2d and x,y,z for 3d). It is a block matrix
	 *for 3 nodes:

	 *     | T1 0  0 |
	 * Q = | 0  T2 0 |
	 *     | 0  0  T3|
	 *
	 * Where T1 is the transform matrix for node 1. It is a simple copy of the coordinate system
	 * associated to this node*/
	counter=0;
	for(i=0;i<numnodes;i++){
		nodes[i]->GetCoordinateSystem(&coord_system[0][0]);
		switch(cs_array[i]){
			case XYEnum:
				/*We remove the z component, we need to renormalize x and y: x=[x1 x2 0] y=[-x2 x1 0]*/
				norm = sqrt( coord_system[0][0]*coord_system[0][0] + coord_system[1][0]*coord_system[1][0]); _assert_(norm>1.e-4);
				transform[(numdofs)*(counter+0) + counter+0] =   coord_system[0][0]/norm;
				transform[(numdofs)*(counter+0) + counter+1] = - coord_system[1][0]/norm;
				transform[(numdofs)*(counter+1) + counter+0] =   coord_system[1][0]/norm;
				transform[(numdofs)*(counter+1) + counter+1] =   coord_system[0][0]/norm;
				counter+=2;
				break;
			case XYZPEnum:
				/*Only the first 3 coordinates are changed (x,y,z), leave the others (P) unchanged*/
				transform[(numdofs)*(counter+0) + counter+0] = coord_system[0][0];
				transform[(numdofs)*(counter+0) + counter+1] = coord_system[0][1];
				transform[(numdofs)*(counter+0) + counter+2] = coord_system[0][2];
				transform[(numdofs)*(counter+1) + counter+0] = coord_system[1][0];
				transform[(numdofs)*(counter+1) + counter+1] = coord_system[1][1];
				transform[(numdofs)*(counter+1) + counter+2] = coord_system[1][2];
				transform[(numdofs)*(counter+2) + counter+0] = coord_system[2][0];
				transform[(numdofs)*(counter+2) + counter+1] = coord_system[2][1];
				transform[(numdofs)*(counter+2) + counter+2] = coord_system[2][2];
				transform[(numdofs)*(counter+3) + counter+3] = 1.0;
				counter+=4;
				break;
			default:
				_error2_("Coordinate system " << EnumToStringx(cs_array[i]) << " not supported yet");
		}
	}

	/*Assign output pointer*/
	*ptransform=transform;
}
