#include "LinearTrajectory.h"
#include "vec.h"
#include "stdlib.h"

template <unsigned SIZE> 
LinearJointTrajectory<SIZE>::LinearJointTrajectory(
        unsigned int steps_,
        float totalTime_,
        Vec<float,SIZE> jointMovement_
        )
{
    Vec< float, SIZE> velocity = jointMovement_ / steps_ * 1.5f;
    LinearJointTrajectory(
            steps_,
            totalTime_,
            jointMovement_,
            velocity);
}

template <unsigned SIZE> 
LinearJointTrajectory<SIZE>::LinearJointTrajectory(
        unsigned int steps_,
        float totalTime_,
        Vec<float,SIZE> jointMovement,
        Vec<float,SIZE> velocity
        )
{
    super(steps_, totalTime_, jointMovement);

    if ( velocity > jointMovement/totalTime_)
    {
        std::cerr << "velocity is to small for Trajectory\n";
    }

    if ( velocity < (2.0f*jointMovement)/totalTime_)
    {
        std::cerr << "velocity is to big for Trajectory\n";
    }

    for (unsigned int currJoint = 0 ; currJoint < SIZE; currJoint++)
    {
        unsigned int timeBlend = (jointMovement(currJoint) + velocity(currJoint)*totalTime_)/ velocity(currJoint);
        float acceleration =  velocity(currJoint) / timeBlend;

        for (unsigned int currStep = 0 ; currStep < steps_ ; ++currStep)
        {
            float currentTime = currStep * totalTime_ / steps_;
            if (currentTime <= timeBlend)
            {
                // speed up till blend time is reached 
                this->nodes[currStep].jointPos     = acceleration/2.0f * currentTime * currentTime;
                this->nodes[currStep].velocity     = acceleration * currentTime;
                this->nodes[currStep].acceleration = acceleration;
            } else
            if ( currentTime <= totalTime_ - timeBlend)
            {
                // constant velocity 
                this->nodes[currStep].jointPos     = (jointMovement(currJoint) - velocity(currJoint) * totalTime_)/2.0f + velocity(currJoint) * currentTime;
                this->nodes[currStep].velocity     = velocity(currJoint);
                this->nodes[currStep].acceleration = 0.0f;
            }
            else
            {
                // slow down until aim is reached 
                // speed up till blend time is reached 
                this->nodes[currStep].jointPos     = jointMovement(currJoint) - acceleration/2*totalTime_*totalTime_ + acceleration * totalTime_ * currentTime - acceleration/2.0f * currentTime *currentTime;
                this->nodes[currStep].velocity     = acceleration*timeBlend - acceleration * currentTime ;
                this->nodes[currStep].acceleration = -acceleration;
            }
            // calculate Cartesian positions
            this->nodes[currStep].cartPos = 0;
        }
    }
}