using System;
using System.Collections.Generic;
using System.Text;
using SalsaModel.Scheduling;

namespace SalsaModel.Actions
{
    class CubanMotionAction : IAction
    {
        public static CubanMotionAction AddTo(TimeLine timeLine, double start, double duration, SalsaMover mover, PositionModel.Side side, double distance, double turnAngle)
        {
            CubanMotionAction newAct = new CubanMotionAction(mover, side, distance, turnAngle);
            timeLine.Add(start, duration, newAct);
            return newAct;
        }

        SalsaMover _mover;
        PositionModel.Side _side;
        double _distance;

        bool _haveStarted = false;
        Location _targetHipPos;
        Location _origHipPos;

        double _fromAngle;
        double _dAngle;

        double _fromHipAngle;
        double _toHipAngle;
        ShoulderAction _shoulderAction;
        HeelMoverAction _heelAction;

        public CubanMotionAction(SalsaMover mover, PositionModel.Side side, double distance, double turnAngle)
        {
            _mover = mover;
            _side = side;
            _distance = distance;
            _dAngle = turnAngle;
            _shoulderAction = new ShoulderAction(mover, side);
            _heelAction = new HeelMoverAction(mover, side);
        }

        public void Start()
        {
            _origHipPos = _mover.Model.GetLeg(_side).Hip;
            // XXX what is the difference between these two?  I can't remember
#if false
            Location targetFootPos = _mover.GetStepLocation(_side, _mover.Model.WallForwardsUnit.Scale(_distance));
#else
            Location targetFootPos = _mover.Model.GetLeg(_side).Foot;
            {
                double fwdist = _mover.Model.GetOtherLeg(_side).Foot.DotProduct(_mover.Model.WallForwardsUnit)
                    - _mover.Model.GetLeg(_side).Foot.DotProduct(_mover.Model.WallForwardsUnit)
                    + _distance;
                targetFootPos = targetFootPos.Plus(_mover.Model.WallForwardsUnit.Scale(fwdist));
            }
#endif
            _targetHipPos = targetFootPos
                .Plus(_mover.Model.SidewaysUnitDirection(_side).RotateAboutZ(Location.Origin, _dAngle).Scale((_mover.Body.HipWidth - _mover.Body.FootSeparation) / 2.0)
                .Plus(_mover.Model.WallForwardsUnit.Scale(_mover.Body.Foot / 2.0))
                );

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

            _fromAngle = _mover.Model.SidewaysUnitDirection(PositionModel.Side.Left).XYOrientation();

            _shoulderAction.Start();
            _heelAction.Start();

            _haveStarted = true;
        }

        public void Update(double percent)
        {
            Update(_mover, percent);
            // Ouch
            _shoulderAction.Update(percent);
            _heelAction.Update(percent);
        }
        
        private void Update(SalsaMover mover, 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);

            if (_dAngle != 0)
            {
                double currentAngle = mover.Model.SidewaysUnitDirection(PositionModel.Side.Left).XYOrientation();
                double desiredAngle = _dAngle * percent + _fromAngle;
                // _mover.Model.GetLeg(_side).Foot
                mover.RotateTorsoAboutZ(mover.Model.HipMidPoint, desiredAngle - currentAngle);
            }
            Check.Equal(mover.Model.HipHeight, mover.Model.LLeg.Hip.HalfWayTo(mover.Model.RLeg.Hip).Z);

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

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

        public void End()
        {
            _shoulderAction.End();
            _heelAction.End();
        }

        private SalsaMover _destMover = null;
        public SalsaMover Destination()
        {
            if (!_haveStarted)
            {
                throw new InvalidOperationException("Cannot call Destination() until the move has Start()ed");
            }

            if (_destMover == null)
            {
                _destMover = _mover.Clone();
                Update(_destMover, 1.0);
            }
            return _destMover;
        }

        public string DescriptiveCode
        {
            get { return "Hip"; }
        }
    }
}