using System;
using System.Collections.Generic;
using System.Text;
using NUnit.Framework;
using SalsaModel.Actions;
using SalsaModel.Scheduling;

namespace SalsaModel.Tests
{
    [TestFixture]
    public class TurningTest
    {
        [Test]
        public void Test1()
        {
            SalsaMover mover = new SalsaMover(BodyGeometry.Girl(), new Location(5, -BodyGeometry.Girl().LowerArm, 0));
            mover.CheckPosture();
            TimeLine tl = new TimeLine();
            CompositeActions.StepForward(tl, 0.0, 1.0, mover, PositionModel.Side.Left, 20);
            tl.Add(1.0, 1.0, new HalfTurn(mover, PositionModel.Side.Right, 0, SalsaMover.TurnDirection.Right));

            double prev = -1.0;
            double now = 0.1;

            assertHipMiddleBetweenFeet(mover);

            tl.RunActions(prev, now);
            assertHipMiddleBetweenFeet(mover);

            for (; now <= 2.0; now += 0.1)
            {
                try
                {
                    tl.RunActions(prev, now);
                    prev = now;
                    assertHipMiddleBetweenFeet(mover);

                    mover.AlignKnees();
                    mover.CheckPosture();
                    assertHipMiddleBetweenFeet(mover);
                }
                catch (Exception e)
                {
                    throw new Exception("At time " + now.ToString() + ": " + e.Message);
                }
            }
        }

        [Test] [Ignore("fails -- not sure why?")]
        public void TestSRTurn()
        {
            SalsaMover mover = new SalsaMover(BodyGeometry.Girl(), new Location(5, -BodyGeometry.Girl().LowerArm, 0));
            mover.CheckPosture();
            TimeLine tl = new TimeLine();
            CompositeActions.SingleRightTurn(tl, 0.0, mover);

            double prev = -1.0;
            double now = 0.1;

            assertHipMiddleBetweenFeet(mover);

            tl.RunActions(prev, now);
            assertHipMiddleBetweenFeet(mover);
            double origSidewaysPos = hipSidewaysPos(mover);

            for (; now <= 5.0; now += 0.1)
            {
                try
                {
                    tl.RunActions(prev, now);
                    prev = now;
                    assertHipMiddleBetweenFeet(mover);

                    mover.AlignKnees();
                    mover.CheckPosture();
                    assertHipMiddleBetweenFeet(mover);
                    Console.WriteLine("{0}: {1}", now, hipSidewaysPos(mover));
                }
                catch (Exception e)
                {
                    throw new Exception("At time " + now.ToString() + ": " + e.Message);
                }
            }

            MyAssert.Similar(origSidewaysPos, hipSidewaysPos(mover), 0.1);
        }

        private static double hipSidewaysPos(SalsaMover mover)
        {
            Location sideways = (new Location(0, 1, 0)).RotateZ(mover.Model.WallAngle);
            return mover.Model.HipMidPoint.DotProduct(sideways);
        }

        private static void assertHipMiddleBetweenFeet(SalsaMover mover)
        {
            Location sideways = (new Location(0, 1, 0)).RotateZ(mover.Model.WallAngle);
            double hipMiddle = hipSidewaysPos(mover);
            double lfoot = mover.Model.LLeg.Toe.DotProduct(sideways);
            double rfoot = mover.Model.RLeg.Toe.DotProduct(sideways);

            if (lfoot > rfoot)
            {
                double temp = lfoot;
                lfoot = rfoot;
                rfoot = temp;
            }

            //Console.WriteLine("{0} <- {1} -> {2}", lfoot, hipMiddle, rfoot);

            // the weight is allowed to go outside the feet, but it shouldn't
            // go further outside than the distance between the feet.  It's only
            // allowed to go that far because, next time we step, we might be
            // bringing a foot around so it'll be magically right.
            double betweenFeet = rfoot - lfoot;

            Assert.Greater(hipMiddle, lfoot - betweenFeet);
            Assert.Less(hipMiddle, rfoot + betweenFeet);
        }
    }
}