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

template <unsigned SIZE>
class LSPBJointTrajectory: public Trajectory<SIZE>
{
public:
    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;
        Vec<float,SIZE> timeBangBang = timeMaxAcceleration.cellmax(timeMaxVelocity);

        std::cout << timeBangBang << "timeBangBang\n";
        float steps_bangbang = timeBangBang.ceil().max();

        // calculate maxAcceleration for bangbang
        Vec<float,SIZE> currMaxAcceleration = (jointMovementAbs * 2.0f ).celldivide(steps_bangbang).celldivide(steps_bangbang)*2.0f;
        std::cout << "currMaxAcceleration : " << currMaxAcceleration << "\n";

        float percentLSPB = 0.7f;
        Vec<float,SIZE> timeLSPB = timeBangBang * (1 + 0.25f * (1.0f - percentLSPB));
        float timeBlend = ((timeBangBang /2.0f) * percentLSPB).ceil().max();
        std::cout << timeBlend << "timeBlend \n";

        std::cout << timeLSPB << "max time \n";
        Vec<float,SIZE> minStepsPerJoint = timeLSPB.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);


        int count = 0;
        for( int i = 0 ; i < this->steps; ++i)
        {
            if (i < timeBlend)
            {
                count += 1;
                this->nodes[i].acceleration = currMaxAcceleration;
            }else if (i < this->steps - timeBlend )
            {
                this->nodes[i].acceleration = Vec<float,SIZE>(0.0f);
            }else 
            {
                count -= 1;
                this->nodes[i].acceleration = currMaxAcceleration* -1.0f;
            }
            this->nodes[i].velocity = jointMovementSgn.cellmultiply(currMaxAcceleration) * count;
            this->nodes[i].jointPos = jointLast + this->nodes[i].velocity; 

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

template <unsigned SIZE>
class LSPBCartTrajectory: public Trajectory<SIZE>
{
};

#endif