#ifndef TRAPEZOIDAL_TRAJECTORY_HPP
#define TRAPEZOIDAL_TRAJECTORY_HPP

#include <vector>
#include "arm_define.h"

#define TRAJECTORY_COMPELETE 1
#define TRAJECTORY_ONGOING 0
#define TRAJECTORY_ERROR -1

#define STEPPING_UNIFORM_TIME_STAMP -1

struct Trajectory {
    float startAngles[USED_ARM_DOF];
    float endAngles[USED_ARM_DOF];
    float aim_velocity[USED_ARM_DOF];
    float now_acceleration[USED_ARM_DOF];
    int64_t startTime;
    int accelerate_time_step;
    int uniform_time_step;
    int goalType;
    int current_time_step;

    int64_t stopTime;
};

class TrapezoidalTrajectory
{
public:
    TrapezoidalTrajectory();
    ~TrapezoidalTrajectory();

    int computeTrajectory(const float *start, const float *end, int type);
    int updateTrajectory(float *aimAngles);
    int checkStartTrajectory(int64_t startTime);
    int getAngleFromTime(float *aimAngles, int64_t timeStamp);
    void getAngleForChecking(float *aimAngles);
    void getAngleForStarting(float *aimAngles);
    void stopTrajectory();

    int updatePoseSteppingTrajectory(float *aimAngles);
    int continuePoseSteppingTrajectory(float *nowAngles, float *aimAngles);

    Trajectory* getNowTrajectory() {
        if (nowTrajectoryIndex_ < 0) {
            return nullptr;
        }
        return &trajectoryHistory_[nowTrajectoryIndex_];
    }

private:
    float max_velocity_;
    float max_acceleration_;
    int max_accelerate_time_;
    float max_accelerate_angle_;
    float min_angle_for_pose_stepping_max_speed_;

    Trajectory trajectoryHistory_[MAX_TRAJECTORY_HISTORY];
    int nowTrajectoryIndex_;
    int trajectoryFullFlag_;

    Trajectory* addTrajectory();
    void computeMovingTrajectory(Trajectory *newTrajectory, const float *start, const float *end);
    void computeSteppingTrajectory(Trajectory *newTrajectory, const float *start, const float *end);
    void computePoseSteppingTrajectory(Trajectory *newTrajectory, const float *start, const float *end);
};

#endif // TRAPEZOIDAL_TRAJECTORY_HPP