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

namespace Annulus.UnitTests.SweptCollisionDetection
{
    public class RotatingPointvsRotatingLineTests : UnitTestSharp.TestFixture
    {
        public class FindNextIntersection : UnitTestSharp.TestFixture
        {
            public void NoMotion_NotIntersecting()
            {
                var input = new Annulus.SweptCollisionDetection.RotatingPointvsRotatingLine.Input
                {
                    pointLeverArm = new Vector(0, 0),
                    initialPointOrientation = 0,
                    initialLineOrientation = 0,
                    initialLineElevation = 0,
                    initialPointPosition = new Vector(0, 1),
                    lineAngularVelocity = 0,
                    pointAngularVelocity = 0,
                    pointLinearVelocity = new Vector(0, 0),
                    impactWindow = new Interval(0, 100),
                    maxIterations = 1000,
                };

                int iterations = 0;
                Scalar timeOfImpact;
                var success = RotatingPointvsRotatingLine.FindNextIntersection(input, out timeOfImpact, ref iterations);

                CheckEqual(EndingState.BOUNDS_EXCEEDED, success);
            }

            public void NoMotion_Intersecting()
            {
                var input = new Annulus.SweptCollisionDetection.RotatingPointvsRotatingLine.Input
                {
                    pointLeverArm = new Vector(0, 0),
                    initialPointOrientation = 0,
                    initialLineOrientation = 0,
                    initialLineElevation = 0,
                    initialPointPosition = new Vector(0, 0),
                    lineAngularVelocity = 0,
                    pointAngularVelocity = 0,
                    pointLinearVelocity = new Vector(0, 0),
                    impactWindow = new Interval(0, 100),
                    maxIterations = 1000,
                };

                int iterations = 0;
                Scalar timeOfImpact;
                var success = RotatingPointvsRotatingLine.FindNextIntersection(input, out timeOfImpact, ref iterations);

                CheckEqual(EndingState.SUCCESS, success);
                CheckEqual(0, timeOfImpact);
            }

            public void SimpleLinearMotion_Velocity()
            {
                var input = new Annulus.SweptCollisionDetection.RotatingPointvsRotatingLine.Input
                {
                    pointLeverArm = new Vector(0, 1),
                    initialPointOrientation = 0,
                    initialLineOrientation = 0,
                    initialLineElevation = 0,
                    initialPointPosition = new Vector(0, 0),
                    lineAngularVelocity = 0,
                    pointAngularVelocity = 0,
                    pointLinearVelocity = new Vector(0, -1),
                    impactWindow = new Interval(0, 100),
                    maxIterations = 1000,
                };

                int iterations = 0;
                Scalar timeOfImpact;
                var success = RotatingPointvsRotatingLine.FindNextIntersection(input, out timeOfImpact, ref iterations);

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

            public void SimpleLinearMotion_Acceleration()
            {
                var input = new Annulus.SweptCollisionDetection.RotatingPointvsRotatingLine.Input
                {
                    pointLeverArm = new Vector(0, 1),
                    initialPointOrientation = 0,
                    initialLineOrientation = 0,
                    initialLineElevation = 0,
                    initialPointPosition = new Vector(0, 0),
                    lineAngularVelocity = 0,
                    pointAngularVelocity = 0,
                    pointLinearVelocity = new Vector(0, 0),
                    pointLinearAcceleration = new Vector(0, -1),
                    impactWindow = new Interval(0, 100),
                    maxIterations = 1000,
                };

                int iterations = 0;
                Scalar timeOfImpact;
                var success = RotatingPointvsRotatingLine.FindNextIntersection(input, out timeOfImpact, ref iterations);

                CheckEqual(EndingState.SUCCESS, success);
                CheckEqual(Math.Sqrt(2), timeOfImpact);
            }

            public void SimpleAngularMotion_Point_XAxis()
            {
                var input = new Annulus.SweptCollisionDetection.RotatingPointvsRotatingLine.Input
                {
                    pointLeverArm = new Vector(1, 0),
                    initialPointOrientation = 0,
                    initialLineOrientation = 0,
                    initialLineElevation = 0,
                    initialPointPosition = new Vector(0, 1),
                    lineAngularVelocity = 0,
                    pointAngularVelocity = -Math.PI / 2,
                    pointLinearVelocity = new Vector(0, 0),
                    impactWindow = new Interval(0, 100),
                    maxIterations = 1000,
                };

                int iterations = 0;
                Scalar timeOfImpact;
                var success = RotatingPointvsRotatingLine.FindNextIntersection(input, out timeOfImpact, ref iterations);

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

            public void SimpleAngularMotion_Point_Backwards_XAxis()
            {
                var input = new Annulus.SweptCollisionDetection.RotatingPointvsRotatingLine.Input
                {
                    pointLeverArm = new Vector(1, 0),
                    initialPointOrientation = 0,
                    initialLineOrientation = 0,
                    initialLineElevation = 0,
                    initialPointPosition = new Vector(0, 1),
                    lineAngularVelocity = 0,
                    pointAngularVelocity = Math.PI / 2,
                    pointLinearVelocity = new Vector(0, 0),
                    impactWindow = new Interval(0, 100),
                    maxIterations = 1000,
                };

                int iterations = 0;
                Scalar timeOfImpact;
                var success = RotatingPointvsRotatingLine.FindNextIntersection(input, out timeOfImpact, ref iterations);

                CheckEqual(EndingState.SUCCESS, success);
                CheckEqual(3, timeOfImpact);
            }

            public void SimpleAngularMotion_Point_YAxis()
            {
                var input = new Annulus.SweptCollisionDetection.RotatingPointvsRotatingLine.Input
                {
                    pointLeverArm = new Vector(0, 1),
                    initialPointOrientation = -Math.PI / 2,
                    initialLineOrientation = 0,
                    initialLineElevation = 0,
                    initialPointPosition = new Vector(0, 1),
                    lineAngularVelocity = 0,
                    pointAngularVelocity = -Math.PI / 2,
                    pointLinearVelocity = new Vector(0, 0),
                    impactWindow = new Interval(0, 100),
                    maxIterations = 1000,
                };

                int iterations = 0;
                Scalar timeOfImpact;
                var success = RotatingPointvsRotatingLine.FindNextIntersection(input, out timeOfImpact, ref iterations);

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

            public void SimpleAngularMotion_Point_Backwards_YAxis()
            {
                var input = new Annulus.SweptCollisionDetection.RotatingPointvsRotatingLine.Input
                {
                    pointLeverArm = new Vector(0, 1),
                    initialPointOrientation = -Math.PI / 2,
                    initialLineOrientation = 0,
                    initialLineElevation = 0,
                    initialPointPosition = new Vector(0, 1),
                    lineAngularVelocity = 0,
                    pointAngularVelocity = Math.PI / 2,
                    pointLinearVelocity = new Vector(0, 0),
                    impactWindow = new Interval(0, 100),
                    maxIterations = 1000,
                };

                int iterations = 0;
                Scalar timeOfImpact;
                var success = RotatingPointvsRotatingLine.FindNextIntersection(input, out timeOfImpact, ref iterations);

                CheckEqual(EndingState.SUCCESS, success);
                CheckEqual(3, timeOfImpact);
            }

            public void SimpleAngularMotion_Line_XAxis()
            {
                var input = new Annulus.SweptCollisionDetection.RotatingPointvsRotatingLine.Input
                {
                    pointLeverArm = new Vector(1, 0),
                    initialPointOrientation = 0,
                    initialLineOrientation = 0,
                    initialLineElevation = 0,
                    initialPointPosition = new Vector(0, 1),
                    lineAngularVelocity = Math.PI / 4,
                    pointAngularVelocity = 0,
                    pointLinearVelocity = new Vector(0, 0),
                    impactWindow = new Interval(0, 100),
                    maxIterations = 1000,
                };

                int iterations = 0;
                Scalar timeOfImpact;
                var success = RotatingPointvsRotatingLine.FindNextIntersection(input, out timeOfImpact, ref iterations);

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

            public void SimpleAngularMotion_Line_XAxis_Backwards()
            {
                var input = new Annulus.SweptCollisionDetection.RotatingPointvsRotatingLine.Input
                {
                    pointLeverArm = new Vector(1, 0),
                    initialPointOrientation = 0,
                    initialLineOrientation = 0,
                    initialLineElevation = 0,
                    initialPointPosition = new Vector(0, 1),
                    lineAngularVelocity = -Math.PI / 4,
                    pointAngularVelocity = 0,
                    pointLinearVelocity = new Vector(0, 0),
                    impactWindow = new Interval(0, 100),
                    maxIterations = 1000,
                };

                int iterations = 0;
                Scalar timeOfImpact;
                var success = RotatingPointvsRotatingLine.FindNextIntersection(input, out timeOfImpact, ref iterations);

                CheckEqual(EndingState.SUCCESS, success);
                CheckEqual(3, timeOfImpact);
            }

            public void SimpleAngularMotion_Line_YAxis()
            {
                var input = new Annulus.SweptCollisionDetection.RotatingPointvsRotatingLine.Input
                {
                    pointLeverArm = new Vector(1, 0),
                    initialPointOrientation = 0,
                    initialLineOrientation = Math.PI / 2,
                    initialLineElevation = 0,
                    initialPointPosition = new Vector(0, 1),
                    lineAngularVelocity = -Math.PI / 4,
                    pointAngularVelocity = 0,
                    pointLinearVelocity = new Vector(0, 0),
                    impactWindow = new Interval(0, 100),
                    maxIterations = 1000,
                };

                int iterations = 0;
                Scalar timeOfImpact;
                var success = RotatingPointvsRotatingLine.FindNextIntersection(input, out timeOfImpact, ref iterations);

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

            public void SimpleAngularMotion_Line_YAxis_Backwards()
            {
                var input = new Annulus.SweptCollisionDetection.RotatingPointvsRotatingLine.Input
                {
                    pointLeverArm = new Vector(1, 0),
                    initialPointOrientation = 0,
                    initialLineOrientation = Math.PI / 2,
                    initialLineElevation = 0,
                    initialPointPosition = new Vector(0, 1),
                    lineAngularVelocity = Math.PI / 4,
                    pointAngularVelocity = 0,
                    pointLinearVelocity = new Vector(0, 0),
                    impactWindow = new Interval(0, 100),
                    maxIterations = 1000,
                };

                int iterations = 0;
                Scalar timeOfImpact;
                var success = RotatingPointvsRotatingLine.FindNextIntersection(input, out timeOfImpact, ref iterations);

                CheckEqual(EndingState.SUCCESS, success);
                CheckEqual(3, timeOfImpact);
            }

            public void SlightlyComplexMotion_1()
            {
                var input = new Annulus.SweptCollisionDetection.RotatingPointvsRotatingLine.Input
                {
                    pointLeverArm = new Vector(1, 0),
                    initialPointOrientation = -Math.PI,
                    initialLineOrientation = 0,
                    initialLineElevation = 0,
                    initialPointPosition = new Vector(-3, 3),
                    lineAngularVelocity = Math.PI / 2,
                    pointAngularVelocity = Math.PI,
                    pointLinearVelocity = new Vector(2, 0),
                    impactWindow = new Interval(0, 100),
                    maxIterations = 1000,
                };

                int iterations = 0;
                Scalar timeOfImpact;
                var success = RotatingPointvsRotatingLine.FindNextIntersection(input, out timeOfImpact, ref iterations);

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

            public void SlightlyComplexMotion_1_Acceleration()
            {
                var input = new Annulus.SweptCollisionDetection.RotatingPointvsRotatingLine.Input
                {
                    pointLeverArm = new Vector(1, 0),
                    initialPointOrientation = -Math.PI,
                    initialLineOrientation = 0,
                    initialLineElevation = 0,
                    initialPointPosition = new Vector(-3, 3),
                    lineAngularVelocity = Math.PI / 2,
                    pointAngularVelocity = Math.PI,
                    pointLinearVelocity = new Vector(1.5, 0),
                    pointLinearAcceleration = new Vector(1, 0),
                    impactWindow = new Interval(0, 100),
                    maxIterations = 1000,
                };

                int iterations = 0;
                Scalar timeOfImpact;
                var success = RotatingPointvsRotatingLine.FindNextIntersection(input, out timeOfImpact, ref iterations);

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

            public void SlightlyComplexMotion_2()
            {
                var input = new Annulus.SweptCollisionDetection.RotatingPointvsRotatingLine.Input
                {
                    pointLeverArm = new Vector(3, 4),
                    initialPointOrientation = -Math.Atan2(4, 3),
                    initialLineOrientation = 0,
                    initialLineElevation = 0,
                    initialPointPosition = new Vector(10, 10),
                    lineAngularVelocity = -Math.PI / 8,
                    pointAngularVelocity = Math.PI / 2,
                    pointLinearVelocity = new Vector(-5, -2.5),
                    impactWindow = new Interval(0, 100),
                    maxIterations = 1000,
                };

                int iterations = 0;
                Scalar timeOfImpact;
                var success = RotatingPointvsRotatingLine.FindNextIntersection(input, out timeOfImpact, ref iterations);

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

            public void SlightlyComplexMotion_2_Acceleration()
            {
                var input = new Annulus.SweptCollisionDetection.RotatingPointvsRotatingLine.Input
                {
                    pointLeverArm = new Vector(3, 4),
                    initialPointOrientation = -Math.Atan2(4, 3),
                    initialLineOrientation = 0,
                    initialLineElevation = 0,
                    initialPointPosition = new Vector(10, 10),
                    lineAngularVelocity = -Math.PI / 8,
                    pointAngularVelocity = Math.PI / 2,
                    pointLinearVelocity = new Vector(-3, -.5),
                    pointLinearAcceleration = new Vector(-2, -2),
                    impactWindow = new Interval(0, 100),
                    maxIterations = 1000,
                };

                int iterations = 0;
                Scalar timeOfImpact;
                var success = RotatingPointvsRotatingLine.FindNextIntersection(input, out timeOfImpact, ref iterations);

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

            public void LineElevation_Angular()
            {
                var input = new Annulus.SweptCollisionDetection.RotatingPointvsRotatingLine.Input
                {
                    pointLeverArm = new Vector(1, 0),
                    initialPointOrientation = 0,
                    initialLineOrientation = 0,
                    initialLineElevation = 1,
                    initialPointPosition = new Vector(0, 0),
                    lineAngularVelocity = 0,
                    pointAngularVelocity = Math.PI / 2,
                    pointLinearVelocity = new Vector(0, 0),
                    impactWindow = new Interval(0, 3),
                    maxIterations = 1000,
                };

                int iterations = 0;
                Scalar timeOfImpact;
                var success = RotatingPointvsRotatingLine.FindNextIntersection(input, out timeOfImpact, ref iterations);

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

            public void LineElevation_AngularAndLinearVelocity()
            {
                var input = new Annulus.SweptCollisionDetection.RotatingPointvsRotatingLine.Input
                {
                    pointLeverArm = new Vector(1, 0),
                    initialPointOrientation = 0,
                    initialLineOrientation = 0,
                    initialLineElevation = 2,
                    initialPointPosition = new Vector(0, 0),
                    lineAngularVelocity = 0,
                    pointAngularVelocity = Math.PI / 2,
                    pointLinearVelocity = new Vector(0, 1),
                    impactWindow = new Interval(0, 3),
                    maxIterations = 1000,
                };

                int iterations = 0;
                Scalar timeOfImpact;
                var success = RotatingPointvsRotatingLine.FindNextIntersection(input, out timeOfImpact, ref iterations);

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

            public void LineElevation_AngularAndLinearAcceleration()
            {
                var input = new Annulus.SweptCollisionDetection.RotatingPointvsRotatingLine.Input
                {
                    pointLeverArm = new Vector(1, 0),
                    initialPointOrientation = 0,
                    initialLineOrientation = 0,
                    initialLineElevation = 2,
                    initialPointPosition = new Vector(0, 0),
                    lineAngularVelocity = 0,
                    pointAngularVelocity = Math.PI / 4,
                    pointLinearVelocity = new Vector(0, 0),
                    pointLinearAcceleration = new Vector(0, 0.5),
                    impactWindow = new Interval(0, 3),
                    maxIterations = 1000,
                };

                int iterations = 0;
                Scalar timeOfImpact;
                var success = RotatingPointvsRotatingLine.FindNextIntersection(input, out timeOfImpact, ref iterations);

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

        public class SafeStepForward : UnitTestSharp.TestFixture
        {
            public void NoMotion_NotIntersecting()
            {
                var input = new Annulus.SweptCollisionDetection.RotatingPointvsRotatingLine.Input
                {
                    pointLeverArm = new Vector(0, 0),
                    initialPointOrientation = 0,
                    initialLineOrientation = 0,
                    initialLineElevation = 0,
                    initialPointPosition = new Vector(0, 1),
                    lineAngularVelocity = 0,
                    pointAngularVelocity = 0,
                    pointLinearVelocity = new Vector(0, 0),
                    impactWindow = new Interval(0, 100),
                    maxIterations = 1000,
                };

                int iterations = 0;
                Interval newWindow;
                var success = RotatingPointvsRotatingLine.SafeStepForward
                    (input, out newWindow, ref iterations);

                CheckEqual(EndingState.BOUNDS_EXCEEDED, success);
                Check(Scalar.IsNaN(newWindow.Max));
                Check(Scalar.IsNaN(newWindow.Min));
            }

            public void NoMotion_Intersecting()
            {
                var input = new Annulus.SweptCollisionDetection.RotatingPointvsRotatingLine.Input
                {
                    pointLeverArm = new Vector(0, 0),
                    initialPointOrientation = 0,
                    initialLineOrientation = 0,
                    initialLineElevation = 0,
                    initialPointPosition = new Vector(0, 0),
                    lineAngularVelocity = 0,
                    pointAngularVelocity = 0,
                    pointLinearVelocity = new Vector(0, 0),
                    impactWindow = new Interval(0, 100),
                    maxIterations = 1000,
                };

                int iterations = 0;
                Interval newWindow;
                var success = RotatingPointvsRotatingLine.SafeStepForward
                    (input, out newWindow, ref iterations);

                CheckEqual(EndingState.BOUNDS_EXCEEDED, success);
                Check(Scalar.IsNaN(newWindow.Max));
                Check(Scalar.IsNaN(newWindow.Min));
            }

            public void PastOnlyRoot_Velocity()
            {
                var input = new Annulus.SweptCollisionDetection.RotatingPointvsRotatingLine.Input
                {
                    pointLeverArm = new Vector(0, 1),
                    initialPointOrientation = 0,
                    initialLineOrientation = 0,
                    initialLineElevation = 0,
                    initialPointPosition = new Vector(0, 0),
                    lineAngularVelocity = 0,
                    pointAngularVelocity = 0,
                    pointLinearVelocity = new Vector(0, -1),
                    impactWindow = new Interval(1, 100),
                    maxIterations = 1000,
                };

                int iterations = 0;
                Interval newWindow;
                var success = RotatingPointvsRotatingLine.SafeStepForward
                    (input, out newWindow, ref iterations);

                CheckEqual(EndingState.BOUNDS_EXCEEDED, success);
                Check(Scalar.IsNaN(newWindow.Max));
                Check(Scalar.IsNaN(newWindow.Min));
            }

            public void PastOnlyRoot_Acceleration()
            {
                var input = new Annulus.SweptCollisionDetection.RotatingPointvsRotatingLine.Input
                {
                    pointLeverArm = new Vector(0, 1),
                    initialPointOrientation = 0,
                    initialLineOrientation = 0,
                    initialLineElevation = 0,
                    initialPointPosition = new Vector(0, 0),
                    lineAngularVelocity = 0,
                    pointAngularVelocity = 0,
                    pointLinearVelocity = new Vector(0, 0),
                    pointLinearAcceleration = new Vector(0, -.5),
                    impactWindow = new Interval(1, 100),
                    maxIterations = 1000,
                };

                int iterations = 0;
                Interval newWindow;
                var success = RotatingPointvsRotatingLine.SafeStepForward
                    (input, out newWindow, ref iterations);

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

            public void SimpleAngularMotion_Point_XAxis()
            {
                var input = new Annulus.SweptCollisionDetection.RotatingPointvsRotatingLine.Input
                {
                    pointLeverArm = new Vector(1, 0),
                    initialPointOrientation = 0,
                    initialLineOrientation = 0,
                    initialLineElevation = 0,
                    initialPointPosition = new Vector(0, 1),
                    lineAngularVelocity = 0,
                    pointAngularVelocity = -Math.PI / 2,
                    pointLinearVelocity = new Vector(0, 0),
                    impactWindow = new Interval(1, 100),
                    maxIterations = 1000,
                };

                int iterations = 0;
                Interval newWindow;
                var success = RotatingPointvsRotatingLine.SafeStepForward
                    (input, out newWindow, ref iterations);

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

            public void SimpleAngularMotion_Point_Backwards_XAxis()
            {
                var input = new Annulus.SweptCollisionDetection.RotatingPointvsRotatingLine.Input
                {
                    pointLeverArm = new Vector(1, 0),
                    initialPointOrientation = 0,
                    initialLineOrientation = 0,
                    initialLineElevation = 0,
                    initialPointPosition = new Vector(0, 1),
                    lineAngularVelocity = 0,
                    pointAngularVelocity = Math.PI / 2,
                    pointLinearVelocity = new Vector(0, 0),
                    impactWindow = new Interval(3, 100),
                    maxIterations = 1000,
                };

                int iterations = 0;
                Interval newWindow;
                var success = RotatingPointvsRotatingLine.SafeStepForward
                    (input, out newWindow, ref iterations);

                CheckEqual(EndingState.SUCCESS, success);
                CheckNotEqual(3, newWindow.Min);
                Check(newWindow.Min > 1);
                Check(newWindow.Min < 5);
                CheckEqual(100, newWindow.Max);
            }

            public void SimpleAngularMotion_Point_YAxis()
            {
                Scalar existingRoot = 1;
                Scalar nextRoot = 5;

                var input = new Annulus.SweptCollisionDetection.RotatingPointvsRotatingLine.Input
                {
                    pointLeverArm = new Vector(0, 1),
                    initialPointOrientation = -Math.PI / 2,
                    initialLineOrientation = 0,
                    initialLineElevation = 0,
                    initialPointPosition = new Vector(0, 1),
                    lineAngularVelocity = 0,
                    pointAngularVelocity = -Math.PI / 2,
                    pointLinearVelocity = new Vector(0, 0),
                    impactWindow = new Interval(existingRoot, 100),
                    maxIterations = 1000,
                };

                int iterations = 0;
                Interval newWindow;
                var success = RotatingPointvsRotatingLine.SafeStepForward
                    (input, out newWindow, ref iterations);

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

            public void SimpleAngularMotion_Point_Backwards_YAxis()
            {
                Scalar existingRoot = 3;
                Scalar nextRoot = 7;

                var input = new Annulus.SweptCollisionDetection.RotatingPointvsRotatingLine.Input
                {
                    pointLeverArm = new Vector(0, 1),
                    initialPointOrientation = -Math.PI / 2,
                    initialLineOrientation = 0,
                    initialLineElevation = 0,
                    initialPointPosition = new Vector(0, 1),
                    lineAngularVelocity = 0,
                    pointAngularVelocity = Math.PI / 2,
                    pointLinearVelocity = new Vector(0, 0),
                    impactWindow = new Interval(existingRoot, 100),
                    maxIterations = 1000,
                };                

                int iterations = 0;
                Interval newWindow;
                var success = RotatingPointvsRotatingLine.SafeStepForward
                    (input, out newWindow, ref iterations);

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

            public void SimpleAngularMotion_Line_XAxis()
            {
                Scalar existingRoot = 1;
                Scalar nextRoot = 5;
                
                var input = new Annulus.SweptCollisionDetection.RotatingPointvsRotatingLine.Input
                {
                    pointLeverArm = new Vector(1, 0),
                    initialPointOrientation = 0,
                    initialLineOrientation = 0,
                    initialLineElevation = 0,
                    initialPointPosition = new Vector(0, 1),
                    lineAngularVelocity = Math.PI / 4,
                    pointAngularVelocity = 0,
                    pointLinearVelocity = new Vector(0, 0),
                    impactWindow = new Interval(existingRoot, 100),
                    maxIterations = 1000,
                };                

                int iterations = 0;
                Interval newWindow;
                var success = RotatingPointvsRotatingLine.SafeStepForward
                    (input, out newWindow, ref iterations);

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

            public void SimpleAngularMotion_Line_XAxis_Backwards()
            {
                Scalar existingRoot = 3;
                Scalar nextRoot = 7;

                var input = new Annulus.SweptCollisionDetection.RotatingPointvsRotatingLine.Input
                {
                    pointLeverArm = new Vector(1, 0),
                    initialPointOrientation = 0,
                    initialLineOrientation = 0,
                    initialLineElevation = 0,
                    initialPointPosition = new Vector(0, 1),
                    lineAngularVelocity = -Math.PI / 4,
                    pointAngularVelocity = 0,
                    pointLinearVelocity = new Vector(0, 0),
                    impactWindow = new Interval(existingRoot, 100),
                    maxIterations = 1000,
                };

                int iterations = 0;
                Interval newWindow;
                var success = RotatingPointvsRotatingLine.SafeStepForward
                    (input, out newWindow, ref iterations);

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

            public void SimpleAngularMotion_Line_YAxis()
            {
                Scalar existingRoot = 1;
                Scalar nextRoot = 5;

                var input = new Annulus.SweptCollisionDetection.RotatingPointvsRotatingLine.Input
                {
                    pointLeverArm = new Vector(1, 0),
                    initialPointOrientation = 0,
                    initialLineOrientation = Math.PI / 2,
                    initialLineElevation = 0,
                    initialPointPosition = new Vector(0, 1),
                    lineAngularVelocity = -Math.PI / 4,
                    pointAngularVelocity = 0,
                    pointLinearVelocity = new Vector(0, 0),
                    impactWindow = new Interval(existingRoot, 100),
                    maxIterations = 1000,
                };

                int iterations = 0;
                Interval newWindow;
                var success = RotatingPointvsRotatingLine.SafeStepForward
                    (input, out newWindow, ref iterations);

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

            public void SimpleAngularMotion_Line_YAxis_Backwards()
            {
                Scalar existingRoot = 3;
                Scalar nextRoot = 7;

                var input = new Annulus.SweptCollisionDetection.RotatingPointvsRotatingLine.Input
                {
                    pointLeverArm = new Vector(1, 0),
                    initialPointOrientation = 0,
                    initialLineOrientation = Math.PI / 2,
                    initialLineElevation = 0,
                    initialPointPosition = new Vector(0, 1),
                    lineAngularVelocity = Math.PI / 4,
                    pointAngularVelocity = 0,
                    pointLinearVelocity = new Vector(0, 0),
                    impactWindow = new Interval(existingRoot, 100),
                    maxIterations = 1000,
                };

                int iterations = 0;
                Interval newWindow;
                var success = RotatingPointvsRotatingLine.SafeStepForward
                    (input, out newWindow, ref iterations);

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

            public void SlightlyComplexMotion_1()
            {
                Scalar existingRoot = 1;
                Scalar nextRoot = 3;

                var input = new Annulus.SweptCollisionDetection.RotatingPointvsRotatingLine.Input
                {
                    pointLeverArm = new Vector(1, 0),
                    initialPointOrientation = -Math.PI,
                    initialLineOrientation = 0,
                    initialLineElevation = 0,
                    initialPointPosition = new Vector(-3, 3),
                    lineAngularVelocity = Math.PI / 2,
                    pointAngularVelocity = Math.PI,
                    pointLinearVelocity = new Vector(2, 0),
                    impactWindow = new Interval(existingRoot, 100),
                    maxIterations = 1000,
                };

                int iterations = 0;
                Interval newWindow;
                var success = RotatingPointvsRotatingLine.SafeStepForward
                    (input, out newWindow, ref iterations);

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

            public void SlightlyComplexMotion_2()
            {
                // dTheta = atan(-4/3)
                // dw = 5pi/8
                // w1 = -pi/8
                // <4,3> * [dTheta + dw * x] + 5 * cos(w1 * x)
                Scalar existingRoot = 2;
                Scalar nextRoot = 3;

                var input = new Annulus.SweptCollisionDetection.RotatingPointvsRotatingLine.Input
                {
                    pointLeverArm = new Vector(3, 4),
                    initialPointOrientation = -Math.Atan2(4, 3),
                    initialLineOrientation = 0,
                    initialLineElevation = 0,
                    initialPointPosition = new Vector(10, 10),
                    lineAngularVelocity = -Math.PI / 8,
                    pointAngularVelocity = Math.PI / 2,
                    pointLinearVelocity = new Vector(-5, -2.5),
                    impactWindow = new Interval(existingRoot, 100),
                    maxIterations = 1000,
                };

                int iterations = 0;
                Interval newWindow;
                var success = RotatingPointvsRotatingLine.SafeStepForward
                    (input, out newWindow, ref iterations);

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

            public void LineElevation_Angular()
            {
                // sin(pi/2*x) - 1
                Scalar existingRoot = 1;
                Scalar nextRoot = 5;

                var input = new Annulus.SweptCollisionDetection.RotatingPointvsRotatingLine.Input
                {
                    pointLeverArm = new Vector(1, 0),
                    initialPointOrientation = 0,
                    initialLineOrientation = 0,
                    initialLineElevation = 1,
                    initialPointPosition = new Vector(0, 0),
                    lineAngularVelocity = 0,
                    pointAngularVelocity = Math.PI / 2,
                    pointLinearVelocity = new Vector(0, 0),
                    impactWindow = new Interval(existingRoot, 100),
                    maxIterations = 1000,
                };

                int iterations = 0;
                Interval newWindow;
                var success = RotatingPointvsRotatingLine.SafeStepForward
                    (input, out newWindow, ref iterations);

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

            public void LineElevation_AngularAndLinear()
            {
                // x - 2 + sin(pi/2*x)
                Scalar existingRoot = 1;
                Scalar nextRoot = 2;
                
                var input = new Annulus.SweptCollisionDetection.RotatingPointvsRotatingLine.Input
                {
                    pointLeverArm = new Vector(1, 0),
                    initialPointOrientation = 0,
                    initialLineOrientation = 0,
                    initialLineElevation = 2,
                    initialPointPosition = new Vector(0, 0),
                    lineAngularVelocity = 0,
                    pointAngularVelocity = Math.PI / 2,
                    pointLinearVelocity = new Vector(0, 1),
                    impactWindow = new Interval(existingRoot, 100),
                    maxIterations = 1000,
                };

                int iterations = 0;
                Interval newWindow;
                var success = RotatingPointvsRotatingLine.SafeStepForward
                    (input, out newWindow, ref iterations);

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

        public class SafeStepBackward : UnitTestSharp.TestFixture
        {
            public void SimpleAngularMotion_Point_YAxis()
            {
                var input = new Annulus.SweptCollisionDetection.RotatingPointvsRotatingLine.Input
                {
                    pointLeverArm = new Vector(0, 1),
                    initialPointOrientation = -Math.PI / 2,
                    initialLineOrientation = 0,
                    initialLineElevation = 0,
                    initialPointPosition = new Vector(0, 1),
                    lineAngularVelocity = 0,
                    pointAngularVelocity = -Math.PI / 2,
                    pointLinearVelocity = new Vector(0, 0),
                };

                Scalar existingRoot = 5;
                Scalar prevRoot = 1;

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

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

            public void SimpleAngularMotion_Point_Backwards_YAxis()
            {
                var input = new Annulus.SweptCollisionDetection.RotatingPointvsRotatingLine.Input
                {
                    pointLeverArm = new Vector(0, 1),
                    initialPointOrientation = -Math.PI / 2,
                    initialLineOrientation = 0,
                    initialLineElevation = 0,
                    initialPointPosition = new Vector(0, 1),
                    lineAngularVelocity = 0,
                    pointAngularVelocity = Math.PI / 2,
                    pointLinearVelocity = new Vector(0, 0),
                };

                Scalar existingRoot = 7;
                Scalar prevRoot = 3;

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

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

            public void SimpleAngularMotion_Line_XAxis()
            {
                var input = new Annulus.SweptCollisionDetection.RotatingPointvsRotatingLine.Input
                {
                    pointLeverArm = new Vector(1, 0),
                    initialPointOrientation = 0,
                    initialLineOrientation = 0,
                    initialLineElevation = 0,
                    initialPointPosition = new Vector(0, 1),
                    lineAngularVelocity = Math.PI / 4,
                    pointAngularVelocity = 0,
                    pointLinearVelocity = new Vector(0, 0),
                };

                Scalar existingRoot = 5;
                Scalar prevRoot = 1;

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

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

            public void SimpleAngularMotion_Line_XAxis_Backwards()
            {
                var input = new Annulus.SweptCollisionDetection.RotatingPointvsRotatingLine.Input
                {
                    pointLeverArm = new Vector(1, 0),
                    initialPointOrientation = 0,
                    initialLineOrientation = 0,
                    initialLineElevation = 0,
                    initialPointPosition = new Vector(0, 1),
                    lineAngularVelocity = -Math.PI / 4,
                    pointAngularVelocity = 0,
                    pointLinearVelocity = new Vector(0, 0),
                };

                Scalar existingRoot = 7;
                Scalar prevRoot = 3;

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

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

            public void SimpleAngularMotion_Line_YAxis()
            {
                var input = new Annulus.SweptCollisionDetection.RotatingPointvsRotatingLine.Input
                {
                    pointLeverArm = new Vector(1, 0),
                    initialPointOrientation = 0,
                    initialLineOrientation = Math.PI / 2,
                    initialLineElevation = 0,
                    initialPointPosition = new Vector(0, 1),
                    lineAngularVelocity = -Math.PI / 4,
                    pointAngularVelocity = 0,
                    pointLinearVelocity = new Vector(0, 0),
                };

                Scalar existingRoot = 5;
                Scalar prevRoot = 1;

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

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

            public void SimpleAngularMotion_Line_YAxis_Backwards()
            {
                var input = new Annulus.SweptCollisionDetection.RotatingPointvsRotatingLine.Input
                {
                    pointLeverArm = new Vector(1, 0),
                    initialPointOrientation = 0,
                    initialLineOrientation = Math.PI / 2,
                    initialLineElevation = 0,
                    initialPointPosition = new Vector(0, 1),
                    lineAngularVelocity = Math.PI / 4,
                    pointAngularVelocity = 0,
                    pointLinearVelocity = new Vector(0, 0),
                };

                Scalar existingRoot = 7;
                Scalar prevRoot = 3;

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

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

            public void SlightlyComplexMotion_1()
            {
                var input = new Annulus.SweptCollisionDetection.RotatingPointvsRotatingLine.Input
                {
                    pointLeverArm = new Vector(1, 0),
                    initialPointOrientation = -Math.PI,
                    initialLineOrientation = 0,
                    initialLineElevation = 0,
                    initialPointPosition = new Vector(-3, 3),
                    lineAngularVelocity = Math.PI / 2,
                    pointAngularVelocity = Math.PI,
                    pointLinearVelocity = new Vector(2, 0),
                };

                Scalar existingRoot = 3;
                Scalar prevRoot = 1;

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

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

            public void SlightlyComplexMotion_2()
            {
                var input = new Annulus.SweptCollisionDetection.RotatingPointvsRotatingLine.Input
                {
                    pointLeverArm = new Vector(3, 4),
                    initialPointOrientation = -Math.Atan2(4, 3),
                    initialLineOrientation = 0,
                    initialLineElevation = 0,
                    initialPointPosition = new Vector(10, 10),
                    lineAngularVelocity = -Math.PI / 8,
                    pointAngularVelocity = Math.PI / 2,
                    pointLinearVelocity = new Vector(-5, -2.5),
                };

                // dTheta = atan(-4/3)
                // dw = 5pi/8
                // w1 = -pi/8
                // <4,3> * [dTheta + dw * x] + 5 * cos(w1 * x)
                Scalar existingRoot = 3;
                Scalar prevRoot = 2;

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

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

            public void LineElevation_Angular()
            {
                var input = new Annulus.SweptCollisionDetection.RotatingPointvsRotatingLine.Input
                {
                    pointLeverArm = new Vector(1, 0),
                    initialPointOrientation = 0,
                    initialLineOrientation = 0,
                    initialLineElevation = 1,
                    initialPointPosition = new Vector(0, 0),
                    lineAngularVelocity = 0,
                    pointAngularVelocity = Math.PI / 2,
                    pointLinearVelocity = new Vector(0, 0),
                };

                // sin(pi/2*x) - 1
                Scalar existingRoot = 5;
                Scalar prevRoot = 1;

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

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

            public void LineElevation_AngularAndLinear()
            {
                var input = new Annulus.SweptCollisionDetection.RotatingPointvsRotatingLine.Input
                {
                    pointLeverArm = new Vector(1, 0),
                    initialPointOrientation = 0,
                    initialLineOrientation = 0,
                    initialLineElevation = 2,
                    initialPointPosition = new Vector(0, 0),
                    lineAngularVelocity = 0,
                    pointAngularVelocity = Math.PI / 2,
                    pointLinearVelocity = new Vector(0, 1),
                };

                // x - 2 + sin(pi/2*x)
                Scalar existingRoot = 2;
                Scalar prevRoot = 1;

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

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