/**
 * @file 
 *
 * @author Philipp Schoenberger <ph.schoenberger@googlemail.com>
 */

#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 should als be included here
#include "LinearTrajectory.h"
#include "LSPBTrajectory.h"
#include "BangBangTrajectory.h"
#include "FivePolyTrajectory.h"

/**
 * enumeration of all suppored Trajectorys. 
 * @info How to add a new Trajectory
 * First of all add a Include line in this file.
 * You should add the new Trajectories in this Enumeration
 * Also in the initTrajectoryType initalisation Routine and GetTrajectory should be extended
 * for the new Trajectory Type.
 *
 * @warning __TrajectoryCount should alsways be the last item in the list
 *
 * @see initTrajectoryType
 * @see GetTrajectory
 */
enum TrajectoryType {
    TrajectoryDefault     = 0,
    TrajectoryJointLinear = 0,
    TrajectoryCartLinear,

    TrajectoryJointBangBang,
    TrajectoryCartBangBang,
    
    TrajectoryJointFivePoly,
    TrajectoryCartFivePoly,
    
    TrajectoryJointLSPB,
    TrajectoryCartLSPB,
    __TrajectoryCount
};

/**
 * Helper structure for the String/Enum convert functions
 *
 * @see toEnum
 * @see toString
 */
static struct TrajectoryTypeStr{
    enum TrajectoryType type;
    std::string str;
}* trajectoryTypeStr = NULL;

/**
 * This function initalize the help structure for the String/Enum convert functions
 *
 * @see toEnum
 * @see toString
 */
static void initTrajectoryType ()
{
    if (trajectoryTypeStr == NULL)
    {
        trajectoryTypeStr = new struct TrajectoryTypeStr[__TrajectoryCount];

        int i = 0;

        trajectoryTypeStr[i].type = TrajectoryJointLinear;
        trajectoryTypeStr[i].str  = "JointLinear";
        ++i;
        trajectoryTypeStr[i].type = TrajectoryJointLSPB;
        trajectoryTypeStr[i].str  = "JointLSPB";
        ++i;
        trajectoryTypeStr[i].type = TrajectoryJointBangBang;
        trajectoryTypeStr[i].str  = "JointBangBang";
        ++i;
        trajectoryTypeStr[i].type = TrajectoryJointFivePoly;
        trajectoryTypeStr[i].str  = "JointFivePoly";
        ++i;

        trajectoryTypeStr[i].type = TrajectoryCartLinear;
        trajectoryTypeStr[i].str  = "CartLinear";
        ++i;
        trajectoryTypeStr[i].type = TrajectoryCartLSPB;
        trajectoryTypeStr[i].str  = "CartLSPB";
        ++i;
        trajectoryTypeStr[i].type = TrajectoryCartBangBang;
        trajectoryTypeStr[i].str  = "CartBangBang";
        ++i;
        trajectoryTypeStr[i].type = TrajectoryCartFivePoly;
        trajectoryTypeStr[i].str  = "CartFivePoly";
        ++i;
    }
}

/**
 * This function converts the enumeration type of a Trajectory to a std::string
 *
 * @parameter type Type of the Trajectory
 * @retval string of the Trajectory of "default" if no trajectory was matched
 *
 * @see toEnum
 * @see initTrajectoryType
 */
static std::string toString(enum TrajectoryType type)
{
    initTrajectoryType();
    int items = __TrajectoryCount;
    for (int i = 0 ; i < items ; ++i)
    {
        if (trajectoryTypeStr[i].type == type)
            return ""+ trajectoryTypeStr[i].str;
    }
    return "default";
}

/**
 * This function converts the string of a Trajectory back to a enumeration id
 *
 * @parameter name String containing the Trajectory type
 * @retval Trajectory id or default if no Trajectory was found
 *
 * @see toString
 * @see initTrajectoryType
 */
static enum TrajectoryType toEnum(std::string name)
{
    initTrajectoryType();
    int items = __TrajectoryCount;
    for (int i = 0 ; i < items ; ++i)
    {
        if (trajectoryTypeStr[i].str == name)
            return trajectoryTypeStr[i].type;
    }
    return TrajectoryDefault;
}

enum MovementType
{
    MovementJointBased= 0,
    MovementCartBased
};

/**
 * Trajectory class with a template
 * The unsigned SIZE is representing the number of joints
 *
 * Use this class to implement a new trajectory
 */
template <unsigned SIZE>
class Trajectory
{
    public:
        /**
         * Default constructor
         */
        Trajectory()
        {
            movementType = MovementJointBased;

            steps = 0;
            currentStep = 0;
            totalTime = 0;
            nodes = NULL;
        }
        /**
         * Normal constructor for a Joint based Trajectory
         * Trajectory is computed within the constructor and contains a stepwise 
         * movement from jointStart to the jointEnd.
         *
         * The Trajectoy class just uses a one step Trajectory
         * for smarter Trajectories use BangBang, LSPB or Linear
         * 
         * @param sampleTimeMs The timestep for each step within the Trajectory
         * @param maxJointVelocity The maximum velocity for each Joint of the Robot
         * @param maxJointAcceleration The maximum acceleration for each Joint of the Robot
         * @param jointStart The joint angles of the starting point
         * @param jointEnd The end position for each joint after running the trajectory
         */
        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 = (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);

            // cast unused parameters to void to supress compiler warnings
            (void)jointStart;
            (void)maxJointVelocity;
            (void)maxJointAcceleration;
        }
        /**
         * Normal constructor for a Joint based Trajectory
         * Trajectory is computed within the constructor and contains a stepwise 
         * movement from jointStart to the jointEnd.
         *
         * The Trajectory class just uses a one step Trajectory
         * for smarter Trajectories use BangBang, LSPB or Linear
         * 
         * @param type The enumeration id of the Trajectory to convert to
         * @param sampleTimeMs The time step for each step within the Trajectory
         * @param maxJointVelocity The maximum velocity for each Joint of the Robot
         * @param maxJointAcceleration The maximum acceleration for each Joint of the Robot
         * @param jointStart The joint angles of the starting point
         * @param jointEnd The end position for each joint after running the trajectory
         *
         * @return A valid pointer to a new Instance for a Trajectory between the start and end point.
         *
         * @info memory deletion has to be done by the receiver for the class
         */
        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 __TrajectoryCount: 
                    // fall through to linear joint trajectory
                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;

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

        /**
         * Destructor for each Trajectory which frees the step container
         * Inherent Classes does not need any destructor if no extra fields are allocated within these.
         */
        virtual ~Trajectory()
        {
            free (nodes);
        }

        /**
         * Returns the number of steps for the trajectory
         * @retval total step count
         */
        unsigned int getSteps()
        {
            return steps;
        }

        /**
         * Returns the number of remaining steps for the trajectory
         * @retval remaining steps for this trajectory
         */
        unsigned int getRemainingSteps()
        {
            if (steps >= currentStep)
                return steps - currentStep;
            else 
                return 0;
        }

        /**
         * Returns the current step number for the trajectory
         * @retval current step number for this trajectory
         */
        unsigned int getCurrentStep()
        {
            return currentStep;
        }

        /**
         * Returns the movement type of the trajectory
         * This can be Cartesian or joint based 
         *
         * @retval type of movement for this trajectory
         *
         * @see MovementType
         */
        enum MovementType getMovementType()
        {
            return movementType;
        }

        /**
         * Returns a Vector with new angles for the Robot
         * If end of the Trajectory is reached it always returns the 
         * last joint angles.
         * If there is not even one step in the Trajectory it returns a Vector
         * containing zeros.
         *
         * @info only returns valid values if this is a joint based Trajectory
         *
         * @retval Vector containing the next joint angles
         *
         * @see getJointPos
         */
        Vec<float,SIZE> getNextJointPos()
        {
            Vec<float,SIZE> retval(0.0f);
            unsigned int pos = currentStep;

            // trajectory does not contain even one step (should not happen)
            if (steps == 0)
                return retval;

            // cap on maximum steps
            if (pos >= steps)
            {
                pos = steps-1;
            }

            // only if there is a step container allocated and the trajectory is of type joint 
            if (nodes != NULL && movementType == MovementJointBased)
            {
                retval = nodes[pos].jointPos;
            }

            // increment step
            currentStep ++;

            return retval;
        }

        /**
         * Returns a homogeneous matrix with the new Cartesian position for the robot
         * If end of the trajectory is reached it always returns the 
         * last Cartesian position.
         * If there is not even one step in the trajectory it returns the 
         * identity matrix
         *
         * @info only returns valid values if this is a Cartesian based trajectory
         *
         * @retval Cartesian matrix containing the next position 
         *
         * @see getCartPos
         */
        MatCarthesian getNextCartPos()
        {
            MatCarthesian retval(0.0f,1.0f);
            unsigned int pos = currentStep;

            // trajectory does not contain even one step (should not happen)
            if (steps == 0)
                return retval;

            // cap on maximum steps
            if (pos >= steps)
            {
                pos = steps-1;
            }

            // only if there is a step container allocated and the trajectory is of type joint 
            if (nodes != NULL && movementType == MovementCartBased)
            {
                retval = nodes[pos].cartPos;
            }

            // increment step for next query 
            currentStep ++;

            return retval;
        }

        /**
         * Returns a joint position for the robot without 
         * incrementing the next position
         * If there is not even one step in the trajectory it returns the 
         * zero vector
         *
         * @info only returns valid values if this is a joint based trajectory
         *
         * @param step id of the needed position
         * @retval joint vector containing the position
         *
         * @see getNextJointPos
         */
        Vec<float,SIZE> getJointPos(unsigned int step)
        {
            Vec<float,SIZE> retval(0.0f);
            if (steps == 0)
                return retval;
            if (step >= steps)
            {
                return retval;
            }

            // only if there is a step container allocated and the trajectory is of type joint 
            if (nodes != NULL && movementType == MovementJointBased)
            {
                retval = nodes[step].jointPos;
            }

            return retval;
        }

        /**
         * Returns a current joint position for the robot without 
         * incrementing the next position
         * If end of the trajectory is reached it always returns the 
         * last joint position.
         * If there is not even one step in the trajectory it returns the 
         * zero vector
         *
         * @info only returns valid values if this is a Cartesian based trajectory
         *
         * @param step id of the needed position
         * @retval joint vector containing the position
         *
         * @see getNextJointPos
         */
        MatCarthesian    getCartPos          (unsigned int step)
        {
            MatCarthesian retval(0.0f,1.0f);

            // trajectory does not contain even one step (should not happen)
            if (steps == 0)
                return retval;

            // cap on maximum steps
            if (step >= steps)
            {
                step = steps-1;
            }

            // only if there is a step container allocated and the trajectory is of type Cartesian
            if (nodes != NULL && movementType == MovementCartBased)
            {
                retval = nodes[step].cartPos;
            }

            return retval;
        }

        /**
         * Returns a joint velocity for the robot without 
         * incrementing the next position
         * If end of the trajectory is reached it always returns the 
         * last velocity.
         * If there is not even one step in the trajectory it returns the 
         * zero vector
         *
         * @info only returns valid values if this is a joint based trajectory
         *
         * @param step id of the needed position
         * @retval joint vector containing the position
         *
         * @see getNextJointPos
         */
        Vec<float,SIZE> getJointVelocity    (unsigned int step)
        {
            Vec<float,SIZE> retval(0.0f);

            // trajectory does not contain even one step (should not happen)
            if (steps == 0)
                return retval;

            // cap on maximum steps
            if (step >= steps)
            {
                step = steps - 1;
            }

            // only if there is a step container allocated and the trajectory is of type joint 
            if (nodes != NULL && movementType == MovementJointBased)
            {
                retval = nodes[step].velocity;
            }

            return retval;
        }

        /**
         * Returns a joint acceleration for the robot without 
         * incrementing the next position
         * If end of the trajectory is reached it always returns the 
         * last velocity.
         * If there is not even one step in the trajectory it returns the 
         * zero vector
         *
         * @info only returns valid values if this is a joint based trajectory
         *
         * @param step id of the needed position
         * @retval joint vector containing the position
         *
         * @see getNextJointPos
         */
        Vec<float,SIZE> getJointAcceleration(unsigned int step)
        {
            Vec<float,SIZE> retval(0.0f);

            // trajectory does not contain even one step (should not happen)
            if (steps == 0)
                return retval;

            // cap on maximum steps
            if (step >= steps)
            {
                step = steps - 1;
            }

            // only if there is a step container allocated and the trajectory is of type joint 
            if (nodes != NULL && movementType == MovementJointBased)
            {
                retval = nodes[step].acceleration;
            }

            return retval;
        }

        /**
         * struct containing one step for the trajectory
         */
        struct trajectoryNode
        {
            /// joint angles only used for joint based trajectories
            Vec<float,SIZE> jointPos; 
            /// Cartesian position only used for Cartesian based trajectories
            MatCarthesian cartPos;
            /// velocity for all Joint 
            Vec<float,SIZE> velocity;
            /// acceleration for all joints
            Vec<float,SIZE> acceleration;
        };

        /**
         * Saves each joint step to the file line wise
         * @param filename filename the trajectory is saved to including the path
         */
        void saveJointToFile(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:
        /// step count for this trajectory which is used for the nodes array
        unsigned int steps;

        /// current position for the trajectory
        unsigned int currentStep;

        /// total time of the trajectory in ms
        float totalTime;

        /// array of the trajectory containing parameters for each step
        struct trajectoryNode* nodes;

        /// movement type of the trajectory which is Cartesian or Joint based
        enum MovementType movementType;
};

#endif