#ifdef HAVE_CONFIG_H
#include <config.h>
#else
#error "Cannot compile with HAVE_CONFIG_H symbol! run configure first!"
#endif
#include "./LevelsetAnalysis.h"
#include "../toolkits/toolkits.h"
#include "../classes/classes.h"
#include "../shared/shared.h"
#include "../modules/modules.h"
#include "../solutionsequences/solutionsequences.h"

void LevelsetAnalysis::CreateConstraints(Constraints* constraints,IoModel* iomodel){/*{{{*/
	int finiteelement;
	iomodel->FindConstant(&finiteelement,"md.levelset.fe");
	IoModelToConstraintsx(constraints,iomodel,"md.levelset.spclevelset",LevelsetAnalysisEnum,finiteelement);
}
/*}}}*/
void LevelsetAnalysis::CreateLoads(Loads* loads, IoModel* iomodel){/*{{{*/
	return;
}/*}}}*/
void LevelsetAnalysis::CreateNodes(Nodes* nodes,IoModel* iomodel,bool isamr){/*{{{*/
	int finiteelement;
	iomodel->FindConstant(&finiteelement,"md.levelset.fe");
	if(iomodel->domaintype!=Domain2DhorizontalEnum) iomodel->FetchData(2,"md.mesh.vertexonbase","md.mesh.vertexonsurface");
	::CreateNodes(nodes,iomodel,LevelsetAnalysisEnum,finiteelement);
	iomodel->DeleteData(2,"md.mesh.vertexonbase","md.mesh.vertexonsurface");
}
/*}}}*/
int  LevelsetAnalysis::DofsPerNode(int** doflist,int domaintype,int approximation){/*{{{*/
	return 1;
}
/*}}}*/
void LevelsetAnalysis::UpdateElements(Elements* elements,Inputs* inputs,IoModel* iomodel,int analysis_counter,int analysis_type){/*{{{*/

	/*Finite element type*/
	int finiteelement;
	iomodel->FindConstant(&finiteelement,"md.levelset.fe");

	/*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(inputs,i,iomodel,analysis_counter,analysis_type,finiteelement);
			counter++;
		}
	}

	iomodel->FetchDataToInput(inputs,elements,"md.mask.ice_levelset",MaskIceLevelsetEnum);
	iomodel->FetchDataToInput(inputs,elements,"md.initialization.vx",VxEnum);
	iomodel->FetchDataToInput(inputs,elements,"md.initialization.vy",VyEnum);

	/*Get moving front parameters*/
	int  calvinglaw;
	iomodel->FindConstant(&calvinglaw,"md.calving.law");
	switch(calvinglaw){
		case DefaultCalvingEnum:
			iomodel->FetchDataToInput(inputs,elements,"md.calving.calvingrate",CalvingCalvingrateEnum);
			break;
		case CalvingLevermannEnum:
			iomodel->FetchDataToInput(inputs,elements,"md.calving.coeff",CalvinglevermannCoeffEnum);
			break;
		case CalvingVonmisesEnum:
			iomodel->FetchDataToInput(inputs,elements,"md.calving.stress_threshold_groundedice",CalvingStressThresholdGroundediceEnum);
			iomodel->FetchDataToInput(inputs,elements,"md.calving.stress_threshold_floatingice",CalvingStressThresholdFloatingiceEnum);
			iomodel->FetchDataToInput(inputs,elements,"md.geometry.bed",BedEnum);
			break;
		case CalvingMinthicknessEnum:
			iomodel->FetchDataToInput(inputs,elements,"md.geometry.bed",BedEnum);
			break;
		case CalvingHabEnum:
			iomodel->FetchDataToInput(inputs,elements,"md.calving.flotation_fraction",CalvingHabFractionEnum);
			break;
		case CalvingCrevasseDepthEnum:
			iomodel->FetchDataToInput(inputs,elements,"md.calving.water_height",WaterheightEnum);
			break;
		case CalvingDev2Enum:
			iomodel->FetchDataToInput(inputs,elements,"md.calving.stress_threshold_groundedice",CalvingStressThresholdGroundediceEnum);
			iomodel->FetchDataToInput(inputs,elements,"md.calving.stress_threshold_floatingice",CalvingStressThresholdFloatingiceEnum);
			break;
		default:
			_error_("Calving law "<<EnumToStringx(calvinglaw)<<" not supported yet");
	}

	/*Get frontal melt parameters*/
	int melt_parameterization;
	iomodel->FindConstant(&melt_parameterization,"md.frontalforcings.parameterization");
	switch(melt_parameterization){
		case FrontalForcingsDefaultEnum:
			iomodel->FetchDataToInput(inputs,elements,"md.frontalforcings.meltingrate",CalvingMeltingrateEnum);
			break;
		case FrontalForcingsRignotEnum:
			iomodel->FetchDataToInput(inputs,elements,"md.frontalforcings.basin",FrontalForcingsBasinIdEnum);
			iomodel->FetchDataToInput(inputs,elements,"md.frontalforcings.subglacial_discharge",FrontalForcingsSubglacialDischargeEnum);
			iomodel->FetchDataToInput(inputs,elements,"md.frontalforcings.thermalforcing",FrontalForcingsThermalForcingEnum);
			break;
		default:
			_error_("Frontal forcings"<<EnumToStringx(melt_parameterization)<<" not supported yet");
	}
}
/*}}}*/
void LevelsetAnalysis::UpdateParameters(Parameters* parameters,IoModel* iomodel,int solution_enum,int analysis_enum){/*{{{*/

	parameters->AddObject(iomodel->CopyConstantObject("md.levelset.stabilization",LevelsetStabilizationEnum));
	parameters->AddObject(iomodel->CopyConstantObject("md.levelset.reinit_frequency",LevelsetReinitFrequencyEnum));
	parameters->AddObject(iomodel->CopyConstantObject("md.levelset.kill_icebergs",LevelsetKillIcebergsEnum));
	parameters->AddObject(iomodel->CopyConstantObject("md.levelset.migration_max",MigrationMaxEnum));

	int  calvinglaw;
	iomodel->FindConstant(&calvinglaw,"md.calving.law");
	switch(calvinglaw){
		case DefaultCalvingEnum:
		case CalvingLevermannEnum:
			break;
		case CalvingVonmisesEnum:
			parameters->AddObject(iomodel->CopyConstantObject("md.calving.min_thickness",CalvingMinthicknessEnum));
			break;
		case CalvingMinthicknessEnum:
			parameters->AddObject(iomodel->CopyConstantObject("md.calving.min_thickness",CalvingMinthicknessEnum));
			break;
		case CalvingHabEnum:
			break;
		case CalvingCrevasseDepthEnum:
			parameters->AddObject(iomodel->CopyConstantObject("md.calving.crevasse_opening_stress",CalvingCrevasseDepthEnum));
			break;
		case CalvingDev2Enum:
			parameters->AddObject(iomodel->CopyConstantObject("md.calving.height_above_floatation",CalvingHeightAboveFloatationEnum));
			break;
		default:
			_error_("Calving law "<<EnumToStringx(calvinglaw)<<" not supported yet");
	}

	/*Get frontal melt parameters*/
	int melt_parameterization;
	iomodel->FindConstant(&melt_parameterization,"md.frontalforcings.parameterization");
	switch(melt_parameterization){
		case FrontalForcingsDefaultEnum:
			break;
		case FrontalForcingsRignotEnum:
			parameters->AddObject(iomodel->CopyConstantObject("md.frontalforcings.numberofbasins",FrontalForcingsNumberofBasinsEnum));
			break;
		default:
			_error_("Frontal forcings "<<EnumToStringx(melt_parameterization)<<" not supported yet");
	}
}
/*}}}*/

/*Finite element Analysis*/
void           LevelsetAnalysis::Core(FemModel* femmodel){/*{{{*/

	/*parameters: */
	int  stabilization;
	bool save_results;
	femmodel->parameters->FindParam(&save_results,SaveResultsEnum);
	femmodel->parameters->FindParam(&stabilization,LevelsetStabilizationEnum);

	/*activate formulation: */
	femmodel->SetCurrentConfiguration(LevelsetAnalysisEnum);

	if(VerboseSolution()) _printf0_("   call computational core:\n");
	if(stabilization==4){
		solutionsequence_fct(femmodel);
	}
	else{
		solutionsequence_linear(femmodel);
	}

	if(save_results){
		int outputs[1] = {MaskIceLevelsetEnum};
		femmodel->RequestedOutputsx(&femmodel->results,&outputs[0],1);
	}
}/*}}}*/
ElementVector* LevelsetAnalysis::CreateDVector(Element* element){/*{{{*/
	/*Default, return NULL*/
	return NULL;
}/*}}}*/
ElementMatrix* LevelsetAnalysis::CreateJacobianMatrix(Element* element){/*{{{*/
	/* Jacobian required for the Newton solver */
	_error_("not implemented yet");
}/*}}}*/
ElementMatrix* LevelsetAnalysis::CreateKMatrix(Element* element){/*{{{*/

	if(!element->IsOnBase()) return NULL;
	Element* basalelement = element->SpawnBasalElement();

	/*Intermediaries */
	int  stabilization,dim, domaintype, calvinglaw;
	int i,j,k,row, col;
	IssmDouble kappa;
	IssmDouble Jdet, dt, D_scalar;
	IssmDouble h,hx,hy,hz;
	IssmDouble vel,v[3],w[3],c[3],m[3],dlsf[3];
	IssmDouble norm_dlsf, norm_calving, calvingrate, meltingrate, groundedice;
	IssmDouble migrationmax, calvinghaf, heaviside, haf_eps;
	IssmDouble* xyz_list = NULL;

	/*Get problem dimension and whether there is moving front or not*/
	basalelement->FindParam(&domaintype,DomainTypeEnum);
	basalelement->FindParam(&calvinglaw,CalvingLawEnum);
	basalelement->FindParam(&stabilization,LevelsetStabilizationEnum);
	switch(domaintype){
		case Domain2DverticalEnum:   dim = 1; break;
		case Domain2DhorizontalEnum: dim = 2; break;
		case Domain3DEnum:           dim = 2; break;
		default: _error_("mesh "<<EnumToStringx(domaintype)<<" not supported yet");
	}

	/*Calving threshold*/

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

	/*Initialize Element vector and other vectors*/
	ElementMatrix* Ke       = basalelement->NewElementMatrix();
	IssmDouble*    basis    = xNew<IssmDouble>(numnodes);
	IssmDouble*    dbasis   = xNew<IssmDouble>(2*numnodes);
	IssmDouble*    Bprime = NULL;
	if(stabilization==2){
		Bprime   = xNew<IssmDouble>(dim*numnodes);
	}

	/*Retrieve all inputs and parameters*/
	basalelement->GetVerticesCoordinates(&xyz_list);
	basalelement->FindParam(&dt,TimesteppingTimeStepEnum);
	basalelement->FindParam(&migrationmax,MigrationMaxEnum);
	Input* vx_input           = NULL;
	Input* vy_input           = NULL;
	Input* calvingratex_input = NULL;
	Input* calvingratey_input = NULL;
	Input* lsf_slopex_input   = NULL;
	Input* lsf_slopey_input   = NULL;
	Input* calvingrate_input  = NULL;
	Input* meltingrate_input  = NULL;
	Input* gr_input           = NULL;

	/*Load velocities*/
	switch(domaintype){
		case Domain2DverticalEnum:
			vx_input=basalelement->GetInput(VxEnum); _assert_(vx_input);
			break;
		case Domain2DhorizontalEnum:
			vx_input=basalelement->GetInput(VxEnum); _assert_(vx_input);
			vy_input=basalelement->GetInput(VyEnum); _assert_(vy_input);
			gr_input=basalelement->GetInput(MaskOceanLevelsetEnum); _assert_(gr_input);
			break;
		case Domain3DEnum:
			vx_input=basalelement->GetInput(VxAverageEnum); _assert_(vx_input);
			vy_input=basalelement->GetInput(VyAverageEnum); _assert_(vy_input);
			gr_input=basalelement->GetInput(MaskOceanLevelsetEnum); _assert_(gr_input);
			break;
		default: _error_("mesh "<<EnumToStringx(domaintype)<<" not supported yet");
	}

	/*Load calving inputs*/
	switch(calvinglaw){
		case DefaultCalvingEnum:
		case CalvingVonmisesEnum:
			lsf_slopex_input  = basalelement->GetInput(LevelsetfunctionSlopeXEnum); _assert_(lsf_slopex_input);
			if(dim==2) lsf_slopey_input  = basalelement->GetInput(LevelsetfunctionSlopeYEnum); _assert_(lsf_slopey_input);
			calvingrate_input = basalelement->GetInput(CalvingCalvingrateEnum);     _assert_(calvingrate_input);
			meltingrate_input = basalelement->GetInput(CalvingMeltingrateEnum);     _assert_(meltingrate_input);
			break;
		case CalvingLevermannEnum:
			switch(domaintype){
				case Domain2DverticalEnum:
					calvingratex_input=basalelement->GetInput(CalvingratexEnum); _assert_(calvingratex_input);
					break;
				case Domain2DhorizontalEnum:
					calvingratex_input=basalelement->GetInput(CalvingratexEnum); _assert_(calvingratex_input);
					calvingratey_input=basalelement->GetInput(CalvingrateyEnum); _assert_(calvingratey_input);
					break;
				case Domain3DEnum:
					calvingratex_input=basalelement->GetInput(CalvingratexAverageEnum); _assert_(calvingratex_input);
					calvingratey_input=basalelement->GetInput(CalvingrateyAverageEnum); _assert_(calvingratey_input);
					break;
				default: _error_("mesh "<<EnumToStringx(domaintype)<<" not supported yet");
			}
			meltingrate_input = basalelement->GetInput(CalvingMeltingrateEnum);     _assert_(meltingrate_input);
			break;
		case CalvingMinthicknessEnum:
			lsf_slopex_input  = basalelement->GetInput(LevelsetfunctionSlopeXEnum); _assert_(lsf_slopex_input);
			if(dim==2) lsf_slopey_input  = basalelement->GetInput(LevelsetfunctionSlopeYEnum); _assert_(lsf_slopey_input);
			meltingrate_input = basalelement->GetInput(CalvingMeltingrateEnum);     _assert_(meltingrate_input);
			break;
		case CalvingHabEnum:
			lsf_slopex_input  = basalelement->GetInput(LevelsetfunctionSlopeXEnum); _assert_(lsf_slopex_input);
			if(dim==2) lsf_slopey_input  = basalelement->GetInput(LevelsetfunctionSlopeYEnum); _assert_(lsf_slopey_input);
			meltingrate_input = basalelement->GetInput(CalvingMeltingrateEnum);     _assert_(meltingrate_input);
			break;
		case CalvingCrevasseDepthEnum:
			lsf_slopex_input  = basalelement->GetInput(LevelsetfunctionSlopeXEnum); _assert_(lsf_slopex_input);
			if(dim==2) lsf_slopey_input  = basalelement->GetInput(LevelsetfunctionSlopeYEnum); _assert_(lsf_slopey_input);
			meltingrate_input = basalelement->GetInput(CalvingMeltingrateEnum);     _assert_(meltingrate_input);
			break;
		case CalvingDev2Enum:
			basalelement->FindParam(&calvinghaf,CalvingHeightAboveFloatationEnum);
			lsf_slopex_input  = basalelement->GetInput(LevelsetfunctionSlopeXEnum); _assert_(lsf_slopex_input);
			if(dim==2) lsf_slopey_input  = basalelement->GetInput(LevelsetfunctionSlopeYEnum); _assert_(lsf_slopey_input);
			calvingrate_input = basalelement->GetInput(CalvingCalvingrateEnum);     _assert_(calvingrate_input);
			meltingrate_input = basalelement->GetInput(CalvingMeltingrateEnum);     _assert_(meltingrate_input);
			break;
		default:
			_error_("Calving law "<<EnumToStringx(calvinglaw)<<" not supported yet");
	}

	/* Start  looping on the number of gaussian points: */
	Gauss* gauss=basalelement->NewGauss(2);
	while(gauss->next()){

		basalelement->JacobianDeterminant(&Jdet,xyz_list,gauss);
		basalelement->NodalFunctions(basis,gauss);
		basalelement->NodalFunctionsDerivatives(dbasis,xyz_list,gauss);
		D_scalar=gauss->weight*Jdet;

		/* Transient */
		if(dt!=0.){
			for(i=0;i<numnodes;i++){
				for(j=0;j<numnodes;j++){
					Ke->values[i*numnodes+j] += D_scalar*basis[j]*basis[i];
				}
			}
			D_scalar=D_scalar*dt;
		}

		/* Advection */
		vx_input->GetInputValue(&v[0],gauss);
		vy_input->GetInputValue(&v[1],gauss);
		gr_input->GetInputValue(&groundedice,gauss);

		/*Get calving speed*/
		switch(calvinglaw){
			case DefaultCalvingEnum:
			case CalvingVonmisesEnum:
				lsf_slopex_input->GetInputValue(&dlsf[0],gauss);
				if(dim==2) lsf_slopey_input->GetInputValue(&dlsf[1],gauss);
				calvingrate_input->GetInputValue(&calvingrate,gauss);
				meltingrate_input->GetInputValue(&meltingrate,gauss);

				/*Limit calving rate to c <= v + 3 km/yr */
				vel=sqrt(v[0]*v[0] + v[1]*v[1]);
				if(calvingrate>migrationmax+vel) calvingrate = vel+migrationmax;
				if(groundedice<0) meltingrate = 0.;

				norm_dlsf=0.;
				for(i=0;i<dim;i++) norm_dlsf+=pow(dlsf[i],2);
				norm_dlsf=sqrt(norm_dlsf);

				if(norm_dlsf>1.e-10)
				 for(i=0;i<dim;i++){
					 c[i]=calvingrate*dlsf[i]/norm_dlsf; m[i]=meltingrate*dlsf[i]/norm_dlsf;
				 }
				else
				 for(i=0;i<dim;i++){
					 c[i]=0.; m[i]=0.;
				 }
				break;

			case CalvingLevermannEnum:
				calvingratex_input->GetInputValue(&c[0],gauss);
				if(dim==2) calvingratey_input->GetInputValue(&c[1],gauss);
				meltingrate_input->GetInputValue(&meltingrate,gauss);
				norm_calving=0.;
				for(i=0;i<dim;i++) norm_calving+=pow(c[i],2);
				norm_calving=sqrt(norm_calving)+1.e-14;
				for(i=0;i<dim;i++) m[i]=meltingrate*c[i]/norm_calving;
				break;

			case CalvingMinthicknessEnum:
				lsf_slopex_input->GetInputValue(&dlsf[0],gauss);
				if(dim==2) lsf_slopey_input->GetInputValue(&dlsf[1],gauss);
				meltingrate_input->GetInputValue(&meltingrate,gauss);

				norm_dlsf=0.;
				for(i=0;i<dim;i++) norm_dlsf+=pow(dlsf[i],2);
				norm_dlsf=sqrt(norm_dlsf);

				if(norm_dlsf>1.e-10)
				 for(i=0;i<dim;i++){
					 c[i]=0.;
					 m[i]=meltingrate*dlsf[i]/norm_dlsf;
				 }
				else
				 for(i=0;i<dim;i++){
					 c[i]=0.;
					 m[i]=0.;
				 }
				break;

			case CalvingHabEnum:
				lsf_slopex_input->GetInputValue(&dlsf[0],gauss);
				if(dim==2) lsf_slopey_input->GetInputValue(&dlsf[1],gauss);
				meltingrate_input->GetInputValue(&meltingrate,gauss);

				norm_dlsf=0.;
				for(i=0;i<dim;i++) norm_dlsf+=pow(dlsf[i],2);
				norm_dlsf=sqrt(norm_dlsf);

				if(norm_dlsf>1.e-10)
				 for(i=0;i<dim;i++){
					 c[i]=0.;
					 m[i]=meltingrate*dlsf[i]/norm_dlsf;
				 }
				else
				 for(i=0;i<dim;i++){
					 c[i]=0.;
					 m[i]=0.;
				 }
				break;

			case CalvingCrevasseDepthEnum:
				lsf_slopex_input->GetInputValue(&dlsf[0],gauss);
				if(dim==2) lsf_slopey_input->GetInputValue(&dlsf[1],gauss);
				meltingrate_input->GetInputValue(&meltingrate,gauss);

				if(groundedice<0) meltingrate = 0.;

				norm_dlsf=0.;
				for(i=0;i<dim;i++) norm_dlsf+=pow(dlsf[i],2);
				norm_dlsf=sqrt(norm_dlsf);

				if(norm_dlsf>1.e-10)
				 for(i=0;i<dim;i++){
					 c[i]=0.;
					 m[i]=meltingrate*dlsf[i]/norm_dlsf;
				 }
				else
				 for(i=0;i<dim;i++){
					 c[i]=0.;
					 m[i]=0.;
				 }
				break;

			case CalvingDev2Enum:
				  {
					lsf_slopex_input->GetInputValue(&dlsf[0],gauss);
					if(dim==2) lsf_slopey_input->GetInputValue(&dlsf[1],gauss);
					calvingrate_input->GetInputValue(&calvingrate,gauss);
					meltingrate_input->GetInputValue(&meltingrate,gauss);
					gr_input->GetInputValue(&groundedice,gauss);

					//idea: no retreat on ice above critical calving height "calvinghaf" . Limit using regularized Heaviside function.
					vel=sqrt(v[0]*v[0] + v[1]*v[1]);
					haf_eps=10.;
					if(groundedice-calvinghaf<=-haf_eps){
						// ice floats freely below calvinghaf: calve freely
						// undercutting has no effect:
						meltingrate=0.;
					}
					else if(groundedice-calvinghaf>=haf_eps){
						// ice is well above calvinghaf -> no calving back, i.e. limit calving rate to ice velocity
						calvingrate=min(calvingrate,vel);
						// ice is almost grounded: frontal undercutting has maximum effect (do nothing).
					}
					else{ // ice is close to calvinghaf: smooth transition between limitation and free calving.
						//heaviside: 0 for floating, 1 for grounded
						heaviside=(groundedice-calvinghaf+haf_eps)/(2.*haf_eps) + sin(PI*(groundedice-calvinghaf)/haf_eps)/(2.*PI);
						calvingrate=heaviside*(min(calvingrate,vel)-calvingrate)+calvingrate;
						meltingrate=heaviside*meltingrate+0.;
					}

					norm_dlsf=0.;
					for(i=0;i<dim;i++) norm_dlsf+=pow(dlsf[i],2);
					norm_dlsf=sqrt(norm_dlsf);

					if(norm_dlsf>1.e-10)
					 for(i=0;i<dim;i++){
						 c[i]=calvingrate*dlsf[i]/norm_dlsf;
						 m[i]=meltingrate*dlsf[i]/norm_dlsf;
					 }
					else
					 for(i=0;i<dim;i++){
						 c[i]=0.;
						 m[i]=0.;
					 }
					break;
				  }

			default:
				_error_("Calving law "<<EnumToStringx(calvinglaw)<<" not supported yet");
		}

		/*Levelset speed is ice velocity - calving rate*/
		for(i=0;i<dim;i++) w[i]=v[i]-c[i]-m[i];

		/*Compute D*/
		for(i=0;i<numnodes;i++){
			for(j=0;j<numnodes;j++){
				for(k=0;k<dim;k++){
					Ke->values[i*numnodes+j] += D_scalar*w[k]*dbasis[k*numnodes+j]*basis[i];
				}
			}
		}

		/* Stabilization */
		vel=0.;
		for(i=0;i<dim;i++) vel+=w[i]*w[i];
		vel=sqrt(vel)+1.e-14;
		switch(stabilization){
			case 0:
				// no stabilization, do nothing
				break;
			case 1:
				/* Artificial Diffusion */
				basalelement->ElementSizes(&hx,&hy,&hz);
				h=sqrt( pow(hx*w[0]/vel,2) + pow(hy*w[1]/vel,2) );
				kappa=h*vel/2.;
				for(i=0;i<numnodes;i++){
					for(j=0;j<numnodes;j++){
						for(k=0;k<dim;k++){
							Ke->values[i*numnodes+j] += D_scalar*kappa*dbasis[k*numnodes+j]*dbasis[k*numnodes+i];
						}
					}
				}
				break;
			case 2:
				  {
					/* Streamline Upwinding */
					basalelement->ElementSizes(&hx,&hy,&hz);
					h=sqrt( pow(hx*w[0]/vel,2) + pow(hy*w[1]/vel,2) );
					for(int i=0;i<numnodes;i++){
						for(int j=0;j<numnodes;j++){
							Ke->values[i*numnodes+j] += D_scalar*h/(2.*vel)*(
										dbasis[0*numnodes+i] *(w[0]*w[0]*dbasis[0*numnodes+j] + w[0]*w[1]*dbasis[1*numnodes+j]) +
										dbasis[1*numnodes+i] *(w[1]*w[0]*dbasis[0*numnodes+j] + w[1]*w[1]*dbasis[1*numnodes+j]) 
										);
						}
					}
				  }
				break;
			default:
				_error_("unknown type of stabilization in LevelsetAnalysis.cpp");
		}
	}

	/*Clean up and return*/
	xDelete<IssmDouble>(xyz_list);
	xDelete<IssmDouble>(basis);
	xDelete<IssmDouble>(dbasis);
	xDelete<IssmDouble>(Bprime);
	delete gauss;
	if(domaintype!=Domain2DhorizontalEnum){basalelement->DeleteMaterials(); delete basalelement;};
	return Ke;
}/*}}}*/
ElementVector* LevelsetAnalysis::CreatePVector(Element* element){/*{{{*/

	if(!element->IsOnBase()) return NULL;
	Element* basalelement = element->SpawnBasalElement();

	/*Intermediaries */
	int domaintype;
	IssmDouble  Jdet,dt;
	IssmDouble  lsf;
	IssmDouble* xyz_list = NULL;

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

	/*Initialize Element vector*/
	ElementVector* pe = basalelement->NewElementVector();
	basalelement->FindParam(&dt,TimesteppingTimeStepEnum);

	if(dt!=0.){
		/*Initialize basis vector*/
		IssmDouble*    basis = xNew<IssmDouble>(numnodes);

		/*Retrieve all inputs and parameters*/
		basalelement->GetVerticesCoordinates(&xyz_list);
		Input* levelset_input     = basalelement->GetInput(MaskIceLevelsetEnum);                    _assert_(levelset_input);

		/* Start  looping on the number of gaussian points: */
		Gauss* gauss=basalelement->NewGauss(2);
		while(gauss->next()){
			basalelement->JacobianDeterminant(&Jdet,xyz_list,gauss);
			basalelement->NodalFunctions(basis,gauss);

			/* old function value */
			levelset_input->GetInputValue(&lsf,gauss);
			for(int i=0;i<numnodes;i++) pe->values[i]+=Jdet*gauss->weight*lsf*basis[i];
		}

		/*Clean up and return*/
		xDelete<IssmDouble>(xyz_list);
		xDelete<IssmDouble>(basis);
		basalelement->FindParam(&domaintype,DomainTypeEnum);
		if(domaintype!=Domain2DhorizontalEnum){basalelement->DeleteMaterials(); delete basalelement;};
		delete gauss;
	}

	return pe;
}/*}}}*/
IssmDouble     LevelsetAnalysis::GetDistanceToStraight(IssmDouble* q, IssmDouble* s0, IssmDouble* s1){/*{{{*/
	// returns distance d of point q to straight going through points s0, s1
	// d=|a x b|/|b|
	// with a=q-s0, b=s1-s0

	/* Intermediaries */
	const int dim=2;
	int i;
	IssmDouble a[dim], b[dim];
	IssmDouble norm_b;

	for(i=0;i<dim;i++){
		a[i]=q[i]-s0[i];
		b[i]=s1[i]-s0[i];
	}

	norm_b=0.;
	for(i=0;i<dim;i++)
	 norm_b+=b[i]*b[i];
	norm_b=sqrt(norm_b);
	_assert_(norm_b>0.);

	return fabs(a[0]*b[1]-a[1]*b[0])/norm_b;
}/*}}}*/
void           LevelsetAnalysis::GetSolutionFromInputs(Vector<IssmDouble>* solution,Element* element){/*{{{*/
	_error_("not implemented yet");
}/*}}}*/
void           LevelsetAnalysis::GradientJ(Vector<IssmDouble>* gradient,Element*  element,int control_type,int control_interp,int control_index){/*{{{*/
	_error_("Not implemented yet");
}/*}}}*/
void           LevelsetAnalysis::InputUpdateFromSolution(IssmDouble* solution,Element* element){/*{{{*/

	int domaintype;
	element->FindParam(&domaintype,DomainTypeEnum);
	switch(domaintype){
		case Domain2DhorizontalEnum:
			element->InputUpdateFromSolutionOneDof(solution,MaskIceLevelsetEnum);
			break;
		case Domain3DEnum:
			element->InputUpdateFromSolutionOneDofCollapsed(solution,MaskIceLevelsetEnum);
			break;
		default: _error_("mesh "<<EnumToStringx(domaintype)<<" not supported yet");
	}
}/*}}}*/
void           LevelsetAnalysis::UpdateConstraints(FemModel* femmodel){/*{{{*/

	/*Intermediaries*/
	int         calvinglaw;
	IssmDouble  min_thickness,thickness,hab_fraction;
	IssmDouble	crevassedepth,surface_crevasse,surface,critical_fraction;
	IssmDouble  rho_ice,rho_water;
	IssmDouble  bed,water_depth;
	IssmDouble  levelset,sealevel;

	femmodel->parameters->FindParam(&calvinglaw,CalvingLawEnum);

	if(calvinglaw==CalvingMinthicknessEnum || calvinglaw==CalvingVonmisesEnum){

		/*Get minimum thickness threshold*/
		femmodel->parameters->FindParam(&min_thickness,CalvingMinthicknessEnum);

		/*Loop over all elements of this partition*/
		for(Object* & object : femmodel->elements->objects){
			Element* element  = xDynamicCast<Element*>(object);

			int      numnodes = element->GetNumberOfNodes();
			Gauss*   gauss    = element->NewGauss();
			Input*   H_input  = element->GetInput(ThicknessEnum); _assert_(H_input);
			Input*   b_input = element->GetInput(BedEnum); _assert_(b_input);
			Input*   sl_input = element->GetInput(SealevelEnum); _assert_(sl_input);

			/*Potentially constrain nodes of this element*/
			for(int in=0;in<numnodes;in++){
				gauss->GaussNode(element->GetElementType(),in);
				Node* node=element->GetNode(in);
				if(!node->IsActive()) continue;

				H_input->GetInputValue(&thickness,gauss);
				b_input->GetInputValue(&bed,gauss);
				sl_input->GetInputValue(&sealevel,gauss);
				if(thickness<min_thickness && bed<sealevel){
					node->ApplyConstraint(0,+1.);
				}
				else {
					/* no ice, set no spc */
					node->DofInFSet(0);
				}
			}
			delete gauss;
		}
	}

	if(calvinglaw==CalvingHabEnum){

		/*Get the fraction of the flotation thickness at the terminus*/
		InputDuplicatex(femmodel,MaskIceLevelsetEnum,DistanceToCalvingfrontEnum);
		femmodel->DistanceToFieldValue(MaskIceLevelsetEnum,0,DistanceToCalvingfrontEnum);

		/*Loop over all elements of this partition*/
		for(Object* & object : femmodel->elements->objects){
			Element* element  = xDynamicCast<Element*>(object);

			rho_ice = element->FindParam(MaterialsRhoIceEnum);
			rho_water = element->FindParam(MaterialsRhoSeawaterEnum);

			int      numnodes           = element->GetNumberOfNodes();
			Gauss*   gauss              = element->NewGauss();
			Input*   H_input            = element->GetInput(ThicknessEnum); _assert_(H_input);
			Input*   bed_input          = element->GetInput(BedEnum); _assert_(bed_input);
			Input*   hab_fraction_input = element->GetInput(CalvingHabFractionEnum); _assert_(hab_fraction_input);
			Input*   ls_input           = element->GetInput(DistanceToCalvingfrontEnum); _assert_(ls_input);

			/*Potentially constrain nodes of this element*/
			for(int in=0;in<numnodes;in++){
				gauss->GaussNode(element->GetElementType(),in);
				Node* node=element->GetNode(in);
				if(!node->IsActive()) continue;

				H_input->GetInputValue(&thickness,gauss);
				bed_input->GetInputValue(&water_depth,gauss);
				ls_input->GetInputValue(&levelset,gauss);
				hab_fraction_input->GetInputValue(&hab_fraction,gauss);

				if(thickness<((rho_water/rho_ice)*(1+hab_fraction)*-water_depth) && levelset>-300. && levelset<0.){
					node->ApplyConstraint(0,+1.);
				}
				else {
					/* no ice, set no spc */
					node->DofInFSet(0);
				}
			}
			delete gauss;
		}
	}

	if(calvinglaw==CalvingCrevasseDepthEnum){

		int                 nflipped,local_nflipped;
		Vector<IssmDouble>* vec_constraint_nodes = NULL;
		IssmDouble* constraint_nodes = NULL;

		/*Get the DistanceToCalvingfront*/
		InputDuplicatex(femmodel,MaskIceLevelsetEnum,DistanceToCalvingfrontEnum);
		femmodel->DistanceToFieldValue(MaskIceLevelsetEnum,0,DistanceToCalvingfrontEnum);

		/*Vector of size number of nodes*/
		vec_constraint_nodes=new Vector<IssmDouble>(femmodel->nodes->NumberOfNodes());

		for(Object* & object : femmodel->elements->objects){
			Element* element               = xDynamicCast<Element*>(object);
			int      numnodes              = element->GetNumberOfNodes();
			Gauss*   gauss                 = element->NewGauss();
			Input*   crevassedepth_input   = element->GetInput(CrevasseDepthEnum); _assert_(crevassedepth_input);
			Input*   bed_input             = element->GetInput(BedEnum); _assert_(bed_input);
			Input*   surface_crevasse_input = element->GetInput(SurfaceCrevasseEnum); _assert_(surface_crevasse_input);
			Input*   thickness_input       = element->GetInput(ThicknessEnum); _assert_(thickness_input);
			Input*   surface_input         = element->GetInput(SurfaceEnum); _assert_(surface_input);

			/*First, look at ice front and figure out if any of the nodes will be calved*/
			if(element->IsIcefront()){
				for(int in=0;in<numnodes;in++){
					gauss->GaussNode(element->GetElementType(),in);
					Node* node=element->GetNode(in);
					if(!node->IsActive()) continue;

					crevassedepth_input->GetInputValue(&crevassedepth,gauss);
					bed_input->GetInputValue(&bed,gauss);
					surface_crevasse_input->GetInputValue(&surface_crevasse,gauss);
					thickness_input->GetInputValue(&thickness,gauss);
					surface_input->GetInputValue(&surface,gauss);

					if((surface_crevasse-surface>0. || crevassedepth-thickness>0.) && bed<0.){
						vec_constraint_nodes->SetValue(node->Sid(),1.0,INS_VAL);
					}
				}
			}
			delete gauss;
		}

		/*Assemble vector and serialize: */
		vec_constraint_nodes->Assemble();
		constraint_nodes=vec_constraint_nodes->ToMPISerial();

		nflipped=1;
		while(nflipped){
			local_nflipped=0;
			for(Object* & object : femmodel->elements->objects){
				Element* element                = xDynamicCast<Element*>(object);
				int      numnodes               = element->GetNumberOfNodes();
				Gauss*   gauss                  = element->NewGauss();
				Input*   levelset_input         = element->GetInput(DistanceToCalvingfrontEnum); _assert_(levelset_input);
				Input*   crevassedepth_input    = element->GetInput(CrevasseDepthEnum); _assert_(crevassedepth_input);
				Input*   bed_input              = element->GetInput(BedEnum); _assert_(bed_input);
				Input*   surface_crevasse_input = element->GetInput(SurfaceCrevasseEnum); _assert_(surface_crevasse_input);
				Input*   thickness_input        = element->GetInput(ThicknessEnum); _assert_(thickness_input);
				Input*   surface_input          = element->GetInput(SurfaceEnum); _assert_(surface_input);

				/*Is this element connected to a node that should be calved*/
				bool isconnected = false;
				for(int in=0;in<numnodes;in++){
					Node* node=element->GetNode(in);
					if(constraint_nodes[node->Sid()]==1.){
						isconnected = true;
						break;
					}
				}

				/*Check status if connected*/
				if(isconnected){
					for(int in=0;in<numnodes;in++){
						gauss->GaussNode(element->GetElementType(),in);
						Node* node=element->GetNode(in);
						levelset_input->GetInputValue(&levelset,gauss);
						crevassedepth_input->GetInputValue(&crevassedepth,gauss);
						bed_input->GetInputValue(&bed,gauss);
						surface_crevasse_input->GetInputValue(&surface_crevasse,gauss);
						thickness_input->GetInputValue(&thickness,gauss);
						surface_input->GetInputValue(&surface,gauss);

						if((surface_crevasse-surface>0. || crevassedepth-thickness>0.) && bed<0. && levelset>-300. && levelset<0. && constraint_nodes[node->Sid()]==0.){
							local_nflipped++;
							vec_constraint_nodes->SetValue(node->Sid(),1.0,INS_VAL);
						}
					}
				}
			}

			/*Count how many new nodes were found*/
			ISSM_MPI_Allreduce(&local_nflipped,&nflipped,1,ISSM_MPI_INT,ISSM_MPI_SUM,IssmComm::GetComm());
			//_printf0_("Found "<<nflipped<<" to flip\n");

			/*Assemble and serialize flag vector*/
			vec_constraint_nodes->Assemble();
			xDelete<IssmDouble>(constraint_nodes);
			constraint_nodes=vec_constraint_nodes->ToMPISerial();
		}
		/*Free ressources:*/
		delete vec_constraint_nodes;

		/*Contrain the nodes that will be calved*/
		for(Object* & object : femmodel->elements->objects){
			Element* element  = xDynamicCast<Element*>(object);
			int      numnodes = element->GetNumberOfNodes();
			Gauss*   gauss    = element->NewGauss();
			/*Potentially constrain nodes of this element*/
			for(int in=0;in<numnodes;in++){
				gauss->GaussNode(element->GetElementType(),in);
				Node* node=element->GetNode(in);
				if(!node->IsActive()) continue;

				if(constraint_nodes[node->Sid()]>0.){
					node->ApplyConstraint(0,+1.);
				}
				else {
					/* no ice, set no spc */
					node->DofInFSet(0);
				}
			}
			delete gauss;
		}
		xDelete<IssmDouble>(constraint_nodes);
	}

	/*Default, do nothing*/
	return;
}/*}}}*/
