#include <math.h>
#include <SST/SST_Vec2.h>
#include <SST/SST_Vec3.h>
#include <SST/SST_Math.h>
#include <SST/SST_Geo.h>

/*
    Helper function to find intersection(s) of two line segments.
    This is adapted from "Geometric Tools For Computer Graphpics", p. 244.

    Note that the segments are being represented here by a start point and a distance offset vector,
    P and D, assuming that the full segment can be represented by the equation:
    
        L(t) = P + tD, where t is on the range [0,1]
*/

#define sqrEpsilon (0.0001f)

static SST_2DLineSegmentIntersectType doFindIntersection( float u0, float u1, float v0, float v1, float w[2])
{
    if ( u1 < v0 || u0 > v1 )
    {
        return SST2DLINESEGINTERSECT_NONE;
    }

    if ( u1 > v0 )
    {
        if (u0 < v1)
        {
            w[0] = (u0 < v0) ? (v0) : (u0);
            w[1] = (u1 > v1) ? (v1) : (u1);
            return SST2DLINESEGINTERSECT_OVERLAP;
        }
        else
        {
            w[0] = u0;
            return SST2DLINESEGINTERSECT_POINT;
        }        
    }
    else
    {
        w[0] = u1;
        return SST2DLINESEGINTERSECT_POINT;
    }
}

SST_2DLineSegmentIntersectType doSegmentSegmentIntersectionF(   const SST_Vec2f* RESTRICT P0,
                                                                const SST_Vec2f* RESTRICT D0,
                                                                const SST_Vec2f* RESTRICT P1,
                                                                const SST_Vec2f* RESTRICT D1,
                                                                SST_Vec2f* RESTRICT overlaps)
{
    SST_Vec2f e;
    float kross;
    float kross2;
    float sqrKross;
    float sqrKross2;
    float sqrLen0;
    float sqrLen1;
    float sqrLenE;
    float s;
    float t;
    float s0;
    float s1;
    float w[2];
    float smin;
    float smax;
    SST_2DLineSegmentIntersectType itype;

    SST_Math_Vec2fSubtract(P1, P0, &e);

    /*
    Note on the kross operator:

    Kross(a,b) = (ax*by) - (bx*ay)
    */
    kross = ((D0->x)*(D1->y)) - ((D0->y)*(D1->x));
    sqrKross = kross * kross;
    sqrLen0 = ((D0->x) * (D0->x)) + ((D0->y) * (D0->y));
    sqrLen1 = ((D1->x) * (D1->x)) + ((D1->y) * (D1->y));

    if (sqrKross > sqrEpsilon * sqrLen0 * sqrLen1)
    {
        s = ((e.x * D1->y) - (e.y * D1->x)) / kross;
        if (s < 0.0f || s > 1.0f)
        {
            return SST2DLINESEGINTERSECT_NONE;
        }

        t = ((e.x * D0->y) - (e.y * D0->x)) / kross;
        if (t < 0.0f || t > 1.0f)
        {
            return SST2DLINESEGINTERSECT_NONE;
        }

        SST_Math_Vec2fWeightedSum(P0, 1.0f, D0, s, overlaps);

        return SST2DLINESEGINTERSECT_POINT;
    }

    sqrLenE = (e.x * e.x) + (e.y * e.y);

    kross2 = ((e.x)*(D0->y)) - ((e.y)*(D0->x));
    sqrKross2 = kross2 * kross2;

    if (sqrKross2 > sqrEpsilon * sqrLen0 * sqrLenE )
    {
        return SST2DLINESEGINTERSECT_NONE;
    }

    s0 = SST_Math_Vec2fDot( D0, &e) / sqrLen0;
    s1 = s0 + SST_Math_Vec2fDot( D0, D1) / sqrLen0;
    smin = s0 < s1 ? s0 : s1;
    smax = s0 > s1 ? s0 : s1;

    itype = doFindIntersection(0.0f, 1.0f, smin, smax, w);

    switch (itype) {
        case SST2DLINESEGINTERSECT_NONE: break;
        case SST2DLINESEGINTERSECT_POINT:
                                            SST_Math_Vec2fWeightedSum(P0, 1.0f, D0, w[0], &(overlaps[0]) );

                                            break;
        case SST2DLINESEGINTERSECT_OVERLAP:
                                            SST_Math_Vec2fWeightedSum(P0, 1.0f, D0, w[0], &(overlaps[0]) );
                                            SST_Math_Vec2fWeightedSum(P0, 1.0f, D0, w[1], &(overlaps[1]) );

                                            break;
        default: break;
    }

    return itype;
}

/*
    Main driver function, mostly just handles moving from the way we think about line segments,
    using start and end points, to the way the routine thinks about things, using origin and offset.

*/
SST_2DLineSegmentIntersectType SST_Geo_2DSegmentSegmentIntersectionF(  const SST_Vec2f* RESTRICT Astart,
                                                                       const SST_Vec2f* RESTRICT Aend,
                                                                       const SST_Vec2f* RESTRICT Bstart,
                                                                       const SST_Vec2f* RESTRICT Bend,
                                                                       SST_Vec2f* RESTRICT overlaps)
{
    SST_Vec2f p0;   // starting point of first line segment
    SST_Vec2f d0;   // the offset of the first line segment
    SST_Vec2f p1;   // starting point of the second line segment
    SST_Vec2f d1;   // the offset of the second line segment

    // store starting points of rays
    memmove( &p0, Astart, sizeof(SST_Vec2f) );
    memmove( &p1, Bstart, sizeof(SST_Vec2f) );

    // grab the offset of the segment from the origin
    SST_Math_Vec2fSubtract(Aend, Astart, &d0);
    SST_Math_Vec2fSubtract(Bend, Bstart, &d1);

    return doSegmentSegmentIntersectionF(&p0, &d0, &p1, &d1, overlaps);
}

float PointToPoint2f(const SST_Vec2f* a,const SST_Vec2f* b)
{
	SST_Vec2f temp;
	SST_Math_Vec2fSubtract(a,b,&temp);
	return SST_Math_Vec2fMagnitude(&temp);
}
float PointToPointSquared2f(const SST_Vec2f* a,const SST_Vec2f* b)
{
	SST_Vec2f temp;
	SST_Math_Vec2fSubtract(a,b,&temp);
	return SST_Math_Vec2fMagnitudeSquared(&temp);
}
double PointToPoint2d(const SST_Vec2d* a,const SST_Vec2d* b)
{
	SST_Vec2d temp;
	SST_Math_Vec2dSubtract(a,b,&temp);
	return SST_Math_Vec2dMagnitude(&temp);
}
double PointToPointSquared2d(const SST_Vec2d* a,const SST_Vec2d* b)
{
	SST_Vec2d temp;
	SST_Math_Vec2dSubtract(a,b,&temp);
	return SST_Math_Vec2dMagnitudeSquared(&temp);
}
float PointToLineSegment2f(SST_Vec2f* close, const SST_Vec2f* a, const SST_Vec2f* b)
{
	/* Line segment b is defined with two points b[0:1] and b[2:3] */
	SST_Vec2f temp;
	float t;
	float distsqr;

	temp.v[0] = b[1].v[0]-b[0].v[0];
	temp.v[1] = b[1].v[1]-b[0].v[1];
	distsqr = SST_Math_Vec2fMagnitudeSquared(&temp);
	t = ((a->v[0]-b[0].v[0]) * temp.v[0] + (a->v[1]-b[0].v[1]) * temp.v[1] ) / distsqr;
	if(t>1.f)
		t = 1.f;
	if(t<0.f)
		t = 0.f;

	/* close[i] = b[i]+ t * temp[i]; */
	SST_Math_Vec2fScale(&temp,t,close);
	SST_Math_Vec2fAddLocal(close,b);

	/* temp[i] = close[i]-a[i]; */
	SST_Math_Vec2fSubtract(close,a,&temp);
	
	return SST_Math_Vec2fMagnitude(&temp);
}
double PointToLineSegment2d(SST_Vec2d* close, const SST_Vec2d* a, const SST_Vec2d* b)
{
	/* Line segment b is defined with two points b[0:1] and b[2:3] */
	SST_Vec2d temp;
	double t;
	double distsqr;
	temp.v[0] = b[1].v[0]-b[0].v[0];
	temp.v[1] = b[1].v[1]-b[0].v[1];
	distsqr = SST_Math_Vec2dMagnitudeSquared(&temp);
	t = ((a->v[0]-b[0].v[0])*temp.v[0] + (a->v[1]-b[0].v[1])*temp.v[1]) / distsqr;
	if(t>1.0)
		t = 1.0;
	if(t<0.0)
		t = 0.0;

	/* close[i] = b[i]+ t * temp[i]; */
	SST_Math_Vec2dScale(&temp,t,close);
	SST_Math_Vec2dAddLocal(close,b);

	//temp[i] = close[i]-a[i];
	SST_Math_Vec2dSubtract(close,a,&temp);
	
	return SST_Math_Vec2dMagnitude(&temp);
}

float PointToPoint3f(const SST_Vec3f* a,const SST_Vec3f* b)
{
	SST_Vec3f temp;
	SST_Math_Vec3fSubtract(a,b,&temp);
	return SST_Math_Vec3fMagnitude(&temp);
}
float PointToPointSquared3f(const SST_Vec3f* a,const SST_Vec3f* b)
{
	SST_Vec3f temp;
	SST_Math_Vec3fSubtract(a,b,&temp);
	return SST_Math_Vec3fMagnitudeSquared(&temp);
}
float PointToLineSegment3f(SST_Vec3f* close, const SST_Vec3f* a, const SST_Vec3f* b)
{
	/* Line segment b is defined with two points b[0:1] and b[2:3] */
	SST_Vec3f temp;
	float t;
	float distsqr;
	temp.v[0] = b[1].v[0]-b[0].v[0];
	temp.v[1] = b[1].v[1]-b[1].v[1];
	temp.v[2] = b[1].v[2]-b[2].v[2];
	distsqr = SST_Math_Vec3fMagnitudeSquared(&temp);
	t = ((a->v[0]-b[0].v[0])*temp.v[0] + (a->v[1]-b[0].v[1])*temp.v[1]+(a->v[2]-b[0].v[2])*temp.v[2]) / distsqr;
	if(t>1.f)
		t = 1.f;
	if(t<0.f)
		t = 0.f;

	/* close[i] = b[i]+ t * temp[i]; */
	SST_Math_Vec3fScale(&temp,t,close);
	SST_Math_Vec3fAddLocal(close,b);

	/* temp[i] = close[i]-a[i]; */
	SST_Math_Vec3fSubtract(close,a,&temp);
	
	return SST_Math_Vec3fMagnitude(&temp);
}
double PointToPoint3d(const SST_Vec3d* a,const SST_Vec3d* b)
{
	SST_Vec3d temp;
	SST_Math_Vec3dSubtract(a,b,&temp);
	return SST_Math_Vec3dMagnitude(&temp);
}
double PointToPointSquared3d(const SST_Vec3d* a,const SST_Vec3d* b)
{
	SST_Vec3d temp;
	SST_Math_Vec3dSubtract(a,b,&temp);
	return SST_Math_Vec3dMagnitudeSquared(&temp);
}
double PointToLineSegment3d(SST_Vec3d* close, const SST_Vec3d* a, const SST_Vec3d* b)
{
	/* Line segment b is defined with two points b[0:1] and b[2:3] */
	SST_Vec3d temp;
	double t;
	double distsqr;
	temp.v[0] = b[1].v[0]-b[0].v[0];
	temp.v[1] = b[1].v[1]-b[0].v[1];
	temp.v[2] = b[1].v[2]-b[0].v[2];
	distsqr = SST_Math_Vec3dMagnitudeSquared(&temp);
	t = ((a->v[0]-b[0].v[0])*temp.v[0] + (a->v[1]-b[0].v[1])*temp.v[1]+(a->v[2]-b[0].v[2])*temp.v[2]) / distsqr;
	if(t>1.0)
		t = 1.0;
	if(t<0.0)
		t = 0.0;

	/* close[i] = b[i]+ t * temp[i]; */
	SST_Math_Vec3dScale(&temp,t,close);
	SST_Math_Vec3dAddLocal(close,b);

	/* temp[i] = close[i]-a[i]; */
	SST_Math_Vec3dSubtract(close,a,&temp);
	
	return SST_Math_Vec3dMagnitude(&temp);
}

double PointToPlane3(const SST_Vec3d* a, const SST_Vec3d plane[3])
{
#pragma message("PointToPlane3-NOT IMPLEMENTED YET")
	return (double)(0.0);
}
#include <stdio.h>
bool SST_Geo_RayPlaneIntersectionD(const SST_Vec3d* RESTRICT ray, const SST_Vec3d* RESTRICT ray_point, const SST_Vec3d* RESTRICT p0, const SST_Vec3d* RESTRICT p1,const SST_Vec3d* RESTRICT p2, SST_Vec3d* RESTRICT Intersection)
{
	/* 
	 *  The x=inv(A)*b
	 [t; u; v] = [l_a-l_b, p1-p0, p2-p0]^-1 * [l_a-p0] 
	Source: Wiki
	In this formulation (to save on computation, t is actually -t from the wiki)
	 */

	SST_Vec3d  s;
	SST_Mat33d A;
	double t;
	bool inTriangle;
	inTriangle = false;

	/* First column */
	SST_Math_Mat33dFromVec3d(&A,ray_point,0);

	/* Second column */
	SST_Math_Vec3dSubtract(p1,p0,&s);
	SST_Math_Mat33dFromVec3d(&A,&s,1);

	/* Third column */
	SST_Math_Vec3dSubtract(p2,p0,&s);
	SST_Math_Mat33dFromVec3d(&A,&s,2);

	/* form b */
	SST_Math_Vec3dSubtract(ray,p0,&s);
	SST_Math_Mat33dCreateLULocal(&A);
	SST_Math_Mat33dApplyLUVec(&A,&s,Intersection);

	/* if u,v in [0,1] and (u+v) <= 1:
	 * ==> then the intersection point is in the plane inside the triangle spanned by the three points p_{i}
	 */
#define SST_Math_RangeContains(x,a,b) ( ( (x) >= a ) & ( (x) <= b ) )
	//printf("t %f, u %f, v %f\n",Intersection->x, Intersection->y, Intersection->z);
	if(SST_Math_RangeContains(Intersection->v[1],0.0,1.0) & SST_Math_RangeContains(Intersection->v[2],0.0,1.0) & SST_Math_RangeContains(Intersection->v[1]+Intersection->v[2],0.0,1.0)) { 
		inTriangle = true; 
	}
#undef SST_Math_RangeContains
	/* calculate intersection point */
	t = -Intersection->v[0];
	/* ray_point + Intersection->v[0] * ray */
	SST_Math_Vec3dScale(ray,t,Intersection);
	SST_Math_Vec3dAddLocal(Intersection,ray_point);

	return inTriangle;
}
bool SST_Geo_RayPlaneIntersectionF(const SST_Vec3f* RESTRICT ray, const SST_Vec3f* RESTRICT ray_point, const SST_Vec3f* RESTRICT p0, const SST_Vec3f* RESTRICT p1,const SST_Vec3f* RESTRICT p2, SST_Vec3f* RESTRICT Intersection)
{
	/* 
	 *  The x=inv(A)*b
	 [t; u; v] = [l_a-l_b, p1-p0, p2-p0]^-1 * [l_a-p0] 
	Source: Wiki
	 */

	SST_Vec3f  s;
	SST_Mat33f A;
	float t;
	bool inTriangle;
	inTriangle = false;

	/* First column */
	SST_Math_Mat33fFromVec3f(&A,ray_point,0);

	/* Second column */
	SST_Math_Vec3fSubtract(p1,p0,&s);
	SST_Math_Mat33fFromVec3f(&A,&s,1);

	/* Third column */
	SST_Math_Vec3fSubtract(p2,p0,&s);
	SST_Math_Mat33fFromVec3f(&A,&s,2);

	/* form b */
	SST_Math_Vec3fSubtract(ray,p0,&s);
	SST_Math_Mat33fCreateLULocal(&A);
	SST_Math_Mat33fApplyLUVec(&A,&s,Intersection);

	/* if u,v in [0,1] and (u+v) <= 1:
	 * ==> then the intersection point is in the plane inside the triangle spanned by the three points p_{i}
	 */
#define SST_Math_RangeContains(x,a,b) ( ( (x) >= a ) & ( (x) <= b ) )
	//printf("t %f, u %f, v %f\n",Intersection->x, Intersection->y, Intersection->z);
	if(SST_Math_RangeContains(Intersection->v[1],0.0f,1.0f) & SST_Math_RangeContains(Intersection->v[2],0.0f,1.0f) & SST_Math_RangeContains(Intersection->v[1]+Intersection->v[2],0.0f,1.0f)) { 
		inTriangle = true; 
	}
#undef SST_Math_RangeContains
	/* calculate intersection point */
	t = -Intersection->v[0];
	/* ray_point + Intersection->v[0] * ray */
	SST_Math_Vec3fScale(ray,t,Intersection);
	SST_Math_Vec3fAddLocal(Intersection,ray_point);

	return inTriangle;
}