#ifndef _BANGBANGTRAJCETORY_H_
#define _BANGBANGTRAJCETORY_H_
#include "Trajectroy.h"
#include "vec.h"

template <unsigned SIZE> 
class BangBangJointTrajectory : public Trajectory<SIZE>
{
    public:
        BangBangJointTrajectory(
                float sampleTimeMs,
                Vec<float,SIZE> maxJointVelocity,
                Vec<float,SIZE> maxJointAcceleration,
                Vec<float,SIZE> jointStart,
                Vec<float,SIZE> jointEnd
                )
        {
            float MStoSec = 1000.0f;
            // calculate maximum velocity and acceleration
            Vec<float, SIZE> maxJointLocalVelocity = maxJointVelocity * (sampleTimeMs / MStoSec);
            Vec<float, SIZE> maxJointLocalAcceleration = maxJointAcceleration * (sampleTimeMs / MStoSec);

            // calculate delta movement
            Vec<float,SIZE> jointMovement = jointEnd - jointStart;
            Vec<float,SIZE> jointMovementAbs = jointMovement.abs();

            // calculate sample count

            // calculate number of movement steps
            // one joint has to reach maxvelocity the others are stepped down to 
            Vec<float,SIZE> minStepsPerJoint = jointMovementAbs.celldivide(maxJointLocalVelocity)*2.0f;
            this->steps = ceil(minStepsPerJoint.max());
            if (this->steps == 0)
                this->steps +=1;

            this->nodes = (struct Trajectory<SIZE>::trajectoryNode* ) calloc(sizeof(struct Trajectory<SIZE>::trajectoryNode),this->steps);

            Vec<float,SIZE> jointLast  = jointStart;
            float percentStep = 1.0f / ((this->steps+1)/2);
            for( int i = 0 ; i < this->steps; ++i)
            {
                // acceleration phase
                if (i > this->steps /2 )
                {
                    float percent = i * percentStep; 
                    this->nodes[i].velocity = maxJointLocalVelocity * percent;
                    this->nodes[i].acceleration = maxJointLocalVelocity * percentStep;
                }
                //  deacceleration phase
                else
                {
                    float percent = ( i - this->steps/2 ) * percentStep; 
                    this->nodes[i].velocity = maxJointLocalVelocity * percent;
                    this->nodes[i].acceleration = maxJointLocalVelocity * (-1.0 * percentStep);
                }

                jointLast = jointLast + this->nodes[i].velocity;
                this->nodes[i].jointPos = jointLast;
            }
            // last step myth be to wide so cut it to the wanted position
            this->nodes[this->steps-1].jointPos = jointEnd;
            this->nodes[this->steps-1].velocity = maxJointLocalVelocity; // TODO this is not the truth
            this->nodes[this->steps-1].acceleration = Vec<float,SIZE>(0.0f); 
        }
    private:

    protected:

};
template <unsigned SIZE> 
class BangBangCartTrajectory: public Trajectory<SIZE>
{
};

#endif