#ifndef _LSPBTRAJCETORY_H_
#define _LSPBTRAJCETORY_H_
#include <math.h>
#include "Trajectroy.h"
#include "sgn.h"

/**
 * @addtogroup trajectory
 * @{
 * @addtogroup LSPB
 * @{
 * @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 LSPB trajectory.
 * The LSPB trajectory is a trajectory with linear acceleration
 * phase followed by a constant speed and a de-acceleration phase
 * The speed curve looks with this like a symmetric Trapez.
 *
 * The slowest joint is defining the speed of the other joints.
 * By that all joints start and stop the movement synchronously
 */

/**
 * 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
 */
template <unsigned SIZE>
class LSPBJointTrajectory: public Trajectory<SIZE>
{
public:
    /**
     * standard constructor which will setup the class for a 
     * trajectory from start to end position with a synchron movement based on joints
     *
     * The trajectory will be of type LSPB (Linear Segment with Parabolic Blend)
     *
     * @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 ]
     */
    LSPBJointTrajectory(
            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;
        std::cout << maxJointLocalVelocity << ", " << maxJointLocalAcceleration << "\n";

        // 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 enouth to reach max speed
        Vec<float,SIZE> maxVeloReachable = (jointMovementAbs.celldivide(maxJointAcceleration)*2.0f).sqrt().cellmultiply(maxJointVelocity);
        maxJointLocalVelocity = (maxVeloReachable/sampleTime).cellmax(maxJointLocalVelocity);

        //      v ^
        //        |
        //  v_bang|      /\  <- bang bang trajectory
        //        |     /  \
        //  v_lspb|    _____\___  <- lspb trajectory
        //        |   /     .  .
        //        |  /      .  . \
        //        | /       .  .  \
        //        |/________.__.___\____> t
        //        | t1|    t2  | t3|
        //                  |tx|
        //
        //  t1/t3 = acceleration phase
        //  t2    = constant speed phase
        //
        //  t = t1 + t2 +t3
        //
        //  v_lspb = percent * v_bang
        //
        // t = t_bang + t_x
        // t = t_bang + (v_bang - v_lspb)/a * 2
        //
        // t = t_bang + (t_bang/2 * a - percent *t_bang/2*a) / a *2
        // t = t_bang + (1- percent) * (t_bang/2 * a) / a *2 
        // t = t_bang + (1- percent) * t_bang
        // t = (2-percent) * t_bang
        
        // calculate time for bang bang
        Vec<float,SIZE> accelerationPhaseTime = maxJointVelocity.celldivide(maxJointAcceleration)/sampleTime;
        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;

        // calculate the time and steps of the bangbang Trajectory
        Vec<float,SIZE> timeBangBang = timeMaxAcceleration.cellmax(timeMaxVelocity);
        float steps_bangbang = timeBangBang.ceil().max();

        // calculate maxAcceleration for a bangbang trajectory
        Vec<float,SIZE> currMaxAcceleration = (jointMovementAbs * 2.0f ).celldivide(steps_bangbang).celldivide(steps_bangbang)*2.0f;

        // use only a percentage of the bangbang max velocity
        float percentLSPB = 0.7f;
        Vec<float,SIZE> timeLSPB = timeBangBang * (1 + 0.25f * (1.0f - percentLSPB));
        float timeBlend = ((timeBangBang /2.0f) * percentLSPB).ceil().max();
        
        // round up and use the maximum of all possible steps 
        Vec<float,SIZE> minStepsPerJoint = timeLSPB.ceil();
        this->steps = minStepsPerJoint.max();
        if (this->steps == 0)
            this->steps +=1;

        // allocate an trajectory with this size
        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);


        // initialize the accelerated counts to zero
        int count = 0;

        for( int i = 0 ; i < this->steps; ++i)
        {
            if (i < timeBlend)
            {
                // acceleration phase
                count += 1;
                this->nodes[i].acceleration = currMaxAcceleration;
            }else if (i < this->steps - timeBlend )
            {
                // constant speed phase
                this->nodes[i].acceleration = Vec<float,SIZE>(0.0f);
            }else 
            {
                // deacceleration phase
                count -= 1;
                this->nodes[i].acceleration = currMaxAcceleration* -1.0f;
            }

            // calculate velocity and joint position accordingly to the acceleration
            // @warning to not add floats over and over again or the error will be huge
            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)
            {
                // mark the current index as the last one within this trajectory
                this->steps = i+1;
                // set the end position
                this->nodes[i].jointPos = jointEnd;
                break;
            }

            jointLast = this->nodes[i].jointPos;
            velocityLast = this->nodes[i].velocity;
        }
    }
};

/**
 * Class for a LSPB Trajectory based on homogenous
 * Cartesian Positions.
 * The joint start and stop synchron
 *
 * This class is based on the Trajectory joint Template so it 
 * all possible joint counts 
 *
 * @TODO implement the Cartesian movement constructor for LSPB trajectories
 * @see Trajectory
 */
template <unsigned SIZE>
class LSPBCartTrajectory: public Trajectory<SIZE>
{
};

/**
 * @}
 * @}
 */
#endif