/*!\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_matpar_id): 
	hnode(pengrid_node_ids,1),
	hmatice(&pengrid_matice_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_hmatpar, Parameters* pengrid_parameters, Inputs* pengrid_inputs):
	hnode(pengrid_hnode),
	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;

}
/*}}}*/
Pengrid::Pengrid(int	pengrid_id, int pengrid_node_id,int pengrid_mparid, int pengrid_dof, int pengrid_active, double pengrid_penalty_offset,int pengrid_thermal_steadystate,int pengrid_stabilize_constraints){
	
	id=pengrid_id;
	node_id=pengrid_node_id;
	mparid=pengrid_mparid;
	dof=pengrid_dof;
	active=pengrid_active;
	penalty_offset =pengrid_penalty_offset;
	thermal_steadystate=pengrid_thermal_steadystate;
	stabilize_constraints=pengrid_stabilize_constraints;

	node_offset=UNDEF;
	node=NULL;
	matpar=NULL;
	matpar_offset=UNDEF;


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

	int i,j;
	int tria_node_ids[3];
	int tria_matice_id;
	int tria_matpar_id;
	double nodeinputs[3];

	/*id: */
	this->id=index+1;
	
	/*hooks: */
	//go recover node ids, needed to initialize the node hook.
	if (iomodel->analysis_type==Prognostic2AnalysisEnum || iomodel->analysis_type==Balancedthickness2AnalysisEnum){
		/*Discontinuous Galerkin*/
		tria_node_ids[0]=3*index+1;
		tria_node_ids[1]=3*index+2;
		tria_node_ids[2]=3*index+3;
	}
	else{
		/*Continuous Galerkin*/
		for(i=0;i<3;i++){ 
			tria_node_ids[i]=(int)*(iomodel->elements+3*index+i); //ids for vertices are in the elements array from Matlab
		}
	}
	tria_matice_id=index+1; //refers to the corresponding ice material object
	tria_matpar_id=iomodel->numberofelements+1; //refers to the constant material parameters object

	this->hnodes.Init(tria_node_ids,3);
	this->hmatice.Init(&tria_matice_id,1);
	this->hmatpar.Init(&tria_matpar_id,1);

	//intialize inputs, and add as many inputs per element as requested: 
	this->inputs=new Inputs();
	
	if (iomodel->thickness) {
		for(i=0;i<3;i++)nodeinputs[i]=iomodel->thickness[tria_node_ids[i]-1];
		this->inputs->AddInput(new PengridVertexInput(ThicknessEnum,nodeinputs));
	}
	if (iomodel->surface) {
		for(i=0;i<3;i++)nodeinputs[i]=iomodel->surface[tria_node_ids[i]-1];
		this->inputs->AddInput(new PengridVertexInput(SurfaceEnum,nodeinputs));
	}
	if (iomodel->bed) {
		for(i=0;i<3;i++)nodeinputs[i]=iomodel->bed[tria_node_ids[i]-1];
		this->inputs->AddInput(new PengridVertexInput(BedEnum,nodeinputs));
	}
	if (iomodel->drag_coefficient) {
		for(i=0;i<3;i++)nodeinputs[i]=iomodel->drag_coefficient[tria_node_ids[i]-1];
		this->inputs->AddInput(new PengridVertexInput(DragCoefficientEnum,nodeinputs));

		if (iomodel->drag_p) this->inputs->AddInput(new DoubleInput(DragPEnum,iomodel->drag_p[index]));
		if (iomodel->drag_q) this->inputs->AddInput(new DoubleInput(DragQEnum,iomodel->drag_q[index]));
		this->inputs->AddInput(new IntInput(DragTypeEnum,iomodel->drag_type));

	}
	if (iomodel->melting_rate) {
		for(i=0;i<3;i++)nodeinputs[i]=iomodel->melting_rate[tria_node_ids[i]-1];
		this->inputs->AddInput(new PengridVertexInput(MeltingRateEnum,nodeinputs));
	}
	if (iomodel->accumulation_rate) {
		for(i=0;i<3;i++)nodeinputs[i]=iomodel->accumulation_rate[tria_node_ids[i]-1];
		this->inputs->AddInput(new PengridVertexInput(AccumulationRateEnum,nodeinputs));
	}
	if (iomodel->geothermalflux) {
		for(i=0;i<3;i++)nodeinputs[i]=iomodel->geothermalflux[tria_node_ids[i]-1];
		this->inputs->AddInput(new PengridVertexInput(GeothermalFluxEnum,nodeinputs));
	}	

	if (iomodel->elementoniceshelf) this->inputs->AddInput(new BoolInput(ElementOnIceShelfEnum,(IssmBool)iomodel->elementoniceshelf[index]));
	if (iomodel->elementonbed) this->inputs->AddInput(new BoolInput(ElementOnBedEnum,(IssmBool)iomodel->elementonbed[index]));
	if (iomodel->elementonwater) this->inputs->AddInput(new BoolInput(ElementOnWaterEnum,(IssmBool)iomodel->elementonwater[index]));
	if (iomodel->elementonsurface) this->inputs->AddInput(new BoolInput(ElementOnSurfaceEnum,(IssmBool)iomodel->elementonsurface[index]));

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


}
/*}}}*/
/*FUNCTION Pengrid::destructor {{{1*/
Pengrid::~Pengrid(){
	return;
}
/*}}}1*/
		
/*Object marshall*/
/*FUNCTION Pengrid::Marshall {{{1*/
void  Pengrid::Marshall(char** pmarshalled_dataset){

	char* marshalled_dataset=NULL;
	int   enum_type=0;

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

	/*get enum type of Pengrid: */
	enum_type=PengridEnum;
	
	/*marshall enum: */
	memcpy(marshalled_dataset,&enum_type,sizeof(enum_type));marshalled_dataset+=sizeof(enum_type);
	
	/*marshall Pengrid data: */
	memcpy(marshalled_dataset,&id,sizeof(id));marshalled_dataset+=sizeof(id);
	memcpy(marshalled_dataset,&mparid,sizeof(mparid));marshalled_dataset+=sizeof(mparid);
	memcpy(marshalled_dataset,&dof,sizeof(dof));marshalled_dataset+=sizeof(dof);
	memcpy(marshalled_dataset,&active,sizeof(active));marshalled_dataset+=sizeof(active);
	memcpy(marshalled_dataset,&penalty_offset,sizeof(penalty_offset));marshalled_dataset+=sizeof(penalty_offset);
	memcpy(marshalled_dataset,&thermal_steadystate,sizeof(thermal_steadystate));marshalled_dataset+=sizeof(thermal_steadystate);
	memcpy(marshalled_dataset,&node_id,sizeof(node_id));marshalled_dataset+=sizeof(node_id);
	memcpy(marshalled_dataset,&node_offset,sizeof(node_offset));marshalled_dataset+=sizeof(node_offset);
	memcpy(marshalled_dataset,&matpar,sizeof(matpar));marshalled_dataset+=sizeof(matpar);
	memcpy(marshalled_dataset,&matpar_offset,sizeof(matpar_offset));marshalled_dataset+=sizeof(matpar_offset);
	memcpy(marshalled_dataset,&stabilize_constraints,sizeof(stabilize_constraints));marshalled_dataset+=sizeof(stabilize_constraints);
	memcpy(marshalled_dataset,&zigzag_counter,sizeof(zigzag_counter));marshalled_dataset+=sizeof(zigzag_counter);

	*pmarshalled_dataset=marshalled_dataset;
	return;
}
/*}}}1*/
/*FUNCTION Pengrid::MarshallSize {{{1*/
int   Pengrid::MarshallSize(){

	return sizeof(id)+
		sizeof(mparid)+
		sizeof(dof)+
		sizeof(active)+
		sizeof(penalty_offset)+
		sizeof(thermal_steadystate)+
		sizeof(node_id)+
		sizeof(node_offset)+
		sizeof(matpar)+
		sizeof(matpar_offset)+
		sizeof(stabilize_constraints)+
		sizeof(zigzag_counter)+
		sizeof(int); //sizeof(int) for enum type
}
/*}}}1*/
/*FUNCTION Pengrid::Demarshall {{{1*/
void  Pengrid::Demarshall(char** pmarshalled_dataset){

	char* marshalled_dataset=NULL;

	/*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(&mparid,marshalled_dataset,sizeof(mparid));marshalled_dataset+=sizeof(mparid);
	memcpy(&dof,marshalled_dataset,sizeof(dof));marshalled_dataset+=sizeof(dof);
	memcpy(&active,marshalled_dataset,sizeof(active));marshalled_dataset+=sizeof(active);
	memcpy(&penalty_offset,marshalled_dataset,sizeof(penalty_offset));marshalled_dataset+=sizeof(penalty_offset);
	memcpy(&thermal_steadystate,marshalled_dataset,sizeof(thermal_steadystate));marshalled_dataset+=sizeof(thermal_steadystate);
	memcpy(&node_id,marshalled_dataset,sizeof(node_id));marshalled_dataset+=sizeof(node_id);
	memcpy(&node_offset,marshalled_dataset,sizeof(node_offset));marshalled_dataset+=sizeof(node_offset);
	memcpy(&matpar,marshalled_dataset,sizeof(matpar));marshalled_dataset+=sizeof(matpar);
	memcpy(&matpar_offset,marshalled_dataset,sizeof(matpar_offset));marshalled_dataset+=sizeof(matpar_offset);
	memcpy(&stabilize_constraints,marshalled_dataset,sizeof(stabilize_constraints));marshalled_dataset+=sizeof(stabilize_constraints);
	memcpy(&zigzag_counter,marshalled_dataset,sizeof(zigzag_counter));marshalled_dataset+=sizeof(zigzag_counter);

	node=NULL;
	matpar=NULL;

	/*return: */
	*pmarshalled_dataset=marshalled_dataset;
	return;
}
/*}}}1*/

/*Object functions*/
/*FUNCTION Pengrid::copy {{{1*/
Object* Pengrid::copy() {
	return new Pengrid(*this); 
}
/*}}}1*/
/*FUNCTION Pengrid::Configure {{{1*/

void  Pengrid::Configure(void* pelementsin,void* pnodesin,void* pmaterialsin){

	DataSet* nodesin=NULL;
	DataSet* materialsin=NULL;

	/*Recover pointers :*/
	nodesin=(DataSet*)pnodesin;
	materialsin=(DataSet*)pmaterialsin;

	/*Link this load with its nodes: */
	ResolvePointers((Object**)&node,&node_id,&node_offset,1,nodesin);
	ResolvePointers((Object**)&matpar,&mparid,&matpar_offset,1,materialsin);
}
/*}}}1*/
/*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::DeepEcho {{{1*/
void Pengrid::DeepEcho(void){

	printf("Pengrid:\n");
	printf("   id: %i\n",id);
	printf("   mparid: %i\n",mparid);
	printf("   dof: %i\n",dof);
	printf("   active: %i\n",active);
	printf("   penalty_offset: %g\n",penalty_offset);
	printf("   thermal_steadystate: %i\n",thermal_steadystate);
	printf("   node_id: [%i]\n",node_id);
	printf("   node_offset: [%i]\n",node_offset);
	printf("   matpar_offset=%i\n",matpar_offset);
	
	if(node)node->Echo();
	if(matpar)matpar->Echo();
	return;
}		
/*}}}1*/
/*FUNCTION Pengrid::DistributenumDofs {{{1*/
void  Pengrid::DistributeNumDofs(int* numdofpernode,int analysis_type,int sub_analysis_type){return;}
/*}}}1*/
/*FUNCTION Pengrid::Echo {{{1*/
void Pengrid::Echo(void){

	printf("Pengrid:\n");
	printf("   id: %i\n",id);
	printf("   mparid: %i\n",mparid);
	printf("   dof: %i\n",dof);
	printf("   active: %i\n",active);
	printf("   penalty_offset: %g\n",penalty_offset);
	printf("   thermal_steadystate: %i\n",thermal_steadystate);
	printf("   node_id: [%i]\n",node_id);
	printf("   node_offset: [%i]\n",node_offset);
	printf("   matpar_offset=%i\n",matpar_offset);
	
	return;
}
/*}}}1*/
/*FUNCTION Pengrid::Enum {{{1*/
int Pengrid::Enum(void){

	return PengridEnum;
}
/*}}}1*/
/*FUNCTION Pengrid::GetDofList {{{1*/
void  Pengrid::GetDofList(int* doflist,int* pnumberofdofspernode){

	int j;
	int doflist_per_node[MAXDOFSPERNODE];
	int numberofdofspernode;
	
	node->GetDofList(&doflist_per_node[0],&numberofdofspernode);
	for(j=0;j<numberofdofspernode;j++){
		doflist[j]=doflist_per_node[j];
	}

	/*Assign output pointers:*/
	*pnumberofdofspernode=numberofdofspernode;
}
/*}}}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::MyRank {{{1*/
int    Pengrid::MyRank(void){ 
	extern int my_rank;
	return my_rank; 
}
/*}}}1*/
/*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  dofs1[1]={0};
	int  unstable=0;
	int  reset_penalties=0;
	
	ParameterInputs* inputs=NULL;

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

	/*recover pointers: */
	inputs=(ParameterInputs*)vinputs;

	//First recover beta, pressure and temperature vectors;
	found=inputs->Recover("pressure",&pressure,1,dofs1,numgrids,(void**)&node);
	if(!found)ISSMERROR(" could not find pressure in inputs!");

	found=inputs->Recover("temperature",&temperature,1,dofs1,numgrids,(void**)&node);
	if(!found)ISSMERROR(" could not find temperature in inputs!");
	
	found=inputs->Recover("reset_penalties",&reset_penalties);

	if(reset_penalties)zigzag_counter=0;

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

	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};
	
	ParameterInputs* inputs=NULL;

	/*recover pointers: */
	inputs=(ParameterInputs*)vinputs;

	/*Get dof list: */
	GetDofList(&doflist[0],&numberofdofspernode);
	
	/*recover slope: */
	found=inputs->Recover("bedslopex",&slope[0],1,dofs1,numgrids,(void**)&node);
	if(!found)ISSMERROR(" bedslopex needed in inputs!");
	found=inputs->Recover("bedslopey",&slope[1],1,dofs2,numgrids,(void**)&node);
	if(!found)ISSMERROR(" bedslopey needed in inputs!");

	//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;

	ParameterInputs* inputs=NULL;

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

	/*recover pointers: */
	inputs=(ParameterInputs*)vinputs;

	found=inputs->Recover("pressure",&pressure,1,dofs1,numgrids,(void**)&node);
	if(!found)ISSMERROR(" could not find pressure in inputs!");

	found=inputs->Recover("temperature",&temperature,1,dofs1,numgrids,(void**)&node);
	if(!found)ISSMERROR(" could not find temperature in inputs!");

	/*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;

	ParameterInputs* inputs=NULL;

	/*recover pointers: */
	inputs=(ParameterInputs*)vinputs;


	if(!active)return;

	/*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;

	ParameterInputs* inputs=NULL;

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

	/*recover pointers: */
	inputs=(ParameterInputs*)vinputs;

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

	//First recover pressure,melting offset and temperature vectors
	found=inputs->Recover("pressure",&pressure,1,dofs1,numgrids,(void**)&node);
	if(!found)ISSMERROR(" could not find pressure in inputs!");

	found=inputs->Recover("temperature",&temperature,1,dofs1,numgrids,(void**)&node);
	if(!found)ISSMERROR(" could not find temperature in inputs!");

	found=inputs->Recover("melting_offset",&melting_offset);
	if(!found)ISSMERROR(" could not find melting_offset in inputs!");

	found=inputs->Recover("dt",&dt);
	if(!found)ISSMERROR(" could not find dt in inputs!");

	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;

	ParameterInputs* inputs=NULL;

	/*recover pointers: */
	inputs=(ParameterInputs*)vinputs;

	if(!active)return;

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

	//First recover pressure
	found=inputs->Recover("pressure",&pressure,1,dofs1,numgrids,(void**)&node);
	if(!found)ISSMERROR(" could not find pressure in inputs!");

	//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*/
/*FUNCTION Pengrid::UpdateFromInputs {{{1*/
void  Pengrid::UpdateFromInputs(void* inputs){
	
}
/*}}}1*/