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 RotatingPointvsRotatingSegmentTests : UnitTestSharp.TestFixture
    {
        public class FindNextIntersectionTests : UnitTestSharp.TestFixture
        {
            public void GrazingHit()
            {
                var hSqrt2 = Math.Sqrt(2) / 2;
                var input = new RotatingPointvsRotatingSegment.Input
                {
                    segment = new Line(
                        new Vector(-hSqrt2, hSqrt2),
                        new Vector(hSqrt2, hSqrt2)
                    ),

                    initialLineOrientation = 0,
                    initialPointOrientation = 0,
                    initialPointPosition = new Vector(0, 2),
                    lineAngularVelocity = Math.PI / 2,
                    pointAngularVelocity = 0,
                    pointLeverArm = new Vector(0, 0),
                    pointLinearVelocity = new Vector(0, -2)
                };

                Scalar timeOfImpact;
                Scalar segmentHitFraction;
                int iterations = 0;
                var success = RotatingPointvsRotatingSegment.FindIntersection
                    (input, new Interval(0, 100), 1000, out timeOfImpact, out segmentHitFraction, ref iterations);

                CheckEqual(EndingState.SUCCESS, success);
                CheckEqual(0.5, timeOfImpact);
                CheckEqual(1, segmentHitFraction);
            }

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

                    initialLineOrientation = 0,
                    initialPointOrientation = 0,
                    initialPointPosition = new Vector(0, 2),
                    lineAngularVelocity = Math.PI / 2,
                    pointAngularVelocity = 0,
                    pointLeverArm = new Vector(0, 0),
                    pointLinearVelocity = new Vector(0, (-2 - hSqrt2) / 2)
                };

                Scalar timeOfImpact;
                Scalar segmentHitFraction;
                int iterations = 0;
                var success = RotatingPointvsRotatingSegment.FindIntersection
                    (input, new Interval(0, 100), 1000, out timeOfImpact, out segmentHitFraction, ref iterations);

                CheckEqual(EndingState.SUCCESS, success);
                CheckEqual(2, timeOfImpact);
                CheckEqual(0.5, segmentHitFraction);
            }

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

                    initialLineOrientation = 0,
                    initialPointOrientation = 0,
                    initialPointPosition = new Vector(0, 2),
                    lineAngularVelocity = Math.PI / 2,
                    pointAngularVelocity = 0,
                    pointLeverArm = new Vector(0, 0),
                    pointLinearVelocity = new Vector(0, (-2 - hSqrt2) / 3)
                };

                Scalar timeOfImpact;
                Scalar segmentHitFraction;
                int iterations = 0;
                var success = RotatingPointvsRotatingSegment.FindIntersection
                    (input, new Interval(0, 100), 1000, out timeOfImpact, out segmentHitFraction, ref iterations);

                CheckEqual(EndingState.BOUNDS_EXCEEDED, success);
                Check(Scalar.IsNaN(timeOfImpact));
                Check(Scalar.IsNaN(segmentHitFraction));
            }

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

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

                    initialLineOrientation = 0,
                    initialPointOrientation = 0,
                    initialPointPosition = new Vector(10, hSqrt2),
                    lineAngularVelocity = 0,
                    pointAngularVelocity = 0,
                    pointLeverArm = new Vector(0, 0),
                    pointLinearVelocity = new Vector(-1, 0)
                };

                Scalar timeOfImpact;
                Scalar segmentHitFraction;
                int iterations = 0;
                var success = RotatingPointvsRotatingSegment.FindIntersection
                    (input, new Interval(0, 100), 1000, out timeOfImpact, out segmentHitFraction, ref iterations);

                CheckEqual(EndingState.BOUNDS_EXCEEDED, success);
                Check(Scalar.IsNaN(timeOfImpact));
                Check(Scalar.IsNaN(segmentHitFraction));
            }

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

                    initialLineOrientation = 0,
                    initialPointOrientation = 0,
                    initialPointPosition = new Vector(7, 0),
                    lineAngularVelocity = 0,
                    pointAngularVelocity = 0,
                    pointLeverArm = new Vector(0, 0),
                    pointLinearVelocity = new Vector(-1, 0)
                };

                Scalar timeOfImpact;
                Scalar segmentHitFraction;
                int iterations = 0;
                var success = RotatingPointvsRotatingSegment.FindIntersection
                    (input, new Interval(0, 100), 1000, out timeOfImpact, out segmentHitFraction, ref iterations);

                CheckEqual(EndingState.SUCCESS, success);
                CheckEqual(6, timeOfImpact);
                CheckEqual(0.25, segmentHitFraction);
            }

            public void MaxIterationsExceededOnce()
            {
                var input = new RotatingPointvsRotatingSegment.Input
                {
                    segment = new Line(
                        new Vector(1, 1),
                        new Vector(1, -1)
                    ),

                    initialLineOrientation = 0,
                    initialPointOrientation = 0,
                    initialPointPosition = new Vector(2, 0),
                    lineAngularVelocity = Math.PI / 4,
                    pointAngularVelocity = Math.PI / 4 * 3,
                    pointLeverArm = new Vector(-1, 0),
                    pointLinearVelocity = new Vector(0, 0)
                };

                //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
                Scalar timeOfImpact;
                Scalar segmentHitFraction;
                int iterations = 0;
                var success = RotatingPointvsRotatingSegment.FindIntersection
                    (input, new Interval(0, 5.4164291317), 10000, out timeOfImpact, out segmentHitFraction, ref iterations);

                CheckEqual(EndingState.BOUNDS_EXCEEDED, success);
                Check(Scalar.IsNaN(timeOfImpact));
                Check(Scalar.IsNaN(segmentHitFraction));
            }
        }

        public class SafeStepForward : UnitTestSharp.TestFixture
        {
            public void GrazingHit()
            {
                var hSqrt2 = Math.Sqrt(2) / 2;
                var input = new RotatingPointvsRotatingSegment.Input
                {
                    segment = new Line(
                        new Vector(-hSqrt2, hSqrt2),
                        new Vector(hSqrt2, hSqrt2)
                    ),

                    initialLineOrientation = 0,
                    initialPointOrientation = 0,
                    initialPointPosition = new Vector(0, 2),
                    lineAngularVelocity = Math.PI / 2,
                    pointAngularVelocity = 0,
                    pointLeverArm = new Vector(0, 0),
                    pointLinearVelocity = new Vector(0, -2)
                };

                Scalar currRoot = 0.5;
                Scalar nextRoot = 1.5;

                Interval newWindow;
                int iterations = 0;
                var success = RotatingPointvsRotatingSegment.SafeStepForward(
                    input, new Interval(currRoot, 100), 1000, out newWindow, ref iterations);

                CheckEqual(EndingState.SUCCESS, success);
                CheckNotEqual(currRoot, newWindow.Min);
                Check(newWindow.Min > currRoot);
                Check(newWindow.Min < nextRoot);
                CheckEqual(100, newWindow.Max);
            }

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

                    initialLineOrientation = 0,
                    initialPointOrientation = 0,
                    initialPointPosition = new Vector(0, 2),
                    lineAngularVelocity = Math.PI / 2,
                    pointAngularVelocity = 0,
                    pointLeverArm = new Vector(0, 0),
                    pointLinearVelocity = new Vector(0, (-2 - hSqrt2) / 2)
                };

                Scalar currRoot = 0;
                Scalar nextRoot = 2;

                Interval newWindow;
                int iterations = 0;
                var success = RotatingPointvsRotatingSegment.SafeStepForward(
                    input, new Interval(currRoot, 100), 1000, out newWindow, ref iterations);

                CheckEqual(EndingState.SUCCESS, success);
                CheckNotEqual(currRoot, newWindow.Min);
                Check(newWindow.Min > currRoot);
                Check(newWindow.Min < nextRoot);
                CheckEqual(100, newWindow.Max);
            }

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

                    initialLineOrientation = 0,
                    initialPointOrientation = 0,
                    initialPointPosition = new Vector(0, 2),
                    lineAngularVelocity = Math.PI / 2,
                    pointAngularVelocity = 0,
                    pointLeverArm = new Vector(0, 0),
                    pointLinearVelocity = new Vector(0, (-2 - hSqrt2) / 3)
                };

                Scalar currRoot = 0;
                Scalar nextRoot = 2;

                Interval newWindow;
                int iterations = 0;
                var success = RotatingPointvsRotatingSegment.SafeStepForward(
                    input, new Interval(currRoot, 100), 1000, out newWindow, ref iterations);

                CheckEqual(EndingState.SUCCESS, success);
                CheckNotEqual(currRoot, newWindow.Min);
                Check(newWindow.Min > currRoot);
                Check(newWindow.Min < nextRoot);
                CheckEqual(100, newWindow.Max);
            }

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

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

                    initialLineOrientation = 0,
                    initialPointOrientation = 0,
                    initialPointPosition = new Vector(10, hSqrt2),
                    lineAngularVelocity = 0,
                    pointAngularVelocity = 0,
                    pointLeverArm = new Vector(0, 0),
                    pointLinearVelocity = new Vector(-1, 0)
                };

                Scalar currRoot = 10;
                Scalar nextRoot = Scalar.PositiveInfinity;

                Interval newWindow;
                int iterations = 0;
                var success = RotatingPointvsRotatingSegment.SafeStepForward(
                    input, new Interval(currRoot, 100), 1000, out newWindow, ref iterations);

                CheckEqual(EndingState.BOUNDS_EXCEEDED, success);
            }

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

                    initialLineOrientation = 0,
                    initialPointOrientation = 0,
                    initialPointPosition = new Vector(7, 0),
                    lineAngularVelocity = 0,
                    pointAngularVelocity = 0,
                    pointLeverArm = new Vector(0, 0),
                    pointLinearVelocity = new Vector(-1, 0)
                };

                Scalar currRoot = 0;
                Scalar nextRoot = 6;

                Interval newWindow;
                int iterations = 0;
                var success = RotatingPointvsRotatingSegment.SafeStepForward(
                    input, new Interval(currRoot, 100), 1000, out newWindow, ref iterations);

                CheckEqual(EndingState.SUCCESS, success);
                CheckNotEqual(currRoot, newWindow.Min);
                Check(newWindow.Min > currRoot);
                Check(newWindow.Min < nextRoot);
                CheckEqual(100, newWindow.Max);
            }
        }

        public class SafeStepBackward : UnitTestSharp.TestFixture
        {
            public void GrazingHit()
            {
                var hSqrt2 = Math.Sqrt(2) / 2;
                var input = new RotatingPointvsRotatingSegment.Input
                {
                    segment = new Line(
                        new Vector(-hSqrt2, hSqrt2),
                        new Vector(hSqrt2, hSqrt2)
                    ),

                    initialLineOrientation = 0,
                    initialPointOrientation = 0,
                    initialPointPosition = new Vector(0, 2),
                    lineAngularVelocity = Math.PI / 2,
                    pointAngularVelocity = 0,
                    pointLeverArm = new Vector(0, 0),
                    pointLinearVelocity = new Vector(0, -2)
                };

                Scalar currRoot = 0.5;
                Scalar nextRoot = 1.5;

                Interval newWindow;
                int iterations = 0;
                var success = RotatingPointvsRotatingSegment.SafeStepBackward(
                    input, new Interval(-100, nextRoot), 1000, out newWindow, ref iterations);

                CheckEqual(EndingState.SUCCESS, success);
                CheckNotEqual(nextRoot, newWindow.Max);
                Check(newWindow.Max > currRoot);
                Check(newWindow.Max < nextRoot);
                CheckEqual(-100, newWindow.Min);
            }

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

                    initialLineOrientation = 0,
                    initialPointOrientation = 0,
                    initialPointPosition = new Vector(0, 2),
                    lineAngularVelocity = Math.PI / 2,
                    pointAngularVelocity = 0,
                    pointLeverArm = new Vector(0, 0),
                    pointLinearVelocity = new Vector(0, (-2 - hSqrt2) / 2)
                };

                Scalar currRoot = 0;
                Scalar nextRoot = 2;

                Interval newWindow;
                int iterations = 0;
                var success = RotatingPointvsRotatingSegment.SafeStepBackward(
                    input, new Interval(-100, nextRoot), 1000, out newWindow, ref iterations);

                CheckEqual(EndingState.SUCCESS, success);
                CheckNotEqual(nextRoot, newWindow.Max);
                Check(newWindow.Max > currRoot);
                Check(newWindow.Max < nextRoot);
                CheckEqual(-100, newWindow.Min);
            }

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

                    initialLineOrientation = 0,
                    initialPointOrientation = 0,
                    initialPointPosition = new Vector(0, 2),
                    lineAngularVelocity = Math.PI / 2,
                    pointAngularVelocity = 0,
                    pointLeverArm = new Vector(0, 0),
                    pointLinearVelocity = new Vector(0, (-2 - hSqrt2) / 3)
                };

                Scalar currRoot = 0;
                Scalar nextRoot = 2;

                Interval newWindow;
                int iterations = 0;
                var success = RotatingPointvsRotatingSegment.SafeStepBackward(
                    input, new Interval(-100, nextRoot), 1000, out newWindow, ref iterations);

                CheckEqual(EndingState.SUCCESS, success);
                CheckNotEqual(nextRoot, newWindow.Max);
                Check(newWindow.Max > currRoot);
                Check(newWindow.Max < nextRoot);
                CheckEqual(-100, newWindow.Min);
            }

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

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

                    initialLineOrientation = 0,
                    initialPointOrientation = 0,
                    initialPointPosition = new Vector(10, hSqrt2),
                    lineAngularVelocity = 0,
                    pointAngularVelocity = 0,
                    pointLeverArm = new Vector(0, 0),
                    pointLinearVelocity = new Vector(-1, 0)
                };

                Scalar currRoot = 10;
                Scalar nextRoot = 11;

                Interval newWindow;
                int iterations = 0;
                var success = RotatingPointvsRotatingSegment.SafeStepBackward(
                    input, new Interval(-100, nextRoot), 1000, out newWindow, ref iterations);

                CheckEqual(EndingState.BOUNDS_EXCEEDED, success);
            }

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

                    initialLineOrientation = 0,
                    initialPointOrientation = 0,
                    initialPointPosition = new Vector(7, 0),
                    lineAngularVelocity = 0,
                    pointAngularVelocity = 0,
                    pointLeverArm = new Vector(0, 0),
                    pointLinearVelocity = new Vector(-1, 0)
                };
                Scalar nextRoot = 6;
                
                Interval newWindow;
                int iterations = 0;
                var success = RotatingPointvsRotatingSegment.SafeStepBackward(
                    input, new Interval(-100, nextRoot), 1000, out newWindow, ref iterations);

                CheckEqual(EndingState.BOUNDS_EXCEEDED, success);
            }
        }
    }
}