#pragma once

#include "rm_control/common/rm75_constants.h"

#include <array>
#include <filesystem>
#include <string>
#include <vector>

namespace rm_control::common {

// 机械臂连接参数。当前默认值是本地 RM75 控制器，
// 后续换机械臂或换 IP 时应优先通过 JSON 覆盖，而不是改驱动代码。
struct RobotConnectionConfig {
    std::string ip = "192.168.1.18";
    int port = 8080;
    int timeout_ms = 1000;
};

// 通用 TCP 位姿结构，位置单位为 m，欧拉角单位为 rad。
// 这里不用 rm_pose_t，是为了让 core/config 不直接依赖睿尔曼 SDK。
struct Pose {
    float x = 0.0F;
    float y = 0.0F;
    float z = 0.0F;
    float rx = 0.0F;
    float ry = 0.0F;
    float rz = 0.0F;
};

// 只保留上层业务真正需要的机器人基础信息，避免把 RM SDK 结构体泄漏到 core。
struct RobotInfo {
    int arm_dof = 0;
    int arm_model = 0;
    int force_type = 0;
    int controller_version = 0;
    bool is_rm75 = false;
    bool six_axis_force_sensor_available = false;
};

// SDK 与控制器软件信息。firmware_version 明确映射 RM_API2 的 ctrl_info.version，
// 其余分层版本单独保留，避免把不同固件组件误合并成一个版本号。
struct RobotSoftwareInfo {
    std::string sdk_version;
    std::string product_version;
    std::string controller_generation;
    std::string firmware_version;
    std::string firmware_build_time;
    std::string plan_version;
    std::string plan_build_time;
    std::string algorithm_version;
    std::string dynamics_version;
    std::string communication_version;
    std::string communication_build_time;
    std::string program_version;
    std::string program_build_time;
};

// 当前机械臂状态：关节角、TCP 位姿、错误码。
// joints_deg 的长度由真实机械臂自由度决定，RM75 当前是 7。
struct ArmState {
    std::vector<float> joints_deg;
    Pose tcp_pose;
    std::vector<int> error_codes;
};

// 六维力数据。数组顺序固定为 Fx, Fy, Fz, Mx, My, Mz。
// raw/zero/work_zero/tool_zero 对应 RM_API2 返回的四组力数据。
struct ForceData {
    std::array<float, 6> raw{};
    std::array<float, 6> zero{};
    std::array<float, 6> work_zero{};
    std::array<float, 6> tool_zero{};
};

// 安全门控需要的只读机械臂状态。RM_API2 v1.1.5 没有独立的急停查询接口，
// 因此适配器必须保持 emergency_stop_state_available=false，不能根据错误码猜测急停正常。
struct RobotSafetyStatus {
    bool real_mode_enabled = false;
    bool arm_power_on = false;
    bool all_joints_enabled = false;
    bool arm_error_free = false;
    bool controller_error_free = false;
    bool virtual_wall_enabled = false;
    bool self_collision_enabled = false;
    bool emergency_stop_state_available = false;
    bool emergency_stop_normal = false;
    int controller_error_code = 0;
    float controller_voltage = 0.0F;
    float controller_current = 0.0F;
    float controller_temperature = 0.0F;
    std::array<int, kRm75JointCount> joint_enable_states{};
    std::array<int, kRm75JointCount> joint_error_codes{};
    std::vector<int> arm_error_codes;
};

// 控制器当前生效的七轴位置、速度和加速度限制。位置为 degree，速度为
// degree/second。RM_API2 v1.1.5 的 get-max-acc 注释误写为 degree/second，
// 而对应 setter 写为 degree/second^2；本工程按函数“加速度”语义显式使用后者。
struct RobotJointLimits {
    std::array<float, kRm75JointCount> minimum_position_deg{};
    std::array<float, kRm75JointCount> maximum_position_deg{};
    std::array<float, kRm75JointCount> maximum_speed_deg_per_second{};
    std::array<float, kRm75JointCount> maximum_acceleration_deg_per_second_squared{};
};

enum class DragTeachMode {
    OrdinaryRecorded,
    SixDofForcePositionAndOrientation,
};

struct DragTeachStartOptions {
    DragTeachMode mode = DragTeachMode::OrdinaryRecorded;
    bool record_vendor_trajectory = true;
    bool enable_singular_wall = true;
};

// 未实际调用厂商保存接口时不能把默认值 0 误记为成功。
inline constexpr int kVendorApiNotCalledReturnCode = -10000;

// 一次 rm_save_trajectory 调用的完整结果。SDK 返回非零时也必须保留返回码、
// 控制器可能写入的点数和目标路径，供 manifest 与故障报告使用。
struct VendorTrajectorySaveAttempt {
    std::filesystem::path path;
    bool api_called = false;
    int return_code = kVendorApiNotCalledReturnCode;
    int point_count = 0;
    std::string error_message;
};

// 上传到控制器保存或兼容性上传即执行的厂商拖动轨迹工程参数。路径仍使用工程统一的
// std::filesystem::path；具体保存/执行语义由调用的 IRobotArm 方法明确表达。
struct VendorDragTrajectoryExecutionRequest {
    std::filesystem::path trajectory_path;
    int plan_speed_percent = 0;
    int controller_program_id = 0;
};

// 运行控制器中已保存程序的参数。速度必须由调用方显式给出 1..100，禁止传 0
// 隐式沿用控制器中的历史速度；blocking=false 可用于状态机非阻塞监控。
struct ControllerProgramRunRequest {
    int controller_program_id = 0;
    int speed_percent = 0;
    bool blocking = false;
};

// 控制器在线编程运行状态。raw_state 保留 SDK 原值，避免上层把未知状态
// 错当成已完成；当前 SDK 已知 0=停止/结束、1=运行、2=暂停。
struct ControllerProgramRunState {
    int raw_state = -1;
    int program_id = 0;
    int plan_number = 0;
    int plan_speed_percent = 0;
};

struct TrajectorySample {
    std::size_t sample_index = 0;  // 从 0 连续递增。
    long long elapsed_ms = 0;      // steady_clock 相对录制起点，单位 millisecond。
    std::vector<float> joints_deg; // RM75 固定七关节，单位 degree。
    Pose tcp_pose;                 // 位置 meter，欧拉姿态 radian。
    std::string source = "realman_rm75_drag_teach";
};

} // namespace rm_control::common
