/*!\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"

		
Riftfront::Riftfront(){
	return;
}

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_penalty_offset, int riftfront_penalty_lock, bool riftfront_active,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;
	penalty_offset=riftfront_penalty_offset;
	penalty_lock=riftfront_penalty_lock;
	active=riftfront_active;
	counter=riftfront_counter;
	prestable=riftfront_prestable;
	shelf=riftfront_shelf;

	return;
}

Riftfront::~Riftfront(){
	return;
}
		
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\n",fill,friction);
	printf("penalty_offset %g\n",penalty_offset);
	printf("penalty_lock %i\n",penalty_lock);
	printf("active %i\n",active);
	printf("counter %i\n",counter);
	printf("prestable %i\n",prestable);
	printf("shelf %i\n",shelf);
}

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\n",fill,friction);
	printf("penalty_offset %g\n",penalty_offset);
	printf("penalty_lock %i\n",penalty_lock);
	printf("active %i\n",active);
	printf("counter %i\n",counter);
	printf("prestable %i\n",prestable);
	printf("shelf %i\n",shelf);
	
}		

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,&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,&counter,sizeof(counter));marshalled_dataset+=sizeof(counter);
	memcpy(marshalled_dataset,&prestable,sizeof(prestable));marshalled_dataset+=sizeof(prestable);
	memcpy(marshalled_dataset,&shelf,sizeof(shelf));marshalled_dataset+=sizeof(shelf);

	*pmarshalled_dataset=marshalled_dataset;
	return;
}
		
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(penalty_offset)+
		sizeof(penalty_lock)+
		sizeof(active)+
		sizeof(counter)+
		sizeof(prestable)+
		sizeof(shelf)+
		sizeof(int); //sizeof(int) for enum type
}

char* Riftfront::GetName(void){
	return "riftfront";
}
		

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(&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(&counter,marshalled_dataset,sizeof(counter));marshalled_dataset+=sizeof(counter);
	memcpy(&prestable,marshalled_dataset,sizeof(prestable));marshalled_dataset+=sizeof(prestable);
	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;
}

int Riftfront::Enum(void){

	return RiftfrontEnum();

}

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

int    Riftfront::MyRank(void){ 
	extern int my_rank;
	return my_rank; 
}

#undef __FUNCT__ 
#define __FUNCT__ "Riftfront::Configure"
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);

}

#undef __FUNCT__ 
#define __FUNCT__ "Riftfront::UpdateFromInputs"
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);

}

#undef __FUNCT__ 
#define __FUNCT__ "Riftfront::GetDofList"

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

#undef __FUNCT__ 
#define __FUNCT__ "Riftfront::PenaltyCreateKMatrix"
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 _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])throw ErrorException(__FUNCT__," different thicknesses not supported for rift fronts");
		thickness=h[0];

		#ifdef _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): */
	}

}
		
#undef __FUNCT__ 
#define __FUNCT__ "Riftfront::PenaltyCreatePVector"
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;
	
	/*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 _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])throw ErrorException(__FUNCT__," 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])throw ErrorException(__FUNCT__," 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==WaterFillEnum()){
			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==AirFillEnum()){
			pressure=rho_ice*gravity*pow(thickness,(double)2)/(double)2;   //icefront on an ice sheet, pressure imbalance ice vs air.
		}
		else if(fill==IceFillEnum()){ //icefront finding itself against another icefront (pressure imbalance is fully compensated, ice vs ice)
			pressure=0;
		}
		else{
			throw ErrorException(__FUNCT__,exprintf("%s%s%i%s",__FUNCT__," error message: 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.*/
	}
}

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

#undef __FUNCT__ 
#define __FUNCT__ "Riftfront::CreateKMatrix"
void  Riftfront::CreateKMatrix(Mat Kgg,void* inputs,int analysis_type,int sub_analysis_type){
	/*do nothing: */
}

#undef __FUNCT__ 
#define __FUNCT__ "Riftfront::CreatePVector"
void  Riftfront::CreatePVector(Vec pg, void* inputs, int analysis_type,int sub_analysis_type){
	/*do nothing: */
}

bool  Riftfront::PreStable(){
	return prestable;
}

void Riftfront::SetPreStable(){
	prestable=1;
}

#undef __FUNCT__ 
#define __FUNCT__ "Riftfront::PreConstrain"
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)throw ErrorException(__FUNCT__," 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;
}

#undef __FUNCT__ 
#define __FUNCT__ "Riftfront::Constrain"
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;


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

	found=inputs->Recover("max_penetration",&max_penetration);
	if(!found)throw ErrorException(__FUNCT__," could not find max_penetration 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];

	/*Activate or deactivate penalties: */
	if(penetration<0){
		//printf("riftfront %i penetrating.\n",this->GetId());
		/*There is penetration, we need to active the penalty so this penetration will be NULL: */
		activate=1;
	}
	else{
		/*Ok, there is no penetration for these two grids of the rift. We want to deactivate  the penalty. If we do 
		 * it all at once, then we will zigzag forever. Only deactivate once at a time. Deactivate the one that most 
		 * wants to, ie the one with the max penetration: */
		if (penetration==max_penetration){
			//printf("riftfront %i at max penetration.\n",this->GetId());
			activate=0;
		}
		else{
			/*Ok, we are dealing with the  rest of the penalties that want to be freed. But may be in this lot there 
			 * are penalties that were already free? Keep them as such. For the rest, do not release them yet: */
			if (this->active==0){
				activate=0;
			}
			else{
				activate=1;
			}
		}
	}

	/*Here, we try to avoid zigzaging. When a penalty activates and deactivates for more than penalty_lock times, 
	 * we lock it: */
	if(this->counter>this->penalty_lock){
		/*This penalty has zig zagged too long, fix it to smooth results: */
		activate=1; this->active=activate;
		if (this->counter==(this->penalty_lock+1)){
			printf("riftfront %i locking at %g m/yr\n",this->GetId(),penetration*365.0*24.0*3600.0);
			this->counter++;
		}
	}

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

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

	/*assign output pointer: */
	*punstable=unstable;
}

#undef __FUNCT__ 
#define __FUNCT__ "Riftfront::Penetration"
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)throw ErrorException(__FUNCT__," 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;

}

#undef __FUNCT__ 
#define __FUNCT__ "Riftfront::PotentialUnstableConstraint"
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)throw ErrorException(__FUNCT__," 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;
}