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

namespace SalsaModel
{
    public class TorsoMover
    {
        SalsaMover _mover;
        PositionModel.Side _side;
        Location _targetHipPos;
        Location _origHipPos;

        Angle _fromAngle;
        //Angle _toAngle;
        double _dAngle;

        double _fromFootZ;
        double _toFootZ;
        double _toFootInitZ;

        double _fromHipAngle;
        double _toHipAngle;

        public TorsoMover(SalsaMover mover, PositionModel.Side side, Location to, SalsaMover.TurnDirection turnDirection)
        {
            _mover = mover;
            _side = side;
            _targetHipPos = to;

            _origHipPos = mover.Model.GetLeg(_side).Hip;

            _fromFootZ = mover.Model.GetLeg(_side).Foot.Z;
            _toFootInitZ = mover.Model.GetOtherLeg(_side).Foot.Z;
            _toFootZ = mover.Body.Foot / 1.5;

            _fromHipAngle = _mover.HipAngle();
            double finalHipAngle = Math.PI / 24;
            _toHipAngle = side == PositionModel.Side.Left ? finalHipAngle : -finalHipAngle;

            _fromAngle = new Angle(mover.Model.SidewaysUnitDirection(PositionModel.Side.Left).XYOrientation() + Math.PI/2);
            Angle toAngle = new Angle(mover.Model.GetLeg(_side).Toe.Minus(mover.Model.GetLeg(_side).Foot).XYOrientation());

            _dAngle = (toAngle - _fromAngle).Value;

            // avoid for very small angles, they're probably rounding error
            if (Math.Abs(_dAngle) > 0.1)
            {
                if (turnDirection == SalsaMover.TurnDirection.Right)
                {
                    if (_dAngle < 0)
                    {
                        _dAngle += Math.PI * 2;
                    }
                }
                else if (turnDirection == SalsaMover.TurnDirection.Left)
                {
                    if (_dAngle > 0)
                    {
                        _dAngle -= Math.PI * 2;
                    }
                }
            }

            if (Math.Abs(_dAngle) > Math.PI * 1.9)
            {
                throw new Exception("schwing");
            }
        }

        public void Update(double percent)
        {
            _mover.CheckPosture();
            Check.Equal(_mover.Model.HipHeight, _mover.Model.LLeg.Hip.HalfWayTo(_mover.Model.RLeg.Hip).Z);

            Location fromStart = _targetHipPos.Minus(_origHipPos).Scale(percent).SetZ(0);
            Location currentOffset = _mover.Model.GetLeg(_side).Hip.Minus(_origHipPos).SetZ(0);
            _mover.OffsetTorso(fromStart.Minus(currentOffset));
            Check.Equal(_mover.Model.HipHeight, _mover.Model.LLeg.Hip.HalfWayTo(_mover.Model.RLeg.Hip).Z);

            Angle currentAngle = new Angle(_mover.Model.SidewaysUnitDirection(PositionModel.Side.Left).XYOrientation() + Math.PI / 2);

            Angle angle = new Angle(_dAngle * percent) + _fromAngle;
            // XXX should be angle - currentAngle -- but this way works?!
            _mover.RotateTorsoAboutZ(_mover.Model.GetLeg(_side).Hip, currentAngle - angle);
            Check.Equal(_mover.Model.HipHeight, _mover.Model.LLeg.Hip.HalfWayTo(_mover.Model.RLeg.Hip).Z);

            const double heelMovingTime = 1.0;

            double fromFootNewZ = _fromFootZ * Math.Min((1.0-percent)/heelMovingTime,1.0);
            double toFootNewZ = (_toFootZ - _toFootInitZ) * Math.Min(percent / heelMovingTime, 1.0) + _toFootInitZ;
            _mover.Model.GetLeg(_side).Foot = _mover.Model.GetLeg(_side).Foot.SetZ(fromFootNewZ);
            _mover.Model.GetOtherLeg(_side).Foot = _mover.Model.GetOtherLeg(_side).Foot.SetZ(toFootNewZ);

            Check.Equal(_mover.Model.HipHeight, _mover.Model.LLeg.Hip.HalfWayTo(_mover.Model.RLeg.Hip).Z);

            double hipAngle = (_toHipAngle - _fromHipAngle) * percent + _fromHipAngle;
            _mover.SetHipAngle(hipAngle);

            _mover.AlignKnees();

            _mover.CheckPosture();
        }
    }
}