#include <stdlib.h>
#include "Trajectroy.h"
#include "vec.h"

template <unsigned SIZE> 
Trajectory<SIZE>::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 Trajectory::trajectoryNode* ) malloc(sizeof(struct Trajectory::trajectoryNode)*steps);

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

    // unused
    (void)jointStart;
    (void)jointEnd;
}

template <unsigned SIZE> 
Trajectory<SIZE>::Trajectory(
        unsigned int steps_,
        float totalTime_,
        Mat<float,4> cartStart,
        Mat<float,4> cartEnd
        )
{
    movementType = MovementCartBased;

    steps = steps_;
    currentStep = 0;

    totalTime = totalTime_;

    nodes = (struct Trajectory::trajectoryNode* ) malloc(sizeof(struct Trajectory::trajectoryNode)*steps_);

    // unused
    (void) cartStart;
    (void) cartEnd;
}

template <unsigned SIZE> 
Trajectory<SIZE>::~Trajectory()
{
    free(nodes);
}


template <unsigned SIZE> 
unsigned int Trajectory<SIZE>::getSteps()
{
    return steps;
}

template <unsigned SIZE> 
unsigned int Trajectory<SIZE>::getRemainingSteps()
{
    if (steps >= currentStep )
        return steps - currentStep;
    else 
        return 0;
}

template <unsigned SIZE> 
unsigned int Trajectory<SIZE>::getCurrentStep()
{
    return currentStep;
}

template <unsigned SIZE> 
enum MovementType Trajectory<SIZE>::getMovementType()
{
    return movementType;
}

template <unsigned SIZE> 
Vec<float,SIZE> Trajectory<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;
}
template <unsigned SIZE> 
Mat<float,4> Trajectory<SIZE>::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;
}

template <unsigned SIZE> 
Vec<float,SIZE> Trajectory<SIZE>::getJointPos (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;
}
template <unsigned SIZE> 
Mat<float,4> Trajectory<SIZE>::getCartPos (unsigned int step)
{
    Mat<float,4> retval(0.0f, 1.0f);
    if (step >= steps)
    {
        return retval;
    }

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

    return retval;
}
template <unsigned SIZE> 
Vec<float,SIZE> Trajectory<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;
}
template <unsigned SIZE> 
Vec<float,SIZE> Trajectory<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;
}