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

namespace Annulus.UnitTests.SweptCollisionDetection
{
    public class SegmentvsPointTests : UnitTestSharp.TestFixture
    {
        public class FindIntersectionsInWindowTests : UnitTestSharp.TestFixture
        {
            public void UncenteredBasicHit_X()
            {
                var segment = new Line(
                    new Vector(2, 1),
                    new Vector(3, 1)
                );

                var input = new CollidingInput
                {
                    collideeFrame = new RotatingFrame
                    {
                        
                    },

                    pointMotion = new Motion
                    {
                        InitialPosition = new Vector(2.75, 2),
                        LinearVelocity = new Vector(0, -1),
                    },

                    impactWindow = new Interval(0, 3),
                    maxIterations = 1000,
                };

                var hits = SegmentvsPoint.FindIntersectionsInWindow(input, segment);

                var expected = new SegmentvsPoint.Hit[]
                {
                    new SegmentvsPoint.Hit
                    {
                        TOI = 1,
                        HitFraction = .75,
                        Transition = SegmentvsPoint.Hit.TransitionState.CrossingFromLeftToRight,
                    },
                };

                CheckEqual(expected, hits);
            }

            public void UncenteredBasicHit_Y()
            {
                var segment = new Line(
                    new Vector(1, 2),
                    new Vector(1, 3)
                );

                var input = new CollidingInput
                {
                    collideeFrame = new RotatingFrame
                    {

                    },

                    pointMotion = new Motion
                    {
                        InitialPosition = new Vector(2, 2.75),
                        LinearVelocity = new Vector(-1, 0),
                    },

                    impactWindow = new Interval(0, 3),
                    maxIterations = 1000,
                };

                var hits = SegmentvsPoint.FindIntersectionsInWindow(input, segment);

                var expected = new SegmentvsPoint.Hit[]
                {
                    new SegmentvsPoint.Hit
                    {
                        TOI = 1,
                        HitFraction = .75,
                        Transition = SegmentvsPoint.Hit.TransitionState.CrossingFromRightToLeft,
                    },
                };

                CheckEqual(expected, hits);
            }

            public void BasicHit()
            {
                var hSqrt2 = Math.Sqrt(2) / 2;
                var segment = new Line(
                    new Vector(-hSqrt2, hSqrt2),
                    new Vector(hSqrt2, hSqrt2)
                );

                var input = new CollidingInput
                {
                    collideeFrame = new RotatingFrame
                    {
                        
                    },

                    pointMotion = new Motion
                    {
                        InitialPosition = new Vector(0, 2),
                        LinearVelocity = new Vector(0, (-2 + hSqrt2) / 2),
                    },

                    impactWindow = new Interval(0, 3),
                    maxIterations = 1000,
                };

                var hits = SegmentvsPoint.FindIntersectionsInWindow(input, segment);

                var expected = new SegmentvsPoint.Hit[]
                {
                    new SegmentvsPoint.Hit
                    {
                        TOI = 2,
                        HitFraction = .5,
                        Transition = SegmentvsPoint.Hit.TransitionState.CrossingFromLeftToRight,
                    },
                };

                CheckEqual(expected, hits);
            }

            public void BasicHit_Acceleration()
            {
                var hSqrt2 = Math.Sqrt(2) / 2;
                var segment = new Line(
                    new Vector(-hSqrt2, hSqrt2),
                    new Vector(hSqrt2, hSqrt2)
                );

                var input = new CollidingInput
                {
                    collideeFrame = new RotatingFrame
                    {

                    },

                    pointMotion = new Motion
                    {
                        InitialPosition = new Vector(0, 2 + hSqrt2 * 2),
                        LinearVelocity = new Vector(0, -hSqrt2 / 2),
                        LinearAcceleration = new Vector(0, -1),
                    },

                    impactWindow = new Interval(0, 3),
                    maxIterations = 1000,
                };

                var hits = SegmentvsPoint.FindIntersectionsInWindow(input, segment);

                var expected = new SegmentvsPoint.Hit[]
                {
                    new SegmentvsPoint.Hit
                    {
                        TOI = 2,
                        HitFraction = .5,
                        Transition = SegmentvsPoint.Hit.TransitionState.CrossingFromLeftToRight,
                    },
                };

                CheckEqual(expected, hits);
            }

            public void BasicMiss()
            {
                var hSqrt2 = Math.Sqrt(2) / 2;
                var segment = new Line(
                    new Vector(-hSqrt2, hSqrt2),
                    new Vector(hSqrt2, hSqrt2)
                );

                var input = new CollidingInput
                {
                    collideeFrame = new RotatingFrame
                    {
                        AngularVelocity = Math.PI / 2,
                    },

                    pointMotion = new Motion
                    {
                        InitialPosition = new Vector(0, 2),
                        LinearVelocity = new Vector(0, -hSqrt2 / 3),
                        LinearAcceleration = new Vector(0, -2.0 / 3),
                    },

                    impactWindow = new Interval(0, 3),
                    maxIterations = 1000,
                };

                var hits = SegmentvsPoint.FindIntersectionsInWindow(input, segment);

                var expected = new SegmentvsPoint.Hit[]
                {
                    
                };

                CheckEqual(expected, hits);
            }

            public void ParallelMiss()
            {
                // Slides along the segment, without actually passing through it.

                var hSqrt2 = Math.Sqrt(2) / 2;
                var segment = new Line(
                    new Vector(-hSqrt2, hSqrt2),
                    new Vector(hSqrt2, hSqrt2)
                );

                var input = new CollidingInput
                {
                    collideeFrame = new RotatingFrame
                    {
                        AngularVelocity = Math.PI / 2,
                    },

                    pointMotion = new Motion
                    {
                        InitialPosition = new Vector(10, hSqrt2),
                        LinearVelocity = new Vector(-1, 0),
                        LinearAcceleration = new Vector(-0.05, 0),
                    },

                    impactWindow = new Interval(0, 3),
                    maxIterations = 1000,
                };

                var hits = SegmentvsPoint.FindIntersectionsInWindow(input, segment);

                var expected = new SegmentvsPoint.Hit[]
                {
                    
                };

                CheckEqual(expected, hits);
            }

            public void Linear()
            {
                var hSqrt2 = Math.Sqrt(2) / 2;
                var segment = new Line(
                    new Vector(1, 1),
                    new Vector(1, -3)
                );

                var input = new CollidingInput
                {
                    collideeFrame = new RotatingFrame
                    {
                        
                    },

                    pointMotion = new Motion
                    {
                        InitialPosition = new Vector(7, 0),
                        LinearVelocity = new Vector(-0.5, 0),
                        LinearAcceleration = new Vector(-1.0 / 6, 0),
                    },

                    impactWindow = new Interval(0, 10),
                    maxIterations = 1000,
                };

                var hits = SegmentvsPoint.FindIntersectionsInWindow(input, segment);

                var expected = new SegmentvsPoint.Hit[]
                {
                    new SegmentvsPoint.Hit
                    {
                        TOI = 6,
                        HitFraction = .25,
                        Transition = SegmentvsPoint.Hit.TransitionState.CrossingFromLeftToRight,
                    },
                };

                CheckEqual(expected, hits);
            }

            public void SlightlyComplexHit()
            {
                var hSqrt2 = Math.Sqrt(2) / 2;
                var segment = new Line(
                    new Vector(-hSqrt2, hSqrt2),
                    new Vector(hSqrt2, hSqrt2)
                );

                var input = new CollidingInput
                {
                    collideeFrame = new RotatingFrame
                    {
                        AngularVelocity = Math.PI / 2,
                    },

                    pointMotion = new Motion
                    {
                        InitialOrientation = -Math.PI,
                        InitialPosition = new Vector(-3, 2),
                        AngularVelocity = Math.PI * 2,
                        LinearVelocity = new Vector(4, -2),
                    },

                    pointLeverArm = new Vector(1, 0),
                    impactWindow = new Interval(0, 3),
                    maxIterations = 1000,
                };

                var hits = SegmentvsPoint.FindIntersectionsInWindow(input, segment);

                var expected = new SegmentvsPoint.Hit[]
                {
                    new SegmentvsPoint.Hit
                    {
                        TOI = 0.5,
                        HitFraction = 1,
                        Transition = SegmentvsPoint.Hit.TransitionState.CrossingFromLeftToRight,
                    },
                };

                CheckEqual(expected, hits);
            }

            public void SlightlyComplexHit_Acceleration()
            {
                var hSqrt2 = Math.Sqrt(2) / 2;
                var segment = new Line(
                    new Vector(-hSqrt2, hSqrt2),
                    new Vector(hSqrt2, hSqrt2)
                );

                var input = new CollidingInput
                {
                    collideeFrame = new RotatingFrame
                    {
                        AngularVelocity = Math.PI / 2,
                    },

                    pointMotion = new Motion
                    {
                        InitialOrientation = -Math.PI,
                        InitialPosition = new Vector(-3, 2),
                        AngularVelocity = Math.PI * 2,
                        LinearVelocity = new Vector(3, -2),
                        LinearAcceleration = new Vector(4, 0),
                    },

                    pointLeverArm = new Vector(1, 0),
                    impactWindow = new Interval(0, 3),
                    maxIterations = 1000,
                };

                var hits = SegmentvsPoint.FindIntersectionsInWindow(input, segment);

                var expected = new SegmentvsPoint.Hit[]
                {
                    new SegmentvsPoint.Hit
                    {
                        TOI = 0.5,
                        HitFraction = 1,
                        Transition = SegmentvsPoint.Hit.TransitionState.CrossingFromLeftToRight,
                    },
                };

                CheckEqual(expected, hits);
            }

            public void MaxIterationsExceededOnce()
            {
                var segment = new Line(
                    new Vector(1, 1),
                    new Vector(1, -1)
                );

                var input = new CollidingInput
                {
                    collideeFrame = new RotatingFrame
                    {
                        AngularVelocity = Math.PI / 4,                        
                    },

                    pointMotion = new Motion
                    {
                        InitialPosition = new Vector(2, 0),
                        AngularVelocity = Math.PI / 4 * 3,
                    },

                    pointLeverArm = new Vector(-1, 0),
                    impactWindow = new Interval(0, 7),
                    maxIterations = 1000,
                };

                //1 - sin(-pi/2+pi/2*x)-2*sin(pi/2+pi/4*x)

                // (Basically the point sneaks up behind the segment, 
                // just barely touches it, then leaves from behind the segment
                // to get to the outside of the segment
                var hits = SegmentvsPoint.FindIntersectionsInWindow(input, segment);

                var expected = new SegmentvsPoint.Hit[]
                {
                    new SegmentvsPoint.Hit
                    {
                        TOI = 0,
                        Transition = SegmentvsPoint.Hit.TransitionState.GlancingLeft,
                        HitFraction = 0.5,
                    },
                };

                CheckEqual(expected, hits);
            }

            public void GrazingHit()
            {
                var hSqrt2 = Math.Sqrt(2) / 2;

                var segment = new Line(
                    new Vector(-hSqrt2, hSqrt2),
                    new Vector(hSqrt2, hSqrt2)
                );

                var input = new CollidingInput
                {
                    collideeFrame = new RotatingFrame
                    {
                        AngularVelocity = Math.PI / 2,
                    },

                    pointMotion = new Motion
                    {
                        InitialPosition = new Vector(0, 2),
                        LinearVelocity = new Vector(0, -2),
                    },

                    impactWindow = new Interval(0, 3),
                    maxIterations = 1000,
                };

                var hits = SegmentvsPoint.FindIntersectionsInWindow(input, segment);

                var expected = new SegmentvsPoint.Hit[]
                {
                    new SegmentvsPoint.Hit
                    {
                        TOI = 0.5,
                        HitFraction = 1,
                        Transition = SegmentvsPoint.Hit.TransitionState.CrossingFromLeftToRight,
                    },

                    new SegmentvsPoint.Hit
                    {
                        TOI = 1.5,
                        HitFraction = 0,
                        Transition = SegmentvsPoint.Hit.TransitionState.CrossingFromRightToLeft,
                    },
                };

                CheckEqual(expected, hits);
            }

            public void GrazingHit_Acceleration()
            {
                var hSqrt2 = Math.Sqrt(2) / 2;

                var segment = new Line(
                    new Vector(-hSqrt2, hSqrt2),
                    new Vector(hSqrt2, hSqrt2)
                );

                var input = new CollidingInput
                {
                    collideeFrame = new RotatingFrame
                    {
                        AngularVelocity = Math.PI / 2,
                    },

                    pointMotion = new Motion
                    {
                        InitialPosition = new Vector(0, 2),
                        LinearVelocity = new Vector(0, -1),
                        LinearAcceleration = new Vector(0, -4),
                    },

                    impactWindow = new Interval(0, 3),
                    maxIterations = 1000,
                };

                var hits = SegmentvsPoint.FindIntersectionsInWindow(input, segment);

                var expected = new SegmentvsPoint.Hit[]
                {
                    new SegmentvsPoint.Hit
                    {
                        TOI = 0.5,
                        HitFraction = 1,
                        Transition = SegmentvsPoint.Hit.TransitionState.CrossingFromLeftToRight,
                    },
                };

                CheckEqual(expected, hits);
            }

            public void DiagonalEdgeHit()
            {
                var segment = new Line(
                    new Vector( 1, -1),
                    new Vector(-1, -1)
                );

                var input = new CollidingInput
                {
                    collideeFrame = new RotatingFrame
                    {

                    },

                    pointMotion = new Motion
                    {
                        InitialPosition = new Vector(7, 7),
                        LinearVelocity = new Vector(-1, -1),
                    },

                    impactWindow = new Interval(0, 10),
                    maxIterations = 1000,
                };

                var hits = SegmentvsPoint.FindIntersectionsInWindow(input, segment);

                var expected = new SegmentvsPoint.Hit[]
                {
                    new SegmentvsPoint.Hit
                    {
                        TOI = 8,
                        HitFraction = 1,
                        Transition = SegmentvsPoint.Hit.TransitionState.CrossingFromRightToLeft,
                    },
                };

                CheckEqual(expected, hits);
            }

            public void RotatingSegment()
            {
                var segment = new Line(
                    new Vector( 3, 3),
                    new Vector( 3,-3)
                );

                var boxAngularVelocity = Math.PI / (Math.PI - 2 * Math.Acos(3.0 / 5.0));
                var firstImpactTime = Math.Acos(3.0 / 5.0) + Math.PI / 2;
                var input = new CollidingInput
                {
                    collideeFrame = new RotatingFrame
                    {
                        AngularVelocity = boxAngularVelocity,
                        InitialOrientation = -boxAngularVelocity * firstImpactTime,
                    },

                    pointMotion = new Motion
                    {
                        InitialPosition = new Vector(7, 0),
                        AngularVelocity = 1,
                    },

                    pointLeverArm = new Vector(5, 0),
                    impactWindow = new Interval(0, 2.5),
                    maxIterations = 1000,
                };

                var hits = SegmentvsPoint.FindIntersectionsInWindow(input, segment);

                var expected = new SegmentvsPoint.Hit[]
                {
                    new SegmentvsPoint.Hit
                    {
                        HitFraction = 0,
                        TOI = Math.Acos(3.0/5.0) + Math.PI / 2,
                        Transition = SegmentvsPoint.Hit.TransitionState.CrossingFromRightToLeft,
                    },
                };

                CheckEqual(expected, hits);
            }

            public void Parallel_Y_Hit()
            {
                var segment = new Line(
                    new Vector(0, 1),
                    new Vector(0, -1)
                );

                var input = new CollidingInput
                {
                    collideeFrame = new RotatingFrame
                    {

                    },

                    pointMotion = new Motion
                    {
                        InitialPosition = new Vector(0, 2),
                        LinearVelocity = new Vector(0, -1),
                    },

                    impactWindow = new Interval(0, 4),
                    maxIterations = 1000,
                };

                var hits = SegmentvsPoint.FindIntersectionsInWindow(input, segment);

                var expected = new SegmentvsPoint.Hit[]
                {
                    new SegmentvsPoint.Hit
                    {
                        TOI = 1,
                        Transition = SegmentvsPoint.Hit.TransitionState.GlancingParallel,
                        HitFraction = 0,
                    },

                    new SegmentvsPoint.Hit
                    {
                        TOI = 3,
                        Transition = SegmentvsPoint.Hit.TransitionState.GlancingParallel,
                        HitFraction = 1,
                    },
                };

                CheckEqual(expected, hits);
            }

            public void SitOnEndPoint_0()
            {
                var segment = new Line(
                    new Vector(0, 1),
                    new Vector(0, -1)
                );

                var input = new CollidingInput
                {
                    collideeFrame = new RotatingFrame
                    {

                    },

                    pointMotion = new Motion
                    {
                        InitialPosition = new Vector(0, 1),
                    },

                    impactWindow = new Interval(3, 4),
                    maxIterations = 1000,
                };

                var hits = SegmentvsPoint.FindIntersectionsInWindow(input, segment);

                var expected = new SegmentvsPoint.Hit[]
                {
                    new SegmentvsPoint.Hit
                    {
                        TOI = Scalar.NaN,
                        Transition = SegmentvsPoint.Hit.TransitionState.StationaryOnSegment,
                        HitFraction = 0,
                    },
                };

                CheckEqual(expected, hits);
            }

            public void SitOnEndPoint_1()
            {
                var segment = new Line(
                    new Vector(0, 1),
                    new Vector(0, -1)
                );

                var input = new CollidingInput
                {
                    collideeFrame = new RotatingFrame
                    {

                    },

                    pointMotion = new Motion
                    {
                        InitialPosition = new Vector(0, -1),
                    },

                    impactWindow = new Interval(3, 4),
                    maxIterations = 1000,
                };

                var hits = SegmentvsPoint.FindIntersectionsInWindow(input, segment);

                var expected = new SegmentvsPoint.Hit[]
                {
                    new SegmentvsPoint.Hit
                    {
                        TOI = Scalar.NaN,
                        Transition = SegmentvsPoint.Hit.TransitionState.StationaryOnSegment,
                        HitFraction = 1,
                    },
                };

                CheckEqual(expected, hits);
            }

            public void SitOnEdge_Quarter()
            {
                var segment = new Line(
                    new Vector(0, 1),
                    new Vector(0, -1)
                );

                var input = new CollidingInput
                {
                    collideeFrame = new RotatingFrame
                    {

                    },

                    pointMotion = new Motion
                    {
                        InitialPosition = new Vector(0, 0.5),
                    },

                    impactWindow = new Interval(10, 20),
                    maxIterations = 1000,
                };

                var hits = SegmentvsPoint.FindIntersectionsInWindow(input, segment);

                var expected = new SegmentvsPoint.Hit[]
                {
                    new SegmentvsPoint.Hit
                    {
                        TOI = Scalar.NaN,
                        Transition = SegmentvsPoint.Hit.TransitionState.StationaryOnSegment,
                        HitFraction = 0.25,
                    },
                };

                CheckEqual(expected, hits);
            }

            public void WrongWinding()
            {
                var segment = new Line(
                    new Vector(1000, 0),
                    new Vector(-1000, 0)
                );

                var input = new CollidingInput
                {
                    collideeFrame = new RotatingFrame
                    {
                        
                    },

                    pointMotion = new Motion
                    {
                        InitialPosition = new Vector(0, 0),
                        LinearVelocity = new Vector(0, 100),
                    },

                    impactWindow = new Interval(0, 3),
                    maxIterations = 1000,
                };

                var hits = SegmentvsPoint.FindIntersectionsInWindow(input, segment);

                var expected = new SegmentvsPoint.Hit[]
                {
                    new SegmentvsPoint.Hit
                    {
                        TOI = 0,
                        HitFraction = 0.5,
                        Transition = SegmentvsPoint.Hit.TransitionState.CrossingFromLeftToRight,
                    },
                };

                CheckEqual(expected, hits);
            }
        }
    }
}