#include "rm_control/modules/trajectory/trajectory_file_store.h"

#include <fstream>
#include <iostream>
#include <sstream>
#include <string>
#include <vector>

#ifndef RM_TEST_OUTPUT_DIR
#define RM_TEST_OUTPUT_DIR "."
#endif

namespace {

int g_failures = 0;

void expect(bool condition, const std::string &message) {
    if (!condition) {
        ++g_failures;
        std::cerr << "[FAIL] " << message << '\n';
    }
}

rm_control::common::TrajectorySample makeSample(long long elapsed_ms, float j2_deg) {
    rm_control::common::TrajectorySample sample;
    sample.elapsed_ms = elapsed_ms;
    sample.joints_deg = {0.0F, j2_deg, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F};
    sample.tcp_pose.x = 0.1F;
    sample.tcp_pose.y = 0.2F;
    sample.tcp_pose.z = 0.3F;
    sample.tcp_pose.rx = 0.01F;
    sample.tcp_pose.ry = 0.02F;
    sample.tcp_pose.rz = 0.03F;
    return sample;
}

std::string readTextFile(const std::string &path) {
    std::ifstream input(path);
    std::ostringstream buffer;
    buffer << input.rdbuf();
    return buffer.str();
}

void testStoragePathsAndCsvRoundTrip() {
    const std::string root = std::string(RM_TEST_OUTPUT_DIR) + "/trajectory_store_test";
    auto files = rm_control::modules::trajectory::makeTrajectoryFileSet(root, "demo session");
    expect(files.ok, "file set should be created");
    if (!files.ok) {
        return;
    }
    expect(files.value.session_name == "demo_session", "session name should be sanitized");

    std::vector<rm_control::common::TrajectorySample> samples;
    samples.push_back(makeSample(0, 0.0F));
    samples.push_back(makeSample(100, 1.0F));

    auto write_result = rm_control::modules::trajectory::writeTrajectoryCsv(files.value.csv_path, samples);
    expect(write_result.ok, "CSV should be written");

    auto loaded = rm_control::modules::trajectory::loadTrajectoryCsv(files.value.csv_path);
    expect(loaded.ok, "CSV should be loaded");
    if (!loaded.ok) {
        return;
    }
    expect(loaded.value.size() == 2, "loaded sample count should match");
    expect(loaded.value[1].joints_deg[1] == 1.0F, "loaded J2 should match");
}

void testExportTcpPathCpp() {
    const std::string root = std::string(RM_TEST_OUTPUT_DIR) + "/trajectory_export_test";
    auto files = rm_control::modules::trajectory::makeTrajectoryFileSet(root, "tcp demo");
    expect(files.ok, "file set should be created for export");
    if (!files.ok) {
        return;
    }

    std::vector<rm_control::common::TrajectorySample> samples;
    samples.push_back(makeSample(0, 0.0F));
    samples.push_back(makeSample(100, 1.0F));

    auto export_result = rm_control::modules::trajectory::exportTcpPathCpp(
        files.value.converted_cpp_path, samples, files.value.session_name);
    expect(export_result.ok, "C++ TCP path should be exported");

    const std::string text = readTextFile(files.value.converted_cpp_path);
    expect(text.find("struct TcpWaypoint") != std::string::npos, "export should contain TcpWaypoint");
    expect(text.find("kTcpPath_tcp_demo") != std::string::npos, "export should contain sanitized variable");
}

} // namespace

int main() {
    testStoragePathsAndCsvRoundTrip();
    testExportTcpPathCpp();

    if (g_failures == 0) {
        std::cout << "[PASS] rm_trajectory_file_store_tests\n";
        return 0;
    }

    std::cerr << g_failures << " test(s) failed\n";
    return 1;
}
