#include"nova5_control/nova5_modbus.h"



cnt_nova5::cnt_nova5(std::string ip_address, int port) 
 : nova_(nullptr),
    movePointRunning_(0),
    run_ptr_(nullptr)
{
    nova_ = boost::make_shared<modbus>(ip_address, port);
    inter_var = false;


    // Declare the values of the sphere O
    a1 = 0.0;
    b1 = 0.0;
    c1 = 0.0;
    r_max = 750.0;

    // Declare the values of cylinder I
    a2 = 0.0;
    b2 = 0.0;
    r_min = 250.0;

    // Deviation of camera 
    x_deviation = 8 + (350/2.0);
    y_deviation = 100.0 + (234.5/2.0);
    z_deviation = 140.5 + 5 - 28.0 + 3.5; // Chiều cao tool + chiều cao camera + dung sai

    z_min_static = 170;
    mode_store = 0;
    flag_move = 0;
}

cnt_nova5::~cnt_nova5()
{
    nova_stopProgram();
    nova_close();
}

int cnt_nova5::nova_connect(void)
{
    ROS_INFO("Nova is connecting.......");
    try
    {
        
        if(!nova_->modbus_connect()) 
        {
            ROS_ERROR("Nova connection is error");
            return 0;
        } 
        else 
        {
            ROS_INFO("Nova is Connected");
            return 1;
        } 

    }

    catch (...) 
    {
        ROS_ERROR_STREAM("Caught unknown exception");
        return -1;
    }   
    
}

bool cnt_nova5::nova_checkConnected(void)
{
    try
    {
        return nova_->_connected;
    }
    catch (const std::exception& e) 
    {
        ROS_ERROR_STREAM(e.what());
    } 
    catch (...) 
    {
        ROS_ERROR_STREAM("Caught unknown exception");
    }  
}

void cnt_nova5::nova_disable(void)
{
    nova_dismissMove();
    nova_stopProgram();
    nova_->modbus_write_coil(COIL_DISABLE, true);
    ros::Duration(0.1).sleep();
    nova_->modbus_write_coil(COIL_DISABLE, false);
    ros::Duration(0.6).sleep();
}

void cnt_nova5::nova_close(void)
{
    ROS_INFO("Close robot");
    nova_disable();
    nova_->modbus_close();
}

int cnt_nova5::workArealimit(double x, double y, double z)
{
    double dO_instant = z + (1/(2*r_max)) * (pow((x + a1), 2) + pow((y + b1), 2));

    // INNER BOUNDING CYLINDER
    double dI_instant = sqrt(pow((x + a2),2) + pow((y + b2),2));

    if(dO_instant < r_max)
    {
        return((dI_instant > r_min) ? WITHIN_LIMIT : OUT_OF_LIMIT);
    }
    else return ERROR;
    
}

int cnt_nova5::nova_sendCoordinates(std::vector<double>& Point)
{
    try
    {
        if(Point.size() != 6U) throw std::invalid_argument("A point has 6 parameters");
        if(workArealimit(Point[0],Point[1],Point[2]) == 0) throw std::out_of_range("Point beyond work area");
        if(workArealimit(Point[0],Point[1],Point[2]) == ERROR) throw std::runtime_error("Unknown error");

        std::array<uint16_t,24> part;
        std::vector<double> Point_send(6);

        if(Point[2] < z_min_static)
        {
            Point_send[0] = Point[0];
            Point_send[1] = Point[1];
            Point_send[2] = z_min_static;
            Point_send[3] = Point[3];
            Point_send[4] = Point[4];
            Point_send[5] = Point[5];
        }
        else 
        {
            Point_send[0] = Point[0];
            Point_send[1] = Point[1];
            Point_send[2] = Point[2];
            Point_send[3] = Point[3];
            Point_send[4] = Point[4];
            Point_send[5] = Point[5];
        }

        for (int i = 0; i < 6U; ++i) 
        {
            std::memcpy(&part[i*4], reinterpret_cast<uint8_t*>(&Point_send[i]), sizeof(double));
        }

        if(nova_->modbus_write_registers(HOLDING_TARGET_POINT_START,24,part.data()) == 0) return 1;
        else return 0;
    }
    catch(const std::invalid_argument& e)
    {
        ROS_ERROR_STREAM("ERROR: " << e.what());
        ros::Duration(TIME_ERR).sleep();
        return ERR;
    }
    catch(const std::out_of_range& e)
    {
        ROS_ERROR_STREAM("ERROR: " << e.what());
        ros::Duration(TIME_ERR).sleep();
        return ERR;
    }
        catch(const std::runtime_error& e)
    {
        ROS_ERROR_STREAM("ERROR send: " << e.what());
        ros::Duration(TIME_ERR).sleep();
        return ERR;
    }
    
}

int cnt_nova5::convert(std::vector<double>& Point, std::vector<double>& Point_initial, std::vector<double>& Coor)
{
    if(Point.size() == 6U)
    {
        for(int i = 0; i < Point.size(); ++i)
        {
            Point[i] = Point_initial[i];
        }

        Point[0] = Point[0] + x_deviation - Coor[1] - (x_deviation - Coor[1])*0.15;     // x
        Point[1] = Point[1] - y_deviation + Coor[2];                                    //y
        Point[2] = Point[2] + z_deviation - Coor[0];                                    // z
        Point[3] = Point[3];                                                            // Rx
        Point[4] = Point[4];                                                            // Ry
        Point[5] = Point[5] + 90.0 - Coor[3];                                             // Rz

        return 1;
    }
    else return ERR;


}

int cnt_nova5::nova_readCoordinates(std::vector<double>& Point,std::vector<double>& Point_initial, uint16_t& isCheck, int& Timestamp)
{
    try
    {
        std::array<uint16_t,21> register_buffer;
        std::vector<double> Point_receive(4); 
        
        if(Point.size() != 6U) throw std::invalid_argument("A point has 6 parameters");
        if (register_buffer.size() < 21) throw std::runtime_error("Buffer size is too small");

        int status = nova_->modbus_read_holding_registers(HOLDING_CAMERA_POINT_START,21,register_buffer.data());

        if (status == 0 ) 
        {
            // ROS_INFO("Successfully read registers coordinates");

            for (int i = 0; i < 4U; ++i) 
            {
                memcpy(&Point_receive[i], &register_buffer[i * 4U], sizeof(double));
                // ROS_INFO("Double value %d: %f", i, Point_receive[i]);
            }
            isCheck = register_buffer[16];
            Timestamp = (static_cast<int>(register_buffer[17]) << 16) | register_buffer[18];

            // convert(Point,Point_initial,Point_receive);
            if(Point_receive[1] == 0 && Point_receive[2] == 0 && isCheck == 0) throw std::runtime_error("Camera error");
            if(convert(Point,Point_initial,Point_receive) == -1) throw std::invalid_argument("A point has 6 parameters");
            if(workArealimit(Point[0],Point[1],Point[2]) == 0) throw std::out_of_range("Point beyond work area");
            if(workArealimit(Point[0],Point[1],Point[2]) == -1) throw std::runtime_error("Unknown error");

            return 1;

        } 
        else 
        {
            // ROS_ERROR("Failed to read registers coordinates");
            return 0;
        }
    }
    catch(const std::invalid_argument& e)
    {
        ROS_ERROR_STREAM("ERROR: " << e.what());
        ros::Duration(TIME_ERR).sleep();
        return ERR;
    }
    catch(const std::out_of_range& e)
    {
        ROS_ERROR_STREAM("ERROR: " << e.what());
        ros::Duration(TIME_ERR).sleep();
        return ERR;
    }
    catch(const std::runtime_error& e)
    {
        ROS_ERROR_STREAM("ERROR: " << e.what());
        ros::Duration(TIME_ERR).sleep();
        return ERR;
    }
}

int cnt_nova5::nova_status(int add, bool &value)
{
    try
    {
        if(add < DISCRETE_STOP_STATE || add > DISCRETE_REMOTE_CONTROL) throw std::out_of_range("Register does not exist");
        int status = nova_->modbus_read_coil(add, value);
        if(status == 0)
        {
            // ROS_INFO("Successfully read register status");
            return 1;
        }
        else 
        {
            // ROS_ERROR("Failed to read register status %d", status);
            return 0;
        }
    }
    catch(const std::out_of_range& e)
    {
        ROS_ERROR_STREAM("ERROR: " << e.what());
        ros::Duration(TIME_ERR).sleep();
        return ERR;
    }
    catch(...)
    {
        ROS_ERROR_STREAM("Caught unknown exception");
        return ERR;
    }
    
}

int cnt_nova5::nova_multistate(int add, int length, bool *buff)
{
    try
    {
        if(add < DISCRETE_STOP_STATE || add > DISCRETE_REMOTE_CONTROL) throw std::out_of_range("Register does not exist");
        if(length > (DISCRETE_REMOTE_CONTROL - add) || length <= 0) throw std::out_of_range("Invalid length value");

        int status = nova_->modbus_read_coils(add, length, buff);
        if(status == 0)
        {
            // ROS_INFO("Successfully read register multistate");
            return 1;
        }
        else 
        {
            // ROS_ERROR("Failed to read register multistate %d", status);
            return 0;
        }
    }
    catch(const std::out_of_range& e)
    {
        ROS_ERROR_STREAM("ERROR: " << e.what());
        ros::Duration(TIME_ERR).sleep();
        return ERR;
    }
    catch(...)
    {
        ROS_ERROR_STREAM("Caught unknown exception");
        return ERR;
    }
}

int cnt_nova5::nova_timeStamp(uint64_t& time)
{
    try
    {
        std::array<uint16_t,4> register_buffer;
        int status = nova_->modbus_read_holding_registers(INPUT_TIMESTAMP_START,4, register_buffer.data());
        if(status == 0)
        {
            // ROS_INFO("Successfully read register time stamp");

            memcpy(&time,&register_buffer,sizeof(uint64_t));

            return 1;
        }
        else
        {
            // ROS_ERROR("Failed to read register time stamp %d", status);
            return 0;
        }
    }
    catch(const std::exception& e)
    {
        ROS_ERROR_STREAM("Caught unknown exception");
        return ERR;
    }

}

double cnt_nova5::nova_robotMode(void)
{
    uint64_t mode_uint64;
    std::array<uint16_t,4> register_buffer;
    int status = nova_->modbus_read_input_registers(INPUT_ROBOT_MODE_START,4,register_buffer.data());
    
    if(status == 0)
    {
        // ROS_INFO("Successfully read register robot mode");

        memcpy(&mode_uint64, &register_buffer, sizeof(uint64_t));
        if(mode_uint64 != mode_store) ROS_INFO("Mode: %ld",mode_uint64);
        mode_store = mode_uint64;
        double mode_double = (double)mode_uint64;
        return mode_uint64;
    }
    else 
    {
        // ROS_ERROR("Failed to read register robot mode %d", status);
        return ERR;
    }

}

int cnt_nova5::nova_powerOn(void)
{
        static int count = 0;
        if(run_ptr_ == nullptr) run_ptr_ = &inter_var;
        while(ros::ok() && nova_robotMode() == 3 && *run_ptr_)
        {
            ROS_INFO("Wait 25s for robot to power on...");
            if(nova_statusAlarm() == 1)
            { 
                nova_->modbus_write_coil(COIL_CLEAR_ALARM, true);
                ros::Duration(0.01).sleep();
            }

            nova_->modbus_write_coil(COIL_POWER_ON, true);
            ros::Duration(20).sleep();

            if(nova_robotMode() == 3)
            {
                nova_->modbus_write_coil(COIL_CLEAR_ALARM, false);
                ros::Duration(0.01).sleep();
                nova_->modbus_write_coil(COIL_POWER_ON, false);
                ros::Duration(0.01).sleep();
                count++;
                if(count>1)
                {
                     ROS_ERROR("Power on failed...");
                     count = 0;
                     return -1;
                     break;
                }
            }
            else
            {
                ROS_INFO("Power on successfully");
                nova_->modbus_write_coil(COIL_CLEAR_ALARM, false);
                ros::Duration(0.01).sleep();
                nova_->modbus_write_coil(COIL_POWER_ON, false);
                ros::Duration(0.01).sleep();
                return 1;
                break;
            }
            ros::spinOnce();
        }
        

    return 0;
}

bool cnt_nova5::nova_enable(void)
{
        int status_clear_alarm = nova_->modbus_write_coil(COIL_CLEAR_ALARM, true);
        ros::Duration(0.01).sleep();
        int status_enable = nova_->modbus_write_coil(COIL_ENABLE, true);
        ros::Duration(0.6).sleep();

        nova_->modbus_write_coil(COIL_CLEAR_ALARM, false);
        ros::Duration(0.01).sleep();
        nova_->modbus_write_coil(COIL_ENABLE, false);
        ros::Duration(0.01).sleep();

        double mode = nova_robotMode();

        if(mode >= 5 && status_clear_alarm == 0 && status_enable == 0 )
        {
            ROS_INFO("Enable done");
            return true;
        }
        else 
        {
            ROS_ERROR("Enable failed");
            return false;
        }
}

void cnt_nova5::nova_allowMove(void)
{
    nova_->modbus_write_coil(COILD_MOVE,true);
    ros::Duration(0.3).sleep();
}

void cnt_nova5::nova_dismissMove(void)
{
    nova_->modbus_write_coil(COILD_MOVE,false);
    ros::Duration(0.01).sleep();
}

void cnt_nova5::nova_stopProgram(void)
{
    nova_->modbus_write_coil(COIL_STOP, true);
    ros::Duration(0.01).sleep();
    nova_->modbus_write_coil(COIL_STOP, false);
    ros::Duration(0.5).sleep();
}

bool cnt_nova5::nova_startProgram(void)
{
    flag_move = 0;
    nova_dismissMove();
    nova_offTool(0.01);
    if(run_ptr_ == nullptr) run_ptr_ = &inter_var;
    int i = 0;
    while(i < 4 && ros::ok() && *run_ptr_)
    {
        nova_->modbus_write_coil(COIL_START, true);
        ros::Duration(0.3).sleep();
        nova_->modbus_write_coil(COIL_START, false);
        ros::Duration(0.02).sleep();
        uint64_t mode = nova_robotMode();
        if(mode == 7) break;
        
        i++;
    }
    uint64_t mode = nova_robotMode();
    if(mode == 7) return true;
    else return false;
}

bool cnt_nova5::nova_movingDone(void)
{

    bool status;
    nova_->modbus_read_coil(COILD_MOVING_DONE, status);
    // ROS_INFO("STATUS movingDone %d",status);
    return status;

}

bool cnt_nova5::nova_statusAlarm(void)
{
    bool alarm = false;
    nova_->modbus_read_input_bit(DISCRETE_ALARM_STATE,alarm);
    // ROS_INFO("ALARM ST: %x",alarm);

    return alarm;
}

void cnt_nova5::nova_clearAlarm()
{
    nova_->modbus_write_coil(COIL_CLEAR_ALARM, true);
    ros::Duration(0.1).sleep();
    nova_->modbus_write_coil(COIL_CLEAR_ALARM, false);
    ros::Duration(0.01).sleep();
}

void cnt_nova5::nova_onTool(void)
{
    ROS_INFO("ON TOOL");
    nova_->modbus_write_coil(COILD_TOOL, true);
    ros::Duration(2).sleep();
}

void cnt_nova5::nova_offTool(const double delay)
{
    ROS_INFO("OFF TOOL");
    nova_->modbus_write_coil(COILD_TOOL, false);
    ros::Duration(delay).sleep();
}

void cnt_nova5::nova_speedGlobal(uint16_t speed)
{
    nova_->modbus_write_register(HOLDING_GLOBAL_SPEED,speed);
    // ros::Duration(0.01).sleep();
}

int cnt_nova5::nova_getCoorcurrent(std::vector<double>& Point)
{
    try
    {
        if(Point.size() != 6U) throw std::invalid_argument("Coordinates have 6 parameters");
        std::array<uint16_t,24> register_buffer;
        for(int i = 0; i <= 5; i++)
        {
            ros::Duration(1).sleep();
            int status = nova_->modbus_read_holding_registers(HOLDING_POINT_CURRENT_START,24,register_buffer.data());
            if (status == 0)
            {
                for(int  i = 0; i < 6U; ++i)
                {
                    memcpy(&Point[i], &register_buffer[i * 4U], sizeof(double));
                }
                if((Point[0] != 0 && Point[1] != 0 && Point[2] !=0) || 
                (!ros::ok()) || nova_robotMode() != 7 || !nova_checkConnected() && *run_ptr_ == false) break;
                if(i == 5) return 0;
            }
            else return ERR;
        }
        return 0;
    }
    catch(const std::invalid_argument& e)
    {
        ROS_ERROR_STREAM("ERROR: " << e.what());
        ros::Duration(TIME_ERR).sleep();
        return ERR;
    }
    catch(const std::exception& e)
    {
        std::cerr << e.what() << '\n';
        return ERR;
    }    
}

double cnt_nova5::getZmin(void)
{
    std::array<uint16_t,4> register_buffer;
    double Z_min;

    nova_->modbus_read_holding_registers(HOLDING_Z_MIN_USER_SET_START,4,register_buffer.data()); 
    memcpy(&Z_min, &register_buffer, sizeof(double));
    ROS_INFO("Z min : %f",Z_min);
    return Z_min;
}

double cnt_nova5::nova_check_z(double& x, double& y, double& z, int choose)
{
    static double z_min_store = getZmin();
    static double z_min;

    double z_max = r_max - 10 - (1/(2*r_max)) * (pow((x + a1), 2) + pow((y + b1), 2));

    if(choose == 1) z_min = z_min_static;
    else if(choose == 0) z_min = z_min_store;

    if(z_max > z_min)
    {
        if(z > z_min && z < z_max) return z;
        else if(z <= z_min) return z_min;
        else if(z >= z_max) return z_max;
        return ERR;
    }
    else return z_min;


}

int cnt_nova5::nova_movePoint(std::vector<double>& Point,const int speed, const int mode)
{

    try
    {
        /* code */
        if(Point.size() != 6U) throw std::invalid_argument("A point has 6 parameters - move Point");
        if(workArealimit(Point[0],Point[1],Point[2]) == 0) throw std::out_of_range("Point beyond work area - move Point");
        if(workArealimit(Point[0],Point[1],Point[2]) == -1) throw std::runtime_error("Unknown error - move Point");

        movePointRunning_ = 1;
        
        if(flag_move == 0)
        {
            // Chọn chế độ di chuyển moveJ move L
            nova_->modbus_write_coil(COILD_MOVE_MODE, mode);

            // Cài đặt tốc độ toàn cục cho tay máy
            nova_speedGlobal(speed);

            // Gửi tọa độ mục tiêu và xác nhận cho robot di chuyển đến tọa độ mục tiêu cho robot tay máy
            if(nova_sendCoordinates(Point) == 1) nova_allowMove();
            else return 0;
            flag_move = 1; 
        }
        
        // Chờ cho robot di chuyển xong
        if(nova_movingDone() == 1)
        {
            nova_dismissMove();
            flag_move = 0;
            return 1;
        }
        // If no movement completed, return an error
        return 0;
    }
    catch(const std::invalid_argument& e)
    {
        ROS_ERROR_STREAM("ERROR: " << e.what());
        ros::Duration(TIME_ERR).sleep();
        movePointRunning_ = ERR;
        return ERR;
    }
    catch(const std::out_of_range& e)
    {
        ROS_ERROR_STREAM("ERROR: " << e.what());
        ros::Duration(TIME_ERR).sleep();
        movePointRunning_ = ERR;
        return ERR;
    }
        catch(const std::runtime_error& e)
    {
        ROS_ERROR_STREAM("ERROR send: " << e.what());
        ros::Duration(TIME_ERR).sleep();
        movePointRunning_ = ERR;
        return ERR;
    }
    catch(const std::exception& e)
    {
        ROS_ERROR_STREAM("ERROR: " << e.what());
        ros::Duration(TIME_ERR).sleep();
        movePointRunning_ = ERR;
        return ERR;
    }
}