/*!\file Riftfront.cpp
 * \brief: implementation of the Riftfront 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 <string.h>
#include "../EnumDefinitions/EnumDefinitions.h"
#include "../shared/shared.h"
#include "../include/typedefs.h"
#include "../include/macros.h"
#include "./objects.h"

		
/*Object constructors and destructor*/
/*FUNCTION Riftfront::default constructor {{{1*/
Riftfront::Riftfront(){
	/*in case :*/
	material_converged=0;
	return;
}
/*}}}1*/
/*FUNCTION Riftfront::constructor {{{1*/
Riftfront::Riftfront(char riftfront_type[RIFTFRONTSTRING],int riftfront_id, int riftfront_node_ids[MAX_RIFTFRONT_GRIDS], int riftfront_mparid, double riftfront_h[MAX_RIFTFRONT_GRIDS],double riftfront_b[MAX_RIFTFRONT_GRIDS],double riftfront_s[MAX_RIFTFRONT_GRIDS],double riftfront_normal[2],double riftfront_length,int riftfront_fill,double riftfront_friction, double riftfront_fraction,double riftfront_fractionincrement, double riftfront_penalty_offset, int riftfront_penalty_lock, bool riftfront_active,bool riftfront_frozen, int riftfront_counter,bool riftfront_prestable,bool riftfront_shelf){

	this->Init(riftfront_type,riftfront_id, riftfront_node_ids, riftfront_mparid, riftfront_h,riftfront_b,riftfront_s,riftfront_normal,riftfront_length,riftfront_fill,riftfront_friction, riftfront_fraction,riftfront_fractionincrement, riftfront_penalty_offset, riftfront_penalty_lock, riftfront_active,riftfront_frozen, riftfront_counter,riftfront_prestable,riftfront_shelf);

}
/*}}}1*/
/*FUNCTION Riftfront::constructor from iomodel{{{1*/
Riftfront::Riftfront(int i, IoModel* iomodel){

	/*rifts: */
	char riftfront_type[RIFTFRONTSTRING];
	int  riftfront_id;
	int  riftfront_node_ids[2];
	int  riftfront_mparid;
	double riftfront_h[2];
	double riftfront_b[2];
	double riftfront_s[2];
	double riftfront_normal[2];
	double riftfront_length;
	int    riftfront_fill;
	double riftfront_friction;
	double riftfront_fraction;
	double riftfront_fractionincrement;
	bool   riftfront_shelf;
	double riftfront_penalty_offset;
	int riftfront_penalty_lock;
	bool riftfront_active;
	bool riftfront_frozen;
	int  riftfront_counter;
	bool riftfront_prestable;
	int el1,el2;
	int grid1,grid2;
	double normal[2];
	double length;
	int    fill;
	double friction;
	double fraction;
	double fractionincrement;

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

	normal[0]=*(iomodel->riftinfo+RIFTINFOSIZE*i+4);
	normal[1]=*(iomodel->riftinfo+RIFTINFOSIZE*i+5);
	length=*(iomodel->riftinfo+RIFTINFOSIZE*i+6);

	fill = (int)*(iomodel->riftinfo+RIFTINFOSIZE*i+7);
	friction=*(iomodel->riftinfo+RIFTINFOSIZE*i+8);
	fraction=*(iomodel->riftinfo+RIFTINFOSIZE*i+9);
	fractionincrement=*(iomodel->riftinfo+RIFTINFOSIZE*i+10);

	strcpy(riftfront_type,"2d");
	riftfront_id=i+1; //matlab indexing
	riftfront_node_ids[0]=grid1;
	riftfront_node_ids[1]=grid2;
	riftfront_mparid=iomodel->numberofelements+1; //matlab indexing

	riftfront_h[0]=iomodel->thickness[grid1-1];
	riftfront_h[1]=iomodel->thickness[grid2-1];

	riftfront_b[0]=iomodel->bed[grid1-1];
	riftfront_b[1]=iomodel->bed[grid2-1];

	riftfront_s[0]=iomodel->surface[grid1-1];
	riftfront_s[1]=iomodel->surface[grid2-1];

	riftfront_normal[0]=normal[0];
	riftfront_normal[1]=normal[1];
	riftfront_length=length;

	riftfront_fill=fill;
	riftfront_friction=friction;
	riftfront_fraction=fraction;
	riftfront_fractionincrement=fractionincrement;
	riftfront_shelf=(bool)iomodel->gridoniceshelf[grid1-1];

	riftfront_penalty_offset=iomodel->penalty_offset;
	riftfront_penalty_lock=iomodel->penalty_lock;

	riftfront_active=0;
	riftfront_frozen=0;
	riftfront_counter=0;
	riftfront_prestable=0;
			
	this->Init(riftfront_type,riftfront_id, riftfront_node_ids, riftfront_mparid, riftfront_h,riftfront_b,riftfront_s,riftfront_normal,riftfront_length,riftfront_fill,riftfront_friction, riftfront_fraction,riftfront_fractionincrement, riftfront_penalty_offset, riftfront_penalty_lock, riftfront_active,riftfront_frozen, riftfront_counter,riftfront_prestable,riftfront_shelf);

}
/*}}}1*/
/*FUNCTION Riftfront::Init, used by constructor {{{1*/
void Riftfront::Init(char riftfront_type[RIFTFRONTSTRING],int riftfront_id, int riftfront_node_ids[MAX_RIFTFRONT_GRIDS], int riftfront_mparid, double riftfront_h[MAX_RIFTFRONT_GRIDS],double riftfront_b[MAX_RIFTFRONT_GRIDS],double riftfront_s[MAX_RIFTFRONT_GRIDS],double riftfront_normal[2],double riftfront_length,int riftfront_fill,double riftfront_friction, double riftfront_fraction,double riftfront_fractionincrement, double riftfront_penalty_offset, int riftfront_penalty_lock, bool riftfront_active,bool riftfront_frozen, int riftfront_counter,bool riftfront_prestable,bool riftfront_shelf){

	int i;
	
	strcpy(type,riftfront_type);
	id=riftfront_id;
	
	for(i=0;i<MAX_RIFTFRONT_GRIDS;i++){
		node_ids[i]=riftfront_node_ids[i];
		node_offsets[i]=UNDEF;
		nodes[i]=NULL;
	}
	
	mparid=riftfront_mparid;
	matpar=NULL;
	matpar_offset=UNDEF;

	for(i=0;i<MAX_RIFTFRONT_GRIDS;i++){
		h[i]=riftfront_h[i];
		b[i]=riftfront_b[i];
		s[i]=riftfront_s[i];
	}

	normal[0]=riftfront_normal[0];
	normal[1]=riftfront_normal[1];
	length=riftfront_length;
	fill=riftfront_fill;
	friction=riftfront_friction;
	fraction=riftfront_fraction;
	fractionincrement=riftfront_fractionincrement;
	penalty_offset=riftfront_penalty_offset;
	penalty_lock=riftfront_penalty_lock;
	active=riftfront_active;
	frozen=riftfront_frozen;
	counter=riftfront_counter;
	prestable=riftfront_prestable;
	shelf=riftfront_shelf;

	/*not in the constructor, but still needed: */
	material_converged=0;

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

	char* marshalled_dataset=NULL;
	int   enum_type=0;

	/*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,&type,sizeof(type));marshalled_dataset+=sizeof(type);
	memcpy(marshalled_dataset,&id,sizeof(id));marshalled_dataset+=sizeof(id);
	memcpy(marshalled_dataset,&node_ids,sizeof(node_ids));marshalled_dataset+=sizeof(node_ids);
	memcpy(marshalled_dataset,&node_offsets,sizeof(node_offsets));marshalled_dataset+=sizeof(node_offsets);
	memcpy(marshalled_dataset,&mparid,sizeof(mparid));marshalled_dataset+=sizeof(mparid);
	memcpy(marshalled_dataset,&matpar_offset,sizeof(matpar_offset));marshalled_dataset+=sizeof(matpar_offset);

	memcpy(marshalled_dataset,&h,sizeof(h));marshalled_dataset+=sizeof(h);
	memcpy(marshalled_dataset,&b,sizeof(b));marshalled_dataset+=sizeof(b);
	memcpy(marshalled_dataset,&s,sizeof(s));marshalled_dataset+=sizeof(s);
	
	memcpy(marshalled_dataset,&normal,sizeof(normal));marshalled_dataset+=sizeof(normal);
	memcpy(marshalled_dataset,&length,sizeof(length));marshalled_dataset+=sizeof(length);
	memcpy(marshalled_dataset,&fill,sizeof(fill));marshalled_dataset+=sizeof(fill);
	memcpy(marshalled_dataset,&friction,sizeof(friction));marshalled_dataset+=sizeof(friction);
	memcpy(marshalled_dataset,&fraction,sizeof(fraction));marshalled_dataset+=sizeof(fraction);
	memcpy(marshalled_dataset,&fractionincrement,sizeof(fractionincrement));marshalled_dataset+=sizeof(fractionincrement);
	memcpy(marshalled_dataset,&penalty_offset,sizeof(penalty_offset));marshalled_dataset+=sizeof(penalty_offset);
	memcpy(marshalled_dataset,&penalty_lock,sizeof(penalty_lock));marshalled_dataset+=sizeof(penalty_lock);
	memcpy(marshalled_dataset,&active,sizeof(active));marshalled_dataset+=sizeof(active);
	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,&material_converged,sizeof(material_converged));marshalled_dataset+=sizeof(material_converged);
	memcpy(marshalled_dataset,&shelf,sizeof(shelf));marshalled_dataset+=sizeof(shelf);

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

	return sizeof(type)+
		sizeof(id)+
		sizeof(node_ids)+
		sizeof(node_offsets)+
		sizeof(mparid)+
		sizeof(matpar_offset)+
		sizeof(h)+
		sizeof(b)+
		sizeof(s)+
		sizeof(normal)+
		sizeof(length)+
		sizeof(fill)+
		sizeof(friction)+
		sizeof(fraction)+
		sizeof(fractionincrement)+
		sizeof(penalty_offset)+
		sizeof(penalty_lock)+
		sizeof(active)+
		sizeof(frozen)+
		sizeof(counter)+
		sizeof(prestable)+
		sizeof(material_converged)+
		sizeof(shelf)+
		sizeof(int); //sizeof(int) for enum type
}
/*}}}1*/
/*FUNCTION Riftfront::Demarshall {{{1*/
void  Riftfront::Demarshall(char** pmarshalled_dataset){

	int i;
	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(&type,marshalled_dataset,sizeof(type));marshalled_dataset+=sizeof(type);
	memcpy(&id,marshalled_dataset,sizeof(id));marshalled_dataset+=sizeof(id);
	memcpy(&node_ids,marshalled_dataset,sizeof(node_ids));marshalled_dataset+=sizeof(node_ids);
	memcpy(&node_offsets,marshalled_dataset,sizeof(node_offsets));marshalled_dataset+=sizeof(node_offsets);
	memcpy(&mparid,marshalled_dataset,sizeof(mparid));marshalled_dataset+=sizeof(mparid);
	memcpy(&matpar_offset,marshalled_dataset,sizeof(matpar_offset));marshalled_dataset+=sizeof(matpar_offset);

	memcpy(&h,marshalled_dataset,sizeof(h));marshalled_dataset+=sizeof(h);
	memcpy(&b,marshalled_dataset,sizeof(b));marshalled_dataset+=sizeof(b);
	memcpy(&s,marshalled_dataset,sizeof(s));marshalled_dataset+=sizeof(s);
	memcpy(&normal,marshalled_dataset,sizeof(normal));marshalled_dataset+=sizeof(normal);
	memcpy(&length,marshalled_dataset,sizeof(length));marshalled_dataset+=sizeof(length);
	memcpy(&fill,marshalled_dataset,sizeof(fill));marshalled_dataset+=sizeof(fill);
	memcpy(&friction,marshalled_dataset,sizeof(friction));marshalled_dataset+=sizeof(friction);
	memcpy(&fraction,marshalled_dataset,sizeof(fraction));marshalled_dataset+=sizeof(fraction);
	memcpy(&fractionincrement,marshalled_dataset,sizeof(fractionincrement));marshalled_dataset+=sizeof(fractionincrement);
	memcpy(&penalty_offset,marshalled_dataset,sizeof(penalty_offset));marshalled_dataset+=sizeof(penalty_offset);
	memcpy(&penalty_lock,marshalled_dataset,sizeof(penalty_lock));marshalled_dataset+=sizeof(penalty_lock);
	memcpy(&active,marshalled_dataset,sizeof(active));marshalled_dataset+=sizeof(active);
	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(&material_converged,marshalled_dataset,sizeof(material_converged));marshalled_dataset+=sizeof(material_converged);
	memcpy(&shelf,marshalled_dataset,sizeof(shelf));marshalled_dataset+=sizeof(shelf);

	for(i=0;i<MAX_RIFTFRONT_GRIDS;i++)nodes[i]=NULL;
	matpar=NULL;

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

/*Object functions*/
/*FUNCTION Riftfront::Configure {{{1*/
void  Riftfront::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**)nodes,node_ids,node_offsets,MAX_RIFTFRONT_GRIDS,nodesin);
	
	/*Same for materials: */
	ResolvePointers((Object**)&matpar,&mparid,&matpar_offset,1,materialsin);

}
/*}}}1*/
/*FUNCTION Riftfront::Constrain {{{1*/
#define _ZIGZAGCOUNTER_

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

	const int     numgrids=2;
	int           dofs[2]={0,1};
	double        vxvy_list[2][2]; //velocities for all grids 
	double        max_penetration;
	double        penetration;
	int           activate;
	int           found;
	int           unstable;

	ParameterInputs* inputs=NULL;

	inputs=(ParameterInputs*)vinputs;

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

	/*First recover parameter inputs: */
	found=inputs->Recover("velocity",&vxvy_list[0][0],2,dofs,numgrids,(void**)nodes);
	if(!found)ISSMERROR(" could not find velocity in inputs!");


	/*Grid 1 faces grid2, compute penetration of 2 into 1 (V2-V1).N (with N normal vector, and V velocity vector: */
	penetration=(vxvy_list[1][0]-vxvy_list[0][0])*normal[0]+(vxvy_list[1][1]-vxvy_list[0][1])*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+=this->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::copy {{{1*/
Object* Riftfront::copy() {
	return new Riftfront(*this); 
}
/*}}}1*/
/*FUNCTION Riftfront::CreateKMatrix {{{1*/
void  Riftfront::CreateKMatrix(Mat Kgg,void* inputs,int analysis_type,int sub_analysis_type){
	/*do nothing: */
}
/*}}}1*/
/*FUNCTION Riftfront::CreatePVector {{{1*/
void  Riftfront::CreatePVector(Vec pg, void* inputs, int analysis_type,int sub_analysis_type){
	/*do nothing: */
}
/*}}}1*/
/*FUNCTION Riftfront::DeepEcho {{{1*/
void Riftfront::DeepEcho(void){

	int i;

	printf("Riftfront:\n");
	printf("   type: %s\n",type);
	printf("   id: %i\n",id);

	printf("   node_ids: ");
	for(i=0;i<MAX_RIFTFRONT_GRIDS;i++)printf("%i ",node_ids[i]);
	for(i=0;i<MAX_RIFTFRONT_GRIDS;i++){
		if (nodes[i])nodes[i]->Echo();
	}
	printf("\n");
	
	printf("   mparid: %i\n",mparid);
	if(matpar)matpar->Echo();

	printf("normal [%g,%g], length %g\n",normal[0],normal[1],normal[2]);
	printf("fill: %i friction %g fraction %g fractionincrement %g \n",fill,friction,fraction,fractionincrement);
	printf("penalty_offset %g\n",penalty_offset);
	printf("penalty_lock %i\n",penalty_lock);
	printf("active %i\n",active);
	printf("frozen %i\n",frozen);
	printf("counter %i\n",counter);
	printf("prestable %i\n",prestable);
	printf("material_converged %i\n",material_converged);
	printf("shelf %i\n",shelf);
	
}		
/*}}}1*/
/*FUNCTION Riftfront::Echo {{{1*/
void Riftfront::Echo(void){

	int i;
	
	printf("Riftfront:\n");
	printf("   type: %s\n",type);
	printf("   id: %i\n",id);
	printf("   mparid: %i\n",mparid);

	printf("   node_ids: ");
	for(i=0;i<MAX_RIFTFRONT_GRIDS;i++)printf("%i ",node_ids[i]);
	printf("\n");

	printf("normal [%g,%g], length %g\n",normal[0],normal[1],normal[2]);
	printf("fill: %i friction %g fraction %g fractionincrement %g \n",fill,friction,fraction,fractionincrement);
	printf("penalty_offset %g\n",penalty_offset);
	printf("penalty_lock %i\n",penalty_lock);
	printf("active %i\n",active);
	printf("frozen %i\n",frozen);
	printf("counter %i\n",counter);
	printf("prestable %i\n",prestable);
	printf("material_converged %i\n",material_converged);
	printf("shelf %i\n",shelf);
}
/*}}}1*/
/*FUNCTION Riftfront::Enum {{{1*/
int Riftfront::Enum(void){

	return RiftfrontEnum();

}
/*}}}1*/
/*FUNCTION Riftfront::FreezeConstraints{{{1*/
void   Riftfront::FreezeConstraints(void* vinputs, 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;
	
	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::GetId {{{1*/
int    Riftfront::GetId(void){ return id; }
/*}}}1*/
/*FUNCTION Riftfront::GetName {{{1*/
char* Riftfront::GetName(void){
	return "riftfront";
}
/*}}}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(void* vinputs, int analysis_type){

	int found=0;
	ParameterInputs* inputs=NULL;
	double converged=0;

	inputs=(ParameterInputs*)vinputs;

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

	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, void* vinputs, int analysis_type){

	const int     numgrids=2;
	int           dofs[2]={0,1};
	double        vxvy_list[2][2]; //velocities for all grids 
	double        max_penetration;
	double        penetration=0;
	int           found;

	ParameterInputs* inputs=NULL;

	inputs=(ParameterInputs*)vinputs;

	//initialize: 
	penetration=-1;

	found=inputs->Recover("velocity",&vxvy_list[0][0],2,dofs,numgrids,(void**)nodes);
	if(!found)ISSMERROR(" could not find velocity in inputs!");

	/*Grid 1 faces grid2, compute penetration of 2 into 1 (V2-V1).N (with N normal vector, and V velocity vector: */
	penetration=(vxvy_list[1][0]-vxvy_list[0][0])*normal[0]+(vxvy_list[1][1]-vxvy_list[0][1])*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::MyRank {{{1*/
int    Riftfront::MyRank(void){ 
	extern int my_rank;
	return my_rank; 
}
/*}}}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,void* vinputs,double kmax,int analysis_type,int sub_analysis_type){

	int i,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;
	ParameterInputs* inputs=NULL;
	
	/*Some pointer intialization: */
	inputs=(ParameterInputs*)vinputs;

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


	if(this->active){
	
		/*There is contact, we need to constrain the normal velocities (zero penetration), and the 
		 *contact slip friction. */
		  
		#ifdef _ISSM_DEBUG_
		printf("Dealing with grid pair (%i,%i)\n",nodes[0]->GetId(),nodes[1]->GetId());
		#endif

		/*Recover input parameters: */
		inputs->Recover("thickness",&h[0],1,dofs,MAX_RIFTFRONT_GRIDS,(void**)nodes);
		if (h[0]!=h[1])ISSMERROR(" different thicknesses not supported for rift fronts");
		thickness=h[0];

		#ifdef _ISSM_DEBUG_
			printf("Thickness at grid (%i,%i): %lg\n",nodes[0]->GetId(),nodes[1]->GetID(),thickness);
		#endif

		/*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,void* vinputs,double kmax,int analysis_type,int sub_analysis_type){

	int          i,j;
	const int    numgrids=MAX_RIFTFRONT_GRIDS;
	int          dofs[1]={0};
	double       pe_g[4];
	const int    numdof=2*numgrids;
	int          doflist[numdof];
	int          numberofdofspernode;
	ParameterInputs* inputs=NULL;
	double       rho_ice;
	double       rho_water;
	double       gravity;
	double       thickness;
	double       bed;
	double       pressure;
	double       pressure_litho;
	double       pressure_air;
	double       pressure_melange;
	double       pressure_water;
	
	/*Some pointer intialization: */
	inputs=(ParameterInputs*)vinputs;

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

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

	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.:*/
	
		#ifdef _ISSM_DEBUG_
		_printf_("Grids  (%i,%i) are free of constraints\n",nodes[0]->GetId(),nodes[1]->GetID());
		#endif

		/*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: */
		inputs->Recover("thickness",&h[0],1,dofs,MAX_RIFTFRONT_GRIDS,(void**)nodes);
		if (h[0]!=h[1])ISSMERROR(" different thicknesses not supported for rift fronts");
		thickness=h[0];

		inputs->Recover("bed",&b[0],1,dofs,MAX_RIFTFRONT_GRIDS,(void**)nodes);
		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(exprintf("%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(exprintf("%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, void* vinputs, int analysis_type){

	const int     numgrids=2;
	int           dofs[2]={0,1};
	double        vxvy_list[2][2]; //velocities for all grids 
	double        max_penetration;
	double        penetration;
	int           found;

	ParameterInputs* inputs=NULL;

	inputs=(ParameterInputs*)vinputs;


	found=inputs->Recover("velocity",&vxvy_list[0][0],2,dofs,numgrids,(void**)nodes);
	if(!found)ISSMERROR(" could not find velocity in inputs!");

	/*Grid 1 faces grid2, compute penetration of 2 into 1 (V2-V1).N (with N normal vector, and V velocity vector: */
	penetration=(vxvy_list[1][0]-vxvy_list[0][0])*normal[0]+(vxvy_list[1][1]-vxvy_list[0][1])*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, void* vinputs, int analysis_type){


	const int     numgrids=2;
	int           dofs[2]={0,1};
	double        vxvy_list[2][2]; //velocities for all grids 
	double        max_penetration;
	double        penetration;
	int           activate;
	int           unstable;
	int           found;

	ParameterInputs* inputs=NULL;

	inputs=(ParameterInputs*)vinputs;

	found=inputs->Recover("velocity",&vxvy_list[0][0],2,dofs,numgrids,(void**)nodes);
	if(!found)ISSMERROR(" could not find velocity in inputs!");

	/*Grid 1 faces grid2, compute penetration of 2 into 1 (V2-V1).N (with N normal vector, and V velocity vector: */
	penetration=(vxvy_list[1][0]-vxvy_list[0][0])*normal[0]+(vxvy_list[1][1]-vxvy_list[0][1])*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, void* vinputs, int analysis_type){

	const int     numgrids=2;
	int           dofs[2]={0,1};
	double        vxvy_list[2][2]; //velocities for all grids 
	double        penetration;
	int           unstable;
	ParameterInputs* inputs=NULL;
	int           found;

	inputs=(ParameterInputs*)vinputs;

	/*First recover velocity: */
	found=inputs->Recover("velocity",&vxvy_list[0][0],2,dofs,numgrids,(void**)nodes);
	if(!found)ISSMERROR(" could not find velocity in inputs!");
	
	/*Grid 1 faces grid2, compute penetration of 2 into 1 (V2-V1).N (with N normal vector, and V velocity vector: */
	penetration=(vxvy_list[1][0]-vxvy_list[0][0])*normal[0]+(vxvy_list[1][1]-vxvy_list[0][1])*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*/
/*FUNCTION Riftfront::SetClone {{{1*/
void  Riftfront::SetClone(int* minranks){

	ISSMERROR("not implemented yet");
}
/*}}}1*/
/*FUNCTION Riftfront::UpdateFromInputs {{{1*/
void  Riftfront::UpdateFromInputs(void* vinputs){

	int  dofs[1]={0};
	ParameterInputs* inputs=NULL;	
	
	inputs=(ParameterInputs*)vinputs;

	inputs->Recover("thickness",&h[0],1,dofs,MAX_RIFTFRONT_GRIDS,(void**)nodes);
	inputs->Recover("bed",&b[0],1,dofs,MAX_RIFTFRONT_GRIDS,(void**)nodes);
	inputs->Recover("surface",&s[0],1,dofs,MAX_RIFTFRONT_GRIDS,(void**)nodes);

}
/*}}}1*/