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

namespace SalsaModel.Tests
{
    [TestFixture]
    public class TestFollowingNothing
    {
        const double c_distance = 70;

        void AssertSimilar(double expected, double actual, double epsilon) {
            double diff = expected - actual;
            Assert.IsTrue(diff * diff < epsilon * epsilon, "\n    expected: {0} +/- {1}\n     but was: {2}\n", expected, epsilon, actual);
        }

        [Test]
        public void TestFollowNothing()
        {
            SalsaMover Boy = new SalsaMover(BodyGeometry.Andy(), new Location(5, -BodyGeometry.Andy().LowerArm, 0));
            SalsaMover Girl = new SalsaMover(BodyGeometry.Girl(), new Location(-5, BodyGeometry.Girl().LowerArm, 0));
            Girl.Model.IsGirl = true;
            Girl.Flip();

            TimeLine timeLine = new TimeLine();
            SetupTimeLineFollowNothing(timeLine, Boy, Girl, 1.0);
            SetupTimeLineFollowNothing(timeLine, Boy, Girl, 9.0);

            TestFollowDistance(Boy, Girl, timeLine, 16.5);
        }

        [Test]
        public void TestFollowMambo()
        {
            SalsaMover Boy = new SalsaMover(BodyGeometry.Andy(), new Location(5, -BodyGeometry.Andy().LowerArm, 0));
            SalsaMover Girl = new SalsaMover(BodyGeometry.Girl(), new Location(-5, BodyGeometry.Girl().LowerArm, 0));
            Girl.Model.IsGirl = true;
            Girl.Flip();

            TimeLine timeLine = new TimeLine();

            CompositeActions.FollowForward(timeLine, 1.0, 1.0, Boy, Girl, PositionModel.Side.Left, -15, c_distance);
            CompositeActions.FollowForward(timeLine, 2.0, 1.0, Boy, Girl, PositionModel.Side.Right, -15, c_distance);
            CompositeActions.FollowForward(timeLine, 3.0, 1.0, Boy, Girl, PositionModel.Side.Left, 15, c_distance);
            CompositeActions.FollowForward(timeLine, 5.0, 1.0, Boy, Girl, PositionModel.Side.Right, 15, c_distance);
            CompositeActions.FollowForward(timeLine, 6.0, 1.0, Boy, Girl, PositionModel.Side.Left, 15, c_distance);
            CompositeActions.FollowForward(timeLine, 7.0, 1.0, Boy, Girl, PositionModel.Side.Right, -15, c_distance);

            CompositeActions.FollowForward(timeLine, 9.0, 1.0, Boy, Girl, PositionModel.Side.Left, -15, c_distance);
            CompositeActions.FollowForward(timeLine, 10.0, 1.0, Boy, Girl, PositionModel.Side.Right, -15, c_distance);
            CompositeActions.FollowForward(timeLine, 11.0, 1.0, Boy, Girl, PositionModel.Side.Left, 15, c_distance);
            CompositeActions.FollowForward(timeLine, 13.0, 1.0, Boy, Girl, PositionModel.Side.Right, 15, c_distance);
            CompositeActions.FollowForward(timeLine, 14.0, 1.0, Boy, Girl, PositionModel.Side.Left, 15, c_distance);
            CompositeActions.FollowForward(timeLine, 15.0, 1.0, Boy, Girl, PositionModel.Side.Right, -15, c_distance);

            TestFollowDistance(Boy, Girl, timeLine, 16.5);
        }

        private void TestFollowDistance(SalsaMover Boy, SalsaMover Girl, TimeLine timeLine, double maxTime)
        {
            Location origOffset = BellyButtonOffset(Boy, Girl);

            double prev = -1;
            double now = 0;
            for (; now < 16.5; now += 0.1)
            {
                timeLine.RunActions(prev, now);
                prev = now;

                Boy.AlignKnees();
                Boy.CheckPosture();
                Girl.AlignKnees();
                Girl.CheckPosture();

                Location offset = BellyButtonOffset(Boy, Girl);
                //Console.WriteLine("@{0}: offset is {1},{2},{3}", now, offset.X, offset.Y, offset.Z);

                AssertSimilar(origOffset.X, offset.X, 0.2);
                AssertSimilar(origOffset.Z, offset.Z, 0.01);
                if (now < 1.0)
                {
                    // nothing happens between 0 and 1; the first action is on 1.
                    Assert.AreEqual(origOffset.Y, offset.Y);
                }
                else if (now < 2.0)
                {
                    // between 1 and 2, we'll be getting into position, so the Y offset
                    // could vary
                    //Assert.AreEqual(origOffset.Y, offset.Y);
                }
                else
                {
                    // by 2, we should be in position, so the offset should be the distance
                    AssertSimilar(c_distance, -offset.Y, 0.01);
                }
            }
        }

        private static Location BellyButtonOffset(SalsaMover Boy, SalsaMover Girl)
        {
            Location offset = Boy.Model.LLeg.Hip.HalfWayTo(Boy.Model.RLeg.Hip)
                .Minus(Girl.Model.LLeg.Hip.HalfWayTo(Girl.Model.RLeg.Hip));
            return offset;
        }

        private void SetupTimeLineFollowNothing(TimeLine timeLine, SalsaMover Boy, SalsaMover Girl, double from)
        {
            CompositeActions.FollowForward(timeLine, from + 0.0, 1.0, Boy, Girl, PositionModel.Side.Left, 0, c_distance);
            CompositeActions.FollowForward(timeLine, from + 1.0, 1.0, Boy, Girl, PositionModel.Side.Right, 0, c_distance);
            CompositeActions.FollowForward(timeLine, from + 2.0, 1.0, Boy, Girl, PositionModel.Side.Left, 0, c_distance);
            CompositeActions.FollowForward(timeLine, from + 4.0, 1.0, Boy, Girl, PositionModel.Side.Right, 0, c_distance);
            CompositeActions.FollowForward(timeLine, from + 5.0, 1.0, Boy, Girl, PositionModel.Side.Left, 0, c_distance);
            CompositeActions.FollowForward(timeLine, from + 6.0, 1.0, Boy, Girl, PositionModel.Side.Right, 0, c_distance);
        }
    }
}