// MIT License

// Copyright (c) 2019 Erin Catto

// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:

// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.

// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.

#include "box2d/b2_collision.h"
#include "box2d/b2_circle_shape.h"
#include "box2d/b2_edge_shape.h"
#include "box2d/b2_polygon_shape.h"


// Compute contact points for edge versus circle.
// This accounts for edge connectivity.
void b2CollideEdgeAndCircle(b2Manifold* manifold,
							const b2EdgeShape* edgeA, const b2Transform& xfA,
							const b2CircleShape* circleB, const b2Transform& xfB)
{
	manifold->pointCount = 0;
	
	// Compute circle in frame of edge
	b2Vec2 Q = b2MulT(xfA, b2Mul(xfB, circleB->m_p));
	
	b2Vec2 A = edgeA->m_vertex1, B = edgeA->m_vertex2;
	b2Vec2 e = B - A;
	
	// Normal points to the right for a CCW winding
	b2Vec2 n(e.y, -e.x);
	float offset = b2Dot(n, Q - A);

	bool oneSided = edgeA->m_oneSided;
	if (oneSided && offset < 0.0f)
	{
		return;
	}

	// Barycentric coordinates
	float u = b2Dot(e, B - Q);
	float v = b2Dot(e, Q - A);
	
	float radius = edgeA->m_radius + circleB->m_radius;
	
	b2ContactFeature cf;
	cf.indexB = 0;
	cf.typeB = b2ContactFeature::e_vertex;
	
	// Region A
	if (v <= 0.0f)
	{
		b2Vec2 P = A;
		b2Vec2 d = Q - P;
		float dd = b2Dot(d, d);
		if (dd > radius * radius)
		{
			return;
		}
		
		// Is there an edge connected to A?
		if (edgeA->m_oneSided)
		{
			b2Vec2 A1 = edgeA->m_vertex0;
			b2Vec2 B1 = A;
			b2Vec2 e1 = B1 - A1;
			float u1 = b2Dot(e1, B1 - Q);
			
			// Is the circle in Region AB of the previous edge?
			if (u1 > 0.0f)
			{
				return;
			}
		}
		
		cf.indexA = 0;
		cf.typeA = b2ContactFeature::e_vertex;
		manifold->pointCount = 1;
		manifold->type = b2Manifold::e_circles;
		manifold->localNormal.SetZero();
		manifold->localPoint = P;
		manifold->points[0].id.key = 0;
		manifold->points[0].id.cf = cf;
		manifold->points[0].localPoint = circleB->m_p;
		return;
	}
	
	// Region B
	if (u <= 0.0f)
	{
		b2Vec2 P = B;
		b2Vec2 d = Q - P;
		float dd = b2Dot(d, d);
		if (dd > radius * radius)
		{
			return;
		}
		
		// Is there an edge connected to B?
		if (edgeA->m_oneSided)
		{
			b2Vec2 B2 = edgeA->m_vertex3;
			b2Vec2 A2 = B;
			b2Vec2 e2 = B2 - A2;
			float v2 = b2Dot(e2, Q - A2);
			
			// Is the circle in Region AB of the next edge?
			if (v2 > 0.0f)
			{
				return;
			}
		}
		
		cf.indexA = 1;
		cf.typeA = b2ContactFeature::e_vertex;
		manifold->pointCount = 1;
		manifold->type = b2Manifold::e_circles;
		manifold->localNormal.SetZero();
		manifold->localPoint = P;
		manifold->points[0].id.key = 0;
		manifold->points[0].id.cf = cf;
		manifold->points[0].localPoint = circleB->m_p;
		return;
	}
	
	// Region AB
	float den = b2Dot(e, e);
	b2Assert(den > 0.0f);
	b2Vec2 P = (1.0f / den) * (u * A + v * B);
	b2Vec2 d = Q - P;
	float dd = b2Dot(d, d);
	if (dd > radius * radius)
	{
		return;
	}
	
	if (offset < 0.0f)
	{
		n.Set(-n.x, -n.y);
	}
	n.Normalize();
	
	cf.indexA = 0;
	cf.typeA = b2ContactFeature::e_face;
	manifold->pointCount = 1;
	manifold->type = b2Manifold::e_faceA;
	manifold->localNormal = n;
	manifold->localPoint = A;
	manifold->points[0].id.key = 0;
	manifold->points[0].id.cf = cf;
	manifold->points[0].localPoint = circleB->m_p;
}

// This structure is used to keep track of the best separating axis.
struct b2EPAxis
{
	enum Type
	{
		e_unknown,
		e_edgeA,
		e_edgeB
	};
	
	b2Vec2 normal;
	Type type;
	int32 index;
	float separation;
};

// This holds polygon B expressed in frame A.
struct b2TempPolygon
{
	b2Vec2 vertices[b2_maxPolygonVertices];
	b2Vec2 normals[b2_maxPolygonVertices];
	int32 count;
};

// Reference face used for clipping
struct b2ReferenceFace
{
	int32 i1, i2;
	b2Vec2 v1, v2;
	b2Vec2 normal;
	
	b2Vec2 sideNormal1;
	float sideOffset1;
	
	b2Vec2 sideNormal2;
	float sideOffset2;
};

static b2EPAxis b2ComputeEdgeSeparation(const b2TempPolygon& polygonB, const b2Vec2& v1, const b2Vec2& normal1)
{
	b2EPAxis axis;
	axis.type = b2EPAxis::e_edgeA;
	axis.index = -1;
	axis.separation = -FLT_MAX;
	axis.normal.SetZero();

	b2Vec2 axes[2] = { normal1, -normal1 };

	// Find axis with least overlap (min-max problem)
	for (int32 j = 0; j < 2; ++j)
	{
		float sj = FLT_MAX;

		// Find deepest polygon vertex along axis j
		for (int32 i = 0; i < polygonB.count; ++i)
		{
			float si = b2Dot(axes[j], polygonB.vertices[i] - v1);
			if (si < sj)
			{
				sj = si;
			}
		}

		if (sj > axis.separation)
		{
			axis.index = j;
			axis.separation = sj;
			axis.normal = axes[j];
		}
	}

	return axis;
}

static b2EPAxis b2ComputePolygonSeparation(const b2TempPolygon& polygonB, const b2Vec2& v1, const b2Vec2& v2)
{
	b2EPAxis axis;
	axis.type = b2EPAxis::e_unknown;
	axis.index = -1;
	axis.separation = -FLT_MAX;
	axis.normal.SetZero();

	for (int32 i = 0; i < polygonB.count; ++i)
	{
		b2Vec2 n = -polygonB.normals[i];

		float s1 = b2Dot(n, polygonB.vertices[i] - v1);
		float s2 = b2Dot(n, polygonB.vertices[i] - v2);
		float s = b2Min(s1, s2);

		if (s > axis.separation)
		{
			axis.type = b2EPAxis::e_edgeB;
			axis.index = i;
			axis.separation = s;
			axis.normal = n;
		}
	}

	return axis;
}

void b2CollideEdgeAndPolygon(b2Manifold* manifold,
							const b2EdgeShape* edgeA, const b2Transform& xfA,
							const b2PolygonShape* polygonB, const b2Transform& xfB)
{
	manifold->pointCount = 0;

	b2Transform xf = b2MulT(xfA, xfB);

	b2Vec2 centroidB = b2Mul(xf, polygonB->m_centroid);

	b2Vec2 v1 = edgeA->m_vertex1;
	b2Vec2 v2 = edgeA->m_vertex2;

	b2Vec2 edge1 = v2 - v1;
	edge1.Normalize();

	// Normal points to the right for a CCW winding
	b2Vec2 normal1(edge1.y, -edge1.x);
	float offset1 = b2Dot(normal1, centroidB - v1);

	bool oneSided = edgeA->m_oneSided;
	if (oneSided && offset1 < 0.0f)
	{
		return;
	}

	// Get polygonB in frameA
	b2TempPolygon tempPolygonB;
	tempPolygonB.count = polygonB->m_count;
	for (int32 i = 0; i < polygonB->m_count; ++i)
	{
		tempPolygonB.vertices[i] = b2Mul(xf, polygonB->m_vertices[i]);
		tempPolygonB.normals[i] = b2Mul(xf.q, polygonB->m_normals[i]);
	}

	float radius = polygonB->m_radius + edgeA->m_radius;

	b2EPAxis edgeAxis = b2ComputeEdgeSeparation(tempPolygonB, v1, normal1);
	if (edgeAxis.separation > radius)
	{
		return;
	}

	b2EPAxis polygonAxis = b2ComputePolygonSeparation(tempPolygonB, v1, v2);
	if (polygonAxis.separation > radius)
	{
		return;
	}

	// Use hysteresis for jitter reduction.
	const float k_relativeTol = 0.98f;
	const float k_absoluteTol = 0.001f;

	b2EPAxis primaryAxis;
	if (polygonAxis.separation - radius > k_relativeTol * (edgeAxis.separation - radius) + k_absoluteTol)
	{
		primaryAxis = polygonAxis;
	}
	else
	{
		primaryAxis = edgeAxis;
	}

	if (oneSided)
	{
		// Smooth collision
		// See https://box2d.org/posts/2020/06/ghost-collisions/

		b2Vec2 edge0 = v1 - edgeA->m_vertex0;
		edge0.Normalize();
		b2Vec2 normal0(edge0.y, -edge0.x);
		bool convex1 = b2Cross(edge0, edge1) >= 0.0f;

		b2Vec2 edge2 = edgeA->m_vertex3 - v2;
		edge2.Normalize();
		b2Vec2 normal2(edge2.y, -edge2.x);
		bool convex2 = b2Cross(edge1, edge2) >= 0.0f;

		const float sinTol = 0.1f;
		bool side1 = b2Dot(primaryAxis.normal, edge1) <= 0.0f;

		// Check Gauss Map
		if (side1)
		{
			if (convex1)
			{
				if (b2Cross(primaryAxis.normal, normal0) > sinTol)
				{
					// Skip region
					return;
				}

				// Admit region
			}
			else
			{
				// Snap region
				primaryAxis = edgeAxis;
			}
		}
		else
		{
			if (convex2)
			{
				if (b2Cross(normal2, primaryAxis.normal) > sinTol)
				{
					// Skip region
					return;
				}

				// Admit region
			}
			else
			{
				// Snap region
				primaryAxis = edgeAxis;
			}
		}
	}

	b2ClipVertex clipPoints[2];
	b2ReferenceFace ref;
	if (primaryAxis.type == b2EPAxis::e_edgeA)
	{
		manifold->type = b2Manifold::e_faceA;

		// Search for the polygon normal that is most anti-parallel to the edge normal.
		int32 bestIndex = 0;
		float bestValue = b2Dot(primaryAxis.normal, tempPolygonB.normals[0]);
		for (int32 i = 1; i < tempPolygonB.count; ++i)
		{
			float value = b2Dot(primaryAxis.normal, tempPolygonB.normals[i]);
			if (value < bestValue)
			{
				bestValue = value;
				bestIndex = i;
			}
		}

		int32 i1 = bestIndex;
		int32 i2 = i1 + 1 < tempPolygonB.count ? i1 + 1 : 0;

		clipPoints[0].v = tempPolygonB.vertices[i1];
		clipPoints[0].id.cf.indexA = 0;
		clipPoints[0].id.cf.indexB = static_cast<uint8>(i1);
		clipPoints[0].id.cf.typeA = b2ContactFeature::e_face;
		clipPoints[0].id.cf.typeB = b2ContactFeature::e_vertex;

		clipPoints[1].v = tempPolygonB.vertices[i2];
		clipPoints[1].id.cf.indexA = 0;
		clipPoints[1].id.cf.indexB = static_cast<uint8>(i2);
		clipPoints[1].id.cf.typeA = b2ContactFeature::e_face;
		clipPoints[1].id.cf.typeB = b2ContactFeature::e_vertex;

		ref.i1 = 0;
		ref.i2 = 1;
		ref.v1 = v1;
		ref.v2 = v2;
		ref.normal = primaryAxis.normal;
		ref.sideNormal1 = -edge1;
		ref.sideNormal2 = edge1;
	}
	else
	{
		manifold->type = b2Manifold::e_faceB;

		clipPoints[0].v = v2;
		clipPoints[0].id.cf.indexA = 1;
		clipPoints[0].id.cf.indexB = static_cast<uint8>(primaryAxis.index);
		clipPoints[0].id.cf.typeA = b2ContactFeature::e_vertex;
		clipPoints[0].id.cf.typeB = b2ContactFeature::e_face;

		clipPoints[1].v = v1;
		clipPoints[1].id.cf.indexA = 0;
		clipPoints[1].id.cf.indexB = static_cast<uint8>(primaryAxis.index);		
		clipPoints[1].id.cf.typeA = b2ContactFeature::e_vertex;
		clipPoints[1].id.cf.typeB = b2ContactFeature::e_face;

		ref.i1 = primaryAxis.index;
		ref.i2 = ref.i1 + 1 < tempPolygonB.count ? ref.i1 + 1 : 0;
		ref.v1 = tempPolygonB.vertices[ref.i1];
		ref.v2 = tempPolygonB.vertices[ref.i2];
		ref.normal = tempPolygonB.normals[ref.i1];

		// CCW winding
		ref.sideNormal1.Set(ref.normal.y, -ref.normal.x);
		ref.sideNormal2 = -ref.sideNormal1;
	}

	ref.sideOffset1 = b2Dot(ref.sideNormal1, ref.v1);
	ref.sideOffset2 = b2Dot(ref.sideNormal2, ref.v2);

	// Clip incident edge against reference face side planes
	b2ClipVertex clipPoints1[2];
	b2ClipVertex clipPoints2[2];
	int32 np;

	// Clip to side 1
	np = b2ClipSegmentToLine(clipPoints1, clipPoints, ref.sideNormal1, ref.sideOffset1, ref.i1);

	if (np < b2_maxManifoldPoints)
	{
		return;
	}

	// Clip to side 2
	np = b2ClipSegmentToLine(clipPoints2, clipPoints1, ref.sideNormal2, ref.sideOffset2, ref.i2);

	if (np < b2_maxManifoldPoints)
	{
		return;
	}

	// Now clipPoints2 contains the clipped points.
	if (primaryAxis.type == b2EPAxis::e_edgeA)
	{
		manifold->localNormal = ref.normal;
		manifold->localPoint = ref.v1;
	}
	else
	{
		manifold->localNormal = polygonB->m_normals[ref.i1];
		manifold->localPoint = polygonB->m_vertices[ref.i1];
	}

	int32 pointCount = 0;
	for (int32 i = 0; i < b2_maxManifoldPoints; ++i)
	{
		float separation;

		separation = b2Dot(ref.normal, clipPoints2[i].v - ref.v1);

		if (separation <= radius)
		{
			b2ManifoldPoint* cp = manifold->points + pointCount;

			if (primaryAxis.type == b2EPAxis::e_edgeA)
			{
				cp->localPoint = b2MulT(xf, clipPoints2[i].v);
				cp->id = clipPoints2[i].id;
			}
			else
			{
				cp->localPoint = clipPoints2[i].v;
				cp->id.cf.typeA = clipPoints2[i].id.cf.typeB;
				cp->id.cf.typeB = clipPoints2[i].id.cf.typeA;
				cp->id.cf.indexA = clipPoints2[i].id.cf.indexB;
				cp->id.cf.indexB = clipPoints2[i].id.cf.indexA;
			}

			++pointCount;
		}
	}

	manifold->pointCount = pointCount;
}