
/****************************************************************************/
/*Copyright (c) 2011, Florent DEVILLE.                                      */
/*All rights reserved.                                                      */
/*                                                                          */
/*Redistribution and use in source and binary forms, with or without        */
/*modification, are permitted provided that the following conditions        */
/*are met:                                                                  */
/*                                                                          */
/* - Redistributions of source code must retain the above copyright         */
/*notice, this list of conditions and the following disclaimer.             */
/* - Redistributions in binary form must reproduce the above                */
/*copyright notice, this list of conditions and the following               */
/*disclaimer in the documentation and/or other materials provided           */
/*with the distribution.                                                    */
/* - The names of its contributors cannot be used to endorse or promote     */
/*products derived from this software without specific prior written        */
/*permission.                                                               */
/* - The source code cannot be used for commercial purposes without         */
/*its contributors' permission.                                             */
/*                                                                          */
/*THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS       */
/*"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT         */
/*LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS         */
/*FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE            */
/*COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,       */
/*INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,      */
/*BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;          */
/*LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER          */
/*CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT        */
/*LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN         */
/*ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE           */
/*POSSIBILITY OF SUCH DAMAGE.                                               */
/****************************************************************************/

using Microsoft.Xna.Framework;
using System.Collections.Generic;
using GE.TimeClock;
using GE.Tools;
using System;

namespace GE.SceneGraph
{
    class ModCatmullRom : Modifier
    {
        #region Variable

        /// <summary>
        /// Flag indicate if the modifier calculate the orientation
        /// </summary>
        bool _bUseSecondDerivate;

        /// <summary>
        /// Flag indicates if the modifier makes a loop
        /// </summary>
        bool _bLoop;

        /// <summary>
        /// Flag set when the modifier gotta do a back and forth
        /// </summary>
        bool _bBackAndForth;

        /// <summary>
        /// List of waypoint
        /// </summary>
        public List<TimePosPair> _waypointList;

        /// <summary>
        /// Ratio between 0 and 1 to compute the interpollation
        /// </summary>
        private float _fAmount;

        /// <summary>
        /// Start time of an interpollation step
        /// </summary>
        private int _timeStartStep;

        /// <summary>
        /// The four indices of the waypoints used
        /// </summary>
        int p1, p2, p3, p4;

        /// <summary>
        /// Time between two waypoints
        /// </summary>
        int _timeBetweenWaypoint;

        /// <summary>
        /// Flag set when the interpollation is doing the back trip
        /// </summary>
        bool _bBackward;

        /// <summary>
        /// Flag set when the interpollation is doing the forward trip
        /// </summary>
        bool _bForward;

        #endregion

        #region Constructor

        /// <summary>
        /// Constructor
        /// </summary>
        public ModCatmullRom()
        {
            _waypointList = new List<TimePosPair>();
            _fAmount = 0;
            _timeStartStep = -1;
            p1 = 0;
            p2 = 0;
            p3 = 1;
            p4 = 2;

            _bLoop = false;
            _bBackAndForth = false;
            _bBackward = false;
            _bForward = true;
        }

        public void reset()
        {
            _fAmount = 0;
            _timeStartStep = -1;
            p1 = 0;
            p2 = 0;
            p3 = 1;
            p4 = 2;
            _isOver = false;
        }

        #endregion

        public override void update()
        {
            if (_bLoop)
            {
                updateLoop();
                return;
            }
            else if (_bBackAndForth)
            {
                updateBackAndForth();
                return;
            }

            updateSingle();
        }

        #region Tools

        /// <summary>
        /// Update the interpollation for a loop
        /// </summary>
        private void updateLoop()
        {
            //if we go through the update for the first time
            if (_timeStartStep == -1)
            {
                _timeStartStep = Clock.instance.millisecs;
                _timeBetweenWaypoint = _waypointList[p3].m_time;// -m_waypointList[p2].m_time;
                p1 = _waypointList.Count - 1;
            }

            //if the next waypoint is reached
            if (_fAmount >= 1)
            {
                //reset the amount
                _fAmount = 0;

                //set the new index
                increaseIndexLoop();

                //reset the clock
                _timeStartStep = Clock.instance.millisecs;

                //calculate the new duration
                _timeBetweenWaypoint = _waypointList[p3].m_time;

                //set the position
                _v2Position = _waypointList[p2].m_position;

                //calculate the orientation
                if (_bUseSecondDerivate)
                    calculateOrientation(_waypointList[p1].m_position, _waypointList[p2].m_position,
                        _waypointList[p3].m_position, _waypointList[p4].m_position, _fAmount);

                return;
            }

            computeInterpollation();
            
        }

        /// <summary>
        /// Update the interpollation for a back and forth
        /// </summary>
        private void updateBackAndForth()
        {
            //if we go through the update for the first time
            if (_timeStartStep == -1)
            {
                _timeStartStep = Clock.instance.millisecs;
                _timeBetweenWaypoint = _waypointList[p3].m_time;// -m_waypointList[p2].m_time; 
                p1 = 0;
                p2 = p1;
                p3 = 1;
                if (p4 > _waypointList.Count - 1)
                    p4 = _waypointList.Count - 1;
            }

            //if the last waypoint is reached
            if (_bForward)
            {
                if (_fAmount >= 1 && p3 == _waypointList.Count - 1)
                {
                    _bForward = false;
                    _bBackward = true;

                    p1 = _waypointList.Count - 1;
                    p2 = p1;
                    p3 = p2 - 1;
                    p4 = p3 - 1;
                    if (p4 < 0) p4 = 0;

                    //reset the clock
                    _timeStartStep = Clock.instance.millisecs;

                    //calculate the new duration
                    _timeBetweenWaypoint = _waypointList[p3].m_time;

                    //set the position
                    _v2Position = _waypointList[p2].m_position;

                    _fAmount = 0;
                }
            }
            else //bBackward
            {
                if (_fAmount >= 1 && p3 == 0)
                {
                    _bForward = true;
                    _bBackward = false;

                    p1 = 0;
                    p2 = p1;
                    p3 = 1;
                    p4 = p3 + 1;
                    if(p4 >_waypointList.Count - 1)
                        p4 = _waypointList.Count - 1;

                    //reset the clock
                    _timeStartStep = Clock.instance.millisecs;

                    //calculate the new duration
                    _timeBetweenWaypoint = _waypointList[p3].m_time;

                    //set the position
                    _v2Position = _waypointList[p2].m_position;

                    _fAmount = 0;
                }
            }

            //if the next waypoint is reached
            if (_fAmount >= 1)
            {
                //reset the amount
                _fAmount = 0;

                if (_bBackward)
                    increaseIndexBackward();
                else
                    increaseIndexForward();

                //reset the clock
                _timeStartStep = Clock.instance.millisecs;

                //calculate the new duration
                _timeBetweenWaypoint = _waypointList[p3].m_time;

                //set the position
                _v2Position = _waypointList[p2].m_position;

                //calculate the orientation
                if (_bUseSecondDerivate)
                    calculateOrientation(_waypointList[p1].m_position, _waypointList[p2].m_position,
                        _waypointList[p3].m_position, _waypointList[p4].m_position, _fAmount);

                return;
            }

            computeInterpollation();
        }

        /// <summary>
        /// Update the interpollation for a single trip
        /// </summary>
        private void updateSingle()
        {
            //if the modifier is over
            if (_isOver)
                return;

            //if we go through the update for the first time
            if (_timeStartStep == -1)
            {
                _timeStartStep = Clock.instance.millisecs;
                _timeBetweenWaypoint = _waypointList[p3].m_time;// -m_waypointList[p2].m_time;

                //if (_bLoop)
                //    p1 = _waypointList.Count - 1;
            }

            //if the last waypoint is reached
            if (_fAmount >= 1 && p3 == _waypointList.Count - 1 && !_bLoop)
            {
                _isOver = true;
                return;
            }

            //if the next waypoint is reached
            if (_fAmount >= 1)
            {
                //reset the amount
                _fAmount = 0;

                //set the new index
                increaseIndex();

                //reset the clock
                _timeStartStep = Clock.instance.millisecs;

                //calculate the new duration
                _timeBetweenWaypoint = _waypointList[p3].m_time;

                //set the position
                _v2Position = _waypointList[p2].m_position;

                //calculate the orientation
                if (_bUseSecondDerivate)
                    calculateOrientation(_waypointList[p1].m_position, _waypointList[p2].m_position,
                        _waypointList[p3].m_position, _waypointList[p4].m_position, _fAmount);

                return;
            }

            computeInterpollation();
        }

        void derivativeCatmullRom1D(float p1, float p2, float p3, float p4, float amount, ref float result)
        {
            float firstPart = p3 - p1;

            float secondPart = 2 * amount * (2 * p1 - 5 * p2 + 4 * p3 - p4);

            float thirdPart = 3 * amount * amount * (p4 - p1 + 3 * p2 - 3 * p3);

            result = (firstPart + secondPart + thirdPart) / 2;
        }

        void derivativeCatmullRom(Vector2 p1, Vector2 p2, Vector2 p3, Vector2 p4, float amount, ref Vector2 result)
        {
            derivativeCatmullRom1D(p1.X, p2.X, p3.X, p4.X, amount, ref result.X);
            derivativeCatmullRom1D(p1.Y, p2.Y, p3.Y, p4.Y, amount, ref result.Y);
        }

        void calculateOrientation(Vector2 p1, Vector2 p2, Vector2 p3, Vector2 p4, float amount)
        {
            Vector2 direction = new Vector2();
            derivativeCatmullRom(p1, p2, p3, p4, amount, ref direction);
            direction.Normalize();

            //get the angle between the direction of the entity and it's classic orientation
            float dot = Vector2.Dot(direction, Vector2.UnitX);
            float angle = (float)Math.Acos(dot);

            //get the angle between the direction and the unit Y
            float dotAlpha = Vector2.Dot(direction, Vector2.UnitY);
            float angleAlpha = (float)Math.Acos(dotAlpha);

            //assign the angle
            _fOrientation = angle;

            //check if we took the right angle
            if (angleAlpha > MathHelper.PiOver2)
                _fOrientation = MathHelper.TwoPi - angle;

        }

        void increaseIndex()
        {
            p1 = p2;
            p2 = p3;
            p3 = p4;
            p4 = p4 + 1;
            if (_bLoop)
            {
                p4 = p4 % _waypointList.Count;
            }
            else
            {
                if (p4 >= _waypointList.Count)
                    p4 = _waypointList.Count - 1;
            }
        }

        /// <summary>
        /// Increase the indices for a loop interpollation
        /// </summary>
        void increaseIndexLoop()
        {
            p1 = p2;
            p2 = p3;
            p3 = p4;
            p4 = p4 + 1;
            p4 = p4 % _waypointList.Count;
        }

        /// <summary>
        /// Increase the indices for a forward trip
        /// </summary>
        void increaseIndexForward()
        {
            p1 = p2;
            p2 = p3;
            p3 = p4;
            p4 = p4 + 1;
            if (p4 >= _waypointList.Count)
                p4 = _waypointList.Count - 1;
        }

        /// <summary>
        /// Increase the indices for a backward trip
        /// </summary>
        void increaseIndexBackward()
        {
            p1 = p2;
            p2 = p3;
            p3 = p4;
            p4 = p4 - 1;
            if (p4 <0)
                p4 = 0;
        }

        /// <summary>
        /// Compute the interpollation
        /// </summary>
        private void computeInterpollation()
        {
            //calculate the amount
            int timeElapsed = Clock.instance.millisecs - _timeStartStep;
            _fAmount = (float)timeElapsed / (float)_timeBetweenWaypoint;

            //calculate the position
            Vector2 newPosition = new Vector2();
            Vector2.CatmullRom(ref _waypointList[p1].m_position, ref _waypointList[p2].m_position,
                        ref _waypointList[p3].m_position, ref _waypointList[p4].m_position, _fAmount,
                        out newPosition);
            _v2Position = newPosition;

            //calculate the orientation
            if (_bUseSecondDerivate)
                calculateOrientation(_waypointList[p1].m_position, _waypointList[p2].m_position,
                    _waypointList[p3].m_position, _waypointList[p4].m_position, _fAmount);
        }

        #endregion

        #region Properties

        public bool BackAndForth
        {
            get { return _bBackAndForth; }
            set { _bBackAndForth = value; }
        }

        public bool UseSecondDerivative
        {
            get { return _bUseSecondDerivate; }
            set { _bUseSecondDerivate = value; }
        }

        public bool Loop
        {
            get { return _bLoop; }
            set { _bLoop = value; }
        }

        public bool isPathOver
        {
            get
            {
                if (_bLoop)
                    return false;

                return _isOver;
            }
        }

        public List<TimePosPair> Path
        {
            set { _waypointList = value; }
        }


        #endregion
    }
}