#include "nova5_control/imr_nova_control.h"

imr_nova_control::imr_nova_control()
 : enable_(nullptr),
    ok_count_max_(nullptr),
    ng_count_max_(nullptr),
    continue_flag_(nullptr),
    go_home_flag_(nullptr),
    power_on_flag_(nullptr)
{
    modeThreadRunning_ = false; // Variable to check if the thread is running
    homeThreadRunning_ = false; // Variable to check if the thread is running
    mode_ = 0;
}


imr_nova_control::~imr_nova_control()
{
    // ROS_WARN("Destructor");
    if (thr_main_ && thr_main_->joinable())
    {
        thr_main_->join();
    }
    if (thr_go_home_ && thr_go_home_->joinable())
    {
        thr_go_home_->join();
    }

    // delete ok_count_max_;  // Giải phóng bộ nhớ
    // delete ng_count_max_;
}


char imr_nova_control::getKey()
{
    struct termios oldt, newt;
    char ch;
    tcgetattr(STDIN_FILENO, &oldt);
    newt = oldt;
    newt.c_lflag &= ~(ICANON | ECHO);
    tcsetattr(STDIN_FILENO, TCSANOW, &newt);
    ch = getchar();
    tcsetattr(STDIN_FILENO, TCSANOW, &oldt);
    return ch;
}


void imr_nova_control::startModeThread()
{
    if(enable_ != NULL && ok_count_max_ != NULL && ng_count_max_ != NULL && continue_flag_ != NULL && go_home_flag_ != NULL && power_on_flag_ != NULL)
    {
        if (!modeThreadRunning_ && !homeThreadRunning_)
        {
            modeThreadRunning_ = true; // Set the flag to start the thread
            thr_main_ = std::make_shared<std::thread>(&imr_nova_control::imr_dobot_control_thread, this);
            ROS_WARN("modeThreadRunning_: %x", modeThreadRunning_);
        }
        else
        {
            ROS_WARN("Please stop go home before starting robot");
        }
    }
    else 
    {
        statusCode_ = imr_nova_control::ROBOT_ERROR;
    }
}


void imr_nova_control::startHomeThread()
{
    if(enable_ != NULL)
    {
        if (!homeThreadRunning_ && !modeThreadRunning_)
        {
            homeThreadRunning_ = true; // Set the flag to start the thread
            thr_go_home_ = std::make_shared<std::thread>(&imr_nova_control::imr_dobot_go_home_thread, this);
            ROS_WARN("homeThreadRunning_: %x", homeThreadRunning_);
        }
        else 
        {
            ROS_WARN("Please stop robot before going home");
        }
    }
    else 
    {
        statusCode_ = imr_nova_control::ROBOT_ERROR;
    }
}


void imr_nova_control::stopModeThread()
{
    if (modeThreadRunning_ && !homeThreadRunning_)
    {
        ROS_WARN("cancel thread");
        modeThreadRunning_ = false; // Set the flag to stop the thread
        // Join threads to ensure proper cleanup
        if (thr_main_ && thr_main_->joinable())
        {
            thr_main_->join();
        }
    }
    // delete ok_count_max_;  // Giải phóng bộ nhớ
    // delete ng_count_max_;
}


void imr_nova_control::stopHomeThread()
{
    if (homeThreadRunning_ && !modeThreadRunning_)
    {
        ROS_WARN("cancel thread");
        homeThreadRunning_ = false; // Set the flag to stop the thread
        // Join threads to ensure proper cleanup
        if (thr_go_home_ && thr_go_home_->joinable())
        {
            thr_go_home_->join();
        }
    }
}


void imr_nova_control::imr_dobot_control_thread()
{
    statusCode_ = imr_nova_control::ROBOT_WAITING;

    std::vector<double> STORE_COOR(6);
    std::vector<double> Point(6);
    std::vector<double> Point_current(6);
    std::vector<double> Center_temp(6);
    std::vector<double> Point_temp1(6);
    std::vector<double> Point_temp2(6);
    std::vector<double> Point_temp3(6);

    
    int timeStamp_cam = 0;
    int count = 0, count_1 = 0xffff;
    int center_count = 0;
    int get_point_current =0;

    uint16_t isCheck = 0;
    uint16_t speed = 0;

    unsigned int count_ok = 0;
    unsigned int count_ng = 0;

    bool connect = false;
    bool drag = false;
    bool flag_home = false;
    bool temp = false;
    
    // Khởi tạo 
    std::shared_ptr<cnt_nova5> nova5_ptr_ = nullptr;
    nova5_ptr_ = std::make_shared<cnt_nova5>(IP,PORT);

    nova5_ptr_->run_ptr_ = enable_;


    // Kết nối đến robot nova
    connect = nova5_ptr_->nova_connect();

    if(connect)
    {
        double mode = nova5_ptr_->nova_robotMode();
        mode_ = mode;
        if(mode == 3)
        {
            statusCode_ = imr_nova_control::ROBOT_POWER_ON;

            if(nova5_ptr_->nova_powerOn() == -1)
            {
                statusCode_ = imr_nova_control::ROBOT_ERROR;

                ROS_ERROR("Please check robot arm");
                modeThreadRunning_ = false;// biến tạm để cho thấy robot bị lỗi, thay thành các biến phản hồi lỗi
            }
        }
        if(mode == 9)
        {
            nova5_ptr_->nova_clearAlarm();

            if(nova5_ptr_->nova_statusAlarm())
            {
                statusCode_ = imr_nova_control::ROBOT_ERROR;

                ROS_ERROR("Please check robot arm or try again!");
                modeThreadRunning_ = false;// biến tạm để cho thấy robot bị lỗi, thay thành các biến phản hồi lỗi
            }
        } 
        if(modeThreadRunning_)
        {
            // Sau khi kết nối thành công cần enable robot
            if(nova5_ptr_->nova_enable())
            {

                // Sau khi enable robot thành công cần bắt đầu chương trình của ccbox
                bool status_start_program = nova5_ptr_->nova_startProgram();
                if(status_start_program) statusCode_ = imr_nova_control::ROBOT_RUNNING;
                else
                {
                    statusCode_ = imr_nova_control::ROBOT_ERROR;

                    ROS_ERROR("Please check robot arm");
                    modeThreadRunning_ = false;
                }
            }
            else
            {
                statusCode_ = imr_nova_control::ROBOT_ERROR;

                ROS_ERROR("Please check robot arm");
                modeThreadRunning_ = false;
            }
        }
    }

    else 
    {
        // alarm = nova5_ptr_->nova_statusAlarm();
        ROS_WARN("Robot is reconnecting with maximum time 15 minutes...");
        for(int i=0; i<180U; ++i)
        {
            statusCode_ = imr_nova_control::ROBOT_RECONNECTING;

            if(enable_ == NULL || !(*enable_))
            {
                statusCode_ = imr_nova_control::ROBOT_ERROR;
                
                modeThreadRunning_ = false;
                break;
            }
            connect = nova5_ptr_->nova_connect();
            if(connect || !ros::ok())
            {
                while(ros::ok()
                && (enable_ != NULL && (*enable_))
                )
                {
                    double mode = nova5_ptr_->nova_robotMode();
                    double alarm = nova5_ptr_->nova_statusAlarm();
                    mode_ = mode;
                    if(alarm && mode == 9)
                    {
                        statusCode_ = imr_nova_control::ROBOT_ERROR;

                        ROS_ERROR("Kinematic ERROR! Please check robot!");
                        modeThreadRunning_ = false;// biến tạm để cho thấy robot bị lỗi, thay thành các biến phản hồi lỗi
                        break;
                    }
                    if(mode == 3)
                    {
                        statusCode_ = imr_nova_control::ROBOT_POWER_ON;

                        if(nova5_ptr_->nova_powerOn() == -1)
                        {
                            statusCode_ = imr_nova_control::ROBOT_ERROR;

                            ROS_ERROR("Please check robot arm");
                            modeThreadRunning_ = false;// biến tạm để cho thấy robot bị lỗi, thay thành các biến phản hồi lỗi
                            break;
                        }
                    } 
                    if(mode == 4) break;
                    ros::Duration(5).sleep();
                }

                if(modeThreadRunning_)
                {
                    // Sau khi kết nối thành công cần enable robot
                    if(nova5_ptr_->nova_enable())
                    {
                        // Sau khi enable robot thành công cần bắt đầu chương trình của ccbox
                        bool status_start_program = nova5_ptr_->nova_startProgram();
                        if(status_start_program) statusCode_ = imr_nova_control::ROBOT_RUNNING;
                        else
                        {
                            statusCode_ = imr_nova_control::ROBOT_ERROR;

                            ROS_ERROR("Please check robot arm");
                            modeThreadRunning_ = false;
                        }
                    }
                    else
                    {
                        statusCode_ = imr_nova_control::ROBOT_ERROR;

                        ROS_ERROR("Please check robot arm");
                        modeThreadRunning_ = false;
                    }
                }

                break;
            }
            ros::Duration(5).sleep();
        }
        
    }

    ros::Rate loop_rate(100);


    //===========================Vòng lặp chính========================//
    //=================================================================//
    //=================================================================//
    gotoTop:

    while(ros::ok() && connect && modeThreadRunning_
    && (enable_ != NULL && (*enable_) && (*ok_count_max_ > 0 || *ng_count_max_ > 0))
    )
    {
        // ROS_WARN("statusCode_: %f",statusCode_);
        static bool checkconnect = false;
        static bool alarm = false;

        //====Kiểm tra kết nối và trạng thái alarm====//
        checkconnect = nova5_ptr_->nova_checkConnected();
        alarm = nova5_ptr_->nova_statusAlarm();
        //============================================//

        if(checkconnect && !alarm)
        {

            // Lấy trạng thái robot
            double robotmode = nova5_ptr_->nova_robotMode();
            mode_ = robotmode;
            //===Kiểm tra trạng thái robot===//
            //===============================//
            if( robotmode == 3)
            {
                ros::Duration(0.5).sleep();
                
                bool alarm = nova5_ptr_->nova_statusAlarm();
                if(alarm)
                {
                    statusCode_ = imr_nova_control::ROBOT_IN_ALARM_STATE;

                    for(int i=0; i<3U; ++i)
                    {
                        nova5_ptr_->nova_clearAlarm();
                        if(nova5_ptr_->nova_statusAlarm() == 0 || !(*enable_) || !ros::ok())
                        {
                            statusCode_ = imr_nova_control::ROBOT_RUNNING;
                            break;
                        }
                        if(i == 2)
                        {
                            double mode = nova5_ptr_->nova_robotMode();
                            mode_ = mode;
                            if(mode == 3)
                            {
                                statusCode_ = imr_nova_control::ROBOT_CHECK_EMERGENCY_STOP_BUTTON;

                                ROS_ERROR("Please check button E-Stop on robot IMR!");
                                goto gotoTop;
                            }
                            else
                            {
                                ROS_ERROR("Robot is in alarm state!");
                            }
                        }
                        ros::Duration(0.2).sleep();
                    }
                }

                statusCode_ = imr_nova_control::ROBOT_POWER_ON_WAITTING;

                ROS_WARN("OPTION: power on");
                while(ros::ok() && modeThreadRunning_
                && (enable_ != NULL && (*enable_))
                )
                {
                    double mode = nova5_ptr_->nova_robotMode();
                    mode_ = mode;
                    if(mode != 3) goto gotoTop;

                    if(*power_on_flag_ == true)
                    {
                        break;
                    }
                }

                // Power on robot
                if(*power_on_flag_ == true)
                {
                    statusCode_ = imr_nova_control::ROBOT_POWER_ON;
                    
                    if(nova5_ptr_->nova_powerOn() == 1)
                    {
                        // Sau khi kết nối thành công cần enable robot
                        if(nova5_ptr_->nova_enable())
                        {
                            // Sau khi enable robot thành công cần bắt đầu chương trình của ccbox
                            bool status_start_program = nova5_ptr_->nova_startProgram();
                            if(status_start_program)
                            {
                                if(count_ok >= *ok_count_max_ && count_ng >= *ng_count_max_)
                                {
                                    statusCode_ = imr_nova_control::ROBOT_ERROR;
                                    modeThreadRunning_ = false;
                                    break;
                                }
                                statusCode_ = imr_nova_control::ROBOT_RUNNING;
                            } 
                            else
                            {
                                statusCode_ = imr_nova_control::ROBOT_ERROR;

                                ROS_ERROR("Please check robot arm");
                                modeThreadRunning_ = false;
                            }

                            temp = false;
                            get_point_current = 0;
                            count = 0;
                            center_count = 0;
                            flag_home = false;
                        }
                        else
                        {
                            statusCode_ = imr_nova_control::ROBOT_ERROR;

                            ROS_ERROR("Please check robot arm");
                            modeThreadRunning_ = false;
                            continue;
                        }
                    }
                    else 
                    {
                        statusCode_ = imr_nova_control::ROBOT_ERROR;

                        ROS_ERROR("Please check robot arm");
                        modeThreadRunning_ = false;
                        continue;
                    }
                }
            }
            else if(robotmode == 4)
            {
                ROS_WARN("Robot is in powered on mode, not enabled");
                modeThreadRunning_ = false;
                continue;
            }
            else if(robotmode == 6)
            {
                statusCode_ = imr_nova_control::ROBOT_DRAG;

                ROS_WARN("Robot is in drag mode");
                drag = true;
            }
            else if(robotmode == 9)
            {
                ros::Duration(2).sleep();
                double mode = nova5_ptr_->nova_robotMode();
                mode_ = mode;
                if(mode == 9)
                {
                    statusCode_ = imr_nova_control::ROBOT_ERROR;

                    ROS_ERROR("Kinematic error of robot nova");
                    // modeThreadRunning_ = false;
                    continue;
                }
                else drag = true;
            }

            if(drag == true && robotmode == 5)
            {
                statusCode_ = imr_nova_control::ROBOT_WAITING_SELECT_CONTINUE_OR_GO_HOME;
                
                ROS_WARN("OPTION: 1 to continute");
                ROS_WARN("OPTION: 2 to go home");
                while(ros::ok() && modeThreadRunning_
                && (enable_ != NULL && (*enable_))
                )
                {
                    double mode = nova5_ptr_->nova_robotMode();
                    mode_ = mode;
                    if(mode != 5) goto gotoTop;
                    if(*continue_flag_ == true || *go_home_flag_ == true)
                    {
                        break;
                    }
                }
                /*Kiểm tra nếu key = n hoặc N thì thực hiện lệnh tiếp tục*/
                if(*continue_flag_ == true)
                {
                    // Sau khi enable robot thành công cần bắt đầu chương trình của ccbox
                    bool status_start_program = nova5_ptr_->nova_startProgram();
                    if(status_start_program)
                    {
                        if(count_ok >= *ok_count_max_ && count_ng >= *ng_count_max_)
                        {
                            statusCode_ = imr_nova_control::ROBOT_COMING_HOME;

                            drag = false;
                            get_point_current = 0;
                            center_count = 0;
                            ROS_INFO("Robot is comming home...");
                            flag_home = true;
                        }
                        else
                        {
                            statusCode_ = imr_nova_control::ROBOT_RUNNING;

                            temp = false;
                            drag = false;
                            get_point_current = 0;
                            count = 0;
                            center_count = 0;
                            flag_home = false;
                        }
                    } 
                    else
                    {
                        statusCode_ = imr_nova_control::ROBOT_ERROR;

                        temp = false;
                        drag = false;
                        get_point_current = 0;
                        count = 0;
                        center_count = 0;
                        flag_home = false;

                        ROS_ERROR("Please check robot arm");
                        modeThreadRunning_ = false;
                    }
                }
                /*Kiểm tra nếu key = h hoặc H thì thực hiện lệnh về home */
                else if(*go_home_flag_ == true)
                {
                    // Sau khi enable robot thành công cần bắt đầu chương trình của ccbox
                    bool status_start_program = nova5_ptr_->nova_startProgram();
                    if(status_start_program) statusCode_ = imr_nova_control::ROBOT_COMING_HOME;
                    else
                    {
                        statusCode_ = imr_nova_control::ROBOT_ERROR;

                        ROS_ERROR("Please check robot arm");
                        modeThreadRunning_ = false;
                    }
                    drag = false;
                    get_point_current = 0;
                    center_count = 0;
                    ROS_INFO("Robot is comming home...");
                    flag_home = true;
                }
                else
                {
                    ROS_WARN("Please press n or h!");
                }
            }
            //===============================//
            //===============================//

            if(robotmode == 7 && !flag_home)
            {
                statusCode_ = imr_nova_control::ROBOT_RUNNING;
            }
            else if(robotmode == 7 && flag_home)
            {
                statusCode_ = imr_nova_control::ROBOT_COMING_HOME;
            }
            
            //===Chu trình gắp phôi và lưu trữ phôi===//
            //========================================//
            if(robotmode == 7 && nova5_ptr_->movePointRunning_ != -1)
            {

                /* Ve home */
                if(flag_home && center_count == 2)
                {
                    if(nova5_ptr_->nova_movePoint(HOME,100,1)==1 && center_count == 2)
                    {
                        flag_home = false;
                        ROS_INFO("Complete");
                        modeThreadRunning_ = false;
                        continue;
                    }
                }

                /* Bước 0 */
                if(center_count == 0)
                {
                    if(get_point_current == 0)
                    {
                        //Đọc tọa độ robot hiện tại trong vòng 6s
                        static uint16_t y_value = 0;
                        
                        nova5_ptr_->nova_getCoorcurrent(Point_current);
                        
                        y_value = abs(Point_current[1]) < 300 ? 300 : static_cast<uint16_t>(abs(Point_current[1]));

                        speed = (y_value / 300.0) * 20;
                        ROS_WARN("Speed: %d", speed);

                        

                        if(CENTER_P[2] > (Point_current[2] + 10))
                        {
                            Center_temp[0] = Point_current[0];
                            Center_temp[1] = Point_current[1];
                            Center_temp[2] = nova5_ptr_->nova_check_z(Point_current[0],Point_current[1],CENTER_P[2],0);
                            Center_temp[3] = Point_current[3];
                            Center_temp[4] = Point_current[4];
                            Center_temp[5] = Point_current[5];
                        }
                        else if(CENTER_P[2] < (Point_current[2] - 10))
                        {
                            Center_temp[0] = CENTER_P[0];
                            Center_temp[1] = CENTER_P[1];
                            Center_temp[2] = nova5_ptr_->nova_check_z(CENTER_P[0],CENTER_P[1],Point_current[2], 0);
                            Center_temp[3] = CENTER_P[3];
                            Center_temp[4] = CENTER_P[4];
                            Center_temp[5] = CENTER_P[5];
                        }
                        else if(CENTER_P[2] >= (Point_current[2] - 10) && CENTER_P[2] < (Point_current[2] + 10)) center_count = 1;

                        // ROS_WARN("x : %f", Center_temp[0]);
                        // ROS_WARN("y : %f", Center_temp[1]);
                        // ROS_WARN("z : %f", Center_temp[2]);
                        get_point_current = 1;
                    }

                    if(center_count == 0)
                    {
                        if(nova5_ptr_->nova_movePoint(Center_temp,speed,0)==1 && center_count == 0) center_count++;
                    }
                }

                /* Bước 1 */
                else if(center_count == 1)
                {
                    if(nova5_ptr_->nova_movePoint(CENTER_P,100,1)==1 && center_count == 1)
                    {
                        center_count = 2;
                    }
                }

                /* Bước 2 di chuyển cánh tay đến vị trí chụp ảnh */
                if(!flag_home && center_count ==  2 && count == 0)
                {
                    if(nova5_ptr_->nova_movePoint(ORIGIN_COOR,100,1)==1 && count == 0)
                    {
                        count++ ;
                    }
                }

                /* Bước 3 xác định vị trí vật thể và di chuyển tới vị trí trên vật thể */
                else if(!flag_home && center_count ==  2 && count == 1)
                {
                    static bool continue_flag = false;
                    if(!temp)
                    {
                        Retry:
                        // Đọc tọa độ vật
                        while(ros::ok()
                        && (enable_ != NULL && (*enable_))
                        )
                        {
                            // Kiểm tra kết nối và trạng thái robot
                            bool connect = nova5_ptr_->nova_checkConnected();
                            bool alarm = nova5_ptr_->nova_statusAlarm();
                            double mode = nova5_ptr_->nova_robotMode();
                            mode_ = mode;

                            // Kiểm tra nếu robot mất kết nối, báo lỗi hoặc robot đang ở chế độ drag thì thoát khỏi vòng lặp
                            if(!connect || alarm || mode != 7 || !modeThreadRunning_)
                            {
                                // Set continue_flag = true và thoát khỏi vòng lặp
                                continue_flag = true;
                                break;
                            } 
                            ros::Duration(1).sleep();
                            ROS_INFO("Read camera...");

                            int status_readCoor = nova5_ptr_->nova_readCoordinates(Point,ORIGIN_COOR,isCheck,timeStamp_cam);
                            if(status_readCoor == 1)
                            {
                                // // Lấy thời gian của máy tính hiện tại
                                // ros::Time current_time = ros::Time::now();

                                // // Chỉ lấy phần giây và ép kiểu sang int
                                // int seconds = static_cast<int>(current_time.sec);
                                double seconds = ros::Time::now().toSec();

                                ROS_ERROR("x : %d", timeStamp_cam);
                                ROS_ERROR("y : %f", seconds);
                                
                                // Nếu khoảng thời giữa 2 lần gửi dữ liệu < 1s thì được coi là hợp lệ và thoát khỏi vòng lặp
                                if((seconds - static_cast<double>(timeStamp_cam)) < TIME_OUT)
                                {
                                    if(isCheck == 1)
                                    {
                                        if(count_ok >= *ok_count_max_)
                                        {
                                            statusCode_ = imr_nova_control::ROBOT_OK_FULL;

                                            ROS_WARN("OK object is over the limit!");
                                            goto Retry;
                                        }
                                    }
                                    if(isCheck == 0)
                                    {
                                        if(count_ng >= *ng_count_max_)
                                        {
                                            statusCode_ = imr_nova_control::ROBOT_NG_FULL;

                                            ROS_WARN("NG object is over the limit!");
                                            goto Retry;
                                        }
                                    }
                                }


                                if ((seconds - static_cast<double>(timeStamp_cam)) >= TIME_OUT)
                                {
                                    // Dữ liệu cũ không được cập nhật, chờ cho đến khi nhận được dữ liệu mới nhất
                                    while ( ros::ok()
                                    && (enable_ != NULL && (*enable_))
                                    )
                                    {
                                        statusCode_ = imr_nova_control::ROBOT_DATA_NOT_UPDATED;

                                        ROS_WARN("Data is out of date!");
                                        ROS_WARN("Wait for data update or press ctrl + c to cancel!");

                                        // // Lấy thời gian của máy tính hiện tại
                                        // ros::Time current_time = ros::Time::now();

                                        // // Chỉ lấy phần giây và ép kiểu sang int
                                        // int seconds = static_cast<int>(current_time.sec);

                                        double seconds = ros::Time::now().toSec();

                                        // Đọc lại tọa độ vật
                                        int status_readCoor = nova5_ptr_->nova_readCoordinates(Point,ORIGIN_COOR,isCheck,timeStamp_cam);
                                        ROS_ERROR("x : %d", timeStamp_cam);
                                        ROS_ERROR("y : %f", seconds);
                                        // Nếu khoảng thời giữa 2 lần gửi dữ liệu < 1s thì được coi là hợp lệ và thoát khỏi vòng lặp
                                        if(((seconds - static_cast<double>(timeStamp_cam)) < TIME_OUT) && status_readCoor)
                                        {
                                            if(isCheck == 1)
                                            {
                                                if(count_ok >= *ok_count_max_)
                                                {
                                                    statusCode_ = imr_nova_control::ROBOT_OK_FULL;

                                                    ROS_WARN("OK object is over the limit!");
                                                    goto Retry;
                                                }
                                                else
                                                {
                                                    break;
                                                }
                                            }
                                            if(isCheck == 0)
                                            {
                                                if(count_ng >= *ng_count_max_)
                                                {
                                                    statusCode_ = imr_nova_control::ROBOT_NG_FULL;

                                                    ROS_WARN("NG object is over the limit!");
                                                    goto Retry;
                                                }
                                                else
                                                {
                                                    break;
                                                }
                                            }
                                        }

                                        // Kiểm tra kết nối và trạng thái robot
                                        bool connect = nova5_ptr_->nova_checkConnected();
                                        bool alarm = nova5_ptr_->nova_statusAlarm();
                                        double mode = nova5_ptr_->nova_robotMode();
                                        mode_ = mode;
                                        
                                        if(!connect || mode != 7 || alarm || !modeThreadRunning_)
                                        {
                                            // ROS_WARN("Robot is in drag mode");
                                            continue_flag = true;
                                            break;
                                        } 
                                        ros::Duration(1).sleep();
                                    }
                                }
                                
                                // Set tọa độ cho điểm trung gian 1
                                Point_temp1[0] = Point[0];
                                Point_temp1[1] = Point[1];
                                Point_temp1[2] = nova5_ptr_->nova_check_z(Point[0],Point[1],ORIGIN_COOR[2], 0);
                                Point_temp1[3] = Point[3];
                                Point_temp1[4] = Point[4];
                                Point_temp1[5] = Point[5];
                                temp = true;
                                break;
                            }
                            else 
                            {
                                statusCode_ = imr_nova_control::ROBOT_READ_DATA_ZERO;
                            }
                        
                        }
                        
                        // Nếu continue_flag = true thì thực hiện vòng vặp tiếp theo
                        if(continue_flag)
                        {
                            continue_flag = false;
                            continue;
                        } 

                        temp = true;
                    }

                    if(count == 1 && continue_flag == false)
                    {
                        if(nova5_ptr_->nova_movePoint(Point_temp1,80,1) == 1) count++;
                    }
                }

                /* Bước 4 di chuyển tới vị trí vật thể */
                else if(!flag_home && center_count ==  2 && count == 2)
                {
                    if(nova5_ptr_->nova_movePoint(Point,20,0)== 1 && count == 2)
                    {
                        nova5_ptr_->nova_onTool();
                        count++;
                    }
                }

                /* Bước 5 di chuyển tới vị trí trên vật thể */
                else if(!flag_home && center_count ==  2 && count == 3)
                {
                    if(nova5_ptr_->nova_movePoint(Point_temp1,40,0)== 1 && count == 3)
                    {
                        count++;
                    }
                }

                /* Bước 6 di chuyển tới vị trí điểm home */
                else if(!flag_home && center_count ==  2 &&count == 4)
                {
                    if(nova5_ptr_->nova_movePoint(CENTER_P,100,1)==1 && count == 4)
                    {
                        
                        // Nếu vật thể là NG thì set điểm lưu trữ NG_COOR
                        if(isCheck == 0)
                        {
                            NG_COOR[0]+=55;

                            STORE_COOR[0] = NG_COOR[0];
                            STORE_COOR[1] = NG_COOR[1];
                            STORE_COOR[2] = NG_COOR[2];
                            STORE_COOR[3] = NG_COOR[3];
                            STORE_COOR[4] = NG_COOR[4];
                            STORE_COOR[5] = NG_COOR[5];

                            Point_temp3[0] = NG_COOR[0];
                            Point_temp3[1] = NG_COOR[1];
                            Point_temp3[2] = nova5_ptr_->nova_check_z(NG_COOR[0],NG_COOR[1],CENTER_P[2], 0);
                            Point_temp3[3] = NG_COOR[3];
                            Point_temp3[4] = NG_COOR[4];
                            Point_temp3[5] = NG_COOR[5];
                            
                            speed = 30;
                            count_ng++;
                        }
                        // Nếu vật thể là OK thì set điểm lưu trữ OK_COOR
                        else if(isCheck == 1)
                        {
                            OK_COOR[0]+=55;

                            STORE_COOR[0] = OK_COOR[0];
                            STORE_COOR[1] = OK_COOR[1];
                            STORE_COOR[2] = OK_COOR[2];
                            STORE_COOR[3] = OK_COOR[3];
                            STORE_COOR[4] = OK_COOR[4];
                            STORE_COOR[5] = OK_COOR[5];

                            Point_temp3[0] = OK_COOR[0];
                            Point_temp3[1] = OK_COOR[1];
                            Point_temp3[2] = nova5_ptr_->nova_check_z(OK_COOR[0],OK_COOR[1],CENTER_P[2], 0);
                            Point_temp3[3] = OK_COOR[3];
                            Point_temp3[4] = OK_COOR[4];
                            Point_temp3[5] = OK_COOR[5];

                            speed = 20;
                            count_ok++;
                        }
                        count++;
                    }
                }

                /* Bước 7 di chuyển đến điểm trung gian trên điểm lưu trữ */
                else if(!flag_home && center_count ==  2 &&count == 5)
                {
                    if(nova5_ptr_->nova_movePoint(Point_temp3,100,1) == 1 && count == 5)
                    {
                        count++ ;
                    }
                }

                /* Bước 8 di chuyển đến điểm lưu trữ */
                else if(!flag_home && center_count ==  2 &&count == 6)
                {
                    if(nova5_ptr_->nova_movePoint(STORE_COOR,speed,0) == 1 && count == 6)
                    {
                        nova5_ptr_->nova_offTool(0.5);
                        count++ ;
                    }
                }

                /* Bước 9 di chuyển đến điểm trung gian trên điểm lưu trữ */
                else if(!flag_home && center_count ==  2 &&count == 7)
                {
                    if(nova5_ptr_->nova_movePoint(Point_temp3,speed,0)==1 && count == 7)
                    {
                        count++;
                    }
                }

                /* Bước 10 reset các biến */
                else if(!flag_home && center_count == 2 && count >= 8)
                {
                    temp = false;
                    count = 0;
                    center_count = 1;    
                    if(ok_count_max_ != NULL && ng_count_max_ != NULL)
                    {
                        if(count_ok >= *ok_count_max_ && count_ng >= *ng_count_max_)
                        {
                            modeThreadRunning_ = false;
                            break;
                        }
                    }
                }
                //========================================//
                //========================================//

                // Hiển thị bước robot đang thực hiện
                if((count + center_count) != count_1) ROS_INFO("STEP: %d", count + center_count);
                count_1 = count + center_count;
            }
            if(nova5_ptr_->movePointRunning_ == -1)
            {
                statusCode_ = imr_nova_control::ROBOT_TARGET_COORDINATES_OUT_OF_RANGE;

                ROS_ERROR("Please check target coordinates!");
                modeThreadRunning_ = false;
                continue;
            }
        }

        //===Kết nối lại robot khi mất kết nối===//
        //=======================================//
        if(!checkconnect)
        {
            ROS_WARN("Robot is reconnecting with maximum time 15 minutes...");
            for(int i=0; i<180U; ++i)
            {
                statusCode_ = imr_nova_control::ROBOT_RECONNECTING;
                
                if(!ros::ok() || !modeThreadRunning_ || enable_ == NULL || !(*enable_)) break;
                if(nova5_ptr_->nova_connect())
                    {
                        nova5_ptr_->nova_stopProgram();
                        // Sau khi enable robot thành công cần bắt đầu chương trình của ccbox
                        bool status_start_program = nova5_ptr_->nova_startProgram();
                        if(status_start_program) statusCode_ = imr_nova_control::ROBOT_RUNNING;
                        else
                        {
                            statusCode_ = imr_nova_control::ROBOT_ERROR;

                            ROS_ERROR("Please check robot arm");
                            modeThreadRunning_ = false;
                        }
                        temp = false;
                        count = 0;
                        center_count = 0;    
                        get_point_current = 0;
                        flag_home = false;

                        break;
                    }
                ros::Duration(5).sleep();

                // Nếu kết nối quá 15 phút thì thoát khỏi vòng lặp
                if(i == 179)
                {
                    statusCode_ = imr_nova_control::ROBOT_DISCONNECTED;

                    ROS_ERROR("Robot is not connected!");
                    modeThreadRunning_ = false;
                    break;
                }
            }
        }
        //========================================//
        //========================================//


        //==============Xóa alarm================//
        //=======================================//
        if(alarm)
        {

            for(int i=0; i<3U; ++i)
            {
                nova5_ptr_->nova_clearAlarm();
                ros::Duration(0.2).sleep();
                if(nova5_ptr_->nova_statusAlarm() == 0 || !(*enable_))
                {
                    ROS_WARN("Alarm is cleared!");
                    break;
                }
                if(i == 2)
                {
                    double mode = nova5_ptr_->nova_robotMode();
                    mode_ = mode;
                    if(mode == 3)
                    {
                        statusCode_ = imr_nova_control::ROBOT_CHECK_EMERGENCY_STOP_BUTTON;

                        ROS_ERROR("Please check button E-Stop on robot IMR!");
                    }
                    else
                    {
                        statusCode_ = imr_nova_control::ROBOT_IN_ALARM_STATE;

                        ROS_ERROR("Robot is in alarm state!");
                    }
                }
            }
        }
        //========================================//
        //========================================//

        loop_rate.sleep();
        ros::spinOnce();
    }
    //=======================Kết thúc vòng lặp chính===================//
    //=================================================================//
    //=================================================================//
}


void imr_nova_control::imr_dobot_go_home_thread()
{
    statusCode_ = imr_nova_control::ROBOT_WAITING;

    std::vector<double> Point_current(6);
    std::vector<double> Center_temp(6);

    int timeStamp_cam = 0;
    int count = 0, count_1 = 0xffff;
    int center_count = 0;
    int get_point_current =0;

    uint16_t speed = 0;

    bool connect = false;
    bool drag = false;
    
    // Khởi tạo 
    std::shared_ptr<cnt_nova5> nova5_ptr_ = nullptr;
    nova5_ptr_ = std::make_shared<cnt_nova5>(IP,PORT);

    nova5_ptr_->run_ptr_ = enable_;


    // Kết nối đến robot nova
    connect = nova5_ptr_->nova_connect();
    if(connect)
    {
        double mode = nova5_ptr_->nova_robotMode();
        mode_ = mode;
        if(mode == 3)
        {
            statusCode_ = imr_nova_control::ROBOT_POWER_ON;

            if(nova5_ptr_->nova_powerOn() == -1)
            {
                statusCode_ = imr_nova_control::ROBOT_ERROR;

                ROS_ERROR("Please check robot arm");
                homeThreadRunning_ = false;// biến tạm để cho thấy robot bị lỗi, thay thành các biến phản hồi lỗi
            }
        } 
        if(homeThreadRunning_)
        {
            // Sau khi kết nối thành công cần enable robot
            if(nova5_ptr_->nova_enable())
            {
                // Sau khi enable robot thành công cần bắt đầu chương trình của ccbox
                bool status_start_program = nova5_ptr_->nova_startProgram();
                if(status_start_program) statusCode_ = imr_nova_control::ROBOT_RUNNING;
                else
                {
                    statusCode_ = imr_nova_control::ROBOT_ERROR;

                    ROS_ERROR("Please check robot arm");
                    modeThreadRunning_ = false;
                }
            }
            else
            {
                statusCode_ = imr_nova_control::ROBOT_ERROR;

                ROS_ERROR("Please check robot arm");
                homeThreadRunning_ = false;
            }
        }

    }
    else 
    {
        
        // alarm = nova5_ptr_->nova_statusAlarm();
        ROS_WARN("Robot is reconnecting with maximum time 15 minutes...");
        for(int i=0; i<180U; ++i)
        {
            statusCode_ = imr_nova_control::ROBOT_RECONNECTING;

            if(enable_ == NULL || !(*enable_))
            {
                homeThreadRunning_ = false;
                break;
            }
            connect = nova5_ptr_->nova_connect();
            if(connect || !ros::ok())
            {
                while(ros::ok()
                && (enable_ != NULL && (*enable_))
                )
                {
                    double mode = nova5_ptr_->nova_robotMode();
                    double alarm = nova5_ptr_->nova_statusAlarm();
                    mode_ = mode;
                    if(mode == 9)
                    {
                        statusCode_ = imr_nova_control::ROBOT_ERROR;

                        ROS_ERROR("Kinematic ERROR! Please check robot!");
                        homeThreadRunning_ = false;// biến tạm để cho thấy robot bị lỗi, thay thành các biến phản hồi lỗi
                        break;
                    }
                    if(mode == 3)
                    {
                        if(nova5_ptr_->nova_powerOn() == -1)
                        {
                            statusCode_ = imr_nova_control::ROBOT_ERROR;

                            ROS_ERROR("Please check robot arm");
                            homeThreadRunning_ = false;// biến tạm để cho thấy robot bị lỗi, thay thành các biến phản hồi lỗi
                            break;
                        }
                    } 
                    if(mode == 4) break;
                    ros::Duration(5).sleep();
                }

                if(homeThreadRunning_)
                {
                    // Sau khi kết nối thành công cần enable robot
                    if(nova5_ptr_->nova_enable())
                    {
                        // Sau khi enable robot thành công cần bắt đầu chương trình của ccbox
                        bool status_start_program = nova5_ptr_->nova_startProgram();
                        if(status_start_program) statusCode_ = imr_nova_control::ROBOT_RUNNING;
                        else
                        {
                            statusCode_ = imr_nova_control::ROBOT_ERROR;

                            ROS_ERROR("Please check robot arm");
                            modeThreadRunning_ = false;
                        }
                    }
                    else
                    {
                        statusCode_ = imr_nova_control::ROBOT_ERROR;

                        ROS_ERROR("Please check robot arm");
                        homeThreadRunning_ = false;
                    }
                }

                break;
            }
            ros::Duration(5).sleep();
        }
        
    }

    ros::Rate loop_rate(100);


    //===========================Vòng lặp chính========================//
    //=================================================================//
    //=================================================================//
    while(ros::ok() && connect && homeThreadRunning_
    && (enable_ != NULL && (*enable_))
    )
    {
        static bool temp = false;
        static bool checkconnect = false;
        static bool alarm = false;

        //====Kiểm tra kết nối và trạng thái alarm====//
        checkconnect = nova5_ptr_->nova_checkConnected();
        alarm = nova5_ptr_->nova_statusAlarm();
        //============================================//

        if(checkconnect && !alarm)
        {
            statusCode_ = imr_nova_control::ROBOT_COMING_HOME;
            // Lấy trạng thái robot
            double robotmode = nova5_ptr_->nova_robotMode();
            mode_ = robotmode;

            //===Kiểm tra trạng thái robot===//
            //===============================//
            if( robotmode == 3)
            {
                statusCode_ = imr_nova_control::ROBOT_ERROR;

                ROS_ERROR("Go home failed!");
                ROS_ERROR("Please try again!");
                homeThreadRunning_ = false;
                continue;
            }

            else if(robotmode == 6)
            {
                ROS_WARN("Robot is in drag mode");
                drag = true;
            }

            if(drag == true && robotmode == 5)
            {
                statusCode_ = imr_nova_control::ROBOT_ERROR;

                ROS_ERROR("Go home failed!");
                ROS_ERROR("Please try again!");
                homeThreadRunning_ = false;
                continue;
            }
            //===============================//
            //===============================//
            
            
            //===Chu trình gắp phôi và lưu trữ phôi===//
            //========================================//
            if(robotmode ==7)
            {
                if(get_point_current == 0)
                {
                    static uint16_t y_value = 0;
                    ROS_INFO("Robot is comming home...");
                    //Đọc tọa độ robot hiện tại trong vòng 6s

                    nova5_ptr_->nova_getCoorcurrent(Point_current);
                    
                    y_value = abs(Point_current[1]) < 300 ? 300 : static_cast<uint16_t>(abs(Point_current[1]));

                    speed = (y_value / 300.0) * 20;
                    ROS_WARN("Speed: %d", speed);
                    

                    if(CENTER_P[2] > (Point_current[2] + 10))
                    {
                        Center_temp[0] = Point_current[0];
                        Center_temp[1] = Point_current[1];
                        Center_temp[2] = nova5_ptr_->nova_check_z(Point_current[0],Point_current[1],CENTER_P[2],0);
                        Center_temp[3] = Point_current[3];
                        Center_temp[4] = Point_current[4];
                        Center_temp[5] = Point_current[5];
                    }
                    else if(CENTER_P[2] < (Point_current[2] - 10))
                    {
                        Center_temp[0] = CENTER_P[0];
                        Center_temp[1] = CENTER_P[1];
                        Center_temp[2] = nova5_ptr_->nova_check_z(CENTER_P[0],CENTER_P[1],Point_current[2], 0);
                        Center_temp[3] = CENTER_P[3];
                        Center_temp[4] = CENTER_P[4];
                        Center_temp[5] = CENTER_P[5];
                    }
                    else if(CENTER_P[2] >= (Point_current[2] - 10) && CENTER_P[2] < (Point_current[2] + 10)) center_count = 1;

                    get_point_current = 1;
                }
                /* Bước 0 ve home */
                if(center_count == 0)
                {
                    if(nova5_ptr_->nova_movePoint(Center_temp,speed,0)==1 && center_count == 0)
                    {
                        center_count++ ;
                    }
                }

                /* Bước 1 */
                else if(center_count == 1)
                {
                    if(nova5_ptr_->nova_movePoint(CENTER_P,100,1)==1 && center_count == 1)
                    {
                        center_count++ ;
                    }
                }

                else if(center_count == 2)
                {
                    if(nova5_ptr_->nova_movePoint(HOME,100,1)==1 && center_count == 2)
                    {
                        ROS_INFO("Complete");
                        homeThreadRunning_ = false;
                        continue;
                    }
                }
            }
        }

        else 
        {
            statusCode_ = imr_nova_control::ROBOT_DISCONNECTED;
            ROS_ERROR("Go home failed!");
            ROS_ERROR("Please try again!");
            homeThreadRunning_ = false;
            continue;
        }
    }
}