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

namespace SalsaModel {
  public class HandHolder {
    SalsaMover _boy;
    SalsaMover _girl;
    public HandHolder(SalsaMover boy, SalsaMover girl) {
      _boy = boy;
      _girl = girl;
    }

    public enum Hand {
      None,
      Left,
      Right
    }

    public Hand LHold = Hand.None;
    public Hand RHold = Hand.None;

    public enum HoldStyle {
      Open,
      Closed,
      High,
      Turning
    }
    public HoldStyle LHoldStyle = HoldStyle.Open;
    public HoldStyle RHoldStyle = HoldStyle.Open;

    private bool _useLHandPos = false;
    private Location _lHandPos;
    public void UseLHandPos(Location location) {
      _useLHandPos = true;
      _lHandPos = location;
    }
    public void DontUseLHandPos() {
      _useLHandPos = false;
    }

    public void Update() {
      if (RHold != Hand.None) {
        switch (RHoldStyle) {
        case HoldStyle.Open: {
            Location rWrist = _boy.Model.RArm.Shoulder.HalfWayTo(_girl.Model.LArm.Shoulder).SetZ(_boy.Model.RLeg.Hip.Z + 5);
            _boy.Model.RArm.Wrist = rWrist;
            _girl.Model.LArm.Wrist = rWrist;
            _boy.AlignElbow(PositionModel.Side.Right, SalsaMover.ElbowStyle.Down);
            _girl.AlignElbow(PositionModel.Side.Left, SalsaMover.ElbowStyle.Down);
            break;
          }
        case HoldStyle.Closed: {
            _boy.Model.RArm.Wrist = _girl.Model.LArm.Shoulder.FractionTo(_girl.Model.RArm.Shoulder, 0.3).Plus(0, 0, -10);
            _girl.Model.LArm.Wrist = _boy.Model.RArm.Shoulder;
            _boy.AlignElbow(PositionModel.Side.Right, SalsaMover.ElbowStyle.Out);
            _girl.AlignElbow(PositionModel.Side.Left, SalsaMover.ElbowStyle.Out);
            break;
          }
        }
      }
      else {
        _boy.AlignElbow(PositionModel.Side.Right, SalsaMover.ElbowStyle.Out);
        _girl.AlignElbow(PositionModel.Side.Left, SalsaMover.ElbowStyle.Out);
      }
      if (LHold != Hand.None) {
        PositionModel.Arm boyArm = _boy.Model.LArm;
        PositionModel.Arm girlArm = _girl.Model.RArm;
        Location wrist;
        switch (LHoldStyle) {
        case HoldStyle.Open:
          wrist = boyArm.Shoulder.HalfWayTo(girlArm.Shoulder);
          wrist = wrist.SetZ(_boy.Model.LLeg.Hip.Z + 5);
          boyArm.Wrist = wrist;
          girlArm.Wrist = wrist;
          break;
        case HoldStyle.Closed:
          wrist = boyArm.Shoulder.HalfWayTo(girlArm.Shoulder);
          Location outDirection = boyArm.Shoulder.Minus(girlArm.Shoulder).CrossWithZ().Scale(0.5);
          wrist = wrist.Minus(outDirection).SetZ(girlArm.Shoulder.Z);
          boyArm.Wrist = wrist;
          girlArm.Wrist = wrist;
          break;
#if false
                    case HoldStyle.High:
                        wrist = girlArm.Shoulder.Plus(boyArm.Shoulder.Minus(girlArm.Shoulder).Normalised().Scale(_girl.Body.UpperArm));
                        wrist = wrist.SetZ(girlArm.Shoulder.Z + _girl.Body.LowerArm);
                        boyArm.Wrist = wrist;
                        girlArm.Wrist = wrist;
                        break;
#endif
        case HoldStyle.High:
        case HoldStyle.Turning:
          wrist = girlArm.Shoulder.Plus(_girl.Model.ForwardUnit().Scale(_girl.Body.ShoulderWidth / 2));
          wrist = wrist.SetZ(girlArm.Shoulder.Z + _girl.Body.LowerArm);
          boyArm.Wrist = wrist;
          girlArm.Wrist = wrist;
          break;
        }
        if (_useLHandPos) {
          boyArm.Wrist = _lHandPos;
          girlArm.Wrist = _lHandPos;
          //_useLHandPos = false;
        }
        switch (LHoldStyle) {
        case HoldStyle.Open:
          _boy.AlignElbow(PositionModel.Side.Left, SalsaMover.ElbowStyle.Down);
          _girl.AlignElbow(PositionModel.Side.Right, SalsaMover.ElbowStyle.Down);
          break;
        case HoldStyle.Closed:
          _boy.AlignElbow(PositionModel.Side.Left, SalsaMover.ElbowStyle.Down);
          _girl.AlignElbow(PositionModel.Side.Right, SalsaMover.ElbowStyle.Down);
          break;
        case HoldStyle.High:
        case HoldStyle.Turning:
          _boy.AlignElbow(PositionModel.Side.Left, SalsaMover.ElbowStyle.Out);
          _girl.AlignElbow(PositionModel.Side.Right, SalsaMover.ElbowStyle.Down);
          break;
        }
      }
      else {
        _boy.AlignElbow(PositionModel.Side.Left, SalsaMover.ElbowStyle.Out);
        _girl.AlignElbow(PositionModel.Side.Right, SalsaMover.ElbowStyle.Out);
      }
    }

    internal void MoveGirl(PositionModel.Side girlFoot, PositionModel.Side leadHand, SalsaMover newBoy) {
      if (leadHand == PositionModel.Side.Left && LHold != Hand.None) {
        MoveGirlHelper(LHoldStyle, girlFoot, leadHand, newBoy,
          newBoy.Model.GetArm(leadHand), _girl.Model.RArm);
      }
      if (leadHand == PositionModel.Side.Right && RHold != Hand.None) {
        MoveGirlHelper(RHoldStyle, girlFoot, leadHand, newBoy,
          newBoy.Model.RArm, _girl.Model.LArm);
      }
    }

    private Location ShoulderToWrist(HoldStyle style, PositionModel.Side side, Location forwardUnit, BodyGeometry body) {
      double sideDirection = (side == PositionModel.Side.Left ? 1 : -1);
      switch (style) {
      case HoldStyle.Open:
        return Location.Origin
          .Plus(forwardUnit.Scale(body.LowerArm * 0.9))
          .Plus(0, 0, -(body.Trunk - 5));
      case HoldStyle.Closed:
        return Location.Origin
          .Plus(forwardUnit.Scale(body.LowerArm * 0.6))
          .Plus(forwardUnit.CrossWithZ().Scale(body.LowerArm * 0.3 * sideDirection))
          .Plus(0, 0, -5);
      case HoldStyle.High:
        return Location.Origin
          .Plus(forwardUnit.Scale(body.LowerArm * 0.6))
          .Plus(forwardUnit.CrossWithZ().Scale(body.LowerArm * 0.3 * sideDirection))
          .Plus(0, 0, 10);

      default:
        throw new Exception("ding dong");
      }
    }

    private void MoveGirlHelper(HoldStyle style, PositionModel.Side girlFoot, PositionModel.Side leadHand, SalsaMover newBoy, PositionModel.Arm boyArm, PositionModel.Arm girlArm) {
      Location wrist;
      if (_useLHandPos && leadHand == PositionModel.Side.Left) {
        wrist = _lHandPos;
      }
      else {
        // calculate the wrist location, based on the boy's shoulder position and direction, but
        // adjusted to be more comfortable for the girl's body size.
        Location wristOffset = ShoulderToWrist(style, leadHand, newBoy.Model.ForwardUnit(), _girl.Body);
        Location girlBoyShoulderCorrection = Location.Origin //newBoy.Model.ForwardUnit().CrossWithZ().Scale((_boy.Body.ShoulderWidth - _girl.Body.ShoulderWidth) / -2)
          .Plus(0, 0, _boy.Model.LArm.Shoulder.Z - _girl.Model.LArm.Shoulder.Z);
        wrist = boyArm.Shoulder.Plus(wristOffset).Plus(girlBoyShoulderCorrection);
      }

      PositionModel.Leg girlLeg = _girl.Model.GetLeg(girlFoot);
      Location footUnitDirection = girlLeg.Toe.Minus(girlLeg.Foot).SetZ(0).Normalised();
      {
        Location girlShoulderPos = wrist.Minus(ShoulderToWrist(style, PositionModel.Other(leadHand), footUnitDirection, _girl.Body));
        _girl.MoveFootToSetShoulder(girlFoot, PositionModel.Other(leadHand), girlShoulderPos);
      }
    }
  }
}