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

#include <algorithm>
#include <cerrno>
#include <cctype>
#include <cmath>
#include <cstdlib>
#include <cstring>
#include <ctime>
#include <fstream>
#include <iomanip>
#include <sstream>
#include <sys/stat.h>
#include <vector>

#ifndef RM_PROJECT_ROOT
#define RM_PROJECT_ROOT "."
#endif

namespace rm_control::modules::trajectory {
namespace {

constexpr int kExpectedCsvColumnCount = 15;

bool isPathSeparator(char value) {
    return value == '/';
}

std::string parentPath(const std::string &path) {
    const size_t slash = path.find_last_of('/');
    if (slash == std::string::npos) {
        return "";
    }
    if (slash == 0) {
        return "/";
    }
    return path.substr(0, slash);
}

bool directoryExists(const std::string &path) {
    struct stat status = {};
    return ::stat(path.c_str(), &status) == 0 && S_ISDIR(status.st_mode);
}

common::Result<void> ensureDirectoryRecursive(const std::string &path) {
    if (path.empty() || directoryExists(path)) {
        return common::Result<void>::success();
    }

    const std::string parent = parentPath(path);
    if (!parent.empty() && parent != path) {
        auto parent_result = ensureDirectoryRecursive(parent);
        if (!parent_result.ok) {
            return parent_result;
        }
    }

    if (::mkdir(path.c_str(), 0755) == 0 || errno == EEXIST) {
        return common::Result<void>::success();
    }

    return common::Result<void>::failure("mkdir failed for " + path + ": " + std::strerror(errno));
}

std::string joinPath(const std::string &left, const std::string &right) {
    if (left.empty()) {
        return right;
    }
    if (right.empty()) {
        return left;
    }
    if (isPathSeparator(left.back())) {
        return left + right;
    }
    return left + "/" + right;
}

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));
    }
    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;
}

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

    long long ignored_sample_index = 0;
    common::TrajectorySample sample;
    if (!parseLongLong(columns[0], &ignored_sample_index) ||
        !parseLongLong(columns[1], &sample.elapsed_ms)) {
        return common::Result<common::TrajectorySample>::failure(
            "CSV line " + std::to_string(line_number) + " has invalid sample_index or elapsed_ms");
    }

    sample.joints_deg.resize(7);
    for (int i = 0; i < 7; ++i) {
        if (!parseFloat(columns[2 + i], &sample.joints_deg[static_cast<size_t>(i)])) {
            return common::Result<common::TrajectorySample>::failure(
                "CSV line " + std::to_string(line_number) + " has invalid J" + std::to_string(i + 1));
        }
    }

    if (!parseFloat(columns[9], &sample.tcp_pose.x) ||
        !parseFloat(columns[10], &sample.tcp_pose.y) ||
        !parseFloat(columns[11], &sample.tcp_pose.z) ||
        !parseFloat(columns[12], &sample.tcp_pose.rx) ||
        !parseFloat(columns[13], &sample.tcp_pose.ry) ||
        !parseFloat(columns[14], &sample.tcp_pose.rz)) {
        return common::Result<common::TrajectorySample>::failure(
            "CSV line " + std::to_string(line_number) + " has invalid TCP pose");
    }

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

void writeCsvHeader(std::ostream &out) {
    out << "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";
}

void writeCsvSample(std::ostream &out, int sample_index, const common::TrajectorySample &sample) {
    out << 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 << '\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;
}

} // namespace

std::string timestampString() {
    const std::time_t now = std::time(nullptr);
    std::tm local_time = {};
    localtime_r(&now, &local_time);

    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_PROJECT_ROOT) + "/data/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) {
    if (samples.empty()) {
        return common::Result<void>::failure("trajectory sample list must not be empty");
    }

    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 (size_t i = 0; i < samples.size(); ++i) {
        if (samples[i].joints_deg.size() != 7) {
            return common::Result<void>::failure("trajectory sample joints_deg size must be 7");
        }
        writeCsvSample(output, static_cast<int>(i), samples[i]);
    }
    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 || 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);
        }
        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);
    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";

    output << std::fixed << std::setprecision(6);
    for (const auto &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";
    }

    output << "};\n\n"
           << "} // namespace rm_control_generated\n";
    return common::Result<void>::success();
}

} // namespace rm_control::modules::trajectory
