#include "flight_robot/realman/realman_arm_adapter.h"
#include "flight_robot/realman/modbus_register_codec.h"

#include "rm_control/common/rm75_constants.h"

#include "rm_interface.h"

#include <algorithm>
#include <array>
#include <chrono>
#include <cstdarg>
#include <cstdint>
#include <cstdio>
#include <cstring>
#include <exception>
#include <filesystem>
#include <string>
#include <thread>

namespace flight_robot::realman {
namespace {

constexpr int kOrdinaryDragTrajectoryRecordEnabled = 1;
constexpr int kSixDofPositionAndOrientationDragMode = 3;
constexpr std::size_t kMaximumVendorTrajectoryPathLength = 299;
constexpr int kMaximumArmErrorCount = 24;
constexpr int kMinimumControllerProgramId = 1;
constexpr int kMaximumControllerProgramId = 100;
constexpr int kMinimumPlanSpeedPercent = 1;
constexpr int kMaximumPlanSpeedPercent = 100;
constexpr int kSaveAndRunControllerProject = 0;
constexpr int kOnlySaveControllerProject = 1;
constexpr int kToolRs485Port = 1;
constexpr int kThirdGenerationController = 3;
constexpr int kToolModbusMasterMode = 1;
constexpr std::chrono::milliseconds kToolModbusModeSettlingDelay(300);
constexpr std::size_t kMinimumMultipleRegisterReadCount = 3;
constexpr std::size_t kMaximumMultipleRegisterReadCount = 12;
constexpr std::size_t kMaximumMultipleRegisterWriteCount = 10;

void apiLogCallback(const char *message, va_list arguments) {
    if (message == nullptr) {
        return;
    }
    std::array<char, 1024> buffer{};
    std::vsnprintf(buffer.data(), buffer.size(), message, arguments);
    std::fprintf(stderr, "[rm_api] %s\n", buffer.data());
}

int destroyApi() noexcept {
#if defined(REALMAN_RM_API_HAS_RM_DESTORY)
    return rm_destory();
#else
    return rm_destroy();
#endif
}

rm_control::common::Result<void> sdkResult(const char *api_name, int return_code) {
    if (return_code == 0) {
        return rm_control::common::Result<void>::success();
    }
    return rm_control::common::Result<void>::failure(
        std::string(api_name) + " failed, return_code=" + std::to_string(return_code));
}

rm_control::common::Result<void> ensureConnected(bool initialized,
                                                  const rm_robot_handle *handle) {
    if (!initialized || handle == nullptr || handle->id <= 0) {
        return rm_control::common::Result<void>::failure(
            "RealmanArmAdapter requires an active RM75 connection");
    }
    return rm_control::common::Result<void>::success();
}

rm_control::common::Result<void> ValidateToolModbusConfiguration(
    const rm_control::interfaces::ToolModbusBusConfiguration &configuration) {
    const bool supported_baudrate = configuration.baudrate == 9600 ||
        configuration.baudrate == 115200 || configuration.baudrate == 460800;
    const bool valid_delays = configuration.power_off_delay_ms >= 0 &&
        configuration.power_off_delay_ms <= 10000 &&
        configuration.startup_delay_ms >= 0 &&
        configuration.startup_delay_ms <= 10000;
    if (configuration.port != kToolRs485Port || !supported_baudrate ||
        configuration.voltage_type < 0 || configuration.voltage_type > 3 ||
        configuration.timeout_units <= 0 || configuration.timeout_units > 1000 ||
        !valid_delays) {
        return rm_control::common::Result<void>::failure(
            "tool Modbus configuration requires port=1, supported baudrate, "
            "voltage_type=0..3, timeout_units=1..1000, and delays=0..10000 ms");
    }
    return rm_control::common::Result<void>::success();
}

rm_control::common::Result<void> VerifyThirdGenerationController(
    rm_robot_handle *handle) {
    rm_robot_info_t information{};
    const int return_code = rm_get_robot_info(handle, &information);
    if (return_code != 0) {
        return sdkResult("rm_get_robot_info", return_code);
    }
    if (static_cast<int>(information.robot_controller_version) !=
        kThirdGenerationController) {
        return rm_control::common::Result<void>::failure(
            "legacy tool Modbus initialization requires controller generation 3; actual=" +
            std::to_string(static_cast<int>(information.robot_controller_version)));
    }
    return rm_control::common::Result<void>::success();
}

rm_control::common::Result<void> SetToolVoltageAndVerify(
    rm_robot_handle *handle,
    int voltage_type) {
    auto result = sdkResult(
        "rm_set_tool_voltage", rm_set_tool_voltage(handle, voltage_type));
    if (!result.ok) {
        return result;
    }
    int actual_voltage_type = -1;
    result = sdkResult(
        "rm_get_tool_voltage", rm_get_tool_voltage(handle, &actual_voltage_type));
    if (!result.ok) {
        return result;
    }
    if (actual_voltage_type != voltage_type) {
        return rm_control::common::Result<void>::failure(
            "tool voltage readback mismatch: requested=" +
            std::to_string(voltage_type) + ", actual=" +
            std::to_string(actual_voltage_type));
    }
    return rm_control::common::Result<void>::success();
}

rm_control::common::Result<void> ResetToolModbusModeIfNeeded(
    rm_robot_handle *handle,
    int port,
    int current_mode,
    bool reset_existing_mode) {
    if (!reset_existing_mode || current_mode != kToolModbusMasterMode) {
        return rm_control::common::Result<void>::success();
    }
    return sdkResult("rm_close_modbus_mode", rm_close_modbus_mode(handle, port));
}

using RmMultipleRegisterReadFunction = int (*)(
    rm_robot_handle *, rm_peripheral_read_write_params_t, int *);

struct MultipleRegisterReadRequest {
    std::uint8_t device_id = 0U;
    std::uint16_t start_address = 0U;
    std::size_t register_count = 0U;
    const char *api_name = nullptr;
    RmMultipleRegisterReadFunction read_function = nullptr;
};

rm_control::common::Result<std::vector<std::uint16_t>>
ReadMultipleRegistersFromRmApi(bool initialized,
                               rm_robot_handle *handle,
                               const MultipleRegisterReadRequest &request) {
    const auto connected = ensureConnected(initialized, handle);
    if (!connected.ok) {
        return rm_control::common::Result<std::vector<std::uint16_t>>::failure(
            connected.error_message);
    }
    if (request.device_id == 0U || request.device_id > 247U ||
        request.register_count < kMinimumMultipleRegisterReadCount ||
        request.register_count > kMaximumMultipleRegisterReadCount ||
        request.api_name == nullptr || request.read_function == nullptr) {
        return rm_control::common::Result<std::vector<std::uint16_t>>::failure(
            "RM_API2 multiple-register read requires device 1..247, count 3..12, "
            "and a valid SDK function");
    }
    rm_peripheral_read_write_params_t parameters{};
    parameters.port = kToolRs485Port;
    parameters.address = static_cast<int>(request.start_address);
    parameters.device = static_cast<int>(request.device_id);
    parameters.num = static_cast<int>(request.register_count);
    std::vector<int> bytes(request.register_count * 2U, 0);
    const int return_code = request.read_function(handle, parameters, bytes.data());
    if (return_code != 0) {
        return rm_control::common::Result<std::vector<std::uint16_t>>::failure(
            std::string(request.api_name) + " failed, return_code=" +
            std::to_string(return_code) + ", device=" +
            std::to_string(static_cast<int>(request.device_id)) + ", address=" +
            std::to_string(request.start_address) + ", count=" +
            std::to_string(request.register_count));
    }
    const auto decoded = DecodeModbusRegistersFromRmApiHighLowBytes(bytes);
    if (!decoded.ok) {
        return rm_control::common::Result<std::vector<std::uint16_t>>::failure(
            std::string(request.api_name) + " returned invalid bytes: " +
            decoded.error_message);
    }
    return decoded;
}

template <std::size_t Size>
std::string boundedString(const char (&text)[Size]) {
    const auto end = std::find(text, text + Size, '\0');
    return std::string(text, end);
}

rm_control::common::Pose convertPose(const rm_pose_t &pose) {
    rm_control::common::Pose result;
    result.x = pose.position.x;
    result.y = pose.position.y;
    result.z = pose.position.z;
    result.rx = pose.euler.rx;
    result.ry = pose.euler.ry;
    result.rz = pose.euler.rz;
    return result;
}

rm_pose_t convertPose(const rm_control::common::Pose &pose) {
    rm_pose_t result{};
    result.position.x = pose.x;
    result.position.y = pose.y;
    result.position.z = pose.z;
    result.euler.rx = pose.rx;
    result.euler.ry = pose.ry;
    result.euler.rz = pose.rz;
    result.quaternion.w = 1.0F;
    return result;
}

void appendNonZeroErrors(const rm_err_t &source, std::vector<int> *destination) {
    if (destination == nullptr) {
        return;
    }
    const int count = std::min(static_cast<int>(source.err_len), kMaximumArmErrorCount);
    for (int index = 0; index < count; ++index) {
        if (source.err[index] != 0) {
            destination->push_back(source.err[index]);
        }
    }
}

rm_control::common::Result<void> readRunModeAndPower(
    rm_robot_handle *handle,
    rm_control::common::RobotSafetyStatus *status) {
    int run_mode = 0;
    int return_code = rm_get_arm_run_mode(handle, &run_mode);
    if (return_code != 0) {
        return sdkResult("rm_get_arm_run_mode", return_code);
    }
    int power_state = 0;
    return_code = rm_get_arm_power_state(handle, &power_state);
    if (return_code != 0) {
        return sdkResult("rm_get_arm_power_state", return_code);
    }
    status->real_mode_enabled = run_mode == 1;
    status->arm_power_on = power_state == 1;
    return rm_control::common::Result<void>::success();
}

rm_control::common::Result<void> readJointAndArmSafety(
    rm_robot_handle *handle,
    rm_control::common::RobotSafetyStatus *status) {
    rm_arm_all_state_t arm_state{};
    const int return_code = rm_get_arm_all_state(handle, &arm_state);
    if (return_code != 0) {
        return sdkResult("rm_get_arm_all_state", return_code);
    }
    status->all_joints_enabled = true;
    for (std::size_t index = 0; index < rm_control::common::kRm75JointCount; ++index) {
        status->joint_enable_states[index] = arm_state.joint_en_flag[index];
        status->joint_error_codes[index] = arm_state.joint_err_code[index];
        status->all_joints_enabled =
            status->all_joints_enabled && arm_state.joint_en_flag[index] == 1;
    }
    appendNonZeroErrors(arm_state.err, &status->arm_error_codes);
    status->arm_error_free = status->arm_error_codes.empty() &&
                             std::all_of(status->joint_error_codes.begin(),
                                         status->joint_error_codes.end(),
                                         [](int code) { return code == 0; });
    return rm_control::common::Result<void>::success();
}

rm_control::common::Result<void> readControllerSafety(
    rm_robot_handle *handle,
    rm_control::common::RobotSafetyStatus *status) {
    const int return_code = rm_get_controller_state(handle,
                                                     &status->controller_voltage,
                                                     &status->controller_current,
                                                     &status->controller_temperature,
                                                     &status->controller_error_code);
    if (return_code != 0) {
        return sdkResult("rm_get_controller_state", return_code);
    }
    status->controller_error_free = status->controller_error_code == 0;
    return rm_control::common::Result<void>::success();
}

rm_control::common::Result<void> readConfiguredSafetyFeatures(
    rm_robot_handle *handle,
    rm_control::common::RobotSafetyStatus *status) {
    rm_electronic_fence_enable_t virtual_wall{};
    int return_code = rm_get_virtual_wall_enable(handle, &virtual_wall);
    if (return_code != 0) {
        return sdkResult("rm_get_virtual_wall_enable", return_code);
    }
    bool self_collision_enabled = false;
    return_code = rm_get_self_collision_enable(handle, &self_collision_enabled);
    if (return_code != 0) {
        return sdkResult("rm_get_self_collision_enable", return_code);
    }
    status->virtual_wall_enabled = virtual_wall.enable_state;
    status->self_collision_enabled = self_collision_enabled;
    return rm_control::common::Result<void>::success();
}

rm_control::common::Result<bool> queryControllerProgramSlotAvailable(
    rm_robot_handle *handle,
    int controller_program_id) {
    rm_program_trajectorys_t programs{};
    const int return_code = rm_get_program_trajectory_list(
        handle, 1, 100, "", &programs);
    if (return_code != 0) {
        return rm_control::common::Result<bool>::failure(
            sdkResult("rm_get_program_trajectory_list", return_code).error_message);
    }
    const int count = std::clamp(programs.list_size, 0, 100);
    for (int index = 0; index < count; ++index) {
        if (programs.trajectory_list[index].id == controller_program_id) {
            return rm_control::common::Result<bool>::success(false);
        }
    }
    return rm_control::common::Result<bool>::success(true);
}

rm_control::common::Result<std::string> validateVendorDragTrajectoryRequest(
    const rm_control::common::VendorDragTrajectoryExecutionRequest &request) {
    std::error_code filesystem_error;
    if (!std::filesystem::is_regular_file(request.trajectory_path, filesystem_error) ||
        filesystem_error) {
        return rm_control::common::Result<std::string>::failure(
            "RM75 vendor drag trajectory must be an existing regular file: " +
            request.trajectory_path.string());
    }
    const std::string path = request.trajectory_path.string();
    if (path.empty() || path.size() > kMaximumVendorTrajectoryPathLength) {
        return rm_control::common::Result<std::string>::failure(
            "rm_send_project path must contain 1..299 bytes, actual=" +
            std::to_string(path.size()));
    }
    if (request.plan_speed_percent < kMinimumPlanSpeedPercent ||
        request.plan_speed_percent > kMaximumPlanSpeedPercent) {
        return rm_control::common::Result<std::string>::failure(
            "rm_send_project plan speed percent must be in the range 1..100");
    }
    if (request.controller_program_id < kMinimumControllerProgramId ||
        request.controller_program_id > kMaximumControllerProgramId) {
        return rm_control::common::Result<std::string>::failure(
            "rm_send_project controller program id must be in the range 1..100");
    }
    return rm_control::common::Result<std::string>::success(path);
}

rm_control::common::Result<void> requireControllerProgramSlotAvailable(
    rm_robot_handle *handle,
    int controller_program_id) {
    const auto available = queryControllerProgramSlotAvailable(handle, controller_program_id);
    if (!available.ok) {
        return rm_control::common::Result<void>::failure(available.error_message);
    }
    if (!available.value) {
        return rm_control::common::Result<void>::failure(
            "refusing to overwrite existing controller program id=" +
            std::to_string(controller_program_id));
    }
    return rm_control::common::Result<void>::success();
}

rm_control::common::Result<void> requireControllerProgramSlotOccupied(
    rm_robot_handle *handle,
    int controller_program_id) {
    const auto available = queryControllerProgramSlotAvailable(handle, controller_program_id);
    if (!available.ok) {
        return rm_control::common::Result<void>::failure(
            "failed to confirm saved controller program id=" +
            std::to_string(controller_program_id) + ": " + available.error_message);
    }
    if (available.value) {
        return rm_control::common::Result<void>::failure(
            "rm_send_project reported success but controller program id=" +
            std::to_string(controller_program_id) + " is absent");
    }
    return rm_control::common::Result<void>::success();
}

rm_control::common::Result<void> sendVendorDragTrajectoryProject(
    rm_robot_handle *handle,
    const rm_control::common::VendorDragTrajectoryExecutionRequest &request,
    const std::string &path,
    int only_save) {
    rm_send_project_t project{};
    std::memcpy(project.project_path, path.data(), path.size());
    project.project_path[path.size()] = '\0';
    project.project_path_len = static_cast<int>(path.size());
    project.plan_speed = request.plan_speed_percent;
    project.only_save = only_save;
    project.save_id = request.controller_program_id;
    project.step_flag = 0;
    project.auto_start = 0;
    project.project_type = 1;

    int error_line = -1;
    const int return_code = rm_send_project(handle, project, &error_line);
    if (return_code != 0 || error_line != -1) {
        return rm_control::common::Result<void>::failure(
            "rm_send_project(project_type=1, only_save=" + std::to_string(only_save) +
            ") failed, return_code=" + std::to_string(return_code) +
            ", error_line=" + std::to_string(error_line));
    }
    return rm_control::common::Result<void>::success();
}

rm_control::common::Result<void> validateControllerProgramRunRequest(
    const rm_control::common::ControllerProgramRunRequest &request) {
    if (request.controller_program_id < kMinimumControllerProgramId ||
        request.controller_program_id > kMaximumControllerProgramId) {
        return rm_control::common::Result<void>::failure(
            "rm_set_program_id_run controller program id must be in the range 1..100");
    }
    if (request.speed_percent < kMinimumPlanSpeedPercent ||
        request.speed_percent > kMaximumPlanSpeedPercent) {
        return rm_control::common::Result<void>::failure(
            "rm_set_program_id_run speed percent must be in the range 1..100");
    }
    return rm_control::common::Result<void>::success();
}

} // namespace

class RealmanArmAdapter::Implementation {
public:
    rm_robot_handle *handle = nullptr;
    bool initialized = false;
    int dof = 0;
};

static_assert(ARM_DOF == rm_control::common::kRm75JointCount,
              "RM_API2 ARM_DOF must match the RM75 seven-joint contract");

RealmanArmAdapter::RealmanArmAdapter()
    : implementation_(std::make_unique<Implementation>()) {}

RealmanArmAdapter::~RealmanArmAdapter() noexcept {
    disconnect();
}

rm_control::common::Result<void> RealmanArmAdapter::connect(
    const rm_control::common::RobotConnectionConfig &config) {
    if (config.ip.empty() || config.port <= 0 || config.port > 65535 || config.timeout_ms <= 0) {
        return rm_control::common::Result<void>::failure(
            "RealmanArmAdapter connection config has invalid IP, port, or timeout");
    }
    disconnect();
    rm_set_log_call_back(apiLogCallback, 2);
    rm_set_timeout(config.timeout_ms);
    const int init_return_code = rm_init(RM_TRIPLE_MODE_E);
    if (init_return_code != 0) {
        return sdkResult("rm_init", init_return_code);
    }
    implementation_->initialized = true;
    implementation_->handle = rm_create_robot_arm(config.ip.c_str(), config.port);
    if (implementation_->handle == nullptr || implementation_->handle->id <= 0) {
        disconnect();
        return rm_control::common::Result<void>::failure(
            "rm_create_robot_arm failed for " + config.ip + ":" + std::to_string(config.port));
    }
    const auto info = getRobotInfo();
    if (!info.ok || info.value.arm_dof != static_cast<int>(rm_control::common::kRm75JointCount) ||
        !info.value.is_rm75) {
        const std::string context = info.ok ? "connected robot is not an RM75 with seven joints"
                                            : info.error_message;
        disconnect();
        return rm_control::common::Result<void>::failure(context);
    }
    implementation_->dof = info.value.arm_dof;
    return rm_control::common::Result<void>::success();
}

void RealmanArmAdapter::disconnect() {
    if (implementation_ == nullptr) {
        return;
    }
    if (implementation_->handle != nullptr) {
        const int delete_return_code = rm_delete_robot_arm(implementation_->handle);
        if (delete_return_code != 0) {
            std::fprintf(stderr, "[rm_api] rm_delete_robot_arm failed, return_code=%d\n",
                         delete_return_code);
        }
        implementation_->handle = nullptr;
    }
    if (implementation_->initialized) {
        const int destroy_return_code = destroyApi();
        if (destroy_return_code != 0) {
            std::fprintf(stderr, "[rm_api] RM API destroy failed, return_code=%d\n",
                         destroy_return_code);
        }
        implementation_->initialized = false;
    }
    implementation_->dof = 0;
}

bool RealmanArmAdapter::isConnected() const noexcept {
    return implementation_ != nullptr && implementation_->initialized &&
           implementation_->handle != nullptr && implementation_->handle->id > 0;
}

rm_control::common::Result<rm_control::common::RobotInfo>
RealmanArmAdapter::getRobotInfo() {
    const auto connected = ensureConnected(implementation_->initialized, implementation_->handle);
    if (!connected.ok) {
        return rm_control::common::Result<rm_control::common::RobotInfo>::failure(
            connected.error_message);
    }
    rm_robot_info_t info{};
    const int return_code = rm_get_robot_info(implementation_->handle, &info);
    if (return_code != 0) {
        return rm_control::common::Result<rm_control::common::RobotInfo>::failure(
            sdkResult("rm_get_robot_info", return_code).error_message);
    }
    rm_control::common::RobotInfo output;
    output.arm_dof = static_cast<int>(info.arm_dof);
    output.arm_model = static_cast<int>(info.arm_model);
    output.force_type = static_cast<int>(info.force_type);
    output.controller_version = static_cast<int>(info.robot_controller_version);
    output.is_rm75 = info.arm_model == RM_MODEL_RM_75_E;
    output.six_axis_force_sensor_available =
        info.force_type == RM_MODEL_RM_SF_E || info.force_type == RM_MODEL_RM_ISF_E;
    return rm_control::common::Result<rm_control::common::RobotInfo>::success(output);
}

rm_control::common::Result<rm_control::common::RobotSoftwareInfo>
RealmanArmAdapter::getRobotSoftwareInfo() {
    const auto connected = ensureConnected(implementation_->initialized, implementation_->handle);
    if (!connected.ok) {
        return rm_control::common::Result<rm_control::common::RobotSoftwareInfo>::failure(
            connected.error_message);
    }
    rm_arm_software_version_t info{};
    const int return_code = rm_get_arm_software_info(implementation_->handle, &info);
    if (return_code != 0) {
        return rm_control::common::Result<rm_control::common::RobotSoftwareInfo>::failure(
            sdkResult("rm_get_arm_software_info", return_code).error_message);
    }
    rm_control::common::RobotSoftwareInfo output;
    const char *api_version = rm_api_version();
    output.sdk_version = api_version == nullptr ? "unknown" : api_version;
    output.product_version = boundedString(info.product_version);
    output.controller_generation = boundedString(info.robot_controller_version);
    output.firmware_version = boundedString(info.ctrl_info.version);
    output.firmware_build_time = boundedString(info.ctrl_info.build_time);
    output.plan_version = boundedString(info.plan_info.version);
    output.plan_build_time = boundedString(info.plan_info.build_time);
    output.algorithm_version = boundedString(info.algorithm_info.version);
    output.dynamics_version = boundedString(info.dynamic_info.model_version);
    output.communication_version = boundedString(info.com_info.version);
    output.communication_build_time = boundedString(info.com_info.build_time);
    output.program_version = boundedString(info.program_info.version);
    output.program_build_time = boundedString(info.program_info.build_time);
    return rm_control::common::Result<rm_control::common::RobotSoftwareInfo>::success(output);
}

rm_control::common::Result<rm_control::common::ArmState>
RealmanArmAdapter::getCurrentArmState() {
    const auto connected = ensureConnected(implementation_->initialized, implementation_->handle);
    if (!connected.ok) {
        return rm_control::common::Result<rm_control::common::ArmState>::failure(
            connected.error_message);
    }
    rm_current_arm_state_t state{};
    const int return_code = rm_get_current_arm_state(implementation_->handle, &state);
    if (return_code != 0) {
        return rm_control::common::Result<rm_control::common::ArmState>::failure(
            sdkResult("rm_get_current_arm_state", return_code).error_message);
    }
    rm_control::common::ArmState output;
    output.joints_deg.assign(state.joint,
                             state.joint + rm_control::common::kRm75JointCount);
    output.tcp_pose = convertPose(state.pose);
    appendNonZeroErrors(state.err, &output.error_codes);
    return rm_control::common::Result<rm_control::common::ArmState>::success(output);
}

rm_control::common::Result<rm_control::common::ForceData>
RealmanArmAdapter::getForceData() {
    const auto connected = ensureConnected(implementation_->initialized, implementation_->handle);
    if (!connected.ok) {
        return rm_control::common::Result<rm_control::common::ForceData>::failure(
            connected.error_message);
    }
    rm_force_data_t data{};
    const int return_code = rm_get_force_data(implementation_->handle, &data);
    if (return_code != 0) {
        return rm_control::common::Result<rm_control::common::ForceData>::failure(
            sdkResult("rm_get_force_data", return_code).error_message);
    }
    rm_control::common::ForceData output;
    for (std::size_t index = 0; index < output.raw.size(); ++index) {
        output.raw[index] = data.force_data[index];
        output.zero[index] = data.zero_force_data[index];
        output.work_zero[index] = data.work_zero_force_data[index];
        output.tool_zero[index] = data.tool_zero_force_data[index];
    }
    return rm_control::common::Result<rm_control::common::ForceData>::success(output);
}

rm_control::common::Result<rm_control::common::RobotSafetyStatus>
RealmanArmAdapter::getRobotSafetyStatus() {
    const auto connected = ensureConnected(implementation_->initialized, implementation_->handle);
    if (!connected.ok) {
        return rm_control::common::Result<rm_control::common::RobotSafetyStatus>::failure(
            connected.error_message);
    }
    rm_control::common::RobotSafetyStatus status;
    const auto mode_and_power = readRunModeAndPower(implementation_->handle, &status);
    if (!mode_and_power.ok) {
        return rm_control::common::Result<rm_control::common::RobotSafetyStatus>::failure(
            mode_and_power.error_message);
    }
    const auto arm_state = readJointAndArmSafety(implementation_->handle, &status);
    if (!arm_state.ok) {
        return rm_control::common::Result<rm_control::common::RobotSafetyStatus>::failure(
            arm_state.error_message);
    }
    const auto controller = readControllerSafety(implementation_->handle, &status);
    if (!controller.ok) {
        return rm_control::common::Result<rm_control::common::RobotSafetyStatus>::failure(
            controller.error_message);
    }
    const auto configured_features = readConfiguredSafetyFeatures(implementation_->handle, &status);
    if (!configured_features.ok) {
        return rm_control::common::Result<rm_control::common::RobotSafetyStatus>::failure(
            configured_features.error_message);
    }
    return rm_control::common::Result<rm_control::common::RobotSafetyStatus>::success(status);
}

rm_control::common::Result<rm_control::common::RobotJointLimits>
RealmanArmAdapter::getJointLimits() {
    const auto connected = ensureConnected(implementation_->initialized, implementation_->handle);
    if (!connected.ok) {
        return rm_control::common::Result<rm_control::common::RobotJointLimits>::failure(
            connected.error_message);
    }

    rm_control::common::RobotJointLimits limits;
    const int minimum_return_code = rm_get_joint_min_pos(
        implementation_->handle, limits.minimum_position_deg.data());
    if (minimum_return_code != 0) {
        return rm_control::common::Result<rm_control::common::RobotJointLimits>::failure(
            sdkResult("rm_get_joint_min_pos", minimum_return_code).error_message);
    }
    const int maximum_return_code = rm_get_joint_max_pos(
        implementation_->handle, limits.maximum_position_deg.data());
    if (maximum_return_code != 0) {
        return rm_control::common::Result<rm_control::common::RobotJointLimits>::failure(
            sdkResult("rm_get_joint_max_pos", maximum_return_code).error_message);
    }
    const int speed_return_code = rm_get_joint_max_speed(
        implementation_->handle, limits.maximum_speed_deg_per_second.data());
    if (speed_return_code != 0) {
        return rm_control::common::Result<rm_control::common::RobotJointLimits>::failure(
            sdkResult("rm_get_joint_max_speed", speed_return_code).error_message);
    }
    const int acceleration_return_code = rm_get_joint_max_acc(
        implementation_->handle,
        limits.maximum_acceleration_deg_per_second_squared.data());
    if (acceleration_return_code != 0) {
        return rm_control::common::Result<rm_control::common::RobotJointLimits>::failure(
            sdkResult("rm_get_joint_max_acc", acceleration_return_code).error_message);
    }
    return rm_control::common::Result<rm_control::common::RobotJointLimits>::success(limits);
}

rm_control::common::Result<void> RealmanArmAdapter::moveJ(
    const std::vector<float> &joints_deg,
    int speed_percent,
    int blend_radius,
    int trajectory_connect,
    bool blocking) {
    const auto connected = ensureConnected(implementation_->initialized, implementation_->handle);
    if (!connected.ok) {
        return connected;
    }
    if (joints_deg.size() != rm_control::common::kRm75JointCount ||
        implementation_->dof != static_cast<int>(rm_control::common::kRm75JointCount)) {
        return rm_control::common::Result<void>::failure(
            "RealmanArmAdapter::moveJ requires exactly seven RM75 joint angles");
    }
    std::array<float, rm_control::common::kRm75JointCount> target{};
    std::copy(joints_deg.begin(), joints_deg.end(), target.begin());
    const int return_code = rm_movej(implementation_->handle,
                                     target.data(),
                                     speed_percent,
                                     blend_radius,
                                     trajectory_connect,
                                     blocking ? 1 : 0);
    return sdkResult("rm_movej", return_code);
}

rm_control::common::Result<void> RealmanArmAdapter::moveL(
    const rm_control::common::Pose &pose,
    int speed_percent,
    int blend_radius,
    int trajectory_connect,
    bool blocking) {
    const auto connected = ensureConnected(implementation_->initialized, implementation_->handle);
    if (!connected.ok) {
        return connected;
    }
    const rm_pose_t target = convertPose(pose);
    const int return_code = rm_movel(implementation_->handle,
                                     target,
                                     speed_percent,
                                     blend_radius,
                                     trajectory_connect,
                                     blocking ? 1 : 0);
    return sdkResult("rm_movel", return_code);
}

rm_control::common::Result<void> RealmanArmAdapter::startDragTeach(
    const rm_control::common::DragTeachStartOptions &options) {
    const auto connected = ensureConnected(implementation_->initialized, implementation_->handle);
    if (!connected.ok) {
        return connected;
    }
    if (options.mode == rm_control::common::DragTeachMode::OrdinaryRecorded) {
        const int record_flag = options.record_vendor_trajectory
                                    ? kOrdinaryDragTrajectoryRecordEnabled
                                    : 0;
        return sdkResult("rm_start_drag_teach",
                         rm_start_drag_teach(implementation_->handle, record_flag));
    }
    const int singular_wall = options.enable_singular_wall ? 1 : 0;
    return sdkResult("rm_start_multi_drag_teach(mode=3)",
                     rm_start_multi_drag_teach(implementation_->handle,
                                               kSixDofPositionAndOrientationDragMode,
                                               singular_wall));
}

rm_control::common::Result<void> RealmanArmAdapter::stopDragTeach() {
    const auto connected = ensureConnected(implementation_->initialized, implementation_->handle);
    if (!connected.ok) {
        return connected;
    }
    return sdkResult("rm_stop_drag_teach", rm_stop_drag_teach(implementation_->handle));
}

rm_control::common::VendorTrajectorySaveAttempt
RealmanArmAdapter::saveDragTeachTrajectory(const std::filesystem::path &path) {
    rm_control::common::VendorTrajectorySaveAttempt attempt;
    attempt.path = path;
    const auto connected = ensureConnected(implementation_->initialized, implementation_->handle);
    if (!connected.ok) {
        attempt.error_message = connected.error_message;
        return attempt;
    }
    std::string native_path;
    try {
        native_path = path.string();
    } catch (const std::exception &exception) {
        attempt.error_message =
            "rm_save_trajectory path conversion failed: " + std::string(exception.what());
        return attempt;
    }
    std::error_code filesystem_error;
    if (std::filesystem::exists(path, filesystem_error)) {
        attempt.error_message =
            "refusing to overwrite existing RM75 raw trajectory: " + native_path;
        return attempt;
    }
    if (filesystem_error) {
        attempt.error_message = "failed to inspect RM75 raw trajectory target " +
                                native_path + ": " + filesystem_error.message();
        return attempt;
    }
    if (path.parent_path().empty() ||
        !std::filesystem::is_directory(path.parent_path(), filesystem_error) ||
        filesystem_error) {
        attempt.error_message =
            "RM75 raw trajectory parent must be an existing directory for " + native_path;
        return attempt;
    }
    if (native_path.empty() || native_path.size() > kMaximumVendorTrajectoryPathLength) {
        attempt.error_message =
            "rm_save_trajectory path must contain 1..299 bytes, actual=" +
            std::to_string(native_path.size());
        return attempt;
    }
    attempt.api_called = true;
    attempt.return_code = rm_save_trajectory(implementation_->handle,
                                             native_path.c_str(),
                                             &attempt.point_count);
    if (attempt.return_code != 0) {
        attempt.error_message = "rm_save_trajectory failed, return_code=" +
                                std::to_string(attempt.return_code);
    }
    return attempt;
}

rm_control::common::Result<void>
RealmanArmAdapter::moveToDragTrajectoryOrigin(bool blocking) {
    const auto connected = ensureConnected(implementation_->initialized, implementation_->handle);
    if (!connected.ok) {
        return connected;
    }
    return sdkResult("rm_drag_trajectory_origin",
                     rm_drag_trajectory_origin(implementation_->handle, blocking ? 1 : 0));
}

rm_control::common::Result<void>
RealmanArmAdapter::replayLastDragTrajectory(bool blocking) {
    const auto connected = ensureConnected(implementation_->initialized, implementation_->handle);
    if (!connected.ok) {
        return connected;
    }
    return sdkResult("rm_run_drag_trajectory",
                     rm_run_drag_trajectory(implementation_->handle, blocking ? 1 : 0));
}

rm_control::common::Result<void> RealmanArmAdapter::pauseDragTrajectoryReplay() {
    const auto connected = ensureConnected(implementation_->initialized, implementation_->handle);
    if (!connected.ok) {
        return connected;
    }
    return sdkResult("rm_pause_drag_trajectory",
                     rm_pause_drag_trajectory(implementation_->handle));
}

rm_control::common::Result<void> RealmanArmAdapter::continueDragTrajectoryReplay() {
    const auto connected = ensureConnected(implementation_->initialized, implementation_->handle);
    if (!connected.ok) {
        return connected;
    }
    return sdkResult("rm_continue_drag_trajectory",
                     rm_continue_drag_trajectory(implementation_->handle));
}

rm_control::common::Result<void> RealmanArmAdapter::stopDragTrajectoryReplay() {
    const auto connected = ensureConnected(implementation_->initialized, implementation_->handle);
    if (!connected.ok) {
        return connected;
    }
    return sdkResult("rm_stop_drag_trajectory",
                     rm_stop_drag_trajectory(implementation_->handle));
}

rm_control::common::Result<void> RealmanArmAdapter::saveVendorDragTrajectoryProgram(
    const rm_control::common::VendorDragTrajectoryExecutionRequest &request) {
    const auto connected = ensureConnected(implementation_->initialized, implementation_->handle);
    if (!connected.ok) {
        return connected;
    }
    const auto validated_path = validateVendorDragTrajectoryRequest(request);
    if (!validated_path.ok) {
        return rm_control::common::Result<void>::failure(validated_path.error_message);
    }
    const auto available = requireControllerProgramSlotAvailable(
        implementation_->handle, request.controller_program_id);
    if (!available.ok) {
        return available;
    }
    const auto saved = sendVendorDragTrajectoryProject(
        implementation_->handle, request, validated_path.value, kOnlySaveControllerProject);
    if (!saved.ok) {
        return saved;
    }
    return requireControllerProgramSlotOccupied(
        implementation_->handle, request.controller_program_id);
}

rm_control::common::Result<void> RealmanArmAdapter::runControllerProgram(
    const rm_control::common::ControllerProgramRunRequest &request) {
    const auto connected = ensureConnected(implementation_->initialized, implementation_->handle);
    if (!connected.ok) {
        return connected;
    }
    const auto valid_request = validateControllerProgramRunRequest(request);
    if (!valid_request.ok) {
        return valid_request;
    }
    const int blocking_mode = request.blocking ? RM_MOVE_MULTI_BLOCK : RM_MOVE_NBLOCK;
    return sdkResult(
        "rm_set_program_id_run",
        rm_set_program_id_run(implementation_->handle,
                              request.controller_program_id,
                              request.speed_percent,
                              blocking_mode));
}

rm_control::common::Result<void> RealmanArmAdapter::executeVendorDragTrajectoryFile(
    const rm_control::common::VendorDragTrajectoryExecutionRequest &request) {
    const auto connected = ensureConnected(implementation_->initialized, implementation_->handle);
    if (!connected.ok) {
        return connected;
    }
    const auto validated_path = validateVendorDragTrajectoryRequest(request);
    if (!validated_path.ok) {
        return rm_control::common::Result<void>::failure(validated_path.error_message);
    }
    const auto unused_program_slot = requireControllerProgramSlotAvailable(
        implementation_->handle, request.controller_program_id);
    if (!unused_program_slot.ok) {
        return unused_program_slot;
    }
    return sendVendorDragTrajectoryProject(
        implementation_->handle, request, validated_path.value, kSaveAndRunControllerProject);
}

rm_control::common::Result<rm_control::common::ControllerProgramRunState>
RealmanArmAdapter::getControllerProgramRunState() {
    const auto connected = ensureConnected(implementation_->initialized, implementation_->handle);
    if (!connected.ok) {
        return rm_control::common::Result<rm_control::common::ControllerProgramRunState>::failure(
            connected.error_message);
    }
    rm_program_run_state_t state{};
    const int return_code = rm_get_program_run_state(implementation_->handle, &state);
    if (return_code != 0) {
        return rm_control::common::Result<rm_control::common::ControllerProgramRunState>::failure(
            sdkResult("rm_get_program_run_state", return_code).error_message);
    }
    rm_control::common::ControllerProgramRunState output;
    output.raw_state = state.run_state;
    output.program_id = state.id;
    output.plan_number = state.plan_num;
    output.plan_speed_percent = state.plan_speed;
    return rm_control::common::Result<rm_control::common::ControllerProgramRunState>::success(
        output);
}

rm_control::common::Result<void> RealmanArmAdapter::slowStopMotion() {
    const auto connected = ensureConnected(implementation_->initialized, implementation_->handle);
    if (!connected.ok) {
        return connected;
    }
    return sdkResult("rm_set_arm_slow_stop",
                     rm_set_arm_slow_stop(implementation_->handle));
}

rm_control::common::Result<bool>
RealmanArmAdapter::isControllerProgramSlotAvailable(int controller_program_id) {
    const auto connected = ensureConnected(implementation_->initialized, implementation_->handle);
    if (!connected.ok) {
        return rm_control::common::Result<bool>::failure(connected.error_message);
    }
    if (controller_program_id < kMinimumControllerProgramId ||
        controller_program_id > kMaximumControllerProgramId) {
        return rm_control::common::Result<bool>::failure(
            "controller program id must be in the range 1..100");
    }
    return queryControllerProgramSlotAvailable(
        implementation_->handle, controller_program_id);
}

rm_control::common::Result<rm_control::interfaces::ToolModbusBusStatus>
RealmanArmAdapter::Configure(
    const rm_control::interfaces::ToolModbusBusConfiguration &configuration) {
    const auto connected = ensureConnected(
        implementation_->initialized, implementation_->handle);
    if (!connected.ok) {
        return rm_control::common::Result<
            rm_control::interfaces::ToolModbusBusStatus>::failure(
                connected.error_message);
    }
    const auto valid = ValidateToolModbusConfiguration(configuration);
    const auto generation = VerifyThirdGenerationController(implementation_->handle);
    const auto current_status = GetStatus();
    if (!valid.ok || !generation.ok || !current_status.ok) {
        return rm_control::common::Result<
            rm_control::interfaces::ToolModbusBusStatus>::failure(
                !valid.ok ? valid.error_message
                          : !generation.ok ? generation.error_message
                                           : current_status.error_message);
    }
    if (configuration.power_cycle_tool) {
        const auto powered_off = SetToolVoltageAndVerify(implementation_->handle, 0);
        if (!powered_off.ok) {
            return rm_control::common::Result<
                rm_control::interfaces::ToolModbusBusStatus>::failure(
                    powered_off.error_message);
        }
        std::this_thread::sleep_for(
            std::chrono::milliseconds(configuration.power_off_delay_ms));
    }
    const auto reset = ResetToolModbusModeIfNeeded(
        implementation_->handle, configuration.port, current_status.value.mode,
        configuration.reset_existing_mode);
    const auto modbus = reset.ok ? sdkResult(
        "rm_set_modbus_mode",
        rm_set_modbus_mode(implementation_->handle, configuration.port,
                           configuration.baudrate,
                           configuration.timeout_units)) : reset;
    if (!modbus.ok) {
        return rm_control::common::Result<
            rm_control::interfaces::ToolModbusBusStatus>::failure(
                modbus.error_message);
    }
    std::this_thread::sleep_for(kToolModbusModeSettlingDelay);
    const auto powered_on = SetToolVoltageAndVerify(
        implementation_->handle, configuration.voltage_type);
    if (!powered_on.ok) {
        return rm_control::common::Result<
            rm_control::interfaces::ToolModbusBusStatus>::failure(
                powered_on.error_message);
    }
    std::this_thread::sleep_for(
        std::chrono::milliseconds(configuration.startup_delay_ms));
    const auto actual_status = GetStatus();
    if (!actual_status.ok) {
        return actual_status;
    }
    const bool matches = actual_status.value.port == configuration.port &&
        actual_status.value.mode == kToolModbusMasterMode &&
        actual_status.value.baudrate == configuration.baudrate &&
        actual_status.value.voltage_type == configuration.voltage_type &&
        actual_status.value.timeout_units == configuration.timeout_units;
    if (!matches) {
        return rm_control::common::Result<
            rm_control::interfaces::ToolModbusBusStatus>::failure(
                "tool Modbus status readback does not match the requested configuration");
    }
    return actual_status;
}

rm_control::common::Result<rm_control::interfaces::ToolModbusBusStatus>
RealmanArmAdapter::GetStatus() {
    const auto connected = ensureConnected(implementation_->initialized, implementation_->handle);
    if (!connected.ok) {
        return rm_control::common::Result<
            rm_control::interfaces::ToolModbusBusStatus>::failure(connected.error_message);
    }
    rm_control::interfaces::ToolModbusBusStatus status;
    status.connected = true;
    status.port = kToolRs485Port;
    const int voltage_return_code =
        rm_get_tool_voltage(implementation_->handle, &status.voltage_type);
    if (voltage_return_code != 0) {
        return rm_control::common::Result<
            rm_control::interfaces::ToolModbusBusStatus>::failure(
            sdkResult("rm_get_tool_voltage", voltage_return_code).error_message);
    }
    const int rs485_return_code = rm_get_tool_RS485_mode(implementation_->handle,
                                                         &status.mode,
                                                         &status.baudrate,
                                                         &status.timeout_units);
    if (rs485_return_code != 0) {
        return rm_control::common::Result<
            rm_control::interfaces::ToolModbusBusStatus>::failure(
            sdkResult("rm_get_tool_RS485_mode", rs485_return_code).error_message);
    }
    return rm_control::common::Result<
        rm_control::interfaces::ToolModbusBusStatus>::success(status);
}

rm_control::common::Result<std::uint16_t>
RealmanArmAdapter::ReadInputRegister(std::uint8_t device_id,
                                    std::uint16_t address) {
    const auto connected = ensureConnected(
        implementation_->initialized, implementation_->handle);
    if (!connected.ok) {
        return rm_control::common::Result<std::uint16_t>::failure(
            connected.error_message);
    }
    if (device_id == 0U || device_id > 247U) {
        return rm_control::common::Result<std::uint16_t>::failure(
            "RM_API2 single input-register read requires device 1..247");
    }
    rm_peripheral_read_write_params_t parameters{};
    parameters.port = kToolRs485Port;
    parameters.address = static_cast<int>(address);
    parameters.device = static_cast<int>(device_id);
    parameters.num = 1;
    int raw_value = 0;
    const int return_code = rm_read_input_registers(
        implementation_->handle, parameters, &raw_value);
    if (return_code != 0) {
        return rm_control::common::Result<std::uint16_t>::failure(
            "rm_read_input_registers failed, return_code=" +
            std::to_string(return_code) + ", device=" +
            std::to_string(static_cast<int>(device_id)) + ", address=" +
            std::to_string(address));
    }
    const auto value = static_cast<std::uint16_t>(
        static_cast<unsigned int>(raw_value) & 0xFFFFU);
    return rm_control::common::Result<std::uint16_t>::success(value);
}

rm_control::common::Result<std::vector<std::uint16_t>>
RealmanArmAdapter::ReadInputRegisters(std::uint8_t device_id,
                                     std::uint16_t start_address,
                                     std::size_t register_count) {
    return ReadMultipleRegistersFromRmApi(
        implementation_->initialized, implementation_->handle,
        MultipleRegisterReadRequest{device_id, start_address, register_count,
                                    "rm_read_multiple_input_registers",
                                    &rm_read_multiple_input_registers});
}

rm_control::common::Result<std::vector<std::uint16_t>>
RealmanArmAdapter::ReadHoldingRegisters(std::uint8_t device_id,
                                       std::uint16_t start_address,
                                       std::size_t register_count) {
    return ReadMultipleRegistersFromRmApi(
        implementation_->initialized, implementation_->handle,
        MultipleRegisterReadRequest{device_id, start_address, register_count,
                                    "rm_read_multiple_holding_registers",
                                    &rm_read_multiple_holding_registers});
}

rm_control::common::Result<void> RealmanArmAdapter::WriteHoldingRegisters(
    std::uint8_t device_id,
    std::uint16_t start_address,
    const std::vector<std::uint16_t> &values) {
    const auto connected = ensureConnected(implementation_->initialized, implementation_->handle);
    if (!connected.ok) {
        return connected;
    }
    if (device_id == 0 || device_id > 247 || values.empty() ||
        values.size() > kMaximumMultipleRegisterWriteCount) {
        return rm_control::common::Result<void>::failure(
            "RM_API2 holding-register write requires device 1..247 and count 1..10");
    }
    rm_peripheral_read_write_params_t parameters{};
    parameters.port = kToolRs485Port;
    parameters.address = static_cast<int>(start_address);
    parameters.device = static_cast<int>(device_id);
    parameters.num = static_cast<int>(values.size());
    auto encoded = EncodeModbusRegistersAsRmApiHighLowBytes(values);
    if (!encoded.ok) {
        return rm_control::common::Result<void>::failure(
            "failed to encode RM_API2 holding-register write: " +
            encoded.error_message);
    }
    const int return_code = rm_write_registers(
        implementation_->handle, parameters, encoded.value.data());
    if (return_code != 0) {
        return rm_control::common::Result<void>::failure(
            "rm_write_registers failed, return_code=" + std::to_string(return_code) +
            ", device=" + std::to_string(static_cast<int>(device_id)) +
            ", address=" + std::to_string(start_address) +
            ", count=" + std::to_string(values.size()));
    }
    return rm_control::common::Result<void>::success();
}

} // namespace flight_robot::realman
