/*!\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 "../shared/shared.h"
#include "../include/typedefs.h"

		
Pengrid::Pengrid(){
	return;
}

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

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

	return;
}

Pengrid::~Pengrid(){
	return;
}
		
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);
	
	if(node)node->Echo();
	if(matpar)matpar->Echo();
	return;
}
		
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);

	*pmarshalled_dataset=marshalled_dataset;
	return;
}
		
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(int); //sizeof(int) for enum type
}

char* Pengrid::GetName(void){
	return "pengrid";
}
		

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


	node=NULL;
	matpar=NULL;

	/*return: */
	*pmarshalled_dataset=marshalled_dataset;
	return;
}
int Pengrid::Enum(void){

	return PengridEnum();
}

int    Pengrid::GetId(void){ return id; }

int    Pengrid::MyRank(void){ 
	extern int my_rank;
	return my_rank; 
}
void  Pengrid::DistributeNumDofs(int* numdofpernode,int analysis_type,int sub_analysis_type){return;}

#undef __FUNCT__ 
#define __FUNCT__ "Pengrid::Configure"

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


#undef __FUNCT__ 
#define __FUNCT__ "Pengrid::CreateKMatrix"

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

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

}

#undef __FUNCT__ 
#define __FUNCT__ "Pengrid::CreatePVector"
void  Pengrid::CreatePVector(Vec pg, void* inputs, int analysis_type,int sub_analysis_type){

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

}
#undef __FUNCT__ 
#define __FUNCT__ "Pengrid::UpdateFromInputs"
void  Pengrid::UpdateFromInputs(void* inputs){
	
}

#undef __FUNCT__ 
#define __FUNCT__ "Pengrid::PenaltyCreateKMatrix"
void  Pengrid::PenaltyCreateKMatrix(Mat Kgg,void* inputs,double kmax,int analysis_type,int sub_analysis_type){

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

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

	}
	else{
		throw ErrorException(__FUNCT__,exprintf("%s%i%s%i%s","analysis: ",analysis_type," and sub_analysis_type: ",sub_analysis_type," not supported yet"));
	}

}

#undef __FUNCT__ 
#define __FUNCT__ "Pengrid::PenaltyCreateKMatrixDiagnosticStokes"
void  Pengrid::PenaltyCreateKMatrixDiagnosticStokes(Mat Kgg,void* vinputs,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)throw ErrorException(__FUNCT__," bedslopex needed in inputs!");
	found=inputs->Recover("bedslopey",&slope[1],1,dofs2,numgrids,(void**)&node);
	if(!found)throw ErrorException(__FUNCT__," 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(10.0,penalty_offset);
	Ke[2][1]=-slope[1]*kmax*pow(10.0,penalty_offset);
	Ke[2][2]=kmax*pow(10,penalty_offset);
	
	/*Add Ke to global matrix Kgg: */
	MatSetValues(Kgg,numdof,doflist,numdof,doflist,(const double*)Ke,ADD_VALUES);
}

#undef __FUNCT__ 
#define __FUNCT__ "Pengrid::PenaltyCreateKMatrixThermal"
void  Pengrid::PenaltyCreateKMatrixThermal(Mat Kgg,void* vinputs,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(10,penalty_offset);
	
	/*Add Ke to global matrix Kgg: */
	MatSetValues(Kgg,numdof,doflist,numdof,doflist,(const double*)Ke,ADD_VALUES);
}

#undef __FUNCT__ 
#define __FUNCT__ "Pengrid::PenaltyCreateKMatrixMelting"
void  Pengrid::PenaltyCreateKMatrixMelting(Mat Kgg,void* vinputs,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;

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

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

	found=inputs->Recover("temperature",&temperature,1,dofs1,numgrids,(void**)&node);
	if(!found)throw ErrorException(__FUNCT__," 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(10,penalty_offset);
	}
	
	MatSetValues(Kgg,numdof,doflist,numdof,doflist,(const double*)Ke,ADD_VALUES);
}

#undef __FUNCT__ 
#define __FUNCT__ "Pengrid::PenaltyCreatePVector"
void  Pengrid::PenaltyCreatePVector(Vec pg,void* inputs,double kmax,int analysis_type,int sub_analysis_type){

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

	}
	else{
		throw ErrorException(__FUNCT__,exprintf("%s%i%s%i%s","analysis: ",analysis_type," and sub_analysis_type: ",sub_analysis_type," not supported yet"));
	}

}

Object* Pengrid::copy() {
	return new Pengrid(*this); 
}


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

void  Pengrid::PenaltyCreatePVectorThermal(Vec pg, void* vinputs, 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)throw ErrorException(__FUNCT__," 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(10,penalty_offset)*t_pmp;

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

void  Pengrid::PenaltyCreatePVectorMelting(Vec pg, void* vinputs, 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;

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

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

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

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

	found=inputs->Recover("dt",&dt);
	if((!found) && (sub_analysis_type==TransientAnalysisEnum()))throw ErrorException(__FUNCT__," 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 (sub_analysis_type==SteadyAnalysisEnum()){
			P_terms[0]=melting_offset*pow(10,penalty_offset)*(temperature-t_pmp);
		}
		else{
			P_terms[0]=melting_offset*pow(10,penalty_offset)*(temperature-t_pmp)/dt;
		}
	}
	/*Add P_terms to global vector pg: */
	VecSetValues(pg,numdof,doflist,(const double*)P_terms,ADD_VALUES);
}
		

#undef __FUNCT__ 
#define __FUNCT__ "Pengrid::PenaltyConstrain"
void  Pengrid::PenaltyConstrain(int* punstable,void* vinputs,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={0};
	int  unstable=0;
	
	ParameterInputs* inputs=NULL;

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


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

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


	//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;
	}
	
	//Set penalty flag
	active=new_active;

	//*Assign output pointers:*/
	*punstable=unstable;
}