#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
                )
        {
            std::cout << "bang bang \n " ;
            float MStoSec = 1000.0f;
            float sampleTime = sampleTimeMs / MStoSec;
            // calculate maximum velocity and acceleration
            Vec<float, SIZE> maxJointLocalVelocity = maxJointVelocity * sampleTime;
            Vec<float, SIZE> maxJointLocalAcceleration = maxJointAcceleration * sampleTime;
            std::cout << maxJointLocalVelocity << ", " << maxJointLocalAcceleration << "\n";

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

            // calculate sample count

            // calculate number of movement steps
            // one joint has to reach maxvelocity the others are stepped down to 
            // calculate time if acceleration is enouth to reach max speed
            Vec<float,SIZE> minBangBangTime = maxJointLocalVelocity.celldivide(maxJointLocalAcceleration) / sampleTime;
            //TODO check if mintime is neceesary
            

            Vec<float,SIZE> minStepsPerJoint = jointMovementAbs.celldivide(maxJointLocalVelocity)*2.0f;
            minStepsPerJoint = minStepsPerJoint.ceil();
            std::cout << minStepsPerJoint << "minStepsPerJoint \n";

            this->steps = 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;
            Vec<float,SIZE> velocityLast(0.0f);

            // percentage of max velocity 
            //Vec<float,SIZE> currJointMovementOfMax = minStepsPerJoint / minStepsPerJoint.max() ;
            // s =  a* t^2 / 2 => a = s* 2 / t^2
            float floaterror = 0.998;
            Vec<float,SIZE> currMaxAcceleration = (jointMovementAbs * 2.0f ).celldivide(this->steps).celldivide(this->steps)*2.0f*floaterror;
            for( int i = 0 ; i < this->steps; ++i)
            {
                // acceleration phase
                if (i <= this->steps /2 )
                {
                    this->nodes[i].acceleration = currMaxAcceleration ;
                }
                //  deacceleration phase
                else
                {
                    this->nodes[i].acceleration = currMaxAcceleration * -1.0f;
                }
                this->nodes[i].velocity = velocityLast + jointMovementSgn.cellmultiply(this->nodes[i].acceleration);
this->nodes[i].jointPos = jointLast + this->nodes[i].velocity; 
                jointLast = this->nodes[i].jointPos;
                velocityLast = this->nodes[i].velocity;
            }
            // 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