#ifndef TRAPEZOIDAL_TRAJECTORY_KINEMATICS_HPP
#define TRAPEZOIDAL_TRAJECTORY_KINEMATICS_HPP

#include "arm_define.h"
#include "rm_service.h"

#define STEPPING_UNIFORM_TIME_STAMP -1

struct TrapezoidalTrajectory {
    float startAngles[USED_ARM_DOF];
    float endAngles[USED_ARM_DOF];
    float aim_velocity[USED_ARM_DOF];
    float now_acceleration[USED_ARM_DOF];
    int accelerate_time_step;
    int uniform_time_step;
    int goalType;
    int current_time_step;
};

class TrapezoidalTrajectoryKinematics
{
public:
    TrapezoidalTrajectoryKinematics(RM_Service *rmService);
    TrapezoidalTrajectoryKinematics();
    ~TrapezoidalTrajectoryKinematics();

    void getAngleForCheckingInner(TrapezoidalTrajectory *trajectory, int timeStep, float *aimAngles);
    void getAngleForCheckingOffset(TrapezoidalTrajectory *trajectory, int timeStepOffset, float *aimAngles);
    int computeMovingTrajectory(TrapezoidalTrajectory *trajectory, const float *start, const float *end, int armId);
    int computeSteppingTrajectory(TrapezoidalTrajectory *trajectory, const float *start, const float *speed, int armId);
    int computePoseSteppingTrajectory(TrapezoidalTrajectory *trajectory, const float *start, const float *pose, int armId);
    int updateTrajectory(TrapezoidalTrajectory *trajectory, float *aimAngles);
    void stopTrajectory(TrapezoidalTrajectory *trajectory);
    int continuePoseSteppingTrajectory(TrapezoidalTrajectory *trajectory, const float *nowAngles, const float *direct);

    int ForwardKinematics(float *jointAngles, float *endEffectorPose);
    int InverseKinematics(const float *endEffectorPose, float *jointAngles);
private:
    RM_Service *rmService_;

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

    int PoseSteppingFromNowAnglesAndDirectPose(float *nowAngles, const float *direct);
};

#endif // TRAPEZOIDAL_TRAJECTORY_KINEMATICS_HPP