#ifndef __NOVA5_CONTROL_IMR_NOVA_CONTROL_H__
#define __NOVA5_CONTROL_IMR_NOVA_CONTROL_H__

// #include <ros/ros.h>
#include <termios.h>
#include <stdio.h>
#include <thread>
#include"nova5_control/nova5_modbus.h"

class imr_nova_control
{
public:
    imr_nova_control();
    virtual ~imr_nova_control(); 

    void startModeThread();
    void stopModeThread(); 

    void startHomeThread();
    void stopHomeThread(); 

    unsigned int *ok_count_max_;
    unsigned int *ng_count_max_;

    bool *enable_;
    double mode_;

    bool *continue_flag_;
    bool *go_home_flag_;
    bool *power_on_flag_;

    enum statusCode
    {
        ROBOT_WAITING = 98,
        ROBOT_RECONNECTING = 99,
        ROBOT_RUNNING = 100,
        ROBOT_DISCONNECTED = 101,
        ROBOT_READ_DATA_ZERO = 102,
        ROBOT_DATA_NOT_UPDATED = 103,
        ROBOT_TARGET_COORDINATES_OUT_OF_RANGE = 104,
        ROBOT_COMING_HOME = 105,
        ROBOT_CHECK_EMERGENCY_STOP_BUTTON = 106,
        ROBOT_IN_ALARM_STATE = 107,
        ROBOT_POWER_ON_WAITTING = 108,
        ROBOT_POWER_ON = 109,
        ROBOT_DRAG = 110,
        ROBOT_WAITING_SELECT_CONTINUE_OR_GO_HOME = 111,
        ROBOT_OK_FULL = 112,
        ROBOT_NG_FULL = 113,
        ROBOT_ERROR = 999
    };

    /**
     * @var statusCode_
     * 98: Robot is waiting
     * 99: Robot is reconnecting
     * 100: Robot is running
     * 101: Robot is disconnected
     * 102: Read data is 0 (Camera can not detect object or lost connection)
     * 103: Data is not updated (camera maybe disconnected)
     * 104: Target coordinates are not within the working range
     * 105: Robot is comming home
     * 106: Check the robot's emergency stop button
     * 107: Robot is in alarm state
     * 108: Robot is power on
     * 109: Robot is in drag mode
     * 110: OK object is over the limit
     * 111: NG object is over the limit
     * 999: Robot is error
     */
    statusCode statusCode_;
    
private:
    char getKey();
    void imr_dobot_control_thread();
    void imr_dobot_go_home_thread();

    // Coordinates
    std::vector<double> HOME = {-141.91,301.01,524.27,-179.4,-0.04,0.0};
    std::vector<double> CENTER_P = {309.11,63.29,524.27,-179.4,-0.04,0.00};
    std::vector<double> ORIGIN_COOR = {150.71,-435.9,524.27,-179.4,-0.04,-90.00};
    std::vector<double> OK_COOR = {-141.91,301.01,150,-179.4,-0.04,0.0};
    std::vector<double> NG_COOR = {-141.91,657.01,150,-179.4,-0.04,0.0};

    std::string IP = "192.168.2.6";
    int PORT = 502;

    bool modeThreadRunning_; // Variable to check if the thread is running
    bool homeThreadRunning_; // Variable to check if the thread is running
    std::shared_ptr<std::thread> thr_main_;
    std::shared_ptr<std::thread> thr_go_home_;

};

#endif // __NOVA5_CONTROL_IMR_NOVA_CONTROL_H__