/**
 * @file drag_teach_recorder.cpp
 * @brief RM75 拖动示教轨迹录制器实现文件。
 *
 * @author mashiro
 * @date 2026-07-09
 * @last_modified 2026-07-09
 *
 * @details
 * 本文件实现拖动示教录制流程。流程会访问真实机械臂，因此实现中保持显式安全倒计时、
 * 配置边界检查、采样失败处理和停止拖动示教保护。CSV 和路径管理委托给 trajectory
 * 模块，真实硬件调用委托给 `IRobotArm`。
 *
 * 设计原则：
 * - 启动拖动示教前完成配置和文件路径校验；
 * - 启动后无论采样是否失败都尝试停止拖动；
 * - 采样循环只负责读取状态和生成样本；
 * - 录制摘要和厂商轨迹保存结果分开处理。
 *
 * @dependencies
 * - rm_control/core/drag_teach_recorder.h
 */
#include "rm_control/core/drag_teach_recorder.h"

#include <chrono>
#include <iostream>
#include <thread>
#include <vector>

namespace rm_control::core {
namespace {

constexpr int kMinimumSamplePeriodMs = 50;
constexpr int kMaximumDurationSeconds = 3600;
constexpr int kSafetyCountdownSeconds = 5;

/**
 * @brief 将机械臂状态转换为轨迹采样点。
 *
 * @author mashiro
 * @date 2026-07-09
 * @last_modified 2026-07-09
 *
 * @details
 * 拖动示教录制需要把当前机械臂状态保存为 CSV。该函数只做结构转换：
 * 设置相对起始时间、复制关节角和 TCP 位姿，不访问文件系统或硬件。
 *
 * @param[in] elapsed_ms 从录制开始到当前采样的毫秒数。
 * @param[in] state 当前机械臂状态。
 *
 * @return 轨迹采样点。
 *
 * @throws 本函数不主动抛出异常；向量复制失败由标准库报告。
 *
 * @note
 * 错误码当前不写入轨迹 CSV，只用于状态读取阶段诊断。
 *
 * @warning
 * 如果后续 CSV 需要记录错误码，应扩展 `TrajectorySample`，不要在此临时拼接文本。
 *
 * @thread_safety
 * 函数只使用局部对象，线程安全。
 *
 * @par 边界条件
 * 空关节列表会被原样保存，后续 CSV 写入阶段会按 7 轴要求校验。
 *
 * @par 后续维护
 * 新增采样字段时优先修改公共数据结构和 trajectory 模块。
 */
common::TrajectorySample makeSample(std::size_t sample_index,
                                    long long elapsed_ms,
                                    const common::ArmState &state) {
    common::TrajectorySample sample;
    sample.sample_index = sample_index;
    sample.elapsed_ms = elapsed_ms;
    sample.joints_deg = state.joints_deg;
    sample.tcp_pose = state.tcp_pose;
    sample.source = "realman_rm75_drag_teach_legacy_recorder";
    return sample;
}

/**
 * @brief 打印拖动示教启动前安全倒计时。
 *
 * @author mashiro
 * @date 2026-07-09
 * @last_modified 2026-07-09
 *
 * @details
 * 拖动示教是需要人工参与的真实硬件流程。该函数在启动前输出安全提示并等待固定秒数，
 * 给操作者最后确认工作空间和急停状态的时间。
 *
 * @param[out] out 过程日志输出流。
 *
 * @return 无返回值。
 *
 * @throws 本函数不主动抛出异常；输出流异常由调用方处理。
 *
 * @note
 * 倒计时长度由 `kSafetyCountdownSeconds` 控制。
 *
 * @warning
 * 不要删除倒计时；如果需要自动化测试，应新增 dry-run 模式而不是绕过此函数。
 *
 * @thread_safety
 * 函数会阻塞当前线程，不适合在实时控制线程中调用。
 *
 * @par 边界条件
 * 倒计时为 0 时不会进入循环；当前常量固定为正数。
 *
 * @par 后续维护
 * 如需人工确认输入，应在 CLI 或 app 层增加显式确认，不在本函数读取 stdin。
 */
void printCountdown(std::ostream &out) {
    out << "Prepare for six-dof force drag teach. Clear workspace and keep E-stop reachable.\n";
    for (int second = kSafetyCountdownSeconds; second > 0; --second) {
        out << "Drag teach starts in " << second << "...\n";
        std::this_thread::sleep_for(std::chrono::seconds(1));
    }
}

} // namespace

DragTeachRecorder::DragTeachRecorder(interfaces::IRobotArm &robot)
    : robot_(robot) {}

common::Result<DragTeachRecordSummary> DragTeachRecorder::record(
    const DragTeachRecordConfig &config,
    std::ostream &out) {
    const auto validation = validateConfig(config);
    if (!validation.ok) {
        return common::Result<DragTeachRecordSummary>::failure(validation.error_message);
    }

    const auto files = createTrajectoryFileSet(config);
    if (!files.ok) {
        return common::Result<DragTeachRecordSummary>::failure(files.error_message);
    }

    printCountdown(out);
    const auto start_result = robot_.startDragTeach(config.drag_options);
    if (!start_result.ok) {
        return common::Result<DragTeachRecordSummary>::failure(start_result.error_message);
    }

    const auto samples = collectTrajectorySamples(config, out);
    const auto stop_result = robot_.stopDragTeach();
    if (!samples.ok) {
        return common::Result<DragTeachRecordSummary>::failure(samples.error_message);
    }
    if (!stop_result.ok) {
        return common::Result<DragTeachRecordSummary>::failure(stop_result.error_message);
    }

    const auto write_result =
        modules::trajectory::writeTrajectoryCsv(files.value.csv_path, samples.value);
    if (!write_result.ok) {
        return common::Result<DragTeachRecordSummary>::failure(write_result.error_message);
    }

    DragTeachRecordSummary summary = buildSummary(files.value, samples.value);
    saveVendorTrajectoryIfRequested(config, &summary);

    out << "CSV saved: " << summary.files.csv_path << '\n';
    out << "Vendor trajectory: " << summary.vendor_save_message << '\n';
    return common::Result<DragTeachRecordSummary>::success(summary);
}

common::Result<void> DragTeachRecorder::validateConfig(const DragTeachRecordConfig &config) const {
    if (config.storage_root.empty()) {
        return common::Result<void>::failure("storage_root must not be empty");
    }
    if (config.session_name.empty()) {
        return common::Result<void>::failure("session_name must not be empty");
    }
    if (config.duration_seconds <= 0 || config.duration_seconds > kMaximumDurationSeconds) {
        return common::Result<void>::failure("duration_seconds must be in range 1.." +
                                            std::to_string(kMaximumDurationSeconds));
    }
    if (config.sample_period_ms < kMinimumSamplePeriodMs) {
        return common::Result<void>::failure("sample_period_ms must be at least " +
                                            std::to_string(kMinimumSamplePeriodMs));
    }
    return common::Result<void>::success();
}

common::Result<modules::trajectory::TrajectoryFileSet>
DragTeachRecorder::createTrajectoryFileSet(const DragTeachRecordConfig &config) const {
    return modules::trajectory::makeTrajectoryFileSet(config.storage_root, config.session_name);
}

common::Result<std::vector<common::TrajectorySample>>
DragTeachRecorder::collectTrajectorySamples(const DragTeachRecordConfig &config,
                                            std::ostream &out) {
    std::vector<common::TrajectorySample> samples;
    const auto start_time = std::chrono::steady_clock::now();
    const int total_samples = (config.duration_seconds * 1000) / config.sample_period_ms + 1;

    for (int sample_index = 0; sample_index < total_samples; ++sample_index) {
        const auto target_time =
            start_time + std::chrono::milliseconds(sample_index * config.sample_period_ms);
        std::this_thread::sleep_until(target_time);

        const auto state = robot_.getCurrentArmState();
        if (!state.ok) {
            return common::Result<std::vector<common::TrajectorySample>>::failure(
                state.error_message);
        }

        const auto now = std::chrono::steady_clock::now();
        const long long elapsed_ms =
            std::chrono::duration_cast<std::chrono::milliseconds>(now - start_time).count();
        samples.push_back(makeSample(static_cast<std::size_t>(sample_index),
                                     elapsed_ms,
                                     state.value));
        if (sample_index == 0 || sample_index == total_samples - 1) {
            out << "sample " << sample_index << "/" << (total_samples - 1)
                << " elapsed_ms=" << elapsed_ms << '\n';
        }
    }

    return common::Result<std::vector<common::TrajectorySample>>::success(samples);
}

DragTeachRecordSummary DragTeachRecorder::buildSummary(
    const modules::trajectory::TrajectoryFileSet &files,
    const std::vector<common::TrajectorySample> &samples) const {
    DragTeachRecordSummary summary;
    summary.files = files;
    summary.sample_count = static_cast<int>(samples.size());
    return summary;
}

void DragTeachRecorder::saveVendorTrajectoryIfRequested(
    const DragTeachRecordConfig &config,
    DragTeachRecordSummary *summary) {
    if (summary == nullptr) {
        return;
    }
    if (!config.save_vendor_trajectory) {
        summary->vendor_save_message = "vendor trajectory save disabled";
        return;
    }

    const common::VendorTrajectorySaveAttempt vendor_attempt =
        robot_.saveDragTeachTrajectory(summary->files.vendor_path);
    if (vendor_attempt.api_called && vendor_attempt.return_code == 0) {
        summary->vendor_trajectory_saved = true;
        summary->vendor_save_message =
            "saved " + std::to_string(vendor_attempt.point_count) + " vendor points";
        return;
    }

    summary->vendor_save_message = "saveDragTeachTrajectory failed";
    if (vendor_attempt.api_called) {
        summary->vendor_save_message +=
            ", return_code=" + std::to_string(vendor_attempt.return_code);
    } else {
        summary->vendor_save_message += " before vendor API call";
    }
    if (!vendor_attempt.error_message.empty()) {
        summary->vendor_save_message += ": " + vendor_attempt.error_message;
    }
}

} // namespace rm_control::core
