#ifndef _LINEAR_TRAJECTORY_H_
#define _LINEAR_TRAJECTORY_H_
/**
 * @addtogroup trajectory
 * @{
 * @addtogroup linear
 * @{
 * @author  Philipp Schoenberger <ph.schoenberger@googlemail.com>
 * @version 1.0
 *
 * @section LICENSE
 *
 * This program is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License as
 * published by the Free Software Foundation; either version 2 of
 * the License, or (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
 * General Public License for more details at
 * https://www.gnu.org/copyleft/gpl.html
 *
 * @section DESCRIPTION
 *
 * This file contains the Trajectory for a linear trajectory
 * The linear trajectory only uses the max speed and ignores
 * the acceleration parameter.
 * The movement is a strict hard acceleration and deacceration.
 *
 * The slowest joint is defining the speed of the other joints.
 * By that all joints start and stop the movement synchronously
 */

#include "Trajectroy.h"
#include "stdlib.h"
#include "Vec.h"
#include "Mat.h"


/**
 * Class for a Bang Bang Trajectory based on joint angles.
 * The joint start and stop synchron
 *
 * This class is based on the Trajectory joint Template so it 
 * all possible joint counts 
 *
 * @see Trajectory
 */
template <unsigned SIZE>
class LinearJointTrajectory : public Trajectory<SIZE>
{
    public:

        /**
         * standard constructor which will setup the class for a 
         * trajectory from start to end position with a synchron movement based on joints
         *
         * The trajectory will be of type linear (linear constant speed)
         *
         * @param sampleTimeMs         The discrete time interval for each step within the Trajectory with the unit [ milliseconds ]
         * @param maxJointVelocity     The maximum velocity for each joint with the unit [ degree/ seconds ]
         * @param maxJointAcceleration The maximum acceleration for each joint with the unit [ degree/(seconds^2) ] (unused)
         * @param jointStart           The starting point for each joint in [ degree ]
         * @param jointEnd             The point which should be reached at the end of the Trajectory [ degree ]
         */
        LinearJointTrajectory(
                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
            Vec<float,SIZE> minStepsPerJoint = jointMovementAbs.celldivide(maxJointLocalVelocity);
            this->steps = ceil(minStepsPerJoint.max());
            if (this->steps == 0)
                this->steps +=1;

            // allocate an array for the joint movements
            this->nodes = (struct Trajectory<SIZE>::trajectoryNode* ) calloc(sizeof(struct Trajectory<SIZE>::trajectoryNode),this->steps);

            // initialize the array of movements
            Vec<float,SIZE> jointLast  = jointStart;
            for( int i = 0 ; i < this->steps; ++i)
            {
                jointLast = jointLast + jointMovement.celldivide((float) this->steps );
                this->nodes[i].jointPos = jointLast;
                this->nodes[i].velocity = maxJointLocalVelocity; 
                this->nodes[i].acceleration = Vec<float,SIZE>(0.0f);
            }
            // last step might be to wide so cut it to the wanted position
            this->nodes[this->steps-1].jointPos = jointEnd;
            this->nodes[this->steps-1].velocity = maxJointLocalVelocity;
            this->nodes[this->steps-1].acceleration = Vec<float,SIZE>(0.0f); 
        }
};

/**
 * Class for a linear Trajectory based on homogenous
 * Cartesian Positions.
 * The joint start and stop synchron
 *
 * This class is based on the Trajectory joint Template so it 
 * all possible joint counts 
 *
 * @TODO implement the Cartesian movement constructor for linear Trajectories
 * @see Trajectory
 */
template <unsigned SIZE>
class LinearCartTrajectory : public Trajectory<SIZE>
{

};

/**
 * @}
 * @}
 */
#endif