#include "core/backends/hl2/MetisProtocol.h"

#include <cmath>

namespace AetherSDR::hl2 {

namespace {

// Decode a 24-bit signed big-endian sample (I/Q wire format) into int32.
inline std::int32_t decode24be(const std::uint8_t* p) noexcept
{
    std::int32_t v = (std::int32_t(p[0]) << 16) | (std::int32_t(p[1]) << 8) | std::int32_t(p[2]);
    if (v & 0x00800000)                                  // sign-extend 24 -> 32
        v |= static_cast<std::int32_t>(0xFF000000u);
    return v;
}

inline bool isEp6Header(std::span<const std::uint8_t> pkt) noexcept
{
    return pkt.size() >= kUsbPacketSize && pkt[0] == 0xEF && pkt[1] == 0xFE
        && pkt[2] == 0x01 && pkt[3] == 0x06;
}

inline std::uint32_t readBe32(const std::uint8_t* p) noexcept
{
    return (std::uint32_t(p[0]) << 24) | (std::uint32_t(p[1]) << 16)
         | (std::uint32_t(p[2]) << 8) | std::uint32_t(p[3]);
}

constexpr std::uint8_t kSync = 0x7F;

}  // namespace

int sampleRateHz(SampleRate rate) noexcept
{
    switch (rate) {
    case SampleRate::R48k:  return 48000;
    case SampleRate::R96k:  return 96000;
    case SampleRate::R192k: return 192000;
    case SampleRate::R384k: return 384000;
    }
    return 48000;
}

std::uint8_t ocFilterByteForHz(double hz) noexcept
{
    const double mhz = hz / 1.0e6;
    // Ordered low to high; the first range that contains the frequency wins.
    // Boundaries sit in the gaps BETWEEN amateur bands, so every band lands
    // wholly inside one range — hl2_band_filter_test asserts that against
    // Quisk's table rather than trusting the arithmetic here.
    if (mhz < 1.6)   return kOcNone;                        // LW/MW: HPF would gut it
    if (mhz < 2.5)   return kOcLpf160;                      // 160 m — HPF out (spurs)
    if (mhz < 4.5)   return kOcHpfAmBc | kOcLpf80;          // 80 m
    if (mhz < 8.5)   return kOcHpfAmBc | kOcLpf60_40;       // 60 m, 40 m
    if (mhz < 16.5)  return kOcHpfAmBc | kOcLpf30_20;       // 30 m, 20 m
    if (mhz < 22.5)  return kOcHpfAmBc | kOcLpf17_15;       // 17 m, 15 m
    if (mhz <= 30.0) return kOcHpfAmBc | kOcLpf12_10;       // 12 m, 10 m
    return kOcNone;                                          // 6 m and up: no filter fitted
}

const char* ocFilterName(std::uint8_t oc) noexcept
{
    switch (static_cast<std::uint8_t>(oc & 0x7F)) {
    case kOcNone:                       return "none (bypass)";
    case kOcLpf160:                     return "160m LPF";
    case kOcHpfAmBc | kOcLpf80:         return "HPF + 80m LPF";
    case kOcHpfAmBc | kOcLpf60_40:      return "HPF + 60/40m LPF";
    case kOcHpfAmBc | kOcLpf30_20:      return "HPF + 30/20m LPF";
    case kOcHpfAmBc | kOcLpf17_15:      return "HPF + 17/15m LPF";
    case kOcHpfAmBc | kOcLpf12_10:      return "HPF + 12/10m LPF";
    default:                            return "custom";
    }
}

Cc ccConfig(SampleRate rate, int numRx, std::uint8_t ocFilterByte) noexcept
{
    const auto c1 = static_cast<std::uint8_t>((static_cast<std::uint8_t>(rate) & 0x03) | kConfigMercury);
    if (numRx < 1) numRx = 1;
    if (numRx > kMaxReceivers) numRx = kMaxReceivers;
    // Open collector outputs are DATA[23:17] == C2[7:1]. The one-bit shift is
    // the whole reason this cannot be a straight assignment: DATA[16] is not
    // part of the field, and writing the byte unshifted would put the 160 m
    // relay's bit there and every real selection one filter too low.
    const auto c2 = static_cast<std::uint8_t>((ocFilterByte & 0x7F) << 1);
    // Receiver count is DATA[6:3] — a FOUR-bit field (0000=1 .. 1011=12), so the
    // mask is 0x0F. It was 0x07 while only one receiver ever ran, which silently
    // capped the encodable count at 8 and would have wrapped 9..12 into 1..4.
    const auto c4 = static_cast<std::uint8_t>(kConfigDuplex | (((numRx - 1) & 0x0F) << 3));
    return {kC0Config, c1, c2, 0x00, c4};
}

Cc ccRxFreq(int rxIndex, std::uint32_t hz) noexcept
{
    // RX1 is register 0x02 and the receivers are contiguous from there: RX2..RX7
    // at 0x03..0x08. C0 is the address shifted left one, because C0 bit 0 is MOX.
    //
    // RX8..RX12 live at 0x12..0x16 and are NOT contiguous with this run — they
    // are deliberately not encoded here rather than being reached by arithmetic
    // that happens to be wrong past RX7. kMaxReceivers is 12 for the config
    // field; this encoder answers for the seven the shipping gateware can use.
    if (rxIndex < 0) rxIndex = 0;
    if (rxIndex > 6) rxIndex = 6;
    const auto c0 = static_cast<std::uint8_t>(kC0Rx1Freq + (rxIndex << 1));
    return {c0,
            static_cast<std::uint8_t>((hz >> 24) & 0xFF),
            static_cast<std::uint8_t>((hz >> 16) & 0xFF),
            static_cast<std::uint8_t>((hz >> 8) & 0xFF),
            static_cast<std::uint8_t>(hz & 0xFF)};
}

Cc ccRx1Freq(std::uint32_t hz) noexcept
{
    return ccRxFreq(0, hz);
}

Cc ccRxGain(int db) noexcept
{
    int code = db + 12;                                  // -12 dB -> 0, +48 dB -> 60
    if (code < 0) code = 0;
    if (code > 60) code = 60;
    return {kC0AdcGain, 0x00, 0x00, 0x00, static_cast<std::uint8_t>(0x40 | code)};
}

Cc ccAdcAssign() noexcept
{
    // RX1..RX7 -> ADC0, TX attenuation 0. All-zero payload is the correct value
    // for a single-ADC Phase-1 receiver; what matters is that the bank is sent.
    return {kC0AdcAssignOrTxGain, 0x00, 0x00, 0x00, 0x00};
}

Cc ccPipelineReset() noexcept
{
    // DATA[7:4] = 0x8 -> C4 = 0x80. Everything else stays zero, which is "no
    // action" for the other command nibbles in this register.
    return {kC0Sync, 0x00, 0x00, 0x00, 0x80};
}

Cc ccTxFreq(std::uint32_t hz) noexcept
{
    return {kC0TxFreq,
            static_cast<std::uint8_t>((hz >> 24) & 0xFF),
            static_cast<std::uint8_t>((hz >> 16) & 0xFF),
            static_cast<std::uint8_t>((hz >> 8) & 0xFF),
            static_cast<std::uint8_t>(hz & 0xFF)};
}

Cc ccTxDrive(int level, bool paEnable) noexcept
{
    if (level < 0) level = 0;
    if (level > kTxDriveMax) level = kTxDriveMax;
    // C1 = DATA[31:24] drive level. C2 = DATA[23:16]; bit 3 of it is DATA[19],
    // the onboard PA enable. ATU tune, Alex filters and VNA stay zero — those
    // are separate decisions and none of them belong in a drive-level write.
    const auto c2 = static_cast<std::uint8_t>(paEnable ? 0x08 : 0x00);
    return {kC0TxDrive, static_cast<std::uint8_t>(level), c2, 0x00, 0x00};
}

void ep2WriteTxIq(std::array<std::uint8_t, kUsbPacketSize>& pkt,
                  std::span<const std::complex<float>> iq) noexcept
{
    const std::size_t frameStarts[2] = {8, 8 + kFrameSize};
    std::size_t consumed = 0;
    for (const std::size_t fs : frameStarts) {
        std::uint8_t* payload = pkt.data() + fs + 8;     // after SYNC(3) + C&C(5)
        for (std::size_t k = 0; k + kTxSampleBytes <= kFramePayload; k += kTxSampleBytes) {
            // payload[k+0..3] is the Hermes headphone-audio slot. On the first
            // sample of each frame it is EADDR (extended address, base 0x3f),
            // NOT audio. We never write it, so it stays zero from ep2Packet's
            // zero fill -- which is exactly what "not using the extended
            // address space" must look like on the wire.
            std::int16_t i = 0;
            std::int16_t q = 0;
            if (consumed < iq.size()) {
                const auto clamp = [](float v) -> std::int16_t {
                    // Symmetric clamp: 32767, not 32768. Letting a full-scale
                    // sample wrap to the negative rail is a click at best.
                    if (v >  1.0f) v =  1.0f;
                    if (v < -1.0f) v = -1.0f;
                    return static_cast<std::int16_t>(v * 32767.0f);
                };
                i = clamp(iq[consumed].real());
                q = clamp(iq[consumed].imag());
                ++consumed;
            }
            const auto ui = static_cast<std::uint16_t>(i);
            const auto uq = static_cast<std::uint16_t>(q);
            payload[k + 4] = static_cast<std::uint8_t>((ui >> 8) & 0xFF);   // I high
            payload[k + 5] = static_cast<std::uint8_t>(ui & 0xFF);          // I low
            payload[k + 6] = static_cast<std::uint8_t>((uq >> 8) & 0xFF);   // Q high
            payload[k + 7] = static_cast<std::uint8_t>(uq & 0xFF);          // Q low
        }
    }
}

std::optional<Ep6Response> parseEp6Response(const std::uint8_t* frame) noexcept
{
    if (frame[0] != kSync || frame[1] != kSync || frame[2] != kSync)
        return std::nullopt;
    const std::uint8_t c0 = frame[3];
    Ep6Response r;
    r.ack = (c0 & 0x80) != 0;
    if (r.ack) {
        r.raddr = (c0 >> 1) & 0x3F;      // full 6 bits when answering a RQST
    } else {
        r.raddr = (c0 >> 3) & 0x0F;      // classic free-running cycle
        r.dot   = (c0 & 0x04) != 0;      // CW key tip; C0[1] Dash is always 0 here
    }
    r.ptt  = (c0 & 0x01) != 0;
    r.data = readBe32(frame + 4);
    return r;
}

void Hl2Telemetry::apply(const Ep6Response& r) noexcept
{
    ptt = r.ptt;
    switch (r.raddr) {
    case 0x00:
        firmwareVersion = static_cast<int>(r.data & 0xFF);
        adcOverload     = (r.data & (1u << 24)) != 0;
        // ACTIVE LOW on the wire: the bit is SET when transmit is permitted.
        // Decoded here so nothing above this layer has to remember the inversion.
        txInhibited     = (r.data & (1u << 25)) == 0;
        // TX IQ FIFO depth. hpsdrsim writes a 15-bit count as C2[6:0]:C3[7:0],
        // i.e. DATA[22:8], and that is what this decodes because it is what we
        // can actually verify.
        //
        // THE ORACLE DISAGREES: §6 lists [14:8] as "FIFO count MSBs" and [15:14]
        // as an under/overflow code, which overlaps bit 14 and cannot both be
        // right. The gateware RTL is the authority and this has NOT been checked
        // against it. Do not build FIFO-servoed TX pacing on this field until it
        // has been — a pacing loop driven by a misread depth is exactly the kind
        // of unverified assumption that wedged a radio once already.
        txFifoCount     = static_cast<int>((r.data >> 8) & 0x7FFF);
        txFifoUnderflow = ((r.data >> 14) & 0x3) == 0x2;
        txFifoOverflow  = ((r.data >> 14) & 0x3) == 0x3;
        break;
    case 0x01:
        temperatureRaw  = static_cast<int>((r.data >> 16) & 0xFFFF);
        forwardPowerRaw = static_cast<int>(r.data & 0xFFFF);
        break;
    case 0x02:
        reversePowerRaw = static_cast<int>((r.data >> 16) & 0xFFFF);
        biasCurrentRaw  = static_cast<int>(r.data & 0xFFFF);
        break;
    default:
        break;                            // 0x03/0x04 carry nothing we consume
    }
}

std::optional<double> swrFromRaw(int forwardRaw, int reverseRaw) noexcept
{
    // No carrier, no SWR. Returning 1.0 here would render as a perfect match
    // when the truth is that the question is meaningless.
    if (forwardRaw <= 0)
        return std::nullopt;
    // The counts are VOLTAGE-proportional, so rho is a plain ratio and there is
    // no square root. Establishing that mattered: the power form would have
    // reported roughly the square root of the true reflection coefficient, i.e.
    // a flattering SWR that hides a real mismatch.
    //
    // Evidence: hpsdrsim derives its reading as j proportional to
    // sqrt(txlevel), and txlevel is a sum of i^2+q^2 — a power — so the reported
    // count is proportional to voltage. pihpsdr's own meter.c is inconsistent
    // (one branch uses the voltage form (Vf+Vr)/(Vf-Vr), another a sqrt form
    // whose arguments are the wrong way round and would return a NEGATIVE SWR),
    // so it is not usable as the tie-breaker.
    const double fwd = static_cast<double>(forwardRaw);
    double rev = static_cast<double>(reverseRaw < 0 ? 0 : reverseRaw);
    // Reverse above forward is physically impossible; it means noise on a tiny
    // reading. Clamp rather than emit a negative or infinite SWR.
    if (rev >= fwd)
        rev = fwd * 0.999;
    return (fwd + rev) / (fwd - rev);
}

std::array<std::uint8_t, 64> metisCommand(std::uint8_t cmd) noexcept
{
    std::array<std::uint8_t, 64> out{};                  // zero-filled pad
    out[0] = 0xEF; out[1] = 0xFE; out[2] = 0x04; out[3] = cmd;
    return out;
}

std::array<std::uint8_t, 63> discoveryRequest() noexcept
{
    std::array<std::uint8_t, 63> out{};
    out[0] = 0xEF; out[1] = 0xFE; out[2] = 0x02;
    return out;
}

std::optional<DiscoveryReply> parseDiscoveryReply(std::span<const std::uint8_t> pkt) noexcept
{
    if (pkt.size() < 11 || pkt[0] != 0xEF || pkt[1] != 0xFE)
        return std::nullopt;
    DiscoveryReply r;
    r.streaming = (pkt[2] == 0x03);                      // 0x02 idle, 0x03 already sending
    for (std::size_t i = 0; i < 6; ++i)
        r.mac[i] = pkt[3 + i];
    r.gatewareVersion = pkt[9];
    r.boardId = pkt[10];
    // OFFSET 19 (0x13), not 20. This was off by one, and the byte it was
    // actually reading is a real field with plausible values — so the error
    // could not show up as an obviously wrong answer.
    //
    // Settled against all three tiers of the source-precedence ladder, which
    // agree:
    //
    //   gateware  usopenhpsdr1.v emits the discovery reply from a DOWN-counting
    //             state, so the packet offset is 0x3B - state. Anchor it on two
    //             knowns — `6'h32: VERSION_MAJOR` is offset 9 and `6'h31:
    //             idhermeslite ? 8'h06 : 8'h01` is offset 10, both fixed by the
    //             map below — and `6'h28: ... NR` lands at 0x3B-0x28 = 0x13.
    //   wiki      discovery map, offset 0x13 = "Number of hardware receivers".
    //   hpsdrsim  writes `buffer[19] = 4` for a Hermes-Lite 2.
    //
    // Offset 20 (0x14) is `{BANDSCOPE_BITS, BOARD[5:0]}` — the wideband format
    // in [7:6] and the board build id in [5:0]. On a build-5 board with the
    // wideband bits set that reads as a receiver count in the dozens, which the
    // caller then clamps to the register maximum. So the old code did not fail
    // loudly on real hardware; it quietly authorised more receivers than the
    // board has, and the extra ones stream correctly framed, correctly paced,
    // all-ZERO IQ — indistinguishable from a dead antenna.
    //
    // Short replies omit it; leave 0 so callers apply their own default.
    if (pkt.size() > 19)
        r.numRx = pkt[19];
    return r;
}

std::array<std::uint8_t, kUsbPacketSize> ep2Packet(std::uint32_t seq, const Cc& a, const Cc& b) noexcept
{
    std::array<std::uint8_t, kUsbPacketSize> pkt{};      // zero-filled (TX payload is all zero)
    pkt[0] = 0xEF; pkt[1] = 0xFE; pkt[2] = 0x01; pkt[3] = 0x02;
    pkt[4] = static_cast<std::uint8_t>((seq >> 24) & 0xFF);
    pkt[5] = static_cast<std::uint8_t>((seq >> 16) & 0xFF);
    pkt[6] = static_cast<std::uint8_t>((seq >> 8) & 0xFF);
    pkt[7] = static_cast<std::uint8_t>(seq & 0xFF);
    // Two 512-byte frames: SYNC(3) + C&C(5) + 504 zero bytes.
    const std::size_t frameStarts[2] = {8, 8 + kFrameSize};
    const Cc* ccs[2] = {&a, &b};
    for (int f = 0; f < 2; ++f) {
        std::uint8_t* fr = pkt.data() + frameStarts[f];
        fr[0] = kSync; fr[1] = kSync; fr[2] = kSync;
        for (std::size_t i = 0; i < 5; ++i)
            fr[3 + i] = (*ccs[f])[i];
    }
    return pkt;
}

std::optional<std::uint32_t> ep6Seq(std::span<const std::uint8_t> pkt) noexcept
{
    if (!isEp6Header(pkt))
        return std::nullopt;
    return readBe32(pkt.data() + 4);
}

namespace {

// Shared round walker for both ep6Samples() and ep6SamplesMulti().
//
// Returns rounds appended (== samples per receiver), or -1 on a bad header.
// `sink(rx, i, q)` is called for each receiver within each round.
template <typename Sink>
int ep6DecodeRounds(std::span<const std::uint8_t> pkt, int numRx, Sink&& sink) noexcept
{
    if (!isEp6Header(pkt))
        return -1;
    constexpr float kInvFullScale = 1.0f / static_cast<float>(kFullScale);
    const std::size_t roundBytes = ep6RoundBytes(numRx);
    int rounds = 0;
    const std::size_t frameStarts[2] = {8, 8 + kFrameSize};
    for (const std::size_t fs : frameStarts) {
        const std::uint8_t* frame = pkt.data() + fs;
        if (frame[0] != kSync || frame[1] != kSync || frame[2] != kSync)
            continue;                                    // skip a corrupt frame, keep the good one
        const std::uint8_t* payload = frame + 8;         // after SYNC(3) + C&C(5)
        // `k + roundBytes <= kFramePayload` is the host-side mirror of the
        // gateware's own "is there room for another round?" test, so the loop
        // stops exactly where the hardware switched to zero padding. Reading the
        // pad as samples would inject a burst of digital silence per packet.
        for (std::size_t k = 0; k + roundBytes <= kFramePayload; k += roundBytes) {
            for (int rx = 0; rx < numRx; ++rx) {
                const std::uint8_t* s = payload + k + static_cast<std::size_t>(rx) * kRxIqBytes;
                sink(rx,
                     static_cast<float>(decode24be(s)) * kInvFullScale,
                     static_cast<float>(decode24be(s + 3)) * kInvFullScale);
            }
            ++rounds;   // the round's trailing 2 mic bytes are ignored
        }
    }
    return rounds;
}

}  // namespace

int ep6Samples(std::span<const std::uint8_t> pkt, std::vector<std::complex<float>>& out) noexcept
{
    return ep6DecodeRounds(pkt, 1, [&out](int, float i, float q) { out.emplace_back(i, q); });
}

int ep6SamplesMulti(std::span<const std::uint8_t> pkt,
                    std::span<std::vector<std::complex<float>>> out) noexcept
{
    const int numRx = static_cast<int>(out.size());
    if (numRx < 1 || numRx > kMaxReceivers)
        return -1;
    return ep6DecodeRounds(pkt, numRx, [&out](int rx, float i, float q) {
        out[static_cast<std::size_t>(rx)].emplace_back(i, q);
    });
}

}  // namespace AetherSDR::hl2
