#pragma once

#include "rm_interface.h"

#include <array>
#include <algorithm>
#include <cerrno>
#include <chrono>
#include <cmath>
#include <cstdarg>
#include <cstdio>
#include <cstdlib>
#include <cstring>
#include <ctime>
#include <fstream>
#include <iomanip>
#include <iostream>
#include <sstream>
#include <string>
#include <sys/stat.h>
#include <utility>
#include <vector>

#ifndef LINKERHAND_PROJECT_ROOT
#define LINKERHAND_PROJECT_ROOT "."
#endif

namespace realman_trajectory {

constexpr const char *kDefaultRobotIp = "192.168.1.18";
constexpr int kDefaultRobotPort = 8080;
constexpr int kDefaultTimeoutMs = 1000;
constexpr int kExpectedRm75Dof = 7;
constexpr int kCsvColumnCount = 15;

struct RobotConnectionConfig {
    std::string robot_ip = kDefaultRobotIp;
    int robot_port = kDefaultRobotPort;
    int timeout_ms = kDefaultTimeoutMs;
};

struct TrajectoryPoint {
    long long elapsed_ms = 0;
    std::array<float, ARM_DOF> joint_deg = {};
    rm_pose_t tcp_pose = {};
};

inline void apiLogCallback(const char *message, va_list args) {
    if (message == nullptr) {
        return;
    }

    char buffer[1024] = {};
    std::vsnprintf(buffer, sizeof(buffer), message, args);
    std::cerr << "[rm_api] " << buffer << '\n';
}

inline int destroyApi() {
#if defined(REALMAN_RM_API_HAS_RM_DESTROY)
    return rm_destroy();
#else
    return rm_destory();
#endif
}

inline bool parseIntArgument(const char *value, int *out) {
    if (value == nullptr || out == nullptr || value[0] == '\0') {
        return false;
    }

    errno = 0;
    char *end = nullptr;
    const long parsed = std::strtol(value, &end, 10);
    if (errno != 0 || end == value || *end != '\0') {
        return false;
    }
    if (parsed < -2147483647L - 1L || parsed > 2147483647L) {
        return false;
    }

    *out = static_cast<int>(parsed);
    return true;
}

inline bool parseLongLongArgument(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;
}

inline bool parseFloatArgument(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;
}

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

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

inline std::string trajectoryCsvHeader() {
    return "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";
}

inline bool ensureDirectoryExists(const std::string &path) {
    struct stat status = {};
    if (::stat(path.c_str(), &status) == 0 && S_ISDIR(status.st_mode)) {
        return true;
    }
    if (::mkdir(path.c_str(), 0755) == 0) {
        return true;
    }

    std::cerr << "Failed to create directory: " << path << '\n';
    return false;
}

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

inline std::string defaultTrajectoryDirectory() {
    const std::string reports = std::string(LINKERHAND_PROJECT_ROOT) + "/reports";
    const std::string trajectories = reports + "/realman_trajectories";
    ensureDirectoryExists(reports);
    ensureDirectoryExists(trajectories);
    return trajectories;
}

inline std::string defaultRecordedTrajectoryPath() {
    return defaultTrajectoryDirectory() + "/rm75_trajectory_" + timestampString() + ".csv";
}

inline bool poseIsFinite(const rm_pose_t &pose) {
    return std::isfinite(pose.position.x) && std::isfinite(pose.position.y) &&
           std::isfinite(pose.position.z) && std::isfinite(pose.euler.rx) &&
           std::isfinite(pose.euler.ry) && std::isfinite(pose.euler.rz);
}

inline bool trajectoryPointIsFinite(const TrajectoryPoint &point) {
    for (float joint : point.joint_deg) {
        if (!std::isfinite(joint)) {
            return false;
        }
    }
    return poseIsFinite(point.tcp_pose);
}

inline void writeTrajectoryPointCsv(std::ostream *out, int sample_index, const TrajectoryPoint &point) {
    if (out == nullptr) {
        return;
    }

    *out << sample_index << ',' << point.elapsed_ms;
    for (float joint : point.joint_deg) {
        *out << ',' << std::fixed << std::setprecision(6) << joint;
    }
    *out << ',' << point.tcp_pose.position.x
         << ',' << point.tcp_pose.position.y
         << ',' << point.tcp_pose.position.z
         << ',' << point.tcp_pose.euler.rx
         << ',' << point.tcp_pose.euler.ry
         << ',' << point.tcp_pose.euler.rz << '\n';
}

inline bool parseTrajectoryPointColumns(const std::vector<std::string> &columns,
                                        int line_number,
                                        TrajectoryPoint *point) {
    if (columns.size() != kCsvColumnCount || point == nullptr) {
        std::cerr << "CSV line " << line_number << " must have " << kCsvColumnCount << " columns.\n";
        return false;
    }

    long long ignored_sample_index = 0;
    if (!parseLongLongArgument(columns[0], &ignored_sample_index) ||
        !parseLongLongArgument(columns[1], &point->elapsed_ms)) {
        std::cerr << "CSV line " << line_number << " has invalid sample_index or elapsed_ms.\n";
        return false;
    }

    for (int i = 0; i < ARM_DOF; ++i) {
        if (!parseFloatArgument(columns[2 + i], &point->joint_deg[static_cast<size_t>(i)])) {
            std::cerr << "CSV line " << line_number << " has invalid joint value J" << (i + 1) << ".\n";
            return false;
        }
    }

    return parseFloatArgument(columns[9], &point->tcp_pose.position.x) &&
           parseFloatArgument(columns[10], &point->tcp_pose.position.y) &&
           parseFloatArgument(columns[11], &point->tcp_pose.position.z) &&
           parseFloatArgument(columns[12], &point->tcp_pose.euler.rx) &&
           parseFloatArgument(columns[13], &point->tcp_pose.euler.ry) &&
           parseFloatArgument(columns[14], &point->tcp_pose.euler.rz);
}

inline bool loadTrajectoryCsv(const std::string &csv_path, std::vector<TrajectoryPoint> *points) {
    if (points == nullptr) {
        return false;
    }

    std::ifstream input(csv_path);
    if (!input) {
        std::cerr << "Failed to open trajectory CSV: " << csv_path << '\n';
        return false;
    }

    points->clear();
    std::string line;
    int line_number = 0;
    while (std::getline(input, line)) {
        ++line_number;
        if (line_number == 1 || trimCopy(line).empty()) {
            continue;
        }

        TrajectoryPoint point = {};
        if (!parseTrajectoryPointColumns(splitCsvLine(line), line_number, &point)) {
            return false;
        }
        if (!trajectoryPointIsFinite(point)) {
            std::cerr << "CSV line " << line_number << " contains non-finite values.\n";
            return false;
        }
        points->push_back(point);
    }

    if (points->empty()) {
        std::cerr << "Trajectory CSV has no trajectory points: " << csv_path << '\n';
        return false;
    }
    return true;
}

inline bool readCurrentArmState(rm_robot_handle *handle, rm_current_arm_state_t *state) {
    const int result = rm_get_current_arm_state(handle, state);
    if (result != 0) {
        std::cerr << "rm_get_current_arm_state failed, error code: " << result << '\n';
        return false;
    }
    return true;
}

inline bool readAndValidateRobotDof(rm_robot_handle *handle, int *dof) {
    rm_robot_info_t robot_info = {};
    const int result = rm_get_robot_info(handle, &robot_info);
    if (result != 0) {
        std::cerr << "rm_get_robot_info failed, error code: " << result << '\n';
        return false;
    }

    *dof = static_cast<int>(robot_info.arm_dof);
    if (*dof != kExpectedRm75Dof || *dof <= 0 || *dof > ARM_DOF) {
        std::cerr << "Expected RM75 DOF " << kExpectedRm75Dof << ", actual DOF: " << *dof << '\n';
        return false;
    }
    return true;
}

inline std::string formatJoints(const std::array<float, ARM_DOF> &joints, int dof) {
    std::ostringstream out;
    out << std::fixed << std::setprecision(3);
    for (int i = 0; i < dof; ++i) {
        out << (i == 0 ? "[" : ", ") << "J" << (i + 1) << "=" << joints[static_cast<size_t>(i)];
    }
    out << "]";
    return out.str();
}

inline float maxAbsJointDelta(const std::array<float, ARM_DOF> &first,
                              const std::array<float, ARM_DOF> &second,
                              int dof) {
    float max_delta = 0.0F;
    for (int i = 0; i < dof; ++i) {
        const float delta = std::fabs(first[static_cast<size_t>(i)] - second[static_cast<size_t>(i)]);
        max_delta = std::max(max_delta, delta);
    }
    return max_delta;
}

inline std::array<float, ARM_DOF> copyJointArray(const float source[ARM_DOF]) {
    std::array<float, ARM_DOF> joints = {};
    for (int i = 0; i < ARM_DOF; ++i) {
        joints[static_cast<size_t>(i)] = source[i];
    }
    return joints;
}

class RobotSession {
public:
    explicit RobotSession(RobotConnectionConfig config)
        : config_(std::move(config)) {}

    RobotSession(const RobotSession &) = delete;
    RobotSession &operator=(const RobotSession &) = delete;

    ~RobotSession() {
        if (handle_ != nullptr) {
            const int delete_result = rm_delete_robot_arm(handle_);
            if (delete_result != 0) {
                std::cerr << "rm_delete_robot_arm failed, error code: " << delete_result << '\n';
            }
        }
        if (initialized_) {
            const int destroy_result = destroyApi();
            if (destroy_result != 0) {
                std::cerr << "RealMan API destroy failed, error code: " << destroy_result << '\n';
            }
        }
    }

    bool connect() {
        rm_set_log_call_back(apiLogCallback, 2);
        rm_set_timeout(config_.timeout_ms);

        const int init_result = rm_init(RM_TRIPLE_MODE_E);
        initialized_ = init_result == 0;
        if (!initialized_) {
            std::cerr << "rm_init failed, error code: " << init_result << '\n';
            return false;
        }

        std::cout << "Connecting to " << config_.robot_ip << ':' << config_.robot_port << "...\n";
        handle_ = rm_create_robot_arm(config_.robot_ip.c_str(), config_.robot_port);
        if (handle_ == nullptr || handle_->id < 0) {
            std::cerr << "Failed to connect to RealMan arm.\n";
            return false;
        }
        return true;
    }

    rm_robot_handle *handle() const {
        return handle_;
    }

private:
    RobotConnectionConfig config_;
    rm_robot_handle *handle_ = nullptr;
    bool initialized_ = false;
};

} // namespace realman_trajectory
