#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
            )
    {
        std::cout << "bang bang \n " ;
        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);

        Vec<float,SIZE> accelerationPhaseTime = maxJointVelocity.celldivide(maxJointAcceleration)/sampleTime;
        Vec<float,SIZE> jointMovementRemaining = jointMovementAbs - maxJointVelocity.cellmultiply(maxJointVelocity).celldivide(maxJointAcceleration);
        Vec<float,SIZE> maxVelocityPhaseThime = jointMovementRemaining.celldivide(maxJointLocalVelocity);
        
        Vec<float,SIZE> minStepsPerJoint = accelerationPhaseTime * 2.0f + maxVelocityPhaseThime;
        std::cout << minStepsPerJoint << "minStepsPerJoint \n";
        std::cout << accelerationPhaseTime << "a time \n";
        std::cout << jointMovementRemaining << "reimain movement\n";
        std::cout << maxVelocityPhaseThime << "max time \n";
        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 
        //Vec<float,SIZE> currJointMovementOfMax = minStepsPerJoint / minStepsPerJoint.max() ;
        // s =  a* t^2 / 2 => a = s* 2 / t^2
        Vec<float,SIZE> currMaxAcceleration = (maxJointLocalAcceleration).cellmultiply(minStepsPerJoint) / minStepsPerJoint.max();
        Vec<float,SIZE> currMaxVelocity     = (maxJointLocalVelocity ).cellmultiply(minStepsPerJoint) / minStepsPerJoint.max();
        std::cout << "max velo curr : " << currMaxVelocity << "\n";

        for( int i = 0 ; i < this->steps; ++i)
        {
            for (int joint = 0 ; joint < SIZE; ++joint)
            {
                if (i < accelerationPhaseTime(i))
                {
                    this->nodes[i].acceleration(i) = currMaxAcceleration(i);
                }else if (i < accelerationPhaseTime(i) + jointMovementRemaining(i))
                {
                    this->nodes[i].acceleration(i) = 0.0f;
                }else 
                {
                    this->nodes[i].acceleration(i) = currMaxAcceleration(i)* -1.0f;
                }
                this->nodes[i].velocity(i) = velocityLast(i) + jointMovementSgn(i) * currMaxAcceleration(i);
            }
            this->nodes[i].jointPos = jointLast + this->nodes[i].velocity; 

            //std::cout << i << this->nodes[i].velocity << this->nodes[i].jointPos <<"\n";
            jointLast = this->nodes[i].jointPos;
            velocityLast = this->nodes[i].velocity;
        }
    }
};

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

#endif