#include "./StressbalanceAnalysis.h"
#include "../toolkits/toolkits.h"
#include "../classes/classes.h"
#include "../shared/shared.h"
#include "../modules/modules.h"
#include "../solutionsequences/solutionsequences.h"
#include "../cores/cores.h"

//#define FSANALYTICAL 10

/*Model processing*/
int  StressbalanceAnalysis::DofsPerNode(int** pdoftype,int meshtype,int approximation){/*{{{*/

	/*output*/
	int *doftype = NULL;
	int  numdofs;

	switch(approximation){
		case SSAApproximationEnum:
			 switch(meshtype){
				 case Mesh3DEnum:           numdofs=2; break;
				 case Mesh3DtetrasEnum:     numdofs=2; break;
				 case Mesh2DhorizontalEnum: numdofs=2; break;
				 case Mesh2DverticalEnum:   numdofs=1; break;
				 default: _error_("mesh type not supported yet");
			 }
			 break;
		case L1L2ApproximationEnum: numdofs =2; break;
		case HOApproximationEnum:   
			 switch(meshtype){
				 case Mesh3DEnum:         numdofs=2; break;
				 case Mesh3DtetrasEnum:   numdofs=2; break;
				 case Mesh2DverticalEnum: numdofs=1; break;
				 default: _error_("mesh type not supported yet");
			 }
			 break;
		case SIAApproximationEnum:  numdofs =2; break;
		case FSvelocityEnum:
			 switch(meshtype){
				 case Mesh3DEnum:         numdofs=3; break;
				 case Mesh3DtetrasEnum:   numdofs=3; break;
				 case Mesh2DverticalEnum: numdofs=2; break;
				 default: _error_("mesh type not supported yet");
			}
			break;
		case FSpressureEnum: numdofs=1; break;
		case NoneApproximationEnum:
			 switch(meshtype){
				 case Mesh3DEnum:         numdofs=4; break;
				 case Mesh3DtetrasEnum:   numdofs=4; break;
				 case Mesh2DverticalEnum: numdofs=3; break;
				 default: _error_("mesh type not supported yet");
			}
			break;
		case SSAHOApproximationEnum:
			numdofs=4;
			doftype=xNew<int>(numdofs);
			doftype[0]=SSAApproximationEnum;
			doftype[1]=SSAApproximationEnum;
			doftype[2]=HOApproximationEnum;
			doftype[3]=HOApproximationEnum;
			break;
		case HOFSApproximationEnum:
			numdofs=5;
			doftype=xNew<int>(numdofs);
			doftype[0]=HOApproximationEnum;
			doftype[1]=HOApproximationEnum;
			doftype[2]=FSvelocityEnum;
			doftype[3]=FSvelocityEnum;
			doftype[4]=FSvelocityEnum;
			break;
		case SSAFSApproximationEnum:
			numdofs=5;
			doftype=xNew<int>(numdofs);
			doftype[0]=SSAApproximationEnum;
			doftype[1]=SSAApproximationEnum;
			doftype[2]=FSvelocityEnum;
			doftype[3]=FSvelocityEnum;
			doftype[4]=FSvelocityEnum;
			break;
		default:
			_error_("Approximation " << EnumToStringx(approximation) << " not implemented yet");
	}

	/*Assign output pointer and return*/
	*pdoftype = doftype;
	return numdofs;
}/*}}}*/
void StressbalanceAnalysis::UpdateParameters(Parameters* parameters,IoModel* iomodel,int solution_enum,int analysis_enum){/*{{{*/

	/*Intermediaries*/
	int         numoutputs;
	char**      requestedoutputs = NULL;

	parameters->AddObject(iomodel->CopyConstantObject(FlowequationIsSIAEnum));
	parameters->AddObject(iomodel->CopyConstantObject(FlowequationIsSSAEnum));
	parameters->AddObject(iomodel->CopyConstantObject(FlowequationIsL1L2Enum));
	parameters->AddObject(iomodel->CopyConstantObject(FlowequationIsHOEnum));
	parameters->AddObject(iomodel->CopyConstantObject(FlowequationIsFSEnum));
	parameters->AddObject(iomodel->CopyConstantObject(FlowequationFeFSEnum));
	parameters->AddObject(iomodel->CopyConstantObject(StressbalanceRestolEnum));
	parameters->AddObject(iomodel->CopyConstantObject(StressbalanceReltolEnum));
	parameters->AddObject(iomodel->CopyConstantObject(StressbalanceAbstolEnum));
	parameters->AddObject(iomodel->CopyConstantObject(StressbalanceIsnewtonEnum));
	parameters->AddObject(iomodel->CopyConstantObject(StressbalanceMaxiterEnum));
	parameters->AddObject(iomodel->CopyConstantObject(StressbalancePenaltyFactorEnum));
	parameters->AddObject(iomodel->CopyConstantObject(StressbalanceRiftPenaltyThresholdEnum));
	parameters->AddObject(iomodel->CopyConstantObject(StressbalanceFSreconditioningEnum));
	parameters->AddObject(iomodel->CopyConstantObject(StressbalanceShelfDampeningEnum));
	parameters->AddObject(iomodel->CopyConstantObject(StressbalanceViscosityOvershootEnum));

	/*Requested outputs*/
	iomodel->FetchData(&requestedoutputs,&numoutputs,StressbalanceRequestedOutputsEnum);
	parameters->AddObject(new IntParam(StressbalanceNumRequestedOutputsEnum,numoutputs));
	if(numoutputs)parameters->AddObject(new StringArrayParam(StressbalanceRequestedOutputsEnum,requestedoutputs,numoutputs));
	iomodel->DeleteData(&requestedoutputs,numoutputs,StressbalanceRequestedOutputsEnum);

}/*}}}*/
void StressbalanceAnalysis::UpdateElements(Elements* elements,IoModel* iomodel,int analysis_counter,int analysis_type){/*{{{*/

	/*Intermediaries*/
	int    materials_type,finiteelement;
	int    approximation;
	int*   finiteelement_list=NULL;
	bool   isSSA,isL1L2,isHO,isFS,iscoupling;
	bool   control_analysis;
	bool   dakota_analysis;
	bool   islevelset;

	/*Fetch constants needed: */
	iomodel->Constant(&isSSA,FlowequationIsSSAEnum);
	iomodel->Constant(&isL1L2,FlowequationIsL1L2Enum);
	iomodel->Constant(&isHO,FlowequationIsHOEnum);
	iomodel->Constant(&isFS,FlowequationIsFSEnum);
	iomodel->Constant(&control_analysis,InversionIscontrolEnum);
	iomodel->Constant(&dakota_analysis,QmuIsdakotaEnum);
	iomodel->Constant(&materials_type,MaterialsEnum);
	iomodel->Constant(&islevelset,TransientIslevelsetEnum);

	/*return if no processing required*/
	if(!isSSA & !isL1L2 & !isHO & !isFS) return;

	/*Fetch data needed and allocate vectors: */
	iomodel->FetchData(1,FlowequationElementEquationEnum);
	finiteelement_list=xNewZeroInit<int>(iomodel->numberofelements);

	/*Do we have coupling*/
	if( (isSSA?1.:0.) + (isL1L2?1.:0.) + (isHO?1.:0.) + (isFS?1.:0.) >1.)
	 iscoupling = true;
	else
	 iscoupling = false;

	/*Get finite element type*/
	if(!iscoupling){
		if(isSSA)       iomodel->Constant(&finiteelement,FlowequationFeSSAEnum);
		else if(isL1L2) finiteelement = P1Enum;
		else if(isHO)   iomodel->Constant(&finiteelement,FlowequationFeHOEnum);
		else if(isFS)   iomodel->Constant(&finiteelement,FlowequationFeFSEnum);
		for(int i=0;i<iomodel->numberofelements;i++){
			finiteelement_list[i]=finiteelement;
		}
	}
	else{
		if(isFS){
			for(int i=0;i<iomodel->numberofelements;i++){
				approximation=reCast<int>(iomodel->Data(FlowequationElementEquationEnum)[i]);
				if(approximation==FSApproximationEnum || approximation==HOFSApproximationEnum || approximation==SSAFSApproximationEnum){
					finiteelement_list[i]=MINIcondensedEnum;
				}
				else{
					finiteelement_list[i]=P1Enum;
				}
			}
		}
		else{
			finiteelement = P1Enum;
			for(int i=0;i<iomodel->numberofelements;i++){
				finiteelement_list[i]=finiteelement;
			}
		}
	}

	/*Update elements: */
	int counter=0;
	for(int i=0;i<iomodel->numberofelements;i++){
		if(iomodel->my_elements[i]){
			Element* element=(Element*)elements->GetObjectByOffset(counter);
			element->Update(i,iomodel,analysis_counter,analysis_type,finiteelement_list[i]);
			counter++;
		}
	}

	/*Create inputs: */
	iomodel->FetchDataToInput(elements,ThicknessEnum);
	iomodel->FetchDataToInput(elements,SurfaceEnum);
	iomodel->FetchDataToInput(elements,BaseEnum);
	iomodel->FetchDataToInput(elements,FrictionCoefficientEnum);
	iomodel->FetchDataToInput(elements,FrictionPEnum);
	iomodel->FetchDataToInput(elements,FrictionQEnum);
	iomodel->FetchDataToInput(elements,MaskIceLevelsetEnum);
	iomodel->FetchDataToInput(elements,MaskGroundediceLevelsetEnum);
	iomodel->FetchDataToInput(elements,MaterialsRheologyBEnum);
	iomodel->FetchDataToInput(elements,MaterialsRheologyNEnum);
	iomodel->FetchDataToInput(elements,VxEnum,0.);
	if(dakota_analysis)elements->InputDuplicate(VxEnum,QmuVxEnum);
	iomodel->FetchDataToInput(elements,VyEnum,0.);
	if(dakota_analysis)elements->InputDuplicate(VyEnum,QmuVyEnum);
	iomodel->FetchDataToInput(elements,LoadingforceXEnum);
	iomodel->FetchDataToInput(elements,LoadingforceYEnum);
	iomodel->FetchDataToInput(elements,DamageDEnum);

	if(iomodel->meshtype==Mesh3DEnum){
		iomodel->FetchDataToInput(elements,MeshElementonbedEnum);
		iomodel->FetchDataToInput(elements,MeshElementonsurfaceEnum);
		iomodel->FetchDataToInput(elements,BasalforcingsMeltingRateEnum);
		iomodel->FetchDataToInput(elements,FlowequationBorderFSEnum);
		iomodel->FetchDataToInput(elements,LoadingforceZEnum);
		iomodel->FetchDataToInput(elements,VzEnum,0.);
		if(dakota_analysis)elements->InputDuplicate(VzEnum,QmuVzEnum);
	}
	if(iomodel->meshtype==Mesh3DtetrasEnum){
		iomodel->FetchDataToInput(elements,MeshVertexonbedEnum);
		iomodel->FetchDataToInput(elements,MeshVertexonsurfaceEnum);
		iomodel->FetchDataToInput(elements,BasalforcingsMeltingRateEnum);
		iomodel->FetchDataToInput(elements,LoadingforceZEnum);
		iomodel->FetchDataToInput(elements,VzEnum,0.);
		if(dakota_analysis)elements->InputDuplicate(VzEnum,QmuVzEnum);
	}
	if(iomodel->meshtype==Mesh2DverticalEnum){
		iomodel->FetchDataToInput(elements,MeshVertexonsurfaceEnum);
	}
	if(isFS){
		iomodel->FetchDataToInput(elements,MeshVertexonbedEnum);
		iomodel->FetchDataToInput(elements,PressureEnum,0.);
		if(dakota_analysis)elements->InputDuplicate(PressureEnum,QmuPressureEnum);
	}
	if(islevelset){
		iomodel->FetchDataToInput(elements,IceMaskNodeActivationEnum);
		if(iomodel->meshtype!=Mesh2DhorizontalEnum)
			iomodel->FetchDataToInput(elements,MeshVertexonbedEnum); // required for updating active nodes
	}

#ifdef _HAVE_ANDROID_
	elements->InputDuplicate(FrictionCoefficientEnum,AndroidFrictionCoefficientEnum);
#endif

	/*Free data: */
	iomodel->DeleteData(1,FlowequationElementEquationEnum);
	xDelete<int>(finiteelement_list);
}/*}}}*/
void StressbalanceAnalysis::CreateNodes(Nodes* nodes,IoModel* iomodel){/*{{{*/

	/*Intermediary*/
	bool isSSA,isL1L2,isHO,isFS,iscoupling;
	int  finiteelement=-1,approximation=-1;

	/*Fetch parameters: */
	iomodel->Constant(&isSSA,FlowequationIsSSAEnum);
	iomodel->Constant(&isL1L2,FlowequationIsL1L2Enum);
	iomodel->Constant(&isHO,FlowequationIsHOEnum);
	iomodel->Constant(&isFS,FlowequationIsFSEnum);

	/*Now, check that we have non SIA elements */
	if(!isSSA & !isL1L2 & !isHO & !isFS) return;

	/*Do we have coupling*/
	if( (isSSA?1.:0.) + (isL1L2?1.:0.) + (isHO?1.:0.) + (isFS?1.:0.) >1.)
	 iscoupling = true;
	else
	 iscoupling = false;

	/*If no coupling, call Regular CreateNodes, else, use P1 elements only*/
	if(!iscoupling){

		/*Get finite element type*/
		if(isSSA){
			approximation=SSAApproximationEnum;
			iomodel->Constant(&finiteelement,FlowequationFeSSAEnum);
		}
		else if(isL1L2){
			approximation = L1L2ApproximationEnum;
			finiteelement = P1Enum;
		}
		else if(isHO){
			approximation = HOApproximationEnum;
			iomodel->Constant(&finiteelement,FlowequationFeHOEnum);
		}
		else if(isFS){
			approximation = FSApproximationEnum;
			iomodel->Constant(&finiteelement,FlowequationFeFSEnum);
		}
		iomodel->FetchData(3,FlowequationBorderSSAEnum,FlowequationVertexEquationEnum,StressbalanceReferentialEnum);
		if(iomodel->meshtype!=Mesh2DhorizontalEnum) iomodel->FetchData(3,MeshVertexonbedEnum,MeshVertexonsurfaceEnum,FlowequationBorderFSEnum);
		::CreateNodes(nodes,iomodel,StressbalanceAnalysisEnum,finiteelement,approximation);
		iomodel->DeleteData(6,MeshVertexonbedEnum,MeshVertexonsurfaceEnum,FlowequationBorderSSAEnum,FlowequationBorderFSEnum,
					FlowequationVertexEquationEnum,StressbalanceReferentialEnum);
	}
	else{
		/*Coupling: we are going to create P1 Elements only*/

		Node*  node  = NULL;
		int    lid=0;
		if(!nodes) nodes = new Nodes();

		iomodel->FetchData(6,MeshVertexonbedEnum,MeshVertexonsurfaceEnum,FlowequationBorderSSAEnum,FlowequationBorderFSEnum,
					FlowequationVertexEquationEnum,StressbalanceReferentialEnum);
		if(isFS){
			/*P1+ velocity*/
			for(int i=0;i<iomodel->numberofvertices;i++){
				if(iomodel->my_vertices[i]){
					approximation=reCast<int>(iomodel->Data(FlowequationVertexEquationEnum)[i]);
					if(approximation==FSApproximationEnum)  approximation=FSvelocityEnum;
					nodes->AddObject(new Node(iomodel->nodecounter+i+1,i,lid++,i,iomodel,StressbalanceAnalysisEnum,approximation));
				}
			}
			for(int i=0;i<iomodel->numberofelements;i++){
				if(iomodel->my_elements[i]){
					node = new Node(iomodel->nodecounter+iomodel->numberofvertices+i+1,iomodel->numberofvertices+i,lid++,0,iomodel,StressbalanceAnalysisEnum,FSvelocityEnum);
					node->Deactivate();
					nodes->AddObject(node);
				}
			}
			/*P1 pressure*/
			for(int i=0;i<iomodel->numberofvertices;i++){
				if(iomodel->my_vertices[i]){
					approximation=reCast<int>(iomodel->Data(FlowequationVertexEquationEnum)[i]);
					node = new Node(iomodel->nodecounter+iomodel->numberofvertices+iomodel->numberofelements+i+1,iomodel->numberofvertices+iomodel->numberofelements+i,lid++,i,iomodel,StressbalanceAnalysisEnum,FSpressureEnum);
					if(approximation==HOApproximationEnum || approximation==SSAApproximationEnum){
						node->Deactivate();
					}
					nodes->AddObject(node);
				}
			}
		}
		else{
			for(int i=0;i<iomodel->numberofvertices;i++){
				if(iomodel->my_vertices[i]){
					nodes->AddObject(new Node(iomodel->nodecounter+i+1,i,lid++,i,iomodel,StressbalanceAnalysisEnum,reCast<int>(iomodel->Data(FlowequationVertexEquationEnum)[i])));
				}
			}
		}
		iomodel->DeleteData(6,MeshVertexonbedEnum,MeshVertexonsurfaceEnum,FlowequationBorderSSAEnum,FlowequationBorderFSEnum,
					FlowequationVertexEquationEnum,StressbalanceReferentialEnum);
	}
}/*}}}*/
void StressbalanceAnalysis::CreateConstraints(Constraints* constraints,IoModel* iomodel){/*{{{*/

	/*Intermediary*/
	int        i,j;
	int        count,finiteelement;
	IssmDouble g;
	IssmDouble rho_ice;
	IssmDouble FSreconditioning;
	bool       isSIA,isSSA,isL1L2,isHO,isFS,iscoupling;
	bool       spcpresent = false;
	int        Mx,Nx;
	int        My,Ny;
	int        Mz,Nz;
	IssmDouble *spcvx          = NULL;
	IssmDouble *spcvy          = NULL;
	IssmDouble *spcvz          = NULL;
	IssmDouble *nodeonSSA = NULL;
	IssmDouble *nodeonHO   = NULL;
	IssmDouble *nodeonFS   = NULL;
	IssmDouble *nodeonbed      = NULL;
	IssmDouble *groundedice_ls = NULL;
	IssmDouble *vertices_type  = NULL;
	IssmDouble *surface        = NULL;
	IssmDouble *z              = NULL;
	IssmDouble *timesx=NULL;
	IssmDouble *timesy=NULL;
	IssmDouble *timesz=NULL;
   IssmDouble* values=NULL;

	/*Fetch parameters: */
	iomodel->Constant(&g,ConstantsGEnum);
	iomodel->Constant(&rho_ice,MaterialsRhoIceEnum);
	iomodel->Constant(&FSreconditioning,StressbalanceFSreconditioningEnum);
	iomodel->Constant(&isSIA,FlowequationIsSIAEnum);
	iomodel->Constant(&isSSA,FlowequationIsSSAEnum);
	iomodel->Constant(&isL1L2,FlowequationIsL1L2Enum);
	iomodel->Constant(&isHO,FlowequationIsHOEnum);
	iomodel->Constant(&isFS,FlowequationIsFSEnum);

	/*Now, is the flag macayaealHO on? otherwise, do nothing: */
	if(!isSSA && !isHO && !isFS && !isL1L2) return;

	/*Do we have coupling*/
	if((isSIA?1.:0.) + (isSSA?1.:0.) + (isL1L2?1.:0.) + (isHO?1.:0.) + (isFS?1.:0.) >1.)
	 iscoupling = true;
	else
	 iscoupling = false;

	/*If no coupling, call Regular IoModelToConstraintsx, else, use P1 elements only*/
	if(!iscoupling){

		/*Get finite element type*/
		if(isSSA)       iomodel->Constant(&finiteelement,FlowequationFeSSAEnum);
		else if(isL1L2) finiteelement = P1Enum;
		else if(isHO)   iomodel->Constant(&finiteelement,FlowequationFeHOEnum);
		else if(isFS){  iomodel->Constant(&finiteelement,FlowequationFeFSEnum);
			/*Deduce velocity interpolation from finite element*/
			switch(finiteelement){
				case P1P1Enum          : finiteelement = P1Enum;       break;
				case P1P1GLSEnum       : finiteelement = P1Enum;       break;
				case MINIcondensedEnum : finiteelement = P1bubbleEnum; break;
				case MINIEnum          : finiteelement = P1bubbleEnum; break;
				case TaylorHoodEnum    : finiteelement = P2Enum;       break;
				case XTaylorHoodEnum   : finiteelement = P2Enum;       break;
				case OneLayerP4zEnum   : finiteelement = P2xP4Enum;    break;
				default: _error_("finite element "<<EnumToStringx(finiteelement)<<" not supported");
			}
		}
		else{
			_error_("model not supported yet");
		}

		if(isFS){

			/*Constraint at the bedrock interface (v.n = vz = 0) (Coordinates will be updated according to the bed slope)*/
			iomodel->FetchData(&vertices_type,NULL,NULL,FlowequationVertexEquationEnum);
			iomodel->FetchData(&nodeonFS,NULL,NULL,FlowequationBorderFSEnum);
			iomodel->FetchData(&nodeonbed,NULL,NULL,MeshVertexonbedEnum);
			iomodel->FetchData(&groundedice_ls,NULL,NULL,MaskGroundediceLevelsetEnum);
			if(iomodel->meshtype==Mesh3DEnum || iomodel->meshtype==Mesh3DtetrasEnum){
				iomodel->FetchData(&spcvz,&Mz,&Nz,StressbalanceSpcvzEnum);
			}
			else if (iomodel->meshtype==Mesh2DverticalEnum){
				iomodel->FetchData(&spcvz,&Mz,&Nz,StressbalanceSpcvyEnum);
			}
			else{
				_error_("not supported yet");
			}
			if(iomodel->meshtype==Mesh3DEnum || iomodel->meshtype==Mesh3DtetrasEnum){
				IoModelToConstraintsx(constraints,iomodel,StressbalanceSpcvxEnum,StressbalanceAnalysisEnum,finiteelement,1);
				IoModelToConstraintsx(constraints,iomodel,StressbalanceSpcvyEnum,StressbalanceAnalysisEnum,finiteelement,2);
				IoModelToConstraintsx(constraints,iomodel,spcvz,Mz,Nz,StressbalanceAnalysisEnum,finiteelement,3);
				iomodel->DeleteData(spcvz,StressbalanceSpcvzEnum);
			}
			else if (iomodel->meshtype==Mesh2DverticalEnum){
				IoModelToConstraintsx(constraints,iomodel,StressbalanceSpcvxEnum,StressbalanceAnalysisEnum,finiteelement,1);
				IoModelToConstraintsx(constraints,iomodel,spcvz,Mz,Nz,StressbalanceAnalysisEnum,finiteelement,2);
				iomodel->DeleteData(spcvz,StressbalanceSpcvyEnum);
			}
			else{
				_error_("not supported yet");
			}
			iomodel->DeleteData(vertices_type,FlowequationVertexEquationEnum);
			iomodel->DeleteData(nodeonFS,FlowequationBorderFSEnum);
			iomodel->DeleteData(nodeonbed,MeshVertexonbedEnum);
			iomodel->DeleteData(groundedice_ls,MaskGroundediceLevelsetEnum);

			/*Pressure spc*/
			count = constraints->Size();
			iomodel->FetchData(&vertices_type,NULL,NULL,FlowequationVertexEquationEnum);
			iomodel->FetchData(&surface,NULL,NULL,SurfaceEnum);
			iomodel->FetchData(&z,NULL,NULL,MeshZEnum);
			switch(finiteelement){
				case P1Enum:
					for(i=0;i<iomodel->numberofvertices;i++){
						if(iomodel->my_vertices[i]){
							if(reCast<int,IssmDouble>(vertices_type[i])==NoneApproximationEnum){
								constraints->AddObject(new SpcStatic(count+1,iomodel->nodecounter+iomodel->numberofvertices+iomodel->numberofelements+i+1,1,g*rho_ice*(surface[i]-z[i])/FSreconditioning,StressbalanceAnalysisEnum));
								count++;
							}
						}
					}
					break;
				case P1bubbleEnum:
					for(i=0;i<iomodel->numberofvertices;i++){
						if(iomodel->my_vertices[i]){
							if(reCast<int,IssmDouble>(vertices_type[i])==NoneApproximationEnum){
								constraints->AddObject(new SpcStatic(count+1,iomodel->nodecounter+iomodel->numberofvertices+iomodel->numberofelements+i+1,1,g*rho_ice*(surface[i]-z[i])/FSreconditioning,StressbalanceAnalysisEnum));
								count++;
							}
						}
					}
					break;
				case P2Enum:
					for(i=0;i<iomodel->numberofvertices;i++){
						if(iomodel->my_vertices[i]){
							if(reCast<int,IssmDouble>(vertices_type[i])==NoneApproximationEnum){
								constraints->AddObject(new SpcStatic(count+1,iomodel->nodecounter+iomodel->numberofvertices+iomodel->numberofedges+i+1,1,g*rho_ice*(surface[i]-z[i])/FSreconditioning,StressbalanceAnalysisEnum));
								count++;
							}
						}
					}
					break;
				case P2xP4Enum:
					//Nothing yet
					break;
				default:
					_error_("not implemented yet");
			}
			iomodel->DeleteData(vertices_type,FlowequationVertexEquationEnum);
			iomodel->DeleteData(surface,SurfaceEnum);
			iomodel->DeleteData(z,MeshZEnum);
		}
		else{
			IoModelToConstraintsx(constraints,iomodel,StressbalanceSpcvxEnum,StressbalanceAnalysisEnum,finiteelement,1);
			if(iomodel->meshtype!=Mesh2DverticalEnum){
				IoModelToConstraintsx(constraints,iomodel,StressbalanceSpcvyEnum,StressbalanceAnalysisEnum,finiteelement,2);
			}
		}

		return;
	}

	/*Constraints: fetch data: */
	iomodel->FetchData(&spcvx,&Mx,&Nx,StressbalanceSpcvxEnum);
	iomodel->FetchData(&spcvy,&My,&Ny,StressbalanceSpcvyEnum);
	iomodel->FetchData(&spcvz,&Mz,&Nz,StressbalanceSpcvzEnum);
	iomodel->FetchData(&nodeonSSA,NULL,NULL,FlowequationBorderSSAEnum);
	if(iomodel->meshtype==Mesh3DEnum || iomodel->meshtype==Mesh3DtetrasEnum)iomodel->FetchData(&nodeonHO,NULL,NULL,FlowequationBorderHOEnum);
	if(iomodel->meshtype==Mesh3DEnum || iomodel->meshtype==Mesh3DtetrasEnum)iomodel->FetchData(&nodeonFS,NULL,NULL,FlowequationBorderFSEnum);
	if(iomodel->meshtype==Mesh3DEnum || iomodel->meshtype==Mesh3DtetrasEnum)iomodel->FetchData(&nodeonbed,NULL,NULL,MeshVertexonbedEnum);
	if(iomodel->meshtype==Mesh3DEnum || iomodel->meshtype==Mesh3DtetrasEnum)iomodel->FetchData(&groundedice_ls,NULL,NULL,MaskGroundediceLevelsetEnum);
	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];
	}
	/*figure out times: */
	timesy=xNew<IssmDouble>(Ny);
	for(j=0;j<Ny;j++){
		timesy[j]=spcvy[(My-1)*Ny+j];
	}
	/*figure out times: */
	timesz=xNew<IssmDouble>(Nz);
	for(j=0;j<Nz;j++){
		timesz[j]=spcvz[(Mz-1)*Nz+j];
	}

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

			/*Start with adding spcs of coupling: zero at the border SSA/HO for the appropriate dofs*/
			if(reCast<int,IssmDouble>(vertices_type[i]==SSAHOApproximationEnum)){
				/*If grionSSA, spc HO dofs: 3 & 4*/
					if (reCast<int,IssmDouble>(nodeonHO[i])){
						constraints->AddObject(new SpcStatic(iomodel->constraintcounter+count+1,iomodel->nodecounter+i+1,1,0,StressbalanceAnalysisEnum)); //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,StressbalanceAnalysisEnum)); //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],StressbalanceAnalysisEnum)); //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],StressbalanceAnalysisEnum)); //add count'th spc, on node i+1, setting dof 1 to vx.
							count++;
						}

					}
					else if (reCast<int,IssmDouble>(nodeonSSA[i])){
						constraints->AddObject(new SpcStatic(iomodel->constraintcounter+count+1,iomodel->nodecounter+i+1,3,0,StressbalanceAnalysisEnum)); //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,StressbalanceAnalysisEnum)); //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],StressbalanceAnalysisEnum)); //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],StressbalanceAnalysisEnum)); //add count'th spc, on node i+1, setting dof 1 to vx.
							count++;
						}

					}
					else _error_("if vertices_type is SSAHO, you shoud have nodeonHO or nodeonSSA");
			}
			/*Also add spcs of coupling: zero at the border HO/FS for the appropriate dofs*/
			else if (reCast<int,IssmDouble>(vertices_type[i])==HOFSApproximationEnum){
				/*If grion,HO spc FS dofs: 3 4 & 5*/
					if (reCast<int,IssmDouble>(nodeonHO[i])){
						constraints->AddObject(new SpcStatic(iomodel->constraintcounter+count+1,iomodel->nodecounter+i+1,3,0,StressbalanceAnalysisEnum)); //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,StressbalanceAnalysisEnum)); //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,StressbalanceAnalysisEnum)); //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],StressbalanceAnalysisEnum)); //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],StressbalanceAnalysisEnum)); //add count'th spc, on node i+1, setting dof 1 to vx.
							count++;
						}

					}
					else if (reCast<int,IssmDouble>(nodeonFS[i])){ //spc HO nodes: 1 & 2
						constraints->AddObject(new SpcStatic(iomodel->constraintcounter+count+1,iomodel->nodecounter+i+1,1,0,StressbalanceAnalysisEnum)); //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,StressbalanceAnalysisEnum)); //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],StressbalanceAnalysisEnum)); //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],StressbalanceAnalysisEnum)); //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],StressbalanceAnalysisEnum)); //add count'th spc, on node i+1, setting dof 1 to vx.
							count++;
						}
					}
					else _error_("if vertices_type is HOFS, you shoud have nodeonHO or nodeonFS");
			}
			/*Also add spcs of coupling: zero at the border HO/FS for the appropriate dofs*/
			else if (reCast<int,IssmDouble>(vertices_type[i])==SSAFSApproximationEnum){
				/*If grion,HO spc FS dofs: 3 4 & 5*/
					if (reCast<int,IssmDouble>(nodeonSSA[i])){
						constraints->AddObject(new SpcStatic(iomodel->constraintcounter+count+1,iomodel->nodecounter+i+1,3,0,StressbalanceAnalysisEnum)); //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,StressbalanceAnalysisEnum)); //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,StressbalanceAnalysisEnum)); //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],StressbalanceAnalysisEnum)); //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],StressbalanceAnalysisEnum)); //add count'th spc, on node i+1, setting dof 1 to vx.
							count++;
						}

					}
					else if (reCast<int,IssmDouble>(nodeonFS[i])){ //spc SSA nodes: 1 & 2
						constraints->AddObject(new SpcStatic(iomodel->constraintcounter+count+1,iomodel->nodecounter+i+1,1,0,StressbalanceAnalysisEnum)); //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,StressbalanceAnalysisEnum)); //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],StressbalanceAnalysisEnum)); //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],StressbalanceAnalysisEnum)); //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],StressbalanceAnalysisEnum)); //add count'th spc, on node i+1, setting dof 1 to vx.
							count++;
						}
					}
					else _error_("if vertices_type is SSAFS, you shoud have nodeonSSA or nodeonFS");
			}
			/*Now add the regular spcs*/
			else{
				if (Mx==iomodel->numberofvertices && !xIsNan<IssmDouble>(spcvx[i])){
					constraints->AddObject(new SpcStatic(iomodel->constraintcounter+count+1,iomodel->nodecounter+i+1,1,spcvx[i],StressbalanceAnalysisEnum)); //add count'th spc, on node i+1, setting dof 1 to vx.
					count++;

				}
				else if (Mx==iomodel->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];
						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,StressbalanceAnalysisEnum));
						count++;
					}
					xDelete<IssmDouble>(values);
				}
				else if (vertices_type[i]==SIAApproximationEnum){
					constraints->AddObject(new SpcDynamic(iomodel->constraintcounter+count+1,iomodel->nodecounter+i+1,1,StressbalanceAnalysisEnum));
					count++;
				}

				if (My==iomodel->numberofvertices && !xIsNan<IssmDouble>(spcvy[i])){
					constraints->AddObject(new SpcStatic(iomodel->constraintcounter+count+1,iomodel->nodecounter+i+1,2,spcvy[i],StressbalanceAnalysisEnum)); //add count'th spc, on node i+1, setting dof 1 to vy.
					count++;
				}
				else if (My==iomodel->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];
						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,StressbalanceAnalysisEnum));
						count++;
					}
					xDelete<IssmDouble>(values);
				}
				else if (vertices_type[i]==SIAApproximationEnum){
					constraints->AddObject(new SpcDynamic(iomodel->constraintcounter+count+1,iomodel->nodecounter+i+1,2,StressbalanceAnalysisEnum));
					count++;
				}

				if (reCast<int,IssmDouble>(vertices_type[i])==FSApproximationEnum ||  (reCast<int,IssmDouble>(vertices_type[i])==NoneApproximationEnum)){
					if (Mz==iomodel->numberofvertices && !xIsNan<IssmDouble>(spcvz[i])){
						constraints->AddObject(new SpcStatic(iomodel->constraintcounter+count+1,iomodel->nodecounter+i+1,3,spcvz[i],StressbalanceAnalysisEnum)); //add count'th spc, on node i+1, setting dof 2 to vy
						count++;
					}
					else if (Mz==iomodel->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];
							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,StressbalanceAnalysisEnum));
							count++;
						}
						xDelete<IssmDouble>(values);
					}

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

	/*Free data: */
	iomodel->DeleteData(spcvx,StressbalanceSpcvxEnum);
	iomodel->DeleteData(spcvy,StressbalanceSpcvyEnum);
	iomodel->DeleteData(spcvz,StressbalanceSpcvzEnum);
	iomodel->DeleteData(nodeonSSA,FlowequationBorderSSAEnum);
	if(iomodel->meshtype==Mesh3DEnum || iomodel->meshtype==Mesh3DtetrasEnum)iomodel->DeleteData(nodeonHO,FlowequationBorderHOEnum);
	if(iomodel->meshtype==Mesh3DEnum || iomodel->meshtype==Mesh3DtetrasEnum)iomodel->DeleteData(nodeonFS,FlowequationBorderFSEnum);
	if(iomodel->meshtype==Mesh3DEnum || iomodel->meshtype==Mesh3DtetrasEnum)iomodel->DeleteData(nodeonbed,MeshVertexonbedEnum);
	if(iomodel->meshtype==Mesh3DEnum || iomodel->meshtype==Mesh3DtetrasEnum)iomodel->DeleteData(groundedice_ls,MaskGroundediceLevelsetEnum);
	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);

}/*}}}*/
void StressbalanceAnalysis::CreateLoads(Loads* loads, IoModel* iomodel){/*{{{*/

	/*Intermediary*/
	const int   RIFTINFOSIZE = 12;
	int         i;
	int         count;
	int         penpair_ids[2];
	bool        isSSA,isL1L2,isHO,isFS;
	int         numpenalties,numrifts,numriftsegments;
	IssmDouble *riftinfo       = NULL;
	IssmDouble *penalties      = NULL;
	int         assert_int;

	/*Fetch parameters: */
	iomodel->Constant(&isL1L2,FlowequationIsL1L2Enum);
	iomodel->Constant(&isFS,FlowequationIsFSEnum);
	iomodel->Constant(&isSSA,FlowequationIsSSAEnum);
	iomodel->Constant(&isHO,FlowequationIsHOEnum);
	iomodel->Constant(&numrifts,RiftsNumriftsEnum);

	/*Now, is the flag macayaealHO on? otherwise, do nothing: */
	if(!isSSA && !isHO && !isFS && !isL1L2) return;

	/*Initialize counter: */
	count=0;

	/*Create Penpair for penalties: */
	iomodel->FetchData(&penalties,&numpenalties,NULL,StressbalanceVertexPairingEnum);

	for(i=0;i<numpenalties;i++){

		if(iomodel->my_vertices[reCast<int,IssmDouble>(penalties[2*i+0]-1)]){

			/*In debugging mode, check that the second node is in the same cpu*/
			assert_int=iomodel->my_vertices[reCast<int,IssmDouble>(penalties[2*i+1]-1)]; _assert_(assert_int);

			/*Get node ids*/
			penpair_ids[0]=iomodel->nodecounter+reCast<int,IssmDouble>(penalties[2*i+0]);
			penpair_ids[1]=iomodel->nodecounter+reCast<int,IssmDouble>(penalties[2*i+1]);

			/*Create Load*/
			loads->AddObject(new Penpair(iomodel->loadcounter+count+1,&penpair_ids[0],StressbalanceAnalysisEnum));
			count++;
		}
	}

	/*free ressources: */
	iomodel->DeleteData(penalties,StressbalanceVertexPairingEnum);

	/*Create Riffront loads for rifts: */
	if(numrifts){
		iomodel->FetchData(&riftinfo,&numriftsegments,NULL,RiftsRiftstructEnum);
		iomodel->FetchData(5,RiftsRiftstructEnum,ThicknessEnum,BaseEnum,SurfaceEnum,MaskGroundediceLevelsetEnum);
		for(i=0;i<numriftsegments;i++){
			if(iomodel->my_elements[reCast<int,IssmDouble>(*(riftinfo+RIFTINFOSIZE*i+2))-1]){
				loads->AddObject(new Riftfront(iomodel->loadcounter+count+1,i,iomodel,StressbalanceAnalysisEnum));
				count++;
			}
		}
		iomodel->DeleteData(5,RiftsRiftstructEnum,ThicknessEnum,BaseEnum,SurfaceEnum,MaskGroundediceLevelsetEnum);
		xDelete<IssmDouble>(riftinfo);
	}
}/*}}}*/

/*Finite Element Analysis*/
void           StressbalanceAnalysis::Core(FemModel* femmodel){/*{{{*/

	/*Intermediaries*/
	bool isSSA,isL1L2,isHO,isFS;
	bool conserve_loads = true;
	int  newton,meshtype,fe_FS;

	/* recover parameters:*/
	femmodel->parameters->FindParam(&isSSA,FlowequationIsSSAEnum);
	femmodel->parameters->FindParam(&isL1L2,FlowequationIsL1L2Enum);
	femmodel->parameters->FindParam(&isHO,FlowequationIsHOEnum);
	femmodel->parameters->FindParam(&isFS,FlowequationIsFSEnum);
	femmodel->parameters->FindParam(&fe_FS,FlowequationFeFSEnum);
	femmodel->parameters->FindParam(&meshtype,MeshTypeEnum);
	femmodel->parameters->FindParam(&newton,StressbalanceIsnewtonEnum);

	if(isFS && !(isSSA || isHO || isL1L2)){
		if(VerboseSolution()) _printf0_("   computing velocities\n");

		femmodel->SetCurrentConfiguration(StressbalanceAnalysisEnum);
		if (fe_FS==XTaylorHoodEnum)
		 solutionsequence_la_theta(femmodel);
		else if(newton>0)
		 solutionsequence_newton(femmodel);
		else
		 solutionsequence_nonlinear(femmodel,conserve_loads); 
	}
	else if(!isFS && (isSSA || isHO || isL1L2)){ 
		if(VerboseSolution()) _printf0_("   computing velocities\n");

		femmodel->SetCurrentConfiguration(StressbalanceAnalysisEnum);
		if(newton>0)
		 solutionsequence_newton(femmodel);
		else
		 solutionsequence_nonlinear(femmodel,conserve_loads); 

		if(meshtype==Mesh2DverticalEnum && isSSA){
			femmodel->parameters->SetParam(VxEnum,InputToExtrudeEnum);
			extrudefrombase_core(femmodel);
			femmodel->parameters->SetParam(VelEnum,InputToExtrudeEnum);
			extrudefrombase_core(femmodel);
		}
	}
	else if ((isSSA || isL1L2 || isHO) && isFS){
		if(VerboseSolution()) _printf0_("   computing coupling between lower order models and FS\n");
		solutionsequence_FScoupling_nonlinear(femmodel,conserve_loads);
	}
	else{
		_error_("not supported");
	}

}/*}}}*/
ElementVector* StressbalanceAnalysis::CreateDVector(Element* element){/*{{{*/

	int approximation;
	element->GetInputValue(&approximation,ApproximationEnum);
	switch(approximation){
		case FSApproximationEnum: 
			return CreateDVectorFS(element);
		default:
			return NULL; //no need for doftypes outside of FS approximation
	}
	return NULL;

}/*}}}*/
ElementMatrix* StressbalanceAnalysis::CreateJacobianMatrix(Element* element){/*{{{*/

	int approximation;
	element->GetInputValue(&approximation,ApproximationEnum);
	switch(approximation){
		case SSAApproximationEnum: 
			return CreateJacobianMatrixSSA(element);
		case HOApproximationEnum: 
			return CreateJacobianMatrixHO(element);
		case FSApproximationEnum: 
			return CreateJacobianMatrixFS(element);
		case NoneApproximationEnum:
			return NULL;
		default:
			_error_("Approximation "<<EnumToStringx(approximation)<<" not supported");
	}
}/*}}}*/
ElementMatrix* StressbalanceAnalysis::CreateKMatrix(Element* element){/*{{{*/
	int approximation;
	element->GetInputValue(&approximation,ApproximationEnum);
	switch(approximation){
		case SIAApproximationEnum:
			return NULL;
		case SSAApproximationEnum: 
			return CreateKMatrixSSA(element);
		case L1L2ApproximationEnum: 
			return CreateKMatrixL1L2(element);
		case HOApproximationEnum: 
			return CreateKMatrixHO(element);
		case FSApproximationEnum: 
			return CreateKMatrixFS(element);
		case SSAHOApproximationEnum: 
			return CreateKMatrixSSAHO(element);
		case HOFSApproximationEnum: 
			return CreateKMatrixHOFS(element);
		case SSAFSApproximationEnum: 
			return CreateKMatrixSSAFS(element);
		case NoneApproximationEnum:
			return NULL;
		default:
			_error_("Approximation "<<EnumToStringx(approximation)<<" not supported");
	}
}/*}}}*/
ElementVector* StressbalanceAnalysis::CreatePVector(Element* element){/*{{{*/

	int approximation;
	element->GetInputValue(&approximation,ApproximationEnum);
	switch(approximation){
		case SIAApproximationEnum:
			return NULL;
		case SSAApproximationEnum: 
			return CreatePVectorSSA(element);
		case L1L2ApproximationEnum: 
			return CreatePVectorL1L2(element);
		case HOApproximationEnum: 
			return CreatePVectorHO(element);
		case FSApproximationEnum: 
			return CreatePVectorFS(element);
		case SSAHOApproximationEnum: 
			return CreatePVectorSSAHO(element);
		case HOFSApproximationEnum: 
			return CreatePVectorHOFS(element);
		case SSAFSApproximationEnum: 
			return CreatePVectorSSAFS(element);
		case NoneApproximationEnum:
			return NULL;
		default:
			_error_("Approximation "<<EnumToStringx(approximation)<<" not supported");
	}
}/*}}}*/
void StressbalanceAnalysis::GetSolutionFromInputs(Vector<IssmDouble>* solution,Element* element){/*{{{*/

	int approximation;
	element->GetInputValue(&approximation,ApproximationEnum);
	switch(approximation){
		case FSApproximationEnum: case NoneApproximationEnum:
			GetSolutionFromInputsFS(solution,element);
			return;
		case SSAApproximationEnum: case HOApproximationEnum: case SIAApproximationEnum:
			GetSolutionFromInputsHoriz(solution,element);
			return;
		case L1L2ApproximationEnum:
			GetSolutionFromInputsHoriz(solution,element);
			return;
		case SSAHOApproximationEnum: case HOFSApproximationEnum: case SSAFSApproximationEnum:
			/*the elements around will create the solution*/
			return;
		default:
			_error_("Approximation "<<EnumToStringx(approximation)<<" not supported");
	}
}/*}}}*/
void StressbalanceAnalysis::GetSolutionFromInputsHoriz(Vector<IssmDouble>* solution,Element* element){/*{{{*/

	IssmDouble   vx,vy;
	int          meshtype,dim,approximation,dofpernode;
	int*         doflist = NULL;

	/*Get some parameters*/
	element->FindParam(&meshtype,MeshTypeEnum);
	switch(meshtype){
		case Mesh2DhorizontalEnum: dim = 2; dofpernode = 2; break;
		case Mesh2DverticalEnum:   dim = 2; dofpernode = 1; break;
		case Mesh3DEnum:           dim = 3; dofpernode = 2; break;
		case Mesh3DtetrasEnum:     dim = 3; dofpernode = 2; break;
		default: _error_("mesh "<<EnumToStringx(meshtype)<<" not supported yet");
	}

	/*Fetch number of nodes and dof for this finite element*/
	int numnodes = element->GetNumberOfNodes();
	int numdof   = numnodes*dofpernode;
	element->GetInputValue(&approximation,ApproximationEnum);

	/*Fetch dof list and allocate solution vector*/
	element->GetDofList(&doflist,approximation,GsetEnum);
	IssmDouble* values = xNew<IssmDouble>(numdof);

	/*Get inputs*/
	Input* vx_input=element->GetInput(VxEnum); _assert_(vx_input);
	Input* vy_input=NULL;
	if(meshtype!=Mesh2DverticalEnum){vy_input=element->GetInput(VyEnum); _assert_(vy_input);}

	/*Ok, we have vx and vy in values, fill in vx and vy arrays: */
	Gauss* gauss=element->NewGauss();
	for(int i=0;i<numnodes;i++){
		gauss->GaussNode(element->FiniteElement(),i);

		/*Recover vx and vy*/
		vx_input->GetInputValue(&vx,gauss);
		values[i*dofpernode+0]=vx;
		if(dofpernode==2){
			vy_input->GetInputValue(&vy,gauss);
			values[i*dofpernode+1]=vy;
		}
	}

	solution->SetValues(numdof,doflist,values,INS_VAL);

	/*Free ressources:*/
	delete gauss;
	xDelete<IssmDouble>(values);
	xDelete<int>(doflist);
}/*}}}*/
void StressbalanceAnalysis::InputUpdateFromSolution(IssmDouble* solution,Element* element){/*{{{*/

	int approximation;
	element->GetInputValue(&approximation,ApproximationEnum);
	switch(approximation){
		case FSApproximationEnum: case NoneApproximationEnum:
			InputUpdateFromSolutionFS(solution,element);
			return;
		case SIAApproximationEnum: 
			return;
		case SSAApproximationEnum: 
			InputUpdateFromSolutionSSA(solution,element);
			return;
		case HOApproximationEnum: 
			InputUpdateFromSolutionHO(solution,element);
			return;
		case L1L2ApproximationEnum:
			InputUpdateFromSolutionL1L2(solution,element);
			return;
		case SSAHOApproximationEnum:
			InputUpdateFromSolutionSSAHO(solution,element);
			return;
		case HOFSApproximationEnum:
			InputUpdateFromSolutionHOFS(solution,element);
			return;
		case SSAFSApproximationEnum:
			InputUpdateFromSolutionSSAFS(solution,element);
			return;
		default:
			_error_("Approximation "<<EnumToStringx(approximation)<<" not supported");
	}
}/*}}}*/
void StressbalanceAnalysis::UpdateConstraints(FemModel* femmodel){/*{{{*/
	/*Default, do nothing*/
	bool islevelset;
	femmodel->parameters->FindParam(&islevelset,TransientIslevelsetEnum);
	if(islevelset){
		SetActiveNodesLSMx(femmodel);
	}
	return;
}/*}}}*/

/*SSA*/
ElementMatrix* StressbalanceAnalysis::CreateJacobianMatrixSSA(Element* element){/*{{{*/

	/*Intermediaries*/
	int      meshtype;
	Element* basalelement;

	/*Get basal element*/
	element->FindParam(&meshtype,MeshTypeEnum);
	switch(meshtype){
		case Mesh2DhorizontalEnum:
			basalelement = element;
			break;
		case Mesh3DEnum:
			if(!element->IsOnBed()) return NULL;
			basalelement = element->SpawnBasalElement();
			break;
		default: _error_("mesh "<<EnumToStringx(meshtype)<<" not supported yet");
	}

	/*Intermediaries */
	IssmDouble Jdet,thickness;
	IssmDouble eps1dotdphii,eps1dotdphij;
	IssmDouble eps2dotdphii,eps2dotdphij;
	IssmDouble mu_prime;
	IssmDouble epsilon[3];/* epsilon=[exx,eyy,exy];*/
	IssmDouble eps1[2],eps2[2];
	IssmDouble *xyz_list = NULL;

	/*Fetch number of nodes and dof for this finite element*/
	int numnodes = basalelement->GetNumberOfNodes();

	/*Initialize Element matrix, vectors and Gaussian points*/
	ElementMatrix* Ke=this->CreateKMatrixSSA(element); //Initialize Jacobian with regular SSA (first part of the Gateau derivative)
	IssmDouble*    dbasis = xNew<IssmDouble>(2*numnodes);

	/*Retrieve all inputs and parameters*/
	element->GetVerticesCoordinates(&xyz_list);
	Input* thickness_input = basalelement->GetInput(ThicknessEnum);_assert_(thickness_input);
	Input* vx_input        = basalelement->GetInput(VxEnum);       _assert_(vx_input);
	Input* vy_input        = basalelement->GetInput(VyEnum);       _assert_(vy_input);

	/* Start  looping on the number of gaussian points: */
	Gauss* gauss = basalelement->NewGauss(2);
	for(int ig=gauss->begin();ig<gauss->end();ig++){
		gauss->GaussPoint(ig);

		basalelement->JacobianDeterminant(&Jdet,xyz_list,gauss);
		basalelement->NodalFunctionsDerivatives(dbasis,xyz_list,gauss);

		thickness_input->GetInputValue(&thickness, gauss);
		basalelement->StrainRateSSA(&epsilon[0],xyz_list,gauss,vx_input,vy_input);
		basalelement->ViscositySSADerivativeEpsSquare(&mu_prime,&epsilon[0]);
		eps1[0]=2*epsilon[0]+epsilon[1];   eps2[0]=epsilon[2];
		eps1[1]=epsilon[2];                eps2[1]=epsilon[0]+2*epsilon[1];

		for(int i=0;i<numnodes;i++){
			for(int j=0;j<numnodes;j++){
				eps1dotdphii=eps1[0]*dbasis[0*numnodes+i]+eps1[1]*dbasis[1*numnodes+i];
				eps1dotdphij=eps1[0]*dbasis[0*numnodes+j]+eps1[1]*dbasis[1*numnodes+j];
				eps2dotdphii=eps2[0]*dbasis[0*numnodes+i]+eps2[1]*dbasis[1*numnodes+i];
				eps2dotdphij=eps2[0]*dbasis[0*numnodes+j]+eps2[1]*dbasis[1*numnodes+j];

				Ke->values[2*numnodes*(2*i+0)+2*j+0]+=gauss->weight*Jdet*2.*mu_prime*thickness*eps1dotdphij*eps1dotdphii;
				Ke->values[2*numnodes*(2*i+0)+2*j+1]+=gauss->weight*Jdet*2.*mu_prime*thickness*eps2dotdphij*eps1dotdphii;
				Ke->values[2*numnodes*(2*i+1)+2*j+0]+=gauss->weight*Jdet*2.*mu_prime*thickness*eps1dotdphij*eps2dotdphii;
				Ke->values[2*numnodes*(2*i+1)+2*j+1]+=gauss->weight*Jdet*2.*mu_prime*thickness*eps2dotdphij*eps2dotdphii;
			}
		}
	}

	/*Transform Coordinate System*/
	basalelement->TransformStiffnessMatrixCoord(Ke,XYEnum);

	/*Clean up and return*/
	delete gauss;
	xDelete<IssmDouble>(xyz_list);
	xDelete<IssmDouble>(dbasis);

	/*clean-up and return*/
	if(meshtype!=Mesh2DhorizontalEnum){basalelement->DeleteMaterials(); delete basalelement;};
	return Ke;

}/*}}}*/
ElementMatrix* StressbalanceAnalysis::CreateKMatrixSSA(Element* element){/*{{{*/

	/* Check if ice in element */
	if(!element->IsIceInElement()) return NULL;

	/*Intermediaries*/
	int      meshtype;
	Element* basalelement;

	/*Get basal element*/
	element->FindParam(&meshtype,MeshTypeEnum);
	switch(meshtype){
		case Mesh2DhorizontalEnum:
			basalelement = element;
			break;
		case Mesh3DEnum: case Mesh2DverticalEnum:
			if(!element->IsOnBed()) return NULL;
			basalelement = element->SpawnBasalElement();
			break;
		default: _error_("mesh "<<EnumToStringx(meshtype)<<" not supported yet");
	}

	/*compute all stiffness matrices for this element*/
	ElementMatrix* Ke1=CreateKMatrixSSAViscous(basalelement);
	ElementMatrix* Ke2=CreateKMatrixSSAFriction(basalelement);
	ElementMatrix* Ke =new ElementMatrix(Ke1,Ke2);

	/*clean-up and return*/
	if(meshtype!=Mesh2DhorizontalEnum){basalelement->DeleteMaterials(); delete basalelement;};
	delete Ke1;
	delete Ke2;
	return Ke;
}/*}}}*/
ElementMatrix* StressbalanceAnalysis::CreateKMatrixSSAFriction(Element* element){/*{{{*/

	/*Return if element is inactive*/
	if(element->IsFloating() || !element->IsIceInElement()) return NULL;

	/*Intermediaries*/
	int         dim,meshtype;
	bool        mainlyfloating;
	int         migration_style,point1;
	IssmDouble  alpha2,Jdet,fraction1,fraction2;
	IssmDouble  gllevelset,phi=1.;
	IssmDouble *xyz_list  = NULL;
	Gauss*      gauss     = NULL;

	/*Get problem dimension*/
	element->FindParam(&meshtype,MeshTypeEnum);
	switch(meshtype){
		case Mesh2DverticalEnum:   dim = 1;break;
		case Mesh2DhorizontalEnum: dim = 2;break;
		case Mesh3DEnum:           dim = 2;break;
		case Mesh3DtetrasEnum:     dim = 2;break;
		default: _error_("mesh "<<EnumToStringx(meshtype)<<" not supported yet");
	}

	/*Fetch number of nodes and dof for this finite element*/
	int numnodes = element->GetNumberOfNodes();
	int numdof   = numnodes*dim;

	/*Initialize Element matrix and vectors*/
	ElementMatrix* Ke = element->NewElementMatrix(SSAApproximationEnum);
	IssmDouble*    B  = xNew<IssmDouble>(dim*numdof);
	IssmDouble*    D  = xNewZeroInit<IssmDouble>(dim*dim);

	/*Retrieve all inputs and parameters*/
	element->GetVerticesCoordinates(&xyz_list);
	element->FindParam(&migration_style,GroundinglineMigrationEnum);
	Input* surface_input    = element->GetInput(SurfaceEnum); _assert_(surface_input);
	Input* vx_input         = element->GetInput(VxEnum);      _assert_(vx_input);
	Input* gllevelset_input = NULL;
	Input* vy_input         = NULL;
	if(dim==2){vy_input     = element->GetInput(VyEnum);      _assert_(vy_input);}

	/*build friction object, used later on: */
	Friction* friction=new Friction(element,1);

	/*Recover portion of element that is grounded*/
	if(migration_style==SubelementMigrationEnum) phi=element->GetGroundedPortion(xyz_list);
	if(migration_style==SubelementMigration2Enum){
		gllevelset_input=element->GetInput(MaskGroundediceLevelsetEnum); _assert_(gllevelset_input);
		element->GetGroundedPart(&point1,&fraction1,&fraction2,&mainlyfloating);
	   gauss = element->NewGauss(point1,fraction1,fraction2,mainlyfloating,2);
	}
	else{
		gauss = element->NewGauss(2);
	}

	/* Start  looping on the number of gaussian points: */
	for(int ig=gauss->begin();ig<gauss->end();ig++){
		gauss->GaussPoint(ig);

		friction->GetAlpha2(&alpha2, gauss,vx_input,vy_input,NULL);
		if(migration_style==SubelementMigrationEnum) alpha2=phi*alpha2;
		if(migration_style==SubelementMigration2Enum){
			gllevelset_input->GetInputValue(&gllevelset, gauss);
			if(gllevelset<0.) alpha2=0.;
		}

		this->GetBSSAFriction(B,element,dim,xyz_list,gauss);
		element->JacobianDeterminant(&Jdet, xyz_list,gauss);
		for(int i=0;i<dim;i++) D[i*dim+i]=alpha2*gauss->weight*Jdet;

		TripleMultiply(B,dim,numdof,1,
					D,dim,dim,0,
					B,dim,numdof,0,
					&Ke->values[0],1);
	}

	/*Transform Coordinate System*/
	if(dim==2) element->TransformStiffnessMatrixCoord(Ke,XYEnum);

	/*Clean up and return*/
	delete gauss;
	delete friction;
	xDelete<IssmDouble>(xyz_list);
	xDelete<IssmDouble>(B);
	xDelete<IssmDouble>(D);
	return Ke;
}/*}}}*/
ElementMatrix* StressbalanceAnalysis::CreateKMatrixSSAViscous(Element* element){/*{{{*/

	/* Check if ice in element */
	if(!element->IsIceInElement()) return NULL;

	/*Intermediaries*/
	int         dim,meshtype,bsize;
	IssmDouble  viscosity,newviscosity,oldviscosity;
	IssmDouble  viscosity_overshoot,thickness,Jdet;
	IssmDouble  D_scalar;
	IssmDouble *xyz_list = NULL;

	/*Get problem dimension*/
	element->FindParam(&meshtype,MeshTypeEnum);
	switch(meshtype){
		case Mesh2DverticalEnum:   dim = 1; bsize = 1; break;
		case Mesh2DhorizontalEnum: dim = 2; bsize = 3; break;
		case Mesh3DEnum:           dim = 2; bsize = 3; break;
		case Mesh3DtetrasEnum:     dim = 2; bsize = 3; break;
		default: _error_("mesh "<<EnumToStringx(meshtype)<<" not supported yet");
	}

	/*Fetch number of nodes and dof for this finite element*/
	int numnodes = element->GetNumberOfNodes();
	int numdof   = numnodes*dim;

	/*Initialize Element matrix and vectors*/
	ElementMatrix* Ke     = element->NewElementMatrix(SSAApproximationEnum);
	IssmDouble*    B      = xNew<IssmDouble>(bsize*numdof);
	IssmDouble*    Bprime = xNew<IssmDouble>(bsize*numdof);
	IssmDouble*    D      = xNewZeroInit<IssmDouble>(bsize*bsize);

	/*Retrieve all inputs and parameters*/
	element->GetVerticesCoordinates(&xyz_list);
	Input* thickness_input=element->GetInput(ThicknessEnum); _assert_(thickness_input);
	Input* vx_input=element->GetInput(VxEnum);               _assert_(vx_input);
	Input* vxold_input=element->GetInput(VxPicardEnum);      _assert_(vxold_input);
	Input* vy_input    = NULL;
	Input* vyold_input = NULL;
	if(dim==2){
		vy_input    = element->GetInput(VyEnum);       _assert_(vy_input);
		vyold_input = element->GetInput(VyPicardEnum); _assert_(vyold_input);
	}
	element->FindParam(&viscosity_overshoot,StressbalanceViscosityOvershootEnum);

	/* Start  looping on the number of gaussian points: */
	Gauss* gauss = element->NewGauss(2);
	for(int ig=gauss->begin();ig<gauss->end();ig++){
		gauss->GaussPoint(ig);

		element->JacobianDeterminant(&Jdet,xyz_list,gauss);
		this->GetBSSA(B,element,dim,xyz_list,gauss);
		this->GetBSSAprime(Bprime,element,dim,xyz_list,gauss);

		element->ViscositySSA(&viscosity,dim,xyz_list,gauss,vx_input,vy_input);
		element->ViscositySSA(&oldviscosity,dim,xyz_list,gauss,vxold_input,vyold_input);
		thickness_input->GetInputValue(&thickness, gauss);

		newviscosity=viscosity+viscosity_overshoot*(viscosity-oldviscosity);
		D_scalar=2.*newviscosity*thickness*gauss->weight*Jdet;
		for(int i=0;i<bsize;i++) D[i*bsize+i]=D_scalar;

		TripleMultiply(B,bsize,numdof,1,
					D,bsize,bsize,0,
					Bprime,bsize,numdof,0,
					&Ke->values[0],1);

		for(int i=0;i<bsize*numdof;i++) if(B[i]==1.) _error_("STREAM");
		for(int i=0;i<bsize*numdof;i++) if(Bprime[i]==1.) _error_("STREAM");
	}

	/*Transform Coordinate System*/
	if(dim==2) element->TransformStiffnessMatrixCoord(Ke,XYEnum);

	/*Clean up and return*/
	delete gauss;
	xDelete<IssmDouble>(xyz_list);
	xDelete<IssmDouble>(D);
	xDelete<IssmDouble>(Bprime);
	xDelete<IssmDouble>(B);
	return Ke;
}/*}}}*/
ElementVector* StressbalanceAnalysis::CreatePVectorSSA(Element* element){/*{{{*/

	/* Check if ice in element */
	if(!element->IsIceInElement()) return NULL;

	/*Intermediaries*/
	int      meshtype;
	Element* basalelement;

	/*Get basal element*/
	element->FindParam(&meshtype,MeshTypeEnum);
	switch(meshtype){
		case Mesh2DhorizontalEnum:
			basalelement = element;
			break;
		case Mesh3DEnum: case Mesh2DverticalEnum:
			if(!element->IsOnBed()) return NULL;
			basalelement = element->SpawnBasalElement();
			break;
		default: _error_("mesh "<<EnumToStringx(meshtype)<<" not supported yet");
	}

	/*compute all load vectors for this element*/
	ElementVector* pe1=CreatePVectorSSADrivingStress(basalelement);
	ElementVector* pe2=CreatePVectorSSAFront(basalelement);
	ElementVector* pe =new ElementVector(pe1,pe2);

	/*clean-up and return*/
	if(meshtype!=Mesh2DhorizontalEnum){basalelement->DeleteMaterials(); delete basalelement;};
	delete pe1;
	delete pe2;
	return pe;
}/*}}}*/
ElementVector* StressbalanceAnalysis::CreatePVectorSSADrivingStress(Element* element){/*{{{*/

	/* Check if ice in element */
	if(!element->IsIceInElement()) return NULL;

	/*Intermediaries */
	int         dim,meshtype;
	IssmDouble  thickness,Jdet,slope[2];
	IssmDouble* xyz_list = NULL;

	/*Get problem dimension*/
	element->FindParam(&meshtype,MeshTypeEnum);
	switch(meshtype){
		case Mesh2DverticalEnum:   dim = 1;break;
		case Mesh2DhorizontalEnum: dim = 2;break;
		case Mesh3DEnum:           dim = 2;break;
		case Mesh3DtetrasEnum:     dim = 2;break;
		default: _error_("mesh "<<EnumToStringx(meshtype)<<" not supported yet");
	}

	/*Fetch number of nodes and dof for this finite element*/
	int numnodes = element->GetNumberOfNodes();

	/*Initialize Element vector and vectors*/
	ElementVector* pe    = element->NewElementVector(SSAApproximationEnum);
	IssmDouble*    basis = xNew<IssmDouble>(numnodes);

	/*Retrieve all inputs and parameters*/
	element->GetVerticesCoordinates(&xyz_list);
	Input*     thickness_input=element->GetInput(ThicknessEnum); _assert_(thickness_input); 
	Input*     surface_input  =element->GetInput(SurfaceEnum);   _assert_(surface_input);
	IssmDouble rhog = element->GetMaterialParameter(MaterialsRhoIceEnum)*element->GetMaterialParameter(ConstantsGEnum);

	/* Start  looping on the number of gaussian points: */
	Gauss* gauss=element->NewGauss(2);
	for(int ig=gauss->begin();ig<gauss->end();ig++){

		gauss->GaussPoint(ig);

		element->JacobianDeterminant(&Jdet,xyz_list,gauss);
		element->NodalFunctions(basis, gauss);

		thickness_input->GetInputValue(&thickness,gauss);
		surface_input->GetInputDerivativeValue(&slope[0],xyz_list,gauss);

		for(int i=0;i<numnodes;i++){
			pe->values[i*dim+0]+=-rhog*thickness*slope[0]*Jdet*gauss->weight*basis[i];
			if(dim==2) pe->values[i*dim+1]+=-rhog*thickness*slope[1]*Jdet*gauss->weight*basis[i];
		}
	}

	/*Transform coordinate system*/
	if(dim==2) element->TransformLoadVectorCoord(pe,XYEnum);

	/*Clean up and return*/
	xDelete<IssmDouble>(xyz_list);
	xDelete<IssmDouble>(basis);
	delete gauss;
	return pe;
}/*}}}*/
ElementVector* StressbalanceAnalysis::CreatePVectorSSAFront(Element* element){/*{{{*/

	/* Check if ice in element */
	if(!element->IsIceInElement()) return NULL;

	/*If no front, return NULL*/
	if(!element->IsIcefront()) return NULL;

	/*Intermediaries*/
	int         dim,meshtype;
	IssmDouble  Jdet,thickness,bed,water_pressure,ice_pressure;
	IssmDouble  surface_under_water,base_under_water,pressure;
	IssmDouble *xyz_list = NULL;
	IssmDouble *xyz_list_front = NULL;
	IssmDouble  normal[2];

	/*Get problem dimension*/
	element->FindParam(&meshtype,MeshTypeEnum);
	switch(meshtype){
		case Mesh2DverticalEnum:   dim = 1;break;
		case Mesh2DhorizontalEnum: dim = 2;break;
		case Mesh3DEnum:           dim = 2;break;
		case Mesh3DtetrasEnum:     dim = 2;break;
		default: _error_("mesh "<<EnumToStringx(meshtype)<<" not supported yet");
	}

	/*Fetch number of nodes for this finite element*/
	int numnodes = element->GetNumberOfNodes();

	/*Initialize Element vector and other vectors*/
	ElementVector* pe    = element->NewElementVector(SSAApproximationEnum);
	IssmDouble*    basis = xNew<IssmDouble>(numnodes);

	/*Retrieve all inputs and parameters*/
	Input* thickness_input = element->GetInput(ThicknessEnum); _assert_(thickness_input);
	Input* base_input       = element->GetInput(BaseEnum);       _assert_(base_input);
	IssmDouble rho_water   = element->GetMaterialParameter(MaterialsRhoWaterEnum);
	IssmDouble rho_ice     = element->GetMaterialParameter(MaterialsRhoIceEnum);
	IssmDouble gravity     = element->GetMaterialParameter(ConstantsGEnum);
	element->GetVerticesCoordinates(&xyz_list);
	//element->ZeroLevelsetCoordinates(&xyz_list_front,xyz_list,MaskIceLevelsetEnum);
	element->GetIcefrontCoordinates(&xyz_list_front,xyz_list,MaskIceLevelsetEnum);
	element->NormalSection(&normal[0],xyz_list_front);

	/*Start looping on Gaussian points*/
	Gauss* gauss=element->NewGauss(xyz_list,xyz_list_front,3);
	for(int ig=gauss->begin();ig<gauss->end();ig++){

		gauss->GaussPoint(ig);
		thickness_input->GetInputValue(&thickness,gauss);
		base_input->GetInputValue(&bed,gauss);
		element->JacobianDeterminantSurface(&Jdet,xyz_list_front,gauss);
		element->NodalFunctions(basis,gauss);

		surface_under_water = min(0.,thickness+bed); // 0 if the top of the glacier is above water level
		base_under_water    = min(0.,bed);           // 0 if the bottom of the glacier is above water level
		water_pressure = 1.0/2.0*gravity*rho_water*(surface_under_water*surface_under_water - base_under_water*base_under_water);
		ice_pressure   = 1.0/2.0*gravity*rho_ice*thickness*thickness;
		pressure = ice_pressure + water_pressure;

		for(int i=0;i<numnodes;i++){
			pe->values[dim*i+0]+= pressure*Jdet*gauss->weight*normal[0]*basis[i];
			if(dim==2) pe->values[dim*i+1]+= pressure*Jdet*gauss->weight*normal[1]*basis[i];
		}
	}

	/*Transform coordinate system*/
	if(dim==2) element->TransformLoadVectorCoord(pe,XYEnum);

	/*Clean up and return*/
	xDelete<IssmDouble>(xyz_list);
	xDelete<IssmDouble>(xyz_list_front);
	xDelete<IssmDouble>(basis);
	delete gauss;
	return pe;
}/*}}}*/
void StressbalanceAnalysis::GetBSSA(IssmDouble* B,Element* element,int dim,IssmDouble* xyz_list,Gauss* gauss){/*{{{*/
	/*Compute B  matrix. B=[B1 B2 B3] where Bi is of size 3*NDOF2. 
	 * For node i, Bi can be expressed in the actual coordinate system
	 * by: 
	 *                   2D                      1D
	 *       Bi=[ dN/dx           0    ]   Bi=[ dN/dx ]
	 *          [   0           dN/dy  ]      
	 *          [ 1/2*dN/dy  1/2*dN/dx ]     
	 * where N is the finiteelement function for node i.
	 *
	 * We assume B has been allocated already, of size: 3x(NDOF2*numnodes)
	 */

	/*Fetch number of nodes for this finite element*/
	int numnodes = element->GetNumberOfNodes();

	/*Get nodal functions derivatives*/
	IssmDouble* dbasis=xNew<IssmDouble>(dim*numnodes);
	element->NodalFunctionsDerivatives(dbasis,xyz_list,gauss);

	/*Build B: */
	if(dim==2){
		for(int i=0;i<numnodes;i++){
			B[2*numnodes*0+2*i+0] = dbasis[0*numnodes+i];
			B[2*numnodes*0+2*i+1] = 0.;
			B[2*numnodes*1+2*i+0] = 0.;
			B[2*numnodes*1+2*i+1] = dbasis[1*numnodes+i];
			B[2*numnodes*2+2*i+0] = .5*dbasis[1*numnodes+i];
			B[2*numnodes*2+2*i+1] = .5*dbasis[0*numnodes+i];
		}
	}
	else{
		for(int i=0;i<numnodes;i++){
			B[i] = dbasis[i];
		}
	}

	/*Clean-up*/
	xDelete<IssmDouble>(dbasis);
}/*}}}*/
void StressbalanceAnalysis::GetBSSAFriction(IssmDouble* B,Element* element,int dim,IssmDouble* xyz_list,Gauss* gauss){/*{{{*/
	/*Compute B  matrix. B=[B1 B2 B3] where Bi is square and of size 2. 
	 * For node i, Bi can be expressed in the actual coordinate system
	 * by: 
	 *                       2D             1D
	 *                 Bi=[ N   0 ]    Bi = N
	 *                    [ 0   N ]
	 * where N is the finiteelement function for node i.
	 *
	 * We assume B has been allocated already, of size: 2 x (numdof*numnodes)
	 */

	/*Fetch number of nodes for this finite element*/
	int numnodes = element->GetNumberOfNodes();

	/*Get nodal functions derivatives*/
	IssmDouble* basis=xNew<IssmDouble>(numnodes);
	element->NodalFunctions(basis,gauss);

	/*Build L: */
	if(dim==2){
		for(int i=0;i<numnodes;i++){
			B[2*numnodes*0+2*i+0] = basis[i];
			B[2*numnodes*0+2*i+1] = 0.;
			B[2*numnodes*1+2*i+0] = 0.;
			B[2*numnodes*1+2*i+1] = basis[i];
		}
	}
	else{
		for(int i=0;i<numnodes;i++){
			B[i] = basis[i];
		}
	}

	/*Clean-up*/
	xDelete<IssmDouble>(basis);
}/*}}}*/
void StressbalanceAnalysis::GetBSSAprime(IssmDouble* Bprime,Element* element,int dim,IssmDouble* xyz_list,Gauss* gauss){/*{{{*/
	/*Compute B'  matrix. B'=[B1' B2' B3'] where Bi' is of size 3*NDOF2. 
	 * For node i, Bi' can be expressed in the actual coordinate system
	 * by: 
	 *                         2D                        1D
	 *       Bi_prime=[ 2*dN/dx    dN/dy ]     Bi_prime=[ 2*dN/dx ]
	 *                [   dN/dx  2*dN/dy ]
	 *                [   dN/dy    dN/dx ]
	 * where hNis the finiteelement function for node i.
	 *
	 * We assume B' has been allocated already, of size: 3x(NDOF2*numnodes)
	 */

	/*Fetch number of nodes for this finite element*/
	int numnodes = element->GetNumberOfNodes();

	/*Get nodal functions derivatives*/
	IssmDouble* dbasis=xNew<IssmDouble>(dim*numnodes);
	element->NodalFunctionsDerivatives(dbasis,xyz_list,gauss);

	/*Build B': */
	if(dim==2){
		for(int i=0;i<numnodes;i++){
			Bprime[2*numnodes*0+2*i+0] = 2.*dbasis[0*numnodes+i];
			Bprime[2*numnodes*0+2*i+1] =    dbasis[1*numnodes+i];
			Bprime[2*numnodes*1+2*i+0] =    dbasis[0*numnodes+i];
			Bprime[2*numnodes*1+2*i+1] = 2.*dbasis[1*numnodes+i];
			Bprime[2*numnodes*2+2*i+0] =    dbasis[1*numnodes+i];
			Bprime[2*numnodes*2+2*i+1] =    dbasis[0*numnodes+i];
		}
	}
	else{
		for(int i=0;i<numnodes;i++){
			Bprime[i] = 2.*dbasis[i];
		}
	}

	/*Clean-up*/
	xDelete<IssmDouble>(dbasis);
}/*}}}*/
void StressbalanceAnalysis::InputUpdateFromSolutionSSA(IssmDouble* solution,Element* element){/*{{{*/

	int         i,dim,meshtype;
	IssmDouble  rho_ice,g;
	int*        doflist=NULL;
	IssmDouble* xyz_list=NULL;
	Element*    basalelement=NULL;

	/*Deal with pressure first*/
	int numvertices = element->GetNumberOfVertices();
	IssmDouble* pressure  = xNew<IssmDouble>(numvertices);
	IssmDouble* thickness = xNew<IssmDouble>(numvertices);
	IssmDouble* surface   = xNew<IssmDouble>(numvertices);

	element->FindParam(&meshtype,MeshTypeEnum);
	rho_ice =element->GetMaterialParameter(MaterialsRhoIceEnum);
	g       =element->GetMaterialParameter(ConstantsGEnum);
	switch(meshtype){
		case Mesh2DhorizontalEnum:
			element->GetInputListOnVertices(thickness,ThicknessEnum);
			for(i=0;i<numvertices;i++) pressure[i]=rho_ice*g*thickness[i];
			dim=2;
			break;
		case Mesh3DEnum:
			element->GetVerticesCoordinates(&xyz_list);
			element->GetInputListOnVertices(surface,SurfaceEnum);
			for(i=0;i<numvertices;i++) pressure[i]=rho_ice*g*(surface[i]-xyz_list[i*3+2]);
			dim=2;
			break;
		case Mesh2DverticalEnum:
			element->GetVerticesCoordinates(&xyz_list);
			element->GetInputListOnVertices(surface,SurfaceEnum);
			for(i=0;i<numvertices;i++) pressure[i]=rho_ice*g*(surface[i]-xyz_list[i*3+2]);
			dim=1;
			break;
		default: _error_("mesh "<<EnumToStringx(meshtype)<<" not supported yet");
	}
	element->AddInput(PressureEnum,pressure,P1Enum);
	xDelete<IssmDouble>(pressure);
	xDelete<IssmDouble>(thickness);
	xDelete<IssmDouble>(surface);

	/*Get basal element*/
	switch(meshtype){
		case Mesh2DhorizontalEnum:
			basalelement = element;
			break;
		case Mesh3DEnum: case Mesh2DverticalEnum:
			if(!element->IsOnBed()){xDelete<IssmDouble>(xyz_list); return;}
			basalelement=element->SpawnBasalElement();
			break;
		default: _error_("mesh "<<EnumToStringx(meshtype)<<" not supported yet");
	}

	/*Fetch number of nodes and dof for this finite element*/
	int numnodes = basalelement->GetNumberOfNodes();
	int numdof   = numnodes*dim;

	/*Fetch dof list and allocate solution vectors*/
	basalelement->GetDofList(&doflist,SSAApproximationEnum,GsetEnum);
	IssmDouble* values    = xNew<IssmDouble>(numdof);
	IssmDouble* vx        = xNew<IssmDouble>(numnodes);
	IssmDouble* vy        = xNew<IssmDouble>(numnodes);
	IssmDouble* vz        = xNew<IssmDouble>(numnodes);
	IssmDouble* vel       = xNew<IssmDouble>(numnodes);

	/*Use the dof list to index into the solution vector: */
	for(i=0;i<numdof;i++) values[i]=solution[doflist[i]];

	/*Transform solution in Cartesian Space*/
	if(dim==2) basalelement->TransformSolutionCoord(&values[0],XYEnum);

	/*Ok, we have vx and vy in values, fill in vx and vy arrays: */
	for(i=0;i<numnodes;i++){
		vx[i]=values[i*dim+0];
		if(xIsNan<IssmDouble>(vx[i])) _error_("NaN found in solution vector");

		if(dim==2){
			vy[i]=values[i*dim+1];
			if(xIsNan<IssmDouble>(vy[i])) _error_("NaN found in solution vector");
		}
	}

	/*Get Vz and compute vel*/
	if(dim==2){
		basalelement->GetInputListOnNodes(&vz[0],VzEnum,0.);
		for(i=0;i<numnodes;i++) vel[i]=sqrt(vx[i]*vx[i] + vy[i]*vy[i] + vz[i]*vz[i]);
	}
	else{
		basalelement->GetInputListOnNodes(&vy[0],VyEnum,0.);
		for(i=0;i<numnodes;i++) vel[i]=sqrt(vx[i]*vx[i] + vy[i]*vy[i]);
	}

	/*Now, we have to move the previous Vx and Vy inputs  to old 
	 * status, otherwise, we'll wipe them off: */
	element->InputChangeName(VxEnum,VxPicardEnum);
	if(dim==2)element->InputChangeName(VyEnum,VyPicardEnum);

	/*Add vx and vy as inputs to the tria element: */
	element->AddBasalInput(VxEnum,vx,P1Enum);
	if(dim==2)element->AddBasalInput(VyEnum,vy,P1Enum);
	element->AddBasalInput(VelEnum,vel,P1Enum);

	/*Free ressources:*/
	xDelete<IssmDouble>(vel);
	xDelete<IssmDouble>(vz);
	xDelete<IssmDouble>(vy);
	xDelete<IssmDouble>(vx);
	xDelete<IssmDouble>(values);
	xDelete<IssmDouble>(xyz_list);
	xDelete<int>(doflist);
	if(meshtype!=Mesh2DhorizontalEnum){basalelement->DeleteMaterials(); delete basalelement;};
}/*}}}*/

/*L1L2*/
ElementMatrix* StressbalanceAnalysis::CreateKMatrixL1L2(Element* element){/*{{{*/

	/*compute all stiffness matrices for this element*/
	ElementMatrix* Ke1=CreateKMatrixL1L2Viscous(element);
	ElementMatrix* Ke2=CreateKMatrixL1L2Friction(element);
	ElementMatrix* Ke =new ElementMatrix(Ke1,Ke2);

	/*clean-up and return*/
	delete Ke1;
	delete Ke2;
	return Ke;
}/*}}}*/
ElementMatrix* StressbalanceAnalysis::CreateKMatrixL1L2Friction(Element* element){/*{{{*/

	if(!element->IsOnBed() || element->IsFloating()) return NULL;
	Element* basalelement = element->SpawnBasalElement();
	ElementMatrix* Ke = CreateKMatrixSSAFriction(basalelement);

	/*clean-up and return*/
	basalelement->DeleteMaterials(); delete basalelement;
	return Ke;
}/*}}}*/
ElementMatrix* StressbalanceAnalysis::CreateKMatrixL1L2Viscous(Element* element){/*{{{*/

	/*Intermediaries*/
	IssmDouble  viscosity,Jdet;
	IssmDouble *xyz_list = NULL;

	/*Get element on base*/
	Element* basalelement = element->GetBasalElement()->SpawnBasalElement();

	/*Fetch number of nodes and dof for this finite element*/
	int numnodes = basalelement->GetNumberOfNodes();
	int numdof   = numnodes*2;

	/*Initialize Element matrix and vectors*/
	ElementMatrix* Ke     = basalelement->NewElementMatrix(L1L2ApproximationEnum);
	IssmDouble*    B      = xNew<IssmDouble>(3*numdof);
	IssmDouble*    Bprime = xNew<IssmDouble>(3*numdof);
	IssmDouble*    D      = xNewZeroInit<IssmDouble>(3*3);

	/*Retrieve all inputs and parameters*/
	element->GetVerticesCoordinates(&xyz_list);
	Input* surface_input = element->GetInput(SurfaceEnum); _assert_(surface_input);
	Input* vx_input      = element->GetInput(VxEnum);      _assert_(vx_input);
	Input* vy_input      = element->GetInput(VyEnum);      _assert_(vy_input);

	/* Start  looping on the number of gaussian points: */
	Gauss* gauss      = element->NewGauss(5);
	Gauss* gauss_base = basalelement->NewGauss();
	for(int ig=gauss->begin();ig<gauss->end();ig++){

		gauss->GaussPoint(ig);
		gauss->SynchronizeGaussBase(gauss_base);

		element->JacobianDeterminant(&Jdet,xyz_list,gauss);
		this->GetBSSA(B,basalelement,2,xyz_list,gauss_base);
		this->GetBSSAprime(Bprime,basalelement,2,xyz_list,gauss_base);

		element->ViscosityL1L2(&viscosity,xyz_list,gauss,vx_input,vy_input,surface_input);

		for(int i=0;i<3;i++) D[i*3+i]=2*viscosity*gauss->weight*Jdet;

		TripleMultiply(B,3,numdof,1,
					D,3,3,0,
					Bprime,3,numdof,0,
					&Ke->values[0],1);
	}

	/*Transform Coordinate System*/
	basalelement->TransformStiffnessMatrixCoord(Ke,XYEnum);

	/*Clean up and return*/
	delete gauss;
	delete gauss_base;
	basalelement->DeleteMaterials(); delete basalelement;
	xDelete<IssmDouble>(xyz_list);
	xDelete<IssmDouble>(D);
	xDelete<IssmDouble>(Bprime);
	xDelete<IssmDouble>(B);
	return Ke;
}/*}}}*/
ElementVector* StressbalanceAnalysis::CreatePVectorL1L2(Element* element){/*{{{*/

	/*Intermediaries*/
	int      meshtype;
	Element* basalelement;

	/*Get basal element*/
	element->FindParam(&meshtype,MeshTypeEnum);
	switch(meshtype){
		case Mesh2DhorizontalEnum:
			basalelement = element;
			break;
		case Mesh3DEnum:
			if(!element->IsOnBed()) return NULL;
			basalelement = element->SpawnBasalElement();
			break;
		default: _error_("mesh "<<EnumToStringx(meshtype)<<" not supported yet");
	}

	/*compute all load vectors for this element*/
	ElementVector* pe1=CreatePVectorL1L2DrivingStress(basalelement);
	ElementVector* pe2=CreatePVectorL1L2Front(basalelement);
	ElementVector* pe =new ElementVector(pe1,pe2);

	/*clean-up and return*/
	if(meshtype!=Mesh2DhorizontalEnum){basalelement->DeleteMaterials(); delete basalelement;};
	delete pe1;
	delete pe2;
	return pe;
}/*}}}*/
ElementVector* StressbalanceAnalysis::CreatePVectorL1L2DrivingStress(Element* element){/*{{{*/

	/*Intermediaries */
	IssmDouble  thickness,Jdet,slope[2];
	IssmDouble* xyz_list = NULL;

	/*Fetch number of nodes and dof for this finite element*/
	int numnodes = element->GetNumberOfNodes();

	/*Initialize Element vector and vectors*/
	ElementVector* pe    = element->NewElementVector(L1L2ApproximationEnum);
	IssmDouble*    basis = xNew<IssmDouble>(numnodes);

	/*Retrieve all inputs and parameters*/
	element->GetVerticesCoordinates(&xyz_list);
	Input*     thickness_input=element->GetInput(ThicknessEnum); _assert_(thickness_input); 
	Input*     surface_input  =element->GetInput(SurfaceEnum);   _assert_(surface_input);
	IssmDouble rhog = element->GetMaterialParameter(MaterialsRhoIceEnum)*element->GetMaterialParameter(ConstantsGEnum);

	/* Start  looping on the number of gaussian points: */
	Gauss* gauss=element->NewGauss(2);
	for(int ig=gauss->begin();ig<gauss->end();ig++){

		gauss->GaussPoint(ig);

		element->JacobianDeterminant(&Jdet,xyz_list,gauss);
		element->NodalFunctions(basis, gauss);

		thickness_input->GetInputValue(&thickness,gauss);
		surface_input->GetInputDerivativeValue(&slope[0],xyz_list,gauss);

		for(int i=0;i<numnodes;i++){
			pe->values[i*2+0]+=-rhog*thickness*slope[0]*Jdet*gauss->weight*basis[i];
			pe->values[i*2+1]+=-rhog*thickness*slope[1]*Jdet*gauss->weight*basis[i];
		}
	}

	/*Transform coordinate system*/
	element->TransformLoadVectorCoord(pe,XYEnum);

	/*Clean up and return*/
	xDelete<IssmDouble>(xyz_list);
	xDelete<IssmDouble>(basis);
	delete gauss;
	return pe;
}/*}}}*/
ElementVector* StressbalanceAnalysis::CreatePVectorL1L2Front(Element* element){/*{{{*/

	/*If no front, return NULL*/
	if(!element->IsIcefront()) return NULL;

	/*Intermediaries*/
	IssmDouble  Jdet,thickness,bed,water_pressure,ice_pressure;
	IssmDouble  surface_under_water,base_under_water,pressure;
	IssmDouble *xyz_list = NULL;
	IssmDouble *xyz_list_front = NULL;
	IssmDouble  normal[2];

	/*Fetch number of nodes for this finite element*/
	int numnodes = element->GetNumberOfNodes();

	/*Initialize Element vector and other vectors*/
	ElementVector* pe    = element->NewElementVector(L1L2ApproximationEnum);
	IssmDouble*    basis = xNew<IssmDouble>(numnodes);

	/*Retrieve all inputs and parameters*/
	Input* thickness_input = element->GetInput(ThicknessEnum); _assert_(thickness_input);
	Input* base_input       = element->GetInput(BaseEnum);       _assert_(base_input);
	IssmDouble rho_water   = element->GetMaterialParameter(MaterialsRhoWaterEnum);
	IssmDouble rho_ice     = element->GetMaterialParameter(MaterialsRhoIceEnum);
	IssmDouble gravity     = element->GetMaterialParameter(ConstantsGEnum);
	element->GetVerticesCoordinates(&xyz_list);
	element->ZeroLevelsetCoordinates(&xyz_list_front,xyz_list,MaskIceLevelsetEnum);
	element->NormalSection(&normal[0],xyz_list_front);

	/*Start looping on Gaussian points*/
	Gauss* gauss=element->NewGauss(xyz_list,xyz_list_front,3);
	for(int ig=gauss->begin();ig<gauss->end();ig++){

		gauss->GaussPoint(ig);
		thickness_input->GetInputValue(&thickness,gauss);
		base_input->GetInputValue(&bed,gauss);
		element->JacobianDeterminantSurface(&Jdet,xyz_list_front,gauss);
		element->NodalFunctions(basis,gauss);

		surface_under_water = min(0.,thickness+bed); // 0 if the top of the glacier is above water level
		base_under_water    = min(0.,bed);           // 0 if the bottom of the glacier is above water level
		water_pressure = 1.0/2.0*gravity*rho_water*(surface_under_water*surface_under_water - base_under_water*base_under_water);
		ice_pressure   = 1.0/2.0*gravity*rho_ice*thickness*thickness;
		pressure = ice_pressure + water_pressure;

		for (int i=0;i<numnodes;i++){
			pe->values[2*i+0]+= pressure*Jdet*gauss->weight*normal[0]*basis[i];
			pe->values[2*i+1]+= pressure*Jdet*gauss->weight*normal[1]*basis[i];
		}
	}

	/*Transform coordinate system*/
	element->TransformLoadVectorCoord(pe,XYEnum);

	/*Clean up and return*/
	xDelete<IssmDouble>(xyz_list);
	xDelete<IssmDouble>(xyz_list_front);
	xDelete<IssmDouble>(basis);
	delete gauss;
	return pe;
}/*}}}*/
void StressbalanceAnalysis::InputUpdateFromSolutionL1L2(IssmDouble* solution,Element* element){/*{{{*/

	int         i,meshtype;
	IssmDouble  rho_ice,g;
	int*        doflist=NULL;
	IssmDouble* xyz_list=NULL;
	Element*    basalelement=NULL;

	/*Deal with pressure first*/
	int numvertices = element->GetNumberOfVertices();
	IssmDouble* pressure  = xNew<IssmDouble>(numvertices);
	IssmDouble* thickness = xNew<IssmDouble>(numvertices);
	IssmDouble* surface   = xNew<IssmDouble>(numvertices);

	element->FindParam(&meshtype,MeshTypeEnum);
	rho_ice =element->GetMaterialParameter(MaterialsRhoIceEnum);
	g       =element->GetMaterialParameter(ConstantsGEnum);
	switch(meshtype){
		case Mesh2DhorizontalEnum:
			element->GetInputListOnVertices(thickness,ThicknessEnum);
			for(i=0;i<numvertices;i++) pressure[i]=rho_ice*g*thickness[i];
			break;
		case Mesh3DEnum:
			element->GetVerticesCoordinates(&xyz_list);
			element->GetInputListOnVertices(surface,SurfaceEnum);
			for(i=0;i<numvertices;i++) pressure[i]=rho_ice*g*(surface[i]-xyz_list[i*3+2]);
			break;
		default: _error_("mesh "<<EnumToStringx(meshtype)<<" not supported yet");
	}
	element->AddInput(PressureEnum,pressure,P1Enum);
	xDelete<IssmDouble>(pressure);
	xDelete<IssmDouble>(thickness);
	xDelete<IssmDouble>(surface);

	/*Get basal element*/
	switch(meshtype){
		case Mesh2DhorizontalEnum:
			basalelement = element;
			break;
		case Mesh3DEnum:
			if(!element->IsOnBed()){xDelete<IssmDouble>(xyz_list); return;}
			basalelement=element->SpawnBasalElement();
			break;
		default: _error_("mesh "<<EnumToStringx(meshtype)<<" not supported yet");
	}

	/*Fetch number of nodes and dof for this finite element*/
	int numnodes = basalelement->GetNumberOfNodes();
	int numdof   = numnodes*2;

	/*Fetch dof list and allocate solution vectors*/
	basalelement->GetDofList(&doflist,NoneApproximationEnum,GsetEnum);
	IssmDouble* values    = xNew<IssmDouble>(numdof);
	IssmDouble* vx        = xNew<IssmDouble>(numnodes);
	IssmDouble* vy        = xNew<IssmDouble>(numnodes);
	IssmDouble* vz        = xNew<IssmDouble>(numnodes);
	IssmDouble* vel       = xNew<IssmDouble>(numnodes);

	/*Use the dof list to index into the solution vector: */
	for(i=0;i<numdof;i++) values[i]=solution[doflist[i]];

	/*Transform solution in Cartesian Space*/
	basalelement->TransformSolutionCoord(&values[0],XYEnum);
	basalelement->FindParam(&meshtype,MeshTypeEnum);

	/*Ok, we have vx and vy in values, fill in vx and vy arrays: */
	for(i=0;i<numnodes;i++){
		vx[i]=values[i*2+0];
		vy[i]=values[i*2+1];

		/*Check solution*/
		if(xIsNan<IssmDouble>(vx[i])) _error_("NaN found in solution vector");
		if(xIsNan<IssmDouble>(vy[i])) _error_("NaN found in solution vector");
	}

	/*Get Vz and compute vel*/
	basalelement->GetInputListOnNodes(&vz[0],VzEnum,0.);
	for(i=0;i<numnodes;i++) vel[i]=sqrt(vx[i]*vx[i] + vy[i]*vy[i] + vz[i]*vz[i]);

	/*Now, we have to move the previous Vx and Vy inputs  to old 
	 * status, otherwise, we'll wipe them off: */
	element->InputChangeName(VxEnum,VxPicardEnum);
	element->InputChangeName(VyEnum,VyPicardEnum);

	/*Add vx and vy as inputs to the tria element: */
	element->AddBasalInput(VxEnum,vx,P1Enum);
	element->AddBasalInput(VyEnum,vy,P1Enum);
	element->AddBasalInput(VelEnum,vel,P1Enum);

	/*Free ressources:*/
	xDelete<IssmDouble>(vel);
	xDelete<IssmDouble>(vz);
	xDelete<IssmDouble>(vy);
	xDelete<IssmDouble>(vx);
	xDelete<IssmDouble>(values);
	xDelete<IssmDouble>(xyz_list);
	xDelete<int>(doflist);
	if(meshtype!=Mesh2DhorizontalEnum){basalelement->DeleteMaterials(); delete basalelement;};
}/*}}}*/

/*HO*/
ElementMatrix* StressbalanceAnalysis::CreateJacobianMatrixHO(Element* element){/*{{{*/

	/*Intermediaries */
	IssmDouble Jdet;
	IssmDouble eps1dotdphii,eps1dotdphij;
	IssmDouble eps2dotdphii,eps2dotdphij;
	IssmDouble mu_prime;
	IssmDouble epsilon[5]; /* epsilon=[exx,eyy,exy,exz,eyz];*/
	IssmDouble eps1[3],eps2[3];
	IssmDouble *xyz_list = NULL;

	/*Fetch number of nodes and dof for this finite element*/
	int numnodes = element->GetNumberOfNodes();

	/*Initialize Element matrix, vectors and Gaussian points*/
	ElementMatrix* Ke=this->CreateKMatrixHO(element); //Initialize Jacobian with regular HO (first part of the Gateau derivative)
	IssmDouble*    dbasis = xNew<IssmDouble>(3*numnodes);

	/*Retrieve all inputs and parameters*/
	element->GetVerticesCoordinates(&xyz_list);
	Input* vx_input = element->GetInput(VxEnum); _assert_(vx_input);
	Input* vy_input = element->GetInput(VyEnum); _assert_(vy_input);

	/* Start  looping on the number of gaussian points: */
	Gauss* gauss = element->NewGauss(5);
	for(int ig=gauss->begin();ig<gauss->end();ig++){
		gauss->GaussPoint(ig);

		element->JacobianDeterminant(&Jdet,xyz_list,gauss);
		element->NodalFunctionsDerivatives(dbasis,xyz_list,gauss);

		element->StrainRateHO(&epsilon[0],xyz_list,gauss,vx_input,vy_input);
		element->ViscosityHODerivativeEpsSquare(&mu_prime,&epsilon[0]);
		eps1[0]=2*epsilon[0]+epsilon[1];   eps2[0]=epsilon[2];
		eps1[1]=epsilon[2];                eps2[1]=epsilon[0]+2*epsilon[1];
		eps1[2]=epsilon[3];                eps2[2]=epsilon[4];

		for(int i=0;i<numnodes;i++){
			for(int j=0;j<numnodes;j++){
				eps1dotdphii=eps1[0]*dbasis[0*numnodes+i]+eps1[1]*dbasis[1*numnodes+i]+eps1[2]*dbasis[2*numnodes+i];
				eps1dotdphij=eps1[0]*dbasis[0*numnodes+j]+eps1[1]*dbasis[1*numnodes+j]+eps1[2]*dbasis[2*numnodes+j];
				eps2dotdphii=eps2[0]*dbasis[0*numnodes+i]+eps2[1]*dbasis[1*numnodes+i]+eps2[2]*dbasis[2*numnodes+i];
				eps2dotdphij=eps2[0]*dbasis[0*numnodes+j]+eps2[1]*dbasis[1*numnodes+j]+eps2[2]*dbasis[2*numnodes+j];

				Ke->values[2*numnodes*(2*i+0)+2*j+0]+=gauss->weight*Jdet*2.*mu_prime*eps1dotdphij*eps1dotdphii;
				Ke->values[2*numnodes*(2*i+0)+2*j+1]+=gauss->weight*Jdet*2.*mu_prime*eps2dotdphij*eps1dotdphii;
				Ke->values[2*numnodes*(2*i+1)+2*j+0]+=gauss->weight*Jdet*2.*mu_prime*eps1dotdphij*eps2dotdphii;
				Ke->values[2*numnodes*(2*i+1)+2*j+1]+=gauss->weight*Jdet*2.*mu_prime*eps2dotdphij*eps2dotdphii;
			}
		}
	}

	/*Transform Coordinate System*/
	element->TransformStiffnessMatrixCoord(Ke,XYEnum);

	/*Clean up and return*/
	delete gauss;
	xDelete<IssmDouble>(xyz_list);
	xDelete<IssmDouble>(dbasis);
	return Ke;
}/*}}}*/
ElementMatrix* StressbalanceAnalysis::CreateKMatrixHO(Element* element){/*{{{*/

	/* Check if ice in element */
	if(!element->IsIceInElement()) return NULL;

	/*compute all stiffness matrices for this element*/
	ElementMatrix* Ke1=CreateKMatrixHOViscous(element);
	ElementMatrix* Ke2=CreateKMatrixHOFriction(element);
	ElementMatrix* Ke =new ElementMatrix(Ke1,Ke2);

	/*clean-up and return*/
	delete Ke1;
	delete Ke2;
	return Ke;
}/*}}}*/
ElementMatrix* StressbalanceAnalysis::CreateKMatrixHOViscous(Element* element){/*{{{*/

	/* Check if ice in element */
	if(!element->IsIceInElement()) return NULL;

	/*Intermediaries*/
	int         dim,meshtype,bsize;
	IssmDouble  viscosity,newviscosity,oldviscosity;
	IssmDouble  viscosity_overshoot,thickness,Jdet;
	IssmDouble  D_scalar;
	IssmDouble *xyz_list = NULL;

	/*Get problem dimension*/
	element->FindParam(&meshtype,MeshTypeEnum);
	switch(meshtype){
		case Mesh2DverticalEnum: dim = 2; bsize = 2; break;
		case Mesh3DEnum:         dim = 3; bsize = 5; break;
		case Mesh3DtetrasEnum:   dim = 3; bsize = 5; break;
		default: _error_("mesh "<<EnumToStringx(meshtype)<<" not supported yet");
	}

	/*Fetch number of nodes and dof for this finite element*/
	int numnodes = element->GetNumberOfNodes();
	int numdof   = numnodes*(dim-1);

	/*Initialize Element matrix and vectors*/
	ElementMatrix* Ke     = element->NewElementMatrix(HOApproximationEnum);
	IssmDouble*    B      = xNew<IssmDouble>(bsize*numdof);
	IssmDouble*    Bprime = xNew<IssmDouble>(bsize*numdof);
	IssmDouble*    D      = xNewZeroInit<IssmDouble>(bsize*bsize);

	/*Retrieve all inputs and parameters*/
	element->GetVerticesCoordinates(&xyz_list);
	Input* vx_input    = element->GetInput(VxEnum);       _assert_(vx_input);
	Input* vxold_input = element->GetInput(VxPicardEnum); _assert_(vxold_input);
	Input* vy_input    = NULL;
	Input* vyold_input = NULL;
	if(dim==3){
		vy_input=element->GetInput(VyEnum);          _assert_(vy_input);
		vyold_input=element->GetInput(VyPicardEnum); _assert_(vyold_input);
	}
	element->FindParam(&viscosity_overshoot,StressbalanceViscosityOvershootEnum);

	/* Start  looping on the number of gaussian points: */
	Gauss* gauss = element->NewGauss(5);
	for(int ig=gauss->begin();ig<gauss->end();ig++){
		gauss->GaussPoint(ig);

		element->JacobianDeterminant(&Jdet,xyz_list,gauss);
		this->GetBHO(B,element,dim,xyz_list,gauss);
		this->GetBHOprime(Bprime,element,dim,xyz_list,gauss);

		element->ViscosityHO(&viscosity,dim,xyz_list,gauss,vx_input,vy_input);
		element->ViscosityHO(&oldviscosity,dim,xyz_list,gauss,vxold_input,vyold_input);

		newviscosity=viscosity+viscosity_overshoot*(viscosity-oldviscosity);
		D_scalar=2.*newviscosity*gauss->weight*Jdet;
		for(int i=0;i<bsize;i++) D[i*bsize+i]=D_scalar;

		TripleMultiply(B,bsize,numdof,1,
					D,bsize,bsize,0,
					Bprime,bsize,numdof,0,
					&Ke->values[0],1);
	}

	/*Transform Coordinate System*/
	if(dim==3) element->TransformStiffnessMatrixCoord(Ke,XYEnum);

	/*Clean up and return*/
	delete gauss;
	xDelete<IssmDouble>(xyz_list);
	xDelete<IssmDouble>(D);
	xDelete<IssmDouble>(Bprime);
	xDelete<IssmDouble>(B);
	return Ke;
}/*}}}*/
ElementMatrix* StressbalanceAnalysis::CreateKMatrixHOFriction(Element* element){/*{{{*/

	/* Check if ice in element */
	if(!element->IsIceInElement()) return NULL;

	if(element->IsFloating() || !element->IsOnBed()) return NULL;

	/*Intermediaries*/
	int         dim,meshtype;
	bool        mainlyfloating;
	int         migration_style,point1;
	IssmDouble  alpha2,Jdet,fraction1,fraction2;
	IssmDouble  gllevelset,phi=1.;
	IssmDouble *xyz_list_base = NULL;
	Gauss*      gauss         = NULL;

	/*Get problem dimension*/
	element->FindParam(&meshtype,MeshTypeEnum);
	switch(meshtype){
		case Mesh2DverticalEnum: dim = 2; break;
		case Mesh3DEnum:         dim = 3; break;
		case Mesh3DtetrasEnum:   dim = 3; break;
		default: _error_("mesh "<<EnumToStringx(meshtype)<<" not supported yet");
	}

	/*Fetch number of nodes and dof for this finite element*/
	int numnodes = element->GetNumberOfNodes();
	int numdof   = numnodes*(dim-1);

	/*Initialize Element matrix and vectors*/
	ElementMatrix* Ke = element->NewElementMatrix(HOApproximationEnum);
	IssmDouble*    B  = xNew<IssmDouble>((dim-1)*numdof);
	IssmDouble*    D  = xNewZeroInit<IssmDouble>((dim-1)*(dim-1));

	/*Retrieve all inputs and parameters*/
	element->GetVerticesCoordinatesBase(&xyz_list_base);
	element->FindParam(&migration_style,GroundinglineMigrationEnum);
	Input* vx_input = element->GetInput(VxEnum);   _assert_(vx_input);
	Input* vy_input = element->GetInput(VyEnum);   _assert_(vy_input);
	Input* vz_input = NULL;
	Input* gllevelset_input = NULL;
	if(dim==3){vz_input=element->GetInput(VzEnum); _assert_(vz_input);}

	/*build friction object, used later on: */
	Friction* friction=new Friction(element,2);

	/*Recover portion of element that is grounded*/
	if(migration_style==SubelementMigrationEnum) phi=element->GetGroundedPortion(xyz_list_base);
	if(migration_style==SubelementMigration2Enum){
		gllevelset_input=element->GetInput(MaskGroundediceLevelsetEnum); _assert_(gllevelset_input);
		element->GetGroundedPart(&point1,&fraction1,&fraction2,&mainlyfloating);
		//gauss = element->NewGauss(point1,fraction1,fraction2,mainlyfloating,2);
		gauss=element->NewGaussBase(2);
	}
	else{
		gauss=element->NewGaussBase(2);
	}

	/* Start  looping on the number of gaussian points: */
	for(int ig=gauss->begin();ig<gauss->end();ig++){
		gauss->GaussPoint(ig);

		friction->GetAlpha2(&alpha2,gauss,vx_input,vy_input,vz_input);
		if(migration_style==SubelementMigrationEnum) alpha2=phi*alpha2;
		if(migration_style==SubelementMigration2Enum){
			gllevelset_input->GetInputValue(&gllevelset, gauss);
			if(gllevelset<0.) alpha2=0.;
		}

		this->GetBHOFriction(B,element,dim,xyz_list_base,gauss);
		element->JacobianDeterminantBase(&Jdet,xyz_list_base,gauss);
		for(int i=0;i<dim-1;i++) D[i*(dim-1)+i]=alpha2*gauss->weight*Jdet;

		TripleMultiply(B,dim-1,numdof,1,
					D,dim-1,dim-1,0,
					B,dim-1,numdof,0,
					&Ke->values[0],1);
	}

	/*Transform Coordinate System*/
	if(dim==3) element->TransformStiffnessMatrixCoord(Ke,XYEnum);

	/*Clean up and return*/
	delete gauss;
	delete friction;
	xDelete<IssmDouble>(xyz_list_base);
	xDelete<IssmDouble>(B);
	xDelete<IssmDouble>(D);
	return Ke;
}/*}}}*/
#ifdef FSANALYTICAL
ElementVector* StressbalanceAnalysis::CreatePVectorHO(Element* element){/*{{{*/

	/* Check if ice in element */
	if(!element->IsIceInElement()) return NULL;

	/*Intermediaries */
	int         dim,meshtype;
	IssmDouble  x_coord,y_coord,z_coord;
	IssmDouble  Jdet,forcex,forcey,forcez;
	IssmDouble* xyz_list = NULL;

	/*Get problem dimension*/
	element->FindParam(&meshtype,MeshTypeEnum);
	switch(meshtype){
		case Mesh2DverticalEnum: dim = 2; break;
		case Mesh3DEnum:         dim = 3; break;
		case Mesh3DtetrasEnum:   dim = 3; break;
		default: _error_("mesh "<<EnumToStringx(meshtype)<<" not supported yet");
	}

	/*Fetch number of nodes and dof for this finite element*/
	int numnodes = element->GetNumberOfNodes();

	/*Initialize Element vector and vectors*/
	ElementVector* pe=element->NewElementVector(HOApproximationEnum);
	IssmDouble*    basis = xNew<IssmDouble>(numnodes);

	/*Retrieve all inputs and parameters*/
	element->GetVerticesCoordinates(&xyz_list);

	/* Start  looping on the number of gaussian points: */
	Gauss* gauss=element->NewGauss(3);
	for(int ig=gauss->begin();ig<gauss->end();ig++){
		gauss->GaussPoint(ig);

		element->JacobianDeterminant(&Jdet,xyz_list,gauss);
		element->NodalFunctions(basis, gauss);

		x_coord=element->GetXcoord(gauss);
		y_coord=element->GetYcoord(gauss);
		if(dim==3) z_coord=element->GetZcoord(gauss);
		else z_coord=0.;

		forcex=fx(x_coord,y_coord,z_coord,FSANALYTICAL);
		forcey=fy(x_coord,y_coord,z_coord,FSANALYTICAL);

		for(int i=0;i<numnodes;i++){
			pe->values[i*(dim-1)+0]+=forcex*Jdet*gauss->weight*basis[i];
			pe->values[i*(dim-1)+1]+=forcey*Jdet*gauss->weight*basis[i];
		}
	}

	/*Transform coordinate system*/
	if(dim==3) element->TransformLoadVectorCoord(pe,XYEnum);

	/*Clean up and return*/
	xDelete<IssmDouble>(basis);
	xDelete<IssmDouble>(xyz_list);
	delete gauss;
	return pe;
}/*}}}*/
#else
ElementVector* StressbalanceAnalysis::CreatePVectorHO(Element* element){/*{{{*/

	/* Check if ice in element */
	if(!element->IsIceInElement()) return NULL;

	/*compute all load vectors for this element*/
	ElementVector* pe1=CreatePVectorHODrivingStress(element);
	ElementVector* pe2=CreatePVectorHOFront(element);
	ElementVector* pe =new ElementVector(pe1,pe2);

	/*clean-up and return*/
	delete pe1;
	delete pe2;
	return pe;
}/*}}}*/
#endif
ElementVector* StressbalanceAnalysis::CreatePVectorHODrivingStress(Element* element){/*{{{*/

	/* Check if ice in element */
	if(!element->IsIceInElement()) return NULL;

	/*Intermediaries */
	int         dim,meshtype;
	IssmDouble  Jdet,slope[3];
	IssmDouble* xyz_list = NULL;

	/*Get problem dimension*/
	element->FindParam(&meshtype,MeshTypeEnum);
	switch(meshtype){
		case Mesh2DverticalEnum: dim = 2; break;
		case Mesh3DEnum:         dim = 3; break;
		case Mesh3DtetrasEnum:   dim = 3; break;
		default: _error_("mesh "<<EnumToStringx(meshtype)<<" not supported yet");
	}

	/*Fetch number of nodes and dof for this finite element*/
	int numnodes = element->GetNumberOfNodes();

	/*Initialize Element vector and vectors*/
	ElementVector* pe=element->NewElementVector(HOApproximationEnum);
	IssmDouble*    basis = xNew<IssmDouble>(numnodes);

	/*Retrieve all inputs and parameters*/
	element->GetVerticesCoordinates(&xyz_list);
	Input*     surface_input = element->GetInput(SurfaceEnum);   _assert_(surface_input);
	IssmDouble rhog = element->GetMaterialParameter(MaterialsRhoIceEnum)*element->GetMaterialParameter(ConstantsGEnum);

	/* Start  looping on the number of gaussian points: */
	Gauss* gauss=element->NewGauss(3);
	for(int ig=gauss->begin();ig<gauss->end();ig++){
		gauss->GaussPoint(ig);

		element->JacobianDeterminant(&Jdet,xyz_list,gauss);
		element->NodalFunctions(basis, gauss);
		surface_input->GetInputDerivativeValue(&slope[0],xyz_list,gauss);

		for(int i=0;i<numnodes;i++){
			pe->values[i*(dim-1)+0]+=-rhog*slope[0]*Jdet*gauss->weight*basis[i];
			if(dim==3) pe->values[i*(dim-1)+1]+=-rhog*slope[1]*Jdet*gauss->weight*basis[i];
		}
	}

	/*Transform coordinate system*/
	if(dim==3) element->TransformLoadVectorCoord(pe,XYEnum);

	/*Clean up and return*/
	xDelete<IssmDouble>(basis);
	xDelete<IssmDouble>(xyz_list);
	delete gauss;
	return pe;
}/*}}}*/
ElementVector* StressbalanceAnalysis::CreatePVectorHOFront(Element* element){/*{{{*/

	/* Check if ice in element */
	if(!element->IsIceInElement()) return NULL;

	/*If no front, return NULL*/
	if(!element->IsIcefront()) return NULL;

	/*Intermediaries*/
	int         dim,meshtype;
	IssmDouble  Jdet,surface,z,water_pressure,ice_pressure;
	IssmDouble  surface_under_water,base_under_water,pressure;
	IssmDouble* xyz_list       = NULL;
	IssmDouble* xyz_list_front = NULL;
	IssmDouble  normal[3];
	Gauss*      gauss = NULL;

	/*Get problem dimension*/
	element->FindParam(&meshtype,MeshTypeEnum);
	switch(meshtype){
		case Mesh2DverticalEnum: dim = 2; break;
		case Mesh3DEnum:         dim = 3; break;
		case Mesh3DtetrasEnum:   dim = 3; break;
		default: _error_("mesh "<<EnumToStringx(meshtype)<<" not supported yet");
	}

	/*Fetch number of nodes and dof for this finite element*/
	int numnodes    = element->GetNumberOfNodes();
	int numvertices = element->GetNumberOfVertices();

	/*Initialize Element vector and other vectors*/
	ElementVector* pe    = element->NewElementVector(HOApproximationEnum);
	IssmDouble*    basis = xNew<IssmDouble>(numnodes);

	/*Retrieve all inputs and parameters*/
	Input* surface_input = element->GetInput(SurfaceEnum); _assert_(surface_input);
	IssmDouble rho_water = element->GetMaterialParameter(MaterialsRhoWaterEnum);
	IssmDouble rho_ice   = element->GetMaterialParameter(MaterialsRhoIceEnum);
	IssmDouble gravity   = element->GetMaterialParameter(ConstantsGEnum);
	element->GetVerticesCoordinates(&xyz_list);
	element->ZeroLevelsetCoordinates(&xyz_list_front,xyz_list,MaskIceLevelsetEnum);
	element->NormalSection(&normal[0],xyz_list_front);

	/*Initialize gauss points*/
	IssmDouble zmax=xyz_list[0*3+(dim-1)]; for(int i=1;i<numvertices;i++) if(xyz_list[i*3+(dim-1)]>zmax) zmax=xyz_list[i*3+(dim-1)];
	IssmDouble zmin=xyz_list[0*3+(dim-1)]; for(int i=1;i<numvertices;i++) if(xyz_list[i*3+(dim-1)]<zmin) zmin=xyz_list[i*3+(dim-1)];
	if(zmax>0. && zmin<0.) gauss=element->NewGauss(xyz_list,xyz_list_front,3,10);//refined in vertical because of the sea level discontinuity
	else                   gauss=element->NewGauss(xyz_list,xyz_list_front,3,3);

	/* Start  looping on the number of gaussian points: */
	for(int ig=gauss->begin();ig<gauss->end();ig++){

		gauss->GaussPoint(ig);
		surface_input->GetInputValue(&surface,gauss);
		if(dim==3) z=element->GetZcoord(gauss);
		else       z=element->GetYcoord(gauss);
		element->NodalFunctions(basis,gauss);
		element->JacobianDeterminantSurface(&Jdet,xyz_list_front,gauss);

		water_pressure = rho_water*gravity*min(0.,z);//0 if the gaussian point is above water level
		ice_pressure   = rho_ice*gravity*(surface-z);
		pressure       = ice_pressure + water_pressure;

		for (int i=0;i<numnodes;i++){
			pe->values[(dim-1)*i+0]+= pressure*Jdet*gauss->weight*normal[0]*basis[i];
			if(dim==3) pe->values[(dim-1)*i+1]+= pressure*Jdet*gauss->weight*normal[1]*basis[i];
		}
	}

	/*Transform coordinate system*/
	if(dim==3)element->TransformLoadVectorCoord(pe,XYEnum);

	/*Clean up and return*/
	xDelete<IssmDouble>(basis);
	xDelete<IssmDouble>(xyz_list);
	xDelete<IssmDouble>(xyz_list_front);
	delete gauss;
	return pe;
}/*}}}*/
void StressbalanceAnalysis::GetBHO(IssmDouble* B,Element* element,int dim,IssmDouble* xyz_list,Gauss* gauss){/*{{{*/
	/*Compute B  matrix. B=[B1 B2 B3 B4 B5 B6] where Bi is of size 5*NDOF2. 
	 * For node i, Bi can be expressed in the actual coordinate system
	 * by: 
	 *                   3D                        2D
	 *
	 *       Bi=[ dh/dx          0      ]  Bi=[ dh/dx]
	 *          [   0           dh/dy   ]     [ dh/dy]
	 *          [ 1/2*dh/dy  1/2*dh/dx  ]     
	 *          [ 1/2*dh/dz      0      ]    
	 *          [  0         1/2*dh/dz  ]   
	 * where h is the interpolation function for node i.
	 *
	 * We assume B has been allocated already, of size: 5x(NDOF2*numnodes)
	 */

	/*Fetch number of nodes for this finite element*/
	int numnodes = element->GetNumberOfNodes();

	/*Get nodal functions derivatives*/
	IssmDouble* dbasis=xNew<IssmDouble>(dim*numnodes);
	element->NodalFunctionsDerivatives(dbasis,xyz_list,gauss);

	/*Build B: */
	if(dim==2){
		for(int i=0;i<numnodes;i++){
			B[numnodes*0+i] = dbasis[0*numnodes+i];
			B[numnodes*1+i] = .5*dbasis[1*numnodes+i];
		}
	}
	else{
		for(int i=0;i<numnodes;i++){
			B[2*numnodes*0+2*i+0] = dbasis[0*numnodes+i];
			B[2*numnodes*0+2*i+1] = 0.;
			B[2*numnodes*1+2*i+0] = 0.;
			B[2*numnodes*1+2*i+1] = dbasis[1*numnodes+i];
			B[2*numnodes*2+2*i+0] = .5*dbasis[1*numnodes+i];
			B[2*numnodes*2+2*i+1] = .5*dbasis[0*numnodes+i];
			B[2*numnodes*3+2*i+0] = .5*dbasis[2*numnodes+i];
			B[2*numnodes*3+2*i+1] = 0.;
			B[2*numnodes*4+2*i+0] = 0.;
			B[2*numnodes*4+2*i+1] = .5*dbasis[2*numnodes+i];
		}
	}


	/*Clean-up*/
	xDelete<IssmDouble>(dbasis);
}/*}}}*/
void StressbalanceAnalysis::GetBHOprime(IssmDouble* Bprime,Element* element,int dim,IssmDouble* xyz_list,Gauss* gauss){/*{{{*/
	/*Compute B'  matrix. B'=[B1' B2' B3'] where Bi' is of size 3*NDOF2. 
	 * For node i, Bi' can be expressed in the actual coordinate system
	 * by: 
	 *                          3D                      2D
	 *       Bi_prime=[ 2*dN/dx    dN/dy ] Bi_prime=[ 2*dN/dx ]
	 *                [   dN/dx  2*dN/dy ]          [   dN/dy ]
	 *                [   dN/dy    dN/dx ]  
	 * where hNis the finiteelement function for node i.
	 *
	 * We assume B' has been allocated already, of size: 3x(NDOF2*numnodes)
	 */

	/*Fetch number of nodes for this finite element*/
	int numnodes = element->GetNumberOfNodes();

	/*Get nodal functions derivatives*/
	IssmDouble* dbasis=xNew<IssmDouble>(dim*numnodes);
	element->NodalFunctionsDerivatives(dbasis,xyz_list,gauss);

	/*Build B': */
	if(dim==3){
		for(int i=0;i<numnodes;i++){
			Bprime[2*numnodes*0+2*i+0] = 2.*dbasis[0*numnodes+i];
			Bprime[2*numnodes*0+2*i+1] = dbasis[1*numnodes+i];
			Bprime[2*numnodes*1+2*i+0] = dbasis[0*numnodes+i];
			Bprime[2*numnodes*1+2*i+1] = 2.*dbasis[1*numnodes+i];
			Bprime[2*numnodes*2+2*i+0] = dbasis[1*numnodes+i];
			Bprime[2*numnodes*2+2*i+1] = dbasis[0*numnodes+i];
			Bprime[2*numnodes*3+2*i+0] = dbasis[2*numnodes+i];
			Bprime[2*numnodes*3+2*i+1] = 0.;
			Bprime[2*numnodes*4+2*i+0] = 0.;
			Bprime[2*numnodes*4+2*i+1] = dbasis[2*numnodes+i];
		}
	}
	else{
		for(int i=0;i<numnodes;i++){
			Bprime[numnodes*0+i] = 2.*dbasis[0*numnodes+i];
			Bprime[numnodes*1+i] = dbasis[1*numnodes+i];
		}
	}

	/*Clean-up*/
	xDelete<IssmDouble>(dbasis);
}/*}}}*/
void StressbalanceAnalysis::GetBHOFriction(IssmDouble* B,Element* element,int dim,IssmDouble* xyz_list,Gauss* gauss){/*{{{*/
	/*Compute B  matrix. B=[B1 B2 B3] where Bi is square and of size 2. 
	 * For node i, Bi can be expressed in the actual coordinate system
	 * by: 
	 *                       3D           2D
	 *                 Bi=[ N   0 ]    Bi=N
	 *                    [ 0   N ]
	 * where N is the finiteelement function for node i.
	 *
	 * We assume B has been allocated already, of size: 2 x (numdof*numnodes)
	 */

	/*Fetch number of nodes for this finite element*/
	int numnodes = element->GetNumberOfNodes();

	/*Get nodal functions derivatives*/
	IssmDouble* basis=xNew<IssmDouble>(numnodes);
	element->NodalFunctions(basis,gauss);

	/*Build L: */
	if(dim==3){
		for(int i=0;i<numnodes;i++){
			B[2*numnodes*0+2*i+0] = basis[i];
			B[2*numnodes*0+2*i+1] = 0.;
			B[2*numnodes*1+2*i+0] = 0.;
			B[2*numnodes*1+2*i+1] = basis[i];
		}
	}
	else{
		for(int i=0;i<numnodes;i++){
			B[i] = basis[i];
		}
	}

	/*Clean-up*/
	xDelete<IssmDouble>(basis);
}/*}}}*/
void StressbalanceAnalysis::InputUpdateFromSolutionHO(IssmDouble* solution,Element* element){/*{{{*/

	int         i,dim,meshtype;
	int*        doflist=NULL;
	IssmDouble* xyz_list=NULL;

	/*Get mesh dimension*/
	element->FindParam(&meshtype,MeshTypeEnum);
	switch(meshtype){
		case Mesh2DverticalEnum: dim = 2; break;
		case Mesh3DEnum:         dim = 3; break;
		case Mesh3DtetrasEnum:   dim = 3; break;
		default: _error_("mesh "<<EnumToStringx(meshtype)<<" not supported yet");
	}

	/*Deal with pressure first*/
	int numvertices = element->GetNumberOfVertices();
	IssmDouble* pressure  = xNew<IssmDouble>(numvertices);
	IssmDouble* surface   = xNew<IssmDouble>(numvertices);
	IssmDouble  rho_ice   = element->GetMaterialParameter(MaterialsRhoIceEnum);
	IssmDouble  g         = element->GetMaterialParameter(ConstantsGEnum);
	element->GetVerticesCoordinates(&xyz_list);
	element->GetInputListOnVertices(surface,SurfaceEnum);
	for(i=0;i<numvertices;i++) pressure[i]=rho_ice*g*(surface[i]-xyz_list[i*3+2]);
	element->AddInput(PressureEnum,pressure,P1Enum);
	xDelete<IssmDouble>(pressure);
	xDelete<IssmDouble>(surface);

	/*Fetch number of nodes and dof for this finite element*/
	int numnodes = element->GetNumberOfNodes();
	int numdof   = numnodes*(dim-1);

	/*Fetch dof list and allocate solution vectors*/
	element->GetDofList(&doflist,HOApproximationEnum,GsetEnum);
	IssmDouble* values = xNew<IssmDouble>(numdof);
	IssmDouble* vx     = xNew<IssmDouble>(numnodes);
	IssmDouble* vy     = xNew<IssmDouble>(numnodes);
	IssmDouble* vz     = xNew<IssmDouble>(numnodes);
	IssmDouble* vel    = xNew<IssmDouble>(numnodes);

	/*Use the dof list to index into the solution vector: */
	for(i=0;i<numdof;i++) values[i]=solution[doflist[i]];

	/*Transform solution in Cartesian Space*/
	if(dim==3) element->TransformSolutionCoord(&values[0],XYEnum);

	/*Ok, we have vx and vy in values, fill in vx and vy arrays: */
	for(i=0;i<numnodes;i++){
		vx[i]=values[i*(dim-1)+0];
		if(xIsNan<IssmDouble>(vx[i])) _error_("NaN found in solution vector");
		if(dim==3){
			vy[i]=values[i*(dim-1)+1];
			if(xIsNan<IssmDouble>(vy[i])) _error_("NaN found in solution vector");
		}
	}

	/*Get Vz and compute vel*/
	if(dim==3){
		element->GetInputListOnNodes(&vz[0],VzEnum,0.);
		for(i=0;i<numnodes;i++) vel[i]=sqrt(vx[i]*vx[i] + vy[i]*vy[i] + vz[i]*vz[i]);
	}
	else{
		element->GetInputListOnNodes(&vy[0],VyEnum,0.);
		for(i=0;i<numnodes;i++) vel[i]=sqrt(vx[i]*vx[i] + vy[i]*vy[i]);
	}

	/*Now, we have to move the previous Vx and Vy inputs  to old 
	 * status, otherwise, we'll wipe them off: */
	element->InputChangeName(VxEnum,VxPicardEnum);
	if(dim==3)element->InputChangeName(VyEnum,VyPicardEnum);

	/*Add vx and vy as inputs to the element: */
	//element->AddInput(VxEnum,  vx,element->GetElementType());
	//element->AddInput(VyEnum,  vy,element->GetElementType());
	//element->AddInput(VelEnum,vel,element->GetElementType());
	element->AddInput(VxEnum,vx,P1Enum);
	if(dim==3)element->AddInput(VyEnum,vy,P1Enum);
	element->AddInput(VelEnum,vel,P1Enum);

	/*Free ressources:*/
	xDelete<IssmDouble>(vel);
	xDelete<IssmDouble>(vz);
	xDelete<IssmDouble>(vy);
	xDelete<IssmDouble>(vx);
	xDelete<IssmDouble>(values);
	xDelete<IssmDouble>(xyz_list);
	xDelete<int>(doflist);
}/*}}}*/

/*FS*/
ElementVector* StressbalanceAnalysis::CreateDVectorFS(Element* element){/*{{{*/

	int         meshtype,dim;

	/*Get problem dimension*/
	element->FindParam(&meshtype,MeshTypeEnum);
	switch(meshtype){
		case Mesh2DverticalEnum: dim = 2; break;
		case Mesh3DEnum:         dim = 3; break;
		case Mesh3DtetrasEnum:   dim = 3; break;
		default: _error_("mesh "<<EnumToStringx(meshtype)<<" not supported yet");
	}

	/*Fetch number of nodes and dof for this finite element*/
	int vnumnodes = element->NumberofNodesVelocity();
	int pnumnodes = element->NumberofNodesPressure();

	/*Initialize output vector*/
	ElementVector* de = element->NewElementVector(FSvelocityEnum);

	for(int i=0;i<vnumnodes;i++){
		for(int j=0;j<dim;j++) de->values[i*dim+j]=VelocityEnum;
	}
	for(int i=0;i<pnumnodes;i++){
		de->values[vnumnodes*dim+i]=PressureEnum;
	}

	return de;

}/*}}}*/
ElementMatrix* StressbalanceAnalysis::CreateJacobianMatrixFS(Element* element){/*{{{*/

	/*Intermediaries */
	int        i,j;
	IssmDouble Jdet;
	IssmDouble eps1dotdphii,eps1dotdphij;
	IssmDouble eps2dotdphii,eps2dotdphij;
	IssmDouble eps3dotdphii,eps3dotdphij;
	IssmDouble mu_prime;
	IssmDouble epsilon[6]; /* epsilon=[exx,eyy,ezz,exy,exz,eyz];*/
	IssmDouble eps1[3],eps2[3],eps3[3];
	IssmDouble *xyz_list = NULL;

	/*Fetch number of nodes and dof for this finite element*/
	int vnumnodes = element->NumberofNodesVelocity();
	int pnumnodes = element->NumberofNodesPressure();
	int numdof    = vnumnodes*NDOF3 + pnumnodes*NDOF1;

	/*Prepare coordinate system list*/
	int* cs_list = xNew<int>(vnumnodes+pnumnodes);
	for(i=0;i<vnumnodes;i++) cs_list[i]           = XYZEnum;
	for(i=0;i<pnumnodes;i++) cs_list[vnumnodes+i] = PressureEnum;

	/*Initialize Element matrix, vectors and Gaussian points*/
	ElementMatrix* Ke=this->CreateKMatrixFS(element); //Initialize Jacobian with regular FS (first part of the Gateau derivative)
	IssmDouble*    dbasis = xNew<IssmDouble>(3*vnumnodes);

	/*Retrieve all inputs and parameters*/
	element->GetVerticesCoordinates(&xyz_list);
	Input* vx_input = element->GetInput(VxEnum); _assert_(vx_input);
	Input* vy_input = element->GetInput(VyEnum); _assert_(vy_input);
	Input* vz_input = element->GetInput(VzEnum); _assert_(vz_input);

	/* Start  looping on the number of gaussian points: */
	Gauss* gauss = element->NewGauss(5);
	for(int ig=gauss->begin();ig<gauss->end();ig++){
		gauss->GaussPoint(ig);

		element->JacobianDeterminant(&Jdet,xyz_list,gauss);
		element->NodalFunctionsDerivativesVelocity(dbasis,xyz_list,gauss);

		//element->StrainRateFS(&epsilon[0],xyz_list,gauss,vx_input,vy_input,vz_input);
		//eps1[0]=epsilon[0];   eps2[0]=epsilon[3];   eps3[0]=epsilon[4];
		//eps1[1]=epsilon[3];   eps2[1]=epsilon[1];   eps3[1]=epsilon[5];
		//eps1[2]=epsilon[4];   eps2[2]=epsilon[5];   eps3[2]=epsilon[2];
		element->StrainRateHO(&epsilon[0],xyz_list,gauss,vx_input,vy_input);
		eps1[0]=epsilon[0];   eps2[0]=epsilon[2];   eps3[0]=epsilon[3];
		eps1[1]=epsilon[2];   eps2[1]=epsilon[1];   eps3[1]=epsilon[4];
		eps1[2]=epsilon[3];   eps2[2]=epsilon[4];   eps3[2]= -epsilon[0] -epsilon[1];
		element->ViscosityFSDerivativeEpsSquare(&mu_prime,&epsilon[0]);

		for(i=0;i<vnumnodes;i++){
			for(j=0;j<vnumnodes;j++){
				eps1dotdphii=eps1[0]*dbasis[0*vnumnodes+i]+eps1[1]*dbasis[1*vnumnodes+i]+eps1[2]*dbasis[2*vnumnodes+i];
				eps1dotdphij=eps1[0]*dbasis[0*vnumnodes+j]+eps1[1]*dbasis[1*vnumnodes+j]+eps1[2]*dbasis[2*vnumnodes+j];
				eps2dotdphii=eps2[0]*dbasis[0*vnumnodes+i]+eps2[1]*dbasis[1*vnumnodes+i]+eps2[2]*dbasis[2*vnumnodes+i];
				eps2dotdphij=eps2[0]*dbasis[0*vnumnodes+j]+eps2[1]*dbasis[1*vnumnodes+j]+eps2[2]*dbasis[2*vnumnodes+j];
				eps3dotdphii=eps3[0]*dbasis[0*vnumnodes+i]+eps3[1]*dbasis[1*vnumnodes+i]+eps3[2]*dbasis[2*vnumnodes+i];
				eps3dotdphij=eps3[0]*dbasis[0*vnumnodes+j]+eps3[1]*dbasis[1*vnumnodes+j]+eps3[2]*dbasis[2*vnumnodes+j];

				Ke->values[numdof*(3*i+0)+3*j+0]+=gauss->weight*Jdet*2*mu_prime*eps1dotdphij*eps1dotdphii;
				Ke->values[numdof*(3*i+0)+3*j+1]+=gauss->weight*Jdet*2*mu_prime*eps2dotdphij*eps1dotdphii;
				Ke->values[numdof*(3*i+0)+3*j+2]+=gauss->weight*Jdet*2*mu_prime*eps3dotdphij*eps1dotdphii;

				Ke->values[numdof*(3*i+1)+3*j+0]+=gauss->weight*Jdet*2*mu_prime*eps1dotdphij*eps2dotdphii;
				Ke->values[numdof*(3*i+1)+3*j+1]+=gauss->weight*Jdet*2*mu_prime*eps2dotdphij*eps2dotdphii;
				Ke->values[numdof*(3*i+1)+3*j+2]+=gauss->weight*Jdet*2*mu_prime*eps3dotdphij*eps2dotdphii;

				Ke->values[numdof*(3*i+2)+3*j+0]+=gauss->weight*Jdet*2*mu_prime*eps1dotdphij*eps3dotdphii;
				Ke->values[numdof*(3*i+2)+3*j+1]+=gauss->weight*Jdet*2*mu_prime*eps2dotdphij*eps3dotdphii;
				Ke->values[numdof*(3*i+2)+3*j+2]+=gauss->weight*Jdet*2*mu_prime*eps3dotdphij*eps3dotdphii;
			}
		}
	}

	/*Transform Coordinate System*/
	element->TransformStiffnessMatrixCoord(Ke,cs_list);

	/*Clean up and return*/
	delete gauss;
	xDelete<IssmDouble>(xyz_list);
	xDelete<IssmDouble>(dbasis);
	xDelete<int>(cs_list);
	return Ke;
}/*}}}*/
ElementMatrix* StressbalanceAnalysis::CreateKMatrixFS(Element* element){/*{{{*/

	/*Get type of algorithm*/
	int fe_FS;
	element->FindParam(&fe_FS,FlowequationFeFSEnum);

	/*compute all stiffness matrices for this element*/
	ElementMatrix* Ke1=NULL;
	if(fe_FS==XTaylorHoodEnum)
	 Ke1=CreateKMatrixFSViscousXTH(element);
	else
	 Ke1=CreateKMatrixFSViscous(element);

	ElementMatrix* Ke2=CreateKMatrixFSFriction(element);
	ElementMatrix* Ke3=CreateKMatrixFSShelf(element);
	ElementMatrix* Ke =new ElementMatrix(Ke1,Ke2,Ke3);

	/*clean-up and return*/
	delete Ke1;
	delete Ke2;
	delete Ke3;
	return Ke;
}/*}}}*/
ElementMatrix* StressbalanceAnalysis::CreateKMatrixFSViscousXTH(Element* element){/*{{{*/

	/*Intermediaries*/
	int         i,meshtype,dim,epssize;
	IssmDouble  r,FSreconditioning,Jdet;
	IssmDouble *xyz_list = NULL;

	/*Get problem dimension*/
	element->FindParam(&meshtype,MeshTypeEnum);
	element->FindParam(&r,AugmentedLagrangianREnum);
	switch(meshtype){
		case Mesh2DverticalEnum: dim = 2; epssize = 3; break;
		case Mesh3DEnum:         dim = 3; epssize = 6; break;
		case Mesh3DtetrasEnum:   dim = 3; epssize = 6; break;
		default: _error_("mesh "<<EnumToStringx(meshtype)<<" not supported yet");
	}

	/*Fetch number of nodes and dof for this finite element*/
	int vnumnodes = element->NumberofNodesVelocity();
	int pnumnodes = element->NumberofNodesPressure();
	int numdof    = vnumnodes*dim + pnumnodes;
	int bsize     = epssize + 2;

	/*Prepare coordinate system list*/
	int* cs_list = xNew<int>(vnumnodes+pnumnodes);
	if(dim==2) for(i=0;i<vnumnodes;i++) cs_list[i] = XYEnum;
	else       for(i=0;i<vnumnodes;i++) cs_list[i] = XYZEnum;
	for(i=0;i<pnumnodes;i++) cs_list[vnumnodes+i] = PressureEnum;

	/*Initialize Element matrix and vectors*/
	ElementMatrix* Ke     = element->NewElementMatrix(FSvelocityEnum);
	IssmDouble*    B      = xNew<IssmDouble>(bsize*numdof);
	IssmDouble*    Bprime = xNew<IssmDouble>(bsize*numdof);
	IssmDouble*    D      = xNewZeroInit<IssmDouble>(bsize*bsize);

	/*Retrieve all inputs and parameters*/
	element->GetVerticesCoordinates(&xyz_list);
	element->FindParam(&FSreconditioning,StressbalanceFSreconditioningEnum);
	Input* vx_input=element->GetInput(VxEnum);     _assert_(vx_input);
	Input* vy_input=element->GetInput(VyEnum);     _assert_(vy_input);
	Input* vz_input;
	if(dim==3){vz_input=element->GetInput(VzEnum); _assert_(vz_input);}

	/* Start  looping on the number of gaussian points: */
	Gauss* gauss = element->NewGauss(5);
	for(int ig=gauss->begin();ig<gauss->end();ig++){
		gauss->GaussPoint(ig);

		element->JacobianDeterminant(&Jdet,xyz_list,gauss);
		this->GetBFS(B,element,dim,xyz_list,gauss);
		this->GetBFSprime(Bprime,element,dim,xyz_list,gauss);

		for(i=0;i<epssize;i++)     D[i*bsize+i] = + r*gauss->weight*Jdet;
		for(i=epssize;i<bsize;i++) D[i*bsize+i] = - FSreconditioning*gauss->weight*Jdet;

		TripleMultiply(B,bsize,numdof,1,
					D,bsize,bsize,0,
					Bprime,bsize,numdof,0,
					&Ke->values[0],1);
	}

	/*Transform Coordinate System*/
	element->TransformStiffnessMatrixCoord(Ke,cs_list);

	/*Clean up and return*/
	delete gauss;
	xDelete<IssmDouble>(xyz_list);
	xDelete<IssmDouble>(D);
	xDelete<IssmDouble>(Bprime);
	xDelete<IssmDouble>(B);
	xDelete<int>(cs_list);
	return Ke;
}/*}}}*/
ElementMatrix* StressbalanceAnalysis::CreateKMatrixFSViscous(Element* element){/*{{{*/

	/*Intermediaries*/
	int         i,meshtype,dim,epssize;
	IssmDouble  viscosity,FSreconditioning,Jdet;
	IssmDouble *xyz_list = NULL;

	/*Get problem dimension*/
	element->FindParam(&meshtype,MeshTypeEnum);
	switch(meshtype){
		case Mesh2DverticalEnum: dim = 2; epssize = 3; break;
		case Mesh3DEnum:         dim = 3; epssize = 6; break;
		case Mesh3DtetrasEnum:   dim = 3; epssize = 6; break;
		default: _error_("mesh "<<EnumToStringx(meshtype)<<" not supported yet");
	}

	/*Fetch number of nodes and dof for this finite element*/
	int vnumnodes = element->NumberofNodesVelocity();
	int pnumnodes = element->NumberofNodesPressure();
	int numdof    = vnumnodes*dim + pnumnodes;
	int bsize     = epssize + 2;

	/*Prepare coordinate system list*/
	int* cs_list = xNew<int>(vnumnodes+pnumnodes);
	if(dim==2) for(i=0;i<vnumnodes;i++) cs_list[i] = XYEnum;
	else       for(i=0;i<vnumnodes;i++) cs_list[i] = XYZEnum;
	for(i=0;i<pnumnodes;i++) cs_list[vnumnodes+i] = PressureEnum;

	/*Initialize Element matrix and vectors*/
	ElementMatrix* Ke     = element->NewElementMatrix(FSvelocityEnum);
	IssmDouble*    B      = xNew<IssmDouble>(bsize*numdof);
	IssmDouble*    Bprime = xNew<IssmDouble>(bsize*numdof);
	IssmDouble*    D      = xNewZeroInit<IssmDouble>(bsize*bsize);

	/*Retrieve all inputs and parameters*/
	element->GetVerticesCoordinates(&xyz_list);
	element->FindParam(&FSreconditioning,StressbalanceFSreconditioningEnum);
	Input* vx_input=element->GetInput(VxEnum);     _assert_(vx_input);
	Input* vy_input=element->GetInput(VyEnum);     _assert_(vy_input);
	Input* vz_input;
	if(dim==3){vz_input=element->GetInput(VzEnum); _assert_(vz_input);}

	/* Start  looping on the number of gaussian points: */
	Gauss* gauss = element->NewGauss(5);
	for(int ig=gauss->begin();ig<gauss->end();ig++){
		gauss->GaussPoint(ig);

		element->JacobianDeterminant(&Jdet,xyz_list,gauss);
		this->GetBFS(B,element,dim,xyz_list,gauss);
		this->GetBFSprime(Bprime,element,dim,xyz_list,gauss);

		element->ViscosityFS(&viscosity,dim,xyz_list,gauss,vx_input,vy_input,vz_input);
		for(i=0;i<epssize;i++)     D[i*bsize+i] = + 2.*viscosity*gauss->weight*Jdet;
		for(i=epssize;i<bsize;i++) D[i*bsize+i] = - FSreconditioning*gauss->weight*Jdet;

		TripleMultiply(B,bsize,numdof,1,
					D,bsize,bsize,0,
					Bprime,bsize,numdof,0,
					&Ke->values[0],1);
	}

	/*Transform Coordinate System*/
	element->TransformStiffnessMatrixCoord(Ke,cs_list);

	/*Clean up and return*/
	delete gauss;
	xDelete<IssmDouble>(xyz_list);
	xDelete<IssmDouble>(D);
	xDelete<IssmDouble>(Bprime);
	xDelete<IssmDouble>(B);
	xDelete<int>(cs_list);
	return Ke;
}/*}}}*/
ElementMatrix* StressbalanceAnalysis::CreateKMatrixFSFriction(Element* element){/*{{{*/

	if(element->IsFloating() || !element->IsOnBed()) return NULL;

	/*If on water or not FS, skip stiffness: */
	int approximation;
	element->GetInputValue(&approximation,ApproximationEnum);
	if(approximation!=FSApproximationEnum && approximation!=SSAFSApproximationEnum && approximation!=HOFSApproximationEnum) return NULL;

	/*Intermediaries*/
	bool        mainlyfloating;
	int         i,meshtype,dim,epssize;
	int         migration_style,point1;
	IssmDouble  alpha2,Jdet,fraction1,fraction2;
	IssmDouble  gllevelset,phi=1.;
	IssmDouble *xyz_list_base = NULL;
	Gauss*      gauss         = NULL;

	/*Get problem dimension*/
	element->FindParam(&meshtype,MeshTypeEnum);
	switch(meshtype){
		case Mesh2DverticalEnum: dim = 2;break;
		case Mesh3DEnum:         dim = 3;break;
		case Mesh3DtetrasEnum:   dim = 3;break;
		default: _error_("mesh "<<EnumToStringx(meshtype)<<" not supported yet");
	}

	/*Fetch number of nodes and dof for this finite element*/
	int vnumnodes = element->NumberofNodesVelocity();
	int pnumnodes = element->NumberofNodesPressure();
	int numdof    = vnumnodes*dim + pnumnodes;

	/*Initialize Element matrix and vectors*/
	ElementMatrix* Ke = element->NewElementMatrix(FSvelocityEnum);
	IssmDouble*    B  = xNew<IssmDouble>((dim-1)*numdof);
	IssmDouble*    D  = xNewZeroInit<IssmDouble>((dim-1)*(dim-1));

	/*Retrieve all inputs and parameters*/
	element->GetVerticesCoordinatesBase(&xyz_list_base);
	element->FindParam(&migration_style,GroundinglineMigrationEnum);
	Input* vx_input         = element->GetInput(VxEnum);      _assert_(vx_input);
	Input* vy_input         = element->GetInput(VyEnum);      _assert_(vy_input);
	Input* vz_input         = NULL;
	if(dim==3){    vz_input = element->GetInput(VzEnum);      _assert_(vz_input);}
	Input* gllevelset_input = NULL;

	/*build friction object, used later on: */
	Friction* friction=new Friction(element,dim==3?3:1);

	/*Recover portion of element that is grounded*/
	if(migration_style==SubelementMigrationEnum) phi=element->GetGroundedPortion(xyz_list_base);
	if(migration_style==SubelementMigration2Enum){
		if(meshtype==Mesh2DverticalEnum) _error_("Subelement Migration 2 not implemented yet for Flowline");
		gllevelset_input=element->GetInput(MaskGroundediceLevelsetEnum); _assert_(gllevelset_input);
		element->GetGroundedPart(&point1,&fraction1,&fraction2,&mainlyfloating);
		//gauss = element->NewGauss(point1,fraction1,fraction2,mainlyfloating,2);
		gauss=element->NewGaussBase(3);
	}
	else{
		gauss=element->NewGaussBase(3);
	}

	/* Start  looping on the number of gaussian points: */
	for(int ig=gauss->begin();ig<gauss->end();ig++){
		gauss->GaussPoint(ig);

		friction->GetAlpha2(&alpha2,gauss,vx_input,vy_input,vz_input);
		if(migration_style==SubelementMigrationEnum) alpha2=phi*alpha2;
		if(migration_style==SubelementMigration2Enum){
			gllevelset_input->GetInputValue(&gllevelset, gauss);
			if(gllevelset<0.) alpha2=0.;
		}

		this->GetBFSFriction(B,element,dim,xyz_list_base,gauss);
		element->JacobianDeterminantBase(&Jdet,xyz_list_base,gauss);
		for(int i=0;i<dim-1;i++) D[i*(dim-1)+i] = alpha2*gauss->weight*Jdet; //taub_x = -alpha2 v_x (same for y)

		TripleMultiply(B,dim-1,numdof,1,
					D,dim-1,dim-1,0,
					B,dim-1,numdof,0,
					&Ke->values[0],1);
	}

	/*DO NOT Transform Coordinate System: this stiffness matrix is already expressed in tangential coordinates*/

	/*Clean up and return*/
	delete gauss;
	delete friction;
	xDelete<IssmDouble>(xyz_list_base);
	xDelete<IssmDouble>(B);
	xDelete<IssmDouble>(D);
	return Ke;
}/*}}}*/
ElementMatrix* StressbalanceAnalysis::CreateKMatrixFSShelf(Element* element){/*{{{*/

	if(!element->IsFloating() || !element->IsOnBed()) return NULL;

	/*If on not water or not FS, skip stiffness: */
	int approximation,shelf_dampening;
	element->GetInputValue(&approximation,ApproximationEnum);
	if(approximation!=FSApproximationEnum && approximation!=SSAFSApproximationEnum && approximation!=HOFSApproximationEnum) return NULL;
	element->FindParam(&shelf_dampening,StressbalanceShelfDampeningEnum);
	if(shelf_dampening==0) return NULL;

	/*Intermediaries*/
	bool        mainlyfloating;
	int         j,i,meshtype,dim;
	IssmDouble  Jdet,slope2,scalar,dt;
	IssmDouble  slope[3];
	IssmDouble *xyz_list_base = NULL;
	IssmDouble *xyz_list      = NULL;
	Gauss*      gauss         = NULL;

	/*Get problem dimension*/
	element->FindParam(&meshtype,MeshTypeEnum);
	switch(meshtype){
		case Mesh2DverticalEnum: dim = 2;break;
		case Mesh3DEnum:         dim = 3;break;
		case Mesh3DtetrasEnum:   dim = 3;break;
		default: _error_("mesh "<<EnumToStringx(meshtype)<<" not supported yet");
	}

	/*Fetch number of nodes and dof for this finite element*/
	int vnumnodes = element->NumberofNodesVelocity();
	int pnumnodes = element->NumberofNodesPressure();
	int numdof    = vnumnodes*dim + pnumnodes;

	/*Initialize Element matrix and vectors*/
	ElementMatrix* Ke = element->NewElementMatrix(FSvelocityEnum);
	IssmDouble*    vbasis = xNew<IssmDouble>(vnumnodes);

	/*Retrieve all inputs and parameters*/
	element->GetVerticesCoordinatesBase(&xyz_list_base);
	element->GetVerticesCoordinates(&xyz_list);
	element->FindParam(&dt,TimesteppingTimeStepEnum);
	if(dt==0)   dt=1.e+5;
	IssmDouble  rho_water     = element->GetMaterialParameter(MaterialsRhoWaterEnum);
	IssmDouble  gravity       = element->GetMaterialParameter(ConstantsGEnum);
	Input*      surface_input = element->GetInput(SurfaceEnum); _assert_(surface_input);

	/* Start  looping on the number of gaussian points: */
	gauss=element->NewGaussBase(3);
	for(int ig=gauss->begin();ig<gauss->end();ig++){
		gauss->GaussPoint(ig);

		surface_input->GetInputDerivativeValue(&slope[0],xyz_list,gauss);
		element->NodalFunctionsVelocity(vbasis,gauss);
		element->JacobianDeterminantBase(&Jdet,xyz_list_base,gauss);
		if(dim==2) slope2=slope[0]*slope[0];
		else if(dim==3) slope2=slope[0]*slope[0]+slope[1]*slope[1];
		scalar  = rho_water*gravity*sqrt(1+slope2)*gauss->weight*Jdet*dt; 
		for(i=0;i<vnumnodes;i++){
			for(j=0;j<vnumnodes;j++){
				Ke->values[numdof*((i+1)*dim-1)+(j+1)*dim-1] += scalar*vbasis[i]*vbasis[j];
			}
		}
	}

	/*DO NOT Transform Coordinate System: this stiffness matrix is already expressed in tangential coordinates*/

	/*Clean up and return*/
	delete gauss;
	xDelete<IssmDouble>(xyz_list_base);
	xDelete<IssmDouble>(xyz_list);
	xDelete<IssmDouble>(vbasis);
	return Ke;
}/*}}}*/
#ifdef FSANALYTICAL
ElementVector* StressbalanceAnalysis::CreatePVectorFS(Element* element){/*{{{*/

	int         i,meshtype,dim;
	IssmDouble  x_coord,y_coord,z_coord;
	IssmDouble  Jdet,forcex,forcey,forcez;
	IssmDouble *xyz_list = NULL;

	element->FindParam(&meshtype,MeshTypeEnum);
	switch(meshtype){
		case Mesh2DverticalEnum: dim = 2; break;
		case Mesh3DEnum:         dim = 3; break;
		case Mesh3DtetrasEnum:   dim = 3; break;
		default: _error_("mesh "<<EnumToStringx(meshtype)<<" not supported yet");
	}

	/*Fetch number of nodes and dof for this finite element*/
	int vnumnodes = element->NumberofNodesVelocity();
	int pnumnodes = element->NumberofNodesPressure();

	/*Prepare coordinate system list*/
	int* cs_list = xNew<int>(vnumnodes+pnumnodes);
	if(dim==2) for(i=0;i<vnumnodes;i++) cs_list[i] = XYEnum;
	else       for(i=0;i<vnumnodes;i++) cs_list[i] = XYZEnum;
	for(i=0;i<pnumnodes;i++) cs_list[vnumnodes+i] = PressureEnum;

	/*Initialize vectors*/
	ElementVector* pe     = element->NewElementVector(FSvelocityEnum);
	IssmDouble*    vbasis = xNew<IssmDouble>(vnumnodes);

	/*Retrieve all inputs and parameters*/
	element->GetVerticesCoordinates(&xyz_list);

	/* Start  looping on the number of gaussian points: */
	Gauss* gauss=element->NewGauss(5);
	for(int ig=gauss->begin();ig<gauss->end();ig++){
		gauss->GaussPoint(ig);

		element->JacobianDeterminant(&Jdet,xyz_list,gauss);
		element->NodalFunctionsVelocity(vbasis,gauss);

		x_coord=element->GetXcoord(gauss);
		y_coord=element->GetYcoord(gauss);
		if(dim==3) z_coord=element->GetZcoord(gauss);
		else z_coord=0.;

		forcex=fx(x_coord,y_coord,z_coord,FSANALYTICAL);
		forcey=fy(x_coord,y_coord,z_coord,FSANALYTICAL);
		forcez=fz(x_coord,y_coord,z_coord,FSANALYTICAL);

		for(i=0;i<vnumnodes;i++){
			pe->values[i*dim+0] += forcex *Jdet*gauss->weight*vbasis[i];
			pe->values[i*dim+1] += forcey *Jdet*gauss->weight*vbasis[i];
			if(dim==3) pe->values[i*dim+2] += forcez *Jdet*gauss->weight*vbasis[i];
		}
	}

	/*Transform coordinate system*/
	element->TransformLoadVectorCoord(pe,cs_list);

	/*Clean up and return*/
	delete gauss;
	xDelete<int>(cs_list);
	xDelete<IssmDouble>(vbasis);
	xDelete<IssmDouble>(xyz_list);
	return pe;
}/*}}}*/
#else
ElementVector* StressbalanceAnalysis::CreatePVectorFS(Element* element){/*{{{*/

	ElementVector* pe = NULL;

	int fe_FS;
	element->FindParam(&fe_FS,FlowequationFeFSEnum);

	if(fe_FS==XTaylorHoodEnum){
		ElementVector* pe1=CreatePVectorFSViscous(element);
		ElementVector* pe2=CreatePVectorFSShelf(element);
		ElementVector* pe3=CreatePVectorFSFront(element);
		ElementVector* petemp =new ElementVector(pe1,pe2,pe3);
		ElementVector* pe4=CreatePVectorFSViscousXTH(element);
		pe = new ElementVector(petemp,pe4);
		delete pe1;
		delete pe2;
		delete pe3;
		delete petemp;
		delete pe4;
	}
	else{
		ElementVector* pe1=CreatePVectorFSViscous(element);
		ElementVector* pe2=CreatePVectorFSShelf(element);
		ElementVector* pe3=CreatePVectorFSFront(element);
		pe =new ElementVector(pe1,pe2,pe3);
		delete pe1;
		delete pe2;
		delete pe3;
	}

	/*clean-up and return*/
	return pe;
}/*}}}*/
#endif
ElementVector* StressbalanceAnalysis::CreatePVectorFSViscous(Element* element){/*{{{*/

	int         i,meshtype,dim;
	IssmDouble  Jdet,forcex,forcey,forcez;
	IssmDouble *xyz_list = NULL;

	/*Get problem dimension*/
	element->FindParam(&meshtype,MeshTypeEnum);
	switch(meshtype){
		case Mesh2DverticalEnum: dim = 2; break;
		case Mesh3DEnum:         dim = 3; break;
		case Mesh3DtetrasEnum:   dim = 3; break;
		default: _error_("mesh "<<EnumToStringx(meshtype)<<" not supported yet");
	}

	/*Fetch number of nodes and dof for this finite element*/
	int vnumnodes = element->NumberofNodesVelocity();
	int pnumnodes = element->NumberofNodesPressure();

	/*Prepare coordinate system list*/
	int* cs_list = xNew<int>(vnumnodes+pnumnodes);
	if(dim==2) for(i=0;i<vnumnodes;i++) cs_list[i] = XYEnum;
	else       for(i=0;i<vnumnodes;i++) cs_list[i] = XYZEnum;
	for(i=0;i<pnumnodes;i++) cs_list[vnumnodes+i] = PressureEnum;

	/*Initialize vectors*/
	ElementVector* pe     = element->NewElementVector(FSvelocityEnum);
	IssmDouble*    vbasis = xNew<IssmDouble>(vnumnodes);

	/*Retrieve all inputs and parameters*/
	element->GetVerticesCoordinates(&xyz_list);
	IssmDouble  rho_ice =element->GetMaterialParameter(MaterialsRhoIceEnum);
	IssmDouble  gravity =element->GetMaterialParameter(ConstantsGEnum);
	Input*      loadingforcex_input=element->GetInput(LoadingforceXEnum);  _assert_(loadingforcex_input);
	Input*      loadingforcey_input=element->GetInput(LoadingforceYEnum);  _assert_(loadingforcey_input);
	Input*      loadingforcez_input=NULL;
	if(dim==3){
		loadingforcez_input=element->GetInput(LoadingforceZEnum);  _assert_(loadingforcez_input);
	}

	/* Start  looping on the number of gaussian points: */
	Gauss* gauss=element->NewGauss(5);
	for(int ig=gauss->begin();ig<gauss->end();ig++){
		gauss->GaussPoint(ig);

		element->JacobianDeterminant(&Jdet,xyz_list,gauss);
		element->NodalFunctionsVelocity(vbasis,gauss);

		loadingforcex_input->GetInputValue(&forcex,gauss);
		loadingforcey_input->GetInputValue(&forcey,gauss);
		if(dim==3) loadingforcez_input->GetInputValue(&forcez,gauss);

		for(i=0;i<vnumnodes;i++){
			pe->values[i*dim+0] += +rho_ice*forcex *Jdet*gauss->weight*vbasis[i];
			pe->values[i*dim+1] += +rho_ice*forcey *Jdet*gauss->weight*vbasis[i];
			if(dim==3){
				pe->values[i*dim+2] += +rho_ice*forcez*Jdet*gauss->weight*vbasis[i];
				pe->values[i*dim+2] += -rho_ice*gravity*Jdet*gauss->weight*vbasis[i];
			}
			else{
				pe->values[i*dim+1] += -rho_ice*gravity*Jdet*gauss->weight*vbasis[i];
			}
		}
	}

	/*Transform coordinate system*/
	element->TransformLoadVectorCoord(pe,cs_list);

	/*Clean up and return*/
	delete gauss;
	xDelete<int>(cs_list);
	xDelete<IssmDouble>(vbasis);
	xDelete<IssmDouble>(xyz_list);
	return pe;
}/*}}}*/
ElementVector* StressbalanceAnalysis::CreatePVectorFSViscousXTH(Element* element){/*{{{*/

	int         i,tausize,meshtype,dim;
	IssmDouble  Jdet,r;
	IssmDouble  epsxx,epsyy,epszz,epsxy,epsxz,epsyz;
	IssmDouble  sigmapxx,sigmapyy,sigmapzz,sigmapxy,sigmapxz,sigmapyz;
	IssmDouble *xyz_list = NULL;
	Gauss*      gauss    = NULL;

	/*Get problem dimension*/
	element->FindParam(&meshtype,MeshTypeEnum);
	switch(meshtype){
		case Mesh2DverticalEnum: dim = 2; tausize = 3; break;
		case Mesh3DEnum:         dim = 3; tausize = 6; break;
		case Mesh3DtetrasEnum:   dim = 3; tausize = 6; break;
		default: _error_("mesh "<<EnumToStringx(meshtype)<<" not supported yet");
	}

	/*Fetch number of nodes and dof for this finite element*/
	int vnumnodes = element->NumberofNodesVelocity();
	int pnumnodes = element->NumberofNodesPressure();
	int tnumnodes = element->GetNumberOfVertices();      //Tensors, P1 DG

	/*Prepare coordinate system list*/
	int* cs_list = xNew<int>(vnumnodes+pnumnodes);
	if(dim==2) for(i=0;i<vnumnodes;i++) cs_list[i] = XYEnum;
	else       for(i=0;i<vnumnodes;i++) cs_list[i] = XYZEnum;
	for(i=0;i<pnumnodes;i++) cs_list[vnumnodes+i]  = PressureEnum;

	/*Initialize vectors*/
	ElementVector* pe      = element->NewElementVector(FSvelocityEnum);
	IssmDouble*    Dstar   = xNewZeroInit<IssmDouble>((dim*vnumnodes)*(tausize*tnumnodes));
	IssmDouble*    tau     = xNew<IssmDouble>(tausize*tnumnodes);
	IssmDouble*    d       = xNew<IssmDouble>(tausize*tnumnodes);
	IssmDouble*    vdbasis = xNew<IssmDouble>(dim*vnumnodes);
	IssmDouble*    tbasis  = xNew<IssmDouble>(tnumnodes);
	IssmDouble*    D       = xNewZeroInit<IssmDouble>(tausize*tnumnodes*tausize*tnumnodes);

	/*Retrieve all inputs and parameters*/
	element->FindParam(&r,AugmentedLagrangianREnum);
	element->GetVerticesCoordinates(&xyz_list);

	/*Get d and tau*/
	Input* epsxx_input=element->GetInput(StrainRatexxEnum); _assert_(epsxx_input);
	Input* epsyy_input=element->GetInput(StrainRateyyEnum); _assert_(epsyy_input);
	Input* epsxy_input=element->GetInput(StrainRatexyEnum); _assert_(epsxy_input);
	Input* epszz_input=NULL; Input* epsxz_input=NULL; Input* epsyz_input=NULL;
	Input* sigmapxx_input=element->GetInput(DeviatoricStressxxEnum); _assert_(sigmapxx_input);
	Input* sigmapyy_input=element->GetInput(DeviatoricStressyyEnum); _assert_(sigmapyy_input);
	Input* sigmapxy_input=element->GetInput(DeviatoricStressxyEnum); _assert_(sigmapxy_input);
	Input* sigmapzz_input=NULL; Input* sigmapxz_input=NULL; Input* sigmapyz_input=NULL;
	if(dim==3){
		epszz_input=element->GetInput(StrainRatezzEnum); _assert_(epszz_input);
		epsxz_input=element->GetInput(StrainRatexzEnum); _assert_(epsxz_input);
		epsyz_input=element->GetInput(StrainRateyzEnum); _assert_(epsyz_input);
		sigmapzz_input=element->GetInput(DeviatoricStresszzEnum); _assert_(sigmapzz_input);
		sigmapxz_input=element->GetInput(DeviatoricStressxzEnum); _assert_(sigmapxz_input);
		sigmapyz_input=element->GetInput(DeviatoricStressyzEnum); _assert_(sigmapyz_input);
	}

	gauss = element->NewGauss();
	for(int i=0;i<tnumnodes;i++){
		gauss->GaussNode(P1DGEnum,i);

		epsxx_input->GetInputValue(&epsxx,gauss); sigmapxx_input->GetInputValue(&sigmapxx,gauss);
		epsyy_input->GetInputValue(&epsyy,gauss); sigmapyy_input->GetInputValue(&sigmapyy,gauss);
		epsxy_input->GetInputValue(&epsxy,gauss); sigmapxy_input->GetInputValue(&sigmapxy,gauss);
		if(dim==2){
			d[i*tausize+0]=epsxx;  tau[i*tausize+0]=sigmapxx;
			d[i*tausize+1]=epsyy;  tau[i*tausize+1]=sigmapyy;
			d[i*tausize+2]=epsxy;  tau[i*tausize+2]=sigmapxy;
		}
		else{
			epszz_input->GetInputValue(&epszz,gauss); sigmapzz_input->GetInputValue(&sigmapzz,gauss);
			epsxz_input->GetInputValue(&epsxz,gauss); sigmapxz_input->GetInputValue(&sigmapxz,gauss);
			epsyz_input->GetInputValue(&epsyz,gauss); sigmapyz_input->GetInputValue(&sigmapyz,gauss);
			d[i*tausize+0]=epsxx;  tau[i*tausize+0]=sigmapxx;
			d[i*tausize+1]=epsyy;  tau[i*tausize+1]=sigmapyy;
			d[i*tausize+2]=epszz;  tau[i*tausize+2]=sigmapzz;
			d[i*tausize+3]=epsxy;  tau[i*tausize+3]=sigmapxy;
			d[i*tausize+4]=epsxz;  tau[i*tausize+4]=sigmapxz;
			d[i*tausize+5]=epsyz;  tau[i*tausize+5]=sigmapyz;
		}
	}

	/* Start  looping on the number of gaussian points: */
	delete gauss;
	gauss=element->NewGauss(5);
	for(int ig=gauss->begin();ig<gauss->end();ig++){
		gauss->GaussPoint(ig);
		element->JacobianDeterminant(&Jdet,xyz_list,gauss);

		/*Create Dstar*/
		/*In dim = 2
		 *
		 *       <----------------- tausize ---------------> x tnumnodes
		 *       |  gamma_ij^x         0       gamma_ij^y  | ^
		 * Dij = |                                         | dim
		 *       |     0          gamma_ij^y   gamma_ij^x  | v
		 *                                                   x
		 *                                                   vnumnodes
		 *
		 *In dim = 3
		 *
		 *       |  gamma_ij^x         0          0         gamma_ij^y  gamma_ij^z      0      |
		 *       |                                                                             |
		 * Dij = |     0          gamma_ij^y      0         gamma_ij^x     0        gamma_ij^z |
		 *       |                                                                             |
		 *       |     0               0      gamma_ij^z        0       gamma_ij^x  gamma_ij^y |
		 *
		 * gamma_ij^x = zeta_j dphi_i/dx
		 *
		 * where:
		 *   - zeta_j is the nodal function for the j^th node of the tensor (P1DG)
		 *   - phi_i  is the nodal function for the i^th node of the velocity (P2)*/
		element->NodalFunctionsDerivativesVelocity(vdbasis,xyz_list,gauss);
		element->NodalFunctionsTensor(tbasis,gauss);
		if(dim==2){
			for(int i=0;i<vnumnodes;i++){
				for(int j=0;j<tnumnodes;j++){
					Dstar[(i*dim+0)*tausize*tnumnodes + j*tausize+0] = tbasis[j]*vdbasis[0*vnumnodes+i];
					Dstar[(i*dim+0)*tausize*tnumnodes + j*tausize+2] = tbasis[j]*vdbasis[1*vnumnodes+i];

					Dstar[(i*dim+1)*tausize*tnumnodes + j*tausize+1] = tbasis[j]*vdbasis[1*vnumnodes+i];
					Dstar[(i*dim+1)*tausize*tnumnodes + j*tausize+2] = tbasis[j]*vdbasis[0*vnumnodes+i];
				}
			}
		}
		else{
			for(int i=0;i<vnumnodes;i++){
				for(int j=0;j<tnumnodes;j++){
					Dstar[(i*dim+0)*tausize*tnumnodes + j*tausize+0] = tbasis[j]*vdbasis[0*vnumnodes+i];
					Dstar[(i*dim+0)*tausize*tnumnodes + j*tausize+3] = tbasis[j]*vdbasis[1*vnumnodes+i];
					Dstar[(i*dim+0)*tausize*tnumnodes + j*tausize+4] = tbasis[j]*vdbasis[2*vnumnodes+i];

					Dstar[(i*dim+1)*tausize*tnumnodes + j*tausize+1] = tbasis[j]*vdbasis[1*vnumnodes+i];
					Dstar[(i*dim+1)*tausize*tnumnodes + j*tausize+3] = tbasis[j]*vdbasis[0*vnumnodes+i];
					Dstar[(i*dim+1)*tausize*tnumnodes + j*tausize+5] = tbasis[j]*vdbasis[2*vnumnodes+i];

					Dstar[(i*dim+2)*tausize*tnumnodes + j*tausize+2] = tbasis[j]*vdbasis[2*vnumnodes+i];
					Dstar[(i*dim+2)*tausize*tnumnodes + j*tausize+4] = tbasis[j]*vdbasis[0*vnumnodes+i];
					Dstar[(i*dim+2)*tausize*tnumnodes + j*tausize+5] = tbasis[j]*vdbasis[1*vnumnodes+i];
				}
			}
		}

		/*contribution -Dstar tau*/
		for(i=0;i<tausize*tnumnodes;i++) D[i*(tausize*tnumnodes)+i] = -gauss->weight*Jdet;
		TripleMultiply(Dstar,dim*tnumnodes,tausize*tnumnodes,0,
					D,tausize*tnumnodes,tausize*tnumnodes,0,
					tau,tausize*tnumnodes,1,0,
					&pe->values[0],1);

		/*contribution + r Dstar d*/
		for(i=0;i<tausize*tnumnodes;i++) D[i*(tausize*tnumnodes)+i] = +r*gauss->weight*Jdet;
		TripleMultiply(Dstar,dim*tnumnodes,tausize*tnumnodes,0,
					D,tausize*tnumnodes,tausize*tnumnodes,0,
					d,tausize*tnumnodes,1,0,
					&pe->values[0],1);
	}

	/*Transform coordinate system*/
	element->TransformLoadVectorCoord(pe,cs_list);

	/*Clean up and return*/
	delete gauss;
	xDelete<int>(cs_list);
	xDelete<IssmDouble>(xyz_list);
	xDelete<IssmDouble>(Dstar);
	xDelete<IssmDouble>(d);
	xDelete<IssmDouble>(D);
	xDelete<IssmDouble>(tau);
	return pe;
}/*}}}*/
ElementVector* StressbalanceAnalysis::CreatePVectorFSShelf(Element* element){/*{{{*/

	int         i,meshtype,dim;
	IssmDouble  Jdet,water_pressure,bed;
	IssmDouble	normal[3];
	IssmDouble *xyz_list_base = NULL;

	/*Get basal element*/
	if(!element->IsOnBed() || !element->IsFloating()) return NULL;

	/*Get problem dimension*/
	element->FindParam(&meshtype,MeshTypeEnum);
	switch(meshtype){
		case Mesh2DverticalEnum: dim = 2; break;
		case Mesh3DEnum:         dim = 3; break;
		case Mesh3DtetrasEnum:   dim = 3; break;
		default: _error_("mesh "<<EnumToStringx(meshtype)<<" not supported yet");
	}

	/*Fetch number of nodes and dof for this finite element*/
	int vnumnodes = element->NumberofNodesVelocity();
	int pnumnodes = element->NumberofNodesPressure();

	/*Prepare coordinate system list*/
	int* cs_list = xNew<int>(vnumnodes+pnumnodes);
	if(dim==2) for(i=0;i<vnumnodes;i++) cs_list[i] = XYEnum;
	else       for(i=0;i<vnumnodes;i++) cs_list[i] = XYZEnum;
	for(i=0;i<pnumnodes;i++) cs_list[vnumnodes+i] = PressureEnum;

	/*Initialize vectors*/
	ElementVector* pe     = element->NewElementVector(FSvelocityEnum);
	IssmDouble*    vbasis = xNew<IssmDouble>(vnumnodes);

	/*Retrieve all inputs and parameters*/
	element->GetVerticesCoordinatesBase(&xyz_list_base);
	Input*      base_input=element->GetInput(BaseEnum); _assert_(base_input);
	IssmDouble  rho_water=element->GetMaterialParameter(MaterialsRhoWaterEnum);
	IssmDouble  gravity  =element->GetMaterialParameter(ConstantsGEnum);

	/* Start  looping on the number of gaussian points: */
	Gauss* gauss=element->NewGaussBase(5);
	for(int ig=gauss->begin();ig<gauss->end();ig++){
		gauss->GaussPoint(ig);

		element->JacobianDeterminantBase(&Jdet,xyz_list_base,gauss);
		element->NodalFunctionsVelocity(vbasis,gauss);

		element->NormalBase(&normal[0],xyz_list_base);
		_assert_(normal[dim-1]<0.);
		base_input->GetInputValue(&bed, gauss);
		water_pressure=gravity*rho_water*bed;

		for(i=0;i<vnumnodes;i++){
			pe->values[i*dim+0] += water_pressure*gauss->weight*Jdet*vbasis[i]*normal[0];
			pe->values[i*dim+1] += water_pressure*gauss->weight*Jdet*vbasis[i]*normal[1];
			if(dim==3){
				pe->values[i*dim+2]+=water_pressure*gauss->weight*Jdet*vbasis[i]*normal[2];
			}
		}
	}

	/*Transform coordinate system*/
	element->TransformLoadVectorCoord(pe,cs_list);

	/*Clean up and return*/
	delete gauss;
	xDelete<int>(cs_list);
	xDelete<IssmDouble>(vbasis);
	xDelete<IssmDouble>(xyz_list_base);
	return pe;
}/*}}}*/
ElementVector* StressbalanceAnalysis::CreatePVectorFSFront(Element* element){/*{{{*/

	/*If no front, return NULL*/
	if(!element->IsIcefront()) return NULL;

	/*Intermediaries*/
	int         i,meshtype,dim;
	IssmDouble  Jdet,pressure,surface,z;
	IssmDouble	normal[3];
	IssmDouble *xyz_list       = NULL;
	IssmDouble *xyz_list_front = NULL;
	Gauss      *gauss          = NULL;

	/*Make sure current element is floating*/
	if(!element->IsFloating()) return NULL;

	/*Get problem dimension*/
	element->FindParam(&meshtype,MeshTypeEnum);
	switch(meshtype){
		case Mesh2DverticalEnum: dim = 2; break;
		case Mesh3DEnum:         dim = 3; break;
		case Mesh3DtetrasEnum:   dim = 3; break;
		default: _error_("mesh "<<EnumToStringx(meshtype)<<" not supported yet");
	}

	/*Fetch number of nodes and dof for this finite element*/
	int vnumnodes   = element->NumberofNodesVelocity();
	int pnumnodes   = element->NumberofNodesPressure();
	int numvertices = element->GetNumberOfVertices();

	/*Prepare coordinate system list*/
	int* cs_list = xNew<int>(vnumnodes+pnumnodes);
	if(dim==2) for(i=0;i<vnumnodes;i++) cs_list[i] = XYEnum;
	else       for(i=0;i<vnumnodes;i++) cs_list[i] = XYZEnum;
	for(i=0;i<pnumnodes;i++) cs_list[vnumnodes+i] = PressureEnum;

	/*Initialize vectors*/
	ElementVector* pe     = element->NewElementVector(FSvelocityEnum);
	IssmDouble*    vbasis = xNew<IssmDouble>(vnumnodes);

	/*Retrieve all inputs and parameters*/
	element->GetVerticesCoordinates(&xyz_list);
	element->ZeroLevelsetCoordinates(&xyz_list_front,xyz_list,MaskIceLevelsetEnum);
	element->NormalSection(&normal[0],xyz_list_front);
	Input* surface_input  = element->GetInput(SurfaceEnum); _assert_(surface_input);
	IssmDouble  rho_water = element->GetMaterialParameter(MaterialsRhoWaterEnum);
	IssmDouble  gravity   = element->GetMaterialParameter(ConstantsGEnum);

	/*Initialize gauss points*/
	IssmDouble zmax=xyz_list[0*3+(dim-1)]; for(int i=1;i<numvertices;i++) if(xyz_list[i*3+(dim-1)]>zmax) zmax=xyz_list[i*3+(dim-1)];
	IssmDouble zmin=xyz_list[0*3+(dim-1)]; for(int i=1;i<numvertices;i++) if(xyz_list[i*3+(dim-1)]<zmin) zmin=xyz_list[i*3+(dim-1)];
	if(zmax>0. && zmin<0.) gauss=element->NewGauss(xyz_list,xyz_list_front,3,30);//refined in vertical because of the sea level discontinuity
	else                   gauss=element->NewGauss(xyz_list,xyz_list_front,3,3);

	/* Start  looping on the number of gaussian points: */
	for(int ig=gauss->begin();ig<gauss->end();ig++){
		gauss->GaussPoint(ig);

		element->JacobianDeterminantSurface(&Jdet,xyz_list_front,gauss);
		element->NodalFunctionsVelocity(vbasis,gauss);
		surface_input->GetInputValue(&surface,gauss);
		if(dim==3) z=element->GetZcoord(gauss);
		else       z=element->GetYcoord(gauss);
		pressure = rho_water*gravity*min(0.,z);//0 if the gaussian point is above water level

		for (int i=0;i<vnumnodes;i++){
			pe->values[dim*i+0]+= pressure*Jdet*gauss->weight*normal[0]*vbasis[i];
			pe->values[dim*i+1]+= pressure*Jdet*gauss->weight*normal[1]*vbasis[i];
			if(dim==3) pe->values[dim*i+2]+= pressure*Jdet*gauss->weight*normal[2]*vbasis[i];
		}
	}

	/*Transform coordinate system*/
	element->TransformLoadVectorCoord(pe,cs_list);

	/*Clean up and return*/
	delete gauss;
	xDelete<int>(cs_list);
	xDelete<IssmDouble>(vbasis);
	xDelete<IssmDouble>(xyz_list);
	xDelete<IssmDouble>(xyz_list_front);
	return pe;
}/*}}}*/
void StressbalanceAnalysis::GetBFS(IssmDouble* B,Element* element,int dim,IssmDouble* xyz_list,Gauss* gauss){/*{{{*/
	/*Compute B  matrix. B=[Bv1 Bv2 ... Bp1 Bp2 ...] where Bvi is of size 3*NDOF3. 
	 * For node i, Bvi can be expressed in the actual coordinate system
	 * by: 	   Bvi=[ dphi/dx          0        ]
	 *					 [   0           dphi/dy     ]
	 *					 [ 1/2*dphi/dy    1/2*dphi/dx]
	 *					 [   0             0         ]
	 *					 [ dphi/dx         dphi/dy   ]
	 *
	 *         Bpi=[  0    ]
	 *					[  0    ]
	 *					[  0    ]
	 *					[ phi_p ]
	 *					[  0    ]
	 *
	 *	In 3d:
	 *     	   Bvi=[ dh/dx          0             0      ]
	 *					[   0           dh/dy           0      ]
	 *					[   0             0           dh/dz    ]
	 *					[ 1/2*dh/dy    1/2*dh/dx        0      ]
	 *					[ 1/2*dh/dz       0         1/2*dh/dx  ]
	 *					[   0          1/2*dh/dz    1/2*dh/dy  ]
	 *					[   0             0             0      ]
	 *					[ dh/dx         dh/dy         dh/dz    ]
	 *
	 *         Bpi=[ 0 ]
	 *					[ 0 ]
	 *					[ 0 ]
	 *					[ 0 ]
	 *					[ 0 ]
	 *					[ 0 ]
	 *					[ h ]
	 *					[ 0 ]
	 *	where phi is the finiteelement function for node i.
	 *	Same thing for Bb except the last column that does not exist.
	 */

	/*Fetch number of nodes for this finite element*/
	int pnumnodes = element->NumberofNodesPressure();
	int vnumnodes = element->NumberofNodesVelocity();

	/*Get nodal functions derivatives*/
	IssmDouble* vdbasis=xNew<IssmDouble>(dim*vnumnodes);
	IssmDouble* pbasis =xNew<IssmDouble>(pnumnodes);
	element->NodalFunctionsDerivativesVelocity(vdbasis,xyz_list,gauss);
	element->NodalFunctionsPressure(pbasis,gauss);

	/*Build B: */
	if(dim==2){
		for(int i=0;i<vnumnodes;i++){
			B[(dim*vnumnodes+pnumnodes)*0+dim*i+0] = vdbasis[0*vnumnodes+i];
			B[(dim*vnumnodes+pnumnodes)*0+dim*i+1] = 0.;
			B[(dim*vnumnodes+pnumnodes)*1+dim*i+0] = 0.;
			B[(dim*vnumnodes+pnumnodes)*1+dim*i+1] = vdbasis[1*vnumnodes+i];
			B[(dim*vnumnodes+pnumnodes)*2+dim*i+0] = .5*vdbasis[1*vnumnodes+i];
			B[(dim*vnumnodes+pnumnodes)*2+dim*i+1] = .5*vdbasis[0*vnumnodes+i];
			B[(dim*vnumnodes+pnumnodes)*3+dim*i+0] = 0.;
			B[(dim*vnumnodes+pnumnodes)*3+dim*i+1] = 0.;
			B[(dim*vnumnodes+pnumnodes)*4+dim*i+0] = vdbasis[0*vnumnodes+i];
			B[(dim*vnumnodes+pnumnodes)*4+dim*i+1] = vdbasis[1*vnumnodes+i];
		}
		for(int i=0;i<pnumnodes;i++){
			B[(dim*vnumnodes+pnumnodes)*0+(dim*vnumnodes)+i] = 0.;
			B[(dim*vnumnodes+pnumnodes)*1+(dim*vnumnodes)+i] = 0.;
			B[(dim*vnumnodes+pnumnodes)*2+(dim*vnumnodes)+i] = 0.;
			B[(dim*vnumnodes+pnumnodes)*3+(dim*vnumnodes)+i] = pbasis[i];
			B[(dim*vnumnodes+pnumnodes)*4+(dim*vnumnodes)+i] = 0.;
		}
	}
	else{
		for(int i=0;i<vnumnodes;i++){
			B[(dim*vnumnodes+pnumnodes)*0+dim*i+0] = vdbasis[0*vnumnodes+i];
			B[(dim*vnumnodes+pnumnodes)*0+dim*i+1] = 0.;
			B[(dim*vnumnodes+pnumnodes)*0+dim*i+2] = 0.;
			B[(dim*vnumnodes+pnumnodes)*1+dim*i+0] = 0.;
			B[(dim*vnumnodes+pnumnodes)*1+dim*i+1] = vdbasis[1*vnumnodes+i];
			B[(dim*vnumnodes+pnumnodes)*1+dim*i+2] = 0.;
			B[(dim*vnumnodes+pnumnodes)*2+dim*i+0] = 0.;
			B[(dim*vnumnodes+pnumnodes)*2+dim*i+1] = 0.;
			B[(dim*vnumnodes+pnumnodes)*2+dim*i+2] = vdbasis[2*vnumnodes+i];
			B[(dim*vnumnodes+pnumnodes)*3+dim*i+0] = .5*vdbasis[1*vnumnodes+i];
			B[(dim*vnumnodes+pnumnodes)*3+dim*i+1] = .5*vdbasis[0*vnumnodes+i];
			B[(dim*vnumnodes+pnumnodes)*3+dim*i+2] = 0.;
			B[(dim*vnumnodes+pnumnodes)*4+dim*i+0] = .5*vdbasis[2*vnumnodes+i];
			B[(dim*vnumnodes+pnumnodes)*4+dim*i+1] = 0.;
			B[(dim*vnumnodes+pnumnodes)*4+dim*i+2] = .5*vdbasis[0*vnumnodes+i];
			B[(dim*vnumnodes+pnumnodes)*5+dim*i+0] = 0.;
			B[(dim*vnumnodes+pnumnodes)*5+dim*i+1] = .5*vdbasis[2*vnumnodes+i];
			B[(dim*vnumnodes+pnumnodes)*5+dim*i+2] = .5*vdbasis[1*vnumnodes+i];
			B[(dim*vnumnodes+pnumnodes)*6+dim*i+0] = 0.;
			B[(dim*vnumnodes+pnumnodes)*6+dim*i+1] = 0.;
			B[(dim*vnumnodes+pnumnodes)*6+dim*i+2] = 0.;
			B[(dim*vnumnodes+pnumnodes)*7+dim*i+0] = vdbasis[0*vnumnodes+i];
			B[(dim*vnumnodes+pnumnodes)*7+dim*i+1] = vdbasis[1*vnumnodes+i];
			B[(dim*vnumnodes+pnumnodes)*7+dim*i+2] = vdbasis[2*vnumnodes+i];
		}
		for(int i=0;i<pnumnodes;i++){
			B[(dim*vnumnodes+pnumnodes)*0+(dim*vnumnodes)+i] = 0.;
			B[(dim*vnumnodes+pnumnodes)*1+(dim*vnumnodes)+i] = 0.;
			B[(dim*vnumnodes+pnumnodes)*2+(dim*vnumnodes)+i] = 0.;
			B[(dim*vnumnodes+pnumnodes)*3+(dim*vnumnodes)+i] = 0.;
			B[(dim*vnumnodes+pnumnodes)*4+(dim*vnumnodes)+i] = 0.;
			B[(dim*vnumnodes+pnumnodes)*5+(dim*vnumnodes)+i] = 0.;
			B[(dim*vnumnodes+pnumnodes)*6+(dim*vnumnodes)+i] = pbasis[i];
			B[(dim*vnumnodes+pnumnodes)*7+(dim*vnumnodes)+i] = 0.;
		}
	}

	/*Clean up*/
	xDelete<IssmDouble>(vdbasis);
	xDelete<IssmDouble>(pbasis);
}/*}}}*/
void StressbalanceAnalysis::GetBFSprime(IssmDouble* Bprime,Element* element,int dim,IssmDouble* xyz_list,Gauss* gauss){/*{{{*/
	/*	Compute B'  matrix. B'=[B1' B2' B3' B4' B5' B6' Bb'] where Bi' is of size 3*NDOF2. 
	 *	For node i, Bi' can be expressed in the actual coordinate system
	 *	by: 
	 *			Bvi' = [  dphi/dx     0     ]
	 *					 [     0      dphi/dy ]
	 *					 [  dphi/dy   dphi/dx ]
	 *					 [  dphi/dx   dphi/dy ]
	 *					 [     0      0       ]
	 *
	 * by: 	  Bpi=[  0  ]
	 *					[  0  ]
	 *					[  0  ]
	 *					[  0  ]
	 *					[ phi ]
	 *
	 *	In 3d
	 *     	   Bvi=[ dh/dx     0        0    ]
	 *					[   0      dh/dy      0    ]
	 *					[   0        0      dh/dz  ]
	 *					[ dh/dy    dh/dx      0    ]
	 *					[ dh/dz      0      dh/dx  ]
	 *					[   0      dh/dz    dh/dy  ]
	 *					[ dh/dx    dh/dy    dh/dz  ]
	 *					[   0        0        0    ]
	 *
	 *         Bpi=[ 0 ]
	 *					[ 0 ]
	 *					[ 0 ]
	 *					[ 0 ]
	 *					[ 0 ]
	 *					[ 0 ]
	 *					[ 0 ]
	 *					[ h ]
	 *	where phi is the finiteelement function for node i.
	 *	In 3d:
	 */

	/*Fetch number of nodes for this finite element*/
	int pnumnodes = element->NumberofNodesPressure();
	int vnumnodes = element->NumberofNodesVelocity();

	/*Get nodal functions derivatives*/
	IssmDouble* vdbasis=xNew<IssmDouble>(dim*vnumnodes);
	IssmDouble* pbasis =xNew<IssmDouble>(pnumnodes);
	element->NodalFunctionsDerivativesVelocity(vdbasis,xyz_list,gauss);
	element->NodalFunctionsPressure(pbasis,gauss);

	/*Build B_prime: */
	if(dim==2){
		for(int i=0;i<vnumnodes;i++){
			Bprime[(dim*vnumnodes+pnumnodes)*0+dim*i+0] = vdbasis[0*vnumnodes+i];
			Bprime[(dim*vnumnodes+pnumnodes)*0+dim*i+1] = 0.;
			Bprime[(dim*vnumnodes+pnumnodes)*1+dim*i+0] = 0.;
			Bprime[(dim*vnumnodes+pnumnodes)*1+dim*i+1] = vdbasis[1*vnumnodes+i];
			Bprime[(dim*vnumnodes+pnumnodes)*2+dim*i+0] = vdbasis[1*vnumnodes+i];
			Bprime[(dim*vnumnodes+pnumnodes)*2+dim*i+1] = vdbasis[0*vnumnodes+i];
			Bprime[(dim*vnumnodes+pnumnodes)*3+dim*i+0] = vdbasis[0*vnumnodes+i];
			Bprime[(dim*vnumnodes+pnumnodes)*3+dim*i+1] = vdbasis[1*vnumnodes+i];
			Bprime[(dim*vnumnodes+pnumnodes)*4+dim*i+0] = 0.;
			Bprime[(dim*vnumnodes+pnumnodes)*4+dim*i+1] = 0.;
		}
		for(int i=0;i<pnumnodes;i++){
			Bprime[(dim*vnumnodes+pnumnodes)*0+(dim*vnumnodes)+i] = 0.;
			Bprime[(dim*vnumnodes+pnumnodes)*1+(dim*vnumnodes)+i] = 0.;
			Bprime[(dim*vnumnodes+pnumnodes)*2+(dim*vnumnodes)+i] = 0.;
			Bprime[(dim*vnumnodes+pnumnodes)*3+(dim*vnumnodes)+i] = 0.;
			Bprime[(dim*vnumnodes+pnumnodes)*4+(dim*vnumnodes)+i] = pbasis[i];
		}
	}
	else{
		for(int i=0;i<vnumnodes;i++){
			Bprime[(dim*vnumnodes+pnumnodes)*0+dim*i+0] = vdbasis[0*vnumnodes+i];
			Bprime[(dim*vnumnodes+pnumnodes)*0+dim*i+1] = 0.;
			Bprime[(dim*vnumnodes+pnumnodes)*0+dim*i+2] = 0.;
			Bprime[(dim*vnumnodes+pnumnodes)*1+dim*i+0] = 0.;
			Bprime[(dim*vnumnodes+pnumnodes)*1+dim*i+1] = vdbasis[1*vnumnodes+i];
			Bprime[(dim*vnumnodes+pnumnodes)*1+dim*i+2] = 0.;
			Bprime[(dim*vnumnodes+pnumnodes)*2+dim*i+0] = 0.;
			Bprime[(dim*vnumnodes+pnumnodes)*2+dim*i+1] = 0.;
			Bprime[(dim*vnumnodes+pnumnodes)*2+dim*i+2] = vdbasis[2*vnumnodes+i];
			Bprime[(dim*vnumnodes+pnumnodes)*3+dim*i+0] = vdbasis[1*vnumnodes+i];
			Bprime[(dim*vnumnodes+pnumnodes)*3+dim*i+1] = vdbasis[0*vnumnodes+i];
			Bprime[(dim*vnumnodes+pnumnodes)*3+dim*i+2] = 0.;
			Bprime[(dim*vnumnodes+pnumnodes)*4+dim*i+0] = vdbasis[2*vnumnodes+i];
			Bprime[(dim*vnumnodes+pnumnodes)*4+dim*i+1] = 0.;
			Bprime[(dim*vnumnodes+pnumnodes)*4+dim*i+2] = vdbasis[0*vnumnodes+i];
			Bprime[(dim*vnumnodes+pnumnodes)*5+dim*i+0] = 0.;
			Bprime[(dim*vnumnodes+pnumnodes)*5+dim*i+1] = vdbasis[2*vnumnodes+i];
			Bprime[(dim*vnumnodes+pnumnodes)*5+dim*i+2] = vdbasis[1*vnumnodes+i];
			Bprime[(dim*vnumnodes+pnumnodes)*6+dim*i+0] = vdbasis[0*vnumnodes+i];
			Bprime[(dim*vnumnodes+pnumnodes)*6+dim*i+1] = vdbasis[1*vnumnodes+i];
			Bprime[(dim*vnumnodes+pnumnodes)*6+dim*i+2] = vdbasis[2*vnumnodes+i];
			Bprime[(dim*vnumnodes+pnumnodes)*7+dim*i+0] = 0.;
			Bprime[(dim*vnumnodes+pnumnodes)*7+dim*i+1] = 0.;
			Bprime[(dim*vnumnodes+pnumnodes)*7+dim*i+2] = 0.;
		}
		for(int i=0;i<pnumnodes;i++){
			Bprime[(dim*vnumnodes+pnumnodes)*0+(dim*vnumnodes)+i] = 0.;
			Bprime[(dim*vnumnodes+pnumnodes)*1+(dim*vnumnodes)+i] = 0.;
			Bprime[(dim*vnumnodes+pnumnodes)*2+(dim*vnumnodes)+i] = 0.;
			Bprime[(dim*vnumnodes+pnumnodes)*3+(dim*vnumnodes)+i] = 0.;
			Bprime[(dim*vnumnodes+pnumnodes)*4+(dim*vnumnodes)+i] = 0.;
			Bprime[(dim*vnumnodes+pnumnodes)*5+(dim*vnumnodes)+i] = 0.;
			Bprime[(dim*vnumnodes+pnumnodes)*6+(dim*vnumnodes)+i] = 0.;
			Bprime[(dim*vnumnodes+pnumnodes)*7+(dim*vnumnodes)+i] = pbasis[i];
		}
	}

	/*Clean up*/
	xDelete<IssmDouble>(vdbasis);
	xDelete<IssmDouble>(pbasis);
}/*}}}*/
void StressbalanceAnalysis::GetBFSFriction(IssmDouble* B,Element* element,int dim,IssmDouble* xyz_list,Gauss* gauss){/*{{{*/
	/* Compute L  matrix. L=[L1 L2 L3] where Li is square and of size numdof. 
	 * For node i, Li can be expressed in the actual coordinate system
	 * by in 3d 
	 *       Li=[ h 0 0 0 ]
	 *	 	      [ 0 h 0 0 ]
	 *	in 2d:
	 *       Li=[ h 0 0 ]
	 * where h is the interpolation function for node i.
	 */

	/*Fetch number of nodes for this finite element*/
	int pnumnodes = element->NumberofNodesPressure();
	int vnumnodes = element->NumberofNodesVelocity();
	int pnumdof   = pnumnodes;
	int vnumdof   = vnumnodes*dim;

	/*Get nodal functions derivatives*/
	IssmDouble* vbasis=xNew<IssmDouble>(vnumnodes);
	element->NodalFunctionsVelocity(vbasis,gauss);

	/*Build B: */
	if(dim==3){
		for(int i=0;i<vnumnodes;i++){
			B[(vnumdof+pnumdof)*0+3*i+0] = vbasis[i];
			B[(vnumdof+pnumdof)*0+3*i+1] = 0.;
			B[(vnumdof+pnumdof)*0+3*i+2] = 0.;

			B[(vnumdof+pnumdof)*1+3*i+0] = 0.;
			B[(vnumdof+pnumdof)*1+3*i+1] = vbasis[i];
			B[(vnumdof+pnumdof)*1+3*i+2] = 0.;
		}
		for(int i=0;i<pnumnodes;i++){
			B[(vnumdof+pnumdof)*0+i+vnumdof+0] = 0.;
			B[(vnumdof+pnumdof)*1+i+vnumdof+0] = 0.;
		}
	}
	else{
		for(int i=0;i<vnumnodes;i++){
			B[2*i+0] = vbasis[i];
			B[2*i+1] = 0.;
		}

		for(int i=0;i<pnumnodes;i++){
			B[i+vnumdof+0] = 0.;
		}
	}

	/*Clean-up*/
	xDelete<IssmDouble>(vbasis);
}/*}}}*/
void StressbalanceAnalysis::GetSolutionFromInputsFS(Vector<IssmDouble>* solution,Element* element){/*{{{*/

	int*         vdoflist=NULL;
	int*         pdoflist=NULL;
	Input*       vz_input=NULL;
	int          meshtype,dim;
	IssmDouble   vx,vy,vz,p;
	IssmDouble   FSreconditioning;

	/*Get some parameters*/
	element->FindParam(&meshtype,MeshTypeEnum);
	element->FindParam(&FSreconditioning,StressbalanceFSreconditioningEnum);
	switch(meshtype){
		case Mesh2DverticalEnum: dim = 2; break;
		case Mesh3DEnum:         dim = 3; break;
		case Mesh3DtetrasEnum:   dim = 3; break;
		default: _error_("mesh "<<EnumToStringx(meshtype)<<" not supported yet");
	}

	/*Fetch number of nodes and dof for this finite element*/
	int vnumnodes = element->NumberofNodesVelocity();
	int pnumnodes = element->NumberofNodesPressure();
	int vnumdof   = vnumnodes*dim;
	int pnumdof   = pnumnodes*1;

	/*Initialize values*/
	IssmDouble* vvalues = xNew<IssmDouble>(vnumdof);
	IssmDouble* pvalues = xNew<IssmDouble>(pnumdof);

	/*Get dof list: */
	element->GetDofListVelocity(&vdoflist,GsetEnum);
	element->GetDofListPressure(&pdoflist,GsetEnum);
	Input*     vx_input=element->GetInput(VxEnum);       _assert_(vx_input);
	Input*     vy_input=element->GetInput(VyEnum);       _assert_(vy_input);
	if(dim==3){vz_input=element->GetInput(VzEnum);       _assert_(vz_input);}
	Input*     p_input =element->GetInput(PressureEnum); _assert_(p_input);

	element->FindParam(&FSreconditioning,StressbalanceFSreconditioningEnum);

	/*Ok, we have the velocities in inputs, fill in solution */
	Gauss* gauss = element->NewGauss();
	for(int i=0;i<vnumnodes;i++){
		gauss->GaussNode(element->VelocityInterpolation(),i);
		vx_input->GetInputValue(&vx,gauss);
		vy_input->GetInputValue(&vy,gauss);
		vvalues[i*dim+0]=vx;
		vvalues[i*dim+1]=vy;
		if(dim==3){
			vz_input->GetInputValue(&vz,gauss);
			vvalues[i*dim+2]=vz;
		}
	}
	for(int i=0;i<pnumnodes;i++){
		gauss->GaussNode(element->PressureInterpolation(),i);
		p_input->GetInputValue(&p ,gauss);
		pvalues[i]=p/FSreconditioning;
	}

	/*Add value to global vector*/
	solution->SetValues(vnumdof,vdoflist,vvalues,INS_VAL);
	solution->SetValues(pnumdof,pdoflist,pvalues,INS_VAL);

	/*Free ressources:*/
	delete gauss;
	xDelete<int>(pdoflist);
	xDelete<int>(vdoflist);
	xDelete<IssmDouble>(pvalues);
	xDelete<IssmDouble>(vvalues);
}/*}}}*/
void StressbalanceAnalysis::InitializeXTH(Elements* elements,Parameters* parameters){/*{{{*/

	/*Intermediaries*/
	int        meshtype,dim;
	IssmDouble dvx[3],dvy[3],dvz[3];
	IssmDouble viscosity;
	IssmDouble *xyz_list = NULL;

	/*Get problem dimension*/
	parameters->FindParam(&meshtype,MeshTypeEnum);
	switch(meshtype){
		case Mesh2DverticalEnum: dim = 2; break;
		case Mesh3DEnum:         dim = 3; break;
		case Mesh3DtetrasEnum:   dim = 3; break;
		default: _error_("mesh "<<EnumToStringx(meshtype)<<" not supported yet");
	}

	for(int i=0;i<elements->Size();i++){
		Element* element=dynamic_cast<Element*>(elements->GetObjectByOffset(i));

		/*Get inputs and parameters*/
		element->GetVerticesCoordinates(&xyz_list);
		Input* vx_input=element->GetInput(VxEnum); _assert_(vx_input);
		Input* vy_input=element->GetInput(VyEnum); _assert_(vy_input);
		Input* vz_input;
		if(dim==3){vz_input=element->GetInput(VzEnum); _assert_(vz_input);}

		/*Allocate new inputs*/
		int tnumnodes = element->GetNumberOfVertices();      //Tensors, P1 DG
		IssmDouble* epsxx = xNew<IssmDouble>(tnumnodes); IssmDouble* sigmapxx = xNew<IssmDouble>(tnumnodes);
		IssmDouble* epsyy = xNew<IssmDouble>(tnumnodes); IssmDouble* sigmapyy = xNew<IssmDouble>(tnumnodes); 
		IssmDouble* epsxy = xNew<IssmDouble>(tnumnodes); IssmDouble* sigmapxy = xNew<IssmDouble>(tnumnodes); 
		IssmDouble* epszz = NULL;                        IssmDouble* sigmapzz = NULL;  
		IssmDouble* epsxz = NULL;                        IssmDouble* sigmapxz = NULL;  
		IssmDouble* epsyz = NULL;                        IssmDouble* sigmapyz = NULL;  
		if(dim==3){
			epszz = xNew<IssmDouble>(tnumnodes); sigmapzz = xNew<IssmDouble>(tnumnodes);
			epsxz = xNew<IssmDouble>(tnumnodes); sigmapxz = xNew<IssmDouble>(tnumnodes); 
			epsyz = xNew<IssmDouble>(tnumnodes); sigmapyz = xNew<IssmDouble>(tnumnodes); 
		}

		/*Get d and tau*/
		Gauss* gauss = element->NewGauss();
		for(int i=0;i<tnumnodes;i++){
			gauss->GaussNode(P1DGEnum,i);

			vx_input->GetInputDerivativeValue(&dvx[0],xyz_list,gauss);
			vy_input->GetInputDerivativeValue(&dvy[0],xyz_list,gauss);
			if(dim==3){
				vz_input->GetInputDerivativeValue(&dvz[0],xyz_list,gauss);
			}

			element->ViscosityFS(&viscosity,dim,xyz_list,gauss,vx_input,vy_input,vz_input);
			epsxx[i] = dvx[0];                sigmapxx[i] = 2.*viscosity*epsxx[i];
			epsyy[i] = dvy[1];                sigmapyy[i] = 2.*viscosity*epsyy[i];
			epsxy[i] = 0.5*(dvx[1] + dvy[0]); sigmapxy[i] = 2.*viscosity*epsxy[i];
			if(dim==3){
				epszz[i] = dvz[2];                sigmapzz[i] = 2.*viscosity*epszz[i];
				epsxz[i] = 0.5*(dvx[2] + dvz[0]); sigmapxz[i] = 2.*viscosity*epsxz[i];
				epsyz[i] = 0.5*(dvy[2] + dvz[1]); sigmapyz[i] = 2.*viscosity*epsyz[i];
			}
		}

		/*Add inputs*/
		element->AddInput(StrainRatexxEnum,epsxx,P1DGEnum); element->AddInput(DeviatoricStressxxEnum,sigmapxx,P1DGEnum);
		element->AddInput(StrainRateyyEnum,epsyy,P1DGEnum); element->AddInput(DeviatoricStressyyEnum,sigmapyy,P1DGEnum);
		element->AddInput(StrainRatexyEnum,epsxy,P1DGEnum); element->AddInput(DeviatoricStressxyEnum,sigmapxy,P1DGEnum);
		if(dim==3){
			element->AddInput(StrainRatezzEnum,epszz,P1DGEnum); element->AddInput(DeviatoricStresszzEnum,sigmapzz,P1DGEnum);
			element->AddInput(StrainRatexzEnum,epsxz,P1DGEnum); element->AddInput(DeviatoricStressxzEnum,sigmapxz,P1DGEnum);
			element->AddInput(StrainRateyzEnum,epsyz,P1DGEnum); element->AddInput(DeviatoricStressyzEnum,sigmapyz,P1DGEnum);
		}

		/*Clean up*/
		xDelete<IssmDouble>(xyz_list);
		xDelete<IssmDouble>(epsxx);
		xDelete<IssmDouble>(epsyy);
		xDelete<IssmDouble>(epszz);
		xDelete<IssmDouble>(epsxy);
		xDelete<IssmDouble>(epsxz);
		xDelete<IssmDouble>(epsyz);
	}

}/*}}}*/
void StressbalanceAnalysis::InputUpdateFromSolutionFS(IssmDouble* solution,Element* element){/*{{{*/

	bool         results_on_nodes;
	int          i,dim,meshtype;
	int*         vdoflist=NULL;
	int*         pdoflist=NULL;
	IssmDouble   FSreconditioning;

	element->FindParam(&meshtype,MeshTypeEnum);
	element->FindParam(&FSreconditioning,StressbalanceFSreconditioningEnum);
	element->FindParam(&results_on_nodes,SettingsResultsOnNodesEnum);
	switch(meshtype){
		case Mesh2DverticalEnum: dim = 2; break;
		case Mesh3DEnum:         dim = 3; break;
		case Mesh3DtetrasEnum:   dim = 3; break;
		default: _error_("mesh "<<EnumToStringx(meshtype)<<" not supported yet");
	}

	/*Fetch number of nodes and dof for this finite element*/
	int vnumnodes = element->NumberofNodesVelocity();
	int pnumnodes = element->NumberofNodesPressure();
	int vnumdof   = vnumnodes*dim;
	int pnumdof   = pnumnodes*1;

	/*Initialize values*/
	IssmDouble* values   = xNew<IssmDouble>(vnumdof+pnumdof);
	IssmDouble* vx       = xNew<IssmDouble>(vnumnodes);
	IssmDouble* vy       = xNew<IssmDouble>(vnumnodes);
	IssmDouble* vz       = xNew<IssmDouble>(vnumnodes);
	IssmDouble* vel      = xNew<IssmDouble>(vnumnodes);
	IssmDouble* pressure = xNew<IssmDouble>(pnumnodes);

	/*Prepare coordinate system list*/
	int* cs_list = xNew<int>(vnumnodes+pnumnodes);
	if(dim==2) for(i=0;i<vnumnodes;i++) cs_list[i] = XYEnum;
	else       for(i=0;i<vnumnodes;i++) cs_list[i] = XYZEnum;
	for(i=0;i<pnumnodes;i++) cs_list[vnumnodes+i] = PressureEnum;

	/*Get dof list: */
	element->GetDofListVelocity(&vdoflist,GsetEnum);
	element->GetDofListPressure(&pdoflist,GsetEnum);

	/*Use the dof list to index into the solution vector: */
	for(i=0;i<vnumdof;i++) values[i]        =solution[vdoflist[i]];
	for(i=0;i<pnumdof;i++) values[vnumdof+i]=solution[pdoflist[i]];

	/*Transform solution in Cartesian Space*/
	element->TransformSolutionCoord(values,cs_list);

	/*Ok, we have vx and vy in values, fill in all arrays: */
	for(i=0;i<vnumnodes;i++){
		vx[i] = values[i*dim+0];
		vy[i] = values[i*dim+1];
		if(xIsNan<IssmDouble>(vx[i])) _error_("NaN found in solution vector");
		if(xIsNan<IssmDouble>(vy[i])) _error_("NaN found in solution vector");

		if(dim==3){
			vz[i] = values[i*dim+2];
			if(xIsNan<IssmDouble>(vz[i])) _error_("NaN found in solution vector");
		}
	}
	for(i=0;i<pnumnodes;i++){
		pressure[i] = values[vnumdof+i];
		if(xIsNan<IssmDouble>(pressure[i])) _error_("NaN found in solution vector");
	}

	/*Recondition pressure and compute vel: */
	for(i=0;i<pnumnodes;i++) pressure[i] = pressure[i]*FSreconditioning;
	if(dim==3) for(i=0;i<vnumnodes;i++) vel[i] = sqrt(vx[i]*vx[i] + vy[i]*vy[i] + vz[i]*vz[i]);
	else       for(i=0;i<vnumnodes;i++) vel[i] = sqrt(vx[i]*vx[i] + vy[i]*vy[i]);

	/*Now, we have to move the previous inputs  to old 
	 * status, otherwise, we'll wipe them off: */
	element->InputChangeName(VxEnum,VxPicardEnum);
	element->InputChangeName(VyEnum,VyPicardEnum);
	element->InputChangeName(PressureEnum,PressurePicardEnum);
	if(dim==3) element->InputChangeName(VzEnum,VzPicardEnum);

	/*Add vx and vy as inputs to the tria element: */
	if(results_on_nodes){
		element->AddInput(VxEnum,      vx,      element->VelocityInterpolation());
		element->AddInput(VyEnum,      vy,      element->VelocityInterpolation());
		element->AddInput(VelEnum,     vel,     element->VelocityInterpolation());
		element->AddInput(PressureEnum,pressure,element->PressureInterpolation());
		if(dim==3) element->AddInput(VzEnum,vz, element->VelocityInterpolation());
	}
	else{
		element->AddInput(VxEnum,vx,P1Enum);
		element->AddInput(VyEnum,vy,P1Enum);
		element->AddInput(VelEnum,vel,P1Enum);
		element->AddInput(PressureEnum,pressure,P1Enum);
		if(dim==3) element->AddInput(VzEnum,vz,P1Enum);
	}

	/*Free ressources:*/
	xDelete<IssmDouble>(pressure);
	xDelete<IssmDouble>(vel);
	xDelete<IssmDouble>(vz);
	xDelete<IssmDouble>(vy);
	xDelete<IssmDouble>(vx);
	xDelete<IssmDouble>(values);
	xDelete<int>(vdoflist);
	xDelete<int>(pdoflist);
	xDelete<int>(cs_list);
}/*}}}*/
void StressbalanceAnalysis::InputUpdateFromSolutionFSXTH(Elements* elements,Parameters* parameters){/*{{{*/

	/*Intermediaries*/
	int         dim,tausize,meshtype;
	IssmDouble  epsxx,epsyy,epszz,epsxy,epsxz,epsyz,D_scalar;
	IssmDouble  sigmapxx,sigmapyy,sigmapzz,sigmapxy,sigmapxz,sigmapyz;
	IssmDouble  dvx[3],dvy[3],dvz[3],B,n;
	IssmDouble *xyz_list = NULL;
	IssmDouble  Jdet,r;
	Gauss*      gauss = NULL;

	parameters->FindParam(&r,AugmentedLagrangianREnum);
	parameters->FindParam(&meshtype,MeshTypeEnum);
	switch(meshtype){
		case Mesh2DverticalEnum: dim = 2; tausize = 3; break;
		case Mesh3DEnum:         dim = 3; tausize = 6; break;
		case Mesh3DtetrasEnum:   dim = 3; tausize = 6; break;
		default: _error_("mesh "<<EnumToStringx(meshtype)<<" not supported yet");
	}

	for(int i=0;i<elements->Size();i++){
		Element* element=dynamic_cast<Element*>(elements->GetObjectByOffset(i));

		/*Get inputs and parameters*/
		element->GetVerticesCoordinates(&xyz_list);
		Input*  B_input=element->GetInput(MaterialsRheologyBEnum); _assert_(B_input);
		Input*  n_input=element->GetInput(MaterialsRheologyNEnum); _assert_(n_input);
		Input* vx_input=element->GetInput(VxEnum);                 _assert_(vx_input);
		Input* vy_input=element->GetInput(VyEnum);                 _assert_(vy_input);
		Input* vz_input;
		if(dim==3){vz_input=element->GetInput(VzEnum); _assert_(vz_input);}

		/*Fetch number of nodes and dof for this finite element*/
		int tnumnodes = element->GetNumberOfVertices();      //Tensors, P1 DG

		/*Initialize vectors*/
		IssmDouble* tbasis = xNew<IssmDouble>(tnumnodes);
		IssmDouble* Ke     = xNewZeroInit<IssmDouble>(tnumnodes*tnumnodes);
		IssmDouble* pe_xx  = xNewZeroInit<IssmDouble>(tnumnodes);
		IssmDouble* pe_yy  = xNewZeroInit<IssmDouble>(tnumnodes);
		IssmDouble* pe_xy  = xNewZeroInit<IssmDouble>(tnumnodes);
		IssmDouble* pe_zz  = NULL; IssmDouble* pe_xz  = NULL; IssmDouble* pe_yz  = NULL;
		if(dim==3){
			pe_zz = xNewZeroInit<IssmDouble>(tnumnodes);
			pe_xz = xNewZeroInit<IssmDouble>(tnumnodes);
			pe_yz = xNewZeroInit<IssmDouble>(tnumnodes);
		}

		/*Get tau*/
		Input* sigmapxx_input=element->GetInput(DeviatoricStressxxEnum); _assert_(sigmapxx_input);
		Input* sigmapyy_input=element->GetInput(DeviatoricStressyyEnum); _assert_(sigmapyy_input);
		Input* sigmapxy_input=element->GetInput(DeviatoricStressxyEnum); _assert_(sigmapxy_input);
		Input* sigmapzz_input=NULL; Input* sigmapxz_input=NULL; Input* sigmapyz_input=NULL;
		if(dim==3){
			sigmapzz_input=element->GetInput(DeviatoricStresszzEnum); _assert_(sigmapzz_input);
			sigmapxz_input=element->GetInput(DeviatoricStressxzEnum); _assert_(sigmapxz_input);
			sigmapyz_input=element->GetInput(DeviatoricStressyzEnum); _assert_(sigmapyz_input);
		}

		gauss=element->NewGauss(5);
		for(int ig=gauss->begin();ig<gauss->end();ig++){
			gauss->GaussPoint(ig);
			element->JacobianDeterminant(&Jdet,xyz_list,gauss);
			element->NodalFunctionsTensor(tbasis,gauss);

			/*Get tau from inputs*/
			sigmapxx_input->GetInputValue(&sigmapxx,gauss);
			sigmapyy_input->GetInputValue(&sigmapyy,gauss);
			sigmapxy_input->GetInputValue(&sigmapxy,gauss);
			if(dim==3){
				sigmapzz_input->GetInputValue(&sigmapzz,gauss);
				sigmapxz_input->GetInputValue(&sigmapxz,gauss);
				sigmapyz_input->GetInputValue(&sigmapyz,gauss);
			}

			/*Calculate d from previous results*/
			vx_input->GetInputDerivativeValue(&dvx[0],xyz_list,gauss);
			vy_input->GetInputDerivativeValue(&dvy[0],xyz_list,gauss);
			if(dim==3){
				vz_input->GetInputDerivativeValue(&dvz[0],xyz_list,gauss);
			}
			epsxx = dvx[0];
			epsyy = dvy[1];
			epsxy = 0.5*(dvx[1] + dvy[0]); 
			if(dim==3){
				epszz = dvz[2];               
				epsxz = 0.5*(dvx[2] + dvz[0]);
				epsyz = 0.5*(dvy[2] + dvz[1]);
			}

			/*Solve 2 eta_0 |d|^s-1 + r |d| = |rD(u) + tau|*/
			IssmDouble coef1,coef2,coef3;
			B_input->GetInputValue(&B,gauss);
			n_input->GetInputValue(&n,gauss);
			coef1 = (B/2.)*pow(1./sqrt(2.),(1.-n)/n); //2 eta_0
			coef2 = r;
			if(dim==2){
				coef3 = sqrt(
							  (r*epsxx + sigmapxx)*(r*epsxx + sigmapxx)
							+ (r*epsyy + sigmapyy)*(r*epsyy + sigmapyy)
							+ 2*(r*epsxy + sigmapxy)*(r*epsxy + sigmapxy)
							);
			}
			else{
				coef3 = sqrt(
					  		  (r*epsxx + sigmapxx)*(r*epsxx + sigmapxx)
							+ (r*epsyy + sigmapyy)*(r*epsyy + sigmapyy)
							+ (r*epszz + sigmapzz)*(r*epszz + sigmapzz)
							+ 2*(r*epsxy + sigmapxy)*(r*epsxy + sigmapxy)
							+ 2*(r*epsxz + sigmapxz)*(r*epsxz + sigmapxz)
							+ 2*(r*epsyz + sigmapyz)*(r*epsyz + sigmapyz)
							);
			}
			IssmDouble dnorm;
			NewtonSolveDnorm(&dnorm,coef1,coef2,coef3,n);

			/*Create Ke*/
			D_scalar=(coef1*pow(dnorm,(1.-n)/n)+r)*gauss->weight*Jdet;
			TripleMultiply(tbasis,tnumnodes,1,0,
						&D_scalar,1,1,0,
						tbasis,1,tnumnodes,0,
						Ke,1);

			/*Create Right hand sides*/
			for(int ii=0;ii<tnumnodes;ii++) pe_xx[ii] += (r*epsxx+sigmapxx)*tbasis[ii]*gauss->weight*Jdet;
			for(int ii=0;ii<tnumnodes;ii++) pe_yy[ii] += (r*epsyy+sigmapyy)*tbasis[ii]*gauss->weight*Jdet;
			for(int ii=0;ii<tnumnodes;ii++) pe_xy[ii] += (r*epsxy+sigmapxy)*tbasis[ii]*gauss->weight*Jdet;
			if(dim==3){
				for(int ii=0;ii<tnumnodes;ii++) pe_zz[ii] += (r*epszz+sigmapzz)*tbasis[ii]*gauss->weight*Jdet;
				for(int ii=0;ii<tnumnodes;ii++) pe_xz[ii] += (r*epsxz+sigmapxz)*tbasis[ii]*gauss->weight*Jdet;
				for(int ii=0;ii<tnumnodes;ii++) pe_yz[ii] += (r*epsyz+sigmapyz)*tbasis[ii]*gauss->weight*Jdet;
			}
		}

		/*Solve the systems*/
		IssmDouble* d_xx = xNew<IssmDouble>(tnumnodes);
		IssmDouble* d_yy = xNew<IssmDouble>(tnumnodes);
		IssmDouble* d_xy = xNew<IssmDouble>(tnumnodes);
		IssmDouble* d_zz = NULL;
		IssmDouble* d_xz = NULL;
		IssmDouble* d_yz = NULL;
		if(dim==2){
			_assert_(tnumnodes==3);
			Matrix3x3Solve(&d_xx[0],Ke,pe_xx);
			Matrix3x3Solve(&d_yy[0],Ke,pe_yy);
			Matrix3x3Solve(&d_xy[0],Ke,pe_xy);
			element->AddInput(StrainRatexxEnum,d_xx,P1DGEnum);
			element->AddInput(StrainRateyyEnum,d_yy,P1DGEnum);
			element->AddInput(StrainRatexyEnum,d_xy,P1DGEnum);
		}
		else{
			_assert_(tnumnodes==4);
			Matrix3x3Solve(&d_xx[0],Ke,pe_xx);
			Matrix3x3Solve(&d_yy[0],Ke,pe_yy);
			Matrix3x3Solve(&d_xy[0],Ke,pe_xy);
			Matrix3x3Solve(&d_zz[0],Ke,pe_zz);
			Matrix3x3Solve(&d_xz[0],Ke,pe_xz);
			Matrix3x3Solve(&d_yz[0],Ke,pe_yz);
			element->AddInput(StrainRatexxEnum,d_xx,P1DGEnum);
			element->AddInput(StrainRateyyEnum,d_yy,P1DGEnum);
			element->AddInput(StrainRatexyEnum,d_xy,P1DGEnum);
			element->AddInput(StrainRatezzEnum,d_zz,P1DGEnum);
			element->AddInput(StrainRatexzEnum,d_xz,P1DGEnum);
			element->AddInput(StrainRateyzEnum,d_yz,P1DGEnum);
		}

		/*Update tau accordingly*/
		IssmDouble* tau_xx = xNew<IssmDouble>(tnumnodes);
		IssmDouble* tau_yy = xNew<IssmDouble>(tnumnodes);
		IssmDouble* tau_xy = xNew<IssmDouble>(tnumnodes);
		IssmDouble* tau_zz = NULL;
		IssmDouble* tau_xz = NULL;
		IssmDouble* tau_yz = NULL;
		Input* epsxx_input=element->GetInput(StrainRatexxEnum); _assert_(epsxx_input);
		Input* epsyy_input=element->GetInput(StrainRateyyEnum); _assert_(epsyy_input);
		Input* epsxy_input=element->GetInput(StrainRatexyEnum); _assert_(epsxy_input);
		Input* epszz_input=NULL; Input* epsxz_input=NULL; Input* epsyz_input=NULL;
		if(dim==3){
			epszz_input=element->GetInput(StrainRatezzEnum); _assert_(epszz_input);
			epsxz_input=element->GetInput(StrainRatexzEnum); _assert_(epsxz_input);
			epsyz_input=element->GetInput(StrainRateyzEnum); _assert_(epsyz_input);
		}
		Gauss* gauss = element->NewGauss();
		for(int ig=0;ig<tnumnodes;ig++){
			gauss->GaussNode(P1DGEnum,ig);

			/*Get D(u)*/
			vx_input->GetInputDerivativeValue(&dvx[0],xyz_list,gauss);
			vy_input->GetInputDerivativeValue(&dvy[0],xyz_list,gauss);
			if(dim==3){
				vz_input->GetInputDerivativeValue(&dvz[0],xyz_list,gauss);
			}
			epsxx = dvx[0];
			epsyy = dvy[1];
			epsxy = 0.5*(dvx[1] + dvy[0]); 
			if(dim==3){
				epszz = dvz[2]; 
				epsxz = 0.5*(dvx[2] + dvz[0]);
				epsyz = 0.5*(dvy[2] + dvz[1]);
			}

			/*Get tau^(n-1) from inputs*/
			sigmapxx_input->GetInputValue(&sigmapxx,gauss);
			sigmapyy_input->GetInputValue(&sigmapyy,gauss);
			sigmapxy_input->GetInputValue(&sigmapxy,gauss);
			if(dim==3){
				sigmapzz_input->GetInputValue(&sigmapzz,gauss);
				sigmapxz_input->GetInputValue(&sigmapxz,gauss);
				sigmapyz_input->GetInputValue(&sigmapyz,gauss);
			}

			/*Get d and update tau accordingly*/
			tau_xx[ig] = sigmapxx + r*(epsxx - d_xx[ig]);
			tau_yy[ig] = sigmapyy + r*(epsyy - d_yy[ig]);
			tau_xy[ig] = sigmapxy + r*(epsxy - d_xy[ig]);
			if(dim==3){
				tau_zz[ig] = sigmapzz + r*(epszz - d_zz[ig]);
				tau_xz[ig] = sigmapxz + r*(epsxz - d_xz[ig]);
				tau_yz[ig] = sigmapyz + r*(epsyz - d_yz[ig]);
			}
		}

		/*Add inputs*/
		element->AddInput(StrainRatexxEnum,tau_xx,P1DGEnum);
		element->AddInput(StrainRateyyEnum,tau_yy,P1DGEnum);
		element->AddInput(StrainRatexyEnum,tau_xy,P1DGEnum);
		if(dim==3){
			element->AddInput(StrainRatezzEnum,tau_zz,P1DGEnum);
			element->AddInput(StrainRatexzEnum,tau_xz,P1DGEnum);
			element->AddInput(StrainRateyzEnum,tau_yz,P1DGEnum);
		}

		/*Clean up and */
		xDelete<IssmDouble>(xyz_list);
		xDelete<IssmDouble>(tbasis);
		xDelete<IssmDouble>(Ke);
		xDelete<IssmDouble>(pe_xx); xDelete<IssmDouble>(d_xx);   xDelete<IssmDouble>(tau_xx);
		xDelete<IssmDouble>(pe_yy); xDelete<IssmDouble>(d_yy);   xDelete<IssmDouble>(tau_yy);
		xDelete<IssmDouble>(pe_zz); xDelete<IssmDouble>(d_zz);   xDelete<IssmDouble>(tau_zz);
		xDelete<IssmDouble>(pe_xy); xDelete<IssmDouble>(d_xy);   xDelete<IssmDouble>(tau_xy);
		xDelete<IssmDouble>(pe_xz); xDelete<IssmDouble>(d_xz);   xDelete<IssmDouble>(tau_xz);
		xDelete<IssmDouble>(pe_yz); xDelete<IssmDouble>(d_yz);   xDelete<IssmDouble>(tau_yz);
	}

	delete gauss;

}/*}}}*/

/*Coupling (Tiling)*/
ElementMatrix* StressbalanceAnalysis::CreateKMatrixSSA3d(Element* element){/*{{{*/

	/*compute all stiffness matrices for this element*/
	ElementMatrix* Ke1=CreateKMatrixSSA3dViscous(element);
	ElementMatrix* Ke2=CreateKMatrixSSA3dFriction(element);
	ElementMatrix* Ke =new ElementMatrix(Ke1,Ke2);

	/*clean-up and return*/
	delete Ke1;
	delete Ke2;
	return Ke;
}/*}}}*/
ElementMatrix* StressbalanceAnalysis::CreateKMatrixSSA3dFriction(Element* element){/*{{{*/

	/*Initialize Element matrix and return if necessary*/
	if(element->IsFloating() || !element->IsOnBed()) return NULL;

	/*Build a tria element using the 3 nodes of the base of the penta. Then use 
	 * the tria functionality to build a friction stiffness matrix on these 3
	 * nodes: */
	Element* basalelement = element->SpawnBasalElement();
	ElementMatrix* Ke=CreateKMatrixSSAFriction(basalelement);
	basalelement->DeleteMaterials(); delete basalelement;

	/*clean-up and return*/
	return Ke;
}/*}}}*/
ElementMatrix* StressbalanceAnalysis::CreateKMatrixSSA3dViscous(Element* element){/*{{{*/

	/*Constants*/
	const int    numdof2d=2*3;

	/*Intermediaries */
	int         i,j,approximation;
	int         dim=3;
	IssmDouble  Jdet,viscosity,oldviscosity,newviscosity,viscosity_overshoot;
	IssmDouble  epsilon[5],oldepsilon[5];       /* epsilon=[exx,eyy,exy,exz,eyz];*/
	IssmDouble  epsilons[6];                    //6 for FS
	IssmDouble  B[3][numdof2d];
	IssmDouble  Bprime[3][numdof2d];
	IssmDouble  D[3][3]= {0.0};                 // material matrix, simple scalar matrix.
	IssmDouble  D_scalar;
	IssmDouble  Ke_gg[numdof2d][numdof2d]={0.0};
	IssmDouble  *xyz_list  = NULL;

	/*Find penta on bed as this is a SSA elements: */
	Element* pentabase=element->GetBasalElement();
	Element* basaltria=pentabase->SpawnBasalElement();

	/*Initialize Element matrix*/
	ElementMatrix* Ke=basaltria->NewElementMatrix(SSAApproximationEnum);
	element->GetInputValue(&approximation,ApproximationEnum);

	/*Retrieve all inputs and parameters*/
	element->GetVerticesCoordinates(&xyz_list);
	element->FindParam(&viscosity_overshoot,StressbalanceViscosityOvershootEnum);
	Input* vx_input   =element->GetInput(VxEnum);       _assert_(vx_input);
	Input* vy_input   =element->GetInput(VyEnum);       _assert_(vy_input);
	Input* vxold_input=element->GetInput(VxPicardEnum); _assert_(vxold_input);
	Input* vyold_input=element->GetInput(VyPicardEnum); _assert_(vyold_input);
	Input* vz_input   =element->GetInput(VzEnum);       _assert_(vz_input);

	/* Start  looping on the number of gaussian points: */
	Gauss* gauss=element->NewGauss(5);
	Gauss* gauss_tria=new GaussTria();
	for(int ig=gauss->begin();ig<gauss->end();ig++){

		gauss->GaussPoint(ig);
		gauss->SynchronizeGaussBase(gauss_tria);

		element->JacobianDeterminant(&Jdet, xyz_list,gauss);
		this->GetBSSA(&B[0][0],basaltria,2,xyz_list, gauss_tria);
		this->GetBSSAprime(&Bprime[0][0],basaltria,2,xyz_list, gauss_tria);

		if(approximation==SSAHOApproximationEnum){
			element->ViscosityHO(&viscosity,dim,xyz_list,gauss,vx_input,vy_input);
			element->ViscosityHO(&oldviscosity,dim,xyz_list,gauss,vxold_input,vyold_input);
			newviscosity=viscosity+viscosity_overshoot*(viscosity-oldviscosity);
		}
		else if (approximation==SSAFSApproximationEnum){
			element->ViscosityFS(&newviscosity,dim,xyz_list,gauss,vx_input,vy_input,vz_input);
		}
		else _error_("approximation " << approximation << " (" << EnumToStringx(approximation) << ") not supported yet");

		D_scalar=2*newviscosity*gauss->weight*Jdet;
		for (i=0;i<3;i++) D[i][i]=D_scalar;

		TripleMultiply( &B[0][0],3,numdof2d,1,
					&D[0][0],3,3,0,
					&Bprime[0][0],3,numdof2d,0,
					&Ke_gg[0][0],1);

	}
	for(i=0;i<numdof2d;i++) for(j=0;j<numdof2d;j++) Ke->values[i*numdof2d+j]+=Ke_gg[i][j];

	/*Transform Coordinate System*/
	basaltria->TransformStiffnessMatrixCoord(Ke,XYEnum);

	/*Clean up and return*/
	xDelete<IssmDouble>(xyz_list);
	delete basaltria->material;
	delete basaltria;
	delete gauss_tria;
	delete gauss;
	return Ke;

}/*}}}*/
ElementMatrix* StressbalanceAnalysis::CreateKMatrixHOFS(Element* element){/*{{{*/

	/*compute all stiffness matrices for this element*/
	ElementMatrix* Ke1=CreateKMatrixFS(element);
	int indices[3]={18,19,20};
	Ke1->StaticCondensation(3,&indices[0]);
	int init = element->FiniteElement();
	element->SetTemporaryElementType(P1Enum); // P1 needed for HO
	ElementMatrix* Ke2=CreateKMatrixHO(element);
	element->SetTemporaryElementType(init); // P1 needed for HO
	ElementMatrix* Ke3=CreateKMatrixCouplingHOFS(element);
	ElementMatrix* Ke =new ElementMatrix(Ke1,Ke2,Ke3);

	/*clean-up and return*/
	delete Ke1;
	delete Ke2;
	delete Ke3;
	return Ke;
}/*}}}*/
ElementMatrix* StressbalanceAnalysis::CreateKMatrixSSAHO(Element* element){/*{{{*/

	/*compute all stiffness matrices for this element*/
	ElementMatrix* Ke1=CreateKMatrixSSA3d(element);
	ElementMatrix* Ke2=CreateKMatrixHO(element);
	ElementMatrix* Ke3=CreateKMatrixCouplingSSAHO(element);
	ElementMatrix* Ke =new ElementMatrix(Ke1,Ke2,Ke3);

	/*clean-up and return*/
	delete Ke1;
	delete Ke2;
	delete Ke3;
	return Ke;
}/*}}}*/
ElementMatrix* StressbalanceAnalysis::CreateKMatrixSSAFS(Element* element){/*{{{*/

	/*compute all stiffness matrices for this element*/
	ElementMatrix* Ke1=CreateKMatrixFS(element);
	int indices[3]={18,19,20};
	Ke1->StaticCondensation(3,&indices[0]);
	int init = element->FiniteElement();
	element->SetTemporaryElementType(P1Enum); 
	ElementMatrix* Ke2=CreateKMatrixSSA3d(element);
	element->SetTemporaryElementType(init); 
	ElementMatrix* Ke3=CreateKMatrixCouplingSSAFS(element);
	ElementMatrix* Ke =new ElementMatrix(Ke1,Ke2,Ke3);

	/*clean-up and return*/
	delete Ke1;
	delete Ke2;
	delete Ke3;
	return Ke;
}/*}}}*/
ElementMatrix* StressbalanceAnalysis::CreateKMatrixCouplingHOFS(Element* element){/*{{{*/

	/*Constants*/
	int numnodes       = 3*6+1;
	int numdofp        = 2*6;
	int numdofs        = 4*6 + 3;
	int numdoftotal    = (2+4)*6+ 3;

	/*Intermediaries*/
	int   i,j,init;
	int*   cs_list     = xNew<int>(6*3+1);
	int*   cs_list2    = xNew<int>(6*2+1);
	Node  **node_list  = xNew<Node*>(6*3+1);

	/*Some parameters needed*/
	init = element->FiniteElement();

	/*prepare node list*/
	for(i=0;i<6+1;i++){
		node_list[i+6] = element->GetNode(i);
		cs_list[i+6]   = XYZEnum;
		cs_list2[i]    = XYZEnum;
	}
	for(i=0;i<6;i++){
		node_list[i]       = element->GetNode(i);
		node_list[i+2*6+1] = element->GetNode(i+6*1);
		cs_list[i]         = XYEnum;
		cs_list[i+2*6+1]   = PressureEnum;
		cs_list2[i+6+1]    = PressureEnum;
	}

	/*compute all stiffness matrices for this element*/
	ElementMatrix* Ke1=element->NewElementMatrixCoupling(6,HOApproximationEnum);
	ElementMatrix* Ke2=element->NewElementMatrix(FSvelocityEnum);
	ElementMatrix* Ke=new ElementMatrix(Ke1,Ke2);
	delete Ke1; delete Ke2;

	/*Compute HO Matrix with P1 element type\n");*/
	Ke1=CreateKMatrixFS(element); element->TransformInvStiffnessMatrixCoord(Ke1,node_list,2*6+1,cs_list2);
	int indices[3]={18,19,20};
	Ke1->StaticCondensation(3,&indices[0]);
	element->SetTemporaryElementType(P1Enum); // P1 needed for HO
	Ke2=CreateKMatrixHO(element); element->TransformInvStiffnessMatrixCoord(Ke2,XYEnum);
	element->SetTemporaryElementType(init); // P1 needed for HO
	/*Compute FS Matrix and condense it \n");*/

	for(i=0;i<numdofs;i++) for(j=0;j<6;j++){
		Ke->values[(i+numdofp)*numdoftotal+2*j+0]+=Ke1->values[i*numdofs+3*j+0];
		Ke->values[(i+numdofp)*numdoftotal+2*j+1]+=Ke1->values[i*numdofs+3*j+1];
	}
	for(i=0;i<numdofp;i++) for(j=0;j<6;j++){
		Ke->values[i*numdoftotal+numdofp+3*j+0]+=Ke2->values[i*numdofp+2*j+0];
		Ke->values[i*numdoftotal+numdofp+3*j+1]+=Ke2->values[i*numdofp+2*j+1];
	}

	/*Transform Coordinate System*/ //Do not transform, already done in the matrices
	element->TransformStiffnessMatrixCoord(Ke,node_list,numnodes,cs_list);

	/*clean-up and return*/
	xDelete<int>(cs_list);
	xDelete<int>(cs_list2);
	xDelete<Node*>(node_list);
	delete Ke1;
	delete Ke2;
	return Ke;
}/*}}}*/
ElementMatrix* StressbalanceAnalysis::CreateKMatrixCouplingSSAHO(Element* element){/*{{{*/

	/*compute all stiffness matrices for this element*/
	ElementMatrix* Ke1=CreateKMatrixCouplingSSAHOViscous(element);
	ElementMatrix* Ke2=CreateKMatrixCouplingSSAHOFriction(element);
	ElementMatrix* Ke=new ElementMatrix(Ke1,Ke2);

	/*clean-up and return*/
	delete Ke1;
	delete Ke2;
	return Ke;
}/*}}}*/
ElementMatrix* StressbalanceAnalysis::CreateKMatrixCouplingSSAHOFriction(Element* element){/*{{{*/

	if(element->IsFloating() || !element->IsOnBed()) return NULL;

	/*Constants*/
	int numnodes    = element->GetNumberOfNodes();
	int numdof      = 2*numnodes;
	int numdoftotal = 4*numnodes;

	/*Intermediaries */
	int         i,j;
	IssmDouble  Jdet2d,alpha2;
	IssmDouble *xyz_list_tria = NULL;
	IssmDouble* L             = xNewZeroInit<IssmDouble>(2*numdof);
	IssmDouble  DL[2][2]      = {{ 0,0 },{0,0}}; //for basal drag
	IssmDouble  DL_scalar;
	IssmDouble* Ke_gg         = xNewZeroInit<IssmDouble>(numdof*numdof);
	Node      **node_list     = xNew<Node*>(2*numnodes);
	int*        cs_list       = xNew<int>(2*numnodes);

	/*Initialize Element matrix and return if necessary*/
	ElementMatrix* Ke1=element->NewElementMatrix(SSAApproximationEnum);
	ElementMatrix* Ke2=element->NewElementMatrix(HOApproximationEnum);
	ElementMatrix* Ke=new ElementMatrix(Ke1,Ke2);
	delete Ke1; delete Ke2;

	/*Prepare node list*/
	for(i=0;i<numnodes;i++){
		node_list[i+0*numnodes] = element->GetNode(i);
		node_list[i+1*numnodes] = element->GetNode(i);
		cs_list[i+0*numnodes] = XYEnum;
		cs_list[i+1*numnodes] = XYEnum;
	}

	/*retrieve inputs :*/
	element->GetVerticesCoordinatesBase(&xyz_list_tria);
	Input* vx_input=element->GetInput(VxEnum);  _assert_(vx_input);
	Input* vy_input=element->GetInput(VyEnum);  _assert_(vy_input);
	Input* vz_input=element->GetInput(VzEnum);  _assert_(vz_input);

	/*build friction object, used later on: */
	Friction* friction=new Friction(element,2);

	/* Start  looping on the number of gaussian points: */
	Gauss* gauss=element->NewGaussBase(2);
	for(int ig=gauss->begin();ig<gauss->end();ig++){

		gauss->GaussPoint(ig);

		/*Friction: */
		friction->GetAlpha2(&alpha2,gauss,vx_input,vy_input,vz_input);
		element->JacobianDeterminantBase(&Jdet2d, xyz_list_tria,gauss);
		this->GetBHOFriction(L,element,3,xyz_list_tria,gauss);

		DL_scalar=alpha2*gauss->weight*Jdet2d;
		for (i=0;i<2;i++) DL[i][i]=DL_scalar; 

		/*  Do the triple producte tL*D*L: */
		TripleMultiply( L,2,numdof,1,
					&DL[0][0],2,2,0,
					L,2,numdof,0,
					Ke_gg,1);
	}

	for(i=0;i<numdof;i++) for(j=0;j<numdof;j++) Ke->values[i*numdoftotal+(numdof+j)]+=Ke_gg[i*numdof+j];
	for(i=0;i<numdof;i++) for(j=0;j<numdof;j++) Ke->values[(i+numdof)*numdoftotal+j]+=Ke_gg[i*numdof+j];

	/*Transform Coordinate System*/
	element->TransformStiffnessMatrixCoord(Ke,node_list,2*numnodes,cs_list);

	/*Clean up and return*/
	delete gauss;
	delete friction;
	xDelete<int>(cs_list);
	xDelete<Node*>(node_list);
	xDelete<IssmDouble>(xyz_list_tria);
	xDelete<IssmDouble>(Ke_gg);
	xDelete<IssmDouble>(L);
	return Ke;
}/*}}}*/
ElementMatrix* StressbalanceAnalysis::CreateKMatrixCouplingSSAHOViscous(Element* element){/*{{{*/

	/*Constants*/
	int numnodes    = element->GetNumberOfNodes();
	int numdofm     = 1 *numnodes; //*2/2
	int numdofp     = 2 *numnodes;
	int numdoftotal = 2 *2 *numnodes;//2 dof per nodes and 2 sets of nodes for HO and SSA

	/*Intermediaries */
	int         i,j;
	IssmDouble  Jdet,viscosity,oldviscosity,newviscosity,viscosity_overshoot; //viscosity
	IssmDouble  *xyz_list      = NULL;
	IssmDouble* B              = xNew<IssmDouble>(3*numdofp);
	IssmDouble* Bprime         = xNew<IssmDouble>(3*numdofm);
	IssmDouble  D[3][3]={0.0}; // material matrix, simple scalar matrix.
	IssmDouble  D_scalar;
	IssmDouble* Ke_gg          = xNewZeroInit<IssmDouble>(numdofp*numdofm);
	Node       **node_list     = xNew<Node*>(2*numnodes);
	int*         cs_list= xNew<int>(2*numnodes);

	/*Find penta on bed as HO must be coupled to the dofs on the bed: */
	Element* pentabase=element->GetBasalElement();
	Element* basaltria=pentabase->SpawnBasalElement();

	/*prepare node list*/
	for(i=0;i<numnodes;i++){
		node_list[i+0*numnodes] = pentabase->GetNode(i);
		node_list[i+1*numnodes] = element  ->GetNode(i);
		cs_list[i+0*numnodes] = XYEnum;
		cs_list[i+1*numnodes] = XYEnum;
	}

	/*Initialize Element matrix*/
	ElementMatrix* Ke1= pentabase->NewElementMatrix(SSAApproximationEnum);
	ElementMatrix* Ke2= element  ->NewElementMatrix(HOApproximationEnum);
	ElementMatrix* Ke =new ElementMatrix(Ke1,Ke2);
	delete Ke1; delete Ke2;

	/* Get node coordinates and dof list: */
	element->GetVerticesCoordinates(&xyz_list);
	element->FindParam(&viscosity_overshoot,StressbalanceViscosityOvershootEnum);
	Input* vx_input   =element->GetInput(VxEnum);       _assert_(vx_input);
	Input* vy_input   =element->GetInput(VyEnum);       _assert_(vy_input);
	Input* vxold_input=element->GetInput(VxPicardEnum); _assert_(vxold_input);
	Input* vyold_input=element->GetInput(VyPicardEnum); _assert_(vyold_input);

	/* Start  looping on the number of gaussian points: */
	Gauss* gauss=element->NewGauss(5);
	Gauss* gauss_tria=new GaussTria();
	for(int ig=gauss->begin();ig<gauss->end();ig++){

		gauss->GaussPoint(ig);
		gauss->SynchronizeGaussBase(gauss_tria);

		element->JacobianDeterminant(&Jdet, xyz_list,gauss);
		this->GetBSSAHO(B, element,xyz_list, gauss);
		this->GetBSSAprime(Bprime,basaltria,2,xyz_list, gauss_tria); 
		element->ViscosityHO(&viscosity,3,xyz_list,gauss,vx_input,vy_input);
		element->ViscosityHO(&oldviscosity,3,xyz_list,gauss,vxold_input,vyold_input);

		newviscosity=viscosity+viscosity_overshoot*(viscosity-oldviscosity);
		D_scalar=2*newviscosity*gauss->weight*Jdet;
		for (i=0;i<3;i++) D[i][i]=D_scalar;

		TripleMultiply( B,3,numdofp,1,
					&D[0][0],3,3,0,
					Bprime,3,numdofm,0,
					Ke_gg,1);
	} 
	for(i=0;i<numdofp;i++) for(j=0;j<numdofm;j++) Ke->values[(i+2*numdofm)*numdoftotal+j]+=Ke_gg[i*numdofm+j];
	for(i=0;i<numdofm;i++) for(j=0;j<numdofp;j++) Ke->values[i*numdoftotal+(j+2*numdofm)]+=Ke_gg[j*numdofm+i];

	/*Transform Coordinate System*/
	element->TransformStiffnessMatrixCoord(Ke,node_list,2*numnodes,cs_list);

	/*Clean-up and return*/
	basaltria->DeleteMaterials(); delete basaltria;
	
	delete gauss;
	delete gauss_tria;
	xDelete<IssmDouble>(B);
	xDelete<IssmDouble>(Bprime);
	xDelete<IssmDouble>(Ke_gg);
	xDelete<IssmDouble>(xyz_list);
	xDelete<Node*>(node_list);
	xDelete<int>(cs_list);
	return Ke;

}/*}}}*/
ElementMatrix* StressbalanceAnalysis::CreateKMatrixCouplingSSAFS(Element* element){/*{{{*/

	/*compute all stiffness matrices for this element*/
	ElementMatrix* Ke1=CreateKMatrixCouplingSSAFSViscous(element);
	ElementMatrix* Ke2=CreateKMatrixCouplingSSAFSFriction(element);
	ElementMatrix* Ke =new ElementMatrix(Ke1,Ke2);

	/*clean-up and return*/
	delete Ke1;
	delete Ke2;
	return Ke;
}/*}}}*/
ElementMatrix* StressbalanceAnalysis::CreateKMatrixCouplingSSAFSFriction(Element* element){/*{{{*/

	/*Constants*/
	const int numdofs   = (6+1)*3 + 6*1;
	const int numdofm   = 6 *2;
	const int numdof2d  = 3 *3;
	const int numdof2dm = 3 *2;
	const int numdoftot = 6*2 + (6+1)*3 +6; // HO + FS vel + FS Pressure

	/*Intermediaries */
	int        i,j,approximation;
	int        dim=3;
	IssmDouble FSreconditioning,viscosity,alpha2_gauss,Jdet2d;
	IssmDouble bed_normal[3];
	IssmDouble LSSAFS[8][numdof2dm];
	IssmDouble LprimeSSAFS[8][numdofs];
	IssmDouble DLSSAFS[8][8]={0.0};
	IssmDouble LFSSSA[4][numdof2d];
	IssmDouble LprimeFSSSA[4][numdof2dm];
	IssmDouble DLFSSSA[4][4]={0.0};
	IssmDouble Ke_drag[numdof2dm][numdofs]={0.0};
	IssmDouble Ke_drag2[numdof2d][numdof2dm]={0.0};
	IssmDouble *xyz_list      = NULL;
	IssmDouble *xyz_list_tria = NULL;

	/*If on water or not FS, skip stiffness: */
	element->GetInputValue(&approximation,ApproximationEnum);
	if(element->IsFloating() || !element->IsOnBed()) return NULL;

	int vnumnodes = element->NumberofNodesVelocity();
	int pnumnodes = element->NumberofNodesPressure();
	int numnodes  = 2*vnumnodes-1+pnumnodes;

	/*Prepare node list*/
	int* cs_list = xNew<int>(2*vnumnodes-1+pnumnodes);
	Node **node_list = xNew<Node*>(2*vnumnodes-1+pnumnodes);
	for(i=0;i<vnumnodes-1;i++){
		node_list[i] = element->GetNode(i);
		cs_list[i]   = XYEnum;
	}
	for(i=0;i<vnumnodes;i++){
		node_list[i+vnumnodes-1] = element->GetNode(i);
		cs_list[i+vnumnodes-1]   = XYZEnum;
	}
	for(i=0;i<pnumnodes;i++){
		node_list[2*vnumnodes-1+i] = element->GetNode(vnumnodes+i);
		cs_list[2*vnumnodes-1+i]   = PressureEnum;
	}

	ElementMatrix* Ke1=element->NewElementMatrixCoupling(6,SSAApproximationEnum);
	ElementMatrix* Ke2=element->NewElementMatrix(FSvelocityEnum);
	ElementMatrix* Ke=new ElementMatrix(Ke1,Ke2);
	delete Ke1; delete Ke2;

	/*Retrieve all inputs and parameters*/
	element->GetVerticesCoordinates(&xyz_list);
	element->GetVerticesCoordinatesBase(&xyz_list_tria);
	element->FindParam(&FSreconditioning,StressbalanceFSreconditioningEnum);
	Input* vx_input=element->GetInput(VxEnum); _assert_(vx_input);
	Input* vy_input=element->GetInput(VyEnum); _assert_(vy_input);
	Input* vz_input=element->GetInput(VzEnum); _assert_(vz_input);

	/*build friction object, used later on: */
	Friction* friction=new Friction(element,3);

	/* Start  looping on the number of gaussian points: */
	Gauss* gauss=element->NewGaussBase(2);
	for(int ig=gauss->begin();ig<gauss->end();ig++){

		gauss->GaussPoint(ig);

		element->JacobianDeterminantBase(&Jdet2d,xyz_list_tria,gauss);
		this->GetLSSAFS(&LSSAFS[0][0], element,gauss);
		this->GetLprimeSSAFS(&LprimeSSAFS[0][0], element,xyz_list, gauss);
		this->GetLFSSSA(&LFSSSA[0][0],element, gauss);
		this->GetLprimeFSSSA(&LprimeFSSSA[0][0], element,xyz_list, gauss);

		element->ViscosityFS(&viscosity,dim,xyz_list,gauss,vx_input,vy_input,vz_input);

		element->NormalBase(&bed_normal[0],xyz_list_tria);
		friction->GetAlpha2(&alpha2_gauss, gauss,vx_input,vy_input,vz_input);

		DLSSAFS[0][0]=alpha2_gauss*gauss->weight*Jdet2d;
		DLSSAFS[1][1]=alpha2_gauss*gauss->weight*Jdet2d;
		DLSSAFS[2][2]=-alpha2_gauss*gauss->weight*Jdet2d*bed_normal[0]*bed_normal[2];
		DLSSAFS[3][3]=-alpha2_gauss*gauss->weight*Jdet2d*bed_normal[1]*bed_normal[2];
		DLSSAFS[4][4]=-2*viscosity*gauss->weight*Jdet2d*bed_normal[0];
		DLSSAFS[5][5]=-2*viscosity*gauss->weight*Jdet2d*bed_normal[1];
		DLSSAFS[6][6]=FSreconditioning*gauss->weight*Jdet2d*bed_normal[0];
		DLSSAFS[7][7]=FSreconditioning*gauss->weight*Jdet2d*bed_normal[1];

		DLFSSSA[0][0]=alpha2_gauss*gauss->weight*Jdet2d;
		DLFSSSA[1][1]=alpha2_gauss*gauss->weight*Jdet2d;
		DLFSSSA[2][2]=-alpha2_gauss*gauss->weight*Jdet2d*bed_normal[0]*bed_normal[2];
		DLFSSSA[3][3]=-alpha2_gauss*gauss->weight*Jdet2d*bed_normal[1]*bed_normal[2];

		TripleMultiply( &LSSAFS[0][0],8,numdof2dm,1,
					&DLSSAFS[0][0],8,8,0,
					&LprimeSSAFS[0][0],8,numdofs,0,
					&Ke_drag[0][0],1);

		TripleMultiply( &LFSSSA[0][0],4,numdof2d,1,
					&DLFSSSA[0][0],4,4,0,
					&LprimeFSSSA[0][0],4,numdof2dm,0,
					&Ke_drag2[0][0],1);
	}

	for(i=0;i<numdof2dm;i++) for(j=0;j<numdofs;j++) Ke->values[i*numdoftot+j+numdofm]+=Ke_drag[i][j];
	for(i=0;i<numdof2d;i++) for(j=0;j<numdof2dm;j++) Ke->values[(i+numdofm)*numdoftot+j]+=Ke_drag2[i][j];
		
	/*Transform Coordinate System*/
	element->TransformStiffnessMatrixCoord(Ke,node_list,numnodes,cs_list);

	/*Clean up and return*/
	xDelete<int>(cs_list);
	xDelete<Node*>(node_list);
	xDelete<IssmDouble>(xyz_list);
	xDelete<IssmDouble>(xyz_list_tria);
	delete gauss;
	delete friction;
	return Ke;
}/*}}}*/
ElementMatrix* StressbalanceAnalysis::CreateKMatrixCouplingSSAFSViscous(Element* element){/*{{{*/

	/*Constants*/
	const int numdofm     = 2 *3;
	const int numdofs     = 4 *6+ 3;
	const int numdoftotal = 2 *numdofm+numdofs;

	/*Intermediaries */
	int        i,j;
	int        dim=3;
	IssmDouble Jdet,viscosity,FSreconditioning,D_scalar;
	IssmDouble B[4][numdofs];
	IssmDouble Bprime[4][numdofm];
	IssmDouble B2[3][numdofm];
	IssmDouble Bprime2[3][numdofs];
	IssmDouble D[4][4]={0.0};            // material matrix, simple scalar matrix.
	IssmDouble D2[3][3]={0.0};            // material matrix, simple scalar matrix.
	IssmDouble Ke_gg[numdofs][numdofm]={0.0}; //local element stiffness matrix 
	IssmDouble Ke_gg2[numdofm][numdofs]={0.0}; //local element stiffness matrix 
	IssmDouble *xyz_list    = NULL;

	/*Find penta on bed as FS must be coupled to the dofs on the bed: */
	Element* pentabase=element->GetBasalElement();
	Element* basaltria=pentabase->SpawnBasalElement();

	int vnumnodes = element->NumberofNodesVelocity();
	int pnumnodes = element->NumberofNodesPressure();
	int numnodes  = 2*vnumnodes-1+pnumnodes;

	/*Prepare node list*/
	int* cs_list     = xNew<int>(2*vnumnodes-1+pnumnodes);
	Node **node_list = xNew<Node*>(2*vnumnodes-1+pnumnodes);
	for(i=0;i<vnumnodes-1;i++){
		node_list[i] = pentabase->GetNode(i);
		cs_list[i]   = XYEnum;
	}
	for(i=0;i<vnumnodes;i++){
		node_list[i+vnumnodes-1] = element->GetNode(i);
		cs_list[i+vnumnodes-1]   = XYZEnum;
	}
	for(i=0;i<pnumnodes;i++){
		node_list[2*vnumnodes-1+i] = element->GetNode(vnumnodes+i);
		cs_list[2*vnumnodes-1+i]   = PressureEnum;
	}

	/*Initialize Element matrix and return if necessary*/
	ElementMatrix* Ke1=pentabase->NewElementMatrixCoupling(6,SSAApproximationEnum);
	ElementMatrix* Ke2=element->NewElementMatrix(FSvelocityEnum);
	ElementMatrix* Ke =new ElementMatrix(Ke1,Ke2);
	delete Ke1; delete Ke2;

	/* Get node coordinates and dof list: */
	element->GetVerticesCoordinates(&xyz_list);
	element->FindParam(&FSreconditioning,StressbalanceFSreconditioningEnum);
	Input* vx_input=element->GetInput(VxEnum); _assert_(vx_input);
	Input* vy_input=element->GetInput(VyEnum); _assert_(vy_input);
	Input* vz_input=element->GetInput(VzEnum); _assert_(vz_input);

	/* Start  looping on the number of gaussian points: */
	Gauss* gauss=element->NewGauss(5);
	Gauss* gauss_tria=new GaussTria();
	for(int ig=gauss->begin();ig<gauss->end();ig++){

		gauss->GaussPoint(ig);
		gauss->SynchronizeGaussBase(gauss_tria);

		element->JacobianDeterminant(&Jdet, xyz_list,gauss);
		this->GetBSSAFS(&B[0][0],element,xyz_list, gauss);
		this->GetBprimeSSAFSTria(&Bprime[0][0], basaltria,xyz_list, gauss_tria);
		this->GetBSSAFSTria(&B2[0][0], basaltria,xyz_list, gauss_tria);
		this->GetBprimeSSAFS(&Bprime2[0][0], element,xyz_list, gauss);

		element->ViscosityFS(&viscosity,dim,xyz_list,gauss,vx_input,vy_input,vz_input);

		D_scalar=2*viscosity*gauss->weight*Jdet;
		for (i=0;i<3;i++) D[i][i]=D_scalar;
		D[3][3]=-gauss->weight*Jdet*FSreconditioning;
		for (i=0;i<3;i++) D2[i][i]=D_scalar;

		TripleMultiply( &B[0][0],4,numdofs,1,
					&D[0][0],4,4,0,
					&Bprime[0][0],4,numdofm,0,
					&Ke_gg[0][0],1);

		TripleMultiply( &B2[0][0],3,numdofm,1,
					&D2[0][0],3,3,0,
					&Bprime2[0][0],3,numdofs,0,
					&Ke_gg2[0][0],1);

	} 
	for(i=0;i<numdofs;i++) for(j=0;j<numdofm;j++) Ke->values[(i+2*numdofm)*numdoftotal+j]+=Ke_gg[i][j];
	for(i=0;i<numdofm;i++) for(j=0;j<numdofs;j++) Ke->values[i*numdoftotal+(j+2*numdofm)]+=Ke_gg2[i][j];

	/*Transform Coordinate System*/
	element->TransformStiffnessMatrixCoord(Ke,node_list,numnodes,cs_list);

	/*Clean-up and return*/
	xDelete<int>(cs_list);
	xDelete<Node*>(node_list);
	xDelete<IssmDouble>(xyz_list);
	delete basaltria->material; delete basaltria;
	delete gauss;
	delete gauss_tria;
	return Ke;

}/*}}}*/
ElementVector* StressbalanceAnalysis::CreatePVectorSSAHO(Element* element){/*{{{*/

	/*compute all load vectors for this element*/
	ElementVector* pe1=CreatePVectorSSA(element);
	ElementVector* pe2=CreatePVectorHO(element);
	ElementVector* pe =new ElementVector(pe1,pe2);

	/*clean-up and return*/
	delete pe1;
	delete pe2;
	return pe;
}/*}}}*/
ElementVector* StressbalanceAnalysis::CreatePVectorSSAFS(Element* element){/*{{{*/

	/*compute all load vectors for this element*/
	int init = element->FiniteElement();
	element->SetTemporaryElementType(P1Enum); // P1 needed for HO
	ElementVector* pe1=CreatePVectorSSA(element);
	element->SetTemporaryElementType(init); // P1 needed for HO
	ElementVector* pe2=CreatePVectorFS(element);
	int indices[3]={18,19,20};
	element->SetTemporaryElementType(MINIcondensedEnum); // P1 needed for HO
	ElementMatrix* Ke = CreateKMatrixFS(element);
	element->SetTemporaryElementType(init); // P1 needed for HO
	pe2->StaticCondensation(Ke,3,&indices[0]);
	delete Ke;
	ElementVector* pe3=CreatePVectorCouplingSSAFS(element);
	ElementVector* pe =new ElementVector(pe1,pe2,pe3);

	/*clean-up and return*/
	delete pe1;
	delete pe2;
	delete pe3;
	return pe;
}/*}}}*/
ElementVector* StressbalanceAnalysis::CreatePVectorHOFS(Element* element){/*{{{*/

	/*compute all load vectors for this element*/
	int init = element->FiniteElement();
	element->SetTemporaryElementType(P1Enum);
	ElementVector* pe1=CreatePVectorHO(element);
	element->SetTemporaryElementType(init);
	ElementVector* pe2=CreatePVectorFS(element);
	int indices[3]={18,19,20};
	element->SetTemporaryElementType(MINIcondensedEnum);
	ElementMatrix* Ke = CreateKMatrixFS(element);
	element->SetTemporaryElementType(init);
	pe2->StaticCondensation(Ke,3,&indices[0]);
	delete Ke;
	ElementVector* pe3=CreatePVectorCouplingHOFS(element);
	ElementVector* pe =new ElementVector(pe1,pe2,pe3);

	/*clean-up and return*/
	delete pe1;
	delete pe2;
	delete pe3;
	return pe;
}/*}}}*/
ElementVector* StressbalanceAnalysis::CreatePVectorCouplingHOFS(Element* element){/*{{{*/

	/*compute all load vectors for this element*/
	ElementVector* pe1=CreatePVectorCouplingHOFSViscous(element);
	ElementVector* pe2=CreatePVectorCouplingHOFSFriction(element);
	ElementVector* pe =new ElementVector(pe1,pe2);

	/*clean-up and return*/
	delete pe1;
	delete pe2;
	return pe;
}/*}}}*/
ElementVector* StressbalanceAnalysis::CreatePVectorCouplingHOFSFriction(Element* element){/*{{{*/

	/*Intermediaries*/
	int         i,j,approximation;
	int         dim=3;
	IssmDouble  Jdet,Jdet2d,FSreconditioning;
	IssmDouble	bed_normal[3];
	IssmDouble  viscosity, w, alpha2_gauss;
	IssmDouble  dw[3];
	IssmDouble	*xyz_list_tria = NULL;
	IssmDouble  *xyz_list      = NULL;
	IssmDouble  basis[6]; //for the six nodes of the penta

	/*Initialize Element vector and return if necessary*/
	if(!element->IsOnBed() || element->IsFloating()) return NULL;
	element->GetInputValue(&approximation,ApproximationEnum);
	if(approximation!=HOFSApproximationEnum) return NULL;

	int vnumnodes = element->NumberofNodesVelocity();
	int pnumnodes = element->NumberofNodesPressure();
	int numnodes  = vnumnodes+pnumnodes;

	/*Prepare coordinate system list*/
	int*   cs_list   = xNew<int>(vnumnodes+pnumnodes);
	Node **node_list = xNew<Node*>(vnumnodes+pnumnodes);
	for(i=0;i<vnumnodes;i++){
		cs_list[i]           = XYZEnum;
		node_list[i]           = element->GetNode(i);
	}
	for(i=0;i<pnumnodes;i++){
		cs_list[vnumnodes+i] = PressureEnum;
		node_list[vnumnodes+i] = element->GetNode(vnumnodes+i);
	}

	ElementVector* pe=element->NewElementVector(FSvelocityEnum);

	/*Retrieve all inputs and parameters*/
	element->GetVerticesCoordinates(&xyz_list);
	element->GetVerticesCoordinatesBase(&xyz_list_tria);
	element->FindParam(&FSreconditioning,StressbalanceFSreconditioningEnum);
	Input* vx_input=  element->GetInput(VxEnum);   _assert_(vx_input);
	Input* vy_input=  element->GetInput(VyEnum);   _assert_(vy_input);
	Input* vz_input=  element->GetInput(VzEnum);   _assert_(vz_input);
	Input* vzHO_input=element->GetInput(VzHOEnum); _assert_(vzHO_input);

	/*build friction object, used later on: */
	Friction* friction=new Friction(element,3);

	/* Start looping on the number of gauss 2d (nodes on the bedrock) */
	Gauss* gauss=element->NewGaussBase(2);
	for(int ig=gauss->begin();ig<gauss->end();ig++){

		gauss->GaussPoint(ig);

		element->JacobianDeterminantBase(&Jdet2d,xyz_list_tria,gauss);
		element->NodalFunctionsP1(basis, gauss);

		vzHO_input->GetInputValue(&w, gauss);
		vzHO_input->GetInputDerivativeValue(&dw[0],xyz_list,gauss);

		element->NormalBase(&bed_normal[0],xyz_list_tria);
		element->ViscosityFS(&viscosity,dim,xyz_list,gauss,vx_input,vy_input,vz_input);
		friction->GetAlpha2(&alpha2_gauss, gauss,vx_input,vy_input,vz_input);

		for(i=0;i<3;i++){
			pe->values[i*3+0]+=Jdet2d*gauss->weight*(alpha2_gauss*w*bed_normal[0]*bed_normal[2]+2*viscosity*dw[2]*bed_normal[0])*basis[i];
			pe->values[i*3+1]+=Jdet2d*gauss->weight*(alpha2_gauss*w*bed_normal[1]*bed_normal[2]+2*viscosity*dw[2]*bed_normal[1])*basis[i];
			pe->values[i*3+2]+=Jdet2d*gauss->weight*2*viscosity*(dw[0]*bed_normal[0]+dw[1]*bed_normal[1]+dw[2]*bed_normal[2])*basis[i];
		}
	}

	/*Transform coordinate system*/
	element->TransformLoadVectorCoord(pe,node_list,vnumnodes+pnumnodes,cs_list);

	/*Clean up and return*/
	xDelete<int>(cs_list);
	xDelete<Node*>(node_list);
	xDelete<IssmDouble>(xyz_list);
	xDelete<IssmDouble>(xyz_list_tria);
	delete gauss;
	delete friction;
	return pe;
}/*}}}*/
ElementVector* StressbalanceAnalysis::CreatePVectorCouplingHOFSViscous(Element* element){/*{{{*/

	/*Intermediaries */
	int         i,approximation;
	int         dim=3;
	IssmDouble  viscosity,Jdet,FSreconditioning;
	IssmDouble  dw[3];
	IssmDouble  *xyz_list = NULL;
	IssmDouble  basis[6]; //for the six nodes of the penta
	IssmDouble  dbasis[3][6]; //for the six nodes of the penta

	/*Initialize Element vector and return if necessary*/
	element->GetInputValue(&approximation,ApproximationEnum);
	if(approximation!=HOFSApproximationEnum) return NULL;
	int   vnumnodes = element->NumberofNodesVelocity();
	int   pnumnodes = element->NumberofNodesPressure();

	/*Prepare coordinate system list*/
	int*   cs_list   = xNew<int>(vnumnodes+pnumnodes);
	Node **node_list = xNew<Node*>(vnumnodes+pnumnodes);
	for(i=0;i<vnumnodes;i++){
		cs_list[i]             = XYZEnum;
		node_list[i]           = element->GetNode(i);
	}
	for(i=0;i<pnumnodes;i++){
		cs_list[vnumnodes+i]   = PressureEnum;
		node_list[vnumnodes+i] = element->GetNode(vnumnodes+i);
	}
	ElementVector* pe = element->NewElementVector(FSvelocityEnum);

	/*Retrieve all inputs and parameters*/
	element->GetVerticesCoordinates(&xyz_list);
	element->FindParam(&FSreconditioning,StressbalanceFSreconditioningEnum);
	Input* vx_input   =element->GetInput(VxEnum);   _assert_(vx_input);
	Input* vy_input   =element->GetInput(VyEnum);   _assert_(vy_input);
	Input* vz_input   =element->GetInput(VzEnum);   _assert_(vz_input);
	Input* vzHO_input=element->GetInput(VzHOEnum);  _assert_(vzHO_input);

	/* Start  looping on the number of gaussian points: */
	Gauss* gauss=element->NewGauss(5);
	for(int ig=gauss->begin();ig<gauss->end();ig++){

		gauss->GaussPoint(ig);

		element->JacobianDeterminant(&Jdet, xyz_list,gauss);
		element->NodalFunctionsP1(&basis[0],gauss);
		element->NodalFunctionsP1Derivatives(&dbasis[0][0],xyz_list,gauss);
		
		element->ViscosityFS(&viscosity,dim,xyz_list,gauss,vx_input,vy_input,vz_input);
		vzHO_input->GetInputDerivativeValue(&dw[0],xyz_list,gauss);

		for(i=0;i<6;i++){
			pe->values[i*3+0]+=-Jdet*gauss->weight*viscosity*dw[0]*dbasis[2][i];
			pe->values[i*3+1]+=-Jdet*gauss->weight*viscosity*dw[1]*dbasis[2][i];
			pe->values[i*3+2]+=-Jdet*gauss->weight*viscosity*(dw[0]*dbasis[0][i]+dw[1]*dbasis[1][i]+2*dw[2]*dbasis[2][i]);
			pe->values[3*vnumnodes+i]+=Jdet*gauss->weight*FSreconditioning*dw[2]*basis[i];
		}
	}

	/*Transform coordinate system*/
	element->TransformLoadVectorCoord(pe,node_list,vnumnodes+pnumnodes,cs_list);

	/*Clean up and return*/
	xDelete<int>(cs_list);
	xDelete<Node*>(node_list);
	xDelete<IssmDouble>(xyz_list);
	delete gauss;
	return pe;
}/*}}}*/
ElementVector* StressbalanceAnalysis::CreatePVectorCouplingSSAFS(Element* element){/*{{{*/

	/*compute all load vectors for this element*/
	ElementVector* pe1=CreatePVectorCouplingSSAFSViscous(element);
	ElementVector* pe2=CreatePVectorCouplingSSAFSFriction(element);
	ElementVector* pe =new ElementVector(pe1,pe2);

	/*clean-up and return*/
	delete pe1;
	delete pe2;
	return pe;
}/*}}}*/
ElementVector* StressbalanceAnalysis::CreatePVectorCouplingSSAFSFriction(Element* element){/*{{{*/

	/*Intermediaries*/
	int         i,j,approximation;
	int         dim=3;
	IssmDouble  Jdet,Jdet2d,FSreconditioning;
	IssmDouble	bed_normal[3];
	IssmDouble  viscosity, w, alpha2_gauss;
	IssmDouble  dw[3];
	IssmDouble  basis[6]; //for the six nodes of the penta
	IssmDouble	*xyz_list_tria = NULL;
	IssmDouble  *xyz_list      = NULL;

	/*Initialize Element vector and return if necessary*/
	if(!element->IsOnBed() || element->IsFloating()) return NULL;
	element->GetInputValue(&approximation,ApproximationEnum);
	if(approximation!=SSAFSApproximationEnum) return NULL;
	int vnumnodes = element->NumberofNodesVelocity();
	int pnumnodes = element->NumberofNodesPressure();

	/*Prepare coordinate system list*/
	int* cs_list     = xNew<int>(vnumnodes+pnumnodes);
	Node **node_list = xNew<Node*>(vnumnodes+pnumnodes);
	for(i=0;i<vnumnodes;i++){
		cs_list[i]             = XYZEnum;
		node_list[i]           = element->GetNode(i);
	}
	for(i=0;i<pnumnodes;i++){
		cs_list[vnumnodes+i]   = PressureEnum;
		node_list[vnumnodes+i] = element->GetNode(vnumnodes+i);
	}
	ElementVector* pe=element->NewElementVector(FSvelocityEnum);

	/*Retrieve all inputs and parameters*/
	element->GetVerticesCoordinates(&xyz_list);
	element->GetVerticesCoordinatesBase(&xyz_list_tria);
	element->FindParam(&FSreconditioning,StressbalanceFSreconditioningEnum);
	Input* vx_input=   element->GetInput(VxEnum);    _assert_(vx_input);
	Input* vy_input=   element->GetInput(VyEnum);    _assert_(vy_input);
	Input* vz_input=   element->GetInput(VzEnum);    _assert_(vz_input);
	Input* vzSSA_input=element->GetInput(VzSSAEnum); _assert_(vzSSA_input);

	/*build friction object, used later on: */
	Friction* friction=new Friction(element,3);

	/* Start looping on the number of gauss 2d (nodes on the bedrock) */
	Gauss* gauss=element->NewGaussBase(2);
	for(int ig=gauss->begin();ig<gauss->end();ig++){

		gauss->GaussPoint(ig);

		element->JacobianDeterminantBase(&Jdet2d,xyz_list_tria,gauss);
		element->NodalFunctionsP1(basis, gauss);

		vzSSA_input->GetInputValue(&w, gauss);
		vzSSA_input->GetInputDerivativeValue(&dw[0],xyz_list,gauss);

		element->NormalBase(&bed_normal[0],xyz_list_tria);
		element->ViscosityFS(&viscosity,dim,xyz_list,gauss,vx_input,vy_input,vz_input);
		friction->GetAlpha2(&alpha2_gauss, gauss,vx_input,vy_input,vz_input);

		for(i=0;i<3;i++){
			pe->values[i*3+0]+=Jdet2d*gauss->weight*(alpha2_gauss*w*bed_normal[0]*bed_normal[2]+2*viscosity*dw[2]*bed_normal[0])*basis[i];
			pe->values[i*3+1]+=Jdet2d*gauss->weight*(alpha2_gauss*w*bed_normal[1]*bed_normal[2]+2*viscosity*dw[2]*bed_normal[1])*basis[i];
			pe->values[i*3+2]+=Jdet2d*gauss->weight*2*viscosity*(dw[0]*bed_normal[0]+dw[1]*bed_normal[1]+dw[2]*bed_normal[2])*basis[i];
		}
	}

	/*Transform coordinate system*/
	element->TransformLoadVectorCoord(pe,node_list,vnumnodes+pnumnodes,cs_list);

	/*Clean up and return*/
	xDelete<int>(cs_list);
	xDelete<IssmDouble>(xyz_list);
	xDelete<IssmDouble>(xyz_list_tria);
	xDelete<Node*>(node_list);
	delete gauss;
	delete friction;
	return pe;
}/*}}}*/
ElementVector* StressbalanceAnalysis::CreatePVectorCouplingSSAFSViscous(Element* element){/*{{{*/

	/*Intermediaries */
	int         i,approximation;
	IssmDouble  viscosity,Jdet,FSreconditioning;
	IssmDouble  dw[3];
	IssmDouble  *xyz_list = NULL;
	IssmDouble  basis[6]; //for the six nodes of the penta
	IssmDouble  dbasis[3][6]; //for the six nodes of the penta

	/*Initialize Element vector and return if necessary*/
	element->GetInputValue(&approximation,ApproximationEnum);
	if(approximation!=SSAFSApproximationEnum) return NULL;
	int vnumnodes = element->NumberofNodesVelocity();
	int pnumnodes = element->NumberofNodesPressure();

	/*Prepare coordinate system list*/
	int* cs_list = xNew<int>(vnumnodes+pnumnodes);
	Node **node_list = xNew<Node*>(vnumnodes+pnumnodes);
	for(i=0;i<vnumnodes;i++){
		cs_list[i]             = XYZEnum;
		node_list[i]           = element->GetNode(i);
	}
	for(i=0;i<pnumnodes;i++){
		cs_list[vnumnodes+i]   = PressureEnum;
		node_list[vnumnodes+i] = element->GetNode(vnumnodes+i);
	}
	ElementVector* pe=element->NewElementVector(FSvelocityEnum);

	/*Retrieve all inputs and parameters*/
	element->GetVerticesCoordinates(&xyz_list);
	element->FindParam(&FSreconditioning,StressbalanceFSreconditioningEnum);
	Input* vx_input   =element->GetInput(VxEnum);      _assert_(vx_input);
	Input* vy_input   =element->GetInput(VyEnum);      _assert_(vy_input);
	Input* vz_input   =element->GetInput(VzEnum);      _assert_(vz_input);
	Input* vzSSA_input=element->GetInput(VzSSAEnum);   _assert_(vzSSA_input);

	/* Start  looping on the number of gaussian points: */
	Gauss* gauss=element->NewGauss(5);
	for(int ig=gauss->begin();ig<gauss->end();ig++){

		gauss->GaussPoint(ig);
		element->JacobianDeterminant(&Jdet,xyz_list,gauss);
		element->NodalFunctionsP1(&basis[0], gauss);
		element->NodalFunctionsP1Derivatives(&dbasis[0][0],xyz_list, gauss);

		vzSSA_input->GetInputDerivativeValue(&dw[0],xyz_list,gauss);
		element->ViscosityFS(&viscosity,3,xyz_list,gauss,vx_input,vy_input,vz_input);

		for(i=0;i<6;i++){
			pe->values[i*3+0]+=-Jdet*gauss->weight*viscosity*dw[0]*dbasis[2][i];
			pe->values[i*3+1]+=-Jdet*gauss->weight*viscosity*dw[1]*dbasis[2][i];
			pe->values[i*3+2]+=-Jdet*gauss->weight*viscosity*(dw[0]*dbasis[0][i]+dw[1]*dbasis[1][i]+2*dw[2]*dbasis[2][i]);
			pe->values[3*vnumnodes+i]+=Jdet*gauss->weight*FSreconditioning*dw[2]*basis[i];
		}
	}

	/*Transform coordinate system*/
	element->TransformLoadVectorCoord(pe,node_list,vnumnodes+pnumnodes,cs_list);

	/*Clean up and return*/
	xDelete<int>(cs_list);
	xDelete<Node*>(node_list);
	xDelete<IssmDouble>(xyz_list);
	delete gauss;
	return pe;
}/*}}}*/
void StressbalanceAnalysis::GetBSSAHO(IssmDouble* B,Element* element,IssmDouble* xyz_list,Gauss* gauss){/*{{{*/
	/*Compute B  matrix. B=[B1 B2 B3 B4 B5 B6] where Bi is of size 3*NDOF2. 
	 * For node i, Bi can be expressed in the actual coordinate system
	 * by: 
	 *       Bi=[ dh/dx          0      ]
	 *          [   0           dh/dy   ]
	 *          [ 1/2*dh/dy  1/2*dh/dx  ]
	 * where h is the interpolation function for node i.
	 *
	 * We assume B has been allocated already, of size: 5x(NDOF2*NUMNODESP1)
	 */

	int numnodes = element->GetNumberOfNodes();
	IssmDouble* dbasis=xNew<IssmDouble>(3*numnodes);

	/*Get dbasis in actual coordinate system: */
	element->NodalFunctionsDerivatives(dbasis,xyz_list,gauss);

	/*Build B: */
	for(int i=0;i<numnodes;i++){
		B[2*numnodes*0+2*i+0] = dbasis[0*numnodes+i];
		B[2*numnodes*0+2*i+1] = 0.;
		B[2*numnodes*1+2*i+0] = 0.;
		B[2*numnodes*1+2*i+1] = dbasis[1*numnodes+i];
		B[2*numnodes*2+2*i+0] = .5*dbasis[1*numnodes+i];
		B[2*numnodes*2+2*i+1] = .5*dbasis[0*numnodes+i];
	}

	/*Clean-up*/
	xDelete<IssmDouble>(dbasis);
}/*}}}*/
void StressbalanceAnalysis::GetBSSAFS(IssmDouble* B,Element* element,IssmDouble* xyz_list,Gauss* gauss){/*{{{*/
	/*Compute B  matrix. B=[B1 B2 B3 B4 B5 B6] where Bi is of size 5*NDOF2. 
	 * For node i, Bi can be expressed in the actual coordinate system
	 * by: 
	 *       Bi=[ dh/dx          0       0   0 ]
	 *          [   0           dh/dy    0   0 ]
	 *          [ 1/2*dh/dy  1/2*dh/dx   0   0 ]
	 *          [   0            0       0   h ]
	 * where h is the interpolation function for node i.
	 *
	 * We assume B has been allocated already, of size: 5x(NDOF2*NUMNODESP1)
	 */

	int i;
	IssmDouble dbasismini[3][7];
	IssmDouble basis[6];

	/*Get dbasis in actual coordinate system: */
	element->NodalFunctionsMINIDerivatives(&dbasismini[0][0],xyz_list, gauss);
	element->NodalFunctionsP1(basis,gauss);

	/*Build B: */
	for(i=0;i<6;i++){
		B[(3*7+6)*0+3*i+0] = dbasismini[0][i];
		B[(3*7+6)*0+3*i+1] = 0.;
		B[(3*7+6)*0+3*i+2] = 0.;
		B[(3*7+6)*1+3*i+0] = 0.;
		B[(3*7+6)*1+3*i+1] = dbasismini[1][i];
		B[(3*7+6)*1+3*i+2] = 0.;
		B[(3*7+6)*2+3*i+0] = 0.5*dbasismini[1][i];
		B[(3*7+6)*2+3*i+1] = 0.5*dbasismini[0][i];
		B[(3*7+6)*2+3*i+2] = 0.;
		B[(3*7+6)*3+3*i+0] = 0.;
		B[(3*7+6)*3+3*i+1] = 0.;
		B[(3*7+6)*3+3*i+2] = 0.;
	}
	for(i=0;i<1;i++){
		B[(3*7+6)*0+3*(6+i)+0] = 0.;
		B[(3*7+6)*0+3*(6+i)+1] = 0.;
		B[(3*7+6)*0+3*(6+i)+2] = 0.;
		B[(3*7+6)*1+3*(6+i)+0] = 0.;
		B[(3*7+6)*1+3*(6+i)+1] = 0.;
		B[(3*7+6)*1+3*(6+i)+2] = 0.;
		B[(3*7+6)*2+3*(6+i)+0] = 0.;
		B[(3*7+6)*2+3*(6+i)+1] = 0.;
		B[(3*7+6)*2+3*(6+i)+2] = 0.;
		B[(3*7+6)*3+3*(6+i)+0] = 0.;
		B[(3*7+6)*3+3*(6+i)+1] = 0.;
		B[(3*7+6)*3+3*(6+i)+2] = 0.;
	}

	for(i=0;i<6;i++){ //last column not for the bubble function
		B[(3*7+6)*0+7*3+i] = 0;
		B[(3*7+6)*1+7*3+i] = 0;
		B[(3*7+6)*2+7*3+i] = 0;
		B[(3*7+6)*3+7*3+i] = basis[i];
	}
}/*}}}*/
void StressbalanceAnalysis::GetBprimeSSAFS(IssmDouble* Bprime,Element* element,IssmDouble* xyz_list,Gauss* gauss){/*{{{*/
	/*Compute Bprime  matrix. Bprime=[Bprime1 Bprime2 Bprime3 Bprime4 Bprime5 Bprime6] where Bprimei is of size 5*NDOF2. 
	 * For node i, Bprimei can be expressed in the actual coordinate system
	 * by: 
	 *       Bprimei=[ 2*dh/dx    dh/dy   0   0 ]
	 *               [  dh/dx    2*dh/dy  0   0 ]
	 *               [  dh/dy     dh/dx   0   0 ]
	 * where h is the interpolation function for node i.
	 *
	 * We assume Bprime has been allocated already, of size: 5x(NDOF2*NUMNODESP1)
	 */

	int    i;
	IssmDouble dbasismini[3][7];

	/*Get dbasis in actual coordinate system: */
	element->NodalFunctionsMINIDerivatives(&dbasismini[0][0],xyz_list, gauss);

	/*Build Bprime: */
	for(i=0;i<6;i++){
		Bprime[(3*7+6)*0+3*i+0] = 2.*dbasismini[0][i];
		Bprime[(3*7+6)*0+3*i+1] = dbasismini[1][i];
		Bprime[(3*7+6)*0+3*i+2] = 0.;
		Bprime[(3*7+6)*1+3*i+0] = dbasismini[0][i];
		Bprime[(3*7+6)*1+3*i+1] = 2.*dbasismini[1][i];
		Bprime[(3*7+6)*1+3*i+2] = 0.;
		Bprime[(3*7+6)*2+3*i+0] = dbasismini[1][i];
		Bprime[(3*7+6)*2+3*i+1] = dbasismini[0][i];
		Bprime[(3*7+6)*2+3*i+2] = 0.;
	}

	for(i=0;i<1;i++){ //Add zeros for the bubble function
		Bprime[(3*7+6)*0+3*(6+i)+0] = 0.;
		Bprime[(3*7+6)*0+3*(6+i)+1] = 0.;
		Bprime[(3*7+6)*0+3*(6+i)+2] = 0.;
		Bprime[(3*7+6)*1+3*(6+i)+0] = 0.;
		Bprime[(3*7+6)*1+3*(6+i)+1] = 0.;
		Bprime[(3*7+6)*1+3*(6+i)+2] = 0.;
		Bprime[(3*7+6)*2+3*(6+i)+0] = 0.;
		Bprime[(3*7+6)*2+3*(6+i)+1] = 0.;
		Bprime[(3*7+6)*2+3*(6+i)+2] = 0.;
	}

	for(i=0;i<6;i++){ //last column not for the bubble function
		Bprime[(3*7+6)*0+7*3+i] = 0.;
		Bprime[(3*7+6)*1+7*3+i] = 0.;
		Bprime[(3*7+6)*2+7*3+i] = 0.;
	}
}/*}}}*/
void StressbalanceAnalysis::GetBSSAFSTria(IssmDouble* B,Element* element,IssmDouble* xyz_list,Gauss* gauss){/*{{{*/
	/*Compute B  matrix. B=[B1 B2 B3] where Bi is of size 3*NDOF2. 
	 * For node i, Bi can be expressed in the actual coordinate system
	 * by: 
	 *       Bi=[   dN/dx         0     ]
	 *          [       0       dN/dy   ]
	 *          [  1/2*dN/dy  1/2*dN/dx ]
	 * where N is the finiteelement function for node i.
	 *
	 * We assume B has been allocated already, of size: 3x(NDOF2*numnodes)
	 */

	/*Fetch number of nodes for this finite element*/
	int numnodes = element->GetNumberOfNodes();

	/*Get nodal functions derivatives*/
	IssmDouble* dbasis=xNew<IssmDouble>(2*numnodes);
	element->NodalFunctionsDerivatives(dbasis,xyz_list,gauss);

	/*Build B': */
	for(int i=0;i<numnodes;i++){
		B[2*numnodes*0+2*i+0] = dbasis[0*numnodes+i];
		B[2*numnodes*0+2*i+1] = 0.;
		B[2*numnodes*1+2*i+0] = 0.;
		B[2*numnodes*1+2*i+1] = dbasis[1*numnodes+i];
		B[2*numnodes*2+2*i+0] = 0.5*dbasis[1*numnodes+i];
		B[2*numnodes*2+2*i+1] = 0.5*dbasis[0*numnodes+i];
	}

	/*Clean-up*/
	xDelete<IssmDouble>(dbasis);
}/*}}}*/
void StressbalanceAnalysis::GetBprimeSSAFSTria(IssmDouble* Bprime,Element* element,IssmDouble* xyz_list,Gauss* gauss){/*{{{*/
	/*Compute Bprime  matrix. Bprime=[Bprime1 Bprime2 Bprime3] where Bprimei is of size 3*NDOF2. 
	 * For node i, Bprimei can be expressed in the actual coordinate system
	 * by: 
	 *       Bprimei=[  dN/dx    0   ]
	 *               [    0    dN/dy ]
	 *               [  dN/dy  dN/dx ]
	 N               [  dN/dx  dN/dy ]
	 * where N is the finiteelement function for node i.
	 *
	 * We assume Bprime has been allocated already, of size: 3x(NDOF2*numnodes)
	 */

	/*Fetch number of nodes for this finite element*/
	int numnodes = element->GetNumberOfNodes();

	/*Get nodal functions*/
	IssmDouble* dbasis=xNew<IssmDouble>(2*numnodes);
	element->NodalFunctionsDerivatives(dbasis,xyz_list,gauss);

	/*Build Bprime: */
	for(int i=0;i<numnodes;i++){
		Bprime[2*numnodes*0+2*i+0] = dbasis[0*numnodes+i];
		Bprime[2*numnodes*0+2*i+1] = 0.;
		Bprime[2*numnodes*1+2*i+0] = 0.;
		Bprime[2*numnodes*1+2*i+1] = dbasis[1*numnodes+i];
		Bprime[2*numnodes*2+2*i+0] = dbasis[1*numnodes+i];
		Bprime[2*numnodes*2+2*i+1] = dbasis[0*numnodes+i];
		Bprime[2*numnodes*3+2*i+0] = dbasis[0*numnodes+i];
		Bprime[2*numnodes*3+2*i+1] = dbasis[1*numnodes+i];
	}

	/*Clean-up*/
	xDelete<IssmDouble>(dbasis);
}/*}}}*/
void StressbalanceAnalysis::GetLSSAFS(IssmDouble* LFS,Element* element,Gauss* gauss_in){/*{{{*/
	/*
	 * Compute L  matrix. L=[L1 L2 L3] where Li is square and of size numdof. 
	 * For node i, Li can be expressed in the actual coordinate system
	 * by: 
	 *       Li=[ h    0 ]
	 *	 	      [ 0    h ]
	 *	 	      [ h    0 ]
	 *	 	      [ 0    h ]
	 *	 	      [ h    0 ]
	 *	 	      [ 0    h ]
	 *	 	      [ h    0 ]
	 *	 	      [ 0    h ]
	 * where h is the interpolation function for node i.
	 */

	int num_dof=2;
	IssmDouble L1L2l3[3];

	/*Cast gauss to GaussPenta*/
	_assert_(gauss_in->Enum()==GaussPentaEnum);
	GaussPenta* gauss = dynamic_cast<GaussPenta*>(gauss_in);

	/*Get L1L2l3 in actual coordinate system: */
	L1L2l3[0]=gauss->coord1*(1-gauss->coord4)/2.0;
	L1L2l3[1]=gauss->coord2*(1-gauss->coord4)/2.0;
	L1L2l3[2]=gauss->coord3*(1-gauss->coord4)/2.0;

	/*Build LFS: */
	for(int i=0;i<3;i++){
		LFS[num_dof*3*0+num_dof*i+0] = L1L2l3[i];
		LFS[num_dof*3*0+num_dof*i+1] = 0;
		LFS[num_dof*3*1+num_dof*i+0] = 0;
		LFS[num_dof*3*1+num_dof*i+1] = L1L2l3[i];
		LFS[num_dof*3*2+num_dof*i+0] = L1L2l3[i];
		LFS[num_dof*3*2+num_dof*i+1] = 0;
		LFS[num_dof*3*3+num_dof*i+0] = 0;
		LFS[num_dof*3*3+num_dof*i+1] = L1L2l3[i];
		LFS[num_dof*3*4+num_dof*i+0] = L1L2l3[i];
		LFS[num_dof*3*4+num_dof*i+1] = 0;
		LFS[num_dof*3*5+num_dof*i+0] = 0;
		LFS[num_dof*3*5+num_dof*i+1] = L1L2l3[i];
		LFS[num_dof*3*6+num_dof*i+0] = L1L2l3[i];
		LFS[num_dof*3*6+num_dof*i+1] = 0;
		LFS[num_dof*3*7+num_dof*i+0] = 0;
		LFS[num_dof*3*7+num_dof*i+1] = L1L2l3[i];
	}
}/*}}}*/
void StressbalanceAnalysis::GetLprimeSSAFS(IssmDouble* LprimeFS,Element* element,IssmDouble* xyz_list,Gauss* gauss_in){/*{{{*/
	/* Compute Lprime  matrix. Lprime=[Lp1 Lp2 Lp3] where Lpi is square and of size numdof. 
	 * For node i, Lpi can be expressed in the actual coordinate system
	 * by: 
	 *       Lpi=[ h    0    0   0]
	 *		       [ 0    h    0   0]
	 *		       [ 0    0    h   0]
	 *		       [ 0    0    h   0]
	 *		       [ 0    0  dh/dz 0]
	 *		       [ 0    0  dh/dz 0]
	 *           [ 0    0    0   h]
	 *           [ 0    0    0   h]
	 * where h is the interpolation function for node i.
	 */
	int num_dof=3;
	int num_dof_vel=3*7;
	int num_dof_total=3*7+1*6;
	IssmDouble L1L2l3[3];
	IssmDouble dbasis[3][6];

	/*Cast gauss to GaussPenta*/
	_assert_(gauss_in->Enum()==GaussPentaEnum);
	GaussPenta* gauss = dynamic_cast<GaussPenta*>(gauss_in);

	/*Get L1L2l3 in actual coordinate system: */
	L1L2l3[0]=gauss->coord1*(1-gauss->coord4)/2.0;
	L1L2l3[1]=gauss->coord2*(1-gauss->coord4)/2.0;
	L1L2l3[2]=gauss->coord3*(1-gauss->coord4)/2.0;

	element->NodalFunctionsP1Derivatives(&dbasis[0][0],xyz_list,gauss);

	/*Build LprimeFS: */
	for(int i=0;i<3;i++){
		LprimeFS[num_dof_total*0+num_dof*i+0] = L1L2l3[i];
		LprimeFS[num_dof_total*0+num_dof*i+1] = 0.;
		LprimeFS[num_dof_total*0+num_dof*i+2] = 0.;
		LprimeFS[num_dof_total*1+num_dof*i+0] = 0.;
		LprimeFS[num_dof_total*1+num_dof*i+1] = L1L2l3[i];
		LprimeFS[num_dof_total*1+num_dof*i+2] = 0.;
		LprimeFS[num_dof_total*2+num_dof*i+0] = 0.;
		LprimeFS[num_dof_total*2+num_dof*i+1] = 0.;
		LprimeFS[num_dof_total*2+num_dof*i+2] = L1L2l3[i];
		LprimeFS[num_dof_total*3+num_dof*i+0] = 0.;
		LprimeFS[num_dof_total*3+num_dof*i+1] = 0.;
		LprimeFS[num_dof_total*3+num_dof*i+2] = L1L2l3[i];
		LprimeFS[num_dof_total*4+num_dof*i+0] = 0.;
		LprimeFS[num_dof_total*4+num_dof*i+1] = 0.;
		LprimeFS[num_dof_total*4+num_dof*i+2] = dbasis[2][i];
		LprimeFS[num_dof_total*5+num_dof*i+0] = 0.;
		LprimeFS[num_dof_total*5+num_dof*i+1] = 0.;
		LprimeFS[num_dof_total*5+num_dof*i+2] = dbasis[2][i];
		LprimeFS[num_dof_total*6+num_dof*i+0] = 0.;
		LprimeFS[num_dof_total*6+num_dof*i+1] = 0.;
		LprimeFS[num_dof_total*6+num_dof*i+2] = 0.;
		LprimeFS[num_dof_total*7+num_dof*i+0] = 0.;
		LprimeFS[num_dof_total*7+num_dof*i+1] = 0.;
		LprimeFS[num_dof_total*7+num_dof*i+2] = 0.;
	}
	for(int i=3;i<7;i++){
		LprimeFS[num_dof_total*0+num_dof*i+0] = 0.;
		LprimeFS[num_dof_total*0+num_dof*i+1] = 0.;
		LprimeFS[num_dof_total*0+num_dof*i+2] = 0.;
		LprimeFS[num_dof_total*1+num_dof*i+0] = 0.;
		LprimeFS[num_dof_total*1+num_dof*i+1] = 0.;
		LprimeFS[num_dof_total*1+num_dof*i+2] = 0.;
		LprimeFS[num_dof_total*2+num_dof*i+0] = 0.;
		LprimeFS[num_dof_total*2+num_dof*i+1] = 0.;
		LprimeFS[num_dof_total*2+num_dof*i+2] = 0.;
		LprimeFS[num_dof_total*3+num_dof*i+0] = 0.;
		LprimeFS[num_dof_total*3+num_dof*i+1] = 0.;
		LprimeFS[num_dof_total*3+num_dof*i+2] = 0.;
		LprimeFS[num_dof_total*4+num_dof*i+0] = 0.;
		LprimeFS[num_dof_total*4+num_dof*i+1] = 0.;
		LprimeFS[num_dof_total*4+num_dof*i+2] = 0.;
		LprimeFS[num_dof_total*5+num_dof*i+0] = 0.;
		LprimeFS[num_dof_total*5+num_dof*i+1] = 0.;
		LprimeFS[num_dof_total*5+num_dof*i+2] = 0.;
		LprimeFS[num_dof_total*6+num_dof*i+0] = 0.;
		LprimeFS[num_dof_total*6+num_dof*i+1] = 0.;
		LprimeFS[num_dof_total*6+num_dof*i+2] = 0.;
		LprimeFS[num_dof_total*7+num_dof*i+0] = 0.;
		LprimeFS[num_dof_total*7+num_dof*i+1] = 0.;
		LprimeFS[num_dof_total*7+num_dof*i+2] = 0.;
	}
	for(int i=0;i<3;i++){
		LprimeFS[num_dof_total*0+num_dof_vel+i] = 0.;
		LprimeFS[num_dof_total*1+num_dof_vel+i] = 0.;
		LprimeFS[num_dof_total*2+num_dof_vel+i] = 0.;
		LprimeFS[num_dof_total*3+num_dof_vel+i] = 0.;
		LprimeFS[num_dof_total*4+num_dof_vel+i] = 0.;
		LprimeFS[num_dof_total*5+num_dof_vel+i] = 0.;
		LprimeFS[num_dof_total*6+num_dof_vel+i] = L1L2l3[i];
		LprimeFS[num_dof_total*7+num_dof_vel+i] = L1L2l3[i];
	}
	for(int i=3;i<6;i++){
		LprimeFS[num_dof_total*0+num_dof_vel+i] = 0.;
		LprimeFS[num_dof_total*1+num_dof_vel+i] = 0.;
		LprimeFS[num_dof_total*2+num_dof_vel+i] = 0.;
		LprimeFS[num_dof_total*3+num_dof_vel+i] = 0.;
		LprimeFS[num_dof_total*4+num_dof_vel+i] = 0.;
		LprimeFS[num_dof_total*5+num_dof_vel+i] = 0.;
		LprimeFS[num_dof_total*6+num_dof_vel+i] = 0.;
		LprimeFS[num_dof_total*7+num_dof_vel+i] = 0.;
	}
}/*}}}*/
void StressbalanceAnalysis::GetLFSSSA(IssmDouble* LFS,Element* element,Gauss* gauss_in){/*{{{*/
	/* Compute L  matrix. L=[L1 L2 L3] where Li is square and of size numdof. 
	 * For node i, Li can be expressed in the actual coordinate system
	 * by: 
	 *       Li=[ h    0    0 ]
	 *	 	      [ 0    h    0 ]
	 *		      [ 0    0    h ]
	 *		      [ 0    0    h ]
	 * where h is the interpolation function for node i.
	 */

	int num_dof=3;
	IssmDouble L1L2l3[3];

	/*Cast gauss to GaussPenta*/
	_assert_(gauss_in->Enum()==GaussPentaEnum);
	GaussPenta* gauss = dynamic_cast<GaussPenta*>(gauss_in);

	/*Get L1L2l3 in actual coordinate system: */
	L1L2l3[0]=gauss->coord1*(1-gauss->coord4)/2.0;
	L1L2l3[1]=gauss->coord2*(1-gauss->coord4)/2.0;
	L1L2l3[2]=gauss->coord3*(1-gauss->coord4)/2.0;

	/*Build LFS: */
	for(int i=0;i<3;i++){
		LFS[num_dof*3*0+num_dof*i+0] = L1L2l3[i];
		LFS[num_dof*3*0+num_dof*i+1] = 0.;
		LFS[num_dof*3*0+num_dof*i+2] = 0.;
		LFS[num_dof*3*1+num_dof*i+0] = 0.;
		LFS[num_dof*3*1+num_dof*i+1] = L1L2l3[i];
		LFS[num_dof*3*1+num_dof*i+2] = 0.;
		LFS[num_dof*3*2+num_dof*i+0] = 0.;
		LFS[num_dof*3*2+num_dof*i+1] = 0.;
		LFS[num_dof*3*2+num_dof*i+2] = L1L2l3[i];
		LFS[num_dof*3*3+num_dof*i+0] = 0.;
		LFS[num_dof*3*3+num_dof*i+1] = 0.;
		LFS[num_dof*3*3+num_dof*i+2] = L1L2l3[i];
	}
}/*}}}*/
void StressbalanceAnalysis::GetLprimeFSSSA(IssmDouble* LprimeFS,Element* element,IssmDouble* xyz_list,Gauss* gauss_in){/*{{{*/
	/* Compute Lprime  matrix. Lprime=[Lp1 Lp2 Lp3] where Lpi is square and of size numdof. 
	 * For node i, Lpi can be expressed in the actual coordinate system
	 * by: 
	 *       Lpi=[ h    0 ]
	 *		       [ 0    h ]
	 *		       [ h    0 ]
	 *		       [ 0    h ]
	 * where h is the interpolation function for node i.
	 */
	int num_dof=2;
	IssmDouble L1L2l3[3];

	/*Cast gauss to GaussPenta*/
	_assert_(gauss_in->Enum()==GaussPentaEnum);
	GaussPenta* gauss = dynamic_cast<GaussPenta*>(gauss_in);

	/*Get L1L2l3 in actual coordinate system: */
	L1L2l3[0]=gauss->coord1*(1-gauss->coord4)/2.0;
	L1L2l3[1]=gauss->coord2*(1-gauss->coord4)/2.0;
	L1L2l3[2]=gauss->coord3*(1-gauss->coord4)/2.0;

	/*Build LprimeFS: */
	for(int i=0;i<3;i++){
		LprimeFS[num_dof*3*0+num_dof*i+0] = L1L2l3[i];
		LprimeFS[num_dof*3*0+num_dof*i+1] = 0.;
		LprimeFS[num_dof*3*1+num_dof*i+0] = 0.;
		LprimeFS[num_dof*3*1+num_dof*i+1] = L1L2l3[i];
		LprimeFS[num_dof*3*2+num_dof*i+0] = L1L2l3[i];
		LprimeFS[num_dof*3*2+num_dof*i+1] = 0.;
		LprimeFS[num_dof*3*3+num_dof*i+0] = 0.;
		LprimeFS[num_dof*3*3+num_dof*i+1] = L1L2l3[i];
	}
}/*}}}*/
void StressbalanceAnalysis::InputUpdateFromSolutionHOFS(IssmDouble* solution,Element* element){/*{{{*/

	int         i;
	IssmDouble  rho_ice,g,FSreconditioning;
	int*        doflistHO  = NULL;
	int*        doflistFSv = NULL;
	int*        doflistFSp = NULL;

	/*Only works with Penta for now*/
	if(element->ObjectEnum()!=PentaEnum) _error_("Coupling not supported for "<<EnumToStringx(element->ObjectEnum()));

	/*Fetch number of nodes and dof for this finite element*/
	int numnodes  = 6;
	int numdofHO  = 6*2;
	int numdofFSv = 6*3;
	int numdofFSp = 6;

	/*Fetch dof list and allocate solution vectors*/
	element->GetDofList(&doflistFSv,FSvelocityEnum,GsetEnum);
	element->GetDofList(&doflistHO, HOApproximationEnum, GsetEnum);
	element->GetDofListPressure(&doflistFSp,GsetEnum);
	IssmDouble* HOvalues  = xNew<IssmDouble>(numdofHO);
	IssmDouble* FSvalues  = xNew<IssmDouble>(numdofFSv+numdofFSp);
	IssmDouble* vx        = xNew<IssmDouble>(numnodes);
	IssmDouble* vy        = xNew<IssmDouble>(numnodes);
	IssmDouble* vz        = xNew<IssmDouble>(numnodes);
	IssmDouble* vzHO      = xNew<IssmDouble>(numnodes);
	IssmDouble* vzFS      = xNew<IssmDouble>(numnodes);
	IssmDouble* vel       = xNew<IssmDouble>(numnodes);
	IssmDouble* pressure  = xNew<IssmDouble>(numnodes);

	/*Prepare coordinate system list*/
	int* cs_list = xNew<int>(2*numnodes);
	for(i=0;i<numnodes;i++) cs_list[i]          = XYZEnum;
	for(i=0;i<numnodes;i++) cs_list[numnodes+i] = PressureEnum;

	/*Use the dof list to index into the solution vector: */
	element->FindParam(&FSreconditioning,StressbalanceFSreconditioningEnum);
	for(i=0;i<numdofHO ;i++) HOvalues[i]=solution[doflistHO[i]];
	for(i=0;i<numdofFSv;i++) FSvalues[i]=solution[doflistFSv[i]];
	for(i=0;i<numdofFSp;i++) FSvalues[numdofFSv+i]=solution[doflistFSp[i]];

	/*Transform solution in Cartesian Space*/
	element->TransformSolutionCoord(FSvalues,2*numnodes,cs_list);
	element->TransformSolutionCoord(HOvalues,numnodes,XYEnum);

	/*Ok, we have vx and vy in values, fill in vx and vy arrays: */
	for(i=0;i<numnodes;i++){
		vx[i]       = FSvalues[i*3+0]+HOvalues[i*2+0];
		vy[i]       = FSvalues[i*3+1]+HOvalues[i*2+1];
		vzFS[i]     = FSvalues[i*3+2];
		pressure[i] = FSvalues[numnodes*3+i]*FSreconditioning;

		/*Check solution*/
		if(xIsNan<IssmDouble>(vx[i]))       _error_("NaN found in solution vector");
		if(xIsNan<IssmDouble>(vy[i]))       _error_("NaN found in solution vector");
		if(xIsNan<IssmDouble>(vzFS[i]))     _error_("NaN found in solution vector");
		if(xIsNan<IssmDouble>(pressure[i])) _error_("NaN found in solution vector");
	}

	/*Get Vz and compute vel*/
	element->GetInputListOnVertices(vzHO,VzHOEnum);
	for(i=0;i<numnodes;i++){
		vz[i] = vzHO[i]+vzFS[i];
		vel[i]= sqrt(vx[i]*vx[i] + vy[i]*vy[i] + vz[i]*vz[i]);
	}

	/*Now, we have to move the previous Vx and Vy inputs  to old 
	 * status, otherwise, we'll wipe them off: */
	element->InputChangeName(VxEnum,VxPicardEnum);
	element->InputChangeName(VyEnum,VyPicardEnum);
	element->InputChangeName(VzEnum,VzPicardEnum);
	element->InputChangeName(PressureEnum,PressurePicardEnum);

	/*Add vx and vy as inputs to element: */
	element->AddInput(VxEnum,vx,P1Enum);
	element->AddInput(VyEnum,vy,P1Enum);
	element->AddInput(VzEnum,vz,P1Enum);
	element->AddInput(VzFSEnum,vzFS,P1Enum);
	element->AddInput(VelEnum,vel,P1Enum);
	element->AddInput(PressureEnum,pressure,P1Enum);

	/*Free ressources:*/
	xDelete<IssmDouble>(pressure);
	xDelete<IssmDouble>(vel);
	xDelete<IssmDouble>(vz);
	xDelete<IssmDouble>(vzHO);
	xDelete<IssmDouble>(vzFS);
	xDelete<IssmDouble>(vy);
	xDelete<IssmDouble>(vx);
	xDelete<IssmDouble>(FSvalues);
	xDelete<IssmDouble>(HOvalues);
	xDelete<int>(doflistFSp);
	xDelete<int>(doflistFSv);
	xDelete<int>(doflistHO);
	xDelete<int>(cs_list);
}/*}}}*/
void StressbalanceAnalysis::InputUpdateFromSolutionSSAFS(IssmDouble* solution,Element* element){/*{{{*/

	int         i;
	IssmDouble  rho_ice,g,FSreconditioning;
	int*        doflistSSA  = NULL;
	int*        doflistFSv = NULL;
	int*        doflistFSp = NULL;

	/*we have to add results of this element for FS and results from the element
	 * at base for SSA, so we need to have the pointer toward the basal element*/
	Element* basalelement=element->GetBasalElement();
	if(basalelement->ObjectEnum()!=PentaEnum){
		_error_("Coupling not supported for "<<EnumToStringx(basalelement->ObjectEnum()));
	}

	/*Fetch number of nodes and dof for this finite element*/
	int numnodes  = 6;
	int numdof2d  = numnodes;
	int numdofSSA = 6*2;
	int numdofFSv = 6*3;
	int numdofFSp = 6;

	/*Fetch dof list and allocate solution vectors*/
	element->GetDofList(&doflistFSv,FSvelocityEnum,GsetEnum);
	element->GetDofListPressure(&doflistFSp,GsetEnum);
	basalelement->GetDofList(&doflistSSA, SSAApproximationEnum, GsetEnum);
	IssmDouble* SSAvalues  = xNew<IssmDouble>(numdofSSA);
	IssmDouble* FSvalues  = xNew<IssmDouble>(numdofFSv+numdofFSp);
	IssmDouble* vx        = xNew<IssmDouble>(numnodes);
	IssmDouble* vy        = xNew<IssmDouble>(numnodes);
	IssmDouble* vz        = xNew<IssmDouble>(numnodes);
	IssmDouble* vzSSA      = xNew<IssmDouble>(numnodes);
	IssmDouble* vzFS      = xNew<IssmDouble>(numnodes);
	IssmDouble* vel       = xNew<IssmDouble>(numnodes);
	IssmDouble* pressure  = xNew<IssmDouble>(numnodes);

	/*Prepare coordinate system list*/
	int* cs_list = xNew<int>(2*numnodes);
	for(i=0;i<numnodes;i++) cs_list[i]          = XYZEnum;
	for(i=0;i<numnodes;i++) cs_list[numnodes+i] = PressureEnum;

	/*Use the dof list to index into the solution vector: */
	element->FindParam(&FSreconditioning,StressbalanceFSreconditioningEnum);
	for(i=0;i<numdof2d;i++){
		SSAvalues[i]          = solution[doflistSSA[i]];
		SSAvalues[i+numdof2d] = solution[doflistSSA[i]];
	}
	for(i=0;i<numdofFSv;i++) FSvalues[i]=solution[doflistFSv[i]];
	for(i=0;i<numdofFSp;i++) FSvalues[numdofFSv+i]=solution[doflistFSp[i]];

	/*Transform solution in Cartesian Space*/
	element->TransformSolutionCoord(FSvalues,2*numnodes,cs_list);
	element->TransformSolutionCoord(SSAvalues,numnodes,XYEnum);

	/*Ok, we have vx and vy in values, fill in vx and vy arrays: */

	for(i=0;i<numnodes;i++){
		vx[i]       = FSvalues[i*3+0]+SSAvalues[i*2+0];
		vy[i]       = FSvalues[i*3+1]+SSAvalues[i*2+1];
		vzFS[i]     = FSvalues[i*3+2];
		pressure[i] = FSvalues[numnodes*3+i]*FSreconditioning;

		/*Check solution*/
		if(xIsNan<IssmDouble>(vx[i]))       _error_("NaN found in solution vector");
		if(xIsNan<IssmDouble>(vy[i]))       _error_("NaN found in solution vector");
		if(xIsNan<IssmDouble>(vzFS[i]))     _error_("NaN found in solution vector");
		if(xIsNan<IssmDouble>(pressure[i])) _error_("NaN found in solution vector");
	}

	/*Get Vz and compute vel*/
	element->GetInputListOnVertices(vzSSA,VzSSAEnum);
	for(i=0;i<numnodes;i++){
		vz[i] = vzSSA[i]+vzFS[i];
		vel[i]= sqrt(vx[i]*vx[i] + vy[i]*vy[i] + vz[i]*vz[i]);
	}

	/*Now, we have to move the previous Vx and Vy inputs  to old 
	 * status, otherwise, we'll wipe them off: */
	element->InputChangeName(VxEnum,VxPicardEnum);
	element->InputChangeName(VyEnum,VyPicardEnum);
	element->InputChangeName(VzEnum,VzPicardEnum);
	element->InputChangeName(PressureEnum,PressurePicardEnum);

	/*Add vx and vy as inputs to element: */
	element->AddInput(VxEnum,vx,P1Enum);
	element->AddInput(VyEnum,vy,P1Enum);
	element->AddInput(VzEnum,vz,P1Enum);
	element->AddInput(VzFSEnum,vzFS,P1Enum);
	element->AddInput(VelEnum,vel,P1Enum);
	element->AddInput(PressureEnum,pressure,P1Enum);

	/*Free ressources:*/
	xDelete<IssmDouble>(pressure);
	xDelete<IssmDouble>(vel);
	xDelete<IssmDouble>(vz);
	xDelete<IssmDouble>(vzSSA);
	xDelete<IssmDouble>(vzFS);
	xDelete<IssmDouble>(vy);
	xDelete<IssmDouble>(vx);
	xDelete<IssmDouble>(FSvalues);
	xDelete<IssmDouble>(SSAvalues);
	xDelete<int>(doflistFSp);
	xDelete<int>(doflistFSv);
	xDelete<int>(doflistSSA);
	xDelete<int>(cs_list);
}/*}}}*/
void StressbalanceAnalysis::InputUpdateFromSolutionSSAHO(IssmDouble* solution,Element* element){/*{{{*/

	int         i,meshtype;
	IssmDouble  rho_ice,g;
	int*        SSAdoflist = NULL;
	int*        HOdoflist  = NULL;
	IssmDouble* xyz_list   = NULL;

	/*we have to add results of this element for HO and results from the element
	 * at base for SSA, so we need to have the pointer toward the basal element*/
	Element* basalelement=element->GetBasalElement();
	if(basalelement->ObjectEnum()!=PentaEnum){
		_error_("Coupling not supported for "<<EnumToStringx(basalelement->ObjectEnum()));
	}

	/*Fetch number of nodes and dof for this finite element*/
	int numnodes = element->GetNumberOfNodes();
	int numdof   = numnodes*2;
	int numdof2d = numnodes;

	/*Fetch dof list and allocate solution vectors*/
	basalelement->GetDofList(&SSAdoflist,SSAApproximationEnum,GsetEnum);
	element     ->GetDofList(&HOdoflist, HOApproximationEnum, GsetEnum);
	IssmDouble* HOvalues  = xNew<IssmDouble>(numdof);
	IssmDouble* SSAvalues = xNew<IssmDouble>(numdof);
	IssmDouble* vx        = xNew<IssmDouble>(numnodes);
	IssmDouble* vy        = xNew<IssmDouble>(numnodes);
	IssmDouble* vz        = xNew<IssmDouble>(numnodes);
	IssmDouble* vel       = xNew<IssmDouble>(numnodes);
	IssmDouble* pressure  = xNew<IssmDouble>(numnodes);
	IssmDouble* surface   = xNew<IssmDouble>(numnodes);

	/*Use the dof list to index into the solution vector: */
	for(i=0;i<numdof2d;i++){
		HOvalues[i]  = solution[HOdoflist[i]];
		SSAvalues[i] = solution[SSAdoflist[i]];
	}
	for(i=numdof2d;i<numdof;i++){
		HOvalues[i]  = solution[HOdoflist[i]];
		SSAvalues[i] = SSAvalues[i-numdof2d];
	}

	/*Transform solution in Cartesian Space*/
	basalelement->TransformSolutionCoord(SSAvalues,XYEnum);
	element->TransformSolutionCoord(HOvalues,XYEnum);

	/*Ok, we have vx and vy in values, fill in vx and vy arrays: */
	for(i=0;i<numnodes;i++){
		vx[i]=SSAvalues[i*2+0]+HOvalues[i*2+0];
		vy[i]=SSAvalues[i*2+1]+HOvalues[i*2+1];

		/*Check solution*/
		if(xIsNan<IssmDouble>(vx[i])) _error_("NaN found in solution vector");
		if(xIsNan<IssmDouble>(vy[i])) _error_("NaN found in solution vector");
	}

	/*Get Vz and compute vel*/
	element->GetInputListOnNodes(&vz[0],VzEnum,0.);
	for(i=0;i<numnodes;i++) vel[i]=sqrt(vx[i]*vx[i] + vy[i]*vy[i] + vz[i]*vz[i]);

	/*For pressure: we have not computed pressure in this analysis, for this element. We are in 2D, 
	 *so the pressure is just the pressure at the bedrock: */
	rho_ice = element->GetMaterialParameter(MaterialsRhoIceEnum);
	g       = element->GetMaterialParameter(ConstantsGEnum);
	element->GetVerticesCoordinates(&xyz_list);
	element->GetInputListOnNodes(&surface[0],SurfaceEnum);
	for(i=0;i<numnodes;i++) pressure[i]=rho_ice*g*(surface[i]-xyz_list[i*3+2]);

	/*Now, we have to move the previous Vx and Vy inputs  to old 
	 * status, otherwise, we'll wipe them off: */
	element->InputChangeName(VxEnum,VxPicardEnum);
	element->InputChangeName(VyEnum,VyPicardEnum);
	element->InputChangeName(PressureEnum,PressurePicardEnum);

	/*Add vx and vy as inputs to element: */
	element->AddInput(VxEnum,vx,P1Enum);
	element->AddInput(VyEnum,vy,P1Enum);
	element->AddInput(VelEnum,vel,P1Enum);
	element->AddInput(PressureEnum,pressure,P1Enum);

	/*Free ressources:*/
	xDelete<IssmDouble>(surface);
	xDelete<IssmDouble>(pressure);
	xDelete<IssmDouble>(vel);
	xDelete<IssmDouble>(vz);
	xDelete<IssmDouble>(vy);
	xDelete<IssmDouble>(vx);
	xDelete<IssmDouble>(xyz_list);
	xDelete<IssmDouble>(SSAvalues);
	xDelete<IssmDouble>(HOvalues);
	xDelete<int>(SSAdoflist);
	xDelete<int>(HOdoflist);
}/*}}}*/
