#include "rm_interface.h"

#include <chrono>
#include <cstdarg>
#include <cstdio>
#include <cstdlib>
#include <cstring>
#include <iostream>
#include <string>
#include <thread>
#include <vector>

namespace {

constexpr const char *kRobotIp = "192.168.1.18";
constexpr int kRobotPort = 8080;
constexpr int kRobotApiTimeoutMs = 1500;
constexpr int kToolModbusPort = 1;
constexpr int kToolModbusType = 1;
constexpr int kToolModbusBaudrate = 115200;
constexpr int kToolModbusTimeoutUnits = 10;
constexpr int kRightHandDeviceId = 0x27;
constexpr int kLeftHandDeviceId = 0x28;
constexpr int kO6IdentityRegisterAddress = 30;
constexpr int kO6JointPositionRegisterAddress = 0x01;
constexpr int kO6ZeroBasedJointPositionRegisterAddress = 0x00;
constexpr int kO6JointRegisterCount = 6;
constexpr int kO6JointPackedRegisterCount = 3;
constexpr int kSwingCycleCount = 6;
constexpr int kUseFixedCycleCount = 0;
constexpr int kStepDelayMs = 650;

struct ModbusLayout {
    int device_id = kRightHandDeviceId;
    int address = kO6JointPositionRegisterAddress;
    int register_count = kO6JointRegisterCount;
};

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 destroyApi() {
    const int destroy_result = rm_destroy();
    if (destroy_result != 0) {
        std::cerr << "rm_destroy failed, error code: " << destroy_result << '\n';
    }
}

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

bool connectRobot(rm_robot_handle **handle) {
    rm_set_log_call_back(apiLogCallback, 2);
    rm_set_timeout(kRobotApiTimeoutMs);

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

    std::cout << "RealMan API version: " << rm_api_version() << '\n';
    std::cout << "Connecting to " << kRobotIp << ':' << kRobotPort << "...\n";

    *handle = rm_create_robot_arm(kRobotIp, kRobotPort);
    if (*handle == nullptr || (*handle)->id < 0) {
        std::cerr << "Failed to connect RealMan arm at " << kRobotIp << ':' << kRobotPort << '\n';
        cleanup(*handle);
        *handle = nullptr;
        return false;
    }

    std::cout << "Connected, handle id: " << (*handle)->id << '\n';
    return true;
}

bool shouldForceWriteWithoutRead(int argc, char **argv) {
    if (argc <= 1 || argv == nullptr) {
        return false;
    }

    for (int i = 1; i < argc; ++i) {
        if (argv[i] != nullptr && std::strcmp(argv[i], "--force-write-six-registers") == 0) {
            return true;
        }
    }

    return false;
}

int parseDurationSeconds(int argc, char **argv) {
    if (argc <= 1 || argv == nullptr) {
        return kUseFixedCycleCount;
    }

    for (int i = 1; i + 1 < argc; ++i) {
        if (argv[i] == nullptr || argv[i + 1] == nullptr) {
            continue;
        }
        if (std::strcmp(argv[i], "--duration-seconds") != 0) {
            continue;
        }

        const int parsed_duration = std::atoi(argv[i + 1]);
        if (parsed_duration <= 0) {
            std::cerr << "Invalid --duration-seconds value: " << argv[i + 1] << '\n';
            return kUseFixedCycleCount;
        }
        return parsed_duration;
    }

    return kUseFixedCycleCount;
}

void printToolRs485Mode(rm_robot_handle *handle, const char *label) {
    int mode = -1;
    int baudrate = -1;
    int timeout = -1;
    const int result = rm_get_tool_RS485_mode(handle, &mode, &baudrate, &timeout);
    if (result != 0) {
        std::cout << label << " rm_get_tool_RS485_mode failed, error code: " << result << '\n';
        return;
    }

    std::cout << label << " tool RS485 mode=" << mode
              << ", baudrate=" << baudrate
              << ", timeout_units=" << timeout << '\n';
}

void printToolVoltage(rm_robot_handle *handle) {
    int voltage_type = -1;
    const int result = rm_get_tool_voltage(handle, &voltage_type);
    if (result != 0) {
        std::cout << "rm_get_tool_voltage failed, error code: " << result << '\n';
        return;
    }

    std::cout << "Tool voltage type=" << voltage_type
              << " (0=0V, 1=5V, 2=12V, 3=24V)\n";
}

bool configureToolRs485AsModbusMaster(rm_robot_handle *handle) {
    printToolVoltage(handle);
    printToolRs485Mode(handle, "Before");

    const int result = rm_set_modbus_mode(
        handle,
        kToolModbusPort,
        kToolModbusBaudrate,
        kToolModbusTimeoutUnits);
    if (result != 0) {
        std::cerr << "rm_set_modbus_mode(port=1, baud=115200) failed, error code: " << result << '\n';
        return false;
    }

    std::this_thread::sleep_for(std::chrono::milliseconds(300));
    printToolRs485Mode(handle, "After");
    return true;
}

rm_peripheral_read_write_params_t makePeripheralParams(const ModbusLayout &layout) {
    rm_peripheral_read_write_params_t param = {};
    param.port = kToolModbusPort;
    param.address = layout.address;
    param.device = layout.device_id;
    param.num = layout.register_count;
    return param;
}

bool readO6JointInputRegisters(rm_robot_handle *handle, const ModbusLayout &layout) {
    rm_peripheral_read_write_params_t param = makePeripheralParams(layout);

    int data[32] = {};
    const int result = rm_read_multiple_input_registers(handle, param, data);
    if (result != 0) {
        std::cerr << "Read O6 input registers failed, device=0x" << std::hex << layout.device_id
                  << ", address=0x" << layout.address << std::dec
                  << ", register_count=" << layout.register_count
                  << ", error code: " << result << '\n';
        return false;
    }

    std::cout << "O6 input register bytes, device=0x" << std::hex << layout.device_id
              << ", address=0x" << layout.address << std::dec
              << ", register_count=" << layout.register_count << ':';
    for (int i = 0; i < layout.register_count * 2; ++i) {
        std::cout << ' ' << data[i];
    }
    std::cout << '\n';
    return true;
}

bool readO6IdentityRegisters(rm_robot_handle *handle, int device_id) {
    ModbusLayout layout = {device_id, kO6IdentityRegisterAddress, kO6JointRegisterCount};
    rm_peripheral_read_write_params_t param = makePeripheralParams(layout);

    int data[32] = {};
    const int result = rm_read_multiple_input_registers(handle, param, data);
    if (result != 0) {
        std::cerr << "Read O6 identity registers failed, device=0x" << std::hex << device_id
                  << std::dec << ", error code: " << result << '\n';
        return false;
    }

    std::cout << "O6 identity bytes, device=0x" << std::hex << device_id << std::dec << ':';
    for (int i = 0; i < kO6JointRegisterCount * 2; ++i) {
        std::cout << ' ' << data[i];
    }
    std::cout << " | ascii=";
    for (int i = 0; i < kO6JointRegisterCount * 2; ++i) {
        const int value = data[i] & 0xFF;
        std::cout << (value >= 32 && value <= 126 ? static_cast<char>(value) : '.');
    }
    std::cout << '\n';
    return true;
}

bool writeO6JointPoseAsSixRegisters(rm_robot_handle *handle,
                                    const std::vector<int> &pose,
                                    const ModbusLayout &layout) {
    if (pose.size() != kO6JointRegisterCount) {
        std::cerr << "Internal error: O6 pose must contain 6 joint values.\n";
        return false;
    }

    rm_peripheral_read_write_params_t param = makePeripheralParams(layout);
    int data[20] = {};
    for (int &value : data) {
        value = 255;
    }
    for (int i = 0; i < kO6JointRegisterCount; ++i) {
        data[i] = pose[static_cast<size_t>(i)];
    }

    const int result = rm_write_registers(handle, param, data);
    if (result != 0) {
        std::cerr << "Write O6 joint pose as 6 direct bytes failed, error code: " << result << '\n';
        return false;
    }

    std::cout << "Wrote O6 pose as 6 direct bytes:";
    for (int value : pose) {
        std::cout << ' ' << value;
    }
    std::cout << '\n';
    return true;
}

bool writeO6JointPoseAsThreePackedRegisters(rm_robot_handle *handle,
                                            const std::vector<int> &pose,
                                            const ModbusLayout &layout) {
    if (pose.size() != kO6JointRegisterCount) {
        std::cerr << "Internal error: O6 pose must contain 6 joint values.\n";
        return false;
    }

    rm_peripheral_read_write_params_t param = makePeripheralParams(layout);
    int data[20] = {};
    for (int i = 0; i < kO6JointRegisterCount; ++i) {
        data[i] = pose[static_cast<size_t>(i)];
    }

    const int result = rm_write_registers(handle, param, data);
    if (result != 0) {
        std::cerr << "Write O6 joint pose as 3 packed registers failed, error code: " << result << '\n';
        return false;
    }

    std::cout << "Wrote O6 pose as 3 packed registers:";
    for (int value : pose) {
        std::cout << ' ' << value;
    }
    std::cout << '\n';
    return true;
}

bool writeO6JointPose(rm_robot_handle *handle, const std::vector<int> &pose, const ModbusLayout &layout) {
    if (layout.register_count == kO6JointRegisterCount) {
        return writeO6JointPoseAsSixRegisters(handle, pose, layout);
    }
    if (layout.register_count == kO6JointPackedRegisterCount) {
        return writeO6JointPoseAsThreePackedRegisters(handle, pose, layout);
    }

    std::cerr << "Unsupported O6 register layout, register_count=" << layout.register_count << '\n';
    return false;
}

bool shouldContinueSwinging(const std::chrono::steady_clock::time_point &deadline,
                            int duration_seconds,
                            int completed_cycle_count) {
    if (duration_seconds > 0) {
        return std::chrono::steady_clock::now() < deadline;
    }

    return completed_cycle_count < kSwingCycleCount;
}

bool swingThumbSideToSide(rm_robot_handle *handle, const ModbusLayout &layout, int duration_seconds) {
    const std::vector<int> thumb_swung_right_pose = {255, 230, 255, 255, 255, 255};
    const std::vector<int> thumb_swung_left_pose = {255, 165, 255, 255, 255, 255};
    const std::vector<int> safe_open_pose = {255, 255, 255, 255, 255, 255};

    const auto deadline = std::chrono::steady_clock::now() + std::chrono::seconds(duration_seconds);
    int completed_cycle_count = 0;

    while (shouldContinueSwinging(deadline, duration_seconds, completed_cycle_count)) {
        if (!writeO6JointPose(handle, thumb_swung_right_pose, layout)) {
            return false;
        }
        std::this_thread::sleep_for(std::chrono::milliseconds(kStepDelayMs));

        if (!writeO6JointPose(handle, thumb_swung_left_pose, layout)) {
            return false;
        }
        std::this_thread::sleep_for(std::chrono::milliseconds(kStepDelayMs));

        ++completed_cycle_count;
    }

    std::cout << "Completed thumb swing cycles: " << completed_cycle_count << '\n';
    return writeO6JointPose(handle, safe_open_pose, layout);
}

} // namespace

int main(int argc, char **argv) {
    const bool force_write_without_read = shouldForceWriteWithoutRead(argc, argv);
    const int duration_seconds = parseDurationSeconds(argc, argv);

    rm_robot_handle *handle = nullptr;
    if (!connectRobot(&handle)) {
        return 1;
    }

    int exit_code = 0;
    ModbusLayout selected_layout = {};
    bool found_readable_layout = false;
    if (!configureToolRs485AsModbusMaster(handle)) {
        exit_code = 2;
    } else {
        const std::vector<int> identity_device_ids = {kRightHandDeviceId, kLeftHandDeviceId};
        for (const int device_id : identity_device_ids) {
            if (readO6IdentityRegisters(handle, device_id)) {
                selected_layout = {device_id, kO6JointPositionRegisterAddress, kO6JointRegisterCount};
                found_readable_layout = true;
                break;
            }
        }

        const std::vector<ModbusLayout> candidate_layouts = {
            {kRightHandDeviceId, kO6JointPositionRegisterAddress, kO6JointRegisterCount},
            {kRightHandDeviceId, kO6JointPositionRegisterAddress, kO6JointPackedRegisterCount},
            {kRightHandDeviceId, kO6ZeroBasedJointPositionRegisterAddress, kO6JointRegisterCount},
            {kRightHandDeviceId, kO6ZeroBasedJointPositionRegisterAddress, kO6JointPackedRegisterCount},
            {kLeftHandDeviceId, kO6JointPositionRegisterAddress, kO6JointRegisterCount},
            {kLeftHandDeviceId, kO6JointPositionRegisterAddress, kO6JointPackedRegisterCount},
            {kLeftHandDeviceId, kO6ZeroBasedJointPositionRegisterAddress, kO6JointRegisterCount},
            {kLeftHandDeviceId, kO6ZeroBasedJointPositionRegisterAddress, kO6JointPackedRegisterCount},
        };

        if (!found_readable_layout) {
            for (const auto &layout : candidate_layouts) {
                if (readO6JointInputRegisters(handle, layout)) {
                    selected_layout = layout;
                    found_readable_layout = true;
                    break;
                }
            }
        }
    }

    if (exit_code == 0 && !found_readable_layout && force_write_without_read) {
        std::cout << "Read failed, forcing one write attempt with the 6-register layout.\n";
        selected_layout = {kRightHandDeviceId, kO6JointPositionRegisterAddress, kO6JointRegisterCount};
        found_readable_layout = true;
    } else if (exit_code == 0 && !found_readable_layout) {
        exit_code = 3;
    }

    if (exit_code == 0 && !swingThumbSideToSide(handle, selected_layout, duration_seconds)) {
        exit_code = 4;
    }

    if (exit_code == 0) {
        std::cout << "Thumb side-swing sequence finished.\n";
    }

    cleanup(handle);
    return exit_code;
}
