/*!\file Pengrid.c
 * \brief: implementation of the Pengrid object
 */


#ifdef HAVE_CONFIG_H
	#include "config.h"
#else
#error "Cannot compile with HAVE_CONFIG_H symbol! run configure first!"
#endif

#include "stdio.h"
#include "./Pengrid.h"
#include <string.h>
#include "../EnumDefinitions/EnumDefinitions.h"
#include "../include/macros.h"
#include "../shared/shared.h"
#include "../include/typedefs.h"
#include "../include/macros.h"
#include "../DataSet/DataSet.h"
#include "../DataSet/Inputs.h"
	
/*Object constructors and destructor*/
/*FUNCTION Pengrid::constructor {{{1*/
Pengrid::Pengrid(){
	this->inputs=NULL;
	this->parameters=NULL;
	
	/*not active, not zigzagging: */
	active=0;
	zigzag_counter=0;

}
/*}}}1*/
/*FUNCTION Pengrid::Pengrid(int id, int node_ids int matpar_id){{{1*/
Pengrid::Pengrid(int pengrid_id,int pengrid_node_id, int pengrid_element_id,int pengrid_matpar_id): 
	hnode(&pengrid_node_id,1),
	helement(&pengrid_element_id,1),
	hmatpar(&pengrid_matpar_id,1)
{

	/*all the initialization has been done by the initializer, just fill in the id: */
	this->id=pengrid_id;
	this->parameters=NULL;
	this->inputs=new Inputs();

	/*not active, not zigzagging: */
	active=0;
	zigzag_counter=0;

}
/*}}}*/
/*FUNCTION Pengrid::Pengrid(int id, Hook* hnodes, Hook* hmatice, Hook* hmatpar, DataSet* parameters, Inputs* pengrid_inputs) {{{1*/
Pengrid::Pengrid(int pengrid_id,Hook* pengrid_hnode, Hook* pengrid_helement,Hook* pengrid_hmatpar, Parameters* pengrid_parameters, Inputs* pengrid_inputs):
	hnode(pengrid_hnode),
	helement(pengrid_helement),
	hmatpar(pengrid_hmatpar)
{

	/*all the initialization has been done by the initializer, just fill in the id: */
	this->id=pengrid_id;
	if(pengrid_inputs){
		this->inputs=(Inputs*)pengrid_inputs->Copy();
	}
	else{
		this->inputs=new Inputs();
	}
	/*point parameters: */
	this->parameters=pengrid_parameters;
	
	/*not active, not zigzagging: */
	active=0;
	zigzag_counter=0;

}
/*}}}*/
/*FUNCTION Pengrid::Pengrid(int index, int id, IoModel* iomodel){{{1*/
Pengrid::Pengrid(int id, int index, IoModel* iomodel){ //i is the element index

	int i,j;
	int pengrid_node_id;
	int pengrid_matpar_id;
	int pengrid_element_id;

	/*id: */
	this->id=id;
	
	/*hooks: */
	pengrid_node_id=index+1;
	pengrid_element_id=iomodel->singlenodetoelementconnectivity[index];
	pengrid_matpar_id=iomodel->numberofelements+1; //refers to the constant material parameters object

	this->hnode.Init(&pengrid_node_id,1);
	this->helement.Init(&pengrid_element_id,1);
	this->hmatpar.Init(&pengrid_matpar_id,1);

	//initialize inputs: none needed
	this->inputs=new Inputs();

	//this->parameters: we still can't point to it, it may not even exist. Configure will handle this.
	this->parameters=NULL;

	//let's not forget internals
	this->active=0;
	this->zigzag_counter=0;

}
/*}}}*/
/*FUNCTION Pengrid::destructor {{{1*/
Pengrid::~Pengrid(){
	return;
}
/*}}}1*/
		
/*Object management*/
/*FUNCTION Pengrid::Configure {{{1*/
void  Pengrid::Configure(DataSet* elementsin,DataSet* loadsin,DataSet* nodesin,DataSet* verticesin,DataSet* materialsin,Parameters* parametersin){

	/*Take care of hooking up all objects for this load, ie links the objects in the hooks to their respective 
	 * datasets, using internal ids and offsets hidden in hooks: */
	hnode.configure(nodesin);
	helement.configure(elementsin);
	hmatpar.configure(materialsin);

	/*point parameters to real dataset: */
	this->parameters=parametersin;
}
/*}}}1*/
/*FUNCTION Pengrid::copy {{{1*/
Object* Pengrid::copy() {
	return new Pengrid(this->id,&this->hnode,&this->helement,&this->hmatpar,this->parameters,this->inputs);
}
/*}}}1*/
/*FUNCTION Pengrid::DeepEcho{{{1*/
void Pengrid::DeepEcho(void){

	printf("Pengrid:\n");
	printf("   id: %i\n",id);
	hnode.DeepEcho();
	helement.DeepEcho();
	hmatpar.DeepEcho();
	printf("   active %i\n",this->active);
	printf("   zigzag_counter %i\n",this->zigzag_counter);
	printf("   parameters\n");
	parameters->DeepEcho();
	printf("   inputs\n");
	inputs->DeepEcho();
}
/*}}}*/
/*FUNCTION Pengrid::Demarshall {{{1*/
void  Pengrid::Demarshall(char** pmarshalled_dataset){

	char* marshalled_dataset=NULL;
	int   i;

	/*recover marshalled_dataset: */
	marshalled_dataset=*pmarshalled_dataset;

	/*this time, no need to get enum type, the pointer directly points to the beginning of the 
	 *object data (thanks to DataSet::Demarshall):*/

	memcpy(&id,marshalled_dataset,sizeof(id));marshalled_dataset+=sizeof(id);
	memcpy(&active,marshalled_dataset,sizeof(active));marshalled_dataset+=sizeof(active);
	memcpy(&zigzag_counter,marshalled_dataset,sizeof(zigzag_counter));marshalled_dataset+=sizeof(zigzag_counter);

	/*demarshall hooks: */
	hnode.Demarshall(&marshalled_dataset);
	helement.Demarshall(&marshalled_dataset);
	hmatpar.Demarshall(&marshalled_dataset);
	
	/*demarshall inputs: */
	inputs=(Inputs*)DataSetDemarshallRaw(&marshalled_dataset); 

	/*parameters: may not exist even yet, so let Configure handle it: */
	this->parameters=NULL;

	/*return: */
	*pmarshalled_dataset=marshalled_dataset;
	return;
}
/*}}}*/
/*FUNCTION Pengrid::Echo {{{1*/
void Pengrid::Echo(void){
	this->DeepEcho();
}
/*}}}1*/
/*FUNCTION Pengrid::Enum {{{1*/
int Pengrid::Enum(void){

	return PengridEnum;
}
/*}}}1*/
/*FUNCTION Pengrid::GetId {{{1*/
int    Pengrid::GetId(void){ return id; }
/*}}}1*/
/*FUNCTION Pengrid::GetName {{{1*/
char* Pengrid::GetName(void){
	return "pengrid";
}
/*}}}1*/
/*FUNCTION Pengrid::Marshall {{{1*/
void  Pengrid::Marshall(char** pmarshalled_dataset){

	char* marshalled_dataset=NULL;
	int   enum_type=0;
	char* marshalled_inputs=NULL;
	int   marshalled_inputs_size;

	/*recover marshalled_dataset: */
	marshalled_dataset=*pmarshalled_dataset;

	/*get enum type of Tria: */
	enum_type=PengridEnum;

	/*marshall enum: */
	memcpy(marshalled_dataset,&enum_type,sizeof(enum_type));marshalled_dataset+=sizeof(enum_type);

	/*marshall Tria data: */
	memcpy(marshalled_dataset,&id,sizeof(id));marshalled_dataset+=sizeof(id);
	memcpy(marshalled_dataset,&active,sizeof(active));marshalled_dataset+=sizeof(active);
	memcpy(marshalled_dataset,&zigzag_counter,sizeof(zigzag_counter));marshalled_dataset+=sizeof(zigzag_counter);

	/*Marshall hooks: */
	hnode.Marshall(&marshalled_dataset);
	helement.Marshall(&marshalled_dataset);
	hmatpar.Marshall(&marshalled_dataset);

	/*Marshall inputs: */
	marshalled_inputs_size=inputs->MarshallSize();
	marshalled_inputs=inputs->Marshall();
	memcpy(marshalled_dataset,marshalled_inputs,marshalled_inputs_size*sizeof(char));
	marshalled_dataset+=marshalled_inputs_size;

	/*parameters: don't do anything about it. parameters are marshalled somewhere else!*/

	xfree((void**)&marshalled_inputs);

	*pmarshalled_dataset=marshalled_dataset;
	return;
}
/*}}}*/
/*FUNCTION Pengrid::MarshallSize {{{1*/
int   Pengrid::MarshallSize(){
	
	return sizeof(id)
		+sizeof(active)
		+sizeof(zigzag_counter)
		+hnode.MarshallSize()
		+helement.MarshallSize()
		+hmatpar.MarshallSize()
		+inputs->MarshallSize()
		+sizeof(int); //sizeof(int) for enum type
}
/*}}}*/
/*FUNCTION Pengrid::MyRank {{{1*/
int    Pengrid::MyRank(void){ 
	extern int my_rank;
	return my_rank; 
}
/*}}}1*/

/*Object functions*/
/*FUNCTION Pengrid::CreateKMatrix {{{1*/

void  Pengrid::CreateKMatrix(Mat Kgg,int analysis_type,int sub_analysis_type){

	/*No loads applied, do nothing: */
	return;

}
/*}}}1*/
/*FUNCTION Pengrid::CreatePVector {{{1*/
void  Pengrid::CreatePVector(Vec pg,  int analysis_type,int sub_analysis_type){

	/*No loads applied, do nothing: */
	return;

}
/*}}}1*/
/*FUNCTION Pengrid::DistributenumDofs {{{1*/
void  Pengrid::DistributeNumDofs(int* numdofpernode,int analysis_type,int sub_analysis_type){return;}
/*}}}1*/
/*FUNCTION Pengrid::GetDofList {{{1*/
void  Pengrid::GetDofList(int* doflist,int* pnumberofdofspernode){

	int i,j;
	int doflist_per_node[MAXDOFSPERNODE];
	int numberofdofspernode;

	/*dynamic objects pointed to by hooks: */
	Node* node=NULL;

	/*recover objects from hooks: */
	node=(Node*)hnode.delivers();
	
	node->GetDofList(&doflist_per_node[0],&numberofdofspernode);
	for(j=0;j<numberofdofspernode;j++){
		doflist[j]=doflist_per_node[j];
	}

	/*Assign output pointers:*/
	*pnumberofdofspernode=numberofdofspernode;

}
/*}}}*/
/*FUNCTION Pengrid::PenaltyConstrain {{{1*/
void  Pengrid::PenaltyConstrain(int* punstable,int analysis_type,int sub_analysis_type){

	if ((analysis_type==DiagnosticAnalysisEnum) && ((sub_analysis_type==StokesAnalysisEnum))){

		/*No penalty to check*/
		return;

	}
	else if (analysis_type==ThermalAnalysisEnum){
		
		PenaltyConstrainThermal(punstable,analysis_type,sub_analysis_type);
		
	}
	else if (analysis_type==MeltingAnalysisEnum){
			
		/*No penalty to check*/
		return;

	}
	else{
		ISSMERROR("%s%i%s%i%s","analysis: ",analysis_type," and sub_analysis_type: ",sub_analysis_type," not supported yet");
	}

}
/*}}}1*/
/*FUNCTION Pengrid::PenaltyConstrainThermal {{{1*/
void  Pengrid::PenaltyConstrainThermal(int* punstable,int analysis_type,int sub_analysis_type){

	//   The penalty is stable if it doesn't change during to successive iterations.   

	int    found=0;
	const int numgrids=1;
	double pressure;
	double temperature;
	double beta,t_pmp;
	double meltingpoint;
	int    new_active;
	int    unstable=0;
	int    reset_penalties=0;
	int    stabilize_constraints;

	/*pointers: */
	Node* node=NULL;
	Penta* penta=NULL;
	Matpar*  matpar=NULL;
	
	/*check that pengrid is not a clone (penalty to be added only once)*/
	if (node->IsClone()){
		unstable=0;
		*punstable=unstable;
		return;
	}

	/*recover pointers: */
	node=(Node*)hnode.delivers();
	penta=(Penta*)helement.delivers();
	matpar=(Matpar*)hmatpar.delivers();

	//First recover pressure and temperature values, using the element: */
	penta->inputs->GetParameterValueAtNode(&pressure,node,PressureEnum);
	penta->inputs->GetParameterValueAtNode(&temperature,node,TemperatureEnum);

	//Recover our data:
	inputs->GetParameterValue(&reset_penalties,ResetPenaltiesEnum);
	meltingpoint=matpar->GetMeltingPoint();
	beta=matpar->GetBeta();
	parameters->FindParam(&stabilize_constraints,"stabilize_constraints");
	
	if(reset_penalties)zigzag_counter=0;

	//Compute pressure melting point
	t_pmp=meltingpoint-beta*pressure;

	//Figure out if temperature is over melting_point, in which case, this penalty needs to be activated.

	if (temperature>t_pmp){
		new_active=1;
	}
	else{
		new_active=0;
	}


	//Figure out stability of this penalty
	if (active==new_active){
		unstable=0;
	}
	else{
		unstable=1;
		if(stabilize_constraints)zigzag_counter++;
	}

	/*If penalty keeps zigzagging more than 5 times: */
	if(stabilize_constraints){
		if(zigzag_counter>stabilize_constraints){
			unstable=0;
			active=1;
		}
	}

	//Set penalty flag
	active=new_active;

	//*Assign output pointers:*/
	*punstable=unstable;
}
/*}}}1*/
/*FUNCTION Pengrid::PenaltyCreateMatrix {{{1*/
void  Pengrid::PenaltyCreateKMatrix(Mat Kgg,double kmax,int analysis_type,int sub_analysis_type){

	if ((analysis_type==DiagnosticAnalysisEnum) && ((sub_analysis_type==StokesAnalysisEnum))){

		PenaltyCreateKMatrixDiagnosticStokes( Kgg,kmax,analysis_type,sub_analysis_type);
	}
	else if (analysis_type==ThermalAnalysisEnum){
		
		PenaltyCreateKMatrixThermal( Kgg,kmax,analysis_type,sub_analysis_type);
		
	}
	else if (analysis_type==MeltingAnalysisEnum){
			
		PenaltyCreateKMatrixMelting( Kgg,kmax,analysis_type,sub_analysis_type);

	}
	else{
		ISSMERROR("%s%i%s%i%s","analysis: ",analysis_type," and sub_analysis_type: ",sub_analysis_type," not supported yet");
	}

}
/*}}}1*/
/*FUNCTION Pengrid::PenaltyCreateKMatrixDiagnosticStokes {{{1*/
void  Pengrid::PenaltyCreateKMatrixDiagnosticStokes(Mat Kgg,double kmax,int analysis_type,int sub_analysis_type){
	
	const int numgrids=1;
	const int NDOF4=4;
	const int numdof=numgrids*NDOF4;
	int       doflist[numdof];
	int       numberofdofspernode;

	int dofs1[1]={0};
	int dofs2[1]={1};
	double slope[2];
	int found=0;
	double Ke[4][4]={0.0};
	double penalty_offset;

	/*pointers: */
	Node* node=NULL;
	Penta* penta=NULL;

	/*Get dof list: */
	GetDofList(&doflist[0],&numberofdofspernode);

	/*recover pointers: */
	node=(Node*)hnode.delivers();
	penta=(Penta*)helement.delivers();

	//recover slope: */
	penta->inputs->GetParameterValueAtNode(&slope[0],node,BedSlopexEnum);
	penta->inputs->GetParameterValueAtNode(&slope[1],node,BedSlopeyEnum);
	
	/*recover parameters: */
	parameters->FindParam(&penalty_offset,"penalty_offset");

	//Create elementary matrix: add penalty to contrain wb (wb=ub*db/dx+vb*db/dy)
	Ke[2][0]=-slope[0]*kmax*pow((double)10.0,penalty_offset);
	Ke[2][1]=-slope[1]*kmax*pow((double)10.0,penalty_offset);
	Ke[2][2]=kmax*pow((double)10,penalty_offset);
	
	/*Add Ke to global matrix Kgg: */
	MatSetValues(Kgg,numdof,doflist,numdof,doflist,(const double*)Ke,ADD_VALUES);
}
/*}}}1*/
/*FUNCTION Pengrid::PenaltyCreateKMatrixMelting {{{1*/
void  Pengrid::PenaltyCreateKMatrixMelting(Mat Kgg,double kmax,int analysis_type,int sub_analysis_type){


	int found=0;
	const int numgrids=1;
	const int NDOF1=1;
	const int numdof=numgrids*NDOF1;
	double Ke[numdof][numdof]={0.0};
	int     dofs1[1]={0};
	int       doflist[numdof];
	int      numberofdofspernode;
	double  meltingpoint;

	double pressure;
	double temperature;
	double beta,t_pmp;
	double penalty_offset;

	/*pointers: */
	Node* node=NULL;
	Penta* penta=NULL;
	Matpar*  matpar=NULL;

	/*recover pointers: */
	node=(Node*)hnode.delivers();
	penta=(Penta*)helement.delivers();
	matpar=(Matpar*)hmatpar.delivers();

	/*check that pengrid is not a clone (penalty to be added only once)*/
	if (node->IsClone()) return;

	//First recover pressure and temperature values, using the element: */
	penta->inputs->GetParameterValueAtNode(&pressure,node,PressureEnum);
	penta->inputs->GetParameterValueAtNode(&temperature,node,TemperatureEnum);

	/*recover parameters: */
	parameters->FindParam(&penalty_offset,"penalty_offset");

	/*Get dof list: */
	GetDofList(&doflist[0],&numberofdofspernode);
	
	//Compute pressure melting point
	meltingpoint=matpar->GetMeltingPoint();
	beta=matpar->GetBeta();
	t_pmp=meltingpoint-beta*pressure;

	//Add penalty load
	if (temperature<t_pmp){ //If T<Tpmp, there must be no melting. Therefore, melting should be  constrained to 0 when T<Tpmp, instead of using spcs, use penalties
		Ke[0][0]=kmax*pow((double)10,penalty_offset);
	}
	
	MatSetValues(Kgg,numdof,doflist,numdof,doflist,(const double*)Ke,ADD_VALUES);
}
/*}}}1*/
/*FUNCTION Pengrid::PenaltyCreateKMatrixThermal {{{1*/
void  Pengrid::PenaltyCreateKMatrixThermal(Mat Kgg,double kmax,int analysis_type,int sub_analysis_type){

	int found=0;
	
	const int numgrids=1;
	const int NDOF1=1;
	const int numdof=numgrids*NDOF1;
	double Ke[numdof][numdof];
	int       doflist[numdof];
	int       numberofdofspernode;
	double    penalty_offset;

	if(!this->active)return;

	/*recover parameters: */
	parameters->FindParam(&penalty_offset,"penalty_offset");

	/*Get dof list: */
	GetDofList(&doflist[0],&numberofdofspernode);

	Ke[0][0]=kmax*pow((double)10,penalty_offset);
	
	/*Add Ke to global matrix Kgg: */
	MatSetValues(Kgg,numdof,doflist,numdof,doflist,(const double*)Ke,ADD_VALUES);
}
/*}}}1*/
/*FUNCTION Pengrid::PenaltyCreatePVector {{{1*/
void  Pengrid::PenaltyCreatePVector(Vec pg,double kmax,int analysis_type,int sub_analysis_type){

	if (analysis_type==ThermalAnalysisEnum){
		
		PenaltyCreatePVectorThermal( pg,kmax,analysis_type,sub_analysis_type);
		
	}
	else if (analysis_type==MeltingAnalysisEnum){
			
		PenaltyCreatePVectorMelting( pg,kmax,analysis_type,sub_analysis_type);

	}
	else if (analysis_type==DiagnosticAnalysisEnum){

		/*No loads applied, do nothing: */
		return;

	}
	else{
		ISSMERROR("%s%i%s%i%s","analysis: ",analysis_type," and sub_analysis_type: ",sub_analysis_type," not supported yet");
	}

}
/*}}}1*/
/*FUNCTION Pengrid::PenaltyCreatePVectorMelting {{{1*/
void  Pengrid::PenaltyCreatePVectorMelting(Vec pg, double kmax,int analysis_type,int sub_analysis_type){
	
	const int numgrids=1;
	const int NDOF1=1;
	const int numdof=numgrids*NDOF1;
	int    doflist[numdof];
	double P_terms[numdof]={0.0};
	int    numberofdofspernode;
	int    found=0;
	int    dofs1[1]={0};
	double pressure;
	double temperature;
	double melting_offset;
	double meltingpoint;
	double beta, heatcapacity;
	double latentheat;
	double t_pmp;
	double dt,penalty_offset;

	/*pointers: */
	Node* node=NULL;
	Penta* penta=NULL;
	Matpar*  matpar=NULL;

	/*recover pointers: */
	node=(Node*)hnode.delivers();
	penta=(Penta*)helement.delivers();
	matpar=(Matpar*)hmatpar.delivers();

	/*check that pengrid is not a clone (penalty to be added only once)*/
	if (node->IsClone()) return;

	/*Get dof list: */
	GetDofList(&doflist[0],&numberofdofspernode);

	//First recover pressure and temperature values, using the element: */
	penta->inputs->GetParameterValueAtNode(&pressure,node,PressureEnum);
	penta->inputs->GetParameterValueAtNode(&temperature,node,TemperatureEnum);
	inputs->GetParameterValue(&melting_offset,MeltingOffsetEnum);
	parameters->FindParam(&dt,"dt");
	parameters->FindParam(&penalty_offset,"penalty_offset");

	meltingpoint=matpar->GetMeltingPoint();
	beta=matpar->GetBeta();
	heatcapacity=matpar->GetHeatCapacity();
	latentheat=matpar->GetLatentHeat();

	//Compute pressure melting point
	t_pmp=meltingpoint-beta*pressure;

	//Add penalty load
	//This time, the penalty must have the same value as the one used for the thermal computation
	//so that the corresponding melting can be computed correctly
	//In the thermal computation, we used kmax=melting_offset, and the same penalty_offset
	if (temperature<t_pmp){ //%no melting
		P_terms[0]=0;
	}
	else{
		if (dt){
			P_terms[0]=melting_offset*pow((double)10,penalty_offset)*(temperature-t_pmp)/dt;
		}
		else{
			P_terms[0]=melting_offset*pow((double)10,penalty_offset)*(temperature-t_pmp);
		}
	}

	/*Add P_terms to global vector pg: */
	VecSetValues(pg,numdof,doflist,(const double*)P_terms,ADD_VALUES);
}
/*}}}1*/
/*FUNCTION Pengrid::PenaltyCreatePVectorThermal {{{1*/
void  Pengrid::PenaltyCreatePVectorThermal(Vec pg,  double kmax,int analysis_type,int sub_analysis_type){

	const int numgrids=1;
	const int NDOF1=1;
	const int numdof=numgrids*NDOF1;
	int       doflist[numdof];
	double  P_terms[numdof]={0.0};
	int    numberofdofspernode;
	int    found=0;
	double pressure;
	int dofs1[1]={0};
	double meltingpoint;
	double beta;
	double t_pmp;
	double penalty_offset;

	/*pointers: */
	Node* node=NULL;
	Penta* penta=NULL;
	Matpar*  matpar=NULL;

	/*recover pointers: */
	node=(Node*)hnode.delivers();
	penta=(Penta*)helement.delivers();
	matpar=(Matpar*)hmatpar.delivers();

	if(!this->active)return;

	/*Get dof list: */
	GetDofList(&doflist[0],&numberofdofspernode);

	//First recover pressure  and penalty_offset
	penta->inputs->GetParameterValueAtNode(&pressure,node,PressureEnum);
	parameters->FindParam(&penalty_offset,"penalty_offset");

	//Compute pressure melting point
	meltingpoint=matpar->GetMeltingPoint();
	beta=matpar->GetBeta();
	t_pmp=meltingpoint-beta*pressure;

	//Add penalty load
	P_terms[0]=kmax*pow((double)10,penalty_offset)*t_pmp;

	/*Add P_terms to global vector pg: */
	VecSetValues(pg,numdof,doflist,(const double*)P_terms,ADD_VALUES);
}
/*}}}1*/

/*Updates: */
/*FUNCTION Pengrid::UpdateFromDakota {{{1*/
void  Pengrid::UpdateFromDakota(void* inputs){
	ISSMERROR("not supported yet!");
}
/*}}}1*/
/*FUNCTION Pengrid::UpdateInputs {{{1*/
void  Pengrid::UpdateInputs(double* solution, int analysis_type, int sub_analysis_type){
	ISSMERROR("not supported yet!");
}
/*}}}1*/