#include "realman_hw_test_common.hpp"

#include <algorithm>
#include <array>
#include <cmath>
#include <sstream>

namespace {

// 预检只短时间采样静态六维力，用来发现明显异常；精度验收仍需外部测力设备。
constexpr int kForceAxisCount = 6;
constexpr int kForceSamples = 12;
constexpr int kForceSampleIntervalMs = 50;

bool inRange(float value, float min_value, float max_value) {
    return std::isfinite(static_cast<double>(value)) && value >= min_value && value <= max_value;
}

bool poseFinite(const rm_pose_t &pose) {
    // SDK 位姿同时提供四元数和欧拉角；这里只检查数值有效性，不做姿态转换。
    const float values[] = {
        pose.position.x,
        pose.position.y,
        pose.position.z,
        pose.quaternion.w,
        pose.quaternion.x,
        pose.quaternion.y,
        pose.quaternion.z,
        pose.euler.rx,
        pose.euler.ry,
        pose.euler.rz,
    };
    return realman_hw_test::isFiniteArray(values, static_cast<int>(sizeof(values) / sizeof(values[0])));
}

std::string poseSummary(const rm_pose_t &pose) {
    std::ostringstream out;
    out << std::fixed << std::setprecision(4)
        << "pos_m=(" << pose.position.x << "," << pose.position.y << "," << pose.position.z << ")"
        << ", euler_rad=(" << pose.euler.rx << "," << pose.euler.ry << "," << pose.euler.rz << ")";
    return out.str();
}

std::string frameSummary(const rm_frame_t &frame) {
    std::ostringstream out;
    out << "name=" << frame.frame_name
        << ", payload_kg=" << std::fixed << std::setprecision(3) << frame.payload
        << ", cog_mm=(" << frame.x << "," << frame.y << "," << frame.z << ")"
        << ", " << poseSummary(frame.pose);
    return out.str();
}

std::string errSummary(const rm_err_t &err) {
    std::ostringstream out;
    out << "err_len=" << static_cast<int>(err.err_len) << ", codes=[";
    for (int i = 0; i < static_cast<int>(err.err_len) && i < 24; ++i) {
        if (i > 0) {
            out << ';';
        }
        out << err.err[i];
    }
    out << "]";
    return out.str();
}

bool errListClear(const rm_err_t &err) {
    // 实测 RM75 会出现 err_len=1 且 codes=[0] 的占位返回。
    // 验收时只把非零错误码视为真实故障，避免误报。
    for (int i = 0; i < static_cast<int>(err.err_len) && i < 24; ++i) {
        if (err.err[i] != 0) {
            return false;
        }
    }
    return true;
}

double maxAbs(const std::array<double, kForceAxisCount> &values, int begin, int end) {
    double result = 0.0;
    for (int i = begin; i < end; ++i) {
        result = std::max(result, std::fabs(values[static_cast<size_t>(i)]));
    }
    return result;
}

} // namespace

int main() {
    using namespace realman_hw_test;

    TestSuite suite("rm75_acceptance_preflight");
    suite.info("source_document", "RM75-6F mechanical arm testing agent operation document v1.0");
    suite.info("scope", "INIT/PREFLIGHT/IDLE read-only acceptance checks; no motion command is sent");

    // 该模块对应文档中的 INIT/PREFLIGHT/IDLE：只读控制器状态，不发送运动或参数修改指令。
    RobotSession session(&suite);
    if (!session.connect()) {
        return suite.finish();
    }

    rm_robot_info_t info = {};
    if (!readRobotInfo(session.handle(), &info, &suite)) {
        return suite.finish();
    }
    suite.expect(validateDof(static_cast<int>(info.arm_dof), nullptr), "rm75_expected_7dof",
                 "actual=" + std::to_string(static_cast<int>(info.arm_dof)));
    suite.expect(info.force_type == RM_MODEL_RM_ISF_E || info.force_type == RM_MODEL_RM_SF_E,
                 "six_axis_force_sensor_available",
                 std::string("actual=") + forceTypeName(info.force_type));
    suite.expect(info.robot_controller_version == 3 || info.robot_controller_version == 4,
                 "controller_generation_known",
                 "actual=" + std::to_string(static_cast<int>(info.robot_controller_version)));

    // 真机验收要求处于真实模式；仿真模式即使 SDK 可连接，也不能代表硬件通过。
    int run_mode = -1;
    int result = rm_get_arm_run_mode(session.handle(), &run_mode);
    suite.expect(result == 0, "rm_get_arm_run_mode", "error=" + std::to_string(result));
    if (result == 0) {
        suite.info("run_mode", run_mode == 1 ? "real" : "simulation_or_unknown");
        suite.expect(run_mode == 1, "real_robot_mode", "mode=" + std::to_string(run_mode));
    }

    int power_state = -1;
    result = rm_get_arm_power_state(session.handle(), &power_state);
    suite.expect(result == 0, "rm_get_arm_power_state", "error=" + std::to_string(result));
    if (result == 0) {
        suite.expect(power_state == 1, "arm_powered_on", "power_state=" + std::to_string(power_state));
    }

    // current_arm_state 提供当前关节角、TCP 位姿和机械臂错误列表，是最小状态闭环。
    rm_current_arm_state_t arm_state = {};
    result = rm_get_current_arm_state(session.handle(), &arm_state);
    suite.expect(result == 0, "rm_get_current_arm_state", "error=" + std::to_string(result));
    if (result == 0) {
        const int dof = static_cast<int>(info.arm_dof);
        suite.expect(isFiniteArray(arm_state.joint, dof), "current_joints_finite",
                     formatArray(arm_state.joint, dof, "J"));
        suite.expect(poseFinite(arm_state.pose), "current_tcp_pose_finite", poseSummary(arm_state.pose));
        suite.expect(errListClear(arm_state.err), "current_arm_errors_clear", errSummary(arm_state.err));
    }

    rm_arm_all_state_t all_state = {};
    result = rm_get_arm_all_state(session.handle(), &all_state);
    suite.expect(result == 0, "rm_get_arm_all_state", "error=" + std::to_string(result));
    if (result == 0) {
        // 逐关节检查用较宽范围，只判断“明显不正常”，不把电流读数当作精确力矩指标。
        bool joints_enabled = true;
        bool joint_errors_zero = true;
        bool joint_values_valid = true;
        const int dof = static_cast<int>(info.arm_dof);
        for (int i = 0; i < dof; ++i) {
            joints_enabled = joints_enabled && all_state.joint_en_flag[i] == 1;
            joint_errors_zero = joint_errors_zero && all_state.joint_err_code[i] == 0;
            joint_values_valid = joint_values_valid &&
                                 inRange(all_state.joint_voltage[i], 18.0F, 30.0F) &&
                                 inRange(all_state.joint_temperature[i], 0.0F, 85.0F) &&
                                 inRange(all_state.joint_current[i], -10000.0F, 10000.0F);
        }
        suite.expect(joints_enabled, "all_joints_enabled");
        suite.expect(joint_errors_zero, "joint_error_codes_zero");
        suite.expect(joint_values_valid, "joint_power_temperature_current_ranges");
        suite.expect(errListClear(all_state.err), "arm_all_state_errors_clear", errSummary(all_state.err));
    }

    float controller_voltage = 0.0F;
    float controller_current = 0.0F;
    float controller_temperature = 0.0F;
    int controller_error = -1;
    result = rm_get_controller_state(session.handle(),
                                     &controller_voltage,
                                     &controller_current,
                                     &controller_temperature,
                                     &controller_error);
    suite.expect(result == 0, "rm_get_controller_state", "error=" + std::to_string(result));
    if (result == 0) {
        suite.info("controller_state",
                   "voltage_V=" + std::to_string(controller_voltage) +
                       ", current_A=" + std::to_string(controller_current) +
                       ", temperature_C=" + std::to_string(controller_temperature) +
                       ", err=" + std::to_string(controller_error));
        suite.expect(inRange(controller_voltage, 18.0F, 30.0F), "controller_voltage_range");
        suite.expect(inRange(controller_current, 0.0F, 10.0F), "controller_current_range");
        suite.expect(inRange(controller_temperature, 0.0F, 85.0F), "controller_temperature_range");
        suite.expect(controller_error == 0, "controller_error_zero", "err=" + std::to_string(controller_error));
    }

    // 坐标系会影响后续运动和力数据方向；预检只确认当前配置可读、位姿数值有效。
    rm_frame_t work_frame = {};
    result = rm_get_current_work_frame(session.handle(), &work_frame);
    suite.expect(result == 0, "rm_get_current_work_frame", "error=" + std::to_string(result));
    if (result == 0) {
        suite.info("current_work_frame", frameSummary(work_frame));
        suite.expect(poseFinite(work_frame.pose), "work_frame_pose_finite");
    }

    rm_frame_t tool_frame = {};
    result = rm_get_current_tool_frame(session.handle(), &tool_frame);
    suite.expect(result == 0, "rm_get_current_tool_frame", "error=" + std::to_string(result));
    if (result == 0) {
        suite.info("current_tool_frame", frameSummary(tool_frame));
        suite.expect(poseFinite(tool_frame.pose), "tool_frame_pose_finite");
        suite.expect(inRange(tool_frame.payload, 0.0F, 20.0F), "tool_payload_reasonable",
                     "payload_kg=" + std::to_string(tool_frame.payload));
    }

    std::array<double, kForceAxisCount> work_zero_mean = {};
    int force_failures = 0;
    for (int sample = 0; sample < kForceSamples; ++sample) {
        rm_force_data_t force = {};
        result = rm_get_force_data(session.handle(), &force);
        if (result != 0 || !isFiniteArray(force.work_zero_force_data, kForceAxisCount)) {
            ++force_failures;
        } else {
            // 在线均值避免额外保存样本数组，报告只需要静态均值和范围判断。
            for (int axis = 0; axis < kForceAxisCount; ++axis) {
                work_zero_mean[static_cast<size_t>(axis)] +=
                    (static_cast<double>(force.work_zero_force_data[axis]) -
                     work_zero_mean[static_cast<size_t>(axis)]) /
                    (sample + 1 - force_failures);
            }
        }
        sleepMs(kForceSampleIntervalMs);
    }

    std::ostringstream force_detail;
    force_detail << std::fixed << std::setprecision(4)
                 << "Fx=" << work_zero_mean[0]
                 << ", Fy=" << work_zero_mean[1]
                 << ", Fz=" << work_zero_mean[2]
                 << ", Mx=" << work_zero_mean[3]
                 << ", My=" << work_zero_mean[4]
                 << ", Mz=" << work_zero_mean[5];
    suite.expect(force_failures == 0, "rm_get_force_data_preflight_samples",
                 "failures=" + std::to_string(force_failures));
    suite.info("work_zero_force_mean", force_detail.str());
    const double force_norm = std::sqrt(work_zero_mean[0] * work_zero_mean[0] +
                                        work_zero_mean[1] * work_zero_mean[1] +
                                        work_zero_mean[2] * work_zero_mean[2]);
    suite.expect(std::isfinite(force_norm), "force_norm_computed",
                 "force_norm_N=" + std::to_string(force_norm));
    suite.expect(maxAbs(work_zero_mean, 0, 3) <= 40.0, "work_zero_force_static_range",
                 "max_abs_force_N=" + std::to_string(maxAbs(work_zero_mean, 0, 3)));
    suite.expect(maxAbs(work_zero_mean, 3, 6) <= 4.0, "work_zero_torque_static_range",
                 "max_abs_torque_Nm=" + std::to_string(maxAbs(work_zero_mean, 3, 6)));

    suite.info("manual_items_not_executed",
               "appearance/load/e-stop/contact-force/repeatability require human safety confirmation or external tools");

    return suite.finish();
}
