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

#include <iostream>
#include <string>

namespace {

void printUsage(const char *program_name) {
    std::cout << "Usage: " << program_name << " <input_csv> [output_cpp]\n"
              << "\n"
              << "Converts recorded CSV into a C++ TCP waypoint table. It does not connect to the robot.\n";
}

std::string fileStem(const std::string &path) {
    const size_t slash = path.find_last_of('/');
    const size_t begin = slash == std::string::npos ? 0 : slash + 1;
    const size_t dot = path.find_last_of('.');
    const size_t end = dot == std::string::npos || dot < begin ? path.size() : dot;
    return path.substr(begin, end - begin);
}

std::string defaultOutputPath(const std::string &input_csv) {
    const std::string session_name = rm_control::modules::trajectory::sanitizeSessionName(fileStem(input_csv));
    auto files = rm_control::modules::trajectory::makeTrajectoryFileSet(
        rm_control::modules::trajectory::defaultTrajectoryStorageRoot(), session_name);
    if (!files.ok) {
        return session_name + "_tcp_path.cpp";
    }
    return files.value.converted_cpp_path;
}

} // namespace

int main(int argc, char **argv) {
    if (argc < 2 || argc > 3) {
        printUsage(argv[0]);
        return 64;
    }
    const std::string first_arg = argv[1] == nullptr ? "" : argv[1];
    if (first_arg == "-h" || first_arg == "--help") {
        printUsage(argv[0]);
        return 0;
    }

    const std::string input_csv = first_arg;
    const std::string output_cpp = argc == 3 ? std::string(argv[2]) : defaultOutputPath(input_csv);
    auto samples = rm_control::modules::trajectory::loadTrajectoryCsv(input_csv);
    if (!samples.ok) {
        std::cerr << "Failed to load CSV: " << samples.error_message << '\n';
        return 1;
    }

    const std::string session_name = rm_control::modules::trajectory::sanitizeSessionName(fileStem(input_csv));
    auto export_result = rm_control::modules::trajectory::exportTcpPathCpp(
        output_cpp, samples.value, session_name);
    if (!export_result.ok) {
        std::cerr << "Failed to export C++: " << export_result.error_message << '\n';
        return 2;
    }

    std::cout << "Loaded samples: " << samples.value.size() << '\n'
              << "C++ TCP path: " << output_cpp << '\n';
    return 0;
}
