/*!\file Riftfront.cpp
 * \brief: implementation of the Riftfront object
 */

/*Headers:*/
/*{{{1*/
#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 "../EnumDefinitions/EnumDefinitions.h"
#include "../shared/shared.h"
#include "../include/typedefs.h"
#include "../include/macros.h"
#include "../ModelProcessorx/ModelProcessorx.h"
#include "./objects.h"
/*}}}*/

/*Object constructors and destructor*/
/*FUNCTION Riftfront::Riftfront(){{{1*/
Riftfront::Riftfront(){
	this->inputs=NULL;
	this->parameters=NULL;
}
/*}}}*/
/*FUNCTION Riftfront::Riftfront(int id, int* node_ids, int matice_id, int matpar_id){{{1*/
Riftfront::Riftfront(int riftfront_id,int* riftfront_node_ids, int riftfront_matpar_id): 
	hnodes(riftfront_node_ids,2),
	hmatpar(&riftfront_matpar_id,1)
{

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

}
/*}}}*/
/*FUNCTION Riftfront::Riftfront(int id, Hook* hnodes, Hook* hmatice, Hook* hmatpar, DataSet* parameters, Inputs* riftfront_inputs) {{{1*/
Riftfront::Riftfront(int riftfront_id,Hook* riftfront_hnodes, Hook* riftfront_hmatpar, Parameters* riftfront_parameters, Inputs* riftfront_inputs):
	hnodes(riftfront_hnodes),
	hmatpar(riftfront_hmatpar)
{

	/*all the initialization has been done by the initializer, just fill in the id: */
	this->id=riftfront_id;
	if(riftfront_inputs){
		this->inputs=(Inputs*)riftfront_inputs->Copy();
	}
	else{
		this->inputs=new Inputs();
	}
	/*point parameters: */
	this->parameters=riftfront_parameters;
}
/*}}}*/
/*FUNCTION Riftfront::Riftfront(int id, int i, IoModel* iomodel){{{1*/
Riftfront::Riftfront(int riftfront_id,int i, IoModel* iomodel){

	/*data: */
	int    riftfront_node_ids[2];
	int    riftfront_matpar_id;
	int    riftfront_type;
	double riftfront_fill;
	double riftfront_friction;
	double riftfront_fractionincrement;
	bool   riftfront_shelf;

	/*intermediary: */
	int el1    ,el2;
	int grid1  ,grid2;

	/*Ok, retrieve all the data needed to add a penalty between the two grids: */
	el1=(int)*(iomodel->riftinfo+RIFTINFOSIZE*i+2);
	el2=(int)*(iomodel->riftinfo+RIFTINFOSIZE*i+3); 

	grid1=(int)*(iomodel->riftinfo+RIFTINFOSIZE*i+0); 
	grid2=(int)*(iomodel->riftinfo+RIFTINFOSIZE*i+1);

	/*id: */
	this->id=riftfront_id;

	/*hooks: */
	riftfront_node_ids[0]=grid1;
	riftfront_node_ids[1]=grid2;
	riftfront_matpar_id=iomodel->numberofelements+1; //matlab indexing

	this->hnodes.Init(riftfront_node_ids,2);
	this->hmatpar.Init(&riftfront_matpar_id,1);

	/*computational parameters: */
	this->active=0;
	this->frozen=0;
	this->counter=0;
	this->prestable=0;
	this->penalty_lock=0;
	this->material_converged=0;
	this->normal[0]=*(iomodel->riftinfo+RIFTINFOSIZE*i+4);
	this->normal[1]=*(iomodel->riftinfo+RIFTINFOSIZE*i+5);
	this->length=*(iomodel->riftinfo+RIFTINFOSIZE*i+6);
	this->fraction=*(iomodel->riftinfo+RIFTINFOSIZE*i+9);

	//intialize inputs, and add as many inputs per element as requested: 
	this->inputs=new Inputs();
		
	riftfront_type=SegmentRiftfrontEnum;
	riftfront_fill = (int)*(iomodel->riftinfo+RIFTINFOSIZE*i+7);
	riftfront_friction=*(iomodel->riftinfo+RIFTINFOSIZE*i+8);
	riftfront_fractionincrement=*(iomodel->riftinfo+RIFTINFOSIZE*i+10);
	riftfront_shelf=(bool)iomodel->gridoniceshelf[grid1-1];

	this->inputs->AddInput(new IntInput(TypeEnum,riftfront_type));
	this->inputs->AddInput(new DoubleInput(FillEnum,riftfront_fill));
	this->inputs->AddInput(new DoubleInput(FrictionEnum,riftfront_friction));
	this->inputs->AddInput(new DoubleInput(FractionIncrementEnum,riftfront_fractionincrement));
	this->inputs->AddInput(new BoolInput(SegmentOnIceShelfEnum,riftfront_shelf));
	
	//this->parameters: we still can't point to it, it may not even exist. Configure will handle this.
	this->parameters=NULL;
		
}
/*}}}1*/
/*FUNCTION Riftfront::~Riftfront(){{{1*/
Riftfront::~Riftfront(){
	delete inputs;
	this->parameters=NULL;
}
/*}}}*/

/*Object marshall*/
/*FUNCTION Riftfront::copy {{{1*/
Object* Riftfront::copy() {
	return new Riftfront(*this); 
}
/*}}}1*/
/*FUNCTION Riftfront::Configure {{{1*/
void  Riftfront::Configure(DataSet* elementsin,DataSet* loadsin,DataSet* nodesin,DataSet* verticesin,DataSet* materialsin,Parameters* parametersin){

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

	/*point parameters to real dataset: */
	this->parameters=parametersin;

}
/*}}}*/
/*FUNCTION Riftfront::DeepEcho{{{1*/
void Riftfront::DeepEcho(void){

	printf("Riftfront:\n");
	printf("   id: %i\n",id);
	hnodes.DeepEcho();
	hmatpar.DeepEcho();
	printf("   parameters\n");
	parameters->DeepEcho();
	printf("   inputs\n");
	inputs->DeepEcho();
}
/*}}}*/
/*FUNCTION Riftfront::Demarshall {{{1*/
void  Riftfront::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(&normal,marshalled_dataset,sizeof(normal));marshalled_dataset+=sizeof(normal);
	memcpy(&length,marshalled_dataset,sizeof(length));marshalled_dataset+=sizeof(length);
	memcpy(&fraction,marshalled_dataset,sizeof(fraction));marshalled_dataset+=sizeof(fraction);
	memcpy(&frozen,marshalled_dataset,sizeof(frozen));marshalled_dataset+=sizeof(frozen);
	memcpy(&counter,marshalled_dataset,sizeof(counter));marshalled_dataset+=sizeof(counter);
	memcpy(&prestable,marshalled_dataset,sizeof(prestable));marshalled_dataset+=sizeof(prestable);
	memcpy(&penalty_lock,marshalled_dataset,sizeof(penalty_lock));marshalled_dataset+=sizeof(penalty_lock);
	memcpy(&material_converged,marshalled_dataset,sizeof(material_converged));marshalled_dataset+=sizeof(material_converged);

	/*demarshall hooks: */
	hnodes.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;
}
/*}}}*/
/*FUNCTION Riftfront::Echo {{{1*/
void Riftfront::Echo(void){
	this->DeepEcho();
}
/*}}}1*/
/*FUNCTION Riftfront::Enum {{{1*/
int Riftfront::Enum(void){

	return RiftfrontEnum;

}
/*}}}1*/
/*FUNCTION Riftfront::GetId {{{1*/
int    Riftfront::GetId(void){ return id; }
/*}}}1*/
/*FUNCTION Riftfront::GetName {{{1*/
char* Riftfront::GetName(void){
	return "riftfront";
}
/*}}}1*/
/*FUNCTION Riftfront::Marshall {{{1*/
void  Riftfront::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 Riftfront: */
	enum_type=RiftfrontEnum;

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

	/*marshall Riftfront data: */
	memcpy(marshalled_dataset,&id,sizeof(id));marshalled_dataset+=sizeof(id);
	memcpy(marshalled_dataset,&active,sizeof(active));marshalled_dataset+=sizeof(active);
	memcpy(marshalled_dataset,&normal,sizeof(normal));marshalled_dataset+=sizeof(normal);
	memcpy(marshalled_dataset,&length,sizeof(length));marshalled_dataset+=sizeof(length);
	memcpy(marshalled_dataset,&fraction,sizeof(fraction));marshalled_dataset+=sizeof(fraction);
	memcpy(marshalled_dataset,&frozen,sizeof(frozen));marshalled_dataset+=sizeof(frozen);
	memcpy(marshalled_dataset,&counter,sizeof(counter));marshalled_dataset+=sizeof(counter);
	memcpy(marshalled_dataset,&prestable,sizeof(prestable));marshalled_dataset+=sizeof(prestable);
	memcpy(marshalled_dataset,&penalty_lock,sizeof(penalty_lock));marshalled_dataset+=sizeof(penalty_lock);

	/*Marshall hooks: */
	hnodes.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 Riftfront::MarshallSize {{{1*/
int   Riftfront::MarshallSize(){
	
	return sizeof(id)
		+sizeof(active)
		+sizeof(normal)
		+sizeof(length)
		+sizeof(fraction)
		+sizeof(frozen)
		+sizeof(counter)
		+sizeof(prestable)
		+sizeof(penalty_lock)
		+hnodes.MarshallSize()
		+hmatpar.MarshallSize()
		+inputs->MarshallSize()
		+sizeof(int); //sizeof(int) for enum type
}
/*}}}*/
/*FUNCTION Riftfront::MyRank {{{1*/
int    Riftfront::MyRank(void){ 
	extern int my_rank;
	return my_rank; 
}
/*}}}1*/

/*Object functions*/
/*FUNCTION Riftfront::Constrain {{{1*/
#define _ZIGZAGCOUNTER_

int   Riftfront::Constrain(int* punstable,  int analysis_type){

	const int   numgrids        = 2;
	double      max_penetration;
	double      penetration;
	int         activate;
	int         found;
	int         unstable;
	double      vx1;
	double      vy1;
	double      vx2;
	double      vy2;
	double      fractionincrement;


	/*Objects: */
	Element   **elements        = NULL;
	Node      **nodes           = NULL;
	Tria       *tria1           = NULL;
	Tria       *tria2           = NULL;

	/*Recover hook objects: */
	elements=(Element**)helements.deliverp();
	nodes=(Node**)hnodes.deliverp();

	/*enum of element? */
	if(elements[0]->Enum()!=TriaEnum)ISSMERROR(" only Tria element allowed for Riftfront load!");

	/*recover elements on both side of rift: */
	tria1=(Tria*)elements[0];
	tria2=(Tria*)elements[1];

	/*Is this constraint frozen? In which case we don't touch: */
	if (this->frozen){
		*punstable=0;
		return 1;
	}

	/*recover parameters: */
	this->inputs->GetParameterValue(&fractionincrement,FractionIncrementEnum);

	/*First recover velocity: */
	tria1->inputs->GetParameterValueAtNode(&vx1,nodes[0],VxEnum);
	tria2->inputs->GetParameterValueAtNode(&vx2,nodes[1],VxEnum);
	tria1->inputs->GetParameterValueAtNode(&vy1,nodes[0],VyEnum);
	tria2->inputs->GetParameterValueAtNode(&vy2,nodes[1],VyEnum);

	/*Node 1 faces node 2, compute penetration of 2 into 1 (V2-V1).N (with N normal vector, and V velocity vector: */
	penetration=(vx2-vx1)*normal[0]+(vy2-vy1)*normal[1];

	/*activation: */
	if(penetration<0)activate=1;
	else  activate=0;

	/*Here, we try to avoid zigzaging. When a penalty activates and deactivates for more than penalty_lock times, 
	 * we increase the fraction of melange:*/
	if(this->counter>this->penalty_lock){
		/*reset counter: */
		this->counter=0;
		/*increase melange fraction: */
		this->fraction+=fractionincrement;
		if (this->fraction>1)this->fraction=(double)1.0;
		//printf("riftfront %i fraction: %g\n",this->GetId(),this->fraction);
	}

	//Figure out stability of this penalty
	if(this->active==activate){
		unstable=0;
	}
	else{
		unstable=1;
		this->counter++;
	}

	//Set penalty flag
	this->active=activate;

	//if ((penetration>0) & (this->active==1))printf("Riftfront %i wants to be released\n",GetId());

	/*assign output pointer: */
	*punstable=unstable;
}
/*}}}1*/
/*FUNCTION Riftfront::CreateKMatrix {{{1*/
void  Riftfront::CreateKMatrix(Mat Kgg,int analysis_type,int sub_analysis_type){
	/*do nothing: */
}
/*}}}1*/
/*FUNCTION Riftfront::CreatePVector {{{1*/
void  Riftfront::CreatePVector(Vec pg,  int analysis_type,int sub_analysis_type){
	/*do nothing: */
}
/*}}}1*/
/*FUNCTION Riftfront::FreezeConstraints{{{1*/
void   Riftfront::FreezeConstraints( int analysis_type){

	/*Just set frozen flag to 1: */
	this->frozen=1;

}
/*}}}1*/
/*FUNCTION Riftfront::GetDofList {{{1*/

void  Riftfront::GetDofList(int* doflist,int* pnumberofdofspernode){

	int i,j;
	int doflist_per_node[MAXDOFSPERNODE];
	int numberofdofspernode;
	Node      **nodes           = NULL;
	
	nodes=(Node**)hnodes.deliverp();

	for(i=0;i<MAX_RIFTFRONT_GRIDS;i++){
		nodes[i]->GetDofList(&doflist_per_node[0],&numberofdofspernode);
		for(j=0;j<numberofdofspernode;j++){
			doflist[i*numberofdofspernode+j]=doflist_per_node[j];
		}
	}

	/*Assign output pointers:*/
	*pnumberofdofspernode=numberofdofspernode;
}
/*}}}1*/
/*FUNCTION Riftfront::IsFrozen{{{1*/
bool   Riftfront::IsFrozen(void){

	/*Just set frozen flag to 1: */
	if(this->frozen)return 1;
	else return 0;
}
/*}}}1*/
/*FUNCTION Riftfront::IsMaterialStable {{{1*/
int   Riftfront::IsMaterialStable( int analysis_type){

	int found=0;
	double converged=0;

	this->inputs->GetParameterValue(&converged,ConvergedEnum);

	if(converged){
		/*ok, material non-linearity has converged. If that was already the case, we keep 
		 * constraining the rift front. If it was not, and this is the first time the material 
		 * has converged, we start constraining now!: */
		this->material_converged=1;
	}

	return this->material_converged;
}
/*}}}1*/
/*FUNCTION Riftfront::MaxPenetration {{{1*/
int   Riftfront::MaxPenetration(double* ppenetration,  int analysis_type){

	const int     numgrids=2;
	double        max_penetration;
	double        penetration=0;
	int           found;
	double      vx1;
	double      vy1;
	double      vx2;
	double      vy2;

	/*Objects: */
	Element   **elements        = NULL;
	Node      **nodes           = NULL;
	Tria       *tria1           = NULL;
	Tria       *tria2           = NULL;

	/*Recover hook objects: */
	elements=(Element**)helements.deliverp();
	nodes=(Node**)hnodes.deliverp();

	/*enum of element? */
	if(elements[0]->Enum()!=TriaEnum)ISSMERROR(" only Tria element allowed for Riftfront load!");

	/*recover elements on both side of rift: */
	tria1=(Tria*)elements[0];
	tria2=(Tria*)elements[1];

	//initialize: 
	penetration=-1;

	/*recover velocity: */
	tria1->inputs->GetParameterValueAtNode(&vx1,nodes[0],VxEnum);
	tria2->inputs->GetParameterValueAtNode(&vx2,nodes[1],VxEnum);
	tria1->inputs->GetParameterValueAtNode(&vy1,nodes[0],VyEnum);
	tria2->inputs->GetParameterValueAtNode(&vy2,nodes[1],VyEnum);

	/*Grid 1 faces grid2, compute penetration of 2 into 1 (V2-V1).N (with N normal vector, and V velocity vector: */
	penetration=(vx2-vx1)*normal[0]+(vy2-vy1)*normal[1];

	/*Now, we return penetration only if we are active!: */
	if(this->active==0)penetration=-1;

	/*If we are zigzag locked, same thing: */
	if(this->counter>this->penalty_lock)penetration=-1;
	
	/*assign output pointer: */
	*ppenetration=penetration;

}
/*}}}1*/
/*FUNCTION Riftfront::OutputProperties {{{1*/
void  Riftfront::OutputProperties(Vec riftproperties){

	int row_id=0;
	double value;

	/*recover id of penalty: */
	row_id=this->GetId()-1; //c indexing, ids were matlab indexed
	value=(double)this->fraction;

	/*Plug id and fraction  into riftproperties matrix: */
	VecSetValues(riftproperties,1,&row_id,&value,INSERT_VALUES);
}
/*}}}1*/
/*FUNCTION Riftfront::PenaltyCreateKMatrix {{{1*/
void  Riftfront::PenaltyCreateKMatrix(Mat Kgg,double kmax,int analysis_type,int sub_analysis_type){

	int         i;
	int         j;
	const int   numgrids            = MAX_RIFTFRONT_GRIDS;
	int         dofs[1]             = {0};
	double      Ke_gg[4][4];
	const int   numdof              = 2 *numgrids;
	int         doflist[numdof];
	int         numberofdofspernode;
	double      thickness;
	double      h[2];
	double      penalty_offset;
	double      friction;

	/*Objects: */
	Element   **elements            = NULL;
	Node      **nodes               = NULL;
	Tria       *tria1               = NULL;
	Tria       *tria2               = NULL;

	/*Recover hook objects: */
	elements=(Element**)helements.deliverp();
	nodes=(Node**)hnodes.deliverp();

	/*enum of element? */
	if(elements[0]->Enum()!=TriaEnum)ISSMERROR(" only Tria element allowed for Riftfront load!");

	/*recover elements on both side of rift: */
	tria1=(Tria*)elements[0];
	tria2=(Tria*)elements[1];

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

	/* Set Ke_gg to 0: */
	for(i=0;i<numdof;i++) for(j=0;j<numdof;j++) Ke_gg[i][j]=0.0;

	/*Get some parameters: */
	this->parameters->FindParam(&penalty_offset,"penalty_offset");
	this->inputs->GetParameterValue(&friction,FrictionEnum);

	if(this->active){
	
		/*There is contact, we need to constrain the normal velocities (zero penetration), and the 
		 *contact slip friction. */
		  
		/*Recover thickness: */
		tria1->inputs->GetParameterValueAtNode(&h[0],nodes[0],ThicknessEnum);
		tria2->inputs->GetParameterValueAtNode(&h[1],nodes[1],ThicknessEnum);

		if (h[0]!=h[1])ISSMERROR(" different thicknesses not supported for rift fronts");
		thickness=h[0];

		/*From Peter Wriggers book (Computational Contact Mechanics, p191): */
		//First line:
		Ke_gg[0][0]+=pow(normal[0],2)*kmax*pow(10,penalty_offset);
		Ke_gg[0][1]+=normal[0]*normal[1]*kmax*pow(10,penalty_offset);
		Ke_gg[0][2]+=-pow(normal[0],2)*kmax*pow(10,penalty_offset);
		Ke_gg[0][3]+=-normal[0]*normal[1]*kmax*pow(10,penalty_offset);
		//Second line:
		Ke_gg[1][0]+=normal[0]*normal[1]*kmax*pow(10,penalty_offset);
		Ke_gg[1][1]+=pow(normal[1],2)*kmax*pow(10,penalty_offset);
		Ke_gg[1][2]+=-normal[0]*normal[1]*kmax*pow(10,penalty_offset);
		Ke_gg[1][3]+=-pow(normal[1],2)*kmax*pow(10,penalty_offset);
		//Third line:
		Ke_gg[2][0]+=-pow(normal[0],2)*kmax*pow(10,penalty_offset);
		Ke_gg[2][1]+=-normal[0]*normal[1]*kmax*pow(10,penalty_offset);
		Ke_gg[2][2]+=pow(normal[0],2)*kmax*pow(10,penalty_offset);
		Ke_gg[2][3]+=normal[0]*normal[1]*kmax*pow(10,penalty_offset);
		//Fourth line:
		Ke_gg[3][0]+=-normal[0]*normal[1]*kmax*pow(10,penalty_offset);
		Ke_gg[3][1]+=-pow(normal[1],2)*kmax*pow(10,penalty_offset);
		Ke_gg[3][2]+=normal[0]*normal[1]*kmax*pow(10,penalty_offset);
		Ke_gg[3][3]+=pow(normal[1],2)*kmax*pow(10,penalty_offset);

		/*Now take care of the friction: of type sigma=frictiontangent_velocity2-tangent_velocity1)*/
		
		//First line:
		Ke_gg[0][0]+=pow(normal[1],2)*thickness*length*friction;
		Ke_gg[0][1]+=-normal[0]*normal[1]*thickness*length*friction;
		Ke_gg[0][2]+=-pow(normal[1],2)*thickness*length*friction;
		Ke_gg[0][3]+=normal[0]*normal[1]*thickness*length*friction;
		//Second line:
		Ke_gg[1][0]+=-normal[0]*normal[1]*thickness*length*friction;
		Ke_gg[1][1]+=pow(normal[0],2)*thickness*length*friction;
		Ke_gg[1][2]+=normal[0]*normal[1]*thickness*length*friction;
		Ke_gg[1][3]+=-pow(normal[0],2)*thickness*length*friction;
		//Third line:
		Ke_gg[2][0]+=-pow(normal[1],2)*thickness*length*friction;
		Ke_gg[2][1]+=normal[0]*normal[1]*thickness*length*friction;
		Ke_gg[2][2]+=pow(normal[1],2)*thickness*length*friction;
		Ke_gg[2][3]+=-normal[0]*normal[1]*thickness*length*friction;
		//Fourth line:
		Ke_gg[3][0]+=normal[0]*normal[1]*thickness*length*friction;
		Ke_gg[3][1]+=-pow(normal[0],2)*thickness*length*friction;
		Ke_gg[3][2]+=-normal[0]*normal[1]*thickness*length*friction;
		Ke_gg[3][3]+=pow(normal[0],2)*thickness*length*friction;

		/*Add Ke_gg to global matrix Kgg: */
		MatSetValues(Kgg,numdof,doflist,numdof,doflist,(const double*)Ke_gg,ADD_VALUES);
	}
	else{
		/*the grids on both sides of the rift do not penetrate.  PenaltyCreatePVector will 
		 *take care of adding point loads to simulate pressure on the rift flanks. But as far as stiffness, 
		 there is none (0 stiffness implies decoupling of the flank rifts, which is exactly what we want): */
	}

}
/*}}}1*/
/*FUNCTION Riftfront::PenaltyCreatePVector {{{1*/
void  Riftfront::PenaltyCreatePVector(Vec pg,double kmax,int analysis_type,int sub_analysis_type){

	int         i                     ,j;
	const int   numgrids            = MAX_RIFTFRONT_GRIDS;
	double      pe_g[4]={0.0};
	const int   numdof              = 2 *numgrids;
	int         doflist[numdof];
	int         numberofdofspernode;

	double      rho_ice;
	double      rho_water;
	double      gravity;
	double      thickness;
	double      h[2];
	double      bed;
	double      b[2];
	double      pressure;
	double      pressure_litho;
	double      pressure_air;
	double      pressure_melange;
	double      pressure_water;
	double      fill;
	bool        shelf;


	/*Objects: */
	Element   **elements            = NULL;
	Node      **nodes               = NULL;
	Tria       *tria1               = NULL;
	Tria       *tria2               = NULL;
	Matpar     *matpar              = NULL;

	
	/*Recover hook objects: */
	elements=(Element**)helements.deliverp();
	nodes=(Node**)hnodes.deliverp();
	matpar=(Matpar*)hmatpar.delivers();

	/*enum of element? */
	if(elements[0]->Enum()!=TriaEnum)ISSMERROR(" only Tria element allowed for Riftfront load!");

	/*recover elements on both side of rift: */
	tria1=(Tria*)elements[0];
	tria2=(Tria*)elements[1];

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

	/*Get some inputs: */
	this->inputs->GetParameterValue(&fill,FillEnum);
	this->inputs->GetParameterValue(&shelf,SegmentOnIceShelfEnum);

	if(!this->active){
		/*Ok, this rift is opening. We should put loads on both sides of the rift flanks. Because we are dealing with contact mechanics, 
		 * and we want to avoid zigzagging of the loads, we want lump the loads onto grids, not onto surfaces between grids.:*/
	
		/*Ok, to compute the pressure, we are going to need material properties, thickness and bed for the two grids. We assume those properties to 
		 * be the same across the rift.: */

		rho_ice=matpar->GetRhoIce();
		rho_water=matpar->GetRhoWater();
		gravity=matpar->GetG();

		/*get thickness: */
		tria1->inputs->GetParameterValueAtNode(&h[0],nodes[0],ThicknessEnum);
		tria2->inputs->GetParameterValueAtNode(&h[1],nodes[1],ThicknessEnum);

		if (h[0]!=h[1])ISSMERROR(" different thicknesses not supported for rift fronts");
		thickness=h[0];

		tria1->inputs->GetParameterValueAtNode(&b[0],nodes[0],BedEnum);
		tria2->inputs->GetParameterValueAtNode(&b[1],nodes[1],BedEnum);

		if (b[0]!=b[1])ISSMERROR(" different beds not supported for rift fronts");
		bed=b[0];

		/*Ok, now compute the pressure (in norm) that is being applied to the flanks, depending on the type of fill: */
		if(fill==WaterEnum){
			if(shelf){
				/*We are on an ice shelf, hydrostatic equilibrium is used to determine the pressure for water fill: */
				pressure=rho_ice*gravity*pow(thickness,(double)2)/(double)2  - rho_water*gravity*pow(bed,(double)2)/(double)2; 
			}
			else{
				//We are on an icesheet, we assume the water column fills the entire front: */
				pressure=rho_ice*gravity*pow(thickness,(double)2)/(double)2  - rho_water*gravity*pow(thickness,(double)2)/(double)2; 
			}
		}
		else if(fill==AirEnum){
			pressure=rho_ice*gravity*pow(thickness,(double)2)/(double)2;   //icefront on an ice sheet, pressure imbalance ice vs air.
		}
		else if(fill==IceEnum){ //icefront finding itself against another icefront (pressure imbalance is fully compensated, ice vs ice)
			pressure=0;
		}
		else if(fill==MelangeEnum){ //icefront finding itself against another icefront (pressure imbalance is fully compensated, ice vs ice)
			
			if(!shelf) ISSMERROR("%s%i%s","fill type ",fill," not supported on ice sheets yet.");

			pressure_litho=rho_ice*gravity*pow(thickness,(double)2)/(double)2;
			pressure_air=0;
			pressure_melange=rho_ice*gravity*pow(fraction*thickness,(double)2)/(double)2;
			pressure_water=1.0/2.0*rho_water*gravity*  ( pow(bed,2.0)-pow(rho_ice/rho_water*fraction*thickness,2.0) );

			pressure=pressure_litho-pressure_air-pressure_melange-pressure_water;
		}
		else{
			ISSMERROR("%s%i%s","fill type ",fill," not supported yet.");
		}

		/*Ok, add contribution to first grid, along the normal i==0: */
		for (j=0;j<2;j++){
			pe_g[j]+=pressure*normal[j]*length;
		}
	
		/*Add contribution to second grid, along the opposite normal: i==1 */
		for (j=0;j<2;j++){
			pe_g[2+j]+= -pressure*normal[j]*length;
		}	
		/*Add pe_g to global vector pg; */
		VecSetValues(pg,numdof,doflist,(const double*)pe_g,ADD_VALUES);

	}
	else{
		/*The penalty is active. No loads implied here.*/
	}
}
/*}}}1*/
/*FUNCTION Riftfront::Penetration {{{1*/
int   Riftfront::Penetration(double* ppenetration,  int analysis_type){

	double    vx1;
	double    vy1;
	double    vx2;
	double    vy2;

	double    penetration;
	int       found;

	/*Objects: */
	Element **elements    = NULL;
	Node    **nodes       = NULL;
	Tria     *tria1       = NULL;
	Tria     *tria2       = NULL;

	/*Recover hook objects: */
	elements=(Element**)helements.deliverp();
	nodes=(Node**)hnodes.deliverp();

	/*enum of element? */
	if(elements[0]->Enum()!=TriaEnum)ISSMERROR(" only Tria element allowed for Riftfront load!");

	/*recover elements on both side of rift: */
	tria1=(Tria*)elements[0];
	tria2=(Tria*)elements[1];

	/*First recover velocity: */
	tria1->inputs->GetParameterValueAtNode(&vx1,nodes[0],VxEnum);
	tria2->inputs->GetParameterValueAtNode(&vx2,nodes[1],VxEnum);
	tria1->inputs->GetParameterValueAtNode(&vy1,nodes[0],VyEnum);
	tria2->inputs->GetParameterValueAtNode(&vy2,nodes[1],VyEnum);

	/*Node 1 faces node 2, compute penetration of 2 into 1 (V2-V1).N (with N normal vector, and V velocity vector: */
	penetration=(vx2-vx1)*normal[0]+(vy2-vy1)*normal[1];

	/*Now, we return penetration only if we are active!: */
	if(this->active==0)penetration=0;
	
	/*assign output pointer: */
	*ppenetration=penetration;

}
/*}}}1*/
/*FUNCTION Riftfront::PotentialUnstableConstraint {{{1*/
int   Riftfront::PotentialUnstableConstraint(int* punstable,  int analysis_type){


	const int   numgrids        = 2;
	double      max_penetration;
	double      penetration;
	int         activate;
	int         unstable;
	int         found;
	double      vx1;
	double      vy1;
	double      vx2;
	double      vy2;


	/*Objects: */
	Element   **elements        = NULL;
	Node      **nodes           = NULL;
	Tria       *tria1           = NULL;
	Tria       *tria2           = NULL;

	/*Recover hook objects: */
	elements=(Element**)helements.deliverp();
	nodes=(Node**)hnodes.deliverp();

	/*enum of element? */
	if(elements[0]->Enum()!=TriaEnum)ISSMERROR(" only Tria element allowed for Riftfront load!");

	/*recover elements on both side of rift: */
	tria1=(Tria*)elements[0];
	tria2=(Tria*)elements[1];

	/*First recover velocity: */
	tria1->inputs->GetParameterValueAtNode(&vx1,nodes[0],VxEnum);
	tria2->inputs->GetParameterValueAtNode(&vx2,nodes[1],VxEnum);
	tria1->inputs->GetParameterValueAtNode(&vy1,nodes[0],VyEnum);
	tria2->inputs->GetParameterValueAtNode(&vy2,nodes[1],VyEnum);

	/*Node 1 faces node 2, compute penetration of 2 into 1 (V2-V1).N (with N normal vector, and V velocity vector: */
	penetration=(vx2-vx1)*normal[0]+(vy2-vy1)*normal[1];

	/*Ok, we are looking for positive penetration in an active constraint: */
	if(this->active){
		if (penetration>=0){
			unstable=1;
		}
		else{
			unstable=0;
		}
	}
	else{
		unstable=0;
	}

	/*assign output pointer: */
	*punstable=unstable;
}
/*}}}1*/
/*FUNCTION Riftfront::PreConstrain {{{1*/
int   Riftfront::PreConstrain(int* punstable,  int analysis_type){

	const int   numgrids    = 2;
	double      penetration;
	int         unstable;
	int         found;
	double      vx1;
	double      vy1;
	double      vx2;
	double      vy2;


	/*Objects: */
	Element   **elements    = NULL;
	Node      **nodes       = NULL;
	Tria       *tria1       = NULL;
	Tria       *tria2       = NULL;

	/*Recover hook objects: */
	elements=(Element**)helements.deliverp();
	nodes=(Node**)hnodes.deliverp();

	/*enum of element? */
	if(elements[0]->Enum()!=TriaEnum)ISSMERROR(" only Tria element allowed for Riftfront load!");

	/*recover elements on both side of rift: */
	tria1=(Tria*)elements[0];
	tria2=(Tria*)elements[1];

	/*First recover velocity: */
	tria1->inputs->GetParameterValueAtNode(&vx1,nodes[0],VxEnum);
	tria2->inputs->GetParameterValueAtNode(&vx2,nodes[1],VxEnum);
	tria1->inputs->GetParameterValueAtNode(&vy1,nodes[0],VyEnum);
	tria2->inputs->GetParameterValueAtNode(&vy2,nodes[1],VyEnum);

	/*Node 1 faces node 2, compute penetration of 2 into 1 (V2-V1).N (with N normal vector, and V velocity vector: */
	penetration=(vx2-vx1)*normal[0]+(vy2-vy1)*normal[1];

	/*Ok, we are preconstraining here. Ie, anything that penetrates is constrained until stability of the entire set 
	 * of constraints is reached.: */
	if(penetration<0){
		if (!this->active){
			/*This is the first time penetration happens: */
			this->active=1;
			unstable=1;
		}
		else{
			/*This constraint was already active: */
			this->active=1;
			unstable=0;
		}
	}
	else{
		/*No penetration happening. : */
		if (!this->active){
			/*This penalty was not active, and no penetration happening. Do nonthing: */
			this->active=0;
			unstable=0; 
		}
		else{
			/*Ok, this penalty wants to get released. But not now, this is preconstraint, not constraint: */
			this->active=1;
			unstable=0;
		}
	}

	/*assign output pointer: */
	*punstable=unstable;
}
/*}}}1*/
/*FUNCTION Riftfront::PreStable {{{1*/
bool  Riftfront::PreStable(){
	return prestable;
}
/*}}}1*/
/*FUNCTION Riftfront::SetPreStable {{{1*/
void Riftfront::SetPreStable(){
	prestable=1;
}
/*}}}1*/