/*
* Copyright (c) 2007 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty.  In no event will the authors be held liable for any damages
* arising from the use of this software.
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
* 3. This notice may not be removed or altered from any source distribution.
*/

#include "b2Collision.h"
#include "Shapes/b2CircleShape.h"
#include "Shapes/b2PolygonShape.h"
#include "Shapes/b2EdgeShape.h"

int32 g_GJK_Iterations = 0;

// GJK using Voronoi regions (Christer Ericson) and region selection
// optimizations (Casey Muratori).

// The origin is either in the region of points[1] or in the edge region. The origin is
// not in region of points[0] because that is the old point.
static int32 ProcessTwo(b2Vec2* x1, b2Vec2* x2, b2Vec2* p1s, b2Vec2* p2s, b2Vec2* points)
{
	// If in point[1] region
	b2Vec2 r = -points[1];
	b2Vec2 d = points[0] - points[1];
	float32 length = d.Normalize();
	float32 lambda = b2Dot(r, d);
	if (lambda <= 0.0f || length < B2_FLT_EPSILON)
	{
		// The simplex is reduced to a point.
		*x1 = p1s[1];
		*x2 = p2s[1];
		p1s[0] = p1s[1];
		p2s[0] = p2s[1];
		points[0] = points[1];
		return 1;
	}

	// Else in edge region
	lambda /= length;
	*x1 = p1s[1] + lambda * (p1s[0] - p1s[1]);
	*x2 = p2s[1] + lambda * (p2s[0] - p2s[1]);
	return 2;
}

// Possible regions:
// - points[2]
// - edge points[0]-points[2]
// - edge points[1]-points[2]
// - inside the triangle
static int32 ProcessThree(b2Vec2* x1, b2Vec2* x2, b2Vec2* p1s, b2Vec2* p2s, b2Vec2* points)
{
	b2Vec2 a = points[0];
	b2Vec2 b = points[1];
	b2Vec2 c = points[2];

	b2Vec2 ab = b - a;
	b2Vec2 ac = c - a;
	b2Vec2 bc = c - b;

	float32 sn = -b2Dot(a, ab), sd = b2Dot(b, ab);
	float32 tn = -b2Dot(a, ac), td = b2Dot(c, ac);
	float32 un = -b2Dot(b, bc), ud = b2Dot(c, bc);

	// In vertex c region?
	if (td <= 0.0f && ud <= 0.0f)
	{
		// Single point
		*x1 = p1s[2];
		*x2 = p2s[2];
		p1s[0] = p1s[2];
		p2s[0] = p2s[2];
		points[0] = points[2];
		return 1;
	}

	// Should not be in vertex a or b region.
	B2_NOT_USED(sd);
	B2_NOT_USED(sn);
	b2Assert(sn > 0.0f || tn > 0.0f);
	b2Assert(sd > 0.0f || un > 0.0f);

	float32 n = b2Cross(ab, ac);

#ifdef TARGET_FLOAT32_IS_FIXED
	n = (n < 0.0)? -1.0 : ((n > 0.0)? 1.0 : 0.0);
#endif

	// Should not be in edge ab region.
	float32 vc = n * b2Cross(a, b);
	b2Assert(vc > 0.0f || sn > 0.0f || sd > 0.0f);

	// In edge bc region?
	float32 va = n * b2Cross(b, c);
	if (va <= 0.0f && un >= 0.0f && ud >= 0.0f && (un+ud) > 0.0f)
	{
		b2Assert(un + ud > 0.0f);
		float32 lambda = un / (un + ud);
		*x1 = p1s[1] + lambda * (p1s[2] - p1s[1]);
		*x2 = p2s[1] + lambda * (p2s[2] - p2s[1]);
		p1s[0] = p1s[2];
		p2s[0] = p2s[2];
		points[0] = points[2];
		return 2;
	}

	// In edge ac region?
	float32 vb = n * b2Cross(c, a);
	if (vb <= 0.0f && tn >= 0.0f && td >= 0.0f && (tn+td) > 0.0f)
	{
		b2Assert(tn + td > 0.0f);
		float32 lambda = tn / (tn + td);
		*x1 = p1s[0] + lambda * (p1s[2] - p1s[0]);
		*x2 = p2s[0] + lambda * (p2s[2] - p2s[0]);
		p1s[1] = p1s[2];
		p2s[1] = p2s[2];
		points[1] = points[2];
		return 2;
	}

	// Inside the triangle, compute barycentric coordinates
	float32 denom = va + vb + vc;
	b2Assert(denom > 0.0f);
	denom = 1.0f / denom;

#ifdef TARGET_FLOAT32_IS_FIXED
	*x1 = denom * (va * p1s[0] + vb * p1s[1] + vc * p1s[2]);
	*x2 = denom * (va * p2s[0] + vb * p2s[1] + vc * p2s[2]);
#else
	float32 u = va * denom;
	float32 v = vb * denom;
	float32 w = 1.0f - u - v;
	*x1 = u * p1s[0] + v * p1s[1] + w * p1s[2];
	*x2 = u * p2s[0] + v * p2s[1] + w * p2s[2];
#endif
	return 3;
}

static bool InPoints(const b2Vec2& w, const b2Vec2* points, int32 pointCount)
{
	const float32 k_tolerance = 100.0f * B2_FLT_EPSILON;
	for (int32 i = 0; i < pointCount; ++i)
	{
		b2Vec2 d = b2Abs(w - points[i]);
		b2Vec2 m = b2Max(b2Abs(w), b2Abs(points[i]));
		
		if (d.x < k_tolerance * (m.x + 1.0f) &&
			d.y < k_tolerance * (m.y + 1.0f))
		{
			return true;
		}
	}

	return false;
}

template <typename T1, typename T2>
float32 DistanceGeneric(b2Vec2* x1, b2Vec2* x2,
				   const T1* shape1, const b2XForm& xf1,
				   const T2* shape2, const b2XForm& xf2)
{
	b2Vec2 p1s[3], p2s[3];
	b2Vec2 points[3];
	int32 pointCount = 0;

	*x1 = shape1->GetFirstVertex(xf1);
	*x2 = shape2->GetFirstVertex(xf2);

	float32 vSqr = 0.0f;
	const int32 maxIterations = 20;
	for (int32 iter = 0; iter < maxIterations; ++iter)
	{
		b2Vec2 v = *x2 - *x1;
		b2Vec2 w1 = shape1->Support(xf1, v);
		b2Vec2 w2 = shape2->Support(xf2, -v);

		vSqr = b2Dot(v, v);
		b2Vec2 w = w2 - w1;
		float32 vw = b2Dot(v, w);
		if (vSqr - vw <= 0.01f * vSqr || InPoints(w, points, pointCount)) // or w in points
		{
			if (pointCount == 0)
			{
				*x1 = w1;
				*x2 = w2;
			}
			g_GJK_Iterations = iter;
			return b2Sqrt(vSqr);
		}

		switch (pointCount)
		{
		case 0:
			p1s[0] = w1;
			p2s[0] = w2;
			points[0] = w;
			*x1 = p1s[0];
			*x2 = p2s[0];
			++pointCount;
			break;

		case 1:
			p1s[1] = w1;
			p2s[1] = w2;
			points[1] = w;
			pointCount = ProcessTwo(x1, x2, p1s, p2s, points);
			break;

		case 2:
			p1s[2] = w1;
			p2s[2] = w2;
			points[2] = w;
			pointCount = ProcessThree(x1, x2, p1s, p2s, points);
			break;
		}

		// If we have three points, then the origin is in the corresponding triangle.
		if (pointCount == 3)
		{
			g_GJK_Iterations = iter;
			return 0.0f;
		}

		float32 maxSqr = -B2_FLT_MAX;
		for (int32 i = 0; i < pointCount; ++i)
		{
			maxSqr = b2Max(maxSqr, b2Dot(points[i], points[i]));
		}

#ifdef TARGET_FLOAT32_IS_FIXED
		if (pointCount == 3 || vSqr <= 5.0*B2_FLT_EPSILON * maxSqr)
#else
		if (vSqr <= 100.0f * B2_FLT_EPSILON * maxSqr)
#endif
		{
			g_GJK_Iterations = iter;
			v = *x2 - *x1;
			vSqr = b2Dot(v, v);
			return b2Sqrt(vSqr);
		}
	}

	g_GJK_Iterations = maxIterations;
	return b2Sqrt(vSqr);
}

static float32 DistanceCC(
	b2Vec2* x1, b2Vec2* x2,
	const b2CircleShape* circle1, const b2XForm& xf1,
	const b2CircleShape* circle2, const b2XForm& xf2)
{
	b2Vec2 p1 = b2Mul(xf1, circle1->GetLocalPosition());
	b2Vec2 p2 = b2Mul(xf2, circle2->GetLocalPosition());

	b2Vec2 d = p2 - p1;
	float32 dSqr = b2Dot(d, d);
	float32 r1 = circle1->GetRadius() - b2_toiSlop;
	float32 r2 = circle2->GetRadius() - b2_toiSlop;
	float32 r = r1 + r2;
	if (dSqr > r * r)
	{
		float32 dLen = d.Normalize();
		float32 distance = dLen - r;
		*x1 = p1 + r1 * d;
		*x2 = p2 - r2 * d;
		return distance;
	}
	else if (dSqr > B2_FLT_EPSILON * B2_FLT_EPSILON)
	{
		d.Normalize();
		*x1 = p1 + r1 * d;
		*x2 = *x1;
		return 0.0f;
	}

	*x1 = p1;
	*x2 = *x1;
	return 0.0f;
}

static float32 DistanceEdgeCircle(
	b2Vec2* x1, b2Vec2* x2,
	const b2EdgeShape* edge, const b2XForm& xf1,
	const b2CircleShape* circle, const b2XForm& xf2)
{
	b2Vec2 vWorld;
	b2Vec2 d;
	float32 dSqr;
	float32 dLen;
	float32 r = circle->GetRadius() - b2_toiSlop;
	b2Vec2 cWorld = b2Mul(xf2, circle->GetLocalPosition());
	b2Vec2 cLocal = b2MulT(xf1, cWorld);
	float32 dirDist = b2Dot(cLocal - edge->GetCoreVertex1(), edge->GetDirectionVector());
	if (dirDist <= 0.0f) {
		vWorld = b2Mul(xf1, edge->GetCoreVertex1());
	} else if (dirDist >= edge->GetLength()) {
		vWorld = b2Mul(xf1, edge->GetCoreVertex2());
	} else {
		*x1 = b2Mul(xf1, edge->GetCoreVertex1() + dirDist * edge->GetDirectionVector());
		dLen = b2Dot(cLocal - edge->GetCoreVertex1(), edge->GetNormalVector());
		if (dLen < 0.0f) {
			if (dLen < -r) {
				*x2 = b2Mul(xf1, cLocal + r * edge->GetNormalVector());
				return -dLen - r;
			} else {
				*x2 = *x1;
				return 0.0f;
			}
		} else {
			if (dLen > r) {
				*x2 = b2Mul(xf1, cLocal  - r * edge->GetNormalVector());
				return dLen - r;
			} else {
				*x2 = *x1;
				return 0.0f;
			}
		}
	}
	
	*x1 = vWorld;
	d = cWorld - vWorld;
	dSqr = b2Dot(d, d);
	if (dSqr > r * r) {
		dLen = d.Normalize();
		*x2 = cWorld - r * d;
		return dLen - r;
	} else {
		*x2 = vWorld;
		return 0.0f;
	}
}

// This is used for polygon-vs-circle distance.
struct Point
{
	b2Vec2 Support(const b2XForm&, const b2Vec2&) const
	{
		return p;
	}

	b2Vec2 GetFirstVertex(const b2XForm&) const
	{
		return p;
	}
	
	b2Vec2 p;
};

// GJK is more robust with polygon-vs-point than polygon-vs-circle.
// So we convert polygon-vs-circle to polygon-vs-point.
static float32 DistancePC(
	b2Vec2* x1, b2Vec2* x2,
	const b2PolygonShape* polygon, const b2XForm& xf1,
	const b2CircleShape* circle, const b2XForm& xf2)
{
	Point point;
	point.p = b2Mul(xf2, circle->GetLocalPosition());

	float32 distance = DistanceGeneric(x1, x2, polygon, xf1, &point, b2XForm_identity);

	float32 r = circle->GetRadius() - b2_toiSlop;

	if (distance > r)
	{
		distance -= r;
		b2Vec2 d = *x2 - *x1;
		d.Normalize();
		*x2 -= r * d;
	}
	else
	{
		distance = 0.0f;
		*x2 = *x1;
	}

	return distance;
}

float32 b2Distance(b2Vec2* x1, b2Vec2* x2,
				   const b2Shape* shape1, const b2XForm& xf1,
				   const b2Shape* shape2, const b2XForm& xf2)
{
	b2ShapeType type1 = shape1->GetType();
	b2ShapeType type2 = shape2->GetType();

	if (type1 == e_circleShape && type2 == e_circleShape)
	{
		return DistanceCC(x1, x2, (b2CircleShape*)shape1, xf1, (b2CircleShape*)shape2, xf2);
	}
	
	if (type1 == e_polygonShape && type2 == e_circleShape)
	{
		return DistancePC(x1, x2, (b2PolygonShape*)shape1, xf1, (b2CircleShape*)shape2, xf2);
	}

	if (type1 == e_circleShape && type2 == e_polygonShape)
	{
		return DistancePC(x2, x1, (b2PolygonShape*)shape2, xf2, (b2CircleShape*)shape1, xf1);
	}

	if (type1 == e_polygonShape && type2 == e_polygonShape)
	{
		return DistanceGeneric(x1, x2, (b2PolygonShape*)shape1, xf1, (b2PolygonShape*)shape2, xf2);
	}

	if (type1 == e_edgeShape && type2 == e_circleShape)
	{
		return DistanceEdgeCircle(x1, x2, (b2EdgeShape*)shape1, xf1, (b2CircleShape*)shape2, xf2);
	}
	
	if (type1 == e_circleShape && type2 == e_edgeShape)
	{
		return DistanceEdgeCircle(x2, x1, (b2EdgeShape*)shape2, xf2, (b2CircleShape*)shape1, xf1);
	}

	if (type1 == e_polygonShape && type2 == e_edgeShape)
	{
		return DistanceGeneric(x2, x1, (b2EdgeShape*)shape2, xf2, (b2PolygonShape*)shape1, xf1);
	}

	if (type1 == e_edgeShape && type2 == e_polygonShape)
	{
		return DistanceGeneric(x1, x2, (b2EdgeShape*)shape1, xf1, (b2PolygonShape*)shape2, xf2);
	}

	return 0.0f;
}