/**
 * @file trajectory_file_store.cpp
 * @brief RM75 拖动示教轨迹文件存储模块实现。
 *
 * @author mashiro
 * @date 2026-07-09
 * @last_modified 2026-07-09
 *
 * @details
 * 本文件负责轨迹目录创建、CSV 读写、会话名清洗和 C++ TCP waypoint 文件导出。
 * 该模块属于离线文件处理模块，不访问真实机械臂、不调用 RM_API2。core 层只把采样数据
 * 传入本模块，本模块负责稳定地落盘和读取。
 *
 * 设计原则：
 * - 文件路径和 CSV 格式集中管理；
 * - 硬件控制逻辑不进入文件模块；
 * - 所有文件错误通过 `Result` 返回；
 * - RM75 7 轴 CSV 格式使用命名常量表达，避免魔法数字散落。
 *
 * @dependencies
 * - rm_control/modules/trajectory/trajectory_file_store.h
 */
#include "rm_control/modules/trajectory/trajectory_file_store.h"

#include "rm_control/common/rm75_constants.h"

#include <algorithm>
#include <cerrno>
#include <cctype>
#include <cmath>
#include <cstdlib>
#include <ctime>
#include <filesystem>
#include <fstream>
#include <iomanip>
#include <limits>
#include <sstream>
#include <vector>

#ifndef RM_RUNTIME_DATA_ROOT
#define RM_RUNTIME_DATA_ROOT "build/runtime_data"
#endif

namespace rm_control::modules::trajectory {
namespace {

constexpr int kCsvSampleIndexColumn = 0;
constexpr int kCsvElapsedMsColumn = 1;
constexpr int kCsvJointStartColumn = 2;
constexpr int kCsvTcpXColumn = 9;
constexpr int kCsvTcpYColumn = 10;
constexpr int kCsvTcpZColumn = 11;
constexpr int kCsvTcpRxColumn = 12;
constexpr int kCsvTcpRyColumn = 13;
constexpr int kCsvTcpRzColumn = 14;
constexpr int kCsvSourceColumn = 15;
constexpr int kLegacyCsvColumnCount = 15;
constexpr int kExpectedCsvColumnCount = 16;
constexpr const char *kCurrentCsvHeader =
    "sample_index,timestamp_ms,"
    "j1_deg,j2_deg,j3_deg,j4_deg,j5_deg,j6_deg,j7_deg,"
    "tcp_x_m,tcp_y_m,tcp_z_m,tcp_rx_rad,tcp_ry_rad,tcp_rz_rad,source";
constexpr const char *kLegacyCsvHeader =
    "sample_index,elapsed_ms,"
    "j1_deg,j2_deg,j3_deg,j4_deg,j5_deg,j6_deg,j7_deg,"
    "tcp_x_m,tcp_y_m,tcp_z_m,tcp_rx_rad,tcp_ry_rad,tcp_rz_rad";

std::string parentPath(const std::string &path) {
    return std::filesystem::path(path).parent_path().string();
}

common::Result<void> ensureDirectoryRecursive(const std::string &path) {
    if (path.empty()) {
        return common::Result<void>::success();
    }
    std::error_code filesystem_error;
    std::filesystem::create_directories(path, filesystem_error);
    if (filesystem_error) {
        return common::Result<void>::failure(
            "failed to create trajectory directory " + path + ": " +
            filesystem_error.message());
    }
    if (!std::filesystem::is_directory(path, filesystem_error) || filesystem_error) {
        return common::Result<void>::failure(
            "trajectory directory path is not a directory: " + path);
    }
    return common::Result<void>::success();
}

std::string joinPath(const std::string &left, const std::string &right) {
    if (left.empty()) {
        return right;
    }
    if (right.empty()) {
        return left;
    }
    return (std::filesystem::path(left) / right).string();
}

bool ConvertToLocalTime(std::time_t time, std::tm *local_time) {
    if (local_time == nullptr) {
        return false;
    }
#if defined(_WIN32)
    return ::localtime_s(local_time, &time) == 0;
#else
    return ::localtime_r(&time, local_time) != nullptr;
#endif
}

std::string trimCopy(const std::string &value) {
    const std::string whitespace = " \t\r\n";
    const size_t begin = value.find_first_not_of(whitespace);
    if (begin == std::string::npos) {
        return "";
    }
    const size_t end = value.find_last_not_of(whitespace);
    return value.substr(begin, end - begin + 1);
}

std::vector<std::string> splitCsvLine(const std::string &line) {
    std::vector<std::string> columns;
    std::stringstream input(line);
    std::string column;
    while (std::getline(input, column, ',')) {
        columns.push_back(trimCopy(column));
    }
    if (!line.empty() && line.back() == ',') {
        columns.emplace_back();
    }
    return columns;
}

bool parseFloat(const std::string &value, float *out) {
    if (value.empty() || out == nullptr) {
        return false;
    }

    errno = 0;
    char *end = nullptr;
    const float parsed = std::strtof(value.c_str(), &end);
    if (errno != 0 || end == value.c_str() || *end != '\0' || !std::isfinite(parsed)) {
        return false;
    }

    *out = parsed;
    return true;
}

bool parseLongLong(const std::string &value, long long *out) {
    if (value.empty() || out == nullptr) {
        return false;
    }

    errno = 0;
    char *end = nullptr;
    const long long parsed = std::strtoll(value.c_str(), &end, 10);
    if (errno != 0 || end == value.c_str() || *end != '\0') {
        return false;
    }

    *out = parsed;
    return true;
}

bool isValidTrajectorySource(const std::string &source) {
    if (source.empty()) {
        return false;
    }
    return std::all_of(source.begin(), source.end(), [](unsigned char character) {
        return std::isalnum(character) != 0 || character == '_' || character == '-' ||
               character == '.';
    });
}

/**
 * @brief 从 CSV 列中解析 RM75 关节角。
 *
 * @author mashiro
 * @date 2026-07-09
 * @last_modified 2026-07-09
 *
 * @details
 * CSV 固定使用 7 个关节角列。该函数集中处理关节列解析和错误信息生成，
 * 避免 `parseSample()` 同时承担列索引细节和样本结构装配。
 *
 * @param[in] columns CSV 行拆分后的列。
 * @param[in] line_number 当前 CSV 行号，用于错误信息。
 * @param[out] joints_deg 解析后的关节角列表，单位为度。
 *
 * @return 成功或具体关节列错误。
 *
 * @throws 本函数不主动抛出异常；向量分配失败由标准库报告。
 *
 * @note
 * 该函数假设调用方已经验证列数为 `kExpectedCsvColumnCount`。
 *
 * @warning
 * 当前 CSV 格式固定为 RM75 7 轴；支持其他 DOF 时需要扩展格式版本。
 *
 * @thread_safety
 * 函数只操作局部变量和输出容器，线程安全。
 *
 * @par 边界条件
 * 任一关节列为空、非数字或非有限值都会失败。
 *
 * @par 后续维护
 * 如果 CSV 增加表头版本，应根据版本选择关节数量。
 */
common::Result<void> parseJointColumns(const std::vector<std::string> &columns,
                                       int line_number,
                                       std::vector<float> *joints_deg) {
    if (joints_deg == nullptr) {
        return common::Result<void>::failure("joints_deg output must not be null");
    }

    joints_deg->assign(common::kRm75JointCount, 0.0F);
    for (std::size_t joint_index = 0;
         joint_index < common::kRm75JointCount;
         ++joint_index) {
        const std::size_t column_index =
            static_cast<std::size_t>(kCsvJointStartColumn) + joint_index;
        if (!parseFloat(columns[column_index], &(*joints_deg)[joint_index])) {
            return common::Result<void>::failure(
                "CSV line " + std::to_string(line_number) + " has invalid J" +
                std::to_string(joint_index + 1));
        }
    }
    return common::Result<void>::success();
}

/**
 * @brief 从 CSV 列中解析 TCP 位姿。
 *
 * @author mashiro
 * @date 2026-07-09
 * @last_modified 2026-07-09
 *
 * @details
 * 轨迹 CSV 使用 6 个 TCP 位姿列：x/y/z/rx/ry/rz。该函数集中处理列索引和格式校验，
 * 让 `parseSample()` 只关注样本级流程。
 *
 * @param[in] columns CSV 行拆分后的列。
 * @param[in] line_number 当前 CSV 行号，用于错误信息。
 * @param[out] pose 解析后的 TCP 位姿。
 *
 * @return 成功或 TCP 位姿字段错误。
 *
 * @throws 本函数不主动抛出异常。
 *
 * @note
 * 位置单位为 m，欧拉角单位为 rad。
 *
 * @warning
 * 本函数不判断位姿是否可达或适合执行。
 *
 * @thread_safety
 * 函数只操作输出对象，线程安全。
 *
 * @par 边界条件
 * 任一 TCP 列为空、非数字或非有限值都会失败。
 *
 * @par 后续维护
 * 如果 CSV 支持四元数，应新增字段并扩展 `TrajectorySample`。
 */
common::Result<void> parseTcpPoseColumns(const std::vector<std::string> &columns,
                                         int line_number,
                                         common::Pose *pose) {
    if (pose == nullptr) {
        return common::Result<void>::failure("pose output must not be null");
    }

    if (!parseFloat(columns[kCsvTcpXColumn], &pose->x) ||
        !parseFloat(columns[kCsvTcpYColumn], &pose->y) ||
        !parseFloat(columns[kCsvTcpZColumn], &pose->z) ||
        !parseFloat(columns[kCsvTcpRxColumn], &pose->rx) ||
        !parseFloat(columns[kCsvTcpRyColumn], &pose->ry) ||
        !parseFloat(columns[kCsvTcpRzColumn], &pose->rz)) {
        return common::Result<void>::failure(
            "CSV line " + std::to_string(line_number) + " has invalid TCP pose");
    }
    return common::Result<void>::success();
}

common::Result<common::TrajectorySample> parseSample(const std::vector<std::string> &columns,
                                                     int line_number) {
    if (columns.size() != kExpectedCsvColumnCount &&
        columns.size() != kLegacyCsvColumnCount) {
        return common::Result<common::TrajectorySample>::failure(
            "CSV line " + std::to_string(line_number) +
            " must have 15 legacy or 16 current columns");
    }

    long long parsed_sample_index = 0;
    common::TrajectorySample sample;
    if (!parseLongLong(columns[kCsvSampleIndexColumn], &parsed_sample_index) ||
        parsed_sample_index < 0 ||
        static_cast<unsigned long long>(parsed_sample_index) >
            std::numeric_limits<std::size_t>::max() ||
        !parseLongLong(columns[kCsvElapsedMsColumn], &sample.elapsed_ms)) {
        return common::Result<common::TrajectorySample>::failure(
            "CSV line " + std::to_string(line_number) + " has invalid sample_index or elapsed_ms");
    }
    sample.sample_index = static_cast<std::size_t>(parsed_sample_index);

    auto joint_result = parseJointColumns(columns, line_number, &sample.joints_deg);
    if (!joint_result.ok) {
        return common::Result<common::TrajectorySample>::failure(joint_result.error_message);
    }

    auto pose_result = parseTcpPoseColumns(columns, line_number, &sample.tcp_pose);
    if (!pose_result.ok) {
        return common::Result<common::TrajectorySample>::failure(pose_result.error_message);
    }

    if (columns.size() == kExpectedCsvColumnCount) {
        sample.source = columns[kCsvSourceColumn];
        if (!isValidTrajectorySource(sample.source)) {
            return common::Result<common::TrajectorySample>::failure(
                "CSV line " + std::to_string(line_number) +
                " has an invalid trajectory source");
        }
    } else {
        sample.source = "legacy_rm75_csv";
    }

    return common::Result<common::TrajectorySample>::success(sample);
}

void writeCsvHeader(std::ostream &out) {
    out << kCurrentCsvHeader << '\n';
}

void writeCsvSample(std::ostream &out, const common::TrajectorySample &sample) {
    out << sample.sample_index << ',' << sample.elapsed_ms;
    out << std::fixed << std::setprecision(6);
    for (float joint : sample.joints_deg) {
        out << ',' << joint;
    }
    out << ',' << sample.tcp_pose.x
        << ',' << sample.tcp_pose.y
        << ',' << sample.tcp_pose.z
        << ',' << sample.tcp_pose.rx
        << ',' << sample.tcp_pose.ry
        << ',' << sample.tcp_pose.rz
        << ',' << sample.source << '\n';
}

std::string makeCppIdentifier(const std::string &session_name) {
    std::string identifier = sanitizeSessionName(session_name);
    std::replace(identifier.begin(), identifier.end(), '-', '_');
    if (identifier.empty() || std::isdigit(static_cast<unsigned char>(identifier[0]))) {
        identifier = "trajectory_" + identifier;
    }
    return identifier;
}

/**
 * @brief 校验轨迹样本能否写入当前 RM75 CSV 格式。
 *
 * @author mashiro
 * @date 2026-07-09
 * @last_modified 2026-07-09
 *
 * @details
 * 当前 CSV 格式固定写出 7 个关节角。该函数在打开文件前统一校验样本列表和每个样本
 * 的关节数量，避免写到一半才发现格式不一致。
 *
 * @param[in] samples 待写入样本列表。
 *
 * @return 成功或样本格式错误。
 *
 * @throws 本函数不主动抛出异常。
 *
 * @note
 * 空样本列表会被拒绝。
 *
 * @warning
 * 支持其他 DOF 前不要放宽此校验，否则会破坏 CSV 读写一致性。
 *
 * @thread_safety
 * 函数只读取输入容器，线程安全。
 *
 * @par 边界条件
 * 任一样本关节数量不是 7 都会失败。
 *
 * @par 后续维护
 * 如果引入 CSV 版本字段，应根据版本校验不同关节数量。
 */
common::Result<void> validateSamplesForRm75Csv(
    const std::vector<common::TrajectorySample> &samples) {
    if (samples.empty()) {
        return common::Result<void>::failure("trajectory sample list must not be empty");
    }

    for (std::size_t sample_index = 0; sample_index < samples.size(); ++sample_index) {
        const common::TrajectorySample &sample = samples[sample_index];
        if (sample.sample_index != sample_index) {
            return common::Result<void>::failure(
                "trajectory sample_index must be contiguous and start at zero");
        }
        if (sample.joints_deg.size() != common::kRm75JointCount) {
            return common::Result<void>::failure("trajectory sample joints_deg size must be 7");
        }
        if (!isValidTrajectorySource(sample.source)) {
            return common::Result<void>::failure(
                "trajectory sample source must contain only letters, digits, '_', '-' or '.'");
        }
    }
    return common::Result<void>::success();
}

/**
 * @brief 写出 C++ waypoint 文件头部。
 *
 * @author mashiro
 * @date 2026-07-09
 * @last_modified 2026-07-09
 *
 * @details
 * C++ 导出文件由头部类型定义、waypoint 数据和尾部命名空间闭合组成。
 * 该函数只负责头部，避免 `exportTcpPathCpp()` 同时拼接所有文本。
 *
 * @param[out] output 输出文件流。
 * @param[in] variable_name waypoint 变量名。
 *
 * @return 无返回值。
 *
 * @throws 本函数不主动抛出异常；输出流异常由调用方处理。
 *
 * @note
 * 输出命名空间固定为 `rm_control_generated`。
 *
 * @warning
 * 修改结构体字段会影响所有生成文件的使用方。
 *
 * @thread_safety
 * 函数无全局状态；并发写同一流需调用方保护。
 *
 * @par 边界条件
 * 空变量名会生成非法 C++，调用方应先构造合法标识符。
 *
 * @par 后续维护
 * 如果生成头文件而非 cpp，应新增导出函数，不要复用此头部文本。
 */
void writeTcpPathCppHeader(std::ostream &output, const std::string &variable_name) {
    output << "#include <cstdint>\n"
           << "#include <vector>\n\n"
           << "namespace rm_control_generated {\n\n"
           << "struct TcpWaypoint {\n"
           << "    std::int64_t elapsed_ms;\n"
           << "    float x;\n"
           << "    float y;\n"
           << "    float z;\n"
           << "    float rx;\n"
           << "    float ry;\n"
           << "    float rz;\n"
           << "};\n\n"
           << "const std::vector<TcpWaypoint> " << variable_name << " = {\n";
}

/**
 * @brief 写出 C++ waypoint 数据项。
 *
 * @author mashiro
 * @date 2026-07-09
 * @last_modified 2026-07-09
 *
 * @details
 * 该函数只负责把样本列表中的 TCP 位姿写成 C++ 初始化列表，不写 include、类型定义或
 * 命名空间尾部。拆分后便于以后复用 waypoint 数据写出逻辑。
 *
 * @param[out] output 输出文件流。
 * @param[in] samples 轨迹样本列表。
 *
 * @return 无返回值。
 *
 * @throws 本函数不主动抛出异常；输出流异常由调用方处理。
 *
 * @note
 * 只导出 TCP 位姿，不导出关节角。
 *
 * @warning
 * 本函数不校验样本为空；调用方负责提前校验。
 *
 * @thread_safety
 * 函数无全局状态；并发写同一流需调用方保护。
 *
 * @par 边界条件
 * 空样本列表只写出空数据项区域。
 *
 * @par 后续维护
 * 如需导出关节轨迹，应新增独立数据结构和导出函数。
 */
void writeTcpPathCppSamples(std::ostream &output,
                            const std::vector<common::TrajectorySample> &samples) {
    output << std::fixed << std::setprecision(6);
    for (const common::TrajectorySample &sample : samples) {
        output << "    {" << sample.elapsed_ms
               << ", " << sample.tcp_pose.x
               << "F, " << sample.tcp_pose.y
               << "F, " << sample.tcp_pose.z
               << "F, " << sample.tcp_pose.rx
               << "F, " << sample.tcp_pose.ry
               << "F, " << sample.tcp_pose.rz
               << "F},\n";
    }
}

/**
 * @brief 写出 C++ waypoint 文件尾部。
 *
 * @author mashiro
 * @date 2026-07-09
 * @last_modified 2026-07-09
 *
 * @details
 * 该函数闭合 waypoint 初始化列表和命名空间。拆分尾部输出可以让导出主流程保持短小。
 *
 * @param[out] output 输出文件流。
 *
 * @return 无返回值。
 *
 * @throws 本函数不主动抛出异常；输出流异常由调用方处理。
 *
 * @note
 * 尾部命名空间必须和头部保持一致。
 *
 * @warning
 * 修改命名空间时必须同步头部和使用方。
 *
 * @thread_safety
 * 函数无全局状态；并发写同一流需调用方保护。
 *
 * @par 边界条件
 * 即使样本为空也能输出语法完整尾部。
 *
 * @par 后续维护
 * 如果生成多段变量，应为每段生成独立尾部或新增 writer 类。
 */
void writeTcpPathCppFooter(std::ostream &output) {
    output << "};\n\n"
           << "} // namespace rm_control_generated\n";
}

} // namespace

std::string timestampString() {
    const std::time_t now = std::time(nullptr);
    std::tm local_time = {};
    if (!ConvertToLocalTime(now, &local_time)) {
        return "time_unavailable";
    }

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

std::string sanitizeSessionName(const std::string &raw_name) {
    std::string output;
    for (char value : raw_name) {
        const unsigned char ch = static_cast<unsigned char>(value);
        if (std::isalnum(ch) || value == '_' || value == '-') {
            output.push_back(value);
        } else {
            output.push_back('_');
        }
    }

    while (!output.empty() && output.front() == '_') {
        output.erase(output.begin());
    }
    while (!output.empty() && output.back() == '_') {
        output.pop_back();
    }

    if (output.empty()) {
        return "rm75_drag_" + timestampString();
    }
    return output;
}

std::string defaultTrajectoryStorageRoot() {
    return std::string(RM_RUNTIME_DATA_ROOT) + "/trajectories";
}

common::Result<TrajectoryStoragePaths> ensureTrajectoryStorage(const std::string &root_dir) {
    if (root_dir.empty()) {
        return common::Result<TrajectoryStoragePaths>::failure("trajectory storage root must not be empty");
    }

    TrajectoryStoragePaths paths;
    paths.root_dir = root_dir;
    paths.csv_dir = joinPath(root_dir, "csv");
    paths.vendor_dir = joinPath(root_dir, "vendor");
    paths.converted_dir = joinPath(root_dir, "converted");
    paths.reports_dir = joinPath(root_dir, "reports");

    for (const std::string &path : {paths.root_dir, paths.csv_dir, paths.vendor_dir,
                                    paths.converted_dir, paths.reports_dir}) {
        auto result = ensureDirectoryRecursive(path);
        if (!result.ok) {
            return common::Result<TrajectoryStoragePaths>::failure(result.error_message);
        }
    }
    return common::Result<TrajectoryStoragePaths>::success(paths);
}

common::Result<TrajectoryFileSet> makeTrajectoryFileSet(const std::string &root_dir,
                                                        const std::string &session_name) {
    auto storage = ensureTrajectoryStorage(root_dir);
    if (!storage.ok) {
        return common::Result<TrajectoryFileSet>::failure(storage.error_message);
    }

    const std::string safe_name = sanitizeSessionName(session_name);
    TrajectoryFileSet files;
    files.session_name = safe_name;
    files.csv_path = joinPath(storage.value.csv_dir, safe_name + ".csv");
    files.vendor_path = joinPath(storage.value.vendor_dir, safe_name + "_vendor.txt");
    files.converted_cpp_path = joinPath(storage.value.converted_dir, safe_name + "_tcp_path.cpp");
    files.report_path = joinPath(storage.value.reports_dir, safe_name + "_summary.md");
    return common::Result<TrajectoryFileSet>::success(files);
}

common::Result<void> writeTrajectoryCsv(const std::string &csv_path,
                                        const std::vector<common::TrajectorySample> &samples) {
    auto validation = validateSamplesForRm75Csv(samples);
    if (!validation.ok) {
        return validation;
    }

    auto parent_result = ensureDirectoryRecursive(parentPath(csv_path));
    if (!parent_result.ok) {
        return parent_result;
    }

    std::ofstream output(csv_path);
    if (!output) {
        return common::Result<void>::failure("failed to open CSV for writing: " + csv_path);
    }

    writeCsvHeader(output);
    for (const common::TrajectorySample &sample : samples) {
        writeCsvSample(output, sample);
    }
    return common::Result<void>::success();
}

common::Result<void> writeTrajectoryCsvWithoutOverwrite(
    const std::filesystem::path &csv_path,
    const std::vector<common::TrajectorySample> &samples) {
    const auto validation = validateSamplesForRm75Csv(samples);
    if (!validation.ok) {
        return validation;
    }
    if (csv_path.empty() || csv_path.parent_path().empty()) {
        return common::Result<void>::failure(
            "synchronized trajectory CSV path and parent must not be empty");
    }

    std::error_code filesystem_error;
    std::filesystem::create_directories(csv_path.parent_path(), filesystem_error);
    if (filesystem_error) {
        return common::Result<void>::failure(
            "failed to create synchronized trajectory CSV directory " +
            csv_path.parent_path().string() + ": " + filesystem_error.message());
    }
    if (std::filesystem::exists(csv_path, filesystem_error)) {
        return common::Result<void>::failure(
            "refusing to overwrite synchronized trajectory CSV: " + csv_path.string());
    }
    if (filesystem_error) {
        return common::Result<void>::failure(
            "failed to inspect synchronized trajectory CSV " + csv_path.string() +
            ": " + filesystem_error.message());
    }

    std::ofstream output(csv_path, std::ios::out);
    if (!output) {
        return common::Result<void>::failure(
            "failed to create synchronized trajectory CSV: " + csv_path.string());
    }
    writeCsvHeader(output);
    for (const common::TrajectorySample &sample : samples) {
        writeCsvSample(output, sample);
    }
    output.flush();
    if (!output) {
        return common::Result<void>::failure(
            "failed to write synchronized trajectory CSV: " + csv_path.string());
    }
    output.close();
    if (!output) {
        return common::Result<void>::failure(
            "failed to close synchronized trajectory CSV: " + csv_path.string());
    }
    return common::Result<void>::success();
}

common::Result<std::vector<common::TrajectorySample>> loadTrajectoryCsv(const std::string &csv_path) {
    std::ifstream input(csv_path);
    if (!input) {
        return common::Result<std::vector<common::TrajectorySample>>::failure(
            "failed to open CSV for reading: " + csv_path);
    }

    std::vector<common::TrajectorySample> samples;
    std::string line;
    int line_number = 0;
    while (std::getline(input, line)) {
        ++line_number;
        if (line_number == 1) {
            const std::string header = trimCopy(line);
            if (header != kCurrentCsvHeader && header != kLegacyCsvHeader) {
                return common::Result<std::vector<common::TrajectorySample>>::failure(
                    "trajectory CSV has an unsupported header: " + csv_path);
            }
            continue;
        }
        if (trimCopy(line).empty()) {
            continue;
        }

        auto sample = parseSample(splitCsvLine(line), line_number);
        if (!sample.ok) {
            return common::Result<std::vector<common::TrajectorySample>>::failure(sample.error_message);
        }
        if (sample.value.sample_index != samples.size()) {
            return common::Result<std::vector<common::TrajectorySample>>::failure(
                "trajectory CSV sample_index must be contiguous and start at zero");
        }
        if (sample.value.elapsed_ms < 0) {
            return common::Result<std::vector<common::TrajectorySample>>::failure(
                "trajectory CSV timestamp must not be negative");
        }
        if (!samples.empty() &&
            sample.value.elapsed_ms <= samples.back().elapsed_ms) {
            return common::Result<std::vector<common::TrajectorySample>>::failure(
                "trajectory CSV timestamps must be strictly increasing");
        }
        samples.push_back(sample.value);
    }

    if (samples.empty()) {
        return common::Result<std::vector<common::TrajectorySample>>::failure("trajectory CSV contains no samples");
    }
    return common::Result<std::vector<common::TrajectorySample>>::success(samples);
}

common::Result<void> exportTcpPathCpp(const std::string &cpp_path,
                                      const std::vector<common::TrajectorySample> &samples,
                                      const std::string &session_name) {
    if (samples.empty()) {
        return common::Result<void>::failure("trajectory sample list must not be empty");
    }

    auto parent_result = ensureDirectoryRecursive(parentPath(cpp_path));
    if (!parent_result.ok) {
        return parent_result;
    }

    std::ofstream output(cpp_path);
    if (!output) {
        return common::Result<void>::failure("failed to open C++ output: " + cpp_path);
    }

    const std::string variable_name = "kTcpPath_" + makeCppIdentifier(session_name);
    writeTcpPathCppHeader(output, variable_name);
    writeTcpPathCppSamples(output, samples);
    writeTcpPathCppFooter(output);
    return common::Result<void>::success();
}

} // namespace rm_control::modules::trajectory
