#include "../remote_compact_m_k209_driver.hpp"

#include <arpa/inet.h>
#include <netdb.h>
#include <netinet/tcp.h>
#include <sys/socket.h>
#include <unistd.h>

#include <cerrno>
#include <cstring>
#include <stdexcept>
#include <string>
#include <utility>
#include <vector>

namespace radar::drivers {
namespace {

constexpr std::uint8_t kCommandConfigure = static_cast<std::uint8_t>('C');
constexpr std::uint8_t kCommandAcquire = static_cast<std::uint8_t>('A');
constexpr std::uint8_t kStatusOk = static_cast<std::uint8_t>('O');
constexpr std::uint8_t kStatusError = static_cast<std::uint8_t>('E');

[[nodiscard]] auto system_error_message(const std::string& context) -> std::string {
    return context + ": " + std::strerror(errno);
}

void append_u32_be(std::vector<std::uint8_t>& buffer, std::uint32_t value) {
    buffer.push_back(static_cast<std::uint8_t>((value >> 24U) & 0xFFU));
    buffer.push_back(static_cast<std::uint8_t>((value >> 16U) & 0xFFU));
    buffer.push_back(static_cast<std::uint8_t>((value >> 8U) & 0xFFU));
    buffer.push_back(static_cast<std::uint8_t>(value & 0xFFU));
}

void append_u64_be(std::vector<std::uint8_t>& buffer, std::uint64_t value) {
    for (int shift = 56; shift >= 0; shift -= 8) {
        buffer.push_back(static_cast<std::uint8_t>((value >> static_cast<unsigned>(shift)) & 0xFFU));
    }
}

void append_double_be(std::vector<std::uint8_t>& buffer, double value) {
    std::uint64_t bits = 0;
    static_assert(sizeof(bits) == sizeof(value));
    std::memcpy(&bits, &value, sizeof(bits));
    append_u64_be(buffer, bits);
}

[[nodiscard]] auto parse_u32_be(const std::uint8_t* data) -> std::uint32_t {
    return (static_cast<std::uint32_t>(data[0]) << 24U) |
           (static_cast<std::uint32_t>(data[1]) << 16U) |
           (static_cast<std::uint32_t>(data[2]) << 8U) |
           static_cast<std::uint32_t>(data[3]);
}

void send_all(int socket_fd, const void* data, std::size_t size) {
    const auto* cursor = static_cast<const std::uint8_t*>(data);
    auto remaining = size;
    while (remaining > 0U) {
        const auto sent = ::send(socket_fd, cursor, remaining, MSG_NOSIGNAL);
        if (sent < 0) {
            throw std::runtime_error(system_error_message("send to K209 remote server failed"));
        }
        if (sent == 0) {
            throw std::runtime_error("send to K209 remote server returned zero bytes");
        }
        cursor += sent;
        remaining -= static_cast<std::size_t>(sent);
    }
}

void recv_exact(int socket_fd, void* data, std::size_t size) {
    auto* cursor = static_cast<std::uint8_t*>(data);
    auto remaining = size;
    while (remaining > 0U) {
        const auto received = ::recv(socket_fd, cursor, remaining, 0);
        if (received < 0) {
            throw std::runtime_error(system_error_message("read from K209 remote server failed"));
        }
        if (received == 0) {
            throw std::runtime_error("K209 remote server closed the connection");
        }
        cursor += received;
        remaining -= static_cast<std::size_t>(received);
    }
}

[[nodiscard]] auto recv_u32(int socket_fd) -> std::uint32_t {
    std::uint8_t bytes[4]{};
    recv_exact(socket_fd, bytes, sizeof(bytes));
    return parse_u32_be(bytes);
}

void read_status(int socket_fd) {
    std::uint8_t status = 0;
    recv_exact(socket_fd, &status, 1);
    if (status == kStatusOk) {
        return;
    }
    if (status == kStatusError) {
        const auto message_size = recv_u32(socket_fd);
        std::string message(message_size, '\0');
        if (message_size > 0U) {
            recv_exact(socket_fd, message.data(), message.size());
        }
        throw std::runtime_error("K209 remote server error: " + message);
    }
    throw std::runtime_error("K209 remote server returned invalid status byte");
}

[[nodiscard]] auto recv_float32_array(int socket_fd, std::uint32_t expected_values, const std::string& context)
    -> std::vector<float> {
    const auto payload_size = recv_u32(socket_fd);
    const auto expected_size = expected_values * static_cast<std::uint32_t>(sizeof(float));
    if (payload_size != expected_size) {
        throw std::runtime_error(
            "K209 remote " + context + " payload has " + std::to_string(payload_size) +
            " bytes, expected " + std::to_string(expected_size)
        );
    }

    std::vector<float> values(expected_values);
    if (payload_size > 0U) {
        recv_exact(socket_fd, values.data(), payload_size);
    }
    return values;
}

[[nodiscard]] auto connect_socket(const std::string& host, std::uint32_t port, std::uint32_t timeout_ms) -> int {
    addrinfo hints{};
    hints.ai_family = AF_UNSPEC;
    hints.ai_socktype = SOCK_STREAM;
    hints.ai_protocol = IPPROTO_TCP;

    addrinfo* result = nullptr;
    const auto port_text = std::to_string(port);
    const auto rc = ::getaddrinfo(host.c_str(), port_text.c_str(), &hints, &result);
    if (rc != 0) {
        throw std::runtime_error("getaddrinfo failed for K209 remote host " + host + ": " + ::gai_strerror(rc));
    }

    int connected_fd = -1;
    std::string last_error;
    for (auto* item = result; item != nullptr; item = item->ai_next) {
        const int fd = ::socket(item->ai_family, item->ai_socktype, item->ai_protocol);
        if (fd < 0) {
            last_error = system_error_message("socket");
            continue;
        }

        const int one = 1;
        static_cast<void>(::setsockopt(fd, IPPROTO_TCP, TCP_NODELAY, &one, sizeof(one)));
        timeval timeout{};
        timeout.tv_sec = static_cast<long>(timeout_ms / 1000U);
        timeout.tv_usec = static_cast<long>((timeout_ms % 1000U) * 1000U);
        static_cast<void>(::setsockopt(fd, SOL_SOCKET, SO_RCVTIMEO, &timeout, sizeof(timeout)));
        static_cast<void>(::setsockopt(fd, SOL_SOCKET, SO_SNDTIMEO, &timeout, sizeof(timeout)));

        if (::connect(fd, item->ai_addr, item->ai_addrlen) == 0) {
            connected_fd = fd;
            break;
        }
        last_error = system_error_message("connect");
        ::close(fd);
    }
    ::freeaddrinfo(result);

    if (connected_fd < 0) {
        throw std::runtime_error("Failed to connect to K209 remote server " + host + ":" + port_text + ": " + last_error);
    }
    return connected_fd;
}

[[nodiscard]] auto complex_from_interleaved(const std::vector<float>& values, std::uint32_t points, const std::string& context)
    -> std::vector<ipc::Complex32> {
    if (values.size() != static_cast<std::size_t>(points) * 2U) {
        throw std::runtime_error("K209 remote " + context + " returned unexpected scalar count");
    }

    std::vector<ipc::Complex32> output(points);
    for (std::uint32_t index = 0; index < points; ++index) {
        output[index] = ipc::Complex32{
            .re = values[static_cast<std::size_t>(index) * 2U],
            .im = values[static_cast<std::size_t>(index) * 2U + 1U],
        };
    }
    return output;
}

}  // namespace

RemoteCompactMK209Driver::RemoteCompactMK209Driver(RemoteCompactMK209DriverSettings settings)
    : settings_(std::move(settings)) {
    if (settings_.host.empty()) {
        throw std::runtime_error("K209 remote host must not be empty");
    }
    if (settings_.port == 0U || settings_.port > 65535U) {
        throw std::runtime_error("K209 remote port must be in 1..65535");
    }
    if (settings_.timeout_ms == 0U) {
        throw std::runtime_error("K209 remote timeout_ms must be > 0");
    }
}

RemoteCompactMK209Driver::~RemoteCompactMK209Driver() {
    try {
        close();
    } catch (...) {
    }
}

void RemoteCompactMK209Driver::open() {
    if (socket_fd_ >= 0) {
        return;
    }

    socket_fd_ = connect_socket(settings_.host, settings_.port, settings_.timeout_ms);

    try {
        std::vector<std::uint8_t> payload{};
        payload.reserve(1U + 8U + 8U + 4U + 8U + 8U);
        payload.push_back(kCommandConfigure);
        append_double_be(payload, static_cast<double>(settings_.sweep.start_hz));
        append_double_be(payload, static_cast<double>(settings_.sweep.stop_hz));
        append_u32_be(payload, settings_.sweep.points);
        append_double_be(payload, static_cast<double>(settings_.sweep.if_bandwidth_hz));
        append_double_be(payload, static_cast<double>(settings_.sweep.power_dbm));
        send_all(socket_fd_, payload.data(), payload.size());

        read_status(socket_fd_);
        const auto points = recv_u32(socket_fd_);
        if (points != settings_.sweep.points) {
            throw std::runtime_error("K209 remote server returned unexpected configured point count");
        }
        frequency_hz_ = recv_float32_array(socket_fd_, points, "frequency");
    } catch (...) {
        close();
        throw;
    }
}

void RemoteCompactMK209Driver::close() {
    if (socket_fd_ >= 0) {
        ::close(socket_fd_);
        socket_fd_ = -1;
    }
    frequency_hz_.clear();
}

auto RemoteCompactMK209Driver::acquire_sweep() -> SweepTrace {
    if (socket_fd_ < 0) {
        throw std::runtime_error("K209 remote driver is not open");
    }
    const std::uint8_t command = kCommandAcquire;
    send_all(socket_fd_, &command, 1);
    read_status(socket_fd_);

    const auto points = recv_u32(socket_fd_);
    if (points != settings_.sweep.points) {
        throw std::runtime_error("K209 remote server returned unexpected sweep point count");
    }
    const auto s11_values = recv_float32_array(socket_fd_, points * 2U, "S11");
    const auto s21_values = recv_float32_array(socket_fd_, points * 2U, "S21");

    return SweepTrace{
        .frequency_hz = frequency_hz_,
        .s11 = complex_from_interleaved(s11_values, points, "S11"),
        .s21 = complex_from_interleaved(s21_values, points, "S21"),
    };
}

}  // namespace radar::drivers
