#include <iostream>
#include <array>
#include <thread>
#include <atomic>
#include <mutex>
#include <winsock2.h>
#pragma comment(lib, "ws2_32.lib")
std::atomic<bool> rt_fault{ false };

#include "rokae/robot.h"
#include "print_helper.hpp"
#include "rokae/utility.h"
#include "rokae/model.h"
#include <conio.h> // 用于 _kbhit() 和 _getch()
using namespace rokae;
using namespace rokae::RtSupportedFields;
std::ostream& os = std::cout;

int main() {   
    xMateErProRobot robot("192.168.2.160", "192.168.2.223");
    std::error_code ec;
    robot.setRtNetworkTolerance(200, ec);
    robot.setMotionControlMode(rokae::MotionControlMode::RtCommand, ec);
    robot.setOperateMode(rokae::OperateMode::automatic, ec);
    robot.setPowerState(true, ec);

    auto rtCon = robot.getRtMotionController().lock();
    try {
        robot.startReceiveRobotState(std::chrono::milliseconds(1), { jointPos_m });
    }
    catch (const std::exception& e) {
        std::cerr << e.what();
        return 0;
    }

    // 运动到初始位姿并记录
    std::array<double, 7u> start_jointPose_rad = { 0, M_PI / 6, 0, M_PI / 3, 0, M_PI_2, 0 };

    rtCon->MoveJ(0.1, robot.jointPos(ec), start_jointPose_rad);
    std::this_thread::sleep_for(std::chrono::seconds(3));
    
    CartesianPosition tcp_init;
    Utils::postureToTransArray(robot.posture(CoordinateType::endInRef, ec), tcp_init.pos);
    // 创建跟踪对象
    auto model = robot.model();
    rokae::FollowPosition follow_pose(robot, model);

    bool running = true;
    try {
        follow_pose.start(start_jointPose_rad);
    }
    catch (const std::exception& e) {
        std::cerr << "[START ERROR] " << e.what() << std::endl;
        running = false;
    }

    std::cout << "控制已启动, 按 ESC 键退出..." << std::endl;
    follow_pose.setScale(2);
    try {
        while (running) {
            if (_kbhit() && _getch() == 27) running = false;
            start_jointPose_rad[4] += 0.05;
            follow_pose.update(start_jointPose_rad);
            std::this_thread::sleep_for(std::chrono::milliseconds(600)); } } 
    catch (const rokae::RealtimeMotionException& e) { 
        std::cerr << "[RT FAULT] " << e.what() << std::endl; 
        rt_fault = true; running = false; 
        // 通知主线程收尾 
    } 
    catch (const std::exception& e) { 
        std::cerr << "[EXCEPTION] " << e.what() << std::endl; 
        rt_fault = true; running = false; } 

    try {
        follow_pose.stop();
    }
    catch (const rokae::RealtimeMotionException& e) {
        std::cerr << "[STOP ERROR] " << e.what() << std::endl;
    }

    //将控制模式设为空闲并下电
    robot.setMotionControlMode(MotionControlMode::Idle, ec);
    robot.setPowerState(false, ec);
    return 0;
}
