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



char 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;
}


int main(int argc,char **argv)
{
    ros::init(argc,argv,"EXAMPLE");
    ros::NodeHandle nh("~");

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


    // 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::vector<double> TEST_COOR = {250.71,-480.9,424.27,-179.4,-0.04,-90.00};

    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);

    uint16_t isCheck = 0;
    int timeStamp_cam = 0;
    static int count = 0, count_1 = 0;
    static int center_count = 0;
    static int get_point_current =0;
    static int ob = 0;
    static uint16_t speed = 0;
    static int count_ok = 0;
    static int count_ng = 0;
    bool connect = false;
    bool drag = false;
    bool flag_home = false;
    bool poweroff = false;
    
    // Khởi tạo 
    std::shared_ptr<cnt_nova5> nova5_ptr_ = nullptr;
    nova5_ptr_ = std::make_shared<cnt_nova5>(IP,PORT);

    // Kết nối đến robot nova
    connect = nova5_ptr_->nova_connect();
    if(connect)
    {
        if(nova5_ptr_->nova_robotMode() == 3)
        {
            if(nova5_ptr_->nova_powerOn() == -1)
            {
                ROS_ERROR("Please check robot arm");
                return -1;// biến tạm để cho thấy robot bị lỗi, thay thành các biến phản hồi lỗi
            }
        } 
        // Sau khi kết nối thành công cần enable robot
        nova5_ptr_->nova_enable();

        // Sau khi enable robot thành công cần bắt đầu chương trình của ccbox
        nova5_ptr_->nova_startProgram();
    }
    else 
    {
        // alarm = nova5_ptr_->nova_statusAlarm();
        ROS_WARN("Robot is reconnecting with maximum time 15 minutes...");
        for(int i=0; i<180U; ++i)
        {
            connect = nova5_ptr_->nova_connect();
            if(connect || !ros::ok())
                {
                    while(ros::ok())
                    {
                        double mode = nova5_ptr_->nova_robotMode();
                        if(nova5_ptr_->nova_statusAlarm() == 1 && mode == 9) ROS_ERROR("Kinematic ERROR! Please check robot!");
                        if(mode == 3)
                        {
                            if(nova5_ptr_->nova_powerOn() == -1)
                            {
                                ROS_ERROR("Please check robot arm");
                                return -1;// 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 == 4) break;
                        ros::Duration(5).sleep();
                    }
                    
                    // Sau khi kết nối thành công cần enable robot
                    nova5_ptr_->nova_enable();

                    // Sau khi enable robot thành công cần bắt đầu chương trình của ccbox
                    nova5_ptr_->nova_startProgram();

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

    ros::Rate loop_rate(100);


    //===========================Vòng lặp chính========================//
    //=================================================================//
    //=================================================================//
    while(ros::ok() && connect)
    {
        static int temp = 0;
        static bool checkconnect = false;
        checkconnect = nova5_ptr_->nova_checkConnected();
        if(checkconnect)
        {
            double robotmode = nova5_ptr_->nova_robotMode();

            //===Kiểm tra trạng thái robot===//
            //===============================//
            if( robotmode == 3)
            {
                poweroff = true;
                // Power on robot
                if(nova5_ptr_->nova_powerOn() == 1)
                {
                    // Sau khi power on cần enable robot
                    nova5_ptr_->nova_enable();
                    nova5_ptr_->nova_startProgram();
                }
            }
            
            else if(robotmode == 6)
            {
                ROS_WARN("Robot is in drag mode");
                drag = true;
            }

            if(drag == true && robotmode == 5)
            {
                ROS_WARN("PRESS: n to continute");
                ROS_WARN("PRESS: h to go home");
                char key = getKey();
                ROS_WARN("%d", key);
                /*Kiểm tra nếu key = n hoặc N thì thực hiện lệnh tiếp tục*/
                if(key == 110 || key == 78)
                {
                    nova5_ptr_->nova_startProgram();
                    drag = false;
                    get_point_current = 0;
                    count = 0;
                    center_count = 0;
                    flag_home = false;
                }
                /*Kiểm tra nếu key = h hoặc H thì thực hiện lệnh về home */
                else if(key == 104)
                {
                    nova5_ptr_->nova_startProgram();
                    drag = false;
                    get_point_current = 0;
                    center_count = 0;
                    ROS_INFO("Robot is comming home...");
                    flag_home = true;
                }
            }
            //===============================//
            //===============================//
            
            
            //===Chu trình gắp phôi và lưu trữ phôi===//
            //========================================//
            /* Bước 0 ve home */
            if(center_count == 0)
            {
                if(get_point_current == 0)
                {
                    //Đọc tọa độ robot hiện tại trong vòng 6s

                    nova5_ptr_->nova_getCoorcurrent(Point_current);
                    
                    ROS_ERROR("x current: %f", Point_current[0]);
                    ROS_ERROR("y current: %f", Point_current[1]);
                    ROS_ERROR("z current: %f", Point_current[2]);

                    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,100,1)==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;
                }
            }

            if(flag_home && center_count == 2)
            {
                if(nova5_ptr_->nova_movePoint(HOME,100,1)==1 && center_count == 2)
                {
                    ROS_INFO("Complete");
                    // nova5_ptr_->nova_stopProgram();
                    // nova5_ptr_->nova_close();
                    return 0;
                }
            }

            /* 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 == 0)
                {
                    
                    // Đọc tọa độ vật trong thời gian max là 10s
                    while( ros::ok() && !nova5_ptr_->nova_statusAlarm())
                    {
                        if(!nova5_ptr_->nova_checkConnected() || !ros::ok() || nova5_ptr_->nova_statusAlarm() || nova5_ptr_->nova_robotMode() != 7)
                        {
                            continue_flag = true;
                            break;
                        } 
                        ros::Duration(1).sleep();
                        ROS_INFO("Read camera");

                        if(nova5_ptr_->nova_readCoordinates(Point,ORIGIN_COOR,isCheck,timeStamp_cam) == 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);

                            // 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 ((seconds - timeStamp_cam) >= TIME_ERR && ros::ok())
                            {
                                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);

                                nova5_ptr_->nova_readCoordinates(Point,ORIGIN_COOR,isCheck,timeStamp_cam);
                                // Nếu khoảng thời giữa 2 lần gửi dữ liệu < 1s thì được coi là hợp lệ
                                if((seconds - timeStamp_cam) < TIME_ERR){
                                    nova5_ptr_->nova_readCoordinates(Point,ORIGIN_COOR,isCheck,timeStamp_cam);
                                    break;
                                }
                                if(!nova5_ptr_->nova_checkConnected() || nova5_ptr_->nova_robotMode() == 6)
                                {
                                    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 = 1;
                            break;
                        }
                    
                    }

                    if(continue_flag)
                    {
                        continue_flag = false;
                        continue;
                    } 

                    // Point_temp1[0] = TEST_COOR[0];
                    // Point_temp1[1] = TEST_COOR[1];
                    // Point_temp1[2] = nova5_ptr_->nova_check_z(TEST_COOR[0],TEST_COOR[1],ORIGIN_COOR[2], 0);
                    // Point_temp1[3] = TEST_COOR[3];
                    // Point_temp1[4] = TEST_COOR[4];
                    // Point_temp1[5] = TEST_COOR[5];
                    temp =1;
                }

                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)
            {
                // timeStamp_cam_1 = timeStamp_cam;
                temp = 0;
                count = 0;
                center_count = 1;    
                // if(isCheck > 1) isCheck =0;

                if(count_ok > 4 && count_ng > 4) break;
                // 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;
        }
        else
        { 
        // alarm = nova5_ptr_->nova_statusAlarm();
        ROS_WARN("Robot is reconnecting with maximum time 15 minutes...");
        for(int i=0; i<180U; ++i)
        {
            
            if(!ros::ok()) 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
                    nova5_ptr_->nova_startProgram();
                    temp = 0;
                    count = 0;
                    center_count = 0;    
                    get_point_current = 0;
                    flag_home = false;

                    break;
                }
            ros::Duration(5).sleep();
        }
        }
        loop_rate.sleep();
        ros::spinOnce();
    }
    
    // nova5_ptr_->nova_stopProgram();
    // nova5_ptr_->nova_close();
}


