#include "rm_interface.h"

#include <cerrno>
#include <climits>
#include <cmath>
#include <cstdarg>
#include <cstdio>
#include <cstdlib>
#include <iomanip>
#include <iostream>
#include <string>
#include <thread>

namespace {

constexpr const char *kDefaultRobotIp = "192.168.1.18";
constexpr int kDefaultRobotPort = 8080;
constexpr int kDefaultTimeoutMs = 1000;
constexpr int kDefaultJoint = 2;
constexpr float kDefaultDeltaDeg = 5.0F;
constexpr int kDefaultSpeedPercent = 10;
constexpr int kSafetyCountdownSeconds = 5;

struct Config {
    std::string ip = kDefaultRobotIp;
    int port = kDefaultRobotPort;
    int timeout_ms = kDefaultTimeoutMs;
    int joint = kDefaultJoint;
    float delta_deg = kDefaultDeltaDeg;
    int speed_percent = kDefaultSpeedPercent;
};

void printUsage(const char *program_name) {
    std::cout << "Usage: " << program_name
              << " [robot_ip] [port] [timeout_ms] [joint] [delta_deg] [speed_percent]\n"
              << "\n"
              << "Defaults:\n"
              << "  robot_ip      " << kDefaultRobotIp << "\n"
              << "  port          " << kDefaultRobotPort << "\n"
              << "  timeout_ms    " << kDefaultTimeoutMs << "\n"
              << "  joint         " << kDefaultJoint << " (1-based joint index)\n"
              << "  delta_deg     " << kDefaultDeltaDeg << "\n"
              << "  speed_percent " << kDefaultSpeedPercent << "\n"
              << "\n"
              << "Motion plan:\n"
              << "  read current joints -> +delta -> return -> -delta -> return\n"
              << "  no Cartesian motion, no blending, blocking movej only.\n"
              << "\n"
              << "Examples:\n"
              << "  " << program_name << "\n"
              << "  " << program_name << " 192.168.1.18 8080 1000 2 5 10\n"
              << "  " << program_name << " 192.168.1.18 8080 1000 2 3 5\n";
}

bool parseInt(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' || parsed < INT_MIN || parsed > INT_MAX) {
        return false;
    }

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

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

    errno = 0;
    char *end = nullptr;
    const float parsed = std::strtof(value, &end);
    if (errno != 0 || end == value || *end != '\0' || !std::isfinite(parsed)) {
        return false;
    }

    *out = parsed;
    return true;
}

bool parseArgs(int argc, char **argv, Config *config) {
    if (config == nullptr) {
        return false;
    }

    if (argc > 1) {
        const std::string first_arg = argv[1] == nullptr ? "" : argv[1];
        if (first_arg == "-h" || first_arg == "--help") {
            printUsage(argv[0]);
            std::exit(0);
        }
        config->ip = first_arg;
    }

    if (argc > 2 && !parseInt(argv[2], &config->port)) {
        std::cerr << "Invalid port: " << argv[2] << "\n";
        return false;
    }
    if (argc > 3 && !parseInt(argv[3], &config->timeout_ms)) {
        std::cerr << "Invalid timeout_ms: " << argv[3] << "\n";
        return false;
    }
    if (argc > 4 && !parseInt(argv[4], &config->joint)) {
        std::cerr << "Invalid joint: " << argv[4] << "\n";
        return false;
    }
    if (argc > 5 && !parseFloat(argv[5], &config->delta_deg)) {
        std::cerr << "Invalid delta_deg: " << argv[5] << "\n";
        return false;
    }
    if (argc > 6 && !parseInt(argv[6], &config->speed_percent)) {
        std::cerr << "Invalid speed_percent: " << argv[6] << "\n";
        return false;
    }
    if (argc > 7) {
        std::cerr << "Too many arguments.\n";
        return false;
    }

    if (config->ip.empty()) {
        std::cerr << "Robot IP must not be empty.\n";
        return false;
    }
    if (config->port <= 0 || config->port > 65535) {
        std::cerr << "Port must be in range 1..65535.\n";
        return false;
    }
    if (config->timeout_ms <= 0) {
        std::cerr << "timeout_ms must be greater than 0.\n";
        return false;
    }
    if (config->joint < 1 || config->joint > ARM_DOF) {
        std::cerr << "Joint must be in range 1.." << ARM_DOF << ".\n";
        return false;
    }
    if (config->delta_deg <= 0.0F || config->delta_deg > 5.0F) {
        std::cerr << "delta_deg must be in range (0, 5]. Keep this demo slow and small.\n";
        return false;
    }
    if (config->speed_percent < 1 || config->speed_percent > 20) {
        std::cerr << "speed_percent must be in range 1..20 for this slow-motion demo.\n";
        return false;
    }

    return true;
}

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

void printJoints(const char *label, const float joints[ARM_DOF], int dof) {
    std::cout << label;
    for (int i = 0; i < dof; ++i) {
        std::cout << (i == 0 ? " [" : ", ")
                  << "J" << (i + 1) << "=" << std::fixed << std::setprecision(3) << joints[i];
    }
    std::cout << "] deg\n";
}

void copyJoints(const float source[ARM_DOF], float target[ARM_DOF]) {
    for (int i = 0; i < ARM_DOF; ++i) {
        target[i] = source[i];
    }
}

bool readJoints(rm_robot_handle *handle, float joints[ARM_DOF]) {
    if (handle == nullptr || joints == nullptr) {
        return false;
    }

    const int result = rm_get_joint_degree(handle, joints);
    if (result != 0) {
        std::cerr << "rm_get_joint_degree failed, error code: " << result << '\n';
        return false;
    }
    return true;
}

bool moveJointTarget(rm_robot_handle *handle,
                     const char *label,
                     const float target[ARM_DOF],
                     int dof,
                     int speed_percent) {
    printJoints(label, target, dof);
    const int result = rm_movej(handle, target, speed_percent, 0, 0, 1);
    if (result != 0) {
        std::cerr << "rm_movej failed at step \"" << label << "\", error code: " << result << '\n';
        return false;
    }

    float actual[ARM_DOF] = {};
    if (!readJoints(handle, actual)) {
        return false;
    }
    printJoints("Actual joints after step:", actual, dof);
    return true;
}

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

void cleanup(rm_robot_handle *handle) {
    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';
        }
    }

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

} // namespace

int main(int argc, char **argv) {
    std::cout.setf(std::ios::unitbuf);
    std::cerr.setf(std::ios::unitbuf);

    Config config;
    if (!parseArgs(argc, argv, &config)) {
        printUsage(argv[0]);
        return 64;
    }

    rm_set_log_call_back(apiLogCallback, 2);
    rm_set_timeout(config.timeout_ms);

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

    std::cout << "RealMan API version: " << rm_api_version() << '\n'
              << "Connecting to " << config.ip << ':' << config.port << "...\n";

    rm_robot_handle *handle = rm_create_robot_arm(config.ip.c_str(), config.port);
    if (handle == nullptr || handle->id < 0) {
        std::cerr << "Failed to connect to RealMan arm.\n";
        cleanup(handle);
        return 2;
    }

    rm_robot_info_t robot_info = {};
    int result = rm_get_robot_info(handle, &robot_info);
    if (result != 0) {
        std::cerr << "rm_get_robot_info failed, error code: " << result << '\n';
        cleanup(handle);
        return 3;
    }

    const int dof = static_cast<int>(robot_info.arm_dof);
    if (dof <= 0 || dof > ARM_DOF) {
        std::cerr << "Invalid arm DOF reported by controller: " << dof << '\n';
        cleanup(handle);
        return 4;
    }
    if (config.joint > dof) {
        std::cerr << "Selected joint J" << config.joint << " exceeds current arm DOF " << dof << ".\n";
        cleanup(handle);
        return 5;
    }

    float start[ARM_DOF] = {};
    if (!readJoints(handle, start)) {
        cleanup(handle);
        return 6;
    }

    std::cout << "Connected. Arm DOF: " << dof
              << ", selected joint: J" << config.joint
              << ", delta: " << config.delta_deg
              << " deg, speed: " << config.speed_percent << "%\n";
    printJoints("Current joints:", start, dof);

    std::cout << "\nSafety countdown before motion. Clear the workspace and keep E-stop reachable.\n";
    for (int second = kSafetyCountdownSeconds; second > 0; --second) {
        std::cout << "Moving in " << second << "...\n";
        std::this_thread::sleep_for(std::chrono::seconds(1));
    }

    float positive[ARM_DOF] = {};
    float negative[ARM_DOF] = {};
    copyJoints(start, positive);
    copyJoints(start, negative);

    const int joint_index = config.joint - 1;
    positive[joint_index] += config.delta_deg;
    negative[joint_index] -= config.delta_deg;

    bool ok = true;
    ok = ok && moveJointTarget(handle, "Step 1 target (+delta):", positive, dof, config.speed_percent);
    ok = ok && moveJointTarget(handle, "Step 2 return home:", start, dof, config.speed_percent);
    ok = ok && moveJointTarget(handle, "Step 3 target (-delta):", negative, dof, config.speed_percent);
    ok = ok && moveJointTarget(handle, "Step 4 return home:", start, dof, config.speed_percent);

    if (!ok) {
        const int stop_result = rm_set_arm_stop(handle);
        if (stop_result != 0) {
            std::cerr << "rm_set_arm_stop failed, error code: " << stop_result << '\n';
        }
        cleanup(handle);
        return 7;
    }

    float final_joints[ARM_DOF] = {};
    if (readJoints(handle, final_joints)) {
        printJoints("Final joints:", final_joints, dof);
    }

    cleanup(handle);
    std::cout << "Slow motion demo finished successfully.\n";
    return 0;
}
