using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Azimuth.RootFinding;
using Annulus.SweptCollisionDetection;
using Azimuth;

namespace Annulus.UnitTests.SweptCollisionDetection
{
    public class RotatingPointvsRotatingSimplePolygonTests : UnitTestSharp.TestFixture
    {
        public void BoxLinearMotion_HitOnEdge()
        {
            var polygon = new SimplePolygon(new Vector[]
            {
                new Vector(-1, 1),
                new Vector( 1, 1),
                new Vector( 1,-1),
                new Vector(-1,-1),
            });

            var input = new RotatingPointvsRotatingSimplePolygon.Input
            {
                polygon = polygon,
                initialPointOrientation = 0,
                initialPointPosition = new Vector(7,0),
                initialPolygonOrientation = 0,
                pointAngularVelocity = 0,
                pointLeverArm = new Vector(0,0),
                pointLinearVelocity = new Vector(-1, 0),
                polygonAngularVelocity = 0,
            };

            int iterations = 0;
            SimplePolygon.Edge? impactedEdge;
            Scalar TOI;
            var success = RotatingPointvsRotatingSimplePolygon.FindNextIntersection(input, new Interval(0, 100), 1000,
                out impactedEdge, out TOI, ref iterations);

            CheckEqual(EndingState.SUCCESS, success);
            CheckEqual(6, TOI);
            CheckEqual(new Polygon.Edge { Polygon = polygon, startVertexIndex = 1, endVertexIndex = 2 }, impactedEdge);
        }

        public void BoxLinearMotion_HitOnCorner()
        {
            var polygon = new SimplePolygon(new Vector[]
            {
                new Vector(-1, 1),
                new Vector( 1, 1),
                new Vector( 1,-1),
                new Vector(-1,-1),
            });

            var input = new RotatingPointvsRotatingSimplePolygon.Input
            {
                polygon = polygon,
                initialPointOrientation = 0,
                initialPointPosition = new Vector(7, 7),
                initialPolygonOrientation = 0,
                pointAngularVelocity = 0,
                pointLeverArm = new Vector(0, 0),
                pointLinearVelocity = new Vector(-1, -1),
                polygonAngularVelocity = 0,
            };

            int iterations = 0;
            SimplePolygon.Edge? impactedEdge;
            Scalar TOI;
            var success = RotatingPointvsRotatingSimplePolygon.FindNextIntersection(input, new Interval(0, 100), 1000,
                out impactedEdge, out TOI, ref iterations);

            CheckEqual(EndingState.SUCCESS, success);
            CheckEqual(6, TOI);
            CheckEqual(new Polygon.Edge { Polygon = polygon, startVertexIndex = 0, endVertexIndex = 1 }, impactedEdge);
        }

        public void BoxLinearMotion_Scrape()
        {
            var polygon = new SimplePolygon(new Vector[]
            {
                new Vector(-1, 1),
                new Vector( 1, 1),
                new Vector( 1,-1),
                new Vector(-1,-1),
            });

            var input = new RotatingPointvsRotatingSimplePolygon.Input
            {
                polygon = polygon,
                initialPointOrientation = 0,
                initialPointPosition = new Vector(7, 1),
                initialPolygonOrientation = 0,
                pointAngularVelocity = 0,
                pointLeverArm = new Vector(0, 0),
                pointLinearVelocity = new Vector(-1, 0),
                polygonAngularVelocity = 0,
            };

            int iterations = 0;
            SimplePolygon.Edge? impactedEdge;
            Scalar TOI;
            var success = RotatingPointvsRotatingSimplePolygon.FindNextIntersection(input, new Interval(0, 100), 1000,
                out impactedEdge, out TOI, ref iterations);

            CheckEqual(EndingState.BOUNDS_EXCEEDED, success);
            Check(Scalar.IsNaN(TOI));
            CheckNull(impactedEdge);
        }

        public void BoxNoMotion_Outside()
        {
            var polygon = new SimplePolygon(new Vector[]
            {
                new Vector(-1, 1),
                new Vector( 1, 1),
                new Vector( 1,-1),
                new Vector(-1,-1),
            });

            var input = new RotatingPointvsRotatingSimplePolygon.Input
            {
                polygon = polygon,
                initialPointOrientation = 0,
                initialPointPosition = new Vector(7, 0),
                initialPolygonOrientation = 0,
                pointAngularVelocity = 0,
                pointLeverArm = new Vector(0, 0),
                pointLinearVelocity = new Vector(0, 0),
                polygonAngularVelocity = 0,
            };

            int iterations = 0;
            SimplePolygon.Edge? impactedEdge;
            Scalar TOI;
            var success = RotatingPointvsRotatingSimplePolygon.FindNextIntersection(input, new Interval(0, 100), 1000,
                out impactedEdge, out TOI, ref iterations);

            CheckEqual(EndingState.BOUNDS_EXCEEDED, success);
            Check(Scalar.IsNaN(TOI));
            CheckNull(impactedEdge);
        }

        public void BoxNoMotion_Inside()
        {
            var polygon = new SimplePolygon(new Vector[]
            {
                new Vector(-1, 1),
                new Vector( 1, 1),
                new Vector( 1,-1),
                new Vector(-1,-1),
            });

            var input = new RotatingPointvsRotatingSimplePolygon.Input
            {
                polygon = polygon,
                initialPointOrientation = 0,
                initialPointPosition = new Vector(0, 0),
                initialPolygonOrientation = 0,
                pointAngularVelocity = 0,
                pointLeverArm = new Vector(0, 0),
                pointLinearVelocity = new Vector(0, 0),
                polygonAngularVelocity = 0,
            };

            int iterations = 0;
            SimplePolygon.Edge? impactedEdge;
            Scalar TOI;
            var success = RotatingPointvsRotatingSimplePolygon.FindNextIntersection(input, new Interval(0, 100), 1000,
                out impactedEdge, out TOI, ref iterations);

            CheckEqual(EndingState.BOUNDS_EXCEEDED, success);
            Check(Scalar.IsNaN(TOI));
            CheckNull(impactedEdge);
        }

        public void BoxNoMotion_On()
        {
            var polygon = new SimplePolygon(new Vector[]
            {
                new Vector(-1, 1),
                new Vector( 1, 1),
                new Vector( 1,-1),
                new Vector(-1,-1),
            });

            var input = new RotatingPointvsRotatingSimplePolygon.Input
            {
                polygon = polygon,
                initialPointOrientation = 0,
                initialPointPosition = new Vector(1, 0),
                initialPolygonOrientation = 0,
                pointAngularVelocity = 0,
                pointLeverArm = new Vector(0, 0),
                pointLinearVelocity = new Vector(0, 0),
                polygonAngularVelocity = 0,
            };

            int iterations = 0;
            SimplePolygon.Edge? impactedEdge;
            Scalar TOI;
            var success = RotatingPointvsRotatingSimplePolygon.FindNextIntersection(input, new Interval(0, 100), 1000,
                out impactedEdge, out TOI, ref iterations);

            CheckEqual(EndingState.BOUNDS_EXCEEDED, success);
            Check(Scalar.IsNaN(TOI));
            CheckNull(impactedEdge);
        }

        public void BoxAngularMotion_NoHit()
        {
            var polygon = new SimplePolygon(new Vector[]
            {
                new Vector(-1, 1),
                new Vector( 1, 1),
                new Vector( 1,-1),
                new Vector(-1,-1),
            });

            var input = new RotatingPointvsRotatingSimplePolygon.Input
            {
                polygon = polygon,
                initialPointOrientation = 0,
                initialPointPosition = new Vector(3, 0),
                initialPolygonOrientation = 0,
                pointAngularVelocity = Math.PI / 4 * 3,
                pointLeverArm = new Vector(-1, 0),
                pointLinearVelocity = new Vector(0, 0),
                polygonAngularVelocity = Math.PI / 4,
            };

            int iterations = 0;
            SimplePolygon.Edge? impactedEdge;
            Scalar TOI;
            var success = RotatingPointvsRotatingSimplePolygon.FindNextIntersection(input, new Interval(0, 4), 1000,
                out impactedEdge, out TOI, ref iterations);

            CheckEqual(EndingState.BOUNDS_EXCEEDED, success);
            Check(Scalar.IsNaN(TOI));
            CheckNull(impactedEdge);
        }

        public void BoxAngularMotion_EdgeHit()
        {
            var polygon = new SimplePolygon(new Vector[]
            {
                new Vector(-1, 1),
                new Vector( 1, 1),
                new Vector( 1,-1),
                new Vector(-1,-1),
            });

            var input = new RotatingPointvsRotatingSimplePolygon.Input
            {
                polygon = polygon,
                initialPointOrientation = 0,
                initialPointPosition = new Vector(Math.Sqrt(2), 0),
                initialPolygonOrientation = 0,
                pointAngularVelocity = Math.PI / 4 * 3,
                pointLeverArm = new Vector(1, 0),
                pointLinearVelocity = new Vector(0, 0),
                polygonAngularVelocity = Math.PI / 4,
            };

            int iterations = 0;
            SimplePolygon.Edge? impactedEdge;
            Scalar TOI;
            var success = RotatingPointvsRotatingSimplePolygon.FindNextIntersection(input, new Interval(0, 4), 1000,
                out impactedEdge, out TOI, ref iterations);

            CheckEqual(EndingState.SUCCESS, success);
            CheckEqual(1, TOI);
            CheckEqual(new Polygon.Edge { Polygon = polygon, startVertexIndex = 1, endVertexIndex = 2 }, impactedEdge);
        }

        public void BoxAngularMotion_CornerHit()
        {
            var polygon = new SimplePolygon(new Vector[]
            {
                new Vector(-1, 1),
                new Vector( 1, 1),
                new Vector( 1,-1),
                new Vector(-1,-1),
            });

            var input = new RotatingPointvsRotatingSimplePolygon.Input
            {
                polygon = polygon,
                initialPointOrientation = 0,
                pointLeverArm = new Vector(Math.Sqrt(5), 0),
                initialPointPosition = new Vector(3, 0),
                initialPolygonOrientation = 0,
                pointAngularVelocity = Math.Acos(-2 / Math.Sqrt(5)),
                pointLinearVelocity = new Vector(0, 0),
                polygonAngularVelocity = 0,
            };

            int iterations = 0;
            SimplePolygon.Edge? impactedEdge;
            Scalar TOI;
            var success = RotatingPointvsRotatingSimplePolygon.FindNextIntersection(input, new Interval(0, 4), 1000,
                out impactedEdge, out TOI, ref iterations);

            CheckEqual(EndingState.SUCCESS, success);
            CheckEqual(1, TOI);
            CheckEqual(new Polygon.Edge { Polygon = polygon, startVertexIndex = 0, endVertexIndex = 1 }, impactedEdge);
        }

        public void BoxAngularMotion_ScrapeCorner()
        {
            var polygon = new SimplePolygon(new Vector[]
            {
                new Vector( 0, 1),
                new Vector( 1, 0),
                new Vector( 0,-1),
                new Vector(-1, 0),
            });

            var input = new RotatingPointvsRotatingSimplePolygon.Input
            {
                polygon = polygon,
                initialPointOrientation = 0,
                pointLeverArm = new Vector(1, 0),
                initialPointPosition = new Vector(2, 0),
                initialPolygonOrientation = 0,
                pointAngularVelocity = Math.PI,
                pointLinearVelocity = new Vector(0, 0),
                polygonAngularVelocity = 0,
            };

            int iterations = 0;
            SimplePolygon.Edge? impactedEdge;
            Scalar TOI;
            var success = RotatingPointvsRotatingSimplePolygon.FindNextIntersection(input, new Interval(0, 4), 1000,
                out impactedEdge, out TOI, ref iterations);

            CheckEqual(EndingState.BOUNDS_EXCEEDED, success);
            Check(Scalar.IsNaN(TOI));
            CheckNull(impactedEdge);
        }

        public void BoxAngularMotion_HitScrapeCorner()
        {
            var polygon = new SimplePolygon(new Vector[]
            {
                new Vector( 0, 1),
                new Vector( 1, 0),
                new Vector( 0,-1),
                new Vector(-1, 0),
            });

            var input = new RotatingPointvsRotatingSimplePolygon.Input
            {
                polygon = polygon,
                initialPointOrientation = 0,
                pointLeverArm = new Vector(1, 0),
                initialPointPosition = new Vector(2 - 1e-5, 0),
                initialPolygonOrientation = 0,
                pointAngularVelocity = Math.PI,
                pointLinearVelocity = new Vector(0, 0),
                polygonAngularVelocity = 0,
            };

            int iterations = 0;
            SimplePolygon.Edge? impactedEdge;
            Scalar TOI;
            var success = RotatingPointvsRotatingSimplePolygon.FindNextIntersection(input, new Interval(0, 4), 1000,
                out impactedEdge, out TOI, ref iterations);

            CheckEqual(EndingState.SUCCESS, success);
            CheckEqual(0.9999968169, TOI);
            CheckEqual(new Polygon.Edge { Polygon = polygon, startVertexIndex = 0, endVertexIndex = 1 }, impactedEdge);
        }

        public void BoxAngularMotion_MissScrapeCorner()
        {
            var polygon = new SimplePolygon(new Vector[]
            {
                new Vector( 0, 1),
                new Vector( 1, 0),
                new Vector( 0,-1),
                new Vector(-1, 0),
            });

            var input = new RotatingPointvsRotatingSimplePolygon.Input
            {
                polygon = polygon,
                initialPointOrientation = 0,
                pointLeverArm = new Vector(1, 0),
                initialPointPosition = new Vector(2 + 1e-5, 0),
                initialPolygonOrientation = 0,
                pointAngularVelocity = Math.PI,
                pointLinearVelocity = new Vector(0, 0),
                polygonAngularVelocity = 0,
            };

            int iterations = 0;
            SimplePolygon.Edge? impactedEdge;
            Scalar TOI;
            var success = RotatingPointvsRotatingSimplePolygon.FindNextIntersection(input, new Interval(0, 4), 1000,
                out impactedEdge, out TOI, ref iterations);

            CheckEqual(EndingState.BOUNDS_EXCEEDED, success);
            Check(Scalar.IsNaN(TOI));
            CheckNull(impactedEdge);
        }
    }
}