using System;
using System.Collections.Generic;
using System.Text;

namespace SalsaModel
{

    /// <summary>
    /// This is the thing that we draw.
    /// </summary>
    public class PositionModel
    {
        public enum Side
        {
            Left,
            Right
        };

        public static string SideCode(Side side)
        {
            switch (side)
            {
                case Side.Left: return "L";
                case Side.Right: return "R";
                default: throw new InvalidOperationException("no such value");
            }
        }

        public class Leg
        {
            public Location Toe;
            public Location Foot;
            public Location Knee;
            public Location Hip;

            public Leg Clone()
            {
                Leg ret = new Leg();
                ret.Toe = Toe;
                ret.Foot = Foot;
                ret.Knee = Knee;
                ret.Hip = Hip;
                return ret;
            }
        }
        public Leg LLeg = new Leg();
        public Leg RLeg = new Leg();

        public class Arm
        {
            public Location Shoulder;
            public Location Elbow;
            public Location Wrist;
            public Arm Clone()
            {
                Arm ret = new Arm();
                ret.Shoulder = Shoulder;
                ret.Elbow = Elbow;
                ret.Wrist = Wrist;
                return ret;
            }
        }
        public Arm RArm = new Arm();
        public Arm LArm = new Arm();

        public Location Head;

        public bool IsGirl = false;

        public double HipHeight;

        public double WallAngle = Math.PI / 2.0;
        public Location WallForwardsUnit
        {
            get
            {
                return new Location(1, 0, 0).RotateZ(WallAngle);
            }
        }

        public PositionModel Clone()
        {
            PositionModel ret = new PositionModel();
            ret.LLeg = LLeg.Clone();
            ret.RLeg = RLeg.Clone();
            ret.LArm = LArm.Clone();
            ret.RArm = RArm.Clone();
            ret.Head = Head;
            ret.IsGirl = IsGirl;
            ret.HipHeight = HipHeight;
            ret.WallAngle = WallAngle;
            return ret;
        }

        public Leg GetLeg(Side side)
        {
            switch (side)
            {
                case Side.Left: return LLeg;
                case Side.Right: return RLeg;
                default: throw new Exception("can't get here");
            }
        }
        public static Side Other(Side side)
        {
            switch (side)
            {
                case Side.Left: return Side.Right;
                case Side.Right: return Side.Left;
                default: throw new Exception("can't get here");
            }
        }

        public Arm GetArm(Side side)
        {
            switch (side)
            {
                case Side.Left: return LArm;
                case Side.Right: return RArm;
                default: throw new Exception("can't get here");
            }
        }
        public Location Neck { get { return LArm.Shoulder.HalfWayTo(RArm.Shoulder); } }

        public Location SidewaysUnitDirection(Side side)
        {
            return GetLeg(side).Hip.Minus(GetOtherLeg(side).Hip).SetZ(0).Normalised();
        }
        public Location ForwardUnit()
        {
            return SidewaysUnitDirection(Side.Right).CrossProduct(new Location(0, 0, 1));
        }

        internal Leg GetOtherLeg(Side side)
        {
            return GetLeg(Other(side));
        }

        public Location HipMidPoint
        {
            get
            {
                return LLeg.Hip.HalfWayTo(RLeg.Hip);
            }
        }

        /// <summary>
        /// Converts a position relative to the body into an absolute world position.
        /// The relative position is an offset from the HipMidPoint, with the Y
        /// axis pointing forwards along the wall.
        /// </summary>
        /// <param name="relativePos">offset from the HipMidPoint, with the Y
        /// axis pointing forwards along the wall.</param>
        /// <returns>position in absolute space</returns>
        public Location RelToAbsPos(Location relativePos)
        {
            return relativePos
                .RotateZ(WallAngle - Math.PI/2.0)
                .Plus(HipMidPoint);
        }

        /// <summary>
        /// Converts an absolute world position into a position relative to the body.
        /// The relative position is an offset from the HipMidPoint, with the Y
        /// axis pointing forwards along the wall.
        /// </summary>
        /// <param name="relativePos">position in absolute space</param>
        /// <returns>offset from the HipMidPoint, with the Y
        /// axis pointing forwards along the wall.</returns>
        public Location AbsToRelPos(Location absolutePos)
        {
            return AbsToRelPos(absolutePos, HipMidPoint);
        }

        public Location AbsToRelPos(Location absolutePos, Location useHipMidPoint)
        {
            return absolutePos
                .Minus(useHipMidPoint)
                .RotateZ(-(WallAngle - Math.PI / 2.0));
        }


    }
}