#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;
	_error_("not implemented yet");
}/*}}}*/
ElementVector* LevelsetAnalysis::CreatePVector(Element* element){/*{{{*/
	
	if(!element->IsOnBase()) return NULL;
	_error_("not implemented yet");
}/*}}}*/
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;
}/*}}}*/
