/*! \file  SegmentIntersect.cpp
*/

#include "./MeshProfileIntersectionx.h"

int SegmentIntersect(double* palpha, double* pbeta, double* x1, double* y1, double* x2, double* y2){

	/*See ISSM_TIER/src/m/utils/Geometry/SegIntersect.m for matlab routine from which we take this routine: */

	/*output: */
	double alpha=-1;
	double beta=-1;
	
	double xA,xB,xC,xD,yA,yB,yC,yD;
	double O2A[2],O2B[2],O1C[2],O1D[2];
	double n1[2],n2[2];
	double test1, test2, test3, test4;
	double det;
	double O2O1[2];
	double pO1A,pO1B,pO1C,pO1D;

	xA=x1[0]; yA=y1[0];
	xB=x1[1]; yB=y1[1];
	xC=x2[0]; yC=y2[0];
	xD=x2[1]; yD=y2[1];

	O2A[0]=xA -(xD/2+xC/2); O2A[1]=yA -(yD/2+yC/2);
	O2B[0]=xB -(xD/2+xC/2); O2B[1]=yB -(yD/2+yC/2);
	O1C[0]=xC -(xA/2+xB/2); O1C[1]=yC -(yA/2+yB/2);
	O1D[0]=xD -(xA/2+xB/2); O1D[1]=yD -(yA/2+yB/2);


	n1[0]=yA-yB; n1[1]=xB-xA;  //normal vector to segA
	n2[0]=yC-yD; n2[1]=xD-xC;  //normal vector to segB

	test1=n2[0]*O2A[0]+n2[1]*O2A[1];
	test2=n2[0]*O2B[0]+n2[1]*O2B[1];

	if (test1*test2>0){
		return SeparateEnum;
	}

	test3=n1[0]*O1C[0]+n1[1]*O1C[1];
	test4=n1[0]*O1D[0]+n1[1]*O1D[1];

	if (test3*test4>0){
		return SeparateEnum;
	}

	/*If colinear: */
	det=n1[0]*n2[1]-n2[0]*y1[1];

	if(test1*test2==0 && test3*test4==0 && det==0){

		//projection on the axis O1O2
		O2O1[0]=(xA/2+xB/2)-(xD/2+xC/2);
		O2O1[1]=(yA/2+yB/2)-(yD/2+yC/2);

		pO1A=O2O1[0]*(O2A[0]-O2O1[0])+O2O1[1]*(O2A[1]-O2O1[1]);
		pO1B=O2O1[0]*(O2B[0]-O2O1[0])+O2O1[1]*(O2B[1]-O2O1[1]);
		pO1C=O2O1[0]*O1C[0]+O2O1[1]*O1C[1];
		pO1D=O2O1[0]*O1D[0]+O2O1[1]*O1D[1];

		//test if one point is included in the other segment (->intersects=true)
		if ((pO1C-pO1A)*(pO1D-pO1A)<0){
			alpha=0; beta=0;
			*palpha=alpha;*pbeta=beta;
			return ColinearEnum;
		}
		if ((pO1C-pO1B)*(pO1D-pO1B)<0){
			alpha=0; beta=0;
			*palpha=alpha;*pbeta=beta;
			return ColinearEnum;
		}
		if ((pO1A-pO1C)*(pO1B-pO1C)<0){
			alpha=0; beta=0;
			*palpha=alpha;*pbeta=beta;
			return ColinearEnum;
		}
		if ((pO1A-pO1D)*(pO1B-pO1D)<0){
			alpha=0; beta=0;
			*palpha=alpha;*pbeta=beta;
			return ColinearEnum;
		}

		//test if the 2 segments have the same middle (->intersects=true)
		if (O2O1==0){
			alpha=0; beta=0;
			*palpha=alpha;*pbeta=beta;
			return ColinearEnum;
		}

		//if we are here, both segments are colinear, but do not interset:
		alpha=-1; beta=-1;
		*palpha=alpha;*pbeta=beta;
		return SeparateEnum;
	}

	/*if we are here, both segments intersect. Determine where in the segment coordinate 
	 * system: */
	beta=-1;
	alpha=-(xA*yB-xC*yB+yC*xB-yC*xA+xC*yA-yA*xB)/(-xD*yB+xD*yA+xC*yB-xC*yA-yD*xA+yD*xB+yC*xA-yC*xB); //from intersect.m in formal calculus

	*palpha=alpha;*pbeta=beta;
	return IntersectEnum;
}