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


template <unsigned SIZE> class Trajectory;

// all types of trajectories
/// START OF TRAJECTORY LIST
#include "LinearTrajectory.h"

enum TrajectoryType {
    TrajectoryDefault     = 0,
    TrajectoryJointLinear = 0,
    TrajectoryCartLinear,

    TrajectoryJointBangBang,
    TrajectoryCartBangBang,
    
    TrajectoryJointFivePoly,
    TrajectoryCartFivePoly,
    
    TrajectoryJointLSPB,
    TrajectoryCartLSPB
};
static struct {
    enum TrajectoryType type;
    std::string str;
} TrajectoryTypeStr[] =
{
    {  .type = TrajectoryDefault     , .str="default" },

    {  .type = TrajectoryJointLinear  , .str="JointLinear" },
    {  .type = TrajectoryJointLSPB    , .str="JointLSPB" },
    {  .type = TrajectoryJointBangBang, .str="JointBangBang" },
    {  .type = TrajectoryJointFivePoly, .str="JointFivePoly" },

    {  .type = TrajectoryCartLinear  , .str="CartLinear" },
    {  .type = TrajectoryCartLSPB    , .str="CartLSPB" },
    {  .type = TrajectoryCartBangBang, .str="CartBangBang" },
    {  .type = TrajectoryCartFivePoly, .str="CartFivePoly" },
};
/// END OF TRAJECTORY LIST



static std::string toString(enum TrajectoryType type)
{
    int items = sizeof(TrajectoryTypeStr) / sizeof(TrajectoryTypeStr[0]);
    for (int i = 0 ; i < items ; ++i)
    {
        if (TrajectoryTypeStr[i].type == type)
            return ""+ TrajectoryTypeStr[i].str;
    }
    return "default";
}
static enum TrajectoryType toEnum(std::string name)
{
    int items = sizeof(TrajectoryTypeStr) / sizeof(TrajectoryTypeStr[0]);
    std::cout <<"items "<< items <<"\n";
    for (int i = 0 ; i < items ; ++i)
    {
        if (TrajectoryTypeStr[i].str == name)
            return TrajectoryTypeStr[i].type;
    }
    return TrajectoryDefault;
}
template <unsigned SIZE>
class Trajectory<SIZE>* calculateTrajectory(enum TrajectoryType type,
            float sampleTimeMs,
            Vec<float,SIZE> maxJointVelocity,
            Vec<float,SIZE> maxJointAcceleration,
            Vec<float,SIZE> jointStart,
            Vec<float,SIZE> jointEnd)

{
    class Trajectory<SIZE>* retval = NULL;
    switch(type)
    {
        case TrajectoryJointLinear:
            retval = (class Trajectory<SIZE>*) new LinearJointTrajectory<SIZE>(
                    sampleTimeMs,
                    maxJointVelocity,
                    maxJointAcceleration,
                    jointStart,
                    jointEnd); 
            break;
        case TrajectoryCartLinear:
        case TrajectoryJointBangBang:
            retval = (class Trajectory<SIZE>*) new LinearJointTrajectory<SIZE>(
                    sampleTimeMs,
                    maxJointVelocity,
                    maxJointAcceleration,
                    jointStart,
                    jointEnd); 
            break;
        case TrajectoryCartBangBang:
        case TrajectoryJointFivePoly:
        case TrajectoryCartFivePoly:
        case TrajectoryJointLSPB:
        case TrajectoryCartLSPB:
            break;
    }
    return retval;
}
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;

            //this->nodes = new struct trajectoryNode [steps];
            this->nodes = (struct Trajectory<SIZE>::trajectoryNode* ) calloc(sizeof(struct Trajectory<SIZE>::trajectoryNode),this->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;
        }

        virtual ~Trajectory()
        {
            std::cout << " test1 \n ";
            std::cout << nodes[steps-1].jointPos << "\n";
            free (nodes);
            std::cout << " test3 \n "<<std::endl;
        }

        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 (steps == 0)
                return retval;
            if (pos >= steps)
            {
                pos = steps-1;
            }

            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 (steps == 0)
                return retval;
            if (pos >= steps)
            {
                pos = steps-1;
            }

            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 (steps == 0)
                return retval;
            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 (steps == 0)
                return retval;
            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 (steps == 0)
                return retval;
            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 (steps == 0)
                return retval;
            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