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

/*Headers*/
/*{{{*/
#ifdef HAVE_CONFIG_H
	#include <config.h>
#else
#error "Cannot compile with HAVE_CONFIG_H symbol! run configure first!"
#endif

#include <stdio.h>
#include <string.h>
#include "../objects.h"
#include "../../EnumDefinitions/EnumDefinitions.h"
#include "../../include/include.h"
#include "../../shared/shared.h"
#include "../../Container/Container.h"
/*}}}*/
	
/*Element macros*/
#define NUMVERTICES   1

/*Pengrid constructors and destructor*/
/*FUNCTION Pengrid::Pengrid(){{{*/
Pengrid::Pengrid(){
	this->inputs=NULL;
	this->parameters=NULL;
	this->hnode=NULL;
	this->node=NULL;
	this->helement=NULL;
	this->element=NULL;
	this->hmatpar=NULL;
	this->matpar=NULL;
	
	/*not active, not zigzagging: */
	active=0;
	zigzag_counter=0;

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

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

	int numberofvertices;
	int numberofelements;

	/*Fetch parameters: */
	iomodel->Constant(&numberofvertices,MeshNumberofverticesEnum);
	iomodel->Constant(&numberofelements,MeshNumberofelementsEnum);

	/*Some checks if debugging activated*/
	_assert_(iomodel->singlenodetoelementconnectivity);
	_assert_(index>=0 && index<numberofvertices);
	_assert_(id);

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

	this->hnode=new Hook(&pengrid_node_id,1);
	this->helement=new Hook(&pengrid_element_id,1);
	this->hmatpar=new Hook(&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;
	this->node=NULL;
	this->element=NULL;
	this->matpar=NULL;

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

}
/*}}}*/
/*FUNCTION Pengrid::~Pengrid(){{{*/
Pengrid::~Pengrid(){
	delete inputs;
	delete hnode;
	delete helement;
	delete hmatpar;
	return;
}
/*}}}*/
			
/*Object virtual functions definitions:*/
/*FUNCTION Pengrid::Echo {{{*/
void Pengrid::Echo(void){
	this->DeepEcho();
}
/*}}}*/
/*FUNCTION Pengrid::DeepEcho{{{*/
void Pengrid::DeepEcho(void){

	_printLine_("Pengrid:");
	_printLine_("   id: " << id);
	_printLine_("   analysis_type: " << EnumToStringx(analysis_type));
	hnode->DeepEcho();
	helement->DeepEcho();
	hmatpar->DeepEcho();
	_printLine_("   active " << this->active);
	_printLine_("   zigzag_counter " << this->zigzag_counter);
	_printLine_("   parameters");
	parameters->DeepEcho();
	_printLine_("   inputs");
	inputs->DeepEcho();
}
/*}}}*/
/*FUNCTION Pengrid::Id {{{*/
int    Pengrid::Id(void){ return id; }
/*}}}*/
/*FUNCTION Pengrid::MyRank {{{*/
int    Pengrid::MyRank(void){ 
	extern int my_rank;
	return my_rank; 
}
/*}}}*/
/*FUNCTION Pengrid::ObjectEnum{{{*/
int Pengrid::ObjectEnum(void){

	return PengridEnum;
}
/*}}}*/
/*FUNCTION Icefront::copy {{{*/
Object* Pengrid::copy() {
	
	Pengrid* pengrid=NULL;

	pengrid=new Pengrid();

	/*copy fields: */
	pengrid->id=this->id;
	pengrid->analysis_type=this->analysis_type;
	if(this->inputs){
		pengrid->inputs=(Inputs*)this->inputs->Copy();
	}
	else{
		pengrid->inputs=new Inputs();
	}
	/*point parameters: */
	pengrid->parameters=this->parameters;

	/*now deal with hooks and objects: */
	pengrid->hnode=(Hook*)this->hnode->copy();
	pengrid->hmatpar=(Hook*)this->hmatpar->copy();
	pengrid->helement=(Hook*)this->helement->copy();

	/*corresponding fields*/
	pengrid->node  =(Node*)pengrid->hnode->delivers();
	pengrid->matpar =(Matpar*)pengrid->hmatpar->delivers();
	pengrid->element=(Element*)pengrid->helement->delivers();

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

	return pengrid;

}
/*}}}*/

/*Load virtual functions definitions:*/
/*FUNCTION Pengrid::Configure {{{*/
void  Pengrid::Configure(Elements* elementsin,Loads* loadsin,Nodes* nodesin,Vertices* verticesin,Materials* 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);

	/*Get corresponding fields*/
	node=(Node*)hnode->delivers();
	element=(Element*)helement->delivers();
	matpar=(Matpar*)hmatpar->delivers();

	/*point parameters to real dataset: */
	this->parameters=parametersin;
}
/*}}}*/
/*FUNCTION Pengrid::SetCurrentConfiguration {{{*/
void  Pengrid::SetCurrentConfiguration(Elements* elementsin,Loads* loadsin,Nodes* nodesin,Vertices* verticesin,Materials* materialsin,Parameters* parametersin){

}
/*}}}*/
/*FUNCTION Pengrid::CreateKMatrix {{{*/
void  Pengrid::CreateKMatrix(Matrix* Kff, Matrix* Kfs){

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

}
/*}}}*/
/*FUNCTION Pengrid::CreatePVector {{{*/
void  Pengrid::CreatePVector(Vector* pf){

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

}
/*}}}*/
/*FUNCTION Pengrid::PenaltyCreateMatrix {{{*/
void  Pengrid::PenaltyCreateKMatrix(Matrix* Kff, Matrix* Kfs,IssmDouble kmax){

	/*Retrieve parameters: */
	ElementMatrix* Ke=NULL;
	int analysis_type;
	this->parameters->FindParam(&analysis_type,AnalysisTypeEnum);

	switch(analysis_type){
		#ifdef _HAVE_DIAGNOSTIC_
		case DiagnosticHorizAnalysisEnum: case AdjointHorizAnalysisEnum:
			Ke=PenaltyCreateKMatrixDiagnosticStokes(kmax);
			break;
		#endif
		#ifdef _HAVE_THERMAL_
		case ThermalAnalysisEnum:
			Ke=PenaltyCreateKMatrixThermal(kmax);
			break;
		case MeltingAnalysisEnum:
			Ke=PenaltyCreateKMatrixMelting(kmax);
			break;
		#endif
		default:
			_error2_("analysis " << analysis_type << " (" << EnumToStringx(analysis_type) << ") not supported yet");
	}

	/*Add to global matrix*/
	if(Ke){
		Ke->AddToGlobal(Kff,Kfs);
		delete Ke;
	}
}
/*}}}*/
/*FUNCTION Pengrid::PenaltyCreatePVector {{{*/
void  Pengrid::PenaltyCreatePVector(Vector* pf,IssmDouble kmax){

	/*Retrieve parameters: */
	ElementVector* pe=NULL;
	int analysis_type;
	this->parameters->FindParam(&analysis_type,AnalysisTypeEnum);

	switch(analysis_type){
		#ifdef _HAVE_DIAGNOSTIC_
		case ThermalAnalysisEnum:
			pe=PenaltyCreatePVectorThermal(kmax);
			break;
		#endif
		#ifdef _HAVE_THERMAL_
		case MeltingAnalysisEnum:
			pe=PenaltyCreatePVectorMelting(kmax);
			break;
		case DiagnosticHorizAnalysisEnum: case AdjointHorizAnalysisEnum:
			break;
		#endif
		default:
			_error2_("analysis " << analysis_type << " (" << EnumToStringx(analysis_type) << ") not supported yet");
	}

	/*Add to global Vector*/
	if(pe){
		pe->AddToGlobal(pf);
		delete pe;
	}
}
/*}}}*/
/*FUNCTION Pengrid::InAnalysis{{{*/
bool Pengrid::InAnalysis(int in_analysis_type){
	if (in_analysis_type==this->analysis_type)return true;
	else return false;
}
/*}}}*/

/*Update virtual functions definitions:*/
/*FUNCTION Pengrid::InputUpdateFromVector(IssmDouble* vector, int name, int type) {{{*/
void  Pengrid::InputUpdateFromVector(IssmDouble* vector, int name, int type){
	/*Nothing updated yet*/
}
/*}}}*/
/*FUNCTION Pengrid::InputUpdateFromVector(int* vector, int name, int type) {{{*/
void  Pengrid::InputUpdateFromVector(int* vector, int name, int type){
	/*Nothing updated yet*/
}
/*}}}*/
/*FUNCTION Pengrid::InputUpdateFromVector(bool* vector, int name, int type) {{{*/
void  Pengrid::InputUpdateFromVector(bool* vector, int name, int type){
	/*Nothing updated yet*/
}
/*}}}*/
/*FUNCTION Pengrid::InputUpdateFromMatrixDakota(IssmDouble* vector, int nrows, int ncols, int name, int type) {{{*/
void  Pengrid::InputUpdateFromMatrixDakota(IssmDouble* matrix, int nrows, int ncols, int name, int type){
	/*Nothing updated yet*/
}
/*}}}*/
/*FUNCTION Pengrid::InputUpdateFromVectorDakota(IssmDouble* vector, int name, int type) {{{*/
void  Pengrid::InputUpdateFromVectorDakota(IssmDouble* vector, int name, int type){
	/*Nothing updated yet*/
}
/*}}}*/
/*FUNCTION Pengrid::InputUpdateFromVectorDakota(int* vector, int name, int type) {{{*/
void  Pengrid::InputUpdateFromVectorDakota(int* vector, int name, int type){
	/*Nothing updated yet*/
}
/*}}}*/
/*FUNCTION Pengrid::InputUpdateFromVectorDakota(bool* vector, int name, int type) {{{*/
void  Pengrid::InputUpdateFromVectorDakota(bool* vector, int name, int type){
	/*Nothing updated yet*/
}
/*}}}*/
/*FUNCTION Pengrid::InputUpdateFromConstant(IssmDouble constant, int name) {{{*/
void  Pengrid::InputUpdateFromConstant(IssmDouble constant, int name){
	switch(name){

		case MeltingOffsetEnum:
			inputs->AddInput(new DoubleInput(name,constant));
			return;

	}
}
/*}}}*/
/*FUNCTION Pengrid::InputUpdateFromConstant(int constant, int name) {{{*/
void  Pengrid::InputUpdateFromConstant(int constant, int name){
	/*Nothing updated yet*/
}
/*}}}*/
/*FUNCTION Pengrid::InputUpdateFromConstant(bool constant, int name) {{{*/
void  Pengrid::InputUpdateFromConstant(bool constant, int name){

	switch(name){

		case ResetPenaltiesEnum:
			if (constant) zigzag_counter=0;
			return;

	}
}
/*}}}*/
/*FUNCTION Pengrid::InputUpdateFromSolution{{{*/
void  Pengrid::InputUpdateFromSolution(IssmDouble* solution){
	/*Nothing updated yet*/
}
/*}}}*/		

/*Pengrid management:*/
/*FUNCTION Pengrid::ConstraintActivate {{{*/
void  Pengrid::ConstraintActivate(int* punstable){

	int analysis_type;

	/*Retrieve parameters: */
	this->parameters->FindParam(&analysis_type,AnalysisTypeEnum);

	if (analysis_type==DiagnosticHorizAnalysisEnum){
		/*No penalty to check*/
		return;
	}
	else if (analysis_type==ThermalAnalysisEnum){
		ConstraintActivateThermal(punstable);
	}
	else if (analysis_type==MeltingAnalysisEnum){
		/*No penalty to check*/
		return;
	}
	else{
		_error2_("analysis: " << EnumToStringx(analysis_type) << " not supported yet");
	}

}
/*}}}*/
/*FUNCTION Pengrid::ConstraintActivateThermal {{{*/
void  Pengrid::ConstraintActivateThermal(int* punstable){

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

	int    found=0;
	const int numnodes=1;
	IssmDouble pressure;
	IssmDouble temperature;
	IssmDouble t_pmp;
	int    new_active;
	int    unstable=0;
	int    reset_penalties=0;
	int    penalty_lock;

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

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

	//Recover our data:
	parameters->FindParam(&penalty_lock,ThermalPenaltyLockEnum);
	
	//Compute pressure melting point
	t_pmp=matpar->TMeltingPoint(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(penalty_lock)zigzag_counter++;
	}

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

	//Set penalty flag
	active=new_active;

	//*Assign output pointers:*/
	*punstable=unstable;
}
/*}}}*/
#ifdef _HAVE_DIAGNOSTIC_
/*FUNCTION Pengrid::PenaltyCreateKMatrixDiagnosticStokes {{{*/
ElementMatrix* Pengrid::PenaltyCreateKMatrixDiagnosticStokes(IssmDouble kmax){
	
	const int numdof = NUMVERTICES *NDOF4;
	IssmDouble    slope[2];
	IssmDouble    penalty_offset;
	int       approximation;

	Penta* penta=(Penta*)element;

	/*Initialize Element vector and return if necessary*/
	penta->inputs->GetInputValue(&approximation,ApproximationEnum);
	if(approximation!=StokesApproximationEnum &&  approximation!=PattynStokesApproximationEnum) return NULL;
	ElementMatrix* Ke=new ElementMatrix(&node,1,this->parameters,StokesApproximationEnum);

	/*Retrieve all inputs and parameters*/
	parameters->FindParam(&penalty_offset,DiagnosticPenaltyFactorEnum);
	penta->GetInputValue(&slope[0],node,BedSlopeXEnum);
	penta->GetInputValue(&slope[1],node,BedSlopeYEnum);

	/*Create elementary matrix: add penalty to constrain wb (wb=ub*db/dx+vb*db/dy)*/
	Ke->values[2*NDOF4+0]=-slope[0]*kmax*pow((IssmDouble)10.0,penalty_offset);
	Ke->values[2*NDOF4+1]=-slope[1]*kmax*pow((IssmDouble)10.0,penalty_offset);
	Ke->values[2*NDOF4+2]= kmax*pow((IssmDouble)10,penalty_offset);

	/*Transform Coordinate System*/
	TransformStiffnessMatrixCoord(Ke,&node,NUMVERTICES,XYZPEnum);

	/*Clean up and return*/
	return Ke;
}
/*}}}*/
#endif
#ifdef _HAVE_THERMAL_
/*FUNCTION Pengrid::PenaltyCreateKMatrixMelting {{{*/
ElementMatrix* Pengrid::PenaltyCreateKMatrixMelting(IssmDouble kmax){

	const int numdof=NUMVERTICES*NDOF1;
	IssmDouble pressure,temperature,t_pmp;
	IssmDouble penalty_factor;

	Penta* penta=(Penta*)element;

	/*check that pengrid is not a clone (penalty to be added only once)*/
	if (node->IsClone()) return NULL;
	ElementMatrix* Ke=new ElementMatrix(&node,1,this->parameters);

	/*Retrieve all inputs and parameters*/
	penta->GetInputValue(&pressure,node,PressureEnum);
	penta->GetInputValue(&temperature,node,TemperatureEnum);
	parameters->FindParam(&penalty_factor,ThermalPenaltyFactorEnum);
	
	/*Compute pressure melting point*/
	t_pmp=matpar->GetMeltingPoint()-matpar->GetBeta()*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->values[0]=kmax*pow((IssmDouble)10,penalty_factor);
	}

	/*Clean up and return*/
	return Ke;
}
/*}}}*/
/*FUNCTION Pengrid::PenaltyCreateKMatrixThermal {{{*/
ElementMatrix* Pengrid::PenaltyCreateKMatrixThermal(IssmDouble kmax){

	const int numdof=NUMVERTICES*NDOF1;
	IssmDouble    penalty_factor;

	/*Initialize Element matrix and return if necessary*/
	if(!this->active) return NULL;
	ElementMatrix* Ke=new ElementMatrix(&node,NUMVERTICES,this->parameters);

	/*recover parameters: */
	parameters->FindParam(&penalty_factor,ThermalPenaltyFactorEnum);

	Ke->values[0]=kmax*pow((IssmDouble)10,penalty_factor);

	/*Clean up and return*/
	return Ke;
}
/*}}}*/
/*FUNCTION Pengrid::PenaltyCreatePVectorMelting {{{*/
ElementVector* Pengrid::PenaltyCreatePVectorMelting(IssmDouble kmax){
	
	const int numdof=NUMVERTICES*NDOF1;
	IssmDouble pressure;
	IssmDouble temperature;
	IssmDouble melting_offset;
	IssmDouble t_pmp;
	IssmDouble dt,penalty_factor;

	/*recover pointers: */
	Penta* penta=(Penta*)element;

	/*check that pengrid is not a clone (penalty to be added only once)*/
	if (node->IsClone()) return NULL;
	ElementVector* pe=new ElementVector(&node,NUMVERTICES,this->parameters);

	/*Retrieve all inputs and parameters*/
	penta->GetInputValue(&pressure,node,PressureEnum);
	penta->GetInputValue(&temperature,node,TemperatureEnum);
	inputs->GetInputValue(&melting_offset,MeltingOffsetEnum);
	parameters->FindParam(&dt,TimesteppingTimeStepEnum);
	parameters->FindParam(&penalty_factor,ThermalPenaltyFactorEnum);

	/*Compute pressure melting point*/
	t_pmp=matpar->GetMeltingPoint()-matpar->GetBeta()*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_factor*/
	if (temperature<t_pmp){ //%no melting
		pe->values[0]=0;
	}
	else{
		if (dt) pe->values[0]=melting_offset*pow((IssmDouble)10,penalty_factor)*(temperature-t_pmp)/dt;
		else    pe->values[0]=melting_offset*pow((IssmDouble)10,penalty_factor)*(temperature-t_pmp);
	}

	/*Clean up and return*/
	return pe;
}
/*}}}*/
/*FUNCTION Pengrid::PenaltyCreatePVectorThermal {{{*/
ElementVector* Pengrid::PenaltyCreatePVectorThermal(IssmDouble kmax){

	const int numdof=NUMVERTICES*NDOF1;
	IssmDouble pressure;
	IssmDouble t_pmp;
	IssmDouble penalty_factor;

	Penta* penta=(Penta*)element;

	/*Initialize Element matrix and return if necessary*/
	if(!this->active) return NULL;
	ElementVector* pe=new ElementVector(&node,1,this->parameters);

	/*Retrieve all inputs and parameters*/
	penta->GetInputValue(&pressure,node,PressureEnum);
	parameters->FindParam(&penalty_factor,ThermalPenaltyFactorEnum);

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

	pe->values[0]=kmax*pow((IssmDouble)10,penalty_factor)*t_pmp;

	/*Clean up and return*/
	return pe;
}
/*}}}*/
#endif
/*FUNCTION Pengrid::ResetConstraint {{{*/
void  Pengrid::ResetConstraint(void){
	active=0;
	zigzag_counter=0;
}
/*}}}*/
/*FUNCTION Pengrid::UpdateInputs {{{*/
void  Pengrid::UpdateInputs(IssmDouble* solution){
	_error2_("not supported yet!");
}
/*}}}*/
