#ifndef MOTION_CMotionProfile_H
#define MOTION_CMotionProfile_H

#ifdef  BUILD_IODLL
#define     IODLLCLASS  __declspec(dllexport)
#else
#define     IODLLCLASS  __declspec(dllimport)
#endif

/**
 * Defines an alias representing the PVT.
 * PVT's are a way of representing a motion profile by defining s series of point that the motor will need to pass.
 * At given times \f$ T \f$ the motor will need to be at position \f$ P \f$ with velocity \f$ V \f$
 */
typedef struct
{
    double P; /**< The position */
    double V; /**< The velocity */
    double T; /**< The time */
} PVT;

#define MAX_PVT 100
class IODLLCLASS CMotionProfile
{
public:
    CMotionProfile(void);
    CMotionProfile(const CMotionProfile &p);

    void Append(const CMotionProfile &p);
    virtual ~CMotionProfile(void);

    void Dump() const;

    void Plot(double delta_t) const;

/**
 * Create a new move with set paramters
 * @param distance distance to travel
 * @param vMax maximum velocity
 * @param aMax maximum acceleration
 * @param tJerk jerk time, time in which you can reach aMax
 */
    void CreateMove(double distance, double vMax, double aMax, double tJerk);
    void CreateMove(double v0, double distance, double vMax, double aMax, double tJerk);
    void CreateSpeedMove(double v0, double distance, double vMax, double aMax, double nextSpeed);
    bool CreateTrajectConstantEndSpeed(double Dist, double Vmax, double Amax,
                                       double Tjerk, double Ve);

/**
 * Calculates total time for current CMotionProfile
 * @return total move time
 */
    double GetMoveTime(void) const;

/**
 * Count number of splines programmed for current CMotionProfile
 * @return total number of splines for current CMotionProfile
 */
    int GetTotalSplines() const;

/**
 * Calculate position and speed for given time
 * @param t time (s)
 * @param position will contain the position at time t for current CMotionProfile
 * @param velocity will contain the velocity at time t for current CMotionProfile
 * @param acceleration will contain the acceleration at time t for current CMotionProfile
 */
    void TimeToDistance(double t, double &position, double &velocity, double &acceleration) const;

/**
 * Calculate total distance for current CMotionProfile
 * @return total distance for current CMotionProfile
 */
    double GetTotalDistance() const;

/**
 * Reset all information in the CMotionProfile
 */
    void Reset();
    void Reverse();
/**
 * Calculate the distance travel for S-curve.
 * @param Vp - Max speed
 * @param Ve - End speed
 * @param Ap - Max acceleration
 * @param Tj - T Jerk
 */
    double CalcS(double Vp, double Ve, double Ap, double Tj);
    double GetEndSpeed();
    double abc(double a, double b, double c);
private:
    int    nPVT;
    PVT    PVTs[MAX_PVT];
    double vStart;

    void CreatePVT(double distance, double aMax, double tA, double tC, double tJ);
    void AddPVT(double a0, double jerk, double t);
    void TimeToDistance(double v0, const PVT &pvt, double t, double &position, double &velocity, double &acceleration) const;

    void   VeCalc1(double Ve, double Ap, double Tj, double P);
    void   VeCalc2(double Ve, double Tj, double P);
    double SolveVp1(double Vp, double Ve, double Ap, double Tj, double P);
    double SolveVp2(double Vp, double Ve, double Ap, double Tj, double P);
    double SolveVp3(double Vp, double Ve, double Ap, double Tj, double P);
    void   VeCalc3(double Vp, double Ve, double Ap, double Tj, double P);
    void   VeCalc4(double Ve, double Ap1, double Ap2, double Tj, double P);
    void   VeCalc5(double Vp, double Ve, double Ap, double Tj, double P);

/**
 * Add a stopping PVT and return the distance it took to stop.
 */
    double InsertStop(double V, double aMax);
};
#endif