#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;
        // calculate maximum velocity and acceleration
        Vec<float, SIZE> maxJointLocalVelocity = maxJointVelocity * (sampleTimeMs / MStoSec);
        Vec<float, SIZE> maxJointLocalAcceleration = maxJointAcceleration * (sampleTimeMs / MStoSec);
        std::cout << "max : " << maxJointLocalVelocity << "\n";

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

        // calculate number of movement steps
        
        // 2 same acceleration and deaceleration phases

        Vec<float,SIZE> minStepsPerJoint(0.0f);
        // check 
        for (int j=0; j<SIZE; j++)
        {
            // only need acceleration and deacceleration phase
            if (maxJointLocalVelocity(j) > jointMovementAbs(j)/2.0f)
            {
                //    s = (a * t^2 )/2
                // => t = sqrt( s * 2 / a)
                minStepsPerJoint(j) = sqrt(jointMovementAbs(j) / maxJointLocalAcceleration (j))*2.0f;
            } else 
            {
                // 2 speedup and slow down phases
                minStepsPerJoint(j) =  maxJointLocalVelocity(j)/maxJointLocalAcceleration(j) * 2.0f ;

                // one phase with constant velocity
                float remainingjointMovementAbs = jointMovementAbs(j);
                // v * t = s
                remainingjointMovementAbs -= maxJointLocalVelocity(j) * minStepsPerJoint(j) * sampleTimeMs;

                // s / v = t 
                minStepsPerJoint(j) += remainingjointMovementAbs/maxJointLocalVelocity(j);
            }
        }

        std::cout << "minsteps : " << minStepsPerJoint << "\n";
        std::cout << "steps : " << minStepsPerJoint.max()  << "\n";
        this->steps = ceil(minStepsPerJoint.max());
        std::cout << "steps : " << this->steps << "\n";
        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;

        // calculate thime of max speed reaching
        Vec<float,SIZE> deltaToMaxSpeed(0.0f);
        Vec<float,SIZE> currJointLocalAcceleration(0.0f);
        for (int j=0; j<SIZE; j++)
        {
            // ceil steps/2 - sqrt(steps^2 - jointmovementabs/maxaccelaration)
            deltaToMaxSpeed(j) = ceil( this->steps /2.0f - sqrt((this->steps/2.0f)*(this->steps/2.0f) - jointMovementAbs(j)/maxJointAcceleration(j)));
            if (deltaToMaxSpeed(j) <= 0.0f)
            {
                currJointLocalAcceleration(j) = 0.0f;
            } else
            {
                currJointLocalAcceleration(j) = -jointMovementAbs(j)/(deltaToMaxSpeed(j)*deltaToMaxSpeed(j)-this->steps*deltaToMaxSpeed(j));
            }
            maxJointLocalVelocity(j) = deltaToMaxSpeed(j)*currJointLocalAcceleration(j);
        }

        Vec<float,SIZE> currentInk(0.0f);
        Vec<float,SIZE> currentInkLast(0.0f);
        Vec<float,SIZE> currentDist(0.0f);
        Vec<float,SIZE> currentDistLast(0.0f);

        for (int currStep=0; currStep<this->steps; currStep++)
        {
            for (int currJoint=0; currJoint<SIZE; currJoint++)
            {
                if (currStep+1 <= this->steps/2.0f)
                {
                    currentInk(currJoint) = std::min(currentInkLast(currJoint)+maxJointLocalAcceleration(currJoint),maxJointLocalVelocity(currJoint));
                }else if (currStep+1 > this->steps-deltaToMaxSpeed(currJoint))
                {
                    currentInk(currJoint) = std::max(currentInkLast(currJoint)-maxJointLocalAcceleration(currJoint),0.0f);
                }else
                {
                    currentInk(currJoint) = currentInkLast(currJoint);
                }
                currentDist(currJoint) = currentDistLast(currJoint) + sgn(jointMovement(currJoint))*currentInk(currJoint);
                this->nodes[currStep].jointPos(currJoint) += sgn(jointMovement(currJoint))*currentInk(currJoint);
                this->nodes[currStep].velocity(currJoint) = currentInk(currJoint);
                this->nodes[currStep].acceleration(currJoint) = currentInkLast(currJoint) - currentInk(currJoint);
                
                currentDistLast(currJoint) = currentDist(currJoint);
                currentInkLast(currJoint) = currentInk(currJoint);
            }
        }

        // last step myth be to wide so cut it to the wanted position
        this->nodes[this->steps-1].jointPos = jointEnd;
        this->nodes[this->steps-1].velocity = maxJointLocalVelocity; // TODO this is not the truth
        this->nodes[this->steps-1].acceleration = Vec<float,SIZE>(0.0f); 
    }
};

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

#endif