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

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

#ifndef RM_TEST_OUTPUT_DIR
#define RM_TEST_OUTPUT_DIR "."
#endif

namespace {

struct TestContext {
    int failures = 0;

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

rm_control::common::TrajectorySample makeSample(std::size_t sample_index,
                                                long long elapsed_ms,
                                                float j2_deg) {
    rm_control::common::TrajectorySample sample;
    sample.sample_index = sample_index;
    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(TestContext *context) {
    const std::string root = std::string(RM_TEST_OUTPUT_DIR) + "/trajectory_store_test";
    auto files = rm_control::modules::trajectory::makeTrajectoryFileSet(root, "demo session");
    context->Expect(files.ok, "file set should be created");
    if (!files.ok) {
        return;
    }
    context->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, 0.0F));
    samples.push_back(makeSample(1, 100, 1.0F));

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

    const auto overwrite_result =
        rm_control::modules::trajectory::writeTrajectoryCsvWithoutOverwrite(
            std::filesystem::path(files.value.csv_path), samples);
    context->Expect(!overwrite_result.ok,
                    "new-session CSV writer must refuse an existing target");

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

void testExportTcpPathCpp(TestContext *context) {
    const std::string root = std::string(RM_TEST_OUTPUT_DIR) + "/trajectory_export_test";
    auto files = rm_control::modules::trajectory::makeTrajectoryFileSet(root, "tcp demo");
    context->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, 0.0F));
    samples.push_back(makeSample(1, 100, 1.0F));

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

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

void testLegacyCsvGetsExplicitLegacySource(TestContext *context) {
    const std::filesystem::path directory =
        std::filesystem::path(RM_TEST_OUTPUT_DIR) / "legacy_trajectory_store_test";
    std::filesystem::create_directories(directory);
    const std::filesystem::path csv_path = directory / "legacy.csv";
    std::ofstream output(csv_path, std::ios::trunc);
    output << "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\n"
           << "0,0,0,0,0,0,0,0,0,0,0,0,0,0,0\n"
           << "1,100,0,0,0,0,0,0,0,0,0,0,0,0,0\n";
    output.close();

    const auto loaded =
        rm_control::modules::trajectory::loadTrajectoryCsv(csv_path.string());
    context->Expect(loaded.ok, "legacy 15-column CSV should remain readable");
    if (loaded.ok) {
        context->Expect(loaded.value[0].source == "legacy_rm75_csv",
                        "legacy CSV must not be mislabeled as a current capture");
    }
}

} // namespace

int main() {
    TestContext context;
    testStoragePathsAndCsvRoundTrip(&context);
    testExportTcpPathCpp(&context);
    testLegacyCsvGetsExplicitLegacySource(&context);

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

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