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

namespace SalsaModel.Actions
{
    class FollowAtFixedDistance : IAction
    {
        SalsaMover _follower;
        SalsaMover _leader;
        double _distance;
        MoveFootAction _toFollow;

        public FollowAtFixedDistance(SalsaMover follower, SalsaMover leader, MoveFootAction toFollow, double distance)
        {
            _follower = follower;
            _leader = leader;
            _toFollow = toFollow;
            // 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()
        {
            Location leaderEndStep = _leader.GetStepLocation(_toFollow.Side, _toFollow.Step);
            Location followerStep = leaderEndStep.Plus(_follower.Model.WallForwardsUnit.Scale(-1.0 * _distance));

            double distToStep = followerStep.Minus(_follower.Model.GetLeg(_toFollow.Side).Foot).DotProduct(_follower.Model.WallForwardsUnit);
            CompositeActions.StepForward(_timeLine, 0.0, 1.0, _follower, PositionModel.Other(_toFollow.Side), 
                distToStep);
        }

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

        public void End()
        {
        }

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