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

namespace SalsaModel.Actions
{
    class ShoulderAction : IAction
    {

        SalsaMover _mover;
        double _maxOffset;
        double _initOffset;
        double _initAngle;
        const double _radius = 2;
        double _sidewaysDist;

        public ShoulderAction(SalsaMover mover, PositionModel.Side direction)
        {

            double displacement = mover.Body.ShoulderWidth / 8; 
            double disp;
            _mover = mover;
            if (direction == PositionModel.Side.Left)
            {
                disp = displacement;
                _initAngle = 0;
            }
            else
            {
                disp = -displacement;
                _initAngle = Math.PI;
            }
            _maxOffset = disp;// +mover.Body.ShoulderWidth / 2;
        }

        #region IAction Members

        public void Start()
        {
            PositionModel model = _mover.Model;
            _initOffset = (model.LArm.Shoulder - model.HipMidPoint).DotProduct(model.SidewaysUnitDirection(PositionModel.Side.Left))- _mover.Body.ShoulderWidth/2;
            _sidewaysDist = Math.Sqrt(_mover.Body.ShoulderWidth * _mover.Body.ShoulderWidth / 4.0 - _radius * _radius);
        }


        public void Update(double percent)
        {
            double side_offset = (_maxOffset - _initOffset)*percent + _initOffset;
            //double side_offset = 0;

            double angle = _initAngle - (percent * Math.PI + Math.PI/2);
         
            
            _mover.Model.LArm.Shoulder = _mover.Model.HipMidPoint 
                + _mover.Model.ForwardUnit() * _radius*Math.Sin(angle)
                + (new Location(0, 0, 1)) * (_mover.Body.Trunk + _radius*Math.Cos(angle))
                + _mover.Model.SidewaysUnitDirection(PositionModel.Side.Left) *  (_sidewaysDist + side_offset);

            _mover.Model.RArm.Shoulder = _mover.Model.HipMidPoint
                - _mover.Model.ForwardUnit() * _radius*Math.Sin(angle)
                + (new Location(0, 0, 1)) * (_mover.Body.Trunk - _radius*Math.Cos(angle))
                + _mover.Model.SidewaysUnitDirection(PositionModel.Side.Left) *  (- _sidewaysDist + side_offset);

            _mover.Model.Head = _mover.Model.HipMidPoint 
                + (new Location(0, 0, 1)) * (_mover.Body.Trunk + _mover.Body.Head)
                + _mover.Model.SidewaysUnitDirection(PositionModel.Side.Left) * (side_offset);
            _mover.AlignElbows(SalsaMover.ElbowStyle.Out);
            
        }

        public void End()
        {
        }

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

        #endregion
    }
}