#ifndef RM_ARM_HANDLER_HPP
#define RM_ARM_HANDLER_HPP

#include <cstdlib>
#include <stdio.h>
#include <unistd.h>
#include <functional>
#include <sys/stat.h>
#include <sys/time.h>
#include <semaphore.h>
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/float64_multi_array.hpp"
#include "sensor_msgs/msg/joint_state.hpp"
#include "goal_manager.hpp"
#include "arm_define.h"
#include "arm_driver_define.hpp"
#include "rm_arm_simulator.hpp"

typedef enum {
    ARM_STATUS_INIT = 0,
    ARM_STATUS_READY,
    ARM_STATUS_MOVING,
    ARM_STATUS_WAITING,
    ARM_STATUS_ERROR
} ARM_STATUS_E;

class RmArmHandler
{
private:
    std::shared_mutex rw_mutex_;
    sem_t *sem_;

    GoalManager *goalManager_;
    ArmDriver *driver_;
public:
    RmArmHandler(ArmDriver *driver, int armType, sem_t *sem, KinematicsManager *trapezoidalTrajectory);
    ~RmArmHandler();

    KinematicsManager *trapezoidalTrajectory_;
    int armType_; // 0:left 1:right

    void ArmDealCallBackInfo(ArmStatusCallbackData *data);

    float nowJoints_[USED_ARM_DOF];
    float nextJoints_[USED_ARM_DOF];

    int AddNextPoint(float points[GOAL_DATA_LENGTH], const int64_t &index, int type);
    int DeletePoint(int64_t index);
    int64_t GetCurrentRunOffGoalIndex();
    int64_t GetCurrentGoalIndex();
    void ArmRun(int64_t timeStamp);
    void ArmStop(int result = 0);
    void ArmStopStart(int result);
    void ArmMove();
    int GoalReached();

    int AddNextPointFromPose(const float pose[POSE_DIMENSION], const int64_t &index, int type);
    int AddNextPointFromPoseMoving(const float pose[POSE_DIMENSION], const int64_t &index);
    int GetNextPointFromPoseStepping(const float pose[POSE_DIMENSION], float *joints);

    int StartNewTrajectory();

    ARM_STATUS_E armStatus;
};


#endif // RM_ARM_HANDLER_HPP
