#ifndef _LINEAR_TRAJECTORY_H_
#define _LINEAR_TRAJECTORY_H_

#include "vec.h"
#include "Trajectroy.h"
#include "stdlib.h"

template <unsigned SIZE>
class LinearJointTrajectory : public Trajectory<SIZE>
{
    public:

        LinearJointTrajectory(
                float sampleTimeMs,
                Vec<float,SIZE> maxJointVelocity,
                Vec<float,SIZE> maxJointAcceleration,
                Vec<float,SIZE> jointStart,
                Vec<float,SIZE> jointEnd
                )
        {
            std::cout << "linear \n " ;
            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 sample count

            // calculate number of movement steps
            Vec<float,SIZE> minStepsPerJoint = jointMovementAbs.celldivide(maxJointLocalVelocity);
            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;
            for( int i = 0 ; i < this->steps; ++i)
            {
                jointLast = jointLast + jointMovement.celldivide((float) this->steps );
                this->nodes[i].jointPos = jointLast;
                this->nodes[i].velocity = maxJointLocalVelocity; 
                this->nodes[i].acceleration = Vec<float,SIZE>(0.0f);
            }
            // 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); 
        }
    private:

    protected:
};

template <unsigned SIZE>
class LinearCartTrajectory : public Trajectory<SIZE>
{

};
#endif