/**
 * @file FairinoRobotWrapper_Example.cpp
 * @brief FairinoRobotWrapper 使用示例
 * @details 演示如何使用封装后的Fairino机器人接口
 *
 * 注意：此文件仅作为示例参考，不参与编译
 */

#include "FairinoRobotWrapper.h"
#include <iostream>

/**
 * @brief 示例1：基本连接和断开
 */
void Example_Connection()
{
    FairinoRobotWrapper robot;

    // 获取SDK版本
    std::cout << "SDK版本: " << robot.GetSDKVersion() << std::endl;

    // 连接机器人（使用默认IP: 192.168.58.2）
    errno_t ret = robot.Connect();
    if (ret != ERR_SUCCESS) {
        std::cout << "连接失败: " << robot.GetLastErrorString() << std::endl;
        return;
    }
    std::cout << "连接成功，控制器IP: " << robot.GetControllerIP() << std::endl;

    // ... 执行操作 ...

    // 断开连接
    robot.Disconnect();
    std::cout << "已断开连接" << std::endl;
}

/**
 * @brief 示例2：直线运动 MoveL
 */
void Example_MoveL()
{
    FairinoRobotWrapper robot;

    if (robot.Connect("192.168.58.2") != ERR_SUCCESS) {
        std::cout << "连接失败" << std::endl;
        return;
    }

    // 使能机器人
    robot.RobotEnable(1);

    // 设置全局速度为30%
    robot.SetSpeed(30);

    // 直线运动到目标位置
    // 参数：x, y, z (mm), rx, ry, rz (deg)
    errno_t ret = robot.MoveL(
        400.0, 0.0, 300.0,      // 目标位置 (mm)
        180.0, 0.0, 0.0,        // 目标姿态 (deg)
        50.0,                    // 速度百分比
        50.0,                    // 加速度百分比
        -1.0                     // blendR = -1.0 表示运动到位（阻塞）
    );

    if (ret != ERR_SUCCESS) {
        std::cout << "MoveL失败: " << robot.GetLastErrorString() << std::endl;
    } else {
        std::cout << "MoveL完成" << std::endl;
    }

    // 连续直线运动（非阻塞模式，使用平滑过渡）
    robot.MoveL(450.0, 50.0, 300.0, 180.0, 0.0, 0.0, 50.0, 50.0, 50.0);  // blendR=50mm
    robot.MoveL(450.0, -50.0, 300.0, 180.0, 0.0, 0.0, 50.0, 50.0, 50.0);
    robot.MoveL(400.0, 0.0, 300.0, 180.0, 0.0, 0.0, 50.0, 50.0, -1.0);   // 最后一个点阻塞

    robot.Disconnect();
}

/**
 * @brief 示例3：关节运动 MoveJ（通过末端位姿）
 */
void Example_MoveJByPose()
{
    FairinoRobotWrapper robot;

    if (robot.Connect() != ERR_SUCCESS) {
        return;
    }

    robot.RobotEnable(1);

    // 通过目标末端位姿进行关节运动
    // 内部会自动进行逆运动学计算
    errno_t ret = robot.MoveJByPose(
        400.0, 0.0, 300.0,      // 目标位置 (mm)
        180.0, 0.0, 0.0,        // 目标姿态 (deg)
        50.0,                    // 速度百分比
        50.0,                    // 加速度百分比
        -1.0                     // blendT = -1.0 表示运动到位（阻塞）
    );

    if (ret != ERR_SUCCESS) {
        std::cout << "MoveJByPose失败: " << robot.GetLastErrorString() << std::endl;
    }

    robot.Disconnect();
}

/**
 * @brief 示例4：关节运动 MoveJ（通过关节角度）
 */
void Example_MoveJByJoint()
{
    FairinoRobotWrapper robot;

    if (robot.Connect() != ERR_SUCCESS) {
        return;
    }

    robot.RobotEnable(1);

    // 直接指定各关节角度进行运动
    errno_t ret = robot.MoveJByJoint(
        0.0, -45.0, 90.0,       // J1, J2, J3 (deg)
        0.0, 45.0, 0.0,         // J4, J5, J6 (deg)
        50.0,                    // 速度百分比
        50.0,                    // 加速度百分比
        -1.0                     // blendT = -1.0 表示运动到位（阻塞）
    );

    if (ret != ERR_SUCCESS) {
        std::cout << "MoveJByJoint失败: " << robot.GetLastErrorString() << std::endl;
    }

    robot.Disconnect();
}

/**
 * @brief 示例5：获取机器人状态
 */
void Example_GetRobotStatus()
{
    FairinoRobotWrapper robot;

    if (robot.Connect() != ERR_SUCCESS) {
        return;
    }

    // 获取当前关节位置
    double j1, j2, j3, j4, j5, j6;
    if (robot.GetCurrentJointPos(j1, j2, j3, j4, j5, j6) == ERR_SUCCESS) {
        std::cout << "当前关节位置(deg):" << std::endl;
        std::cout << "  J1=" << j1 << ", J2=" << j2 << ", J3=" << j3 << std::endl;
        std::cout << "  J4=" << j4 << ", J5=" << j5 << ", J6=" << j6 << std::endl;
    }

    // 获取当前TCP位姿
    double x, y, z, rx, ry, rz;
    if (robot.GetCurrentTCPPose(x, y, z, rx, ry, rz) == ERR_SUCCESS) {
        std::cout << "当前TCP位姿:" << std::endl;
        std::cout << "  位置(mm): x=" << x << ", y=" << y << ", z=" << z << std::endl;
        std::cout << "  姿态(deg): rx=" << rx << ", ry=" << ry << ", rz=" << rz << std::endl;
    }

    // 获取完整的机器人实时状态
    ROBOT_STATE_PKG state;
    if (robot.GetRobotState(state) == ERR_SUCCESS) {
        std::cout << "机器人状态:" << std::endl;
        std::cout << "  程序状态: " << (int)state.program_state << std::endl;
        std::cout << "  运动状态: " << (int)state.robot_state << std::endl;
        std::cout << "  使能状态: " << state.rbtEnableState << std::endl;
        std::cout << "  运动到位: " << state.motion_done << std::endl;
    }

    // 检查错误码
    int maincode, subcode;
    if (robot.GetErrorCode(maincode, subcode) == ERR_SUCCESS) {
        if (maincode != 0 || subcode != 0) {
            std::cout << "机器人错误码: 主=" << maincode << ", 子=" << subcode << std::endl;
        }
    }

    robot.Disconnect();
}

/**
 * @brief 示例6：运动学计算
 */
void Example_Kinematics()
{
    FairinoRobotWrapper robot;

    if (robot.Connect() != ERR_SUCCESS) {
        return;
    }

    // 正运动学：根据关节角度计算末端位姿
    double x, y, z, rx, ry, rz;
    if (robot.ForwardKinematics(0.0, -45.0, 90.0, 0.0, 45.0, 0.0,
                                 x, y, z, rx, ry, rz) == ERR_SUCCESS) {
        std::cout << "正运动学结果:" << std::endl;
        std::cout << "  位置(mm): x=" << x << ", y=" << y << ", z=" << z << std::endl;
        std::cout << "  姿态(deg): rx=" << rx << ", ry=" << ry << ", rz=" << rz << std::endl;
    }

    // 逆运动学：根据末端位姿计算关节角度
    double j1, j2, j3, j4, j5, j6;
    if (robot.InverseKinematics(400.0, 0.0, 300.0, 180.0, 0.0, 0.0,
                                 j1, j2, j3, j4, j5, j6) == ERR_SUCCESS) {
        std::cout << "逆运动学结果:" << std::endl;
        std::cout << "  关节角度(deg): J1=" << j1 << ", J2=" << j2 << ", J3=" << j3 << std::endl;
        std::cout << "                J4=" << j4 << ", J5=" << j5 << ", J6=" << j6 << std::endl;
    } else {
        std::cout << "逆运动学计算失败: " << robot.GetLastErrorString() << std::endl;
    }

    robot.Disconnect();
}

/**
 * @brief 示例7：设置坐标系
 */
void Example_SetCoordinates()
{
    FairinoRobotWrapper robot;

    if (robot.Connect() != ERR_SUCCESS) {
        return;
    }

    // 设置工具坐标系 (工具中心点相对于末端法兰的位姿)
    robot.SetToolCoord(1, 0.0, 0.0, 100.0, 0.0, 0.0, 0.0);

    // 设置工件坐标系 (工件坐标系相对于基坐标系的位姿)
    robot.SetWObjCoord(1, 500.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0);

    // 使用指定的工具和工件坐标系进行运动
    robot.MoveL(100.0, 0.0, 50.0, 180.0, 0.0, 0.0, 50.0, 50.0, -1.0, 1, 1);

    robot.Disconnect();
}

/**
 * @brief 示例8：错误处理和恢复
 */
void Example_ErrorHandling()
{
    FairinoRobotWrapper robot;

    if (robot.Connect() != ERR_SUCCESS) {
        std::cout << "连接失败: " << robot.GetLastErrorString() << std::endl;
        return;
    }

    // 检查并清除错误
    int maincode, subcode;
    robot.GetErrorCode(maincode, subcode);
    if (maincode != 0) {
        std::cout << "检测到错误，正在清除..." << std::endl;
        robot.ResetAllError();
    }

    // 使能机器人
    robot.RobotEnable(1);

    // 执行运动
    errno_t ret = robot.MoveL(400.0, 0.0, 300.0, 180.0, 0.0, 0.0);
    if (ret != ERR_SUCCESS) {
        std::cout << "运动失败: " << robot.GetLastErrorString() << std::endl;
        robot.StopMotion();
        robot.ResetAllError();
    }

    // 等待运动完成
    uint8_t motionDone = 0;
    while (motionDone == 0) {
        robot.GetMotionDone(motionDone);
        robot.Wait(100);
    }
    std::cout << "运动完成" << std::endl;

    robot.Disconnect();
}

int main(int argc, char* argv[])
{
    std::cout << "=== FairinoRobotWrapper 使用示例 ===" << std::endl;
    return 0;
}