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

namespace SalsaModel {
  public class SalsaMover {
    public PositionModel Model;
    public BodyGeometry Body;
    public SalsaMover(BodyGeometry body, Location initialPosition) {
      Body = body;
      Model = new PositionModel();
      Pose(initialPosition);
    }

    private SalsaMover() {
    }

    public SalsaMover Clone() {
      SalsaMover ret = new SalsaMover();
      ret.Body = Body;
      ret.Model = Model.Clone();
      return ret;
    }

    /// <summary>
    /// Move the given foot to the given position relative to the other foot,
    /// correcting for hip width.
    /// 
    /// So, MoveFootTo(side, Location.Origin) would leave feet in natural standing posture.
    /// </summary>
    public void MoveFootTo(PositionModel.Side side, Location loc) {
      MoveFootRel(side, loc, 0);
    }

    /// <summary>
    /// Move the given foot to the given position relative to the other foot,
    /// correcting for hip width.
    /// 
    /// So, MoveFootTo(side, Location.Origin, 0) would leave feet in natural standing posture.
    /// </summary>
    public void MoveFootRel(PositionModel.Side side, Location offset, double angle) {
      //CheckPosture();

      PositionModel.Leg leg = Model.GetLeg(side);
      Location toeOffset = leg.Toe.Minus(leg.Foot);
      Location newFootPos = GetStepLocation(side, offset);
      leg.Foot = newFootPos;
      leg.Toe = newFootPos.Plus(toeOffset.RotateZ(angle)).SetZ(0);

      //Location forward = ForwardKneePlane();
      //AlignKnee(leg, forward);
      //CheckPosture();
    }

      /// <summary>
      /// Returns the position to which the given foot should be moved, to be at the
      /// given offset relative to it's natural position with respect to the other
      /// foot.  So, moving the foot to GetStepLocation(side, Location.Origin) would
      /// leave the model standing feet parallel.
      /// </summary>
      public Location GetStepLocation(PositionModel.Side side, Location offset)
      {
          PositionModel.Leg leg = Model.GetLeg(side);
          PositionModel.Leg otherLeg = Model.GetOtherLeg(side);

          return otherLeg.Foot.Plus(NaturalFootOffset(side)).Plus(offset).SetZ(0);
      }

    public void MoveFootToNoOffset(PositionModel.Side side, Location loc, double angle) {
      PositionModel.Leg leg = Model.GetLeg(side);
      PositionModel.Leg otherLeg = Model.GetOtherLeg(side);

      Location hipOffset = leg.Hip.Minus(otherLeg.Hip);
      Location toeOffset = leg.Toe.Minus(leg.Foot);
      leg.Foot = otherLeg.Foot.Plus(loc).SetZ(0);
      leg.Toe = leg.Foot.Plus(toeOffset.RotateZ(angle)).SetZ(0);

      Location forward = ForwardKneePlane();
      AlignKnee(leg, forward);
      CheckPosture();
    }

    internal void MoveFootToSetShoulder(PositionModel.Side footSide, PositionModel.Side leadSide, Location leadShoulder) {
      Location targetOnFloor = leadShoulder.SetZ(0)
        .Minus(Model.SidewaysUnitDirection(leadSide).Scale((Body.ShoulderWidth - Body.FootSeparation) / 2.0));
      if (footSide != leadSide) {
        targetOnFloor = targetOnFloor.Plus(Model.SidewaysUnitDirection(footSide).Scale(Body.FootSeparation));
      }

      PositionModel.Leg leg = Model.GetLeg(footSide);
      Location currentOnFloor = leg.Foot.HalfWayTo(leg.Toe).SetZ(0);
      Location footOrientation = leg.Toe.Minus(leg.Foot).SetZ(0).Normalised();
      Location offset = targetOnFloor.Minus(currentOnFloor);

      leg.Foot = leg.Foot.Plus(offset);
      leg.Toe = leg.Toe.Plus(offset);

      Location forward = ForwardKneePlane();
      AlignKnee(leg, forward);
      CheckPosture();
    }

    public Location NaturalFootOffset(PositionModel.Side side) {
      return Model.SidewaysUnitDirection(side).Scale(Body.FootSeparation);
    }

    private Location ForwardKneePlane() {
      return Model.RLeg.Hip.Minus(Model.LLeg.Hip);
    }

    private void AlignKnee(PositionModel.Leg leg, Location kneePlaneNormal) {
      double legExtent = leg.Foot.DistanceTo(leg.Hip);
      // how far forward does the knee go?  Simplify by assuming that Shin == Thigh.
      double kneeOff = JointOffsetForLimbExtent(Body.Shin + Body.Thigh, legExtent);

      double ls = checked(Math.Sqrt(Body.Shin * Body.Shin - kneeOff * kneeOff));
      double lt = checked(Math.Sqrt(Body.Thigh * Body.Thigh - kneeOff * kneeOff));

      // the algorithm is not correct for large angles, this checks it.
      //Check.Equal(legExtent, ls + lt);

      Location footHip = leg.Foot.Minus(leg.Hip);
      Location kneeCenterLoc = leg.Hip.Plus(footHip.Scale(lt / legExtent));
      //double xTotal = leg.Foot.X - leg.Hip.X;
      //double x1 = (lt / legExtent) * xTotal;
      //double x2 = (ls / legExtent) * xTotal;
      //Check.Equal(xTotal, x1 + x2);

      //double z2 = (ls / legExtent) * (leg.Hip.Z - leg.Foot.Z);

      Location kneeDirection = footHip.CrossProduct(kneePlaneNormal);

      leg.Knee = kneeCenterLoc.Plus(kneeDirection.Normalised().Scale(kneeOff));
      //leg.Knee = leg.Foot.OffsetBy(-x2, kneeOff, z2);
    }

    /// <summary>
    /// Returns the offset for the knee/elbow, given the total length of the
    /// limb, and the distance from one end to the other.
    /// 
    /// NB this does make the simplicifacting assumption that both parts of the
    /// limb are the same length.
    /// </summary>
    /// <param name="length"></param>
    /// <param name="extent"></param>
    /// <returns></returns>
    public static double JointOffsetForLimbExtent(double length, double extent) {
      double partLength = length / 2;
      double offSqr = (partLength * partLength) - 0.25 * (extent * extent);
      return Math.Sqrt(offSqr);
    }

    public enum TurnDirection {
      Any,
      Left,
      Right
    }
    public TorsoMover HipOverFootMover(PositionModel.Side side, TurnDirection turnDirection) {
      PositionModel.Leg leg = Model.GetLeg(side);
      PositionModel.Leg otherLeg = Model.GetLeg(PositionModel.Other(side));
      Location target = leg.Foot.HalfWayTo(leg.Toe).SetZ(0);
      Location finalHipPosOnFloor = target.Plus(Model.SidewaysUnitDirection(side).Scale((Body.HipWidth-Body.FootSeparation)/2.0));

      //Location newHipPosOnFloor = finalHipPosOnFloor.Minus(startHipPosOnFloor).Scale(percent)
      //    .Plus(startHipPosOnFloor);

      Location finalHipPos = finalHipPosOnFloor.Plus(0, 0, leg.Hip.Z - leg.Toe.Z);
      //Check.Equal(Model.HipHeight, leg.Hip.Z - leg.Toe.Z, 0.0001);

      return new TorsoMover(this, side, finalHipPos, turnDirection);
    }

    public void AlignKnees() {
      Location forward = ForwardKneePlane();

      AlignKnee(Model.LLeg, forward);
      AlignKnee(Model.RLeg, forward);
    }

    public void OffsetTorso(Location shift) {
      Model.LLeg.Hip = Model.LLeg.Hip.Plus(shift);
      Model.LArm.Shoulder = Model.LArm.Shoulder.Plus(shift);
      Model.LArm.Elbow = Model.LArm.Elbow.Plus(shift);
      Model.LArm.Wrist = Model.LArm.Wrist.Plus(shift);
      Model.RLeg.Hip = Model.RLeg.Hip.Plus(shift);
      Model.RArm.Shoulder = Model.RArm.Shoulder.Plus(shift);
      Model.RArm.Elbow = Model.RArm.Elbow.Plus(shift);
      Model.RArm.Wrist = Model.RArm.Wrist.Plus(shift);
      Model.Head = Model.Head.Plus(shift);
    }

    internal void RotateTorsoAboutZ(Location axis, Angle angle) {
      RotateTorsoAboutZ(axis, angle.Value);
    }
    internal void RotateTorsoAboutZ(Location axis, double angle) {
      Model.LLeg.Hip = Model.LLeg.Hip.RotateAboutZ(axis, angle);
      Model.LArm.Shoulder = Model.LArm.Shoulder.RotateAboutZ(axis, angle);
      Model.LArm.Elbow = Model.LArm.Elbow.RotateAboutZ(axis, angle);
      Model.LArm.Wrist = Model.LArm.Wrist.RotateAboutZ(axis, angle);
      Model.RLeg.Hip = Model.RLeg.Hip.RotateAboutZ(axis, angle);
      Model.RArm.Shoulder = Model.RArm.Shoulder.RotateAboutZ(axis, angle);
      Model.RArm.Elbow = Model.RArm.Elbow.RotateAboutZ(axis, angle);
      Model.RArm.Wrist = Model.RArm.Wrist.RotateAboutZ(axis, angle);
      Model.Head = Model.Head.RotateAboutZ(axis, angle);
    }

    public void CheckPosture() {
      Check.Posture(Model, Body);
    }

    private void Pose(Location initialPosition) {
      double hipToShoulderX = (Body.ShoulderWidth - Body.HipWidth) / 2;

      // an arbitary guess as to how far we'd want to step, used to calculate
      // how far to drop the bum
      const double maxStep = 30;

      double arseHeight = Math.Sqrt(Math.Pow(Body.Shin + Body.Thigh, 2) - maxStep * maxStep);
      Model.HipHeight = arseHeight;

      Model.RLeg.Foot = initialPosition.Plus(-(Body.FootSeparation) / 2, 0, 0);
      Model.RLeg.Hip = initialPosition.Plus(-(Body.HipWidth) / 2, 0, arseHeight);
      Model.RArm.Shoulder = Model.RLeg.Hip.Plus(-hipToShoulderX, 0, Body.Trunk);
      Model.RLeg.Toe = Model.RLeg.Foot.Plus(0, Body.Foot, 0);

      Model.LLeg.Foot = initialPosition.Plus(Body.FootSeparation / 2, 0, 0);
      Model.LLeg.Hip = initialPosition.Plus(Body.HipWidth / 2, 0, arseHeight);
      Model.LArm.Shoulder = Model.LLeg.Hip.Plus(+hipToShoulderX, 0, Body.Trunk);
      Model.LLeg.Toe = Model.LLeg.Foot.Plus(0, Body.Foot, 0);

      //Model.RArm.Elbow = Model.RArm.Shoulder.OffsetBy(-Body.UpperArm, 0, 0);
      //Model.RArm.Wrist = Model.RArm.Elbow.OffsetBy(-Body.LowerArm, 0, 0);
      Model.RArm.Wrist = Model.RLeg.Hip.HalfWayTo(Model.LLeg.Hip).Plus(-Body.HandSeparation / 2, 10, 10);

      //Model.LArm.Elbow = Model.LArm.Shoulder.OffsetBy(Body.UpperArm, 0, 0);
      //Model.LArm.Wrist = Model.LArm.Elbow.OffsetBy(Body.LowerArm, 0, 0);
      Model.LArm.Wrist = Model.RLeg.Hip.HalfWayTo(Model.LLeg.Hip).Plus(Body.HandSeparation / 2, 10, 10);

      Model.Head = Model.Neck.Plus(0, 0, Body.Head);

      AlignKnees();
      AlignElbows(ElbowStyle.Out);

      Check.Posture(Model, Body);

      // left or right, should be the same
      HipOverFootMover(PositionModel.Side.Left, TurnDirection.Any).Update(1);

      Check.Posture(Model, Body);
    }

    public enum ElbowStyle {
      Out,
      Down
    }

    public void AlignElbow(PositionModel.Side side, ElbowStyle style) {
      PositionModel.Arm arm = Model.GetArm(side);
      switch (style) {
      case ElbowStyle.Out:
        AlignElbow(arm, Model.SidewaysUnitDirection(PositionModel.Other(side))
            .CrossProduct(arm.Shoulder.Minus(arm.Wrist)));
        break;
      case ElbowStyle.Down:
        AlignElbow(arm, Model.SidewaysUnitDirection(PositionModel.Side.Left));
        break;
      }
    }

    public void AlignElbows(ElbowStyle style) {
      AlignElbow(PositionModel.Side.Left, style);
      AlignElbow(PositionModel.Side.Right, style);
    }

    private void AlignElbow(PositionModel.Arm arm, Location planeNormal) {
      double offset = JointOffsetForLimbExtent(Body.UpperArm + Body.LowerArm, arm.Shoulder.DistanceTo(arm.Wrist));
      arm.Elbow = arm.Shoulder.HalfWayTo(arm.Wrist).Plus(planeNormal.CrossProduct(arm.Shoulder.Minus(arm.Wrist)).Normalised().Scale(offset));
    }

    public void Flip() {
      RotateTorsoAboutZ(Model.LLeg.Hip.HalfWayTo(Model.RLeg.Hip), Math.PI);

      // rotating the torso will have swapped the hips; swap them back, then we'll
      // swap the entire legs
      Location tmp;
      tmp = Model.LLeg.Hip;
      Model.LLeg.Hip = Model.RLeg.Hip;
      Model.RLeg.Hip = tmp;

      PositionModel.Leg tmpleg = Model.LLeg;
      Model.LLeg = Model.RLeg;
      Model.RLeg = tmpleg;

      Model.RLeg.Toe = Model.RLeg.Toe.Plus(Model.RLeg.Foot.Minus(Model.RLeg.Toe).SetZ(0).Scale(2));
      Model.LLeg.Toe = Model.LLeg.Toe.Plus(Model.LLeg.Foot.Minus(Model.LLeg.Toe).SetZ(0).Scale(2));

      Model.WallAngle += Math.PI;

      AlignKnees();

      CheckPosture();
    }

    /// <summary>
    /// The angle of the hip from the horizontal.
    /// </summary>
    /// <returns></returns>
    public double HipAngle() {
      double angle = Math.Acos(Model.RLeg.Hip.Minus(Model.LLeg.Hip).Normalised().DotProduct(Model.SidewaysUnitDirection(PositionModel.Side.Right).SetZ(0).Normalised()));
      if (Model.RLeg.Hip.Z < Model.LLeg.Hip.Z) {
        return angle;
      }
      else {
        return -angle;
      }
    }
    public void SetHipAngle(double angle) {
      Check.Equal(Model.HipHeight, Model.LLeg.Hip.HalfWayTo(Model.RLeg.Hip).Z);
      Location mid = Model.LLeg.Hip.HalfWayTo(Model.RLeg.Hip);
      Location XYDir = Model.SidewaysUnitDirection(PositionModel.Side.Left);
      Location offset = XYDir.Scale(Math.Cos(angle)).Plus(0, 0, Math.Sin(angle)).Scale(Body.HipWidth / 2.0);
      Model.LLeg.Hip = mid.Plus(offset);
      Model.RLeg.Hip = mid.Minus(offset);
      Check.Equal(0, mid.DistanceTo(Model.LLeg.Hip.HalfWayTo(Model.RLeg.Hip)));
      Check.Equal(Body.HipWidth, Model.LLeg.Hip.DistanceTo(Model.RLeg.Hip));
      Check.Equal(Model.HipHeight, Model.LLeg.Hip.HalfWayTo(Model.RLeg.Hip).Z);
    }

    public void RotateFootBy(PositionModel.Side side, double a) {
      PositionModel.Leg leg = Model.GetLeg(side);
      leg.Toe = leg.Toe.RotateAboutZ(leg.Foot, a);
    }

      public void RotateFootAroundToeBy(PositionModel.Side side, double a)
      {
          PositionModel.Leg leg = Model.GetLeg(side);
          leg.Foot = leg.Foot.RotateAboutZ(leg.Toe, a);
      }

  }
}