#include "flight_robot/linker_hand/linker_hand_adapter.h"

#include <algorithm>
#include <string>
#include <vector>

namespace flight_robot::linker_hand {
namespace {

using rm_control::common::Result;
using rm_control::interfaces::DexterousHandReadiness;
using rm_control::interfaces::DexterousHandSide;
using rm_control::interfaces::IToolModbusBus;
using rm_control::interfaces::O6JointPose;
using rm_control::interfaces::O6MotionCommand;
using rm_control::interfaces::ToolModbusBusStatus;

constexpr std::uint8_t kRightHandDeviceId = 0x27U;
constexpr std::uint8_t kLeftHandDeviceId = 0x28U;
constexpr int kRequiredToolPort = 1;
constexpr int kRequiredModbusMode = 1;
constexpr int kRequiredBaudrate = 115200;
constexpr int kRequiredVoltageType = 3;
constexpr std::uint16_t kPositionStartAddress = 0U;
constexpr std::uint16_t kTorqueStartAddress = 6U;
constexpr std::uint16_t kSpeedStartAddress = 12U;
constexpr std::uint16_t kTemperatureStartAddress = 18U;
constexpr std::uint16_t kFaultStartAddress = 24U;
constexpr std::uint16_t kIdentityStartAddress = 30U;
constexpr std::uint16_t kMaximumJointPosition = 255U;
constexpr std::uint16_t kMaximumTemperatureCelsius = 70U;
constexpr std::uint16_t kExpectedFreedom = 6U;

std::uint8_t DeviceIdForSide(DexterousHandSide side) {
    return side == DexterousHandSide::Right ? kRightHandDeviceId
                                            : kLeftHandDeviceId;
}

char DirectionForSide(DexterousHandSide side) {
    return side == DexterousHandSide::Right ? 'R' : 'L';
}

Result<void> ValidateToolBusStatus(const ToolModbusBusStatus &status) {
    if (!status.connected) {
        return Result<void>::failure("O6 readiness rejected: tool bus is disconnected");
    }
    if (status.port != kRequiredToolPort || status.mode != kRequiredModbusMode ||
        status.baudrate != kRequiredBaudrate ||
        status.voltage_type != kRequiredVoltageType || status.timeout_units <= 0) {
        return Result<void>::failure(
            "O6 readiness rejected: expected port=1, mode=1, baudrate=115200, "
            "voltage_type=3 and timeout_units>0; actual port=" +
            std::to_string(status.port) + ", mode=" + std::to_string(status.mode) +
            ", baudrate=" + std::to_string(status.baudrate) +
            ", voltage_type=" + std::to_string(status.voltage_type) +
            ", timeout_units=" + std::to_string(status.timeout_units));
    }
    return Result<void>::success();
}

Result<std::vector<std::uint16_t>> ReadSixInputRegisters(
    IToolModbusBus &tool_bus,
    std::uint8_t device_id,
    std::uint16_t start_address,
    const std::string &group_name) {
    std::vector<std::uint16_t> registers;
    registers.reserve(rm_control::interfaces::kO6JointCount);
    for (std::size_t offset = 0U;
         offset < rm_control::interfaces::kO6JointCount; ++offset) {
        const auto address = static_cast<std::uint16_t>(start_address + offset);
        const auto result = tool_bus.ReadInputRegister(device_id, address);
        if (!result.ok) {
            return Result<std::vector<std::uint16_t>>::failure(
                "failed to read O6 " + group_name + " input register address " +
                std::to_string(address) + ": " + result.error_message);
        }
        registers.push_back(result.value);
    }
    return Result<std::vector<std::uint16_t>>::success(registers);
}

Result<void> CopyBoundedRegisterGroup(
    const std::vector<std::uint16_t> &registers,
    std::uint16_t maximum_value,
    const std::string &group_name,
    O6JointPose *destination) {
    for (std::size_t index = 0; index < registers.size(); ++index) {
        if (registers[index] > maximum_value) {
            return Result<void>::failure(
                "invalid O6 " + group_name + " at index " +
                std::to_string(index) + ": " + std::to_string(registers[index]) +
                " exceeds " + std::to_string(maximum_value));
        }
    }
    std::copy(registers.begin(), registers.end(), destination->begin());
    return Result<void>::success();
}

Result<void> ReadAndCopyBoundedInputRegisterGroup(
    IToolModbusBus &tool_bus,
    std::uint8_t device_id,
    std::uint16_t start_address,
    std::uint16_t maximum_value,
    const std::string &group_name,
    O6JointPose *destination) {
    const auto registers = ReadSixInputRegisters(
        tool_bus, device_id, start_address, group_name);
    if (!registers.ok) {
        return Result<void>::failure(registers.error_message);
    }
    return CopyBoundedRegisterGroup(
        registers.value, maximum_value, group_name, destination);
}

Result<void> CopyFaultRegisters(const std::vector<std::uint16_t> &registers,
                                O6JointPose *destination) {
    for (std::size_t index = 0; index < registers.size(); ++index) {
        if (registers[index] != 0U) {
            return Result<void>::failure(
                "O6 fault at joint index " + std::to_string(index) +
                ": code=" + std::to_string(registers[index]));
        }
    }
    std::copy(registers.begin(), registers.end(), destination->begin());
    return Result<void>::success();
}

Result<void> CopyAndValidateIdentity(
    const std::vector<std::uint16_t> &registers,
    char expected_direction,
    DexterousHandReadiness *readiness) {
    if (registers[0] != kExpectedFreedom) {
        return Result<void>::failure(
            "invalid O6 freedom: expected 6, got " +
            std::to_string(registers[0]));
    }
    if (registers[5] != static_cast<std::uint16_t>(expected_direction)) {
        return Result<void>::failure(
            "invalid O6 direction: expected " + std::string(1, expected_direction) +
            ", got register value " + std::to_string(registers[5]));
    }
    readiness->freedom = registers[0];
    readiness->hand_version = registers[1];
    std::copy_n(registers.begin() + 2, readiness->hand_number_parts.size(),
                readiness->hand_number_parts.begin());
    readiness->direction = expected_direction;
    return Result<void>::success();
}

Result<void> ReadAndValidateO6Registers(IToolModbusBus &tool_bus,
                                       char expected_direction,
                                       DexterousHandReadiness *readiness) {
    const auto identity = ReadSixInputRegisters(
        tool_bus, readiness->device_id, kIdentityStartAddress, "identity");
    if (!identity.ok) {
        return Result<void>::failure(identity.error_message);
    }
    auto validation =
        CopyAndValidateIdentity(identity.value, expected_direction, readiness);
    if (!validation.ok) {
        return validation;
    }

    validation = ReadAndCopyBoundedInputRegisterGroup(
        tool_bus, readiness->device_id, kPositionStartAddress,
        kMaximumJointPosition, "position", &readiness->joint_positions);
    if (!validation.ok) return validation;
    validation = ReadAndCopyBoundedInputRegisterGroup(
        tool_bus, readiness->device_id, kTorqueStartAddress,
        kMaximumJointPosition, "torque", &readiness->joint_torques);
    if (!validation.ok) return validation;
    validation = ReadAndCopyBoundedInputRegisterGroup(
        tool_bus, readiness->device_id, kSpeedStartAddress,
        kMaximumJointPosition, "speed", &readiness->joint_speeds);
    if (!validation.ok) return validation;
    validation = ReadAndCopyBoundedInputRegisterGroup(
        tool_bus, readiness->device_id, kTemperatureStartAddress,
        kMaximumTemperatureCelsius, "temperature",
        &readiness->temperatures_celsius);
    if (!validation.ok) return validation;

    const auto faults = ReadSixInputRegisters(
        tool_bus, readiness->device_id, kFaultStartAddress, "faults");
    if (!faults.ok) {
        return Result<void>::failure(faults.error_message);
    }
    return CopyFaultRegisters(faults.value, &readiness->fault_codes);
}

Result<void> ValidateMotionCommandGroup(const O6JointPose &values,
                                        const std::string &group_name) {
    for (std::size_t index = 0; index < values.size(); ++index) {
        if (values[index] > kMaximumJointPosition) {
            return Result<void>::failure(
                "invalid O6 " + group_name + " command at joint index " +
                std::to_string(index) + ": " + std::to_string(values[index]) +
                " exceeds 255");
        }
    }
    return Result<void>::success();
}

Result<void> WriteCommandRegisterGroup(
    IToolModbusBus &tool_bus,
    std::uint8_t device_id,
    std::uint16_t start_address,
    const O6JointPose &values,
    const std::string &group_name) {
    const std::vector<std::uint16_t> register_values(values.begin(), values.end());
    const auto write = tool_bus.WriteHoldingRegisters(
        device_id, start_address, register_values);
    if (!write.ok) {
        return Result<void>::failure(
            "failed to write O6 " + group_name + " commands: " +
            write.error_message);
    }
    return Result<void>::success();
}

} // namespace

LinkerHandAdapter::LinkerHandAdapter(IToolModbusBus &tool_bus,
                                     DexterousHandSide side)
    : tool_bus_(tool_bus), side_(side) {}

Result<DexterousHandReadiness> LinkerHandAdapter::ReadReadiness() {
    const auto status = tool_bus_.GetStatus();
    if (!status.ok) {
        return Result<DexterousHandReadiness>::failure(
            "failed to read O6 tool bus status: " + status.error_message);
    }
    const auto status_validation = ValidateToolBusStatus(status.value);
    if (!status_validation.ok) {
        return Result<DexterousHandReadiness>::failure(
            status_validation.error_message);
    }

    DexterousHandReadiness readiness;
    readiness.side = side_;
    readiness.device_id = DeviceIdForSide(side_);
    readiness.tool_bus_status = status.value;
    const auto registers = ReadAndValidateO6Registers(
        tool_bus_, DirectionForSide(side_), &readiness);
    if (!registers.ok) {
        return Result<DexterousHandReadiness>::failure(registers.error_message);
    }
    readiness.ready = true;
    return Result<DexterousHandReadiness>::success(readiness);
}

Result<void> LinkerHandAdapter::CommandMotion(const O6MotionCommand &command) {
    auto validation = ValidateMotionCommandGroup(
        command.target_positions, "position");
    if (!validation.ok) {
        return validation;
    }
    validation = ValidateMotionCommandGroup(
        command.joint_speed_commands, "speed");
    if (!validation.ok) {
        return validation;
    }
    validation = ValidateMotionCommandGroup(
        command.joint_torque_commands, "torque");
    if (!validation.ok) {
        return validation;
    }

    const std::uint8_t device_id = DeviceIdForSide(side_);
    auto configured = WriteCommandRegisterGroup(
        tool_bus_, device_id, kTorqueStartAddress,
        command.joint_torque_commands, "torque");
    if (!configured.ok) {
        return configured;
    }
    configured = WriteCommandRegisterGroup(
        tool_bus_, device_id, kSpeedStartAddress,
        command.joint_speed_commands, "speed");
    if (!configured.ok) {
        return configured;
    }

    const std::vector<std::uint16_t> positions(
        command.target_positions.begin(), command.target_positions.end());
    const auto position_write = tool_bus_.WriteHoldingRegisters(
        device_id, kPositionStartAddress, positions);
    if (!position_write.ok) {
        return Result<void>::failure("failed to write O6 joint pose: " +
                                     position_write.error_message);
    }
    return Result<void>::success();
}

} // namespace flight_robot::linker_hand
