/**
 * @file FairinoRobotWrapper.cpp
 * @brief Fairino机器人SDK二次封装实现
 */

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

FairinoRobotWrapper::FairinoRobotWrapper()
    : robot_(std::make_unique<FRRobot>())
    , connected_(false)
    , last_error_(ERR_SUCCESS)
{
}

FairinoRobotWrapper::~FairinoRobotWrapper()
{
    if (connected_) {
        Disconnect();
    }
}

errno_t FairinoRobotWrapper::Connect(const std::string& ip)
{
    if (connected_) {
        return ERR_SUCCESS;
    }

    last_error_ = robot_->RPC(ip.c_str());
    if (last_error_ == ERR_SUCCESS) {
        connected_ = true;
    }
    return last_error_;
}

errno_t FairinoRobotWrapper::Disconnect()
{
    if (!connected_) {
        return ERR_SUCCESS;
    }

    last_error_ = robot_->CloseRPC();
    connected_ = false;
    return last_error_;
}

bool FairinoRobotWrapper::IsConnected() const
{
    return connected_;
}

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

errno_t FairinoRobotWrapper::MoveL(double x, double y, double z,
                                   double rx, double ry, double rz,
                                   float vel, float acc, float blendR,
                                   int tool, int user)
{
    if (!connected_) {
        last_error_ = ERR_SOCKET_COM_FAILED;
        return last_error_;
    }

    // 构造目标笛卡尔位姿
    DescPose desc_pos(x, y, z, rx, ry, rz);

    // 扩展轴位置（不使用）
    ExaxisPos epos;
    memset(&epos, 0, sizeof(epos));

    // 偏移量（不偏移）
    DescPose offset_pos;

    // 使用不需要关节位置输入的MoveL重载版本
    last_error_ = robot_->MoveL(&desc_pos, tool, user, vel, acc, 100.0f, blendR, 0, &epos, 0, 0, &offset_pos, -1, 0, 0, 10);
    return last_error_;
}

errno_t FairinoRobotWrapper::MoveJByPose(double x, double y, double z,
                                         double rx, double ry, double rz,
                                         float vel, float acc, float blendT,
                                         int tool, int user)
{
    if (!connected_) {
        last_error_ = ERR_SOCKET_COM_FAILED;
        return last_error_;
    }

    // 首先使用逆运动学计算关节位置
    DescPose desc_pos(x, y, z, rx, ry, rz);
    JointPos joint_pos;

    last_error_ = robot_->GetInverseKin(0, &desc_pos, -1, &joint_pos);
    if (last_error_ != ERR_SUCCESS) {
        return last_error_;
    }

    // 扩展轴位置（不使用）
    ExaxisPos epos;
    memset(&epos, 0, sizeof(epos));

    // 偏移量（不偏移）
    DescPose offset_pos;

    // 使用MoveJ进行关节空间运动
    last_error_ = robot_->MoveJ(&joint_pos, &desc_pos, tool, user, vel, acc, 100.0f, &epos, blendT, 0, &offset_pos);
    return last_error_;
}

errno_t FairinoRobotWrapper::MoveJByJoint(double j1, double j2, double j3,
                                          double j4, double j5, double j6,
                                          float vel, float acc, float blendT,
                                          int tool, int user)
{
    if (!connected_) {
        last_error_ = ERR_SOCKET_COM_FAILED;
        return last_error_;
    }

    // 构造目标关节位置
    JointPos joint_pos(j1, j2, j3, j4, j5, j6);

    // 扩展轴位置（不使用）
    ExaxisPos epos;
    memset(&epos, 0, sizeof(epos));

    // 偏移量（不偏移）
    DescPose offset_pos;

    // 使用不需要笛卡尔位置输入的MoveJ重载版本
    last_error_ = robot_->MoveJ(&joint_pos, tool, user, vel, acc, 100.0f, &epos, blendT, 0, &offset_pos);
    return last_error_;
}

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

errno_t FairinoRobotWrapper::StopMotion()
{
    if (!connected_) {
        last_error_ = ERR_SOCKET_COM_FAILED;
        return last_error_;
    }
    last_error_ = robot_->StopMotion();
    return last_error_;
}

errno_t FairinoRobotWrapper::PauseMotion()
{
    if (!connected_) {
        last_error_ = ERR_SOCKET_COM_FAILED;
        return last_error_;
    }
    last_error_ = robot_->PauseMotion();
    return last_error_;
}

errno_t FairinoRobotWrapper::ResumeMotion()
{
    if (!connected_) {
        last_error_ = ERR_SOCKET_COM_FAILED;
        return last_error_;
    }
    last_error_ = robot_->ResumeMotion();
    return last_error_;
}

errno_t FairinoRobotWrapper::RobotEnable(uint8_t state)
{
    if (!connected_) {
        last_error_ = ERR_SOCKET_COM_FAILED;
        return last_error_;
    }
    last_error_ = robot_->RobotEnable(state);
    return last_error_;
}

errno_t FairinoRobotWrapper::ResetAllError()
{
    if (!connected_) {
        last_error_ = ERR_SOCKET_COM_FAILED;
        return last_error_;
    }
    last_error_ = robot_->ResetAllError();
    return last_error_;
}

errno_t FairinoRobotWrapper::GetCurrentJointPos(double& j1, double& j2, double& j3,
                                                double& j4, double& j5, double& j6,
                                                uint8_t blocking)
{
    if (!connected_) {
        last_error_ = ERR_SOCKET_COM_FAILED;
        return last_error_;
    }

    JointPos joint_pos;
    last_error_ = robot_->GetActualJointPosDegree(blocking, &joint_pos);
    if (last_error_ == ERR_SUCCESS) {
        j1 = joint_pos.jPos[0];
        j2 = joint_pos.jPos[1];
        j3 = joint_pos.jPos[2];
        j4 = joint_pos.jPos[3];
        j5 = joint_pos.jPos[4];
        j6 = joint_pos.jPos[5];
    }
    return last_error_;
}

errno_t FairinoRobotWrapper::GetCurrentTCPPose(double& x, double& y, double& z,
                                               double& rx, double& ry, double& rz,
                                               uint8_t blocking)
{
    if (!connected_) {
        last_error_ = ERR_SOCKET_COM_FAILED;
        return last_error_;
    }

    DescPose desc_pos;
    last_error_ = robot_->GetActualTCPPose(blocking, &desc_pos);
    if (last_error_ == ERR_SUCCESS) {
        x = desc_pos.tran.x;
        y = desc_pos.tran.y;
        z = desc_pos.tran.z;
        rx = desc_pos.rpy.rx;
        ry = desc_pos.rpy.ry;
        rz = desc_pos.rpy.rz;
    }
    return last_error_;
}

errno_t FairinoRobotWrapper::GetMotionDone(uint8_t& state)
{
    if (!connected_) {
        last_error_ = ERR_SOCKET_COM_FAILED;
        return last_error_;
    }
    last_error_ = robot_->GetRobotMotionDone(&state);
    return last_error_;
}

errno_t FairinoRobotWrapper::GetErrorCode(int& maincode, int& subcode)
{
    if (!connected_) {
        last_error_ = ERR_SOCKET_COM_FAILED;
        return last_error_;
    }
    last_error_ = robot_->GetRobotErrorCode(&maincode, &subcode);
    return last_error_;
}

errno_t FairinoRobotWrapper::GetRobotState(ROBOT_STATE_PKG& state)
{
    if (!connected_) {
        last_error_ = ERR_SOCKET_COM_FAILED;
        return last_error_;
    }
    last_error_ = robot_->GetRobotRealTimeState(&state);
    return last_error_;
}

errno_t FairinoRobotWrapper::SetToolCoord(int id, double x, double y, double z,
                                          double rx, double ry, double rz)
{
    if (!connected_) {
        last_error_ = ERR_SOCKET_COM_FAILED;
        return last_error_;
    }

    DescPose coord(x, y, z, rx, ry, rz);
    last_error_ = robot_->SetToolCoord(id, &coord, 0, 0, 0, 0);
    return last_error_;
}

errno_t FairinoRobotWrapper::SetWObjCoord(int id, double x, double y, double z,
                                          double rx, double ry, double rz, int refFrame)
{
    if (!connected_) {
        last_error_ = ERR_SOCKET_COM_FAILED;
        return last_error_;
    }

    DescPose coord(x, y, z, rx, ry, rz);
    last_error_ = robot_->SetWObjCoord(id, &coord, refFrame);
    return last_error_;
}

errno_t FairinoRobotWrapper::SetSpeed(int vel)
{
    if (!connected_) {
        last_error_ = ERR_SOCKET_COM_FAILED;
        return last_error_;
    }
    last_error_ = robot_->SetSpeed(vel);
    return last_error_;
}

errno_t FairinoRobotWrapper::Wait(int ms)
{
    if (!connected_) {
        last_error_ = ERR_SOCKET_COM_FAILED;
        return last_error_;
    }
    last_error_ = robot_->WaitMs(ms);
    return last_error_;
}

std::string FairinoRobotWrapper::GetSDKVersion()
{
    char version[64] = {0};
    if (robot_) {
        robot_->GetSDKVersion(version);
    }
    return std::string(version);
}

std::string FairinoRobotWrapper::GetControllerIP()
{
    if (!connected_) {
        return "";
    }

    char ip[64] = {0};
    robot_->GetControllerIP(ip);
    return std::string(ip);
}

errno_t FairinoRobotWrapper::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)
{
    if (!connected_) {
        last_error_ = ERR_SOCKET_COM_FAILED;
        return last_error_;
    }

    JointPos joint_pos(j1, j2, j3, j4, j5, j6);
    DescPose desc_pos;

    last_error_ = robot_->GetForwardKin(&joint_pos, &desc_pos);
    if (last_error_ == ERR_SUCCESS) {
        x = desc_pos.tran.x;
        y = desc_pos.tran.y;
        z = desc_pos.tran.z;
        rx = desc_pos.rpy.rx;
        ry = desc_pos.rpy.ry;
        rz = desc_pos.rpy.rz;
    }
    return last_error_;
}

errno_t FairinoRobotWrapper::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)
{
    if (!connected_) {
        last_error_ = ERR_SOCKET_COM_FAILED;
        return last_error_;
    }

    DescPose desc_pos(x, y, z, rx, ry, rz);
    JointPos joint_pos;

    last_error_ = robot_->GetInverseKin(0, &desc_pos, config, &joint_pos);
    if (last_error_ == ERR_SUCCESS) {
        j1 = joint_pos.jPos[0];
        j2 = joint_pos.jPos[1];
        j3 = joint_pos.jPos[2];
        j4 = joint_pos.jPos[3];
        j5 = joint_pos.jPos[4];
        j6 = joint_pos.jPos[5];
    }
    return last_error_;
}

std::string FairinoRobotWrapper::GetLastErrorString() const
{
    return ErrorToString(last_error_);
}

std::string FairinoRobotWrapper::ErrorToString(errno_t error) const
{
    switch (error) {
    case ERR_SUCCESS:
        return "成功";
    case ERR_OTHER:
        return "其他错误";
    case ERR_SOCKET_COM_FAILED:
        return "网络通讯异常";
    case ERR_XMLRPC_COM_FAILED:
        return "XMLRPC通讯失败，请检查网络连接以及服务器IP地址";
    case ERR_XMLRPC_CMD_FAILED:
        return "XMLRPC接口执行失败";
    case ERR_LUA_FILE_NOT_FOUND:
        return "LUA文件不存在";
    case ERR_SAVE_FILE_PATH_NOT_FOUND:
        return "保存文件路径不存在";
    case ERR_UPLOAD_FILE_NOT_FOUND:
        return "上传文件不存在";
    case ERR_DOWN_LOAD_FILE_FAILED:
        return "文件下载失败";
    case ERR_DOWN_LOAD_FILE_CHECK_FAILED:
        return "文件下载校验失败";
    case ERR_DOWN_LOAD_FILE_WRITE_FAILED:
        return "下载文件写入失败";
    case ERR_FILE_NAME:
        return "文件名称异常";
    case ERR_UPLOAD_FILE_ERROR:
        return "上传文件异常";
    case ERR_FILE_TOO_LARGE:
        return "文件大小超限";
    case ERR_FILE_OPEN_FAILED:
        return "文件打开失败";
    case ERR_SOCKET_SEND_FAILED:
        return "Socket发送失败";
    case ERR_SOCKET_RECV_FAILED:
        return "Socket接收失败";
    case ERR_PARAM_NUM:
        return "参数个数不一致";
    case ERR_PARAM_VALUE:
        return "参数值不在合理范围";
    case ERR_TPD_FILE_OPEN_FAILED:
        return "TPD文件打开失败";
    case ERR_EXECUTION_FAILED:
        return "指令执行失败";
    case ERR_PROGRAM_IS_RUNNING:
        return "程序正在运行";
    case ERR_COMPUTE_FAILED:
        return "计算失败";
    case ERR_INVERSE_KINEMATICS_COMPUTE_FAILED:
        return "逆运动学计算失败";
    case ERR_SERVOJ_JOINT_OVERRUN:
        return "关节值超限";
    case ERR_NON_RESSETTABLE_FAULT:
        return "不可复位故障，请断电重启控制箱";
    case ERR_STRANGE_POSE:
        return "奇异位姿";
    case ERR_TARGET_POSE_CANNOT_REACHED:
        return "目标位姿无法到达";
    default:
        return "未知错误码: " + std::to_string(error);
    }
}