#ifndef _TRAJECTORY_H_
#define _TRAJECTORY_H_
#include <stdlib.h>
#include <iostream>
#include <fstream>

#include <iomanip>
#include <cmath>
#include <limits>

#include "Vec.h"
#include "Mat.h"
template <unsigned SIZE> class Trajectory;

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

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

    TrajectoryJointBangBang,
    TrajectoryCartBangBang,
    
    TrajectoryJointFivePoly,
    TrajectoryCartFivePoly,
    
    TrajectoryJointLSPB,
    TrajectoryCartLSPB
};
static struct {
    enum TrajectoryType type;
    std::string str;
} TrajectoryTypeStr[] =
{
    {  .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]);
    for (int i = 0 ; i < items ; ++i)
    {
        if (TrajectoryTypeStr[i].str == name)
            return TrajectoryTypeStr[i].type;
    }
    return TrajectoryDefault;
}
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;
        }
        static class Trajectory* GetTrajectory(
            enum TrajectoryType type,
            float sampleTimeMs,
            Vec<float,SIZE> maxJointVelocity,
            Vec<float,SIZE> maxJointAcceleration,
            Vec<float,SIZE> jointStart,
            Vec<float,SIZE> jointEnd
            )
        {
            class Trajectory* retval = NULL;

            switch ( type )
            {
                case TrajectoryJointLinear:
                    retval = (class Trajectory<SIZE>*) new LinearJointTrajectory<SIZE>(sampleTimeMs, maxJointVelocity, maxJointAcceleration, jointStart, jointEnd); 
                    break;
                case TrajectoryJointBangBang:
                    retval = (class Trajectory<SIZE>*) new BangBangJointTrajectory<SIZE>(sampleTimeMs, maxJointVelocity, maxJointAcceleration, jointStart, jointEnd); 
                    break;
                case TrajectoryJointLSPB:
                    retval = (class Trajectory<SIZE>*) new LSPBJointTrajectory<SIZE>(sampleTimeMs, maxJointVelocity, maxJointAcceleration, jointStart, jointEnd); 
                    break;
                case TrajectoryJointFivePoly:
                    break;

                case TrajectoryCartLSPB:
                case TrajectoryCartLinear:
                case TrajectoryCartBangBang:
                case TrajectoryCartFivePoly:
                    // TODO implement carthesian movement
                    retval = (class Trajectory<SIZE>*) new LinearJointTrajectory<SIZE>(sampleTimeMs, maxJointVelocity, maxJointAcceleration, jointStart, jointEnd); 
                    break;
            }
            return retval;
        }

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

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

            // increment step
            currentStep ++;

            return retval;
        }
        MatCarthesian    getNextCartPos     ()
        {
            MatCarthesian 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)
            {
                retval = nodes[step].jointPos;
            }

            return retval;
        }

        MatCarthesian    getCartPos          (unsigned int step)
        {
            MatCarthesian retval(0.0f, 1.0f);
            if (steps == 0)
                return retval;
            if (step >= steps)
            {
                return retval;
            }

            if (nodes != NULL)
            {
                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)
            {
                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)
            {
                retval = nodes[step].acceleration;
            }

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

        void saveToFile(std::string filename)
        {
            std::ofstream file;
            file.open(filename.c_str());
            for (unsigned int currentStep = 0 ; currentStep < steps ; ++ currentStep)
            {
                for (unsigned int currentJoint = 0 ; currentJoint < SIZE; ++ currentJoint)
                {
                    if (currentJoint != 0)
                        file << ", ";
                    file << std::fixed << std::setprecision(20) << nodes[currentStep].jointPos(currentJoint);
                }
                file << "\n";
            }
            file.close();
        }

    protected:
        static const unsigned int defaultSteps = 100;

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

#endif