// nova5_modbus.h
#ifndef NOVA5_CONTROL_H
#define NOVA5_CONTROL_H
// #pragma once

#include<ros/ros.h>
#include <boost/shared_ptr.hpp>
#include<iostream>
#include<math.h>
#include<cstring>
#include<array>
#include<stdexcept>

#include"modbus_tcp/modbus.h"


#define TIME_ERR  1
#define ERR  -1
#define TIME_OUT  1.5

// Coil Registers
#define COIL_START                0
#define COIL_PAUSE                1
#define COIL_CONTINUE             2
#define COIL_STOP                 3
#define COIL_EMERGENCY_STOP       4
#define COIL_CLEAR_ALARM          5
#define COIL_POWER_ON             6
#define COIL_ENABLE               100
#define COIL_DISABLE              101
// #define USER_DEFINED_COIL_START   3095
#define COILD_MOVE                3095
#define COILD_MOVING_DONE         3096
#define COILD_TOOL                3097
#define COILD_MOVE_MODE           3098 // 0 moveJ - 1 moveL
// #define USER_DEFINED_COIL_END     9999

// Discrete Input Registers
#define DISCRETE_STOP_STATE         1
#define DISCRETE_PAUSE_STATE        2
#define DISCRETE_RUNNING_STATE      3
#define DISCRETE_ALARM_STATE        4
#define DISCRETE_RESERVED_1         5
#define DISCRETE_REMOTE_CONTROL     7


// Input Registers
#define INPUT_ROBOT_MODE_START                          1012
#define INPUT_ROBOT_MODE_END                            1015
#define INPUT_TIMESTAMP_START                           1016
#define INPUT_TIMESTAMP_END                             1019
#define INPUT_TARGET_JOINT_POSITION_START               1096
#define INPUT_TARGET_JOINT_POSITION_END                 1119
#define INPUT_TARGET_JOINT_CURRENT_START                1168
#define INPUT_TARGET_JOINT_CURRENT_END                  1191
#define INPUT_ACTUAL_JOINT_POSITION_START               1216
#define INPUT_ACTUAL_JOINT_POSITION_END                 1239
#define INPUT_ACTUAL_JOINT_CURRENT_START                1264
#define INPUT_ACTUAL_JOINT_CURRENT_END                  1287
#define INPUT_ROBOT_BRAKE_STATUS_H                      1512 //HIGH BYTE
#define INPUT_ROBOT_ENABLING_STATUS_L                   1513 //LOW BYTE
#define INPUT_ROBOT_DRAG_STATUS_H                       1513 //HIGH BYTE
#define INPUT_ROBOT_DRAG_STATUS_L                       1514 //HIGH BYTE
#define INPUT_ROBOT_ALARM_STATUS_H                      1514 //HIGH BYTE
#define INPUT_LOAD_WEIGHT_START                         1584
#define INPUT_LOAD_WEIGHT_END                           1587
#define INPUT_ECCENTRIC_DISTANCE_X_START                1588
#define INPUT_ECCENTRIC_DISTANCE_X_END                  1591
#define INPUT_ECCENTRIC_DISTANCE_Y_START                1592
#define INPUT_ECCENTRIC_DISTANCE_Y_END                  1595
#define INPUT_ECCENTRIC_DISTANCE_Z_START                1596
#define INPUT_ECCENTRIC_DISTANCE_Z_END                  1599
#define INPUT_USER_COORDINATES_START                    1600
#define INPUT_USER_COORDINATES_END                      1623
#define INPUT_TOOL_COORDINATES_START                    1624
#define INPUT_TOOL_COORDINATES_END                      1647

// Holding Registers
#define HOLDING_GLOBAL_SPEED                100

// #define HOLDING_USER_DEFINED_START          3095
#define HOLDING_TARGET_POINT_START          3095
#define HOLDING_TARGET_POINT_END            3118
#define HOLDING_CAMERA_POINT_START          3119
#define HOLDING_CAMERA_POINT_END            3134
#define HOLDING_CAMERA_OK                   3135
#define HOLDING_CAMERA_TIMESTAMP_START      3136
#define HOLDING_CAMERA_TIMESTAMP_END        3139
#define HOLDING_POINT_CURRENT_START         3140
#define HOLDING_POINT_CURRENT_END           3163
#define HOLDING_Z_MIN_USER_SET_START        3164
#define HOLDING_Z_MIN_USER_SET_END          3167
// #define HOLDING_USER_DEFINED_END            8999

// #define HOLDING_CURRENT_SCREW_INDEX         9000
#define HOLDING_ALARM_CODE                  9054




class cnt_nova5
{
public:

    cnt_nova5(std::string ip_address, int port);
    virtual ~cnt_nova5();
    /**
     * @brief Connect to nova
     */
    int nova_connect(void);
    /**
     * @brief Check connection
     */
    bool nova_checkConnected(void);
    
    void nova_disable(void);
    /**
     * @brief Close to nova
     */
    void nova_close(void);
    /**
     * @brief Alarm notification
     */
    bool nova_statusAlarm(void);
    /**
     * @brief Clear alarm
     */
    void nova_clearAlarm(void);
    /**
     * @brief Power on robot
     */
    int nova_powerOn(void);
    /**
     * @brief enable robot
     */
    bool nova_enable(void);
    /**
     * @brief start program
     */
    bool nova_startProgram(void);
    /**
     * @brief stop program
     */
    void nova_stopProgram(void);
    /**
     * @brief   Read robot mode and return robot mode
    3: Emergency stop mode, not powered on
    4: Powered on mode, not enabled
    5: Enabled mode, program not started
    6: Drag mode
    7: Program started mode
    9: Kinematic error of robot nova
     */
    double nova_robotMode(void);
    /**
     * @brief     Select moveJ or moveL 
     */
    void nova_robotMove(void);
    /**
     * @brief      Set speed global for nova
     * @param speed Speed setting for nova
     */
    void nova_speedGlobal(uint16_t speed);
    /**
     * @brief       Check nova status (nova has 7 status registers form 1-7)
     * @param add   Address of status register
     * @param value Register status
     */
    int nova_status(int add, bool &value);
    /**
     * @brief           Check nova multistate (nova has 7 status registers form 1-7)
     * @param add       First status register address
     * @param length    Number of rgisters to read
     * @param buff      Register states
     */
    int nova_multistate(int add, int length, bool *buff);
    /**
     * @brief               Get 6 values ​​x, y, z, Rx, Ry, Rz
     * @param Point         Condinates after conversion
     * @param Point_initial Scan coordinates
     * @param isCheck       True or false check variable
     * @param Timestamp     Time stamp
     */

    int nova_readCoordinates(std::vector<double>& Point,std::vector<double>& Point_initial, uint16_t& isCheck, int& Timestamp);
    /**
     * @brief      Get coordinates from camera
     * @param Point Variable store coordinates current
     */
    int nova_getCoorcurrent(std::vector<double>& Point);
    /**
     * @brief      Read time stamp nova
     * @param time Time stamp of nova
     */
    int nova_timeStamp(uint64_t& time);
    /**
     * @brief      
     */
    int nova_createArea(void);
    /**
     * @brief      Turn on tool
     */
    void nova_onTool(void);
    /**
     * @brief      Turn off tool
     */
    void nova_offTool(const double delay);
    /**
     * @brief      Move robot nova
     * @param Point Target coordinates
     * @param speed cai toc do cho tay may
     * @param mode 0 - moveL, const int mode = 1
     *             1 - moveJ 
     */
    int nova_movePoint(std::vector<double>& Point,const int speed, const int mode = 1);
    /**
     * @brief      Check z 
     * 
     * @param x
     * @param y
     * @param z 
     * @param choose 0 - z min theo mac dinh 200 mm
     *               1 - z min theo tham so duoc set tu CCBOX    
     */
    double nova_check_z(double& x, double& y, double& z, int choose = 0);

    int movePointRunning_;
    bool *run_ptr_;


private:
    /**
     * @brief     Robot move done notification
     */
    bool nova_movingDone(void);
    /**
     * @brief     Allow the robot to start moving
     */
    void nova_allowMove(void);
    /**
     * @brief     Dismiss the robot moving
     */
    void nova_dismissMove(void);
    /**
     * @brief         Send 6 values ​​x, y, z, Rx, Ry, Rz
     * @param mode 0 - moveJ, const int mode = 0
     *             1 - moveL
     * @param Point   Coordinates to sent
     */
    int nova_sendCoordinates(std::vector<double>& Point);
    /**
     * @brief       Check point coordinates    
     * return 1:    Point within range
     * return 0:    Point is not range
     * return -1:   Error
     * @param x 
     * @param y
     * @param z
     */
    int workArealimit(double x, double y, double z);
    /**
     * @brief               Convert object coordinates to robot tool condinates
     * @param Point         Condinates after conversion
     * @param Point_initial Scan coordinates
     * @param Coor          Object coordinates
     */
    int convert(std::vector<double>& Point,std::vector<double>& Point_initial, std::vector<double>& Coor);
    /**
     * @brief      Get z min and return z min
     */
    double getZmin(void);

    /* data */
    enum WorkAreaStatus {
    WITHIN_LIMIT = 1,
    OUT_OF_LIMIT = 0,
    ERROR = -1
    };

    boost::shared_ptr<modbus> nova_;
    bool inter_var;
    
    bool send;
    double a1,b1,c1;
    double a2,b2;
    double r_max;
    double r_min;
    double z_min_static;
    
    // double safety_factor;

    double x_deviation;
    double y_deviation;
    double z_deviation;

    bool flag_move;
    uint64_t mode_store;
};

#endif // NOVA5_CONTROL_H