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

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

	/*Intermediaries*/
	int         numoutputs;
	char**      requestedoutputs = NULL;

	/*retrieve some parameters: */
	parameters->AddObject(iomodel->CopyConstantObject(DamageLawEnum));
	parameters->AddObject(iomodel->CopyConstantObject(DamageStabilizationEnum));
	parameters->AddObject(iomodel->CopyConstantObject(DamagePenaltyThresholdEnum));
	parameters->AddObject(iomodel->CopyConstantObject(DamagePenaltyLockEnum));
	parameters->AddObject(iomodel->CopyConstantObject(DamagePenaltyFactorEnum));
	parameters->AddObject(iomodel->CopyConstantObject(DamageMaxiterEnum));
	parameters->AddObject(iomodel->CopyConstantObject(DamageMaxDamageEnum));

	/*Requested outputs*/
	iomodel->FetchData(&requestedoutputs,&numoutputs,DamageEvolutionRequestedOutputsEnum);
	parameters->AddObject(new IntParam(DamageEvolutionNumRequestedOutputsEnum,numoutputs));
	if(numoutputs)parameters->AddObject(new StringArrayParam(DamageEvolutionRequestedOutputsEnum,requestedoutputs,numoutputs));
	iomodel->DeleteData(&requestedoutputs,numoutputs,DamageEvolutionRequestedOutputsEnum);

	/*Retrieve law dependent parameters: */
	char* law  = NULL;
	iomodel->Constant(&law,DamageLawEnum);
	if (strcmp(law,"pralong")==0){
		parameters->AddObject(iomodel->CopyConstantObject(DamageC1Enum));
		parameters->AddObject(iomodel->CopyConstantObject(DamageC2Enum));
		parameters->AddObject(iomodel->CopyConstantObject(DamageC3Enum));
		parameters->AddObject(iomodel->CopyConstantObject(DamageC4Enum));
		parameters->AddObject(iomodel->CopyConstantObject(DamageStressThresholdEnum));
		parameters->AddObject(iomodel->CopyConstantObject(DamageHealingEnum));
		parameters->AddObject(iomodel->CopyConstantObject(DamageEquivStressEnum));
	}
	xDelete<char>(law);

}/*}}}*/
void DamageEvolutionAnalysis::UpdateElements(Elements* elements,IoModel* iomodel,int analysis_counter,int analysis_type){/*{{{*/

	/*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,P1Enum);
			counter++;
		}
	}

	/*What input do I need to run my damage evolution model?*/
	iomodel->FetchDataToInput(elements,VxEnum);
	iomodel->FetchDataToInput(elements,VyEnum);
	iomodel->FetchDataToInput(elements,VzEnum);
	iomodel->FetchDataToInput(elements,DamageDEnum);
	iomodel->FetchDataToInput(elements,MaskIceLevelsetEnum);
	iomodel->FetchDataToInput(elements,PressureEnum);

	bool dakota_analysis;
	iomodel->Constant(&dakota_analysis,QmuIsdakotaEnum);
	if(dakota_analysis){
		elements->InputDuplicate(DamageDEnum, QmuDamageDEnum);
	}
}/*}}}*/
void DamageEvolutionAnalysis::CreateNodes(Nodes* nodes,IoModel* iomodel){/*{{{*/

	if(iomodel->meshtype!=Mesh2DhorizontalEnum) iomodel->FetchData(1,MeshVertexonbedEnum);
	::CreateNodes(nodes,iomodel,DamageEvolutionAnalysisEnum,P1Enum);
	iomodel->DeleteData(1,MeshVertexonbedEnum);
}/*}}}*/
void DamageEvolutionAnalysis::CreateConstraints(Constraints* constraints,IoModel* iomodel){/*{{{*/

	int stabilization;
	iomodel->Constant(&stabilization,DamageStabilizationEnum);

	IoModelToConstraintsx(constraints,iomodel,DamageSpcdamageEnum,DamageEvolutionAnalysisEnum,P1Enum);

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

	/*Nothing for now*/

}/*}}}*/

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

	/* Check if ice in element */
	if(!element->IsIceInElement()) return NULL;

	/*Intermediaries*/
	Element*    basalelement;
	int         meshtype,dim;
	int         stabilization;
	IssmDouble  Jdet,dt,D_scalar,h;
	IssmDouble  vel,vx,vy,dvxdx,dvydy,dvx[2],dvy[2];
	IssmDouble *xyz_list  = NULL;

	/*Get problem dimension and basal element*/
	element->FindParam(&meshtype,MeshTypeEnum);
	switch(meshtype){
		case Mesh2DhorizontalEnum:
			basalelement = element;
			dim = 2;
			break;
		case Mesh3DEnum:
			if(!element->IsOnBed()) return NULL;
			basalelement = element->SpawnBasalElement();
			dim = 2;
			break;
		default: _error_("mesh "<<EnumToStringx(meshtype)<<" not supported yet");
	}

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

	/*Initialize Element vector*/
	ElementMatrix* Ke     = basalelement->NewElementMatrix();
	IssmDouble*    basis  = xNew<IssmDouble>(numnodes);
	IssmDouble*    B      = xNew<IssmDouble>(dim*numnodes);
	IssmDouble*    Bprime = xNew<IssmDouble>(dim*numnodes);
	IssmDouble*    D      = xNewZeroInit<IssmDouble>(dim*dim);

	/*Retrieve all inputs and parameters*/
	basalelement->GetVerticesCoordinates(&xyz_list);
	basalelement->FindParam(&dt,TimesteppingTimeStepEnum);
	basalelement->FindParam(&stabilization,DamageStabilizationEnum);
	Input* vxaverage_input=NULL;
	Input* vyaverage_input=NULL;
	if(meshtype==Mesh2DhorizontalEnum){
		vxaverage_input=basalelement->GetInput(VxEnum); _assert_(vxaverage_input);
		vyaverage_input=basalelement->GetInput(VyEnum); _assert_(vyaverage_input);
	}
	else{
		if(dim==1){
			vxaverage_input=element->GetInput(VxEnum); _assert_(vxaverage_input);
		}
		if(dim==2){
			vxaverage_input=element->GetInput(VxAverageEnum); _assert_(vxaverage_input);
			vyaverage_input=element->GetInput(VyAverageEnum); _assert_(vyaverage_input);
		}
	}
	h=basalelement->CharacteristicLength();

	/* 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);
		basalelement->NodalFunctions(basis,gauss);
		
		vxaverage_input->GetInputValue(&vx,gauss);
		vxaverage_input->GetInputDerivativeValue(&dvx[0],xyz_list,gauss);
		if(dim==2){
			vyaverage_input->GetInputValue(&vy,gauss);
			vyaverage_input->GetInputDerivativeValue(&dvy[0],xyz_list,gauss);
		}

		D_scalar=gauss->weight*Jdet;
		TripleMultiply(basis,1,numnodes,1,
					&D_scalar,1,1,0,
					basis,1,numnodes,0,
					&Ke->values[0],1);

		GetB(B,basalelement,dim,xyz_list,gauss);
		GetBprime(Bprime,basalelement,dim,xyz_list,gauss);

		dvxdx=dvx[0];
		if(dim==2) dvydy=dvy[1];
		D_scalar=dt*gauss->weight*Jdet;

		D[0*dim+0]=D_scalar*dvxdx;
		if(dim==2) D[1*dim+1]=D_scalar*dvydy;

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

		D[0*dim+0]=D_scalar*vx;
		if(dim==2) D[1*dim+1]=D_scalar*vy;

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

		if(stabilization==2){
			if(dim==1){
				vel=fabs(vx)+1.e-8;
				D[0]=h/(2*vel)*vx*vx;
			}
			else{
				/*Streamline upwinding*/
				vel=sqrt(vx*vx+vy*vy)+1.e-8;
				D[0*dim+0]=h/(2*vel)*vx*vx;
				D[1*dim+0]=h/(2*vel)*vy*vx;
				D[0*dim+1]=h/(2*vel)*vx*vy;
				D[1*dim+1]=h/(2*vel)*vy*vy;
			}
		}
		else if(stabilization==1){
			vxaverage_input->GetInputAverage(&vx);
			if(dim==2) vyaverage_input->GetInputAverage(&vy);
			D[0*dim+0]=h/2.0*fabs(vx);
			if(dim==2) D[1*dim+1]=h/2.0*fabs(vy);
		}
		if(stabilization==1 || stabilization==2){
			if(dim==1) D[0]=D_scalar*D[0];
			else{
				D[0*dim+0]=D_scalar*D[0*dim+0];
				D[1*dim+0]=D_scalar*D[1*dim+0];
				D[0*dim+1]=D_scalar*D[0*dim+1];
				D[1*dim+1]=D_scalar*D[1*dim+1];
			}

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

	}

	/*Clean up and return*/
	xDelete<IssmDouble>(xyz_list);
	xDelete<IssmDouble>(basis);
	xDelete<IssmDouble>(B);
	xDelete<IssmDouble>(Bprime);
	xDelete<IssmDouble>(D);
	delete gauss;
	if(meshtype!=Mesh2DhorizontalEnum){basalelement->DeleteMaterials(); delete basalelement;};
	return Ke;
}/*}}}*/
ElementVector* DamageEvolutionAnalysis::CreatePVector(Element* element){/*{{{*/

	/* Check if ice in element */
	if(!element->IsIceInElement()) return NULL;

	/*Intermediaries*/
	int      meshtype;
	Element* basalelement;
	IssmDouble  Jdet,dt;
	IssmDouble  f,damage;
	IssmDouble* xyz_list = NULL;

	/*Get basal element*/
	element->FindParam(&meshtype,MeshTypeEnum);
	switch(meshtype){
		case Mesh2DhorizontalEnum:
			basalelement = element;
			break;
		case Mesh3DEnum:
			if(!element->IsOnBed()) return NULL;
			basalelement = element->SpawnBasalElement();
			break;
		default: _error_("mesh "<<EnumToStringx(meshtype)<<" not supported yet");
	}

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

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

	/*Retrieve all inputs and parameters*/
	basalelement->GetVerticesCoordinates(&xyz_list);
	basalelement->FindParam(&dt,TimesteppingTimeStepEnum);
	this->CreateDamageFInput(basalelement);
	Input* damaged_input = basalelement->GetInput(DamageDEnum); _assert_(damaged_input);
	Input* damagef_input = basalelement->GetInput(DamageFEnum); _assert_(damagef_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);
		basalelement->NodalFunctions(basis,gauss);

		damaged_input->GetInputValue(&damage,gauss);
		damagef_input->GetInputValue(&f,gauss);

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

	/*Clean up and return*/
	xDelete<IssmDouble>(xyz_list);
	xDelete<IssmDouble>(basis);
	if(meshtype!=Mesh2DhorizontalEnum){basalelement->DeleteMaterials(); delete basalelement;};
	delete gauss;
	return pe;
}/*}}}*/
void DamageEvolutionAnalysis::GetB(IssmDouble* B,Element* element,int dim,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++){
		for(int j=0;j<dim;j++){
			B[numnodes*j+i] = basis[i];
		}
	}

	/*Clean-up*/
	xDelete<IssmDouble>(basis);
}/*}}}*/
void DamageEvolutionAnalysis::GetBprime(IssmDouble* Bprime,Element* element,int dim,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>(dim*numnodes);
	element->NodalFunctionsDerivatives(dbasis,xyz_list,gauss);

	/*Build B': */
	for(int i=0;i<numnodes;i++){
		for(int j=0;j<dim;j++){
			Bprime[numnodes*j+i] = dbasis[j*numnodes+i];
		}
	}

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

}/*}}}*/
void DamageEvolutionAnalysis::GetSolutionFromInputs(Vector<IssmDouble>* solution,Element* element){/*{{{*/
	   _error_("not implemented yet");
}/*}}}*/
void DamageEvolutionAnalysis::InputUpdateFromSolution(IssmDouble* solution,Element* element){/*{{{*/

	int meshtype;
	IssmDouble  max_damage;
	int			*doflist = NULL;
	Element*   basalelement=NULL;

	element->FindParam(&meshtype,MeshTypeEnum);
	if(meshtype!=Mesh2DhorizontalEnum){
		if(!element->IsOnBed()) return;
		basalelement=element->SpawnBasalElement();
	}
	else{
		basalelement = element;
	}
	/*Fetch number of nodes and dof for this finite element*/
	int numnodes = basalelement->GetNumberOfNodes();

	/*Fetch dof list and allocate solution vector*/
	basalelement->GetDofList(&doflist,NoneApproximationEnum,GsetEnum);
	IssmDouble* newdamage = xNew<IssmDouble>(numnodes);

	/*Get user-supplied max_damage: */
	basalelement->FindParam(&max_damage,DamageMaxDamageEnum);

	/*Use the dof list to index into the solution vector: */
	for(int i=0;i<numnodes;i++){
		newdamage[i]=solution[doflist[i]];
		/*Check solution*/
		if(xIsNan<IssmDouble>(newdamage[i])) _error_("NaN found in solution vector");
		/*Enforce D < max_damage and D > 0 */
		if(newdamage[i]>max_damage) newdamage[i]=max_damage;
		else if(newdamage[i]<0.)    newdamage[i]=0.;
	}

	/*Get all inputs and parameters*/
	basalelement->AddBasalInput(DamageDEnum,newdamage,P1Enum);

	/*Free ressources:*/
	xDelete<IssmDouble>(newdamage);
	xDelete<int>(doflist);
	if(meshtype!=Mesh2DhorizontalEnum){basalelement->DeleteMaterials(); delete basalelement;};
}/*}}}*/
void DamageEvolutionAnalysis::UpdateConstraints(FemModel* femmodel){/*{{{*/
	/*Default, do nothing*/
	return;
}/*}}}*/

/*Intermediaries*/
void DamageEvolutionAnalysis::CreateDamageFInput(Element* element){/*{{{*/

	/*Intermediaries */
	IssmDouble c1,c2,c3,healing,stress_threshold;
	IssmDouble s_xx,s_xy,s_yy,s1,s2,stmp;
	IssmDouble J2s,Chi,Psi,PosPsi,NegPsi;
	IssmDouble damage,sigma_xx,sigma_xy,sigma_yy;
	int equivstress;

	/*Fetch number of vertices and allocate output*/
	int numvertices = element->GetNumberOfVertices();
	IssmDouble* f   = xNew<IssmDouble>(numvertices);

	/*retrieve parameters:*/
	element->FindParam(&c1,DamageC1Enum);
	element->FindParam(&c2,DamageC2Enum);
	element->FindParam(&c3,DamageC3Enum);
	element->FindParam(&healing,DamageHealingEnum);
	element->FindParam(&stress_threshold,DamageStressThresholdEnum);

	/*Compute stress tensor: */
	element->ComputeStressTensor();

	/*retrieve what we need: */
	Input* sigma_xx_input  = element->GetInput(StressTensorxxEnum);     _assert_(sigma_xx_input);
	Input* sigma_xy_input  = element->GetInput(StressTensorxyEnum);     _assert_(sigma_xy_input);
	Input* sigma_yy_input  = element->GetInput(StressTensoryyEnum);     _assert_(sigma_yy_input);
	Input* damage_input    = element->GetInput(DamageDEnum); _assert_(damage_input);

	/*retrieve the desired type of equivalent stress*/
	element->FindParam(&equivstress,DamageEquivStressEnum);

	/*Calculate damage evolution source term: */
	Gauss* gauss=element->NewGauss();
	for (int iv=0;iv<numvertices;iv++){
		gauss->GaussVertex(iv);
		
		damage_input->GetInputValue(&damage,gauss);
		sigma_xx_input->GetInputValue(&sigma_xx,gauss);
		sigma_xy_input->GetInputValue(&sigma_xy,gauss);
		sigma_yy_input->GetInputValue(&sigma_yy,gauss);
	
		/*Calculate effective stress components*/
		s_xx=sigma_xx/(1.-damage);
		s_xy=sigma_xy/(1.-damage);
		s_yy=sigma_yy/(1.-damage);

		/*Calculate principal effective stresses*/
		s1=(s_xx+s_yy)/2.+sqrt(pow((s_xx-s_yy)/2.,2)+pow(s_xy,2));
		s2=(s_xx+s_yy)/2.-sqrt(pow((s_xx-s_yy)/2.,2)+pow(s_xy,2));
		if(fabs(s2)>fabs(s1)){stmp=s2; s2=s1; s1=stmp;}

		if(equivstress==0){ /* von Mises */
			Chi=sqrt(s1*s1-s1*s2+s2*s2);
		}
		else if(equivstress==1){ /* max principal stress */
			Chi=s1;
		}
		Psi=Chi-stress_threshold;
		PosPsi=max(Psi,0.);
		NegPsi=max(-Chi,0.); /* healing only for compressive stresses */

		f[iv]= c1*(pow(PosPsi,c2) - healing*pow(NegPsi,c2))*pow((1.-damage),-c3);
	}

	/*Add input*/
	element->AddInput(DamageFEnum,f,P1Enum);
	
	/*Clean up and return*/
	xDelete<IssmDouble>(f);
	delete gauss;
}/*}}}*/
