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

/*Model processing*/
int  UzawaPressureAnalysis::DofsPerNode(int** doflist,int domaintype,int approximation){/*{{{*/
	return 1;
}/*}}}*/
void UzawaPressureAnalysis::UpdateParameters(Parameters* parameters,IoModel* iomodel,int solution_enum,int analysis_enum){/*{{{*/

	parameters->AddObject(iomodel->CopyConstantObject(AugmentedLagrangianRhopEnum));
}/*}}}*/
void UzawaPressureAnalysis::UpdateElements(Elements* elements,IoModel* iomodel,int analysis_counter,int analysis_type){/*{{{*/

	/*Update elements: */
	int finiteelement = P1Enum;
	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,VxEnum,0.);
	iomodel->FetchDataToInput(elements,VyEnum,0.);
	if(iomodel->domaintype==Domain3DEnum) iomodel->FetchDataToInput(elements,VzEnum,0.);
	iomodel->FetchDataToInput(elements,PressureEnum,0.);
}/*}}}*/
void UzawaPressureAnalysis::CreateNodes(Nodes* nodes,IoModel* iomodel){/*{{{*/

	int finiteelement = P1Enum;
	::CreateNodes(nodes,iomodel,UzawaPressureAnalysisEnum,finiteelement);
}/*}}}*/
void UzawaPressureAnalysis::CreateConstraints(Constraints* constraints,IoModel* iomodel){/*{{{*/
	return;
}/*}}}*/
void UzawaPressureAnalysis::CreateLoads(Loads* loads, IoModel* iomodel){/*{{{*/
	return;
}/*}}}*/

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

	/*Intermediaries*/
	IssmDouble  D_scalar,Jdet;
	IssmDouble *xyz_list = NULL;
	int numnodes = element->GetNumberOfNodes();

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

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

	Gauss* gauss = element->NewGauss(5);
	for(int ig=gauss->begin();ig<gauss->end();ig++){
		gauss->GaussPoint(ig);

		element->JacobianDeterminant(&Jdet,xyz_list,gauss);
		this->GetM(M,element,gauss);
		D_scalar=gauss->weight*Jdet;
		TripleMultiply(M,1,numnodes,1,
					&D_scalar,1,1,0,
					M,1,numnodes,0,
					&Ke->values[0],1);
	}

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

	/*Intermediaries*/
	int          dim;
	IssmDouble   Jdet,rhop,divu;
	IssmDouble   *xyz_list = NULL;
	int numnodes = element->GetNumberOfNodes();

	/*Retrieve all inputs and parameters*/
	element->FindParam(&dim,DomainDimensionEnum);
	element->FindParam(&rhop,AugmentedLagrangianRhopEnum);
	element->GetVerticesCoordinates(&xyz_list);

	/*Initialize Element matrix and vectors*/
	ElementVector* pe    = element->NewElementVector();
	IssmDouble*    basis = xNew<IssmDouble>(numnodes);
	IssmDouble*    dvx   = xNew<IssmDouble>(dim);
	IssmDouble*    dvy   = xNew<IssmDouble>(dim);
	IssmDouble*    dvz   = xNew<IssmDouble>(dim);

	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);}

	Gauss* gauss = element->NewGauss(5);
	for(int ig=gauss->begin();ig<gauss->end();ig++){
		gauss->GaussPoint(ig);

		element->JacobianDeterminant(&Jdet,xyz_list,gauss);
		element->NodalFunctions(basis, gauss);
		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);
		}

		divu=dvx[0]+dvy[1];
		if (dim==3) divu=divu+dvz[2];

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

	/*Clean up and return*/
	delete gauss;
	xDelete<IssmDouble>(xyz_list);
	xDelete<IssmDouble>(basis);
	xDelete<IssmDouble>(dvx);
	xDelete<IssmDouble>(dvy);
	xDelete<IssmDouble>(dvz);
	return pe;
}/*}}}*/
void UzawaPressureAnalysis::GetM(IssmDouble* M,Element* element,Gauss* gauss){/*{{{*/
	/*Compute B  matrix. M=[M1 M2 M3] */

	/*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++){
		M[i] = basis[i];
	}

	/*Clean-up*/
	xDelete<IssmDouble>(basis);
}/*}}}*/
void UzawaPressureAnalysis::GetSolutionFromInputs(Vector<IssmDouble>* solution,Element* element){/*{{{*/
	_error_("not implemented yet");
}/*}}}*/
void UzawaPressureAnalysis::GradientJ(Vector<IssmDouble>* gradient,Element* element,int control_type,int control_index){/*{{{*/
	_error_("Not implemented yet");
}/*}}}*/
void UzawaPressureAnalysis::InputUpdateFromSolution(IssmDouble* solution,Element* element){/*{{{*/

	int        *doflist   = NULL;

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

	/*Fetch dof list and allocate solution vector*/
	element->GetDofList(&doflist,NoneApproximationEnum,GsetEnum);
	IssmDouble* values    = xNew<IssmDouble>(numnodes);
	IssmDouble* pressure = xNew<IssmDouble>(numnodes);
	element->GetInputListOnNodes(&pressure[0],PressureEnum);

	for(int i=0;i<numnodes;i++){
		values[i]   = pressure[i] + solution[doflist[i]];
	}

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

	/*Free ressources:*/
	xDelete<IssmDouble>(values);
	xDelete<IssmDouble>(pressure);
	xDelete<int>(doflist);

}/*}}}*/
void UzawaPressureAnalysis::UpdateConstraints(FemModel* femmodel){/*{{{*/
	/*Default, do nothing*/
	return;
}/*}}}*/
