#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"

int LevelsetAnalysis::DofsPerNode(int** doflist,int domaintype,int approximation){/*{{{*/
	return 1;
}
/*}}}*/
void LevelsetAnalysis::UpdateParameters(Parameters* parameters,IoModel* iomodel,int solution_enum,int analysis_enum){/*{{{*/
	return;
}
/*}}}*/
void LevelsetAnalysis::UpdateElements(Elements* elements,IoModel* iomodel,int analysis_counter,int analysis_type){/*{{{*/
	int  finiteelement;

	/*Finite element type*/
	finiteelement = P1Enum;

	/*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);
			counter++;
		}
	}
	
	iomodel->FetchDataToInput(elements,MaskIceLevelsetEnum);
	iomodel->FetchDataToInput(elements,VxEnum);
	iomodel->FetchDataToInput(elements,VyEnum);
	iomodel->FetchDataToInput(elements,MasstransportCalvingrateEnum);
}
/*}}}*/
void LevelsetAnalysis::CreateNodes(Nodes* nodes,IoModel* iomodel){/*{{{*/
	int finiteelement=P1Enum;
	if(iomodel->domaintype!=Domain2DhorizontalEnum) iomodel->FetchData(2,MeshVertexonbaseEnum,MeshVertexonsurfaceEnum);
	::CreateNodes(nodes,iomodel,LevelsetAnalysisEnum,finiteelement);
	iomodel->DeleteData(2,MeshVertexonbaseEnum,MeshVertexonsurfaceEnum);
}
/*}}}*/
void LevelsetAnalysis::CreateConstraints(Constraints* constraints,IoModel* iomodel){/*{{{*/
	return;
}
/*}}}*/
void LevelsetAnalysis::CreateLoads(Loads* loads, IoModel* iomodel){/*{{{*/
	return;
}/*}}}*/

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

	#if !defined(_DEVELOPMENT_)
	_error_("Not implemented yet");
	#endif

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

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

	if(VerboseSolution()) _printf0_("call computational core:\n");
	solutionsequence_linear(femmodel);

	if(save_results){
		if(VerboseSolution()) _printf0_("   saving results\n");
		int outputs = MaskIceLevelsetEnum;
		femmodel->RequestedOutputsx(&femmodel->results,&outputs,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  dim, domaintype;
	int i, row, col;
	IssmDouble kappa;
	IssmDouble Jdet, dt, D_scalar;
	IssmDouble h,hx,hy,hz;
	IssmDouble vel, calvingrate;
	IssmDouble norm_dlsf;
	IssmDouble* xyz_list = NULL;

	/*Get problem dimension*/
	basalelement->FindParam(&domaintype,DomainTypeEnum);
	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");
	}

	/*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*    B        = xNew<IssmDouble>(dim*numnodes);
	IssmDouble*    Bprime   = xNew<IssmDouble>(dim*numnodes);
	IssmDouble*    D        = xNew<IssmDouble>(dim*dim);
	IssmDouble*    v        = xNew<IssmDouble>(dim);
	IssmDouble*    w        = xNew<IssmDouble>(dim);
	IssmDouble*    c        = xNew<IssmDouble>(dim);
	IssmDouble*    dlsf     = xNew<IssmDouble>(dim);

	/*Retrieve all inputs and parameters*/
	basalelement->GetVerticesCoordinates(&xyz_list);
	basalelement->FindParam(&dt,TimesteppingTimeStepEnum);
	Input* vx_input=NULL;
	Input* vy_input=NULL;
	if(domaintype==Domain2DhorizontalEnum){
		vx_input=basalelement->GetInput(VxEnum); _assert_(vx_input);
		vy_input=basalelement->GetInput(VyEnum); _assert_(vy_input);
	}
	else{
		if(dim==1){
			vx_input=basalelement->GetInput(VxEnum); _assert_(vx_input);
		}
		if(dim==2){
			vx_input=basalelement->GetInput(VxAverageEnum); _assert_(vx_input);
			vy_input=basalelement->GetInput(VyAverageEnum); _assert_(vy_input);
		}
	}

	Input* lsf_slopex_input  = basalelement->GetInput(LevelsetfunctionSlopeXEnum); _assert_(lsf_slopex_input);
	Input* lsf_slopey_input  = basalelement->GetInput(LevelsetfunctionSlopeYEnum); _assert_(lsf_slopey_input);
	Input* calvingrate_input  = basalelement->GetInput(MasstransportCalvingrateEnum);     _assert_(calvingrate_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);
		D_scalar=gauss->weight*Jdet;

		/* Transient */
		if(dt!=0.){
			basalelement->NodalFunctions(basis,gauss);
			TripleMultiply(basis,numnodes,1,0,
						&D_scalar,1,1,0,
						basis,1,numnodes,0,
						&Ke->values[0],1);
			D_scalar*=dt;
		}

		/* Advection */
		GetB(B,basalelement,xyz_list,gauss); 
		GetBprime(Bprime,basalelement,xyz_list,gauss); 
		vx_input->GetInputValue(&v[0],gauss); // in 3D case, add mesh velocity 
		vy_input->GetInputValue(&v[1],gauss); 
		lsf_slopex_input->GetInputValue(&dlsf[0],gauss);
		lsf_slopey_input->GetInputValue(&dlsf[1],gauss);
		calvingrate_input->GetInputValue(&calvingrate,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]=calvingrate*dlsf[i]/norm_dlsf;
		else
			for(i=0;i<dim;i++) c[i]=0.;
		
		for(i=0;i<dim;i++) w[i]=v[i]-c[i];

		for(row=0;row<dim;row++)
			for(col=0;col<dim;col++)
				if(row==col)
					D[row*dim+col]=D_scalar*w[row];
				else
				   D[row*dim+col]=0.;

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

		/* Stabilization */
		int stabilization=2;
		vel=0.;
		for(i=0;i<dim;i++) vel+=v[i]*v[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*v[0]/vel,2) + pow(hy*v[1]/vel,2) ); 
				kappa=h*vel/2.;
				for(row=0;row<dim;row++)
					for(col=0;col<dim;col++)
					if(row==col)
						D[row*dim+col]=D_scalar*kappa;
					else
						D[row*dim+col]=0.;

				TripleMultiply(Bprime,dim,numnodes,1,
							D,dim,dim,0,
							Bprime,dim,numnodes,0,
							&Ke->values[0],1);
				break;	
			case 2:
				/* Streamline Upwinding */
				basalelement->ElementSizes(&hx,&hy,&hz);
				h=sqrt( pow(hx*v[0]/vel,2) + pow(hy*v[1]/vel,2) );
				for(row=0;row<dim;row++) 
					for(col=0;col<dim;col++) 
						D[row*dim+col] = D_scalar*h/(2.*vel)*v[row]*v[col];

				TripleMultiply(Bprime,dim,numnodes,1,
							D,dim,dim,0,
							Bprime,dim,numnodes,0,
							&Ke->values[0],1);
				break;
			default:
				_error_("unknown type of stabilization in LevelsetAnalysis.cpp");
		}
	}

	/*Clean up and return*/
	xDelete<IssmDouble>(xyz_list);
	xDelete<IssmDouble>(basis);
	xDelete<IssmDouble>(B);
	xDelete<IssmDouble>(D);
	xDelete<IssmDouble>(Bprime);
	xDelete<IssmDouble>(v);
	xDelete<IssmDouble>(w);
	xDelete<IssmDouble>(c);
	xDelete<IssmDouble>(dlsf);
	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 i, ig, 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);
		for(ig=gauss->begin();ig<gauss->end();ig++){
			gauss->GaussPoint(ig);

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

			/* old function value */
			levelset_input->GetInputValue(&lsf,gauss);
			for(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;
}/*}}}*/
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_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){/*{{{*/
	/*Default, do nothing*/
	return;
}/*}}}*/
void LevelsetAnalysis::GetB(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=[ N ]
	 *          [ N ]
	 * where N is the finiteelement function for node i.
	 *
	 * We assume B_prog has been allocated already, of size: 2x(NDOF1*numnodes)
	 */

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

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

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

	/*Clean-up*/
	xDelete<IssmDouble>(basis);
}/*}}}*/
void LevelsetAnalysis::GetBprime(IssmDouble* Bprime,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_prime=[ dN/dx ]
	 *                [ dN/dy ]
	 * 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++){
		Bprime[numnodes*0+i] = dbasis[0*numnodes+i];
		Bprime[numnodes*1+i] = dbasis[1*numnodes+i];
	}

	/*Clean-up*/
	xDelete<IssmDouble>(dbasis);

}/*}}}*/
void LevelsetAnalysis::SetDistanceOnIntersectedElements(FemModel* femmodel){/*{{{*/

	/* Intermediaries */
	int i,k;
	IssmDouble dmaxp=0.,dmaxm=0,val=0.;

	/*Initialize vector with number of vertices*/
	int numvertices=femmodel->vertices->NumberOfVertices();
	Element* element;

	Vector<IssmDouble>* vec_dist_zerolevelset = NULL;
	GetVectorFromInputsx(&vec_dist_zerolevelset, femmodel, MaskIceLevelsetEnum, VertexEnum);
	
	/* set NaN on elements intersected by zero levelset */
	for(i=0;i<femmodel->elements->Size();i++){
		element=dynamic_cast<Element*>(femmodel->elements->GetObjectByOffset(i));
		if(element->IsZeroLevelset(MaskIceLevelsetEnum))
			for(k=0;k<element->GetNumberOfVertices();k++)
				vec_dist_zerolevelset->SetValue(element->vertices[k]->Sid(),NAN,INS_VAL); 
	}

	/* set distance on elements intersected by zero levelset */
	for(i=0;i<femmodel->elements->Size();i++){
		element=dynamic_cast<Element*>(femmodel->elements->GetObjectByOffset(i));
		if(element->IsZeroLevelset(MaskIceLevelsetEnum)){
			SetDistanceToZeroLevelsetElement(vec_dist_zerolevelset, element);

			/* Get maximum distance to interface along vertices */
			for(k=0;k<element->GetNumberOfVertices();k++){
					vec_dist_zerolevelset->GetValue(&val,element->vertices[k]->Sid()); 
					if((val>0.) && (val>dmaxp))
						 dmaxp=val;
					else if((val<0.) && (val<dmaxm))
						 dmaxm=val;
			}
		}
	}

	/* set all none intersected vertices to max/min distance */
	for(i=0;i<numvertices;i++){
		vec_dist_zerolevelset->GetValue(&val,i);
		if(val==1.) //FIXME: improve check
			vec_dist_zerolevelset->SetValue(i,3.*dmaxp,INS_VAL);
		else if(val==-1.)
			vec_dist_zerolevelset->SetValue(i,3.*dmaxm,INS_VAL);
	}

	/*Assemble vector and serialize */
	vec_dist_zerolevelset->Assemble();
	IssmDouble* dist_zerolevelset=vec_dist_zerolevelset->ToMPISerial();
	InputUpdateFromVectorx(femmodel,dist_zerolevelset,MaskIceLevelsetEnum,VertexSIdEnum);

	/*Clean up and return*/
	delete vec_dist_zerolevelset;
	delete dist_zerolevelset;
}/*}}}*/
void LevelsetAnalysis::SetDistanceToZeroLevelsetElement(Vector<IssmDouble>* vec_signed_dist, Element* element){/*{{{*/

	if(!element->IsZeroLevelset(MaskIceLevelsetEnum))
		return;

	/* Intermediaries */
	const int dim=3;
	int i,d;
	int numvertices=element->GetNumberOfVertices();
	IssmDouble s0[dim], s1[dim], v[dim];
	IssmDouble dist,lsf_old;

	IssmDouble* lsf = xNew<IssmDouble>(numvertices);
	IssmDouble* sign_lsf = xNew<IssmDouble>(numvertices);
	IssmDouble* signed_dist = xNew<IssmDouble>(numvertices);
	IssmDouble* xyz_list = NULL;
	IssmDouble* xyz_list_zero = NULL;

	/* retrieve inputs and parameters */
	element->GetVerticesCoordinates(&xyz_list);
	element->GetInputListOnVertices(lsf,MaskIceLevelsetEnum);

	/* get sign of levelset function */
	for(i=0;i<numvertices;i++)
		sign_lsf[i]=(lsf[i]>=0.?1.:-1.);

	element->ZeroLevelsetCoordinates(&xyz_list_zero, xyz_list, MaskIceLevelsetEnum);
	for(d=0;d<dim;d++){
		s0[d]=xyz_list_zero[0+d];
		s1[d]=xyz_list_zero[3+d];
	}

	/* get signed_distance of vertices to zero levelset straight */
	for(i=0;i<numvertices;i++){
		for(d=0;d<dim;d++)
			v[d]=xyz_list[3*i+d];
		dist=GetDistanceToStraight(&v[0],&s0[0],&s1[0]);
		signed_dist[i]=sign_lsf[i]*dist;
	}
	
	/* insert signed_distance into vec_signed_dist, if computed distance is smaller */
	for(i=0;i<numvertices;i++){
		vec_signed_dist->GetValue(&lsf_old, element->vertices[i]->Sid());
		if(xIsNan<IssmDouble>(lsf_old) || fabs(signed_dist[i])<fabs(lsf_old))
			vec_signed_dist->SetValue(element->vertices[i]->Sid(),signed_dist[i],INS_VAL);
	}

	xDelete<IssmDouble>(lsf);
	xDelete<IssmDouble>(sign_lsf);
	xDelete<IssmDouble>(signed_dist);
	xDelete<IssmDouble>(xyz_list);
	xDelete<IssmDouble>(xyz_list_zero);
}/*}}}*/
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;
}/*}}}*/
