/*
 * TriMeshUtils: mesh manipulation routines: 
 */

#include <stdio.h>

#include "./trimesh.h"
#include "../Exceptions/exceptions.h"
#include "../Alloc/xNewDelete.h"
#include "../Alloc/alloc.h"
#include "../../include/include.h"

#define RIFTPENALTYPAIRSWIDTH 8
/*FUNCTION IsGridOnRift{{{*/
int IsGridOnRift(int* riftsegments, int nriftsegs, int node){

	/*Does this node belong to 4 elements, or just 2? If it belongs to 4 elements, it is inside a rift, 
	 *if it belongs to 2 elements, it is on the tip of a rift, or it has already been split across the rift (see below).*/
	
	int i;
	int j;
	int count;

	count=0;
	for (i=0;i<nriftsegs;i++){
		for (j=0;j<2;j++){
			if ((*(riftsegments+4*i+2+j))==node) count++;
		}
	}
	if (count==2){
		return 1;
	}
	else{
		return 0;
	}
}/*}}}*/
/*FUNCTION GridElementsList{{{*/
int GridElementsList(int** pGridElements, int* pNumGridElements,int node,double * index,int nel){

	/*From a node, recover all the elements that are connected to it: */
	int i,j;
	int noerr=1;

	int max_number_elements=12;
	int current_size;
	int NumGridElements;
	int* GridElements=NULL;
	int* GridElementsRealloc=NULL;

	/*From a mesh with 30 degrees minimum angle, we get 12 possible elements that own 
	 * the node. We start by allocating GridElements with that size, and realloc 
	 * more if needed.*/

	current_size=max_number_elements;
	NumGridElements=0;
	GridElements=xNew<int>(max_number_elements);

	for (i=0;i<nel;i++){
		for (j=0;j<3;j++){
			if ((int)*(index+3*i+j)==node){
				if (NumGridElements<=(current_size-1)){
					GridElements[NumGridElements]=i;
					NumGridElements++;
					break;
				}
				else{
					/*Reallocate another max_number_elements slots in the GridElements: */
					GridElementsRealloc=xReNew<int>(GridElements,current_size,(current_size+max_number_elements));
					if (!GridElementsRealloc){
						noerr=0;
						goto cleanup_and_return;
					}
					current_size+=max_number_elements;
					GridElements=GridElementsRealloc;
					GridElements[NumGridElements]=i;
					NumGridElements++;
					break;
				}
			}
		}
	}
	cleanup_and_return:
	if(!noerr){
		xDelete<int>(GridElements);
	}
	/*Allocate return pointers: */
	*pGridElements=GridElements;
	*pNumGridElements=NumGridElements;
	return noerr;
}/*}}}*/
/*FUNCTION IsNeighbor{{{*/
int IsNeighbor(int el1,int el2,double* index){
	/*From a triangulation held in index, figure out if elements 1 and 2 have two nodes in common: */
	int i,j;
	int count=0;
	for (i=0;i<3;i++){
		for (j=0;j<3;j++){
			if (*(index+3*el1+i)==*(index+3*el2+j))count++;
		}
	}
	if (count==2){
		return 1;
	}
	else{
		return 0;
	}
}/*}}}*/
/*FUNCTION IsOnRift{{{*/
int IsOnRift(int el,int nriftsegs,int* riftsegments){
	/*From a list of elements segments, figure out if el belongs to it: */
	int i;
	for (i=0;i<nriftsegs;i++){
		if ((*(riftsegments+4*i+0)==el) | (*(riftsegments+4*i+1)==el)){
			return 1;
		}
	}
	return 0;
}/*}}}*/
/*FUNCTION RiftSegmentsFromSegments{{{*/
void RiftSegmentsFromSegments(int* pnriftsegs, int** priftsegments, int nel, double* index, int nsegs,double* segments){
	
	int i,counter;
	int el,el2;
	
	int  nriftsegs;
	int* riftsegments=NULL;
	int* riftsegments_uncompressed=NULL; 
	double element_nodes[3];

	/*Allocate segmentflags: */
	riftsegments_uncompressed=xNewZeroInit<int>(nsegs*5);

	/*Find the segments that belong to a rift: they are the ones that see two elements. The other ones belong to a boundary 
	 *or a hole: */
	nriftsegs=0;
	for (i=0;i<nsegs;i++){
		el=(int)*(segments+3*i+2)-1; //element found in AssociateSegmentToElements
		/*Temporarily set nodes belonging to the segments to -1 in the triangulation index, and 
		 *then  proceed to find another element that owns the segment. If we don't find it, we know 
		 *we are dealing with a boundary or hole, otherwise, we are dealing with a rift: */
		element_nodes[0]=*(index+3*el+0);
		element_nodes[1]=*(index+3*el+1);
		element_nodes[2]=*(index+3*el+2);

		*(index+3*el+0)=-1;
		*(index+3*el+1)=-1;
		*(index+3*el+2)=-1;

		el2=FindElement(*(segments+3*i+0),*(segments+3*i+1),index,nel); 

		/*Restore index: */
		*(index+3*el+0)=element_nodes[0];
		*(index+3*el+1)=element_nodes[1];
		*(index+3*el+2)=element_nodes[2];

		if (el2!=-1){
			/*el and el2 are on a segment rift, facing one another, plug them into riftsegments_uncompressed: */
		    *(riftsegments_uncompressed+5*i+0)=1;
		    *(riftsegments_uncompressed+5*i+1)=el;
		    *(riftsegments_uncompressed+5*i+2)=el2;
		    *(riftsegments_uncompressed+5*i+3)=(int)*(segments+3*i+0);
		    *(riftsegments_uncompressed+5*i+4)=(int)*(segments+3*i+1);
			nriftsegs++;
		}
	}

	/*Compress riftsegments_uncompressed:*/
	riftsegments=xNew<int>(nriftsegs*4);
	counter=0;
	for (i=0;i<nsegs;i++){
		if (*(riftsegments_uncompressed+5*i+0)){
			*(riftsegments+counter*4+0)=*(riftsegments_uncompressed+5*i+1);
			*(riftsegments+counter*4+1)=*(riftsegments_uncompressed+5*i+2);
			*(riftsegments+counter*4+2)=*(riftsegments_uncompressed+5*i+3);
			*(riftsegments+counter*4+3)=*(riftsegments_uncompressed+5*i+4);
			counter++;
		}
	}

	xDelete<int>(riftsegments_uncompressed);
	
	/*Assign output pointers: */
	*priftsegments=riftsegments;
	*pnriftsegs=nriftsegs;
}/*}}}*/
/*FUNCTION DetermineGridElementListOnOneSideOfRift{{{*/
int DetermineGridElementListOnOneSideOfRift(int* pNumGridElementListOnOneSideOfRift, int** pGridElementListOnOneSideOfRift, int segmentnumber, int nriftsegs, int* riftsegments, int node,double* index,int nel){

	int noerr=1;
	int k,l,counter;
	int newel;

	int* GridElements=NULL;
	int  NumGridElements;

	/*Output: */
	int NumGridElementListOnOneSideOfRift;
	int* GridElementListOnOneSideOfRift=NULL;

	/*Build a list of all the elements connected to this node: */
	GridElementsList(&GridElements,&NumGridElements,node,index,nel);

	/*Figure out the list of elements  that are on the same side of the rift. To do so, we start from one 
	 * side of the rift and keep rotating in the same direction:*/
	GridElementListOnOneSideOfRift=xNew<int>(NumGridElements);
	//bootstrap the GridElementListOnOneSideOfRift by filling elements from riftsegments: */
	GridElementListOnOneSideOfRift[0]=*(riftsegments+4*segmentnumber+0); /*this one does not belong to the same side, but is just there 
															   for a rotation direction, we 'll take it out when we are 
															   done rotating*/
	GridElementListOnOneSideOfRift[1]=*(riftsegments+4*segmentnumber+1);
	counter=1;
	for (;;){
		/*Find neighbour of element GridElementListOnOneSideOfRift[counter], not 
		 * equal to GridElementListOnOneSideOfRift[counter-1]*/
		for (k=0;k<NumGridElements;k++){
			if(IsNeighbor(GridElements[k],GridElementListOnOneSideOfRift[counter],index)){
				/*Verify this element is not already in our list of element on the same side of the rift: */
				newel=1;
				for (l=0;l<=counter;l++){
					if (GridElements[k]==GridElementListOnOneSideOfRift[l]){
						newel=0;
						break;
					}
				}
				if (newel){
					counter++;
					GridElementListOnOneSideOfRift[counter]=GridElements[k];
					if (IsOnRift(GridElements[k],nriftsegs,riftsegments)){
						break;
					}
					k=-1;
				}
			}
		}
		/*Reduce counter by 1 and get rift of first element in GridElementListOnOneSideOfRift:*/
		NumGridElementListOnOneSideOfRift=counter;
		for (l=0;l<NumGridElementListOnOneSideOfRift;l++){
			GridElementListOnOneSideOfRift[l]=GridElementListOnOneSideOfRift[l+1];
		}
		break;
	}// for (;;)

	/*Free ressources: */
	xDelete<int>(GridElements);
	/*Assign output pointers: */
	*pNumGridElementListOnOneSideOfRift=NumGridElementListOnOneSideOfRift;
	*pGridElementListOnOneSideOfRift=GridElementListOnOneSideOfRift;
	return noerr;
}/*}}}*/
/*FUNCTION UpdateSegments{{{*/
int UpdateSegments(double** psegments,double** psegmentmarkerlist, int* pnsegs, double* index, double* x,double* y,int* riftsegments,int nriftsegs,int nods,int nel){

	int noerr=1;
	int i,j,k;
	int el1,el2;

	double* segments=NULL;
	double*    segmentmarkerlist=NULL;
	int     nsegs;

	/*Recover input: */
	segments=*psegments;
	segmentmarkerlist=*psegmentmarkerlist;
	nsegs=*pnsegs;

	/*Reallocate segments: */
	segments=(double*)xrealloc(segments,(nsegs+nriftsegs)*3*sizeof(double));
	segmentmarkerlist=(double*)xrealloc(segmentmarkerlist,(nsegs+nriftsegs)*sizeof(double));

	/*First, update the existing segments to the new nodes :*/
	for (i=0;i<nriftsegs;i++){
		el1=riftsegments[4*i+0];
		el2=riftsegments[4*i+1];
		for (j=0;j<nsegs;j++){
			if (segments[3*j+2]==(el1+1)){
				/*segment j is the same as rift segment i.Let's update segments[j][:] using  element el1 and the corresponding rift segment.
				 *Because riftsegments does not represent a list of rift segments anymore (it got heavily modified in SplitElementsForRifts, 
				 *we can only rely on the position (x,y) of the rift nodes to create a segment:*/
				for (k=0;k<3;k++){
					if ((x[(int)*(index+el1*3+k)-1]==x[(int)*(segments+3*j+0)-1]) && (y[(int)*(index+el1*3+k)-1]==y[(int)*(segments+3*j+0)-1])){
						*(segments+3*j+0)=*(index+el1*3+k); _assert_(segments[3*j+0]<nods+1);
						break;
					}
				}
				for (k=0;k<3;k++){
					if ((x[(int)*(index+el1*3+k)-1]==x[(int)*(segments+3*j+1)-1])  && (y[(int)*(index+el1*3+k)-1]==y[(int)*(segments+3*j+1)-1])){
						*(segments+3*j+1)=*(index+el1*3+k); _assert_(segments[3*j+1]<nods+1);
						break;
					}
				}
				/*Deal with el2: */
				*(segments+3*(nsegs+i)+2)=el2+1;
				*(segmentmarkerlist+(nsegs+i))=*(segmentmarkerlist+j);
				for (k=0;k<3;k++){
					if ((x[(int)*(index+el2*3+k)-1]==x[(int)*(segments+3*j+0)-1]) && (y[(int)*(index+el2*3+k)-1]==y[(int)*(segments+3*j+0)-1])){
						*(segments+3*(nsegs+i)+0)=*(index+el2*3+k); _assert_(segments[3*(nsegs+i)+0]<nods+1);
						break;
					}
				}
				for (k=0;k<3;k++){
					if ((x[(int)*(index+el2*3+k)-1]==x[(int)*(segments+3*j+1)-1]) && (y[(int)*(index+el2*3+k)-1]==y[(int)*(segments+3*j+1)-1])){
						*(segments+3*(nsegs+i)+1)=*(index+el2*3+k); _assert_(segments[3*(nsegs+i)+1]<nods+1);
						break;
					}
				}
			}
			if (*(segments+3*j+2)==(el2+1)){
				/*segment j is the same as rift segment i.*/
				/*Let's update segments[j][:] using  element el2 and the corresponding rift segment: */
				for (k=0;k<3;k++){
					if ((x[(int)*(index+el2*3+k)-1]==x[(int)*(segments+3*j+0)-1]) && (y[(int)*(index+el2*3+k)-1]==y[(int)*(segments+3*j+0)-1])){
						*(segments+3*j+0)=*(index+el2*3+k); _assert_(segments[3*j+0]<nods+1);
						break;
					}
				}
				for (k=0;k<3;k++){
					if ((x[(int)*(index+el2*3+k)-1]==x[(int)*(segments+3*j+1)-1]) && (y[(int)*(index+el2*3+k)-1]==y[(int)*(segments+3*j+1)-1])){
						*(segments+3*j+1)=*(index+el2*3+k);_assert_(segments[3*j+1]<nods+1);
						break;
					}
				}
				/*Deal with el1: */
				*(segments+3*(nsegs+i)+2)=el1+1;
				*(segmentmarkerlist+(nsegs+i))=*(segmentmarkerlist+j);
				for (k=0;k<3;k++){
					if ((x[(int)*(index+el1*3+k)-1]==x[(int)*(segments+3*j+0)-1]) && (y[(int)*(index+el1*3+k)-1]==y[(int)*(segments+3*j+0)-1])){
						*(segments+3*(nsegs+i)+0)=*(index+el1*3+k);_assert_(segments[3*(nsegs+i)+0]<nods+1);
						break;
					}
				}
				for (k=0;k<3;k++){
					if ((x[(int)*(index+el1*3+k)-1]==x[(int)*(segments+3*j+1)-1]) && (y[(int)*(index+el1*3+k)-1]==y[(int)*(segments+3*j+1)-1])){
						*(segments+3*(nsegs+i)+1)=*(index+el1*3+k);_assert_(segments[3*(nsegs+i)+1]<nods+1);
						break;
					}
				}
			}
		}
	}
	nsegs+=nriftsegs;

	/*Assign output pointers: */
	*psegments=segments;
	*psegmentmarkerlist=segmentmarkerlist;
	*pnsegs=nsegs;
	
	return noerr;
}/*}}}*/
/*FUNCTION pnpoly{{{*/
int pnpoly(int npol, double *xp, double *yp, double x, double y) {
	int i, j, c = 0;
	for (i = 0, j = npol-1; i < npol; j = i++) {
		if ((((yp[i]<=y) && (y<yp[j])) ||
					((yp[j]<=y) && (y<yp[i]))) &&
				(x < (xp[j] - xp[i]) * (y - yp[i]) / (yp[j] - yp[i]) + xp[i]))
			c = !c;
	}
	return c;
}/*}}}*/
/*FUNCTION FindElement{{{*/
int FindElement(double A,double B,double* index,int nel){

	int n;
	int el=-1;
	for (n=0;n<nel;n++){
		if (((*(index+3*n+0)==A)  || (*(index+3*n+1)==A) || (*(index+3*n+2)==A) ) && ((*(index+3*n+0)==B)  || (*(index+3*n+1)==B) || (*(index+3*n+2)==B))){
			el=n;
			break;
		}
	}
	return el;
}/*}}}*/
/*FUNCTION SplitRiftSegments{{{*/
int SplitRiftSegments(double** psegments,double** psegmentmarkerlist, int* pnumsegs, int* pnumrifts,int** priftsnumsegs,double*** priftssegments,int numrifts,int nods,int nel){

	/*Using segment markers, wring out the rift segments from the segments. Rift markers are 
	 *of the form 2+i where i=0 to number of rifts */

	int noerr=1;
	int i,j,counter;

	/*input: */
	double *segments          = NULL;
	double *segmentmarkerlist = NULL;
	int numsegs;
	
	/*output: */
	int      new_numsegs;
	int     *riftsnumsegs       = NULL;
	double **riftssegments      = NULL;
	double  *new_segments       = NULL;
	double  *new_segmentmarkers = NULL;

	/*intermediary: */
	double* riftsegment=NULL;

	/*Recover input arguments: */
	segments=*psegments;
	numsegs=*pnumsegs;
	segmentmarkerlist=*psegmentmarkerlist;

	/*First, figure out  how many segments will be left in 'segments': */
	counter=0;
	for (i=0;i<numsegs;i++){
		if (segmentmarkerlist[i]==1)counter++; //1 is default marker for non-rifts;
	}
	/*Allocate new segments: */
	new_numsegs=counter;
	new_segments=xNew<double>(new_numsegs*3);
	new_segmentmarkers=xNew<double>(new_numsegs);

	/*Copy new segments info : */
	counter=0;
	for (i=0;i<numsegs;i++){
		if (segmentmarkerlist[i]==1){
			new_segments[3*counter+0]=segments[3*i+0];
			new_segments[3*counter+1]=segments[3*i+1];
			new_segments[3*counter+2]=segments[3*i+2];
			new_segmentmarkers[counter]=segmentmarkerlist[i];
			counter++;
		}
	}

	/*Now deal with rift segments: */
	riftsnumsegs=xNew<int>(numrifts);
	riftssegments=xNew<double*>(numrifts);
	for (i=0;i<numrifts;i++){
		/*Figure out how many segments for rift i: */
		counter=0;
		for (j=0;j<numsegs;j++){
			if (segmentmarkerlist[j]==2+i)counter++;
		}
		riftsnumsegs[i]=counter;
		riftsegment=xNew<double>(counter*3);
		/*Copy new segments info :*/
		counter=0;
		for (j=0;j<numsegs;j++){
			if (segmentmarkerlist[j]==(2+i)){
				riftsegment[3*counter+0]=segments[3*j+0];_assert_(riftsegment[3*counter+0]<nods+1);
				riftsegment[3*counter+1]=segments[3*j+1];_assert_(riftsegment[3*counter+1]<nods+1);
				riftsegment[3*counter+2]=segments[3*j+2];_assert_(riftsegment[3*counter+2]<nel+1);
				counter++;
			}
		}
		*(riftssegments+i)=riftsegment;
	}

	/*Free ressources: */
	xDelete<double>(segments);

	/*Assign output pointers: */
	*psegments=new_segments;
	*psegmentmarkerlist=new_segmentmarkers;
	*pnumsegs=new_numsegs;
	*pnumrifts=numrifts;
	*priftssegments=riftssegments;
	*priftsnumsegs=riftsnumsegs;
	return noerr;
}/*}}}*/
/*FUNCTION PairRiftElements{{{*/
int PairRiftElements(int** priftsnumpairs, double*** priftspairs,int numrifts,int* riftsnumsegments, double** riftssegments,double* x,double* y){


	int noerr=1;
	int i,j,k;

	/*output: */
	int     *riftsnumpairs = NULL;
	double **riftspairs    = NULL;

	/*intermediary :*/
	int     numsegs;
	double* segments=NULL;
	double* pairs=NULL;
	int     node1,node2,node3,node4;

	riftsnumpairs=xNew<int>(numrifts);
	riftspairs=xNew<double*>(numrifts);
	for (i=0;i<numrifts;i++){
		segments=riftssegments[i];
		numsegs=riftsnumsegments[i];
		riftsnumpairs[i]=numsegs;
		pairs=xNew<double>(2*numsegs);
		for (j=0;j<numsegs;j++){
			*(pairs+2*j+0)=*(segments+3*j+2); //retrieve element to which this segment belongs.
			node1=(int)*(segments+3*j+0)-1; node2=(int)*(segments+3*j+1)-1;
			/*Find element facing on other side of rift: */
			for (k=0;k<numsegs;k++){
				if (k==j)continue;
				node3=(int)*(segments+3*k+0)-1; node4=(int)*(segments+3*k+1)-1;
				/*We are trying to find 2 elements, where position of node3 == position of node1, and position of node4 == position of node2*/
				if (   (x[node3]==x[node1]) && (y[node3]==y[node1]) && (x[node4]==x[node2]) && (y[node4]==y[node2])
				    || (x[node3]==x[node2]) && (y[node3]==y[node2]) && (x[node4]==x[node1]) && (y[node4]==y[node1])  ){
					/*We found the corresponding element: */
					*(pairs+2*j+1)=*(segments+3*k+2);
					break;
				}
			}
		}
		riftspairs[i]=pairs;
	}

	/*Assign output pointers: */
	*priftsnumpairs=riftsnumpairs;
	*priftspairs=riftspairs;
	return noerr;
}/*}}}*/
/*FUNCTION RemoveRifts{{{*/
int RemoveRifts(double** pindex,double** px,double** py,int* pnods,double** psegments,int* pnumsegs,int numrifts1,int* rifts1numsegs,double** rifts1segments,double** rifts1pairs,int nel){

	int noerr=1;
	int i,j,k,counter,counter1,counter2;

	/*input: */
	double* index=NULL;
	double* x=NULL;
	double* y=NULL;
	int     nods;
	double* segments=NULL;
	int     numsegs;

	/*intermediary: */
	double *riftsegments = NULL;
	double *riftpairs    = NULL;
	int     node1,node2,node3,node4,temp_node;
	double  el2;
	int     newnods; //temporary # node counter.
	double  xmin,ymin;
	double *xreal        = NULL;
	double *yreal        = NULL;
	int    *nodes        = NULL;
	int    *mergingnodes = NULL;
	int     max_size;
	int     redundant;

	/*Recover input: */
	index=*pindex;
	x=*px;
	y=*py;
	nods=*pnods;;
	segments=*psegments;
	numsegs=*pnumsegs;

	/*initialize newnods : */
	newnods=nods;

	/*Figure out a unique value to flag x and y for node removal: */
	xmin=x[0];
	ymin=y[0];
	for (i=0;i<nods;i++){
		if (x[i]<xmin)xmin=x[i];
		if (y[i]<ymin)ymin=y[i];
	}
	xmin=xmin-fabs(xmin); 
	ymin=ymin-fabs(ymin);

	/*Initialize two arrays, one for nodes that are going to be merged, the other with corresponding nodes being merge into: */
	max_size=0;
	for (i=0;i<numrifts1;i++){
		max_size+=rifts1numsegs[i];
	}
	nodes=xNew<int>(max_size);
	mergingnodes=xNew<int>(max_size);

	/*Go through the rifts segments, and identify which node we are going to merge with its counterpart on the other side 
	 *of the rift. The way we identify this node is by looking at the element pairs, and the corresponding nodes: */
	counter=0;
	for (i=0;i<numrifts1;i++){
		riftsegments=rifts1segments[i];
		riftpairs=rifts1pairs[i];
		for (j=0;j<rifts1numsegs[i];j++){
			el2=*(riftpairs+2*j+1);
			node1=(int)*(riftsegments+3*j+0);
			node2=(int)*(riftsegments+3*j+1);
			/*Summary, el1 and el2 are facing one another across the rift. node1 and node2 belong to el1 and 
			 *are located on the rift. Find node3 and node4, nodes belonging to el2 and located on the rift: */
			for (k=0;k<rifts1numsegs[i];k++){
				if (*(riftsegments+3*k+2)==el2){
					node3=(int)*(riftsegments+3*k+0);
					node4=(int)*(riftsegments+3*k+1);
					break;
				}
			}
			/* Make sure node3 faces node1 and node4 faces node2: */
			if ((x[node1]==x[node4]) && (y[node1]==y[node4])){
				/*Swap node3 and node4:*/
				temp_node=node3;
				node3=node4;
				node4=temp_node;
			}
			/* Is any of these two node pairs on the tip of a rift, in which case, we don't include it in nodes or mergingnodes: */
			if ((node1==node3) || (node2==node4)){
				if(node1!=node3){
					/*Add node1 and node3 to nodes and mergingnodes if they have not already been added: */
					redundant=0;
					for (k=0;k<counter;k++){
						if ((mergingnodes[k]==node1) || (nodes[k]==node1))redundant=1;
					}
					if(!redundant){
						/*Ok, add node1 to nodes, and node3 to mergingnodes: */
						nodes[counter]=node1;
						mergingnodes[counter]=node3;
						counter++;
					}
				}
				if(node2!=node4){
					/*Add node2 and node4 to nodes and mergingnodes if they have not already been added: */
					redundant=0;
					for (k=0;k<counter;k++){
						if ((mergingnodes[k]==node2) || (nodes[k]==node2))redundant=1;
					}
					if(!redundant){
						/*Ok, add node2 to nodes, and node4 to mergingnodes: */
						nodes[counter]=node2;
						mergingnodes[counter]=node4;
						counter++;
					}
				}
			}
			else{
				/*Check that node1 is not already present in the mergingnodes: */
				redundant=0;
				for (k=0;k<counter;k++){
					if ((mergingnodes[k]==node1) || (nodes[k]==node1))redundant=1;
				}
				if(!redundant){
					/*Ok, add node1 to nodes, and node3 to mergingnodes: */
					nodes[counter]=node1;
					mergingnodes[counter]=node3;
					counter++;
				}
				/*Check that node2 is not already present in the mergingnodes: */
				redundant=0;
				for (k=0;k<counter;k++){
					if ((mergingnodes[k]==node1) || (nodes[k]==node1))redundant=1;
				}
				if(!redundant){
					/*Ok, add node2 to nodes, and node4 to mergingnodes: */
					nodes[counter]=node2;
					mergingnodes[counter]=node4;
					counter++;
				}
			}
		}
	}

	/*Ok, we have counter pairs of nodes (nodes and mergingnodes): start merging nodes in the triangulation: */
	newnods=nods;
	for (i=0;i<counter;i++){
		node1=nodes[i];
		node3=mergingnodes[i];
		/*Merge node3 into node1: */ 
		x[node3]=xmin; //flag  for later removal from x
		y[node3]=ymin; //flag  for later removal from y
		newnods--;
		for (k=0;k<nel;k++){
			if (*(index+3*k+0)==node3)*(index+3*k+0)=node1;
			if (*(index+3*k+1)==node3)*(index+3*k+1)=node1;
			if (*(index+3*k+2)==node3)*(index+3*k+2)=node1;
		}
	}

	/*Reallocate x and y: */
	xreal=(double*)xrealloc(x,newnods*sizeof(double));
	yreal=(double*)xrealloc(y,newnods*sizeof(double));
	counter1=0;
	counter2=0;
	for (i=0;i<nods;i++){
		if (x[i]!=xmin){
			xreal[counter1]=x[i];
			counter1++;
		}
		if (y[i]!=ymin){
			yreal[counter2]=y[i];
			counter2++;
		}
	}
	xDelete<double>(x); x=xreal;
	xDelete<double>(y); y=yreal;

	/*Assign output pointers:*/
	*pindex=index;
	*px=x;
	*py=y;
	*pnods=newnods;
	*psegments=segments;
	*pnumsegs=numsegs;
	return noerr;
}/*}}}*/
/*FUNCTION IsRiftPresent{{{*/
int IsRiftPresent(int* priftflag,int* pnumrifts, double* segmentmarkerlist,int nsegs){

	int i;
	int noerr=1;
	
	/*output: */
	int riftflag=0;
	int numrifts=0;

	double maxmark=1; //default marker for regular segments

	/*Any marker >=2 indicates a certain rift: */
	numrifts=0;
	for (i=0;i<nsegs;i++){
		if (segmentmarkerlist[i]>maxmark){
			numrifts++;
			maxmark=segmentmarkerlist[i];
		}
	}
	if (numrifts)riftflag=1;

	/*Assign output pointers:*/
	*priftflag=riftflag;
	*pnumrifts=numrifts;
	return noerr;
}/*}}}*/
/*FUNCTION OrderRifts{{{*/
int OrderRifts(double** priftstips, double** riftssegments,double** riftspairs,int numrifts,int* riftsnumsegments,double* x,double* y,int nods,int nels){
	
	int noerr=1;
	int i,j,k,counter;

	/*intermediary: */
	double *riftsegments = NULL;
	double *riftpairs    = NULL;
	int numsegs;

	/*ordering and copy: */
	int    *order             = NULL;
	double *riftsegments_copy = NULL;
	double *riftpairs_copy    = NULL;

	/*node and element manipulation: */
	int     node1,node2,node3,node4,temp_node,tip1,tip2,node;
	double  el2;
	int     already_ordered=0;

	/*output: */
	double* riftstips=NULL;

	/*Allocate byproduct of this routine, riftstips: */
	riftstips=xNew<double>(numrifts*2);

	/*Go through all rifts: */
	for (i=0;i<numrifts;i++){
		riftsegments=riftssegments[i];
		riftpairs=riftspairs[i];
		numsegs=riftsnumsegments[i];
	
		/*Allocate copy of riftsegments and riftpairs, 
		 *as well as ordering vector: */
		riftsegments_copy=xNew<double>(numsegs*3);
		riftpairs_copy=xNew<double>(numsegs*2);
		order=xNew<int>(numsegs);

		/*First find the tips, using the pairs. If a pair of elements has one node in common, this node is a rift tip: */
		tip1=-1;
		tip2=-1;

		for (j=0;j<numsegs;j++){
			el2=*(riftpairs+2*j+1);
			node1=(int)*(riftsegments+3*j+0);
			node2=(int)*(riftsegments+3*j+1);
			/*Summary, el1 and el2 are facing one another across the rift. node1 and node2 belong to el1 and 
			 *are located on the rift. Find node3 and node4, nodes belonging to el2 and located on the rift: */
			for (k=0;k<numsegs;k++){
				if (*(riftsegments+3*k+2)==el2){
					node3=(int)*(riftsegments+3*k+0);
					node4=(int)*(riftsegments+3*k+1);
					break;
				}
			}
			/* Make sure node3 faces node1 and node4 faces node2: */
			_assert_(node1<nods+1 && node4<nods+1);
			_assert_(node1>0 && node4>0);
			if ((x[node1-1]==x[node4-1]) && (y[node1-1]==y[node4-1])){
				/*Swap node3 and node4:*/
				temp_node=node3;
				node3=node4;
				node4=temp_node;
			}

			/*Figure out if a tip is on this element: */
			if (node3==node1){
				/*node1 is a tip*/
				if (tip1==-1) {
					tip1=node1;
					continue;
				}
				if ((tip2==-1) && (node1!=tip1)){
					tip2=node1;
					break;
				}
			}
		
			if (node4==node2){
				/*node2 is a tip*/
				if (tip1==-1){
					tip1=node2;
					continue;
				}
				if ((tip2==-1) && (node2!=tip1)){
					tip2=node2;
					break;
				}
			}
		}

		/*Record tips in riftstips: */
		*(riftstips+2*i+0)=(double)tip1;
		*(riftstips+2*i+1)=(double)tip2;

		/*We have the two tips for this rift.  Go from tip1 to tip2, and figure out the order in which segments are sequential. 
		 *Because two elements are connected to tip1, we chose one first, which defines the direction we are rotating along the rift. */
		node=tip1;
		for (counter=0;counter<numsegs;counter++){
			for (j=0;j<numsegs;j++){
				node1=(int)*(riftsegments+3*j+0);
				node2=(int)*(riftsegments+3*j+1);
				
				if ((node1==node) || (node2==node)){
					/*Ok, this segment is connected to node, plug its index into order, unless we already plugged it before: */
					already_ordered=0;
					for (k=0;k<counter;k++){
						if(order[k]==j){
							already_ordered=1;
							break;
						}
					}
					if (!already_ordered){
						order[counter]=j;
						if(node1==node){
							node=node2;
						}
						else if(node2==node){
							node=node1;
						}
						break;
					}
				}
			}
		}

		/*Using the order vector, and the riftsegments_copy and riftspairs_copy, reorder the segments and the pairs: */
		for (j=0;j<numsegs;j++){
			_assert_(order[j]<numsegs);
			*(riftsegments_copy+3*j+0)=*(riftsegments+3*order[j]+0);
			*(riftsegments_copy+3*j+1)=*(riftsegments+3*order[j]+1);
			*(riftsegments_copy+3*j+2)=*(riftsegments+3*order[j]+2);
			*(riftpairs_copy+2*j+0)=*(riftpairs+2*order[j]+0);
			*(riftpairs_copy+2*j+1)=*(riftpairs+2*order[j]+1);
		}
		
		for (j=0;j<numsegs;j++){
			*(riftsegments+3*j+0)=*(riftsegments_copy+3*j+0);
			*(riftsegments+3*j+1)=*(riftsegments_copy+3*j+1);
			*(riftsegments+3*j+2)=*(riftsegments_copy+3*j+2);
			*(riftpairs+2*j+0)=*(riftpairs_copy+2*j+0);
			*(riftpairs+2*j+1)=*(riftpairs_copy+2*j+1);
		}

		xDelete<int>(order);
		xDelete<double>(riftsegments_copy);
		xDelete<double>(riftpairs_copy);

	}

	/*Assign output pointer:*/
	*priftstips=riftstips;
	return noerr;
}/*}}}*/
/*FUNCTION PenaltyPairs{{{*/
int PenaltyPairs(double*** priftspenaltypairs,int** priftsnumpenaltypairs,int numrifts,double** riftssegments,
		int* riftsnumsegs,double** riftspairs,double* riftstips,double* x,double* y){
		

	int noerr=1;
	int i,j,k,k0;

	double el1,el2,node1,node2,node3,node4;
	double temp_node;

	/*output: */
	double **riftspenaltypairs    = NULL;
	double  *riftpenaltypairs     = NULL;
	int     *riftsnumpenaltypairs = NULL;

	/*intermediary: */
	int numsegs;
	double* riftsegments=NULL;
	double* riftpairs=NULL;
	int counter;
	double normal[2];
	double length;
	int    k1,k2;

	/*Allocate: */
	riftspenaltypairs=xNew<double*>(numrifts);
	riftsnumpenaltypairs=xNew<int>(numrifts);

	for(i=0;i<numrifts;i++){
		numsegs=riftsnumsegs[i];
		riftsegments=riftssegments[i];
		riftpairs=riftspairs[i];

		/*allocate riftpenaltypairs, and riftnumpenaltypairs: */
		if((numsegs/2-1)!=0)riftpenaltypairs=xNewZeroInit<double>((numsegs/2-1)*RIFTPENALTYPAIRSWIDTH);
		
		/*Go through only one flank of the rifts, not counting the tips: */
		counter=0;
		for(j=0;j<(numsegs/2);j++){
			el1=*(riftpairs+2*j+0);
			el2=*(riftpairs+2*j+1);
			node1=*(riftsegments+3*j+0);
			node2=*(riftsegments+3*j+1);
			/*Find segment index to recover node3 and node4, facing node1 and node2: */
			k0=-1;
			for(k=0;k<numsegs;k++){
				if(*(riftsegments+3*k+2)==el2){
					k0=k;
					break;
				}
			}
			node3=*(riftsegments+3*k0+0);
			node4=*(riftsegments+3*k0+1);

			/* Make sure node3 faces node1 and node4 faces node2: */
			if ((x[(int)node1-1]==x[(int)node4-1]) && (y[(int)node1-1]==y[(int)node4-1])){
				/*Swap node3 and node4:*/
				temp_node=node3;
				node3=node4;
				node4=temp_node;
			}	
			/*Ok, we have node1 facing node3, and node2 facing node4. Compute the normal to 
			 *this segment, and its length: */
			normal[0]=cos(atan2(x[(int)node1-1]-x[(int)node2-1],y[(int)node2-1]-y[(int)node1-1]));
			normal[1]=sin(atan2(x[(int)node1-1]-x[(int)node2-1],y[(int)node2-1]-y[(int)node1-1]));
			length=sqrt(pow(x[(int)node2-1]-x[(int)node1-1],(double)2)+pow(y[(int)node2-1]-y[(int)node1-1],(double)2));

			/*Be careful here, we want penalty loads on each node, not on each segment. This means we cannot plug node1,
			 * node2, node3 and node4 directly into riftpenaltypairs. We need to include node1, node2, node3 and node4, 
			 * only once. We'll add the normals and the lengths : */

			if(node1!=node3){ //exclude tips from loads
				k1=-1;
				for(k=0;k<counter;k++){
					if( (*(riftpenaltypairs+k*7+0))==node1){
						k1=k; 
						break;
					}
				}
				if(k1==-1){
					*(riftpenaltypairs+counter*7+0)=node1;
					*(riftpenaltypairs+counter*7+1)=node3;
					*(riftpenaltypairs+counter*7+2)=el1;
					*(riftpenaltypairs+counter*7+3)=el2;
					*(riftpenaltypairs+counter*7+4)=normal[0];
					*(riftpenaltypairs+counter*7+5)=normal[1];
					*(riftpenaltypairs+counter*7+6)=length/2;
					counter++;
				}
				else{
					*(riftpenaltypairs+k1*7+4)+=normal[0];
					*(riftpenaltypairs+k1*7+5)+=normal[1];
					*(riftpenaltypairs+k1*7+6)+=length/2;
				}
			}
			if(node2!=node4){
				k2=-1;
				for(k=0;k<counter;k++){
					if( (*(riftpenaltypairs+k*7+0))==node2){
						k2=k;
						break;
					}
				}
				if(k2==-1){
					*(riftpenaltypairs+counter*7+0)=node2;
					*(riftpenaltypairs+counter*7+1)=node4;
					*(riftpenaltypairs+counter*7+2)=el1;
					*(riftpenaltypairs+counter*7+3)=el2;
					*(riftpenaltypairs+counter*7+4)=normal[0];
					*(riftpenaltypairs+counter*7+5)=normal[1];
					*(riftpenaltypairs+counter*7+6)=length/2;
					counter++;
				}
				else{
					*(riftpenaltypairs+k2*7+4)+=normal[0];
					*(riftpenaltypairs+k2*7+5)+=normal[1];
					*(riftpenaltypairs+k2*7+6)+=length/2;
				}
			}
		}
		/*Renormalize normals: */
		for(j=0;j<counter;j++){
			double magnitude=sqrt(pow( *(riftpenaltypairs+j*7+4),2) + pow( *(riftpenaltypairs+j*7+5),2) );
			*(riftpenaltypairs+j*7+4)=*(riftpenaltypairs+j*7+4)/magnitude;
			*(riftpenaltypairs+j*7+5)=*(riftpenaltypairs+j*7+5)/magnitude;
		}
		
		riftspenaltypairs[i]=riftpenaltypairs;
		riftsnumpenaltypairs[i]=(numsegs/2-1);
	}

	/*Assign output pointers: */
	*priftspenaltypairs=riftspenaltypairs;
	*priftsnumpenaltypairs=riftsnumpenaltypairs;
	return noerr;
}

/******************************************************************************************************************************
                                   RemoveCorners
******************************************************************************************************************************/

int RemoveCornersFromRifts(double** pindex,int* pnel,double** px,double** py,int* pnods, double* segments,double* segmentmarkers,int num_seg){

	int noerr=1;
	int i,j,k;
	double node1,node2,node3;
	int el;

	/*input: */
	double* index=NULL;
	int     nel;
	double* x=NULL;
	double* y=NULL;
	int     nods;
	double  pair[2];
	int     pair_count=0;
	int     triple=0;

	/*Recover input: */
	index=*pindex;
	nel=*pnel;
	x=*px;
	y=*py;
	nods=*pnods;


	for (i=0;i<num_seg;i++){
		node1=*(segments+3*i+0);
		node2=*(segments+3*i+1);
		/*Find all elements connected to [node1 node2]: */
		pair_count=0;
		for (j=0;j<nel;j++){
			if (*(index+3*j+0)==node1){
				if ((*(index+3*j+1)==node2) || (*(index+3*j+2)==node2)){
					pair[pair_count]=j;
					pair_count++;
				}
			}
			if (*(index+3*j+1)==node1){
				if ((*(index+3*j+0)==node2) || (*(index+3*j+2)==node2)){
					pair[pair_count]=j;
					pair_count++;
				}
			}
			if (*(index+3*j+2)==node1){
				if ((*(index+3*j+0)==node2) || (*(index+3*j+1)==node2)){
					pair[pair_count]=j;
					pair_count++;
				}
			}
		}
		/*Ok, we have pair_count elements connected to this segment. For each of these elements, 
		 *figure out if the third node also belongs to a segment: */
		if ((pair_count==0) || (pair_count==1)){ //we only select the rift segments, which belong to  2 elements
			continue;
		}
		else{
			for (j=0;j<pair_count;j++){
				el=(int)pair[j];
				triple=0;
				/*First find node3: */
				if (*(index+3*el+0)==node1){
					if (*(index+3*el+1)==node2)node3=*(index+3*el+2);
					else node3=*(index+3*el+1);
				}
				if (*(index+3*el+1)==node1){
					if (*(index+3*el+0)==node2)node3=*(index+3*el+2);
					else node3=*(index+3*el+0);
				}
				if (*(index+3*el+2)==node1){
					if (*(index+3*el+0)==node2)node3=*(index+3*el+1);
					else node3=*(index+3*el+0);
				}
				/*Ok, we have node3. Does node3 belong to a segment? : */
				for (k=0;k<num_seg;k++){
					if ((node3==*(segments+3*k+0)) || (node3==*(segments+3*k+1))){
						triple=1;
						break;
					}
				}
				if(triple==1){
					/*el is a corner element: we need to split it in 3 triangles: */
					x=(double*)xrealloc(x,(nods+1)*sizeof(double));
					y=(double*)xrealloc(y,(nods+1)*sizeof(double));
					x[nods]=(x[(int)node1-1]+x[(int)node2-1]+x[(int)node3-1])/3;
					y[nods]=(y[(int)node1-1]+y[(int)node2-1]+y[(int)node3-1])/3;

					index=(double*)xrealloc(index,(nel+2)*3*sizeof(double));
					/*First, reassign element el: */
					*(index+3*el+0)=node1;
					*(index+3*el+1)=node2;
					*(index+3*el+2)=nods+1;
					/*Other two elements: */
					*(index+3*nel+0)=node2;
					*(index+3*nel+1)=node3;
					*(index+3*nel+2)=nods+1;

					*(index+3*(nel+1)+0)=node3;
					*(index+3*(nel+1)+1)=node1;
					*(index+3*(nel+1)+2)=nods+1;
					/*we need  to change the segment elements corresponding to el: */
					for (k=0;k<num_seg;k++){
						if (*(segments+3*k+2)==(double)(el+1)){
							if ( ((*(segments+3*k+0)==node1) && (*(segments+3*k+1)==node2)) || ((*(segments+3*k+0)==node2) && (*(segments+3*k+1)==node1))) *(segments+3*k+2)=(double)(el+1);
							if ( ((*(segments+3*k+0)==node2) && (*(segments+3*k+1)==node3)) || ((*(segments+3*k+0)==node3) && (*(segments+3*k+1)==node2))) *(segments+3*k+2)=(double)(nel+1);
							if ( ((*(segments+3*k+0)==node3) && (*(segments+3*k+1)==node1)) || ((*(segments+3*k+0)==node1) && (*(segments+3*k+1)==node3))) *(segments+3*k+2)=(double)(nel+2);
						}
					}

					nods=nods+1;
					nel=nel+2;
					i=0;
					break;
				}
			} //for (j=0;j<pair_count;j++)
		}
	}// for (i=0;i<num_seg;i++)

	/*Assign output pointers: */
	*pindex=index;
	*pnel=nel;
	*px=x;
	*py=y;
	*pnods=nods;
	return noerr;
}
