/**
 * @file FairinoRobotWrapper.h
 * @brief Fairino机器人SDK二次封装
 * @details 提供简化的接口用于控制Fairino机器人，包括moveL, moveJ等基本运动指令
 */

#ifndef FAIRINO_ROBOT_WRAPPER_H
#define FAIRINO_ROBOT_WRAPPER_H

#include "robot.h"
#include "robot_types.h"
#include "robot_error.h"
#include <string>
#include <memory>

// 导出宏定义
#ifdef FAIRINO_WRAPPER_LIB
    #ifdef _WIN32
        #define FAIRINO_WRAPPER_EXPORT __declspec(dllexport)
    #else
        #define FAIRINO_WRAPPER_EXPORT __attribute__((visibility("default")))
    #endif
#else
    #ifdef _WIN32
        #define FAIRINO_WRAPPER_EXPORT __declspec(dllimport)
    #else
        #define FAIRINO_WRAPPER_EXPORT
    #endif
#endif

/**
 * @class FairinoRobotWrapper
 * @brief Fairino机器人SDK二次封装类
 *
 * 提供简化的接口控制Fairino机器人，主要功能包括：
 * 1. moveL - 直线运动（输入末端坐标和姿态）
 * 2. moveJ - 关节运动（输入末端坐标和姿态或关节角度）
 * 3. 其他辅助接口（状态查询、连接管理等）
 */
class FAIRINO_WRAPPER_EXPORT FairinoRobotWrapper
{
public:
    /**
     * @brief 构造函数
     */
    FairinoRobotWrapper();

    /**
     * @brief 析构函数
     */
    ~FairinoRobotWrapper();

    /**
     * @brief 连接机器人控制器
     * @param ip 机器人控制器IP地址，默认为192.168.58.2
     * @return 错误码，0表示成功
     */
    errno_t Connect(const std::string& ip = "192.168.58.2");

    /**
     * @brief 断开与机器人控制器的连接
     * @return 错误码，0表示成功
     */
    errno_t Disconnect();

    /**
     * @brief 是否已连接
     * @return true-已连接，false-未连接
     */
    bool IsConnected() const;

    // ==================== 运动控制接口 ====================

    /**
     * @brief 笛卡尔空间直线运动（简化版）
     * @param x 目标X坐标，单位mm
     * @param y 目标Y坐标，单位mm
     * @param z 目标Z坐标，单位mm
     * @param rx 绕X轴旋转角度，单位deg
     * @param ry 绕Y轴旋转角度，单位deg
     * @param rz 绕Z轴旋转角度，单位deg
     * @param vel 速度百分比，范围[0~100]，默认50
     * @param acc 加速度百分比，范围[0~100]，默认50
     * @param blendR 平滑半径，-1.0表示运动到位（阻塞），[0~1000.0]平滑半径（非阻塞），单位mm，默认-1.0
     * @param tool 工具坐标系编号，范围[0~14]，默认0
     * @param user 工件坐标系编号，范围[0~14]，默认0
     * @return 错误码，0表示成功
     */
    errno_t MoveL(double x, double y, double z, double rx, double ry, double rz,
                  float vel = 50.0f, float acc = 50.0f, float blendR = -1.0f,
                  int tool = 0, int user = 0);

    /**
     * @brief 关节空间运动（通过末端笛卡尔位姿）
     * @param x 目标X坐标，单位mm
     * @param y 目标Y坐标，单位mm
     * @param z 目标Z坐标，单位mm
     * @param rx 绕X轴旋转角度，单位deg
     * @param ry 绕Y轴旋转角度，单位deg
     * @param rz 绕Z轴旋转角度，单位deg
     * @param vel 速度百分比，范围[0~100]，默认50
     * @param acc 加速度百分比，范围[0~100]，默认50
     * @param blendT 平滑时间，-1.0表示运动到位（阻塞），[0~500.0]平滑时间（非阻塞），单位ms，默认-1.0
     * @param tool 工具坐标系编号，范围[0~14]，默认0
     * @param user 工件坐标系编号，范围[0~14]，默认0
     * @return 错误码，0表示成功
     */
    errno_t MoveJByPose(double x, double y, double z, double rx, double ry, double rz,
                        float vel = 50.0f, float acc = 50.0f, float blendT = -1.0f,
                        int tool = 0, int user = 0);

    /**
     * @brief 关节空间运动（通过关节角度）
     * @param j1 关节1角度，单位deg
     * @param j2 关节2角度，单位deg
     * @param j3 关节3角度，单位deg
     * @param j4 关节4角度，单位deg
     * @param j5 关节5角度，单位deg
     * @param j6 关节6角度，单位deg
     * @param vel 速度百分比，范围[0~100]，默认50
     * @param acc 加速度百分比，范围[0~100]，默认50
     * @param blendT 平滑时间，-1.0表示运动到位（阻塞），[0~500.0]平滑时间（非阻塞），单位ms，默认-1.0
     * @param tool 工具坐标系编号，范围[0~14]，默认0
     * @param user 工件坐标系编号，范围[0~14]，默认0
     * @return 错误码，0表示成功
     */
    errno_t MoveJByJoint(double j1, double j2, double j3, double j4, double j5, double j6,
                         float vel = 50.0f, float acc = 50.0f, float blendT = -1.0f,
                         int tool = 0, int user = 0);

    // ==================== 辅助接口 ====================

    /**
     * @brief 停止运动
     * @return 错误码，0表示成功
     */
    errno_t StopMotion();

    /**
     * @brief 暂停运动
     * @return 错误码，0表示成功
     */
    errno_t PauseMotion();

    /**
     * @brief 恢复运动
     * @return 错误码，0表示成功
     */
    errno_t ResumeMotion();

    /**
     * @brief 机器人上使能/下使能
     * @param state 0-下使能，1-上使能
     * @return 错误码，0表示成功
     */
    errno_t RobotEnable(uint8_t state);

    /**
     * @brief 清除所有错误
     * @return 错误码，0表示成功
     */
    errno_t ResetAllError();

    /**
     * @brief 获取当前关节位置
     * @param j1~j6 六个关节的角度输出，单位deg
     * @param blocking 是否阻塞，0-阻塞，1-非阻塞，默认0
     * @return 错误码，0表示成功
     */
    errno_t GetCurrentJointPos(double& j1, double& j2, double& j3,
                               double& j4, double& j5, double& j6,
                               uint8_t blocking = 0);

    /**
     * @brief 获取当前TCP位姿
     * @param x,y,z 位置坐标输出，单位mm
     * @param rx,ry,rz 姿态角度输出，单位deg
     * @param blocking 是否阻塞，0-阻塞，1-非阻塞，默认0
     * @return 错误码，0表示成功
     */
    errno_t GetCurrentTCPPose(double& x, double& y, double& z,
                              double& rx, double& ry, double& rz,
                              uint8_t blocking = 0);

    /**
     * @brief 查询运动是否完成
     * @param state 输出状态，0-未完成，1-完成
     * @return 错误码，0表示成功
     */
    errno_t GetMotionDone(uint8_t& state);

    /**
     * @brief 获取机器人错误码
     * @param maincode 主错误码
     * @param subcode 子错误码
     * @return 错误码，0表示成功
     */
    errno_t GetErrorCode(int& maincode, int& subcode);

    /**
     * @brief 获取机器人实时状态
     * @param state 机器人状态结构体
     * @return 错误码，0表示成功
     */
    errno_t GetRobotState(ROBOT_STATE_PKG& state);

    /**
     * @brief 设置工具坐标系
     * @param id 坐标系编号，范围[0~14]
     * @param x,y,z 工具中心点相对于末端法兰中心的位置，单位mm
     * @param rx,ry,rz 工具中心点相对于末端法兰中心的姿态，单位deg
     * @return 错误码，0表示成功
     */
    errno_t SetToolCoord(int id, double x, double y, double z,
                         double rx, double ry, double rz);

    /**
     * @brief 设置工件坐标系
     * @param id 坐标系编号，范围[0~14]
     * @param x,y,z 工件坐标系相对于基坐标系的位置，单位mm
     * @param rx,ry,rz 工件坐标系相对于基坐标系的姿态，单位deg
     * @param refFrame 参考坐标系，默认0
     * @return 错误码，0表示成功
     */
    errno_t SetWObjCoord(int id, double x, double y, double z,
                         double rx, double ry, double rz, int refFrame = 0);

    /**
     * @brief 设置全局速度百分比
     * @param vel 速度百分比，范围[0~100]
     * @return 错误码，0表示成功
     */
    errno_t SetSpeed(int vel);

    /**
     * @brief 等待指定时间
     * @param ms 等待时间，单位ms
     * @return 错误码，0表示成功
     */
    errno_t Wait(int ms);

    /**
     * @brief 获取SDK版本号
     * @return SDK版本号字符串
     */
    std::string GetSDKVersion();

    /**
     * @brief 获取控制器IP地址
     * @return 控制器IP地址
     */
    std::string GetControllerIP();

    /**
     * @brief 正运动学求解（根据关节角度计算末端位姿）
     * @param j1~j6 六个关节角度，单位deg
     * @param x,y,z 输出位置坐标，单位mm
     * @param rx,ry,rz 输出姿态角度，单位deg
     * @return 错误码，0表示成功
     */
    errno_t ForwardKinematics(double j1, double j2, double j3, double j4, double j5, double j6,
                              double& x, double& y, double& z, double& rx, double& ry, double& rz);

    /**
     * @brief 逆运动学求解（根据末端位姿计算关节角度）
     * @param x,y,z 位置坐标，单位mm
     * @param rx,ry,rz 姿态角度，单位deg
     * @param j1~j6 输出六个关节角度，单位deg
     * @param config 关节空间配置，[-1]-参考当前关节位置解算，[0~7]-依据特定关节空间配置求解，默认-1
     * @return 错误码，0表示成功
     */
    errno_t InverseKinematics(double x, double y, double z, double rx, double ry, double rz,
                              double& j1, double& j2, double& j3, double& j4, double& j5, double& j6,
                              int config = -1);

    /**
     * @brief 获取最后一次操作的错误信息描述
     * @return 错误信息字符串
     */
    std::string GetLastErrorString() const;

private:
    std::unique_ptr<FRRobot> robot_;  ///< Fairino机器人SDK对象
    bool connected_;                   ///< 连接状态
    errno_t last_error_;               ///< 最后一次错误码

    /**
     * @brief 将错误码转换为可读字符串
     * @param error 错误码
     * @return 错误信息字符串
     */
    std::string ErrorToString(errno_t error) const;
};

#endif // FAIRINO_ROBOT_WRAPPER_H