#include "../librevna_minimal_driver.hpp"

#include <algorithm>
#include <array>
#include <cmath>
#include <exception>
#include <string>
#include <string_view>
#include <stdexcept>
#include <utility>
#include <vector>

#include "librevna_protocol_common.hpp"

namespace radar::drivers {
namespace detail = radar::drivers::librevna::detail;
namespace {

constexpr std::uint32_t kNativeAcquireMaxAttempts = 3U;
constexpr auto kNativeSweepResponseTimeout = std::chrono::milliseconds(1500);

[[nodiscard]] auto is_retryable_native_acquire_error(std::string_view message) -> bool {
    constexpr std::array<std::string_view, 5> kRetryableSubstrings = {
        "Timeout waiting for expected LibreVNA packet type",
        "Timeout waiting for LibreVNA ACK",
        "LibreVNA returned NACK",
        "Failed to read LibreVNA USB bulk packet",
        "Failed to write LibreVNA USB bulk packet",
    };

    return std::any_of(
        kRetryableSubstrings.begin(),
        kRetryableSubstrings.end(),
        [message](std::string_view needle) {
            return message.find(needle) != std::string_view::npos;
        }
    );
}

}  // namespace

LibreVnaMinimalDriver::LibreVnaMinimalDriver(LibreVnaMinimalDriverSettings settings) : settings_(std::move(settings)) {}

void LibreVnaMinimalDriver::open() {
    if (is_open_) {
        return;
    }
    if (settings_.sweep.points == 0U) {
        throw std::runtime_error("Radar sweep points must be > 0");
    }

    if (settings_.mode == config::DriverMode::Native) {
        open_native();
    }

    is_open_ = true;
}

void LibreVnaMinimalDriver::close() {
    if (!is_open_) {
        return;
    }

    if (settings_.mode == config::DriverMode::Native) {
        close_native();
    }

    is_open_ = false;
}

auto LibreVnaMinimalDriver::acquire_sweep() -> SweepTrace {
    if (!is_open_) {
        throw std::runtime_error("Radar driver is not open");
    }

    ++sweep_index_;
    switch (settings_.mode) {
        case config::DriverMode::Mock:
            return acquire_mock();
        case config::DriverMode::Native: {
            std::exception_ptr last_exception{};
            std::string last_message{};

            for (std::uint32_t attempt = 1; attempt <= kNativeAcquireMaxAttempts; ++attempt) {
                try {
                    return acquire_native();
                } catch (const std::exception& exception) {
                    last_exception = std::current_exception();
                    last_message = exception.what();

                    if (!is_retryable_native_acquire_error(last_message) || attempt == kNativeAcquireMaxAttempts) {
                        break;
                    }

                    // Recover from transient USB/protocol stalls by reconnecting the device.
                    close_native();
                    open_native();
                }
            }

            if (last_exception != nullptr) {
                std::rethrow_exception(last_exception);
            }
            throw std::runtime_error("Native acquisition failed without exception detail");
        }
        default:
            throw std::runtime_error("Unsupported radar driver mode");
    }
}

auto LibreVnaMinimalDriver::acquire_mock() -> SweepTrace {
    SweepTrace trace{};
    trace.frequency_hz.reserve(settings_.sweep.points);
    trace.s11.reserve(settings_.sweep.points);
    trace.s21.reserve(settings_.sweep.points);

    const auto span_hz = settings_.sweep.stop_hz - settings_.sweep.start_hz;
    const auto denominator = settings_.sweep.points > 1U ? static_cast<float>(settings_.sweep.points - 1U) : 1.0F;

    for (std::uint32_t point = 0; point < settings_.sweep.points; ++point) {
        const auto ratio = static_cast<float>(point) / denominator;
        const auto frequency_hz = settings_.sweep.start_hz + span_hz * ratio;
        const auto phase = 2.0F * detail::kPi * (frequency_hz / std::max(settings_.mock_signal_hz, 1.0F)) +
                           static_cast<float>(sweep_index_) * 0.05F;
        const auto envelope = 0.6F + 0.4F * std::sin(0.5F * phase);

        ipc::Complex32 sample{};
        sample.re = envelope * std::cos(phase);
        sample.im = envelope * std::sin(phase);

        const auto reflection_phase = 0.7F * phase + 0.35F;
        const auto reflection_envelope = 0.15F + 0.1F * std::cos(0.25F * phase);
        ipc::Complex32 reflection{};
        reflection.re = reflection_envelope * std::cos(reflection_phase);
        reflection.im = reflection_envelope * std::sin(reflection_phase);

        trace.frequency_hz.push_back(frequency_hz);
        trace.s11.push_back(reflection);
        trace.s21.push_back(sample);
    }

    return trace;
}

auto LibreVnaMinimalDriver::acquire_native() -> SweepTrace {
    if (usb_handle_ == nullptr) {
        throw std::runtime_error("LibreVNA native handle is not open");
    }

    // Drop stale datapoints left in queue by previous operations.
    while (pop_packet(detail::kPacketVnaDatapoint).has_value()) {
    }

    send_packet_no_payload(detail::kPacketInitiateSweep, true);

    SweepTrace trace{};
    trace.frequency_hz.assign(settings_.sweep.points, 0.0F);
    trace.s11.assign(settings_.sweep.points, ipc::Complex32{});
    trace.s21.assign(settings_.sweep.points, ipc::Complex32{});

    std::vector<std::uint8_t> received(settings_.sweep.points, 0U);
    std::uint32_t received_count = 0;

    const auto deadline = std::chrono::steady_clock::now() + kNativeSweepResponseTimeout;

    while (received_count < settings_.sweep.points) {
        NativePacket packet{};
        try {
            packet = wait_for_packet(detail::kPacketVnaDatapoint, deadline);
        } catch (const std::exception& exception) {
            throw std::runtime_error(
                "Timeout while collecting VNADatapoints (" + std::to_string(received_count) + "/" +
                std::to_string(settings_.sweep.points) + " points received): " + exception.what()
            );
        }

        DecodedVnaDatapoint datapoint{};
        if (!decode_vna_datapoint_traces(packet.payload, datapoint)) {
            throw std::runtime_error("Failed to decode traces from VNADatapoint packet");
        }
        if (datapoint.point_number >= settings_.sweep.points) {
            throw std::runtime_error("Received out-of-range VNADatapoint index");
        }

        if (received[datapoint.point_number] == 0U) {
            received[datapoint.point_number] = 1U;
            ++received_count;
        }

        trace.frequency_hz[datapoint.point_number] = datapoint.frequency_hz;
        trace.s11[datapoint.point_number] = datapoint.s11;
        trace.s21[datapoint.point_number] = datapoint.s21;
    }

    return trace;
}

void LibreVnaMinimalDriver::validate_device_info_payload(std::span<const std::uint8_t> payload) const {
    if (payload.size() != detail::kDeviceInfoPayloadSize) {
        throw std::runtime_error("Unexpected DeviceInfo payload size");
    }

    const auto protocol = detail::read_u16_le(payload, detail::kDeviceInfoProtocolOffset);
    if (protocol != detail::kProtocolV14) {
        throw std::runtime_error("Unsupported LibreVNA protocol version (expected v14)");
    }

    const auto ports = payload[detail::kDeviceInfoNumPortsOffset];
    if (ports < 2U) {
        throw std::runtime_error("LibreVNA reported invalid number of ports");
    }
}

}  // namespace radar::drivers
