using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Azimuth;
using UnitTestSharp;
using Lodestone.Shapes;
using Lodestone.DiscreteCollisionDetection;

namespace Lodestone.UnitTests.DiscreteCollisionDetection
{
	public class ShapeToShape : TestFixture
	{
		ISimpleConvexShape quad = new Polygon(new Vector[] {
            new Vector(-1,  1),
            new Vector( 1,  1),
            new Vector( 1, -1),
            new Vector(-1, -1),
        } );

        ISimpleConvexShape diamond = new Polygon(new Vector[] {
            new Vector( 1,  0),
            new Vector( 0,  1),
            new Vector(-1,  0),
            new Vector( 0, -1),
        });

		DualVector ptA;
		DualVector ptB;

		public void ClosestPoints_VertexEdge()
		{
			Matrix transformA = new Matrix(0, new Vector(0, 0));
			Matrix transformB = new Matrix(-Math.PI / 4, new Vector(-20, 0));

			Lodestone.DiscreteCollisionDetection.ShapeToShape.ClosestPointsOnShapesToEachOther(
				quad, transformA, transformA.Inverse(), out ptA,
				quad, transformB, transformB.Inverse(), out ptB
				);

			CheckEqual(new Vector(-1, 0), ptA.LocalSpace);
			CheckEqual(new Vector(1, 1), ptB.LocalSpace);
		}
		
		public void ClosestPoints_VertexVertex()
		{
			Matrix transformA = new Matrix(-Math.PI / 4, new Vector(-20, 0));
			Matrix transformB = new Matrix(Math.PI / 4, new Vector(20, 0));

			Lodestone.DiscreteCollisionDetection.ShapeToShape.ClosestPointsOnShapesToEachOther(
				quad, transformA, transformA.Inverse(), out ptA,
				quad, transformB, transformB.Inverse(), out ptB
				);

			CheckEqual(new Vector(1, 1), ptA.LocalSpace);
			CheckEqual(new Vector(-1, 1), ptB.LocalSpace);
		}

		public void ClosestPoints_EdgeEdge()
		{
			Matrix transformA = new Matrix(0, new Vector(0, 0));
			Matrix transformB = new Matrix(0, new Vector(2.5, 1));
			Matrix inverseA = transformA.Inverse();
			Matrix inverseB = transformB.Inverse();

			Lodestone.DiscreteCollisionDetection.ShapeToShape.ClosestPointsOnShapesToEachOther(
				diamond, transformA, inverseA, out ptA,
				diamond, transformB, inverseB, out ptB
				);

			CheckEqual(1.125, (ptB.WorldSpace - ptA.WorldSpace).LengthSquared());
		}

		public void ClosestPoints_VertexEdge_Touching()
		{
			Matrix transformA = new Matrix(0, new Vector(0, 0));
			Matrix transformB = new Matrix(-Math.PI / 4, new Vector(-1 - Math.Sqrt(2), 0));

			Lodestone.DiscreteCollisionDetection.ShapeToShape.ClosestPointsOnShapesToEachOther(
				quad, transformA, transformA.Inverse(), out ptA,
				quad, transformB, transformB.Inverse(), out ptB
				);

			CheckEqual(new Vector(-1, 0), ptA.LocalSpace);
			CheckEqual(new Vector(1, 1), ptB.LocalSpace);
		}

		public void ClosestPoints_VertexVertex_Touching()
		{
			Matrix transformA = new Matrix(-Math.PI / 4, new Vector(0, 0));
			Matrix transformB = new Matrix(Math.PI / 4, new Vector(2 * Math.Sqrt(2), 0));

			Lodestone.DiscreteCollisionDetection.ShapeToShape.ClosestPointsOnShapesToEachOther(
				quad, transformA, transformA.Inverse(), out ptA,
				quad, transformB, transformB.Inverse(), out ptB
				);

			CheckEqual(new Vector(1, 1), ptA.LocalSpace);
			CheckEqual(new Vector(-1, 1), ptB.LocalSpace);
		}

		public void ClosestPoints_EdgeEdge_Touching()
		{
			Matrix transformA = new Matrix(0, new Vector(0, 0));
			Matrix transformB = new Matrix(0, new Vector(1, 1));
			Matrix inverseA = transformA.Inverse();
			Matrix inverseB = transformB.Inverse();

			Lodestone.DiscreteCollisionDetection.ShapeToShape.ClosestPointsOnShapesToEachOther(
				diamond, transformA, inverseA, out ptA,
				diamond, transformB, inverseB, out ptB
				);

			CheckEqual(0, (ptB.WorldSpace - ptA.WorldSpace).LengthSquared());
		}
	}
}