#include "realman_hw_test_common.hpp"

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

namespace {

// 文档建议做长时间稳定性测试；这里默认 30 秒作为可频繁运行的通信冒烟验收。
constexpr int kDurationSeconds = 30;
constexpr int kSampleIntervalMs = 100;
constexpr int kForceAxisCount = 6;

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

long long epochMs() {
    // CSV 记录使用真实时间戳，采样周期统计使用 steady_clock，避免系统时间跳变影响周期。
    const auto now = std::chrono::system_clock::now();
    return std::chrono::duration_cast<std::chrono::milliseconds>(now.time_since_epoch()).count();
}

std::string csvEscape(const std::string &value) {
    // CSV 字段中包含分号数组和错误说明，仍按标准 CSV 规则处理逗号/引号/换行。
    bool needs_quotes = false;
    for (const char ch : value) {
        needs_quotes = needs_quotes || ch == ',' || ch == '"' || ch == '\n' || ch == '\r';
    }
    if (!needs_quotes) {
        return value;
    }

    std::string escaped = "\"";
    for (const char ch : value) {
        if (ch == '"') {
            escaped += "\"\"";
        } else {
            escaped += ch;
        }
    }
    escaped += "\"";
    return escaped;
}

std::string joinFloats(const float *values, int count, int precision = 4) {
    if (values == nullptr || count <= 0) {
        return "NA";
    }

    std::ostringstream out;
    out << std::fixed << std::setprecision(precision);
    for (int i = 0; i < count; ++i) {
        if (i > 0) {
            out << ';';
        }
        out << values[i];
    }
    return out.str();
}

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

bool poseFinite(const rm_pose_t &pose) {
    const float values[] = {
        pose.position.x,
        pose.position.y,
        pose.position.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 errSummary(const rm_err_t &err) {
    std::ostringstream out;
    out << "arm_err_len=" << static_cast<int>(err.err_len) << ";arm_err_codes=";
    if (err.err_len == 0) {
        out << "none";
        return out.str();
    }
    for (int i = 0; i < static_cast<int>(err.err_len) && i < 24; ++i) {
        if (i > 0) {
            out << '|';
        }
        out << err.err[i];
    }
    return out.str();
}

bool errListClear(const rm_err_t &err) {
    // 与预检保持一致：只把非零错误码计入 arm_error_count。
    for (int i = 0; i < static_cast<int>(err.err_len) && i < 24; ++i) {
        if (err.err[i] != 0) {
            return false;
        }
    }
    return true;
}

void writeCsvRow(std::ofstream &csv, const std::array<std::string, 19> &values) {
    for (size_t i = 0; i < values.size(); ++i) {
        if (i > 0) {
            csv << ',';
        }
        csv << csvEscape(values[i]);
    }
    csv << '\n';
}

} // namespace

int main() {
    using namespace realman_hw_test;

    TestSuite suite("rm75_acceptance_communication");
    suite.info("source_document", "RM75-6F mechanical arm testing agent operation document v1.0");
    suite.info("scope", "30-second read-only communication smoke test and acceptance CSV data logging");

    RobotSession session(&suite);
    if (!session.connect()) {
        return suite.finish();
    }

    rm_robot_info_t info = {};
    if (!readRobotInfo(session.handle(), &info, &suite) ||
        !validateDof(static_cast<int>(info.arm_dof), &suite)) {
        return suite.finish();
    }
    const int dof = static_cast<int>(info.arm_dof);

    rm_frame_t tool_frame = {};
    float payload_kg = 0.0F;
    const int tool_frame_result = rm_get_current_tool_frame(session.handle(), &tool_frame);
    suite.expect(tool_frame_result == 0, "rm_get_current_tool_frame", "error=" + std::to_string(tool_frame_result));
    if (tool_frame_result == 0) {
        payload_kg = tool_frame.payload;
    }

    const std::string report_dir = reportsDirectory();
    const std::string latest_csv_path = report_dir + "/rm75_acceptance_communication_latest.csv";
    const std::string timestamped_csv_path = report_dir + "/rm75_acceptance_communication_" + timestampString() + ".csv";
    std::ofstream latest_csv(latest_csv_path);
    std::ofstream timestamped_csv(timestamped_csv_path);
    suite.expect(static_cast<bool>(latest_csv) && static_cast<bool>(timestamped_csv),
                 "communication_csv_open",
                 latest_csv_path);

    const std::array<std::string, 19> header = {
        // 字段顺序对齐测试文档的数据记录要求，方便后续直接导入表格或脚本分析。
        "timestamp_ms",
        "test_id",
        "robot_model",
        "robot_ip",
        "motion_mode",
        "speed_percent",
        "payload_kg",
        "target_joints_deg",
        "actual_joints_deg",
        "target_tcp_pose",
        "actual_tcp_pose",
        "force_torque",
        "joint_current",
        "joint_temperature",
        "voltage",
        "power",
        "error_code",
        "safety_state",
        "comment",
    };
    writeCsvRow(latest_csv, header);
    writeCsvRow(timestamped_csv, header);

    int samples = 0;
    int successful_full_samples = 0;
    int read_attempts = 0;
    int read_failures = 0;
    int controller_error_count = 0;
    int arm_error_count = 0;
    int finite_failures = 0;
    double max_period_ms = 0.0;
    double period_sum_ms = 0.0;
    int period_count = 0;

    auto previous_sample_time = std::chrono::steady_clock::now();
    const auto end_time = previous_sample_time + std::chrono::seconds(kDurationSeconds);

    while (std::chrono::steady_clock::now() < end_time) {
        // steady_clock 只用于性能统计，避免 NTP/手动改时导致采样周期异常。
        const auto sample_start = std::chrono::steady_clock::now();
        if (samples > 0) {
            const double period_ms = std::chrono::duration<double, std::milli>(sample_start - previous_sample_time).count();
            max_period_ms = std::max(max_period_ms, period_ms);
            period_sum_ms += period_ms;
            ++period_count;
        }
        previous_sample_time = sample_start;

        rm_current_arm_state_t arm_state = {};
        rm_force_data_t force = {};
        std::array<float, ARM_DOF> joint_current = {};
        std::array<float, ARM_DOF> joint_temperature = {};
        float controller_voltage = 0.0F;
        float controller_current = 0.0F;
        float controller_temperature = 0.0F;
        int controller_error = -1;

        bool sample_ok = true;
        int result = 0;

        // 每轮采样只调用 rm_get_* 接口，不发送运动、清零或配置修改指令。
        ++read_attempts;
        result = rm_get_current_arm_state(session.handle(), &arm_state);
        if (result != 0) {
            ++read_failures;
            sample_ok = false;
        } else if (!errListClear(arm_state.err)) {
            ++arm_error_count;
        }

        ++read_attempts;
        result = rm_get_force_data(session.handle(), &force);
        if (result != 0) {
            ++read_failures;
            sample_ok = false;
        }

        ++read_attempts;
        result = rm_get_current_joint_current(session.handle(), joint_current.data());
        if (result != 0) {
            ++read_failures;
            sample_ok = false;
        }

        ++read_attempts;
        result = rm_get_current_joint_temperature(session.handle(), joint_temperature.data());
        if (result != 0) {
            ++read_failures;
            sample_ok = false;
        }

        ++read_attempts;
        result = rm_get_controller_state(session.handle(),
                                         &controller_voltage,
                                         &controller_current,
                                         &controller_temperature,
                                         &controller_error);
        if (result != 0) {
            ++read_failures;
            sample_ok = false;
        } else if (controller_error != 0) {
            ++controller_error_count;
        }

        const bool finite_sample = poseFinite(arm_state.pose) &&
                                   isFiniteArray(arm_state.joint, dof) &&
                                   isFiniteArray(force.work_zero_force_data, kForceAxisCount) &&
                                   isFiniteArray(joint_current.data(), dof) &&
                                   isFiniteArray(joint_temperature.data(), dof) &&
                                   inRange(controller_voltage, 18.0F, 30.0F) &&
                                   inRange(controller_current, 0.0F, 10.0F) &&
                                   inRange(controller_temperature, 0.0F, 85.0F);
        if (!finite_sample) {
            ++finite_failures;
            sample_ok = false;
        }
        if (sample_ok) {
            ++successful_full_samples;
        }

        const double power_w = static_cast<double>(controller_voltage) * controller_current;
        const std::string safety_state =
            controller_error == 0 && errListClear(arm_state.err) ? "no_error" : "error_present";
        const std::string error_code =
            errSummary(arm_state.err) +
            ";controller_err=" + std::to_string(controller_error);
        const std::string comment = sample_ok ? "ok" : "read_or_range_failure";

        std::array<std::string, 19> row = {
            std::to_string(epochMs()),
            "rm75_acceptance_communication",
            "RM75-6F",
            kRobotIp,
            "IDLE_READONLY",
            "0",
            std::to_string(payload_kg),
            // 本测试不规划轨迹，因此目标关节/TCP 写 NA，只记录实际状态。
            "NA",
            joinFloats(arm_state.joint, dof, 4),
            "NA",
            poseCsv(arm_state.pose),
            joinFloats(force.work_zero_force_data, kForceAxisCount, 4),
            joinFloats(joint_current.data(), dof, 3),
            joinFloats(joint_temperature.data(), dof, 3),
            std::to_string(controller_voltage),
            std::to_string(power_w),
            error_code,
            safety_state,
            comment,
        };
        writeCsvRow(latest_csv, row);
        writeCsvRow(timestamped_csv, row);

        ++samples;
        sleepMs(kSampleIntervalMs);
    }

    const double failure_rate = read_attempts > 0
                                    ? static_cast<double>(read_failures) / static_cast<double>(read_attempts)
                                    : 1.0;
    const double avg_period_ms = period_count > 0 ? period_sum_ms / period_count : 0.0;
    suite.info("samples", std::to_string(samples));
    suite.info("successful_full_samples", std::to_string(successful_full_samples));
    suite.info("read_attempts", std::to_string(read_attempts));
    suite.info("read_failures", std::to_string(read_failures));
    suite.info("failure_rate", std::to_string(failure_rate));
    suite.info("avg_sample_period_ms", std::to_string(avg_period_ms));
    suite.info("max_sample_period_ms", std::to_string(max_period_ms));
    suite.info("csv_latest", latest_csv_path);

    suite.expect(samples >= 100, "sample_count_minimum", "samples=" + std::to_string(samples));
    suite.expect(successful_full_samples >= samples * 95 / 100,
                 "successful_sample_ratio_at_least_95_percent",
                 "successful=" + std::to_string(successful_full_samples) + ", samples=" + std::to_string(samples));
    suite.expect(failure_rate <= 0.01, "sdk_read_failure_rate_at_most_1_percent",
                 "failure_rate=" + std::to_string(failure_rate));
    suite.expect(finite_failures == 0, "sample_values_finite_and_in_expected_ranges",
                 "finite_failures=" + std::to_string(finite_failures));
    suite.expect(controller_error_count == 0, "controller_error_absent_during_communication",
                 "count=" + std::to_string(controller_error_count));
    suite.expect(arm_error_count == 0, "arm_error_absent_during_communication",
                 "count=" + std::to_string(arm_error_count));
    suite.expect(max_period_ms <= 1000.0, "sample_period_below_1s", "max_ms=" + std::to_string(max_period_ms));

    return suite.finish();
}
