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

namespace SalsaModel.Actions
{
    class LeadWithHand : IAction
    {
        SalsaMover _follower;
        SalsaMover _leader;
        CubanMotionAction _toFollow;
        Location _handDest;
        PositionModel.Side _followerHoldSide;
        PositionModel.Side _followerStepSide;

        public LeadWithHand(SalsaMover follower, SalsaMover leader, CubanMotionAction toFollow, PositionModel.Side followerStepSide, PositionModel.Side followerHoldSide, Location handPos)
        {
            _follower = follower;
            _leader = leader;
            _toFollow = toFollow;
            _handDest = handPos;
            _followerHoldSide = followerHoldSide;
            _followerStepSide = followerStepSide;
            // the programmer wants to specify the hip-hip distance, but internally we're working
            // with the feet.  The hips are in the middle of each foot, so correct for that first.
        //    _distance = distance + leader.Body.Foot / 2.0 + follower.Body.Foot / 2.0;
        }

        TimeLine _timeLine = new TimeLine();
        double _prev = -1;

        public void Start()
        {
            SalsaMover leaderDest = _toFollow.Destination();
            Location leaderHandDest = leaderDest.Model.RelToAbsPos(_handDest);

            // the follower's final location lines her up along her wall, to the best position
            // to hold the leader's hand.
            double followerDesiredRadius = _follower.Body.LowerArm * 1.5;

            double verticalDistance = leaderHandDest.Z - _follower.Model.GetArm(_followerHoldSide).Shoulder.Z;

            double distHandToShoulderAtNearsetPointOnWall = leaderHandDest.Minus(_follower.Model.GetArm(_followerHoldSide).Shoulder).DotProduct(_follower.Model.WallForwardsUnit.RotateZ(Math.PI * 0.5));
            double distAlongWallFromFinalFollowerShoulderToNearestPoint
                = Math.Sqrt(followerDesiredRadius * followerDesiredRadius
                    - distHandToShoulderAtNearsetPointOnWall * distHandToShoulderAtNearsetPointOnWall
                    // correction so that she steps closer when the hand is raised.  
                    // XXX This assumes no bending in the elbow.
                    - verticalDistance * verticalDistance
                    );

            double distAlongWallFromCurrentFollowerShoulderToNearestPoint
                = leaderHandDest.Minus(_follower.Model.GetArm(_followerHoldSide).Shoulder).DotProduct(_follower.Model.WallForwardsUnit);

            double followerMoveTorso = distAlongWallFromFinalFollowerShoulderToNearestPoint - distAlongWallFromCurrentFollowerShoulderToNearestPoint;

            double distToStep = -followerMoveTorso;
            CubanMotionAction fwStep = CompositeActions.StepForward(_timeLine, 0.0, 1.0, _follower, _followerStepSide, distToStep);

            // this will happen soon anyway.  Not sure if there're problems doing it twice.  It allows us
            // to call Destination on it; since we're in Start() here anyway, the information should be 
            // correct.
            fwStep.Start();

            Location followerHandRel = fwStep.Destination().Model.AbsToRelPos(leaderHandDest);
            _timeLine.Add(0.0, 1.0, new MoveHand(_followerHoldSide, _follower, followerHandRel));
        }

        public void Update(double now)
        {
            _timeLine.RunActions(_prev, now);
            _prev = now;
        }

        public void End()
        {
        }

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

    }
}