#include "rm_interface.h"

#include <array>
#include <atomic>
#include <cerrno>
#include <chrono>
#include <cmath>
#include <cstdarg>
#include <cstdio>
#include <ctime>
#include <fstream>
#include <iomanip>
#include <iostream>
#include <limits>
#include <sstream>
#include <string>
#include <sys/stat.h>
#include <sys/types.h>
#include <thread>

namespace {

#ifndef LINKERHAND_PROJECT_ROOT
#define LINKERHAND_PROJECT_ROOT "."
#endif

// ===== 测试参数区：需要改测试强度时改这里 =====
constexpr const char *kRobotIp = "192.168.1.18";
constexpr int kRobotPort = 8080;
constexpr int kTimeoutMs = 1000;
constexpr int kSamplePeriodMs = 50;
constexpr int kStaticBaselineSeconds = 5;
constexpr int kFinalStaticSeconds = 3;
constexpr int kSafetyCountdownSeconds = 5;
constexpr int kMotionJoint = 2; // 1-based joint index
constexpr float kMotionDeltaDeg = 3.0F;
constexpr int kMotionSpeedPercent = 5;
constexpr bool kRunMotionTrackingTest = true;
constexpr bool kDryRunOnly = false;
constexpr float kMaxAllowedDeltaDeg = 5.0F;
constexpr int kMaxAllowedSpeedPercent = 10;
// ===== 测试参数区结束 =====

constexpr int kAxisCount = 6;
constexpr int kPhaseCount = 6;
constexpr std::array<const char *, kPhaseCount> kPhaseNames = {
    "static_baseline",
    "move_positive",
    "return_positive",
    "move_negative",
    "return_negative",
    "final_static",
};

struct Stats {
    int count = 0;
    double mean = 0.0;
    double m2 = 0.0;
    double min = std::numeric_limits<double>::infinity();
    double max = -std::numeric_limits<double>::infinity();

    void add(double value) {
        if (!std::isfinite(value)) {
            return;
        }
        ++count;
        const double delta = value - mean;
        mean += delta / count;
        const double delta2 = value - mean;
        m2 += delta * delta2;
        if (value < min) {
            min = value;
        }
        if (value > max) {
            max = value;
        }
    }

    double stddev() const {
        return count > 1 ? std::sqrt(m2 / static_cast<double>(count - 1)) : 0.0;
    }

    double range() const {
        return count > 0 ? max - min : 0.0;
    }
};

struct Metrics {
    std::array<int, kPhaseCount> phase_samples = {};
    std::array<Stats, ARM_DOF> static_joint_current;
    std::array<Stats, ARM_DOF> motion_joint_current;
    std::array<Stats, ARM_DOF> static_joint_voltage;
    std::array<Stats, ARM_DOF> static_joint_temperature;
    std::array<Stats, kAxisCount> static_zero_force;
    std::array<Stats, kAxisCount> static_raw_force;
    std::array<Stats, kAxisCount> motion_zero_force;
    Stats controller_voltage;
    Stats controller_current;
    Stats controller_temperature;
    int total_samples = 0;
    int arm_state_failures = 0;
    int joint_current_failures = 0;
    int joint_voltage_failures = 0;
    int joint_temperature_failures = 0;
    int force_failures = 0;
    int controller_failures = 0;
};

struct Sample {
    rm_current_arm_state_t arm_state = {};
    std::array<float, ARM_DOF> joint_current = {};
    std::array<float, ARM_DOF> joint_voltage = {};
    std::array<float, ARM_DOF> joint_temperature = {};
    rm_force_data_t force = {};
    float controller_voltage = std::numeric_limits<float>::quiet_NaN();
    float controller_current = std::numeric_limits<float>::quiet_NaN();
    float controller_temperature = std::numeric_limits<float>::quiet_NaN();
    int controller_err = 0;
    bool arm_state_ok = false;
    bool joint_current_ok = false;
    bool joint_voltage_ok = false;
    bool joint_temperature_ok = false;
    bool force_ok = false;
    bool controller_ok = false;
};

void apiLogCallback(const char *message, va_list args) {
    if (message == nullptr) {
        return;
    }

    char buffer[1024] = {};
    std::vsnprintf(buffer, sizeof(buffer), message, args);
    std::cerr << "[rm_api] " << buffer << '\n';
}

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

void cleanup(rm_robot_handle *handle) {
    if (handle != nullptr) {
        const int delete_result = rm_delete_robot_arm(handle);
        if (delete_result != 0) {
            std::cerr << "rm_delete_robot_arm failed, error code: " << delete_result << '\n';
        }
    }

    const int destroy_result = destroyApi();
    if (destroy_result != 0) {
        std::cerr << "RealMan API destroy failed, error code: " << destroy_result << '\n';
    }
}

bool ensureDirectory(const std::string &path) {
    if (::mkdir(path.c_str(), 0755) == 0 || errno == EEXIST) {
        return true;
    }
    std::cerr << "Failed to create directory: " << path << '\n';
    return false;
}

std::string timestampString() {
    const std::time_t now = std::time(nullptr);
    std::tm local_time = {};
    localtime_r(&now, &local_time);

    std::ostringstream out;
    out << std::put_time(&local_time, "%Y%m%d_%H%M%S");
    return out.str();
}

void printJoints(const char *label, const float joints[ARM_DOF], int dof) {
    std::cout << label;
    for (int i = 0; i < dof; ++i) {
        std::cout << (i == 0 ? " [" : ", ")
                  << "J" << (i + 1) << "=" << std::fixed << std::setprecision(3) << joints[i];
    }
    std::cout << "] deg\n";
}

bool readSample(rm_robot_handle *handle, Sample *sample, Metrics *metrics) {
    if (handle == nullptr || sample == nullptr || metrics == nullptr) {
        return false;
    }

    int result = rm_get_current_arm_state(handle, &sample->arm_state);
    sample->arm_state_ok = result == 0;
    if (!sample->arm_state_ok) {
        ++metrics->arm_state_failures;
    }

    result = rm_get_current_joint_current(handle, sample->joint_current.data());
    sample->joint_current_ok = result == 0;
    if (!sample->joint_current_ok) {
        ++metrics->joint_current_failures;
    }

    result = rm_get_current_joint_voltage(handle, sample->joint_voltage.data());
    sample->joint_voltage_ok = result == 0;
    if (!sample->joint_voltage_ok) {
        ++metrics->joint_voltage_failures;
    }

    result = rm_get_current_joint_temperature(handle, sample->joint_temperature.data());
    sample->joint_temperature_ok = result == 0;
    if (!sample->joint_temperature_ok) {
        ++metrics->joint_temperature_failures;
    }

    result = rm_get_force_data(handle, &sample->force);
    sample->force_ok = result == 0;
    if (!sample->force_ok) {
        ++metrics->force_failures;
    }

    result = rm_get_controller_state(handle,
                                     &sample->controller_voltage,
                                     &sample->controller_current,
                                     &sample->controller_temperature,
                                     &sample->controller_err);
    sample->controller_ok = result == 0;
    if (!sample->controller_ok) {
        ++metrics->controller_failures;
    }

    return sample->arm_state_ok;
}

void writeCsvHeader(std::ofstream *csv) {
    *csv << "elapsed_ms,phase";
    for (int i = 0; i < ARM_DOF; ++i) {
        *csv << ",joint" << (i + 1) << "_deg";
    }
    for (int i = 0; i < ARM_DOF; ++i) {
        *csv << ",joint" << (i + 1) << "_current_mA";
    }
    for (int i = 0; i < ARM_DOF; ++i) {
        *csv << ",joint" << (i + 1) << "_voltage_V";
    }
    for (int i = 0; i < ARM_DOF; ++i) {
        *csv << ",joint" << (i + 1) << "_temperature_C";
    }
    for (const char *axis : {"Fx", "Fy", "Fz", "Mx", "My", "Mz"}) {
        *csv << ",raw_" << axis;
    }
    for (const char *axis : {"Fx", "Fy", "Fz", "Mx", "My", "Mz"}) {
        *csv << ",zero_" << axis;
    }
    *csv << ",controller_voltage_V,controller_current_A,controller_temperature_C,controller_err\n";
}

void writeCsvSample(std::ofstream *csv, long long elapsed_ms, const char *phase, const Sample &sample) {
    *csv << elapsed_ms << ',' << phase;
    for (int i = 0; i < ARM_DOF; ++i) {
        *csv << ',' << (sample.arm_state_ok ? sample.arm_state.joint[i] : std::numeric_limits<float>::quiet_NaN());
    }
    for (int i = 0; i < ARM_DOF; ++i) {
        *csv << ',' << (sample.joint_current_ok ? sample.joint_current[static_cast<size_t>(i)]
                                                : std::numeric_limits<float>::quiet_NaN());
    }
    for (int i = 0; i < ARM_DOF; ++i) {
        *csv << ',' << (sample.joint_voltage_ok ? sample.joint_voltage[static_cast<size_t>(i)]
                                                : std::numeric_limits<float>::quiet_NaN());
    }
    for (int i = 0; i < ARM_DOF; ++i) {
        *csv << ',' << (sample.joint_temperature_ok ? sample.joint_temperature[static_cast<size_t>(i)]
                                                    : std::numeric_limits<float>::quiet_NaN());
    }
    for (int i = 0; i < kAxisCount; ++i) {
        *csv << ',' << (sample.force_ok ? sample.force.force_data[i] : std::numeric_limits<float>::quiet_NaN());
    }
    for (int i = 0; i < kAxisCount; ++i) {
        *csv << ',' << (sample.force_ok ? sample.force.zero_force_data[i] : std::numeric_limits<float>::quiet_NaN());
    }
    *csv << ',' << (sample.controller_ok ? sample.controller_voltage : std::numeric_limits<float>::quiet_NaN())
         << ',' << (sample.controller_ok ? sample.controller_current : std::numeric_limits<float>::quiet_NaN())
         << ',' << (sample.controller_ok ? sample.controller_temperature : std::numeric_limits<float>::quiet_NaN())
         << ',' << (sample.controller_ok ? sample.controller_err : -1)
         << '\n';
}

void updateMetrics(int phase_index, const Sample &sample, Metrics *metrics) {
    ++metrics->total_samples;
    if (phase_index >= 0 && phase_index < kPhaseCount) {
        ++metrics->phase_samples[static_cast<size_t>(phase_index)];
    }

    const bool is_static = phase_index == 0 || phase_index == 5;
    const bool is_motion = phase_index >= 1 && phase_index <= 4;

    if (sample.joint_current_ok) {
        for (int i = 0; i < ARM_DOF; ++i) {
            if (is_static) {
                metrics->static_joint_current[static_cast<size_t>(i)].add(sample.joint_current[static_cast<size_t>(i)]);
            }
            if (is_motion) {
                metrics->motion_joint_current[static_cast<size_t>(i)].add(sample.joint_current[static_cast<size_t>(i)]);
            }
        }
    }

    if (sample.joint_voltage_ok && is_static) {
        for (int i = 0; i < ARM_DOF; ++i) {
            metrics->static_joint_voltage[static_cast<size_t>(i)].add(sample.joint_voltage[static_cast<size_t>(i)]);
        }
    }

    if (sample.joint_temperature_ok && is_static) {
        for (int i = 0; i < ARM_DOF; ++i) {
            metrics->static_joint_temperature[static_cast<size_t>(i)].add(sample.joint_temperature[static_cast<size_t>(i)]);
        }
    }

    if (sample.force_ok) {
        for (int i = 0; i < kAxisCount; ++i) {
            if (is_static) {
                metrics->static_zero_force[static_cast<size_t>(i)].add(sample.force.zero_force_data[i]);
                metrics->static_raw_force[static_cast<size_t>(i)].add(sample.force.force_data[i]);
            }
            if (is_motion) {
                metrics->motion_zero_force[static_cast<size_t>(i)].add(sample.force.zero_force_data[i]);
            }
        }
    }

    if (sample.controller_ok) {
        metrics->controller_voltage.add(sample.controller_voltage);
        metrics->controller_current.add(sample.controller_current);
        metrics->controller_temperature.add(sample.controller_temperature);
    }
}

void samplerLoop(rm_robot_handle *handle,
                 const std::string &csv_path,
                 std::atomic<int> *phase_index,
                 std::atomic<bool> *running,
                 Metrics *metrics) {
    std::ofstream csv(csv_path);
    if (!csv) {
        std::cerr << "Failed to open CSV file: " << csv_path << '\n';
        running->store(false);
        return;
    }

    csv << std::fixed << std::setprecision(6);
    writeCsvHeader(&csv);

    const auto start = std::chrono::steady_clock::now();
    while (running->load()) {
        const auto now = std::chrono::steady_clock::now();
        const auto elapsed_ms = std::chrono::duration_cast<std::chrono::milliseconds>(now - start).count();
        const int phase = phase_index->load();
        const char *phase_name = (phase >= 0 && phase < kPhaseCount) ? kPhaseNames[static_cast<size_t>(phase)] : "unknown";

        Sample sample;
        readSample(handle, &sample, metrics);
        writeCsvSample(&csv, elapsed_ms, phase_name, sample);
        updateMetrics(phase, sample, metrics);

        std::this_thread::sleep_for(std::chrono::milliseconds(kSamplePeriodMs));
    }
}

void sleepSeconds(int seconds) {
    for (int i = 0; i < seconds; ++i) {
        std::this_thread::sleep_for(std::chrono::seconds(1));
    }
}

bool copyJoints(const float source[ARM_DOF], float target[ARM_DOF]) {
    if (source == nullptr || target == nullptr) {
        return false;
    }
    for (int i = 0; i < ARM_DOF; ++i) {
        target[i] = source[i];
    }
    return true;
}

bool validateTestConfig(int dof) {
    if (kSamplePeriodMs < 20) {
        std::cerr << "kSamplePeriodMs must be >= 20 ms.\n";
        return false;
    }
    if (kMotionJoint < 1 || kMotionJoint > dof) {
        std::cerr << "kMotionJoint must be in range 1.." << dof << ".\n";
        return false;
    }
    if (kMotionDeltaDeg <= 0.0F || kMotionDeltaDeg > kMaxAllowedDeltaDeg) {
        std::cerr << "kMotionDeltaDeg must be in range (0, " << kMaxAllowedDeltaDeg << "].\n";
        return false;
    }
    if (kMotionSpeedPercent < 1 || kMotionSpeedPercent > kMaxAllowedSpeedPercent) {
        std::cerr << "kMotionSpeedPercent must be in range 1.." << kMaxAllowedSpeedPercent << ".\n";
        return false;
    }
    return true;
}

bool moveAndMeasure(rm_robot_handle *handle,
                    std::atomic<int> *phase_index,
                    int phase,
                    const char *label,
                    const float target[ARM_DOF],
                    int dof,
                    double *duration_seconds,
                    double *max_final_error_deg) {
    phase_index->store(phase);
    printJoints(label, target, dof);

    const auto start = std::chrono::steady_clock::now();
    int result = 0;
    if (!kDryRunOnly) {
        result = rm_movej(handle, target, kMotionSpeedPercent, 0, 0, 1);
    }
    const auto end = std::chrono::steady_clock::now();
    *duration_seconds = std::chrono::duration<double>(end - start).count();

    if (result != 0) {
        std::cerr << "rm_movej failed at " << label << ", error code: " << result << '\n';
        const int stop_result = rm_set_arm_stop(handle);
        if (stop_result != 0) {
            std::cerr << "rm_set_arm_stop failed, error code: " << stop_result << '\n';
        }
        return false;
    }

    rm_current_arm_state_t actual = {};
    result = rm_get_current_arm_state(handle, &actual);
    if (result != 0) {
        std::cerr << "rm_get_current_arm_state after move failed, error code: " << result << '\n';
        return false;
    }

    double max_error = 0.0;
    for (int i = 0; i < dof; ++i) {
        max_error = std::max(max_error, std::fabs(static_cast<double>(actual.joint[i] - target[i])));
    }
    *max_final_error_deg = max_error;
    printJoints("Actual after move:", actual.joint, dof);
    std::cout << "Move duration: " << std::fixed << std::setprecision(3) << *duration_seconds
              << " s, max final joint error: " << max_error << " deg\n";
    return true;
}

const char *forceTypeName(rm_force_type_e type) {
    switch (type) {
    case RM_MODEL_RM_B_E:
        return "Standard";
    case RM_MODEL_RM_ZF_E:
        return "1D force";
    case RM_MODEL_RM_SF_E:
        return "6D force";
    case RM_MODEL_RM_ISF_E:
        return "Integrated 6D force";
    default:
        return "Unknown";
    }
}

void writeStatsRow(std::ofstream *md, const char *name, const char *unit, const Stats &stats) {
    *md << "| " << name << " | " << stats.count << " | "
        << std::fixed << std::setprecision(4) << stats.mean << " | "
        << stats.stddev() << " | "
        << stats.min << " | "
        << stats.max << " | "
        << stats.range() << " | " << unit << " |\n";
}

bool writeMarkdownReport(const std::string &report_path,
                         const std::string &csv_path,
                         const std::string &timestamp,
                         const rm_robot_info_t &robot_info,
                         const rm_arm_software_version_t &software_info,
                         const Metrics &metrics,
                         const std::array<double, 4> &move_durations,
                         const std::array<double, 4> &move_errors,
                         bool motion_test_ok) {
    std::ofstream md(report_path);
    if (!md) {
        std::cerr << "Failed to open report file: " << report_path << '\n';
        return false;
    }

    md << "# 睿尔曼机械臂力/力矩与关节指标测试报告\n\n"
       << "| 项目 | 值 |\n"
       << "| --- | --- |\n"
       << "| 测试时间 | `" << timestamp << "` |\n"
       << "| 控制器地址 | `" << kRobotIp << ':' << kRobotPort << "` |\n"
       << "| API 版本 | `" << rm_api_version() << "` |\n"
       << "| 产品版本 | `" << software_info.product_version << "` |\n"
       << "| 软件控制器版本 | `" << software_info.robot_controller_version << "` |\n"
       << "| 控制器代际 | `" << static_cast<int>(robot_info.robot_controller_version) << "` |\n"
       << "| 自由度 | `" << static_cast<int>(robot_info.arm_dof) << "` |\n"
       << "| 力传感器类型 | `" << forceTypeName(robot_info.force_type) << "` |\n"
       << "| 原始 CSV | `" << csv_path << "` |\n\n";

    md << "## 测试范围\n\n"
       << "- 已测试：六维力/力矩传感器读数、静态噪声/漂移、关节电流/电压/温度、控制器电压/电流/温度、低速小幅关节运动跟踪误差。\n"
       << "- 未测试：接触式力位混合控制闭环精度、外部负载下输出力矩精度、碰撞阈值。这些需要固定工装、标定砝码/力传感器和明确接触方向。\n"
       << "- SDK 当前暴露的是关节电流和末端六维力/力矩数据，没有裸关节力矩指令接口；因此本文中的关节侧“力矩相关”指标以电流作为负载代理量。\n\n";

    md << "## 采样概况\n\n"
       << "| Phase | 样本数 |\n"
       << "| --- | ---: |\n";
    for (int i = 0; i < kPhaseCount; ++i) {
        md << "| `" << kPhaseNames[static_cast<size_t>(i)] << "` | "
           << metrics.phase_samples[static_cast<size_t>(i)] << " |\n";
    }
    md << "| **合计** | **" << metrics.total_samples << "** |\n\n";

    md << "## 静态六维力/力矩噪声\n\n"
       << "单位：`Fx/Fy/Fz = N`，`Mx/My/Mz = Nm`。统计对象为系统外受力 `zero_force_data`。\n\n"
       << "| 轴 | 样本数 | 均值 | 标准差 | 最小值 | 最大值 | 峰峰值 | 单位 |\n"
       << "| --- | ---: | ---: | ---: | ---: | ---: | ---: | --- |\n";
    const std::array<const char *, kAxisCount> axes = {"Fx", "Fy", "Fz", "Mx", "My", "Mz"};
    for (int i = 0; i < kAxisCount; ++i) {
        writeStatsRow(&md,
                      axes[static_cast<size_t>(i)],
                      i < 3 ? "N" : "Nm",
                      metrics.static_zero_force[static_cast<size_t>(i)]);
    }

    md << "\n## 静态关节电流\n\n"
       << "| 关节 | 样本数 | 均值 | 标准差 | 最小值 | 最大值 | 峰峰值 | 单位 |\n"
       << "| --- | ---: | ---: | ---: | ---: | ---: | ---: | --- |\n";
    for (int i = 0; i < static_cast<int>(robot_info.arm_dof); ++i) {
        const std::string joint = "J" + std::to_string(i + 1);
        writeStatsRow(&md, joint.c_str(), "mA", metrics.static_joint_current[static_cast<size_t>(i)]);
    }

    md << "\n## 运动过程关节电流\n\n"
       << "| 关节 | 样本数 | 均值 | 标准差 | 最小值 | 最大值 | 峰峰值 | 单位 |\n"
       << "| --- | ---: | ---: | ---: | ---: | ---: | ---: | --- |\n";
    for (int i = 0; i < static_cast<int>(robot_info.arm_dof); ++i) {
        const std::string joint = "J" + std::to_string(i + 1);
        writeStatsRow(&md, joint.c_str(), "mA", metrics.motion_joint_current[static_cast<size_t>(i)]);
    }

    md << "\n## 低速小幅运动跟踪\n\n"
       << "| 步骤 | 持续时间 s | 到位最大关节误差 deg |\n"
       << "| --- | ---: | ---: |\n";
    const std::array<const char *, 4> move_names = {
        "J2 +delta", "return from +delta", "J2 -delta", "return from -delta",
    };
    for (int i = 0; i < 4; ++i) {
        md << "| " << move_names[static_cast<size_t>(i)] << " | "
           << std::fixed << std::setprecision(3) << move_durations[static_cast<size_t>(i)] << " | "
           << std::setprecision(4) << move_errors[static_cast<size_t>(i)] << " |\n";
    }
    md << "\n运动测试结果：`" << (motion_test_ok ? "通过" : "未通过或未执行") << "`。\n\n";

    md << "## 控制器状态\n\n"
       << "| 指标 | 样本数 | 均值 | 标准差 | 最小值 | 最大值 | 峰峰值 | 单位 |\n"
       << "| --- | ---: | ---: | ---: | ---: | ---: | ---: | --- |\n";
    writeStatsRow(&md, "controller_voltage", "V", metrics.controller_voltage);
    writeStatsRow(&md, "controller_current", "A", metrics.controller_current);
    writeStatsRow(&md, "controller_temperature", "C", metrics.controller_temperature);

    md << "\n## 接口失败计数\n\n"
       << "| 接口 | 失败次数 |\n"
       << "| --- | ---: |\n"
       << "| rm_get_current_arm_state | " << metrics.arm_state_failures << " |\n"
       << "| rm_get_current_joint_current | " << metrics.joint_current_failures << " |\n"
       << "| rm_get_current_joint_voltage | " << metrics.joint_voltage_failures << " |\n"
       << "| rm_get_current_joint_temperature | " << metrics.joint_temperature_failures << " |\n"
       << "| rm_get_force_data | " << metrics.force_failures << " |\n"
       << "| rm_get_controller_state | " << metrics.controller_failures << " |\n\n";

    md << "## 结论\n\n"
       << "- 本次测试可以评估静态六维力/力矩读数稳定性、关节电流负载变化、基础运动到位误差和控制器供电状态。\n"
       << "- 若要测试真实接触力控或力矩输出能力，需要增加外部测力设备、刚性固定治具、接触方向限位和急停监护，再启用 `rm_set_force_position` 或 `rm_force_position_move_*`。\n";

    return true;
}

} // namespace

int main() {
    std::cout.setf(std::ios::unitbuf);
    std::cerr.setf(std::ios::unitbuf);

    rm_set_log_call_back(apiLogCallback, 2);
    rm_set_timeout(kTimeoutMs);

    int result = rm_init(RM_TRIPLE_MODE_E);
    if (result != 0) {
        std::cerr << "rm_init failed, error code: " << result << '\n';
        return 1;
    }

    std::cout << "RealMan API version: " << rm_api_version() << '\n'
              << "Connecting to " << kRobotIp << ':' << kRobotPort << "...\n";

    rm_robot_handle *handle = rm_create_robot_arm(kRobotIp, kRobotPort);
    if (handle == nullptr || handle->id < 0) {
        std::cerr << "Failed to connect to RealMan arm.\n";
        cleanup(handle);
        return 2;
    }

    rm_robot_info_t robot_info = {};
    result = rm_get_robot_info(handle, &robot_info);
    if (result != 0) {
        std::cerr << "rm_get_robot_info failed, error code: " << result << '\n';
        cleanup(handle);
        return 3;
    }

    const int dof = static_cast<int>(robot_info.arm_dof);
    if (dof <= 0 || dof > ARM_DOF) {
        std::cerr << "Invalid arm DOF reported by controller: " << dof << '\n';
        cleanup(handle);
        return 4;
    }
    if (!validateTestConfig(dof)) {
        cleanup(handle);
        return 5;
    }

    rm_arm_software_version_t software_info = {};
    result = rm_get_arm_software_info(handle, &software_info);
    if (result != 0) {
        std::cerr << "rm_get_arm_software_info failed, error code: " << result << '\n';
    }

    rm_current_arm_state_t start_state = {};
    result = rm_get_current_arm_state(handle, &start_state);
    if (result != 0) {
        std::cerr << "rm_get_current_arm_state failed, error code: " << result << '\n';
        cleanup(handle);
        return 6;
    }

    std::cout << "Connected. Arm DOF: " << dof
              << ", force type: " << forceTypeName(robot_info.force_type)
              << ", sample period: " << kSamplePeriodMs << " ms\n";
    printJoints("Start joints:", start_state.joint, dof);

    const std::string report_dir = std::string(LINKERHAND_PROJECT_ROOT) + "/reports";
    if (!ensureDirectory(report_dir)) {
        cleanup(handle);
        return 7;
    }

    const std::string timestamp = timestampString();
    const std::string csv_path = report_dir + "/realman_metrics_" + timestamp + ".csv";
    const std::string latest_csv_path = report_dir + "/realman_metrics_latest.csv";
    const std::string report_path = report_dir + "/realman_metrics_" + timestamp + ".md";
    const std::string latest_report_path = report_dir + "/REALMAN_METRICS_TEST_RESULT.md";

    Metrics metrics;
    std::atomic<int> phase_index(0);
    std::atomic<bool> running(true);
    std::thread sampler(samplerLoop, handle, csv_path, &phase_index, &running, &metrics);

    std::cout << "Static baseline sampling for " << kStaticBaselineSeconds << " s...\n";
    sleepSeconds(kStaticBaselineSeconds);

    std::array<double, 4> move_durations = {};
    std::array<double, 4> move_errors = {};
    bool motion_test_ok = true;

    if (kRunMotionTrackingTest) {
        std::cout << "\nSafety countdown before motion. Clear workspace and keep E-stop reachable.\n";
        for (int second = kSafetyCountdownSeconds; second > 0; --second) {
            std::cout << "Moving in " << second << "...\n";
            sleepSeconds(1);
        }

        float positive[ARM_DOF] = {};
        float negative[ARM_DOF] = {};
        copyJoints(start_state.joint, positive);
        copyJoints(start_state.joint, negative);
        positive[kMotionJoint - 1] += kMotionDeltaDeg;
        negative[kMotionJoint - 1] -= kMotionDeltaDeg;

        motion_test_ok = motion_test_ok && moveAndMeasure(handle, &phase_index, 1, "Move positive target:", positive, dof, &move_durations[0], &move_errors[0]);
        motion_test_ok = motion_test_ok && moveAndMeasure(handle, &phase_index, 2, "Return from positive:", start_state.joint, dof, &move_durations[1], &move_errors[1]);
        motion_test_ok = motion_test_ok && moveAndMeasure(handle, &phase_index, 3, "Move negative target:", negative, dof, &move_durations[2], &move_errors[2]);
        motion_test_ok = motion_test_ok && moveAndMeasure(handle, &phase_index, 4, "Return from negative:", start_state.joint, dof, &move_durations[3], &move_errors[3]);
    }

    phase_index.store(5);
    std::cout << "Final static sampling for " << kFinalStaticSeconds << " s...\n";
    sleepSeconds(kFinalStaticSeconds);

    running.store(false);
    sampler.join();

    cleanup(handle);

    // Keep stable "latest" filenames for CLion users while preserving timestamped artifacts.
    {
        std::ifstream src(csv_path, std::ios::binary);
        std::ofstream dst(latest_csv_path, std::ios::binary);
        dst << src.rdbuf();
    }

    if (!writeMarkdownReport(report_path,
                             csv_path,
                             timestamp,
                             robot_info,
                             software_info,
                             metrics,
                             move_durations,
                             move_errors,
                             motion_test_ok)) {
        return 8;
    }
    if (!writeMarkdownReport(latest_report_path,
                             latest_csv_path,
                             timestamp,
                             robot_info,
                             software_info,
                             metrics,
                             move_durations,
                             move_errors,
                             motion_test_ok)) {
        return 9;
    }

    std::cout << "Metrics test finished.\n"
              << "CSV: " << csv_path << '\n'
              << "Report: " << report_path << '\n'
              << "Latest report: " << latest_report_path << '\n';
    return motion_test_ok ? 0 : 10;
}
