#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
                )
        {
            // calculate maximum velocity and acceleration
            Vec<float, SIZE> maxJointLocalVelocity = maxJointVelocity * sampleTimeMs;
            Vec<float, SIZE> maxJointLocalAcceleration = maxJointAcceleration * sampleTimeMs;

            // 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);
            this->steps = ceil(minStepsPerJoint.max());

            this->nodes = (struct Trajectory<SIZE>::trajectoryNode* ) malloc(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[0].jointPos = jointLast;
                this->nodes[0].velocity = maxJointLocalVelocity; 
                this->nodes[0].acceleration;
            }
        }
        ~LinearJointTrajectory()
        {
            free (this->nodes);
        }
    private:

    protected:
};

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

};
#endif