#ifndef _TRAJECTORY_H_
#define _TRAJECTORY_H_
#include "vec.h"
#include <stdlib.h>

// all types of trajectories
enum TrajectoryType {
    TrajectoryDefault     = 0,
    TrajectoryJointLinear = 0,
    TrajectoryCartLinear,

    TrajectoryJointBangBang,
    TrajectoryCartBangBang,
    
    TrajectoryJointFivePoly,
    TrajectoryCartFivePoly,
    
    TrajectoryJointLSPB,
    TrajectoryCartLSPB
};
enum MovementType
{
    MovementJointBased= 0,
    MovementCartBased
};

template <unsigned SIZE>
class Trajectory
{
    public:
        Trajectory()
        {
            movementType = MovementJointBased;

            steps = 0;
            currentStep = 0;
            totalTime = 0;
            nodes = NULL;
        }
        Trajectory(
            float sampleTimeMs,
            Vec<float,SIZE> maxJointVelocity,
            Vec<float,SIZE> maxJointAcceleration,
            Vec<float,SIZE> jointStart,
            Vec<float,SIZE> jointEnd
            )
        {
            movementType = MovementJointBased;

            steps = 1;
            currentStep = 0;

            totalTime = steps * sampleTimeMs;

            nodes = (struct trajectoryNode* ) malloc(sizeof(struct trajectoryNode)*steps);

            nodes[0].jointPos = jointEnd;
            nodes[0].velocity = (jointEnd-jointStart)/sampleTimeMs;
            nodes[0].acceleration = Vec<float,SIZE>(9999.0f);

            // unused
            (void)jointStart;
            (void)maxJointVelocity;
            (void)maxJointAcceleration;
        }

        ~Trajectory()
        {
            free(nodes);
        }

        unsigned int getSteps()
        {
            return steps;
        }

        unsigned int getRemainingSteps()
        {
            if (steps >= currentStep )
                return steps - currentStep;
            else 
                return 0;
        }

        unsigned int getCurrentStep()
        {
            return currentStep;
        }

        enum MovementType getMovementType()
        {
            return movementType;
        }

        Vec<float,SIZE> getNextJointPos()
        {
            Vec<float,SIZE> retval(0.0f);
            unsigned int pos = currentStep;
            if (pos >= steps)
            {
                pos = steps;
            }

            if (nodes != NULL && movementType == MovementJointBased)
            {
                retval = nodes[pos].jointPos;
            }

            // increment step
            currentStep ++;

            return retval;
        }
        Mat<float,4>    getNextCartPos     ()
        {
            Mat<float,4> retval(0.0f,1.0f);
            unsigned int pos = currentStep;
            if (pos >= steps)
            {
                pos = steps;
            }

            if (nodes != NULL && movementType == MovementCartBased)
            {
                retval = nodes[pos].cartPos;
            }

            // increment step
            currentStep ++;

            return retval;
        }

        Vec<float,SIZE> getJointPos         (unsigned int step)
        {
            Vec<float,SIZE> retval(0.0f);
            if (step >= steps)
            {
                return retval;
            }

            if (nodes != NULL && movementType == MovementJointBased)
            {
                retval = nodes[step].jointPos;
            }

            return retval;
        }

        Mat<float,4>    getCartPos          (unsigned int step)
        {
            Vec<float,SIZE> retval(0.0f);
            if (step >= steps)
            {
                return retval;
            }

            if (nodes != NULL && movementType == MovementJointBased)
            {
                retval = nodes[step].cartPos;
            }

            return retval;
        }
        Vec<float,SIZE> getJointVelocity    (unsigned int step)
        {
            Vec<float,SIZE> retval(0.0f);
            if (step >= steps)
            {
                return retval;
            }

            if (nodes != NULL && movementType == MovementCartBased)
            {
                retval = nodes[step].velocity;
            }

            return retval;
        }
        Vec<float,SIZE> getJointAcceleration(unsigned int step)
        {
            Vec<float,SIZE> retval(0.0f);
            if (step >= steps)
            {
                return retval;
            }

            if (nodes != NULL && movementType == MovementCartBased)
            {
                retval = nodes[step].acceleration;
            }

            return retval;
        }
        
        struct trajectoryNode
        {
            Vec<float,SIZE> jointPos;
            Mat<float,4>    cartPos;
            Vec<float,SIZE> velocity;
            Vec<float,SIZE> acceleration;
        };

    protected:
        static const unsigned int defaultSteps = 100;

        unsigned int steps;
        unsigned int currentStep;
        float totalTime;
        struct trajectoryNode* nodes;
        enum MovementType movementType;
};

#endif