#ifndef _BANGBANGTRAJCETORY_H_
#define _BANGBANGTRAJCETORY_H_
#include "Trajectroy.h"
#include "Vec.h"

/**
 * @addtogroup trajectory
 * @{
 *
 * @addtogroup BangBang
 * @{
 * @author  Philipp Schoenberger <ph.schoenberger@googlemail.com>
 * @version 1.0
 *
 * @section LICENSE
 *
 * This program is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License as
 * published by the Free Software Foundation; either version 2 of
 * the License, or (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
 * General Public License for more details at
 * https://www.gnu.org/copyleft/gpl.html
 *
 * @section DESCRIPTION
 *
 * This file contains the Trajectory for a bang bang trajectory.
 * The bang bang trajectory is a trajectory with linear acceleration
 * phase followed by a direct de-acceleration phase
 *
 * The slowest joint is defining the speed of the other joints.
 * By that all joints start and stop the movement synchronously
 */
template <unsigned SIZE> 

/**
 * Class for a Bang Bang Trajectory based on joint angles.
 * The joint start and stop synchron
 *
 * This class is based on the Trajectory joint Template so it 
 * all possible joint counts 
 *
 * @see Trajectory
 */
class BangBangJointTrajectory : public Trajectory<SIZE>
{
    public:
        /**
         * standard constructor which will setup the class for a 
         * trajectory from start to end position with a synchron movement
         *
         * @param sampleTimeMs         The discrete time interval for each step within the Trajectory with the unit [ milliseconds ]
         * @param maxJointVelocity     The maximum velocity for each joint with the unit [ degree/ seconds ]
         * @param maxJointAcceleration The maximum acceleration for each joint with the unit [ degree/(seconds^2) ]
         * @param jointStart           The starting point for each joint in [ degree ]
         * @param jointEnd             The point which should be reached at the end of the Trajectory [ degree ]
         */
        BangBangJointTrajectory(
                float sampleTimeMs,
                Vec<float,SIZE> maxJointVelocity,
                Vec<float,SIZE> maxJointAcceleration,
                Vec<float,SIZE> jointStart,
                Vec<float,SIZE> jointEnd
                )
        {
            float MStoSec = 1000.0f;
            float sampleTime = sampleTimeMs / MStoSec;
            // calculate maximum velocity and acceleration
            Vec<float, SIZE> maxJointLocalVelocity = maxJointVelocity * sampleTime;
            Vec<float, SIZE> maxJointLocalAcceleration = maxJointAcceleration * sampleTime;

            // calculate delta movement
            Vec<float,SIZE> jointMovement = jointEnd - jointStart;
            Vec<float,SIZE> jointMovementAbs = jointMovement.abs();
            Vec<float,SIZE> jointMovementSgn = jointMovement.sgn();

            // calculate sample count

            // calculate number of movement steps
            // one joint has to reach maxvelocity the others are stepped down to 
            // calculate time if acceleration is enough to reach max speed
            // s = a * t^2 / 2
            Vec<float,SIZE> minBangBangTime = maxJointLocalVelocity.celldivide(maxJointLocalAcceleration) / sampleTime;
            //TODO check if mintime is neceesary
            Vec<float,SIZE> timeMaxAcceleration = ((jointMovementAbs/2.0f).celldivide(maxJointAcceleration) * 2.0f).sqrt() * 2.0f / sampleTime;
            Vec<float,SIZE> timeMaxVelocity = ((jointMovementAbs/2.0f).celldivide(maxJointVelocity) * 2.0f)*2.0f / sampleTime;
            Vec<float,SIZE> time = timeMaxAcceleration.cellmax(timeMaxVelocity);

            Vec<float,SIZE> minStepsPerJoint = time;//jointMovementAbs.celldivide(maxJointLocalVelocity)*2.0f;
            minStepsPerJoint = minStepsPerJoint.ceil();

            this->steps = minStepsPerJoint.max();
            if (this->steps == 0)
                this->steps +=1;

            this->nodes = (struct Trajectory<SIZE>::trajectoryNode* ) calloc(sizeof(struct Trajectory<SIZE>::trajectoryNode),this->steps);

            Vec<float,SIZE> jointLast  = jointStart;
            Vec<float,SIZE> velocityLast(0.0f);

            // percentage of max velocity 
            // s =  a* t^2 / 2 
            // <=> 
            // a = s* 2 / t^2
            Vec<float,SIZE> currMaxAcceleration = (jointMovementAbs * 2.0f ).celldivide(this->steps).celldivide(this->steps)*2.0f;
            std::cout << "currMaxAcceleration : " << currMaxAcceleration << "\n";

            float count = 0.0f;
            for( int i = 0 ; i < this->steps; ++i)
            {
                // acceleration phase
                if (i <= this->steps /2 )
                {
                    this->nodes[i].acceleration = currMaxAcceleration ;
                    count +=1.0f;
                }
                //  de-acceleration phase
                else
                {
                    this->nodes[i].acceleration = currMaxAcceleration * -1.0f;
                    count -=1.0f;
                }
                this->nodes[i].velocity = jointMovementSgn.cellmultiply(currMaxAcceleration) * count;
                this->nodes[i].jointPos = jointLast + this->nodes[i].velocity; 


                // check if we already reached end 
                // This can happen if the steps calculation was
                // to far off caused by floating point precision
                 
                float maxdiffLast = (jointEnd - jointLast).abs().max();
                float maxdiffCurr = (jointEnd - this->nodes[i].jointPos).abs().max();
                if ( maxdiffLast < maxdiffCurr)
                {
                    this->steps = i+1;

                    this->nodes[i].jointPos = jointEnd;
                    break;
                }
                
                // save last joints
                jointLast = this->nodes[i].jointPos;
                velocityLast = this->nodes[i].velocity;
            }
        }
};
template <unsigned SIZE> 
class BangBangCartTrajectory: public Trajectory<SIZE>
{
};

/**
 * @}
 * @}
 */

#endif