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

/*Element macros*/
#define NUMVERTICES 2

/*Riftfront constructors and destructor*/
/*FUNCTION Riftfront::Riftfront(){{{*/
Riftfront::Riftfront(){
	this->inputs=NULL;
	this->parameters=NULL;
	this->hnodes=NULL;
	this->helements=NULL;
	this->hmatpar=NULL;
	this->nodes=NULL;
	this->elements=NULL;
	this->matpar=NULL;
}
/*}}}*/
/*FUNCTION Riftfront::Riftfront(int id, int i, IoModel* iomodel,int analysis_type){{{*/
Riftfront::Riftfront(int riftfront_id,int i, IoModel* iomodel,int riftfront_analysis_type){

	/*data: */
	int    riftfront_node_ids[2];
	int    riftfront_elem_ids[2];
	int    riftfront_matpar_id;
	int    riftfront_type;
	int    riftfront_fill;
	IssmDouble riftfront_friction;
	IssmDouble riftfront_fractionincrement;
	bool   riftfront_shelf;
	int    numberofelements;
	int    penalty_lock;

	/*intermediary: */
	int el1    ,el2;
	int node1  ,node2;

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

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

	node1=(int)*(iomodel->Data(RiftsRiftstructEnum)+RIFTINFOSIZE*i+0); 
	node2=(int)*(iomodel->Data(RiftsRiftstructEnum)+RIFTINFOSIZE*i+1);

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

	/*hooks: */
	riftfront_node_ids[0]=iomodel->nodecounter+node1;
	riftfront_node_ids[1]=iomodel->nodecounter+node2;
	riftfront_elem_ids[0]=el1;
	riftfront_elem_ids[1]=el2;
	riftfront_matpar_id=numberofelements+1; //matlab indexing

	/*Hooks: */
	this->hnodes=new Hook(riftfront_node_ids,2);
	this->helements=new Hook(riftfront_elem_ids,2);
	this->hmatpar=new Hook(&riftfront_matpar_id,1);

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

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

	this->inputs->AddInput(new IntInput(TypeEnum,riftfront_type));
	this->inputs->AddInput(new IntInput(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));
	
	//parameters and hooked fields: we still can't point to them, they may not even exist. Configure will handle this.
	this->parameters=NULL;
	this->nodes= NULL;
	this->elements= NULL;
	this->matpar= NULL;
		
}
/*}}}*/
/*FUNCTION Riftfront::~Riftfront(){{{*/
Riftfront::~Riftfront(){
	delete inputs;
	this->parameters=NULL;

	delete hnodes;
	delete helements;
	delete hmatpar;
}
/*}}}*/

/*Object virtual functions definitions:*/
/*FUNCTION Riftfront::Echo {{{*/
void Riftfront::Echo(void){

	Input* input=NULL;
	int fill;
	IssmDouble friction,fractionincrement;

	
	/*recover some inputs first: */
	input=(Input*)this->inputs->GetInput(FillEnum); input->GetInputValue(&fill);
	input=(Input*)this->inputs->GetInput(FrictionEnum); input->GetInputValue(&friction);
	input=(Input*)this->inputs->GetInput(FractionIncrementEnum); input->GetInputValue(&fractionincrement);

	_printLine_("Riftfront:");
	_printLine_("   id: " << id);
	_printLine_("   analysis_type: " << EnumToStringx(analysis_type));
	_printLine_("   hnodes: " << hnodes);
	_printLine_("   helements: " << helements);
	_printLine_("   hmatpar: " << hmatpar);
	_printLine_("   parameters: " << parameters);
	_printLine_("   inputs: " << inputs);
	_printLine_("   internal parameters: ");
	_printLine_("   normal: " << normal[0] << "|" << normal[1]);
	_printLine_("   length: " << length);
	_printLine_("   penalty_lock: " << penalty_lock);
	_printLine_("   active: " <<(active ? "true":"false"));
	_printLine_("   counter: " << counter);
	_printLine_("   prestable: " << (prestable ? "true":"false"));
	_printLine_("   material_converged: " << (material_converged ? "true":"false"));
	_printLine_("   fill: " << fill);
	_printLine_("   friction: " << friction);
	_printLine_("   fraction: " << fraction);
	_printLine_("   fractionincrement: " << fractionincrement);
	_printLine_("   state: " << state);
	_printLine_("   frozen: " << (frozen ? "true":"false"));
		
}
/*}}}*/
/*FUNCTION Riftfront::DeepEcho{{{*/
void Riftfront::DeepEcho(void){

	_printLine_("Riftfront:");
	_printLine_("   id: " << id);
	_printLine_("   analysis_type: " << EnumToStringx(analysis_type));
	hnodes->DeepEcho();
	helements->DeepEcho();
	hmatpar->DeepEcho();
	_printLine_("   parameters");
	if(parameters)parameters->DeepEcho();
	_printLine_("   inputs");
	if(inputs)inputs->DeepEcho();
}
/*}}}*/
/*FUNCTION Riftfront::Id {{{*/
int    Riftfront::Id(void){ return id; }
/*}}}*/
/*FUNCTION Riftfront::MyRank {{{*/
int    Riftfront::MyRank(void){ 
	extern int my_rank;
	return my_rank; 
}
/*}}}*/
/*FUNCTION Riftfront::ObjectEnum{{{*/
int Riftfront::ObjectEnum(void){

	return RiftfrontEnum;

}
/*}}}*/
/*FUNCTION Riftfront::copy {{{*/
Object* Riftfront::copy() {
	
	Riftfront* riftfront=NULL;

	riftfront=new Riftfront();

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

	/*now deal with hooks and objects: */
	riftfront->hnodes=(Hook*)this->hnodes->copy();
	riftfront->helements=(Hook*)this->helements->copy();
	riftfront->hmatpar=(Hook*)this->hmatpar->copy();

	/*corresponding fields*/
	riftfront->nodes   =(Node**)riftfront->hnodes->deliverp();
	riftfront->elements=(Element**)riftfront->helements->deliverp();
	riftfront->matpar  =(Matpar*)riftfront->hmatpar->delivers();

	/*internal data: */
	riftfront->penalty_lock=this->penalty_lock;
	riftfront->active=this->active;
	riftfront->frozen=this->frozen;
	riftfront->state=this->state;
	riftfront->counter=this->counter;
	riftfront->prestable=this->prestable;
	riftfront->material_converged=this->material_converged;
	riftfront->normal[0]=this->normal[0];
	riftfront->normal[1]=this->normal[1];
	riftfront->length=this->length;
	riftfront->fraction=this->fraction;
	
	return riftfront;

}
/*}}}*/
		
/*Update virtual functions definitions:*/
/*FUNCTION Riftfront::InputUpdateFromConstant(bool constant,int name) {{{*/
void  Riftfront::InputUpdateFromConstant(bool constant,int name){

	/*Check that name is a Riftfront input*/
	if (!IsInput(name)) return;

	/*update input*/
	this->inputs->AddInput(new BoolInput(name,constant));

}
/*}}}*/
/*FUNCTION Riftfront::InputUpdateFromConstant(IssmDouble constant,int name) {{{*/
void  Riftfront::InputUpdateFromConstant(IssmDouble constant,int name){

	/*Check that name is a Riftfront input*/
	if (!IsInput(name)) return;

	/*update input*/
	this->inputs->AddInput(new DoubleInput(name,constant));

}
/*}}}*/
/*FUNCTION Riftfront::InputUpdateFromConstant(IssmDouble* constant,int name) {{{*/
void    Riftfront::InputUpdateFromVector(IssmDouble* vector, int name, int type){

	/*Check that name is a Riftfront input*/
	if (!IsInput(name)) return;

	/*update input*/
	_error2_("not implemented yet");
	//this->inputs->AddInput(new DoubleInput(name,constant));

}
/*}}}*/


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

	/*Initialize hooked fields*/
	this->nodes   =(Node**)hnodes->deliverp();
	this->elements=(Element**)helements->deliverp();
	this->matpar  =(Matpar*)hmatpar->delivers();

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

}
/*}}}*/
/*FUNCTION Riftfront::SetCurrentConfiguration {{{*/
void  Riftfront::SetCurrentConfiguration(Elements* elementsin,Loads* loadsin,Nodes* nodesin,Vertices* verticesin,Materials* materialsin,Parameters* parametersin){

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

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

	switch(analysis_type){
		case DiagnosticHorizAnalysisEnum:
			Ke=PenaltyCreateKMatrixDiagnosticHoriz(kmax);
			break;
		case AdjointHorizAnalysisEnum:
			Ke=PenaltyCreateKMatrixDiagnosticHoriz(kmax);
			break;
		default:
			_error2_("analysis " << analysis_type << " (" << EnumToStringx(analysis_type) << ") not supported yet");
	}

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

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

	switch(analysis_type){
		case DiagnosticHorizAnalysisEnum:
			pe=PenaltyCreatePVectorDiagnosticHoriz(kmax);
			break;
		case AdjointHorizAnalysisEnum:
			/*No penalty applied on load vector*/
			break;
		default:
			_error2_("analysis " << analysis_type << " (" << EnumToStringx(analysis_type) << ") not supported yet");
	}

	/*Add to global Vector*/
	if(pe){
		pe->AddToGlobal(pf);
		delete pe;
	}
}
/*}}}*/
/*FUNCTION Riftfront::CreateKMatrix {{{*/
void  Riftfront::CreateKMatrix(Matrix* Kff, Matrix* Kfs){
	/*do nothing: */
	return;
}
/*}}}*/
/*FUNCTION Riftfront::CreatePVector {{{*/
void  Riftfront::CreatePVector(Vector* pf){
	/*do nothing: */
	return;
}
/*}}}*/
/*FUNCTION Riftfront::InAnalysis{{{*/
bool Riftfront::InAnalysis(int in_analysis_type){
	if (in_analysis_type==this->analysis_type) return true;
	else return false;
}
/*}}}*/

/*Riftfront numerics*/
/*FUNCTION Riftfront::PenaltyCreateKMatrixDiagnosticHoriz {{{*/
ElementMatrix* Riftfront::PenaltyCreateKMatrixDiagnosticHoriz(IssmDouble kmax){

	const int   numdof = NDOF2*NUMVERTICES;
	int         i,j;
	int         dofs[1]             = {0};
	IssmDouble      Ke_gg[4][4];
	IssmDouble      thickness;
	IssmDouble      h[2];
	IssmDouble      penalty_offset;
	IssmDouble      friction;

	/*Objects: */
	Tria       *tria1               = NULL;
	Tria       *tria2               = NULL;

	/*enum of element? */
	if(elements[0]->ObjectEnum()!=TriaEnum)_error2_("only Tria element allowed for Riftfront load!");
	tria1=(Tria*)elements[0];
	tria2=(Tria*)elements[1];

	/*Initialize Element Matrix*/
	if(!this->active) return NULL;
	ElementMatrix* Ke=new ElementMatrix(nodes,NUMVERTICES,this->parameters);

	/*Get some parameters: */
	this->parameters->FindParam(&penalty_offset,DiagnosticPenaltyFactorEnum);
	this->inputs->GetInputValue(&friction,FrictionEnum);
	tria1->GetInputValue(&h[0],nodes[0],ThicknessEnum);
	tria2->GetInputValue(&h[1],nodes[1],ThicknessEnum);
	if (h[0]!=h[1])_error2_("different thicknesses not supported for rift fronts");
	thickness=h[0];

	/*There is contact, we need to constrain the normal velocities (zero penetration), and the 
	 *contact slip friction. */

	/*From Peter Wriggers book (Computational Contact Mechanics, p191): */
	Ke->values[0*numdof+0]+= +pow(normal[0],2)*kmax*pow(10,penalty_offset);
	Ke->values[0*numdof+1]+= +normal[0]*normal[1]*kmax*pow(10,penalty_offset);
	Ke->values[0*numdof+2]+= -pow(normal[0],2)*kmax*pow(10,penalty_offset);
	Ke->values[0*numdof+3]+= -normal[0]*normal[1]*kmax*pow(10,penalty_offset);

	Ke->values[1*numdof+0]+= +normal[0]*normal[1]*kmax*pow(10,penalty_offset);
	Ke->values[1*numdof+1]+= +pow(normal[1],2)*kmax*pow(10,penalty_offset);
	Ke->values[1*numdof+2]+= -normal[0]*normal[1]*kmax*pow(10,penalty_offset);
	Ke->values[1*numdof+3]+= -pow(normal[1],2)*kmax*pow(10,penalty_offset);

	Ke->values[2*numdof+0]+= -pow(normal[0],2)*kmax*pow(10,penalty_offset);
	Ke->values[2*numdof+1]+= -normal[0]*normal[1]*kmax*pow(10,penalty_offset);
	Ke->values[2*numdof+2]+= +pow(normal[0],2)*kmax*pow(10,penalty_offset);
	Ke->values[2*numdof+3]+= +normal[0]*normal[1]*kmax*pow(10,penalty_offset);

	Ke->values[3*numdof+0]+= -normal[0]*normal[1]*kmax*pow(10,penalty_offset);
	Ke->values[3*numdof+1]+= -pow(normal[1],2)*kmax*pow(10,penalty_offset);
	Ke->values[3*numdof+2]+= +normal[0]*normal[1]*kmax*pow(10,penalty_offset);
	Ke->values[3*numdof+3]+= +pow(normal[1],2)*kmax*pow(10,penalty_offset);

	/*Now take care of the friction: of type sigma=frictiontangent_velocity2-tangent_velocity1)*/

	Ke->values[0*numdof+0]+= +pow(normal[1],2)*thickness*length*friction;
	Ke->values[0*numdof+1]+= -normal[0]*normal[1]*thickness*length*friction;
	Ke->values[0*numdof+2]+= -pow(normal[1],2)*thickness*length*friction;
	Ke->values[0*numdof+3]+= +normal[0]*normal[1]*thickness*length*friction;

	Ke->values[1*numdof+0]+= -normal[0]*normal[1]*thickness*length*friction;
	Ke->values[1*numdof+1]+= +pow(normal[0],2)*thickness*length*friction;
	Ke->values[1*numdof+2]+= +normal[0]*normal[1]*thickness*length*friction;
	Ke->values[1*numdof+3]+= -pow(normal[0],2)*thickness*length*friction;

	Ke->values[2*numdof+0]+= -pow(normal[1],2)*thickness*length*friction;
	Ke->values[2*numdof+1]+= +normal[0]*normal[1]*thickness*length*friction;
	Ke->values[2*numdof+2]+= +pow(normal[1],2)*thickness*length*friction;
	Ke->values[2*numdof+3]+= -normal[0]*normal[1]*thickness*length*friction;

	Ke->values[3*numdof+0]+= +normal[0]*normal[1]*thickness*length*friction;
	Ke->values[3*numdof+1]+= -pow(normal[0],2)*thickness*length*friction;
	Ke->values[3*numdof+2]+= -normal[0]*normal[1]*thickness*length*friction;
	Ke->values[3*numdof+3]+= +pow(normal[0],2)*thickness*length*friction;

	/*Clean up and return*/
	return Ke;
}
/*}}}*/
/*FUNCTION Riftfront::PenaltyCreatePVectorDiagnosticHoriz {{{*/
ElementVector* Riftfront::PenaltyCreatePVectorDiagnosticHoriz(IssmDouble kmax){

	const int   numdof = NDOF2*NUMVERTICES;
	int         i,j;
	IssmDouble      rho_ice;
	IssmDouble      rho_water;
	IssmDouble      gravity;
	IssmDouble      thickness;
	IssmDouble      h[2];
	IssmDouble      bed;
	IssmDouble      b[2];
	IssmDouble      pressure;
	IssmDouble      pressure_litho;
	IssmDouble      pressure_air;
	IssmDouble      pressure_melange;
	IssmDouble      pressure_water;
	int         fill;
	bool        shelf;

	/*Objects: */
	Tria       *tria1               = NULL;
	Tria       *tria2               = NULL;

	/*enum of element? */
	if(elements[0]->ObjectEnum()!=TriaEnum)_error2_("only Tria element allowed for Riftfront load!");
	tria1=(Tria*)elements[0];
	tria2=(Tria*)elements[1];

	/*Initialize Element Matrix*/
	if(this->active) return NULL; /*The penalty is active. No loads implied here.*/
	ElementVector* pe=new ElementVector(nodes,NUMVERTICES,this->parameters);

	/*Get some inputs: */
	this->inputs->GetInputValue(&fill,FillEnum);
	this->inputs->GetInputValue(&shelf,SegmentOnIceShelfEnum);
	rho_ice=matpar->GetRhoIce();
	rho_water=matpar->GetRhoWater();
	gravity=matpar->GetG();
	tria1->GetInputValue(&h[0],nodes[0],ThicknessEnum);
	tria2->GetInputValue(&h[1],nodes[1],ThicknessEnum);
	if (h[0]!=h[1])_error2_("different thicknesses not supported for rift fronts");
	thickness=h[0];
	tria1->GetInputValue(&b[0],nodes[0],BedEnum);
	tria2->GetInputValue(&b[1],nodes[1],BedEnum);
	if (b[0]!=b[1])_error2_("different beds not supported for rift fronts");
	bed=b[0];

	/*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 nodes, not onto surfaces between nodes.:*/

	/*Ok, to compute the pressure, we are going to need material properties, thickness and bed for the two nodes. We assume those properties to 
	 * be the same across the rift.: */

	/*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,(IssmDouble)2)/(IssmDouble)2  - rho_water*gravity*pow(bed,(IssmDouble)2)/(IssmDouble)2; 
		}
		else{
			//We are on an icesheet, we assume the water column fills the entire front: */
			pressure=rho_ice*gravity*pow(thickness,(IssmDouble)2)/(IssmDouble)2  - rho_water*gravity*pow(thickness,(IssmDouble)2)/(IssmDouble)2; 
		}
	}
	else if(fill==AirEnum){
		pressure=rho_ice*gravity*pow(thickness,(IssmDouble)2)/(IssmDouble)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) _error2_("fill type " << fill << " not supported on ice sheets yet.");

		pressure_litho=rho_ice*gravity*pow(thickness,(IssmDouble)2)/(IssmDouble)2;
		pressure_air=0;
		pressure_melange=rho_ice*gravity*pow(fraction*thickness,(IssmDouble)2)/(IssmDouble)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{
		_error2_("fill type " << fill << " not supported yet.");
	}

	/*Ok, add contribution to first node, along the normal i==0: */
	for (j=0;j<2;j++){
		pe->values[j]+=pressure*normal[j]*length;
	}

	/*Add contribution to second node, along the opposite normal: i==1 */
	for (j=0;j<2;j++){
		pe->values[2+j]+= -pressure*normal[j]*length;
	}	

	/*Clean up and return*/
	return pe;
}
/*}}}*/
/*FUNCTION Riftfront::Constrain {{{*/
#define _ZIGZAGCOUNTER_

int Riftfront::Constrain(int* punstable){

	const int   numnodes        = 2;
	IssmDouble      max_penetration;
	IssmDouble      penetration;
	int         activate;
	int         found;
	int         unstable;
	IssmDouble      vx1;
	IssmDouble      vy1;
	IssmDouble      vx2;
	IssmDouble      vy2;
	IssmDouble      fractionincrement;

	/*Objects: */
	Tria       *tria1           = NULL;
	Tria       *tria2           = NULL;

	/*enum of element? */
	if(elements[0]->ObjectEnum()!=TriaEnum)_error2_("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;
	}

	/*Is this rift segment state specified by user input? :*/
	if (this->state==OpenEnum || this->state==ClosedEnum){

		if(this->state==OpenEnum)this->active=0;
		if(this->state==ClosedEnum)this->active=1;
		
		/*this segment is like frozen, no instability here: */
		*punstable=0;
		return 1;
	}


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

	/*First recover velocity: */
	tria1->GetInputValue(&vx1,nodes[0],VxEnum);
	tria2->GetInputValue(&vx2,nodes[1],VxEnum);
	tria1->GetInputValue(&vy1,nodes[0],VyEnum);
	tria2->GetInputValue(&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=(IssmDouble)1.0;
		//_printLine_("riftfront " << this->Id() << " fraction: " << 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))_printLine_("Riftfront " << Id() << " wants to be released");

	/*assign output pointer: */
	*punstable=unstable;
	return 1;
}
/*}}}*/
/*FUNCTION Riftfront::FreezeConstraints{{{*/
void   Riftfront::FreezeConstraints(void){

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

}
/*}}}*/
/*FUNCTION Riftfront::IsFrozen{{{*/
bool   Riftfront::IsFrozen(void){

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

	int found=0;
	IssmDouble converged=0;

	this->inputs->GetInputValue(&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;
}
/*}}}*/
/*FUNCTION Riftfront::MaxPenetration {{{*/
int   Riftfront::MaxPenetration(IssmDouble* ppenetration){

	const int     numnodes=2;
	IssmDouble        max_penetration;
	IssmDouble        penetration=0;
	int           found;
	IssmDouble      vx1;
	IssmDouble      vy1;
	IssmDouble      vx2;
	IssmDouble      vy2;

	/*Objects: */
	Tria       *tria1           = NULL;
	Tria       *tria2           = NULL;

	/*enum of element? */
	if(elements[0]->ObjectEnum()!=TriaEnum)_error2_("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->GetInputValue(&vx1,nodes[0],VxEnum);
	tria2->GetInputValue(&vx2,nodes[1],VxEnum);
	tria1->GetInputValue(&vy1,nodes[0],VyEnum);
	tria2->GetInputValue(&vy2,nodes[1],VyEnum);

	/*Node1 faces node2, 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;
	return 1;
}
/*}}}*/
/*FUNCTION Riftfront::Penetration {{{*/
int   Riftfront::Penetration(IssmDouble* ppenetration){

	IssmDouble    vx1;
	IssmDouble    vy1;
	IssmDouble    vx2;
	IssmDouble    vy2;

	IssmDouble    penetration;
	int       found;

	/*Objects: */
	Tria     *tria1       = NULL;
	Tria     *tria2       = NULL;

	/*enum of element? */
	if(elements[0]->ObjectEnum()!=TriaEnum)_error2_("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->GetInputValue(&vx1,nodes[0],VxEnum);
	tria2->GetInputValue(&vx2,nodes[1],VxEnum);
	tria1->GetInputValue(&vy1,nodes[0],VyEnum);
	tria2->GetInputValue(&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;
	return 1;
}
/*}}}*/
/*FUNCTION Riftfront::PotentialUnstableConstraint {{{*/
int   Riftfront::PotentialUnstableConstraint(int* punstable){


	const int   numnodes        = 2;
	IssmDouble      max_penetration;
	IssmDouble      penetration;
	int         activate;
	int         unstable;
	int         found;
	IssmDouble      vx1;
	IssmDouble      vy1;
	IssmDouble      vx2;
	IssmDouble      vy2;

	/*Objects: */
	Tria       *tria1           = NULL;
	Tria       *tria2           = NULL;

	/*enum of element? */
	if(elements[0]->ObjectEnum()!=TriaEnum)_error2_("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->GetInputValue(&vx1,nodes[0],VxEnum);
	tria2->GetInputValue(&vx2,nodes[1],VxEnum);
	tria1->GetInputValue(&vy1,nodes[0],VyEnum);
	tria2->GetInputValue(&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;
	return 1;
}
/*}}}*/
/*FUNCTION Riftfront::PreConstrain {{{*/
int   Riftfront::PreConstrain(int* punstable){

	const int   numnodes    = 2;
	IssmDouble      penetration;
	int         unstable;
	int         found;
	IssmDouble      vx1;
	IssmDouble      vy1;
	IssmDouble      vx2;
	IssmDouble      vy2;

	/*Objects: */
	Tria       *tria1       = NULL;
	Tria       *tria2       = NULL;

	/*enum of element? */
	if(elements[0]->ObjectEnum()!=TriaEnum)_error2_("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->GetInputValue(&vx1,nodes[0],VxEnum);
	tria2->GetInputValue(&vx2,nodes[1],VxEnum);
	tria1->GetInputValue(&vy1,nodes[0],VyEnum);
	tria2->GetInputValue(&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;
	return 1;
}
/*}}}*/
/*FUNCTION Riftfront::PreStable {{{*/
bool  Riftfront::PreStable(){
	return prestable;
}
/*}}}*/
/*FUNCTION Riftfront::SetPreStable {{{*/
void Riftfront::SetPreStable(){
	prestable=1;
}
/*}}}*/
/*FUNCTION Riftfront::IsInput{{{*/
bool Riftfront::IsInput(int name){
	if (
				name==ConvergedEnum ||
				name==ThicknessEnum ||
				name==SurfaceEnum ||
				name==BedEnum 
		){
		return true;
	}
	else return false;
}
/*}}}*/
