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

template <class T, unsigned SIZE> 
LinearJointTrajectory::LinearJointTrajectory(
        unsigned int steps_,
        float totalTime_,
        Vec<float,SIZE> jointMovement,
        Vec<float,SIZE> maxJointVelocityPerStep,
        Vec<float,SIZE> maxJointAccelarationPerStep
        )
{
    Vec< float, SIZE> velocity = jointMovement / steps * 1.5f;
    LinearJointTrajectory(
            steps_,
            totalTime_,
            jointMovement,
            maxJointVelocityPerStep,
            maxJointAccelarationPerStep,
            velocity);
}

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

    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 < joints ; 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 
                nodes[currStep].jointPos     = acceleration/2.0f * currentTime * currentTime;
                nodes[currStep].velocity     = acceleration * currentTime;
                nodes[currStep].acceleration = acceleration;
            } else
            if ( currentTime <= totalTime - timeBlend)
            {
                // constant velocity 
                nodes[currStep].jointPos     = (jointMovement(currJoint) - velocity(currJoint) * totalTime)/2.0f + velocity(currJoint) * currentTime;
                nodes[currStep].velocity     = velocity(currJoint);
                nodes[currStep].acceleration = 0.0f;
            }
            else
            {
                // slow down untill aim is reached 
                // speed up till blend time is reached 
                nodes[currStep].jointPos     = jointMovement(currJoint) - acceleration/2*totalTime*totalTime + acceleration * totalTime * currentTime - acceleration/2.0f * currentTime *currentTime;
                nodes[currStep].velocity     = acceleration*timeBlend - acceleration * currentTime ;
                nodes[currStep].acceleration = -acceleration;
            }
            // calculate  carthesian positions
            nodes[currStep].cartPos = 0;
        }
    }
}