/*
 * CreateConstraintsDiagnosticHoriz.c:
 */

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

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

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

	/*Output*/
	Constraints *constraints      = NULL;
	SpcStatic   *spcstatic        = NULL;
	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(&isl1l2,FlowequationIsl1l2Enum);
	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 & !isl1l2){
		*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<IssmDouble>(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<IssmDouble>(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<IssmDouble>(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 (reCast<int,IssmDouble>(vertices_type[i]==MacAyealPattynApproximationEnum)){
				/*If grionmacayeal, spc pattyn dofs: 3 & 4*/
					if (reCast<int,IssmDouble>(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 (reCast<int,IssmDouble>(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 _error_("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 (reCast<int,IssmDouble>(vertices_type[i])==PattynStokesApproximationEnum){
				/*If grion,pattyn spc stokes dofs: 3 4 & 5*/
					if (reCast<int,IssmDouble>(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 (reCast<int,IssmDouble>(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 _error_("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 (reCast<int,IssmDouble>(vertices_type[i])==MacAyealStokesApproximationEnum){
				/*If grion,pattyn spc stokes dofs: 3 4 & 5*/
					if (reCast<int,IssmDouble>(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 (reCast<int,IssmDouble>(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 _error_("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<IssmDouble>(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<IssmDouble>(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<IssmDouble>(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<IssmDouble>(values);
				}
				else if (vertices_type[i]==HutterApproximationEnum){
					constraints->AddObject(new SpcDynamic(iomodel->constraintcounter+count+1,iomodel->nodecounter+i+1,2,DiagnosticHorizAnalysisEnum));
					count++;
				}

				if (reCast<int,IssmDouble>(vertices_type[i])==StokesApproximationEnum ||  (reCast<int,IssmDouble>(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<IssmDouble>(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<IssmDouble>(values);
					}

				}
				if (reCast<int,IssmDouble>(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(reCast<int,IssmDouble>(nodeonbed[i]) && reCast<int,IssmDouble>(nodeonicesheet[i]) && reCast<int,IssmDouble>(nodeonstokes[i])){
				 switch(reCast<int,IssmDouble>(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: _error_("Vertex approximation " << EnumToStringx(reCast<int,IssmDouble>(vertices_type[i])) << " not supported");
				}
			}
		}
	}

	/*Free data: */
	iomodel->DeleteData(spcvx,DiagnosticSpcvxEnum);
	iomodel->DeleteData(spcvy,DiagnosticSpcvyEnum);
	iomodel->DeleteData(spcvz,DiagnosticSpcvzEnum);
	iomodel->DeleteData(nodeonmacayeal,FlowequationBordermacayealEnum);
	if(dim==3)iomodel->DeleteData(nodeonpattyn,FlowequationBorderpattynEnum);
	if(dim==3)iomodel->DeleteData(nodeonstokes,FlowequationBorderstokesEnum);
	if(dim==3)iomodel->DeleteData(nodeonbed,MeshVertexonbedEnum);
	if(dim==3)iomodel->DeleteData(nodeonicesheet,MaskVertexongroundediceEnum);
	iomodel->DeleteData(vertices_type,FlowequationVertexEquationEnum);
	iomodel->DeleteData(surface,SurfaceEnum);
	iomodel->DeleteData(z,MeshZEnum);

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

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