#include "./Balancethickness2Analysis.h"
#include "../toolkits/toolkits.h"
#include "../classes/classes.h"
#include "../shared/shared.h"
#include "../modules/modules.h"

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

	/*Finite element type*/
	int 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,BalancethicknessApparentMassbalanceEnum);
	iomodel->FetchDataToInput(elements,BalancethicknessNuxEnum);
	iomodel->FetchDataToInput(elements,BalancethicknessNuyEnum);
	iomodel->FetchDataToInput(elements,BalancethicknessVxObsEnum);
	iomodel->FetchDataToInput(elements,BalancethicknessVyObsEnum);
	iomodel->FetchDataToInput(elements,BalancethicknessThicknessObsEnum);
	iomodel->FetchDataToInput(elements,MeshVertexonboundaryEnum);
	iomodel->FetchDataToInput(elements,MaskIceLevelsetEnum);
}/*}}}*/
void Balancethickness2Analysis::CreateNodes(Nodes* nodes,IoModel* iomodel){/*{{{*/

	int finiteelement = P1Enum;
	::CreateNodes(nodes,iomodel,Balancethickness2AnalysisEnum,finiteelement);
}/*}}}*/
void Balancethickness2Analysis::CreateConstraints(Constraints* constraints,IoModel* iomodel){/*{{{*/


	int finiteelement = P1Enum;
	IoModelToConstraintsx(constraints,iomodel,BalancethicknessSpcpotentialEnum,Balancethickness2AnalysisEnum,finiteelement);

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

}/*}}}*/

/*Finite Element Analysis*/
void           Balancethickness2Analysis::Core(FemModel* femmodel){/*{{{*/
	_error_("not implemented yet");
}/*}}}*/
ElementVector* Balancethickness2Analysis::CreateDVector(Element* element){/*{{{*/
	/*Default, return NULL*/
	return NULL;
}/*}}}*/
ElementMatrix* Balancethickness2Analysis::CreateJacobianMatrix(Element* element){/*{{{*/
_error_("Not implemented");
}/*}}}*/
ElementMatrix* Balancethickness2Analysis::CreateKMatrix(Element* element){/*{{{*/

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

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

	/*Initialize Element vector and other vectors*/
	ElementMatrix* Ke     = element->NewElementMatrix();
	IssmDouble*    dbasis = xNew<IssmDouble>(2*numnodes);

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

	/* 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->NodalFunctionsDerivatives(dbasis,xyz_list,gauss);
		element->JacobianDeterminant(&Jdet,xyz_list,gauss);

		for(int i=0;i<numnodes;i++){
			for(int j=0;j<numnodes;j++){
				Ke->values[i*numnodes+j] += gauss->weight*Jdet*(dbasis[0*numnodes+i]*dbasis[0*numnodes+j] + dbasis[1*numnodes+i]*dbasis[1*numnodes+j]);
			}
		}
	}

	/*Clean up and return*/
	xDelete<IssmDouble>(xyz_list);
	xDelete<IssmDouble>(dbasis);
	delete gauss;
	return Ke;
}/*}}}*/
ElementVector* Balancethickness2Analysis::CreatePVector(Element* element){/*{{{*/

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

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

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

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

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

	/*Retrieve all inputs and parameters*/
	element->GetVerticesCoordinates(&xyz_list);
	Input* adot_input   = element->GetInput(BalancethicknessApparentMassbalanceEnum); _assert_(adot_input);

	/* 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);
		adot_input->GetInputValue(&adot,gauss);

		for(int i=0;i<numnodes;i++) pe->values[i]+=Jdet*gauss->weight*adot*basis[i];
	}

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

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

	/*Intermediaries*/
	IssmDouble  Jdet,thickness,vx,vy;
	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();
	IssmDouble*    basis = xNew<IssmDouble>(numnodes);

	/*Retrieve all inputs and parameters*/
	Input* thickness_input = element->GetInput(BalancethicknessThicknessObsEnum); _assert_(thickness_input);
	Input* vx_input        = element->GetInput(BalancethicknessVxObsEnum);        _assert_(vx_input);
	Input* vy_input        = element->GetInput(BalancethicknessVyObsEnum);        _assert_(vy_input);

	element->GetVerticesCoordinates(&xyz_list);
	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);
		vx_input->GetInputValue(&vx,gauss);
		vy_input->GetInputValue(&vy,gauss);
		element->JacobianDeterminantSurface(&Jdet,xyz_list_front,gauss);
		element->NodalFunctions(basis,gauss);

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

	/*Clean up and return*/
	xDelete<IssmDouble>(xyz_list);
	xDelete<IssmDouble>(xyz_list_front);
	xDelete<IssmDouble>(basis);
	delete gauss;
	return pe;
}/*}}}*/
void Balancethickness2Analysis::GetSolutionFromInputs(Vector<IssmDouble>* solution,Element* element){/*{{{*/
	   _error_("not implemented yet");
}/*}}}*/
void Balancethickness2Analysis::GradientJ(Vector<IssmDouble>* gradient,Element* element,int control_type,int control_index){/*{{{*/
	_error_("Not implemented yet");
}/*}}}*/
void Balancethickness2Analysis::InputUpdateFromSolution(IssmDouble* solution,Element* element){/*{{{*/

	/*Intermediaries */
	int         Hinterpolation;
	IssmDouble  vx,vy,vbar,nux,nuy,normdphi,dphi[2];
	IssmDouble* xyz_list = NULL;
	int       * doflist  = NULL;

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

	/*Fetch dof list and allocate solution vector*/
	element->GetDofList(&doflist,NoneApproximationEnum,GsetEnum);
	IssmDouble* values         = xNew<IssmDouble>(numnodes);
	IssmDouble* thickness_list = xNew<IssmDouble>(numvertices);
	IssmDouble* vx_list        = xNew<IssmDouble>(numvertices);
	IssmDouble* vy_list        = xNew<IssmDouble>(numvertices);

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

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

	element->AddInput(PotentialEnum,values,element->GetElementType());

	/*Retrieve all inputs and parameters*/
	element->GetVerticesCoordinates(&xyz_list);
	Input* potential_input = element->GetInput(PotentialEnum);             _assert_(potential_input);
	Input* vx_input        = element->GetInput(BalancethicknessVxObsEnum); _assert_(vx_input);
	Input* vy_input        = element->GetInput(BalancethicknessVyObsEnum); _assert_(vy_input);
	Input* nux_input       = element->GetInput(BalancethicknessNuxEnum);   _assert_(nux_input);
	Input* nuy_input       = element->GetInput(BalancethicknessNuyEnum);   _assert_(nuy_input);

	switch(element->GetElementType()){
		case P1Enum: Hinterpolation = P0Enum; break;
		default:     _error_("not implemented");
	}

	Gauss* gauss=element->NewGauss();
	for (int iv=0;iv<1;iv++){
		gauss->GaussNode(Hinterpolation,iv);//P0 Only for now

		vx_input->GetInputValue(&vx,gauss);
		vy_input->GetInputValue(&vy,gauss);
		nux_input->GetInputValue(&nux,gauss);
		nuy_input->GetInputValue(&nuy,gauss);
		potential_input->GetInputDerivativeValue(&dphi[0],xyz_list,gauss);

		vx = vx*nux; vy = vy*nuy;
		vbar = sqrt(vx*vx + vy*vy) + 1.e-10;
		normdphi = sqrt(dphi[0]*dphi[0] + dphi[1]*dphi[1]);

		thickness_list[iv] = normdphi/vbar;
		vx_list[iv]        = -1./thickness_list[iv] * dphi[0];
		vy_list[iv]        = -1./thickness_list[iv] * dphi[1];
	}
	element->AddInput(ThicknessEnum,thickness_list,Hinterpolation);
	element->AddInput(VxEnum,vx_list,Hinterpolation);
	element->AddInput(VyEnum,vy_list,Hinterpolation);

	/*Clean up and return*/
	delete gauss;
	xDelete<int>(doflist);
	xDelete<IssmDouble>(xyz_list);
	xDelete<IssmDouble>(values);
	xDelete<IssmDouble>(thickness_list);
	xDelete<IssmDouble>(vx_list);
	xDelete<IssmDouble>(vy_list);
}/*}}}*/
void Balancethickness2Analysis::UpdateConstraints(FemModel* femmodel){/*{{{*/
	/*Default, do nothing*/
	return;
}/*}}}*/
