/*
 * CreateConstraintsDiagnosticHoriz.c:
 */

#include "../../../Container/Container.h"
#include "../../../toolkits/toolkits.h"
#include "../../../io/io.h"
#include "../../../EnumDefinitions/EnumDefinitions.h"
#include "../../../objects/objects.h"
#include "../../../shared/shared.h"
#include "../ModelProcessorx.h"

void	CreateConstraintsDiagnosticHoriz(Constraints** pconstraints, IoModel* iomodel){

	/*Intermediary*/
	int     i,j;
	int     count;
	double  yts;
	double  g;
	double  rho_ice;
	double  stokesreconditioning;
	bool    isstokes,ismacayealpattyn;
   bool    spcpresent=false;
	int Mx,Nx;
	int My,Ny;
	int Mz,Nz;
	double *spcvx          = NULL;
	double *spcvy          = NULL;
	double *spcvz          = NULL;
	double *nodeonmacayeal = NULL;
	double *nodeonpattyn   = NULL;
	double *nodeonstokes   = NULL;
	double *nodeonbed      = NULL;
	double *nodeonicesheet = NULL;
	double *vertices_type  = NULL;
	double *surface        = NULL;
	double *z              = NULL;
	double *timesx=NULL;
	double *timesy=NULL;
	double *timesz=NULL;
   double* values=NULL;

	/*Output*/
	Constraints *constraints      = NULL;
	SpcStatic   *spcstatic        = NULL;
	int          node1,node2;
	int          dim;
	int          numberofvertices;

	/*Fetch parameters: */
	iomodel->Constant(&dim,MeshDimensionEnum);
	iomodel->Constant(&yts,ConstantsYtsEnum);
	iomodel->Constant(&numberofvertices,MeshNumberofverticesEnum);
	iomodel->Constant(&g,ConstantsGEnum);
	iomodel->Constant(&rho_ice,MaterialsRhoIceEnum);
	iomodel->Constant(&stokesreconditioning,DiagnosticStokesreconditioningEnum);
	iomodel->Constant(&isstokes,FlowequationIsstokesEnum);
	iomodel->Constant(&ismacayealpattyn,FlowequationIsmacayealpattynEnum);

	/*Recover pointer: */
	constraints=*pconstraints;

	/*Create constraints if they do not exist yet*/
	if(!constraints) constraints = new Constraints();
	
	/*Now, is the flag macayaealpattyn on? otherwise, do nothing: */
	if (!ismacayealpattyn & !isstokes){
		*pconstraints=constraints;
		return;
	}
	
	/*Constraints: fetch data: */
	iomodel->FetchData(&spcvx,&Mx,&Nx,DiagnosticSpcvxEnum);
	iomodel->FetchData(&spcvy,&My,&Ny,DiagnosticSpcvyEnum);
	iomodel->FetchData(&spcvz,&Mz,&Nz,DiagnosticSpcvzEnum);
	iomodel->FetchData(&nodeonmacayeal,NULL,NULL,FlowequationBordermacayealEnum);
	if(dim==3)iomodel->FetchData(&nodeonpattyn,NULL,NULL,FlowequationBorderpattynEnum);
	if(dim==3)iomodel->FetchData(&nodeonstokes,NULL,NULL,FlowequationBorderstokesEnum);
	if(dim==3)iomodel->FetchData(&nodeonbed,NULL,NULL,MeshVertexonbedEnum);
	if(dim==3)iomodel->FetchData(&nodeonicesheet,NULL,NULL,MaskVertexongroundediceEnum);
	iomodel->FetchData(&vertices_type,NULL,NULL,FlowequationVertexEquationEnum);
	iomodel->FetchData(&surface,NULL,NULL,SurfaceEnum);
	iomodel->FetchData(&z,NULL,NULL,MeshZEnum);

	/*Initialize counter: */
	count=0;

	/*figure out times: */
	timesx=xNew<double>(Nx);
	for(j=0;j<Nx;j++){
		timesx[j]=spcvx[(Mx-1)*Nx+j];
	}
	/*unit conversion: */
	UnitConversion(timesx,Nx,ExtToIuEnum,TimeEnum);
	/*figure out times: */
	timesy=xNew<double>(Ny);
	for(j=0;j<Ny;j++){
		timesy[j]=spcvy[(My-1)*Ny+j];
	}
	/*unit conversion: */
	UnitConversion(timesy,Ny,ExtToIuEnum,TimeEnum);
	/*figure out times: */
	timesz=xNew<double>(Nz);
	for(j=0;j<Nz;j++){
		timesz[j]=spcvz[(Mz-1)*Nz+j];
	}
	/*unit conversion: */
	UnitConversion(timesz,Nz,ExtToIuEnum,TimeEnum);

	/*Create spcs from x,y,z, as well as the spc values on those spcs: */
	for (i=0;i<numberofvertices;i++){
		if(iomodel->my_vertices[i]){

			/*Start with adding spcs of coupling: zero at the border macayeal/pattyn for the appropriate dofs*/
			if ((int)vertices_type[i]==MacAyealPattynApproximationEnum){
				/*If grionmacayeal, spc pattyn dofs: 3 & 4*/
					if ((int)nodeonpattyn[i]){
						constraints->AddObject(new SpcStatic(iomodel->constraintcounter+count+1,iomodel->nodecounter+i+1,1,0,DiagnosticHorizAnalysisEnum)); //add count'th spc, on node i+1, setting dof 1 to vx.
						count++;
						constraints->AddObject(new SpcStatic(iomodel->constraintcounter+count+1,iomodel->nodecounter+i+1,2,0,DiagnosticHorizAnalysisEnum)); //add count'th spc, on node i+1, setting dof 1 to vx.
						count++;
						if (!xIsNan<IssmDouble>(spcvx[i])){
							constraints->AddObject(new SpcStatic(iomodel->constraintcounter+count+1,iomodel->nodecounter+i+1,3,spcvx[i]/yts,DiagnosticHorizAnalysisEnum)); //add count'th spc, on node i+1, setting dof 1 to vx.
							count++;
						}
						if (!xIsNan<IssmDouble>(spcvy[i])){
							constraints->AddObject(new SpcStatic(iomodel->constraintcounter+count+1,iomodel->nodecounter+i+1,4,spcvy[i]/yts,DiagnosticHorizAnalysisEnum)); //add count'th spc, on node i+1, setting dof 1 to vx.
							count++;
						}

					}
					else if ((int)nodeonmacayeal[i]){
						constraints->AddObject(new SpcStatic(iomodel->constraintcounter+count+1,iomodel->nodecounter+i+1,3,0,DiagnosticHorizAnalysisEnum)); //add count'th spc, on node i+1, setting dof 1 to vx.
						count++;
						constraints->AddObject(new SpcStatic(iomodel->constraintcounter+count+1,iomodel->nodecounter+i+1,4,0,DiagnosticHorizAnalysisEnum)); //add count'th spc, on node i+1, setting dof 1 to vx.
						count++;
						if (!xIsNan<IssmDouble>(spcvx[i])){
							constraints->AddObject(new SpcStatic(iomodel->constraintcounter+count+1,iomodel->nodecounter+i+1,1,spcvx[i]/yts,DiagnosticHorizAnalysisEnum)); //add count'th spc, on node i+1, setting dof 1 to vx.
							count++;
						}
						if (!xIsNan<IssmDouble>(spcvy[i])){
							constraints->AddObject(new SpcStatic(iomodel->constraintcounter+count+1,iomodel->nodecounter+i+1,2,spcvy[i]/yts,DiagnosticHorizAnalysisEnum)); //add count'th spc, on node i+1, setting dof 1 to vx.
							count++;
						}

					}
					else _error2_("if vertices_type is MacAyealPattyn, you shoud have nodeonpattyn or nodeonmacayeal");
			}
			/*Also add spcs of coupling: zero at the border pattyn/stokes for the appropriate dofs*/
			else if ((int)vertices_type[i]==PattynStokesApproximationEnum){
				/*If grion,pattyn spc stokes dofs: 3 4 & 5*/
					if ((int)nodeonpattyn[i]){
						constraints->AddObject(new SpcStatic(iomodel->constraintcounter+count+1,iomodel->nodecounter+i+1,3,0,DiagnosticHorizAnalysisEnum)); //add count'th spc, on node i+1, setting dof 1 to vx.
						count++;
						constraints->AddObject(new SpcStatic(iomodel->constraintcounter+count+1,iomodel->nodecounter+i+1,4,0,DiagnosticHorizAnalysisEnum)); //add count'th spc, on node i+1, setting dof 1 to vx.
						count++;
						constraints->AddObject(new SpcStatic(iomodel->constraintcounter+count+1,iomodel->nodecounter+i+1,5,0,DiagnosticHorizAnalysisEnum)); //add count'th spc, on node i+1, setting dof 1 to vx.
						count++;
						if (!xIsNan<IssmDouble>(spcvx[i])){
							constraints->AddObject(new SpcStatic(iomodel->constraintcounter+count+1,iomodel->nodecounter+i+1,1,spcvx[i]/yts,DiagnosticHorizAnalysisEnum)); //add count'th spc, on node i+1, setting dof 1 to vx.
							count++;
						}
						if (!xIsNan<IssmDouble>(spcvy[i])){
							constraints->AddObject(new SpcStatic(iomodel->constraintcounter+count+1,iomodel->nodecounter+i+1,2,spcvy[i]/yts,DiagnosticHorizAnalysisEnum)); //add count'th spc, on node i+1, setting dof 1 to vx.
							count++;
						}

					}
					else if ((int)nodeonstokes[i]){ //spc pattyn nodes: 1 & 2
						constraints->AddObject(new SpcStatic(iomodel->constraintcounter+count+1,iomodel->nodecounter+i+1,1,0,DiagnosticHorizAnalysisEnum)); //add count'th spc, on node i+1, setting dof 1 to vx.
						count++;
						constraints->AddObject(new SpcStatic(iomodel->constraintcounter+count+1,iomodel->nodecounter+i+1,2,0,DiagnosticHorizAnalysisEnum)); //add count'th spc, on node i+1, setting dof 1 to vx.
						count++;
						if (!xIsNan<IssmDouble>(spcvx[i])){
							constraints->AddObject(new SpcStatic(iomodel->constraintcounter+count+1,iomodel->nodecounter+i+1,3,spcvx[i]/yts,DiagnosticHorizAnalysisEnum)); //add count'th spc, on node i+1, setting dof 1 to vx.
							count++;
						}
						if (!xIsNan<IssmDouble>(spcvy[i])){
							constraints->AddObject(new SpcStatic(iomodel->constraintcounter+count+1,iomodel->nodecounter+i+1,4,spcvy[i]/yts,DiagnosticHorizAnalysisEnum)); //add count'th spc, on node i+1, setting dof 1 to vx.
							count++;
						}
						if (!xIsNan<IssmDouble>(spcvz[i])){
							constraints->AddObject(new SpcStatic(iomodel->constraintcounter+count+1,iomodel->nodecounter+i+1,5,spcvz[i]/yts,DiagnosticHorizAnalysisEnum)); //add count'th spc, on node i+1, setting dof 1 to vx.
							count++;
						}
					}
					else _error2_("if vertices_type is PattynStokes, you shoud have nodeonpattyn or nodeonstokes");
			}
			/*Also add spcs of coupling: zero at the border pattyn/stokes for the appropriate dofs*/
			else if ((int)vertices_type[i]==MacAyealStokesApproximationEnum){
				/*If grion,pattyn spc stokes dofs: 3 4 & 5*/
					if ((int)nodeonmacayeal[i]){
						constraints->AddObject(new SpcStatic(iomodel->constraintcounter+count+1,iomodel->nodecounter+i+1,3,0,DiagnosticHorizAnalysisEnum)); //add count'th spc, on node i+1, setting dof 1 to vx.
						count++;
						constraints->AddObject(new SpcStatic(iomodel->constraintcounter+count+1,iomodel->nodecounter+i+1,4,0,DiagnosticHorizAnalysisEnum)); //add count'th spc, on node i+1, setting dof 1 to vx.
						count++;
						constraints->AddObject(new SpcStatic(iomodel->constraintcounter+count+1,iomodel->nodecounter+i+1,5,0,DiagnosticHorizAnalysisEnum)); //add count'th spc, on node i+1, setting dof 1 to vx.
						count++;
						if (!xIsNan<IssmDouble>(spcvx[i])){
							constraints->AddObject(new SpcStatic(iomodel->constraintcounter+count+1,iomodel->nodecounter+i+1,1,spcvx[i]/yts,DiagnosticHorizAnalysisEnum)); //add count'th spc, on node i+1, setting dof 1 to vx.
							count++;
						}
						if (!xIsNan<IssmDouble>(spcvy[i])){
							constraints->AddObject(new SpcStatic(iomodel->constraintcounter+count+1,iomodel->nodecounter+i+1,2,spcvy[i]/yts,DiagnosticHorizAnalysisEnum)); //add count'th spc, on node i+1, setting dof 1 to vx.
							count++;
						}

					}
					else if ((int)nodeonstokes[i]){ //spc macayeal nodes: 1 & 2
						constraints->AddObject(new SpcStatic(iomodel->constraintcounter+count+1,iomodel->nodecounter+i+1,1,0,DiagnosticHorizAnalysisEnum)); //add count'th spc, on node i+1, setting dof 1 to vx.
						count++;
						constraints->AddObject(new SpcStatic(iomodel->constraintcounter+count+1,iomodel->nodecounter+i+1,2,0,DiagnosticHorizAnalysisEnum)); //add count'th spc, on node i+1, setting dof 1 to vx.
						count++;
						if (!xIsNan<IssmDouble>(spcvx[i])){
							constraints->AddObject(new SpcStatic(iomodel->constraintcounter+count+1,iomodel->nodecounter+i+1,3,spcvx[i]/yts,DiagnosticHorizAnalysisEnum)); //add count'th spc, on node i+1, setting dof 1 to vx.
							count++;
						}
						if (!xIsNan<IssmDouble>(spcvy[i])){
							constraints->AddObject(new SpcStatic(iomodel->constraintcounter+count+1,iomodel->nodecounter+i+1,4,spcvy[i]/yts,DiagnosticHorizAnalysisEnum)); //add count'th spc, on node i+1, setting dof 1 to vx.
							count++;
						}
						if (!xIsNan<IssmDouble>(spcvz[i])){
							constraints->AddObject(new SpcStatic(iomodel->constraintcounter+count+1,iomodel->nodecounter+i+1,5,spcvz[i]/yts,DiagnosticHorizAnalysisEnum)); //add count'th spc, on node i+1, setting dof 1 to vx.
							count++;
						}
					}
					else _error2_("if vertices_type is MacAyealStokes, you shoud have nodeonmacayeal or nodeonstokes");
			}
			/*Now add the regular spcs*/
			else{
				if (Mx==numberofvertices && !xIsNan<IssmDouble>(spcvx[i])){
					constraints->AddObject(new SpcStatic(iomodel->constraintcounter+count+1,iomodel->nodecounter+i+1,1,spcvx[i]/yts,DiagnosticHorizAnalysisEnum)); //add count'th spc, on node i+1, setting dof 1 to vx.
					count++;
				}
				else if (Mx==numberofvertices+1) {
					/*figure out times and values: */
					values=xNew<double>(Nx);
					spcpresent=false;
					for(j=0;j<Nx;j++){
						values[j]=spcvx[i*Nx+j]/yts;
						if(!xIsNan<IssmDouble>(values[j]))spcpresent=true; //NaN means no spc by default
					}

					if(spcpresent){
						constraints->AddObject(new SpcTransient(iomodel->constraintcounter+count+1,iomodel->nodecounter+i+1,1,Nx,timesx,values,DiagnosticHorizAnalysisEnum));
						count++;
					}
					xDelete<double>(values);
				}
				else if (vertices_type[i]==HutterApproximationEnum){
					constraints->AddObject(new SpcDynamic(iomodel->constraintcounter+count+1,iomodel->nodecounter+i+1,1,DiagnosticHorizAnalysisEnum));
					count++;
				}

				if (My==numberofvertices && !xIsNan<IssmDouble>(spcvy[i])){
					constraints->AddObject(new SpcStatic(iomodel->constraintcounter+count+1,iomodel->nodecounter+i+1,2,spcvy[i]/yts,DiagnosticHorizAnalysisEnum)); //add count'th spc, on node i+1, setting dof 1 to vy.
					count++;
				}
				else if (My==numberofvertices+1){
					/*figure out times and values: */
					values=xNew<double>(Ny);
					spcpresent=false;
					for(j=0;j<Ny;j++){
						values[j]=spcvy[i*Ny+j]/yts;
						if(!xIsNan<IssmDouble>(values[j]))spcpresent=true; //NaN means no spc by default
					}
					if(spcpresent){
						constraints->AddObject(new SpcTransient(iomodel->constraintcounter+count+1,iomodel->nodecounter+i+1,2,Ny,timesy,values,DiagnosticHorizAnalysisEnum));
						count++;
					}
					xDelete<double>(values);
				}
				else if (vertices_type[i]==HutterApproximationEnum){
					constraints->AddObject(new SpcDynamic(iomodel->constraintcounter+count+1,iomodel->nodecounter+i+1,2,DiagnosticHorizAnalysisEnum));
					count++;
				}

				if ((int)vertices_type[i]==StokesApproximationEnum ||  ((int)vertices_type[i]==NoneApproximationEnum)){
					if (Mz==numberofvertices && !xIsNan<IssmDouble>(spcvz[i])){
						constraints->AddObject(new SpcStatic(iomodel->constraintcounter+count+1,iomodel->nodecounter+i+1,3,spcvz[i]/yts,DiagnosticHorizAnalysisEnum)); //add count'th spc, on node i+1, setting dof 2 to vy
						count++;
					}
					else if (Mz==numberofvertices+1){
						/*figure out times and values: */
						values=xNew<double>(Nz);
						spcpresent=false;
						for(j=0;j<Nz;j++){
							values[j]=spcvz[i*Nz+j]/yts;
							if(!xIsNan<IssmDouble>(values[j]))spcpresent=true; //NaN means no spc by default
						}
						if(spcpresent){
							constraints->AddObject(new SpcTransient(iomodel->constraintcounter+count+1,iomodel->nodecounter+i+1,3,Nz,timesz,values,DiagnosticHorizAnalysisEnum));
							count++;
						}
						xDelete<double>(values);
					}

				}
				if ((int)vertices_type[i]==NoneApproximationEnum){
					constraints->AddObject(new SpcStatic(iomodel->constraintcounter+count+1,iomodel->nodecounter+i+1,4,g*rho_ice*(surface[i]-z[i])/stokesreconditioning,DiagnosticHorizAnalysisEnum)); //add count'th spc, on node i+1, setting dof 2 to vy
					count++;
				}
			}

			/*Constraint at the bedrock interface (v.n = vz = 0) (Coordinates will be updated according to the bed slope)*/
			if (dim==3) if(nodeonbed[i] && nodeonicesheet[i] && nodeonstokes[i]){
				 switch((int)vertices_type[i]){
					case MacAyealStokesApproximationEnum:
						constraints->AddObject(new SpcStatic(iomodel->constraintcounter+count+1,iomodel->nodecounter+i+1,5,0.,DiagnosticHorizAnalysisEnum));
						count++;
						break;
					case PattynStokesApproximationEnum:
						constraints->AddObject(new SpcStatic(iomodel->constraintcounter+count+1,iomodel->nodecounter+i+1,5,0.,DiagnosticHorizAnalysisEnum));
						count++;
						break;
					case StokesApproximationEnum:
						constraints->AddObject(new SpcStatic(iomodel->constraintcounter+count+1,iomodel->nodecounter+i+1,3,0.,DiagnosticHorizAnalysisEnum));
						count++;
						break;
					default: _error2_("Vertex approximation " << EnumToStringx((int)vertices_type[i]) << " not supported");
				}
			}
		}
	}
	  
	/*Free data: */
	xDelete<double>(spcvx);
	xDelete<double>(spcvy);
	xDelete<double>(spcvz);
	xDelete<double>(nodeonmacayeal);
	xDelete<double>(nodeonpattyn);
	xDelete<double>(nodeonstokes);
	xDelete<double>(nodeonicesheet);
	xDelete<double>(nodeonbed);
	xDelete<double>(vertices_type);
	xDelete<double>(surface);
	xDelete<double>(z);

	/*Free resources:*/
	xDelete<double>(timesx);
	xDelete<double>(timesy);
	xDelete<double>(timesz);
	xDelete<double>(values);

	/*Assign output pointer: */
	*pconstraints=constraints;
}
