#include "rm_interface.h"

#include <cerrno>
#include <climits>
#include <cstdarg>
#include <cstdio>
#include <cstdlib>
#include <cstring>
#include <iostream>
#include <string>

namespace {

constexpr const char *kDefaultRobotIp = "192.168.1.18";
constexpr int kDefaultRobotPort = 8080;
constexpr int kDefaultTimeoutMs = 1000;

struct Config {
    std::string ip = kDefaultRobotIp;
    int port = kDefaultRobotPort;
    int timeout_ms = kDefaultTimeoutMs;
};

void printUsage(const char *program_name) {
    std::cout << "Usage: " << program_name << " [robot_ip] [port] [timeout_ms]\n"
              << "\n"
              << "Examples:\n"
              << "  " << program_name << "\n"
              << "  " << program_name << " 192.168.1.18 8080\n"
              << "  " << program_name << " 192.168.1.18 8080 1500\n"
              << "\n"
              << "This minimal demo only connects to the RealMan arm, reads version/info,\n"
              << "and disconnects. It does not send motion commands.\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 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) {
        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;
    }

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

const char *armModelName(rm_robot_arm_model_e model) {
    switch (model) {
    case RM_MODEL_RM_65_E:
        return "RM_65";
    case RM_MODEL_RM_75_E:
        return "RM_75";
    case RM_MODEL_RM_63_I_E:
        return "RML_63I";
    case RM_MODEL_RM_63_II_E:
        return "RML_63II";
    case RM_MODEL_RM_63_III_E:
        return "RML_63III";
    case RM_MODEL_ECO_65_E:
        return "ECO_65";
    case RM_MODEL_ECO_62_E:
        return "ECO_62";
    case RM_MODEL_GEN_72_E:
        return "GEN_72";
    case RM_MODEL_ECO_63_E:
        return "ECO_63";
    case RM_MODEL_UNIVERSAL_E:
        return "UNIVERSAL";
    default:
        return "UNKNOWN";
    }
}

const char *forceTypeName(rm_force_type_e type) {
    switch (type) {
    case RM_MODEL_RM_B_E:
        return "Standard";
    case RM_MODEL_RM_ZF_E:
        return "1D force";
    case RM_MODEL_RM_SF_E:
        return "6D force";
    case RM_MODEL_RM_ISF_E:
        return "Integrated 6D force";
    default:
        return "Unknown";
    }
}

void printSoftwareInfo(const rm_arm_software_version_t &info) {
    std::cout << "\n================ RealMan Arm Software Info ================\n"
              << "Product Version:          " << info.product_version << '\n'
              << "Robot Controller Version: " << info.robot_controller_version << '\n'
              << "Algorithm Version:        " << info.algorithm_info.version << '\n'
              << "Control Layer Version:    " << info.ctrl_info.version << '\n'
              << "Control Layer Build Time: " << info.ctrl_info.build_time << '\n'
              << "Dynamics Model Version:   " << info.dynamic_info.model_version << '\n'
              << "Planning Layer Version:   " << info.plan_info.version << '\n'
              << "Planning Layer Build Time: " << info.plan_info.build_time << '\n'
              << "===========================================================\n";
}

void printRobotInfo(const rm_robot_info_t &info) {
    std::cout << "\n================ RealMan Arm Basic Info ====================\n"
              << "Arm DOF:                  " << static_cast<int>(info.arm_dof) << '\n'
              << "Arm Model:                " << armModelName(info.arm_model)
              << " (" << static_cast<int>(info.arm_model) << ")\n"
              << "Force Type:               " << forceTypeName(info.force_type)
              << " (" << static_cast<int>(info.force_type) << ")\n"
              << "Controller Version:       " << static_cast<int>(info.robot_controller_version) << '\n'
              << "===========================================================\n";
}

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

} // namespace

int main(int argc, char **argv) {
    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;
    }

    int exit_code = 0;
    rm_robot_handle *handle = nullptr;

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

    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. Check IP, port, network, and robot power state.\n";
        exit_code = 2;
        if (handle != nullptr) {
            rm_delete_robot_arm(handle);
            handle = nullptr;
        }
        destroyApi();
        return exit_code;
    }

    std::cout << "Robot handle created, id: " << handle->id << '\n';

    rm_arm_software_version_t software_info = {};
    const int software_result = rm_get_arm_software_info(handle, &software_info);
    if (software_result == 0) {
        printSoftwareInfo(software_info);
    } else {
        std::cerr << "rm_get_arm_software_info failed, error code: " << software_result << '\n';
        exit_code = 3;
    }

    rm_robot_info_t robot_info = {};
    const int robot_info_result = rm_get_robot_info(handle, &robot_info);
    if (robot_info_result == 0) {
        printRobotInfo(robot_info);
    } else {
        std::cerr << "rm_get_robot_info failed, error code: " << robot_info_result << '\n';
        if (exit_code == 0) {
            exit_code = 4;
        }
    }

    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 (exit_code == 0) {
            exit_code = 5;
        }
    }

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

    if (exit_code == 0) {
        std::cout << "RealMan minimal demo finished successfully.\n";
    }

    return exit_code;
}
