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

namespace Azimuth.UnitTests.RootFinding
{
    public class NewtonsMethodTests : UnitTestSharp.TestFixture
    {
        public class TraditionalTests : UnitTestSharp.TestFixture
        {
            public void Basic()
            {
                Scalar root;
                var success = NewtonsMethod.FindRoot(x => Math.Cos(x), x => -Math.Sin(x), 1, out root);

                CheckEqual(EndingState.SUCCESS, success);

                CheckEqual(Math.PI / 2, root);
            }

            public void NegativeMaxIterations()
            {
                Scalar root;
                int evaluations = 0;
                CheckThrow(typeof(Exception));
                var success = NewtonsMethod.FindRoot(x => Math.Cos(x), x => -Math.Sin(x), 1, -1, out root, ref evaluations);
            }

            public void ZeroMaxIterations()
            {
                Scalar root;
                int evaluations = 0;
                CheckThrow(typeof(Exception));
                var success = NewtonsMethod.FindRoot(x => Math.Cos(x), x => -Math.Sin(x), 1, 0, out root, ref evaluations);
            }

            public void ZeroDerivative()
            {
                Scalar root;
                var success = NewtonsMethod.FindRoot(x => 1 - x * x, x => -2 * x, 0, out root);

                CheckEqual(EndingState.MALFORMED_INPUT, success);

                Check(Scalar.IsNaN(root));
            }

            public void Cycle()
            {
                Scalar root;
                int evaluations = 0;
                Func<Scalar, Scalar> func = x => x * x * x - 2 * x + 2;
                Func<Scalar, Scalar> derivative = x => 3 * x * x - 2;
                var success = NewtonsMethod.FindRoot(func, derivative, 0, 64, out root, ref evaluations);

                CheckEqual(EndingState.DOUBLE_ROOT, success);

                Check(derivative(0) * derivative(root) < 0);
            }

            public void ExactHit()
            {
                Scalar root;
                int evaluations = 0;
                var success = NewtonsMethod.FindRoot(x => x, x => 0, 0, 64, out root, ref evaluations);

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

            public void HardToConverge()
            {
                // Just a rather complex function that has a double root at 2.

                var initialGuess = 1.9999972824853871;
                var initialLineOrientation = Math.PI / 4;
                var lineAngularVelocity = Math.PI / 8;
                var ptLeverArm = new Vector(10, 10);
                var ptLinearAcceleration = new Vector(0, -5);
                var ptLinearVelocity = new Vector(0, -5);
                var ptPos = new Vector(-10, 0);
                var dw = -Math.PI / 4;

                Func<Scalar, Scalar> function = (t) =>
                {
                    Scalar dAngle;
                    {
                        var dAngle_accum = new RobustAccumulator();
                        dAngle_accum.Add(dw);
                        dAngle_accum.Mul(t);
                        dAngle = dAngle_accum.ComputedSum;
                    }

                    Scalar angle1;
                    {
                        var angle1_accum = new RobustAccumulator();
                        angle1_accum.AddQuadratic(0, lineAngularVelocity, initialLineOrientation, t);
                        angle1 = angle1_accum.ComputedSum;
                    }

                    var displacementAccumulator_X = new RobustAccumulator();
                    displacementAccumulator_X.AddQuadratic(
                        -0.5 * ptLinearAcceleration.X,
                        -ptLinearVelocity.X,
                        -ptPos.X,
                        t);

                    var displacementAccumulator_Y = new RobustAccumulator();
                    displacementAccumulator_Y.AddQuadratic(
                        0.5 * ptLinearAcceleration.Y,
                        ptLinearVelocity.Y,
                        ptPos.Y,
                        t);

                    var accum = new RobustAccumulator();

                    accum.AddHarmonic(ptLeverArm.Y, ptLeverArm.X, dAngle);
                    accum.AddHarmonic(displacementAccumulator_Y, displacementAccumulator_X, angle1);

                    return accum.ComputedSum;
                };

                Func<Scalar, Scalar> derivative = (t) =>
                {
                    Scalar dAngle;
                    {
                        var dAngle_accum = new RobustAccumulator();
                        dAngle_accum.Add(dw);
                        dAngle_accum.Mul(t);
                        dAngle = dAngle_accum.ComputedSum;
                    }

                    Scalar angle1;
                    {
                        var angle1_accum = new RobustAccumulator();
                        angle1_accum.AddQuadratic(0, lineAngularVelocity, initialLineOrientation, t);
                        angle1 = angle1_accum.ComputedSum;
                    }

                    var w1 = lineAngularVelocity;

                    var displacementAccumulator_X = new RobustAccumulator();
                    displacementAccumulator_X.AddQuadratic(
                        -0.5 * ptLinearAcceleration.X,
                        -ptLinearVelocity.X,
                        -ptPos.X,
                        t);
                    displacementAccumulator_X.Mul(w1);
                    displacementAccumulator_X.AddQuadratic(
                        0,
                        -ptLinearAcceleration.X,
                        -ptLinearVelocity.X,
                        t);

                    var displacementAccumulator_Y = new RobustAccumulator();
                    displacementAccumulator_Y.AddQuadratic(
                        -0.5 * ptLinearAcceleration.Y,
                        -ptLinearVelocity.Y,
                        -ptPos.Y,
                        t);
                    displacementAccumulator_Y.Mul(w1);
                    displacementAccumulator_X.AddQuadratic(
                        0,
                        ptLinearAcceleration.Y,
                        ptLinearVelocity.Y,
                        t);

                    var accum = new RobustAccumulator();

                    accum.AddHarmonic(ptLeverArm.X * dw, -ptLeverArm.Y * dw, dAngle);
                    accum.AddHarmonic(displacementAccumulator_X, displacementAccumulator_Y, angle1);

                    return accum.ComputedSum;
                };

                Scalar root;
                var success = NewtonsMethod.FindRoot(function, derivative, initialGuess, out root);

                CheckEqual(EndingState.DOUBLE_ROOT, success);
                Check(derivative(initialGuess) * derivative(root) < 0);
            }
        }

        public class WithSecondDerivativeTests : UnitTestSharp.TestFixture
        {
            public void Basic()
            {
                Scalar root;
                var success = NewtonsMethod.FindRoot(x => Math.Cos(x), x => -Math.Sin(x), x => -Math.Cos(x), 1, out root);

                CheckEqual(EndingState.SUCCESS, success);

                CheckEqual(Math.PI / 2, root);
            }

            public void NegativeMaxIterations()
            {
                Scalar root;
                int evaluations = 0;
                CheckThrow(typeof(Exception));
                var success = NewtonsMethod.FindRoot(x => Math.Cos(x), x => -Math.Sin(x), 1, -1, out root, ref evaluations);
            }

            public void ZeroMaxIterations()
            {
                Scalar root;
                int evaluations = 0;
                CheckThrow(typeof(Exception));
                var success = NewtonsMethod.FindRoot(x => Math.Cos(x), x => -Math.Sin(x), 1, 0, out root, ref evaluations);
            }

            public void ZeroDerivative()
            {
                Scalar root;
                var success = NewtonsMethod.FindRoot(x => 1 - x * x, x => -2 * x, x => -2, 0, out root);

                CheckEqual(EndingState.MALFORMED_INPUT, success);

                Check(Scalar.IsNaN(root));
            }

            public void Cycle()
            {
                Scalar root;
                int evaluations = 0;
                Func<Scalar, Scalar> func = x => x * x * x - 2 * x + 2;
                Func<Scalar, Scalar> derivative = x => 3 * x * x - 2;
                Func<Scalar, Scalar> _2derivative = x => 6 * x;
                var success = NewtonsMethod.FindRoot(func, derivative, _2derivative, 0, 64, out root, ref evaluations);

                CheckEqual(EndingState.DOUBLE_ROOT, success);

                Check(derivative(0) * derivative(root) < 0);
            }

            public void ExactHit()
            {
                Scalar root;
                int evaluations = 0;
                var success = NewtonsMethod.FindRoot(x => x, x => 0, x => 0, 0, 64, out root, ref evaluations);

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

            public void QuadrupleRoot_OnRoot()
            {
                Scalar root;
                int evaluations = 0;
                var success = NewtonsMethod.FindRoot(
                    x => x * x * x * x - 1e-20,
                    x => 4 * x * x * x,
                    x => 12 * x * x, 0, 64, out root, ref evaluations);

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

            public void FuzzyDerivatives()
            {
                Func<Scalar, Scalar> func = t =>
                {
                    RobustAccumulator accum = new RobustAccumulator();
                    accum.AddQuadratic(
                        0.5 * 2.5,
                        -5,
                        6,
                        t);

                    accum.Add(-1);

                    return accum.ComputedSum;
                };

                Func<Scalar, Scalar> deriv = t =>
                {
                    RobustAccumulator accum = new RobustAccumulator();
                    accum.AddQuadratic(0, 2.5, -5, t);
                    accum.Add(-0.000000000000000000000000001009);

                    return accum.ComputedSum;
                };

                Func<Scalar, Scalar> secondDeriv = t =>
                {
                    return 2.5;
                };

                Scalar root;
                var success = NewtonsMethod.FindRoot(func, deriv, secondDeriv,
                    1.9999999999999716, out root);

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