#include "services/radio_service.h"

#include <M5Cardputer.h>
#include <RadioLib.h>

#include <algorithm>
#include <new>

#include "meshtastic_header.h"
#include "sweep_view.h"
#include "platform/console.h"
#include "platform/pins.h"
#include "platform/shared_spi.h"

namespace roro {

namespace {

// The Cap's PI4IOE5V6408: its P0 connects the antenna; at power-on it's an input and the receiver
// is deaf (M3 step 1, measured). Only touched from the main loop, which owns the I2C bus.
constexpr uint8_t kExpander = 0x43;
constexpr uint32_t kI2cFreq = 400000;
enum ExpanderReg : uint8_t { kId = 0x01, kDirection = 0x03, kOutput = 0x05, kHighZ = 0x07, kInput = 0x0F };

bool antennaOn() {
    auto& i2c = M5.In_I2C;
    if (!i2c.scanID(kExpander, kI2cFreq)) return false;
    i2c.writeRegister8(kExpander, kOutput, i2c.readRegister8(kExpander, kOutput, kI2cFreq) | 1, kI2cFreq);
    i2c.writeRegister8(kExpander, kHighZ, i2c.readRegister8(kExpander, kHighZ, kI2cFreq) & ~1, kI2cFreq);
    i2c.writeRegister8(kExpander, kDirection, i2c.readRegister8(kExpander, kDirection, kI2cFreq) | 1, kI2cFreq);
    return true;
}

// Below this the receiver hears only itself: the antenna path is open (step 1: -111.9 dBm).
constexpr float kDeafBelow = -108.0f;

}  // namespace

RadioService* RadioService::instance_ = nullptr;

RadioService::RadioService() : lock_(xSemaphoreCreateMutex()) {
    const meshtastic::Preset& p = meshtastic::kEu868Presets[0];  // LongFast (Q95)
    config_ = {meshtastic::eu868FrequencyHz(p), p.bwKHz, p.sf, p.cr, meshtastic::kSyncWord, meshtastic::kPreambleLength,
               p.name};
}

void RadioService::start() {
    instance_ = this;
    expander_ = antennaOn();
    // SPI transactions take the bus lock the SD card uses; the HAL doesn't re-begin the bus.
    hal_.reset(new ArduinoHal(sharedSpi(), SPISettings(8000000, MSBFIRST, SPI_MODE0)));
    module_.reset(new Module(hal_.get(), pins::kLoraCs, pins::kLoraIrq, pins::kLoraReset, pins::kLoraBusy));
    radio_.reset(new SX1262(module_.get()));
    // Above the main loop and the other Services' tasks, so a packet is read before the next.
    xTaskCreatePinnedToCore(taskEntry, "radio", 4096, this, 2, &task_, 1);  // peak 2.0 KB (M3 step 3)
}

void RadioService::taskEntry(void* self) { static_cast<RadioService*>(self)->run(); }

void IRAM_ATTR RadioService::onDio1() {
    BaseType_t woken = pdFALSE;
    if (instance_ && instance_->task_) xTaskNotifyFromISR(instance_->task_, kIrq, eSetBits, &woken);
    portYIELD_FROM_ISR(woken);
}

void RadioService::wake(Notify why) {
    if (task_) xTaskNotify(task_, why, eSetBits);
}

bool RadioService::beginRadio(Print* report) {
    Config c = config();
    float mhz = c.frequencyHz / 1e6f;
    int16_t state = radio_->begin(mhz, c.bandwidthKHz, c.spreadingFactor, c.codingRate, c.syncWord, 0, c.preamble,
                                  tcxo_, ldo_);
    if (state != RADIOLIB_ERR_NONE && tcxo_ > 0) {  // Meshtastic's TCXO_OPTIONAL: fall back to the crystal
        if (report) report->printf("lora probe: begin with TCXO %.1f V: error %d, trying the crystal\n", tcxo_, state);
        tcxo_ = 0;
        state = radio_->begin(mhz, c.bandwidthKHz, c.spreadingFactor, c.codingRate, c.syncWord, 0, c.preamble, tcxo_,
                              ldo_);
    }
    if (state != RADIOLIB_ERR_NONE) {
        if (report) report->printf("lora probe: error %d, no SX1262 found\n", state);
        return false;
    }
    radio_->setDio2AsRfSwitch(true);       // DIO2 selects TX or RX in the switch, as in Meshtastic
    radio_->setRxBoostedGainMode(boostedGain_);  // about 2 dB more sensitivity for about 2 mA
    return true;
}

void RadioService::run() {
    present_ = beginRadio(nullptr);
    if (present_) radio_->sleep();
    console.printf("radio: %s\n", present_ ? (tcxo_ > 0 ? "SX1262 ready (TCXO), asleep" : "SX1262 ready (crystal), asleep")
                                           : "no radio found");
    for (;;) {
        uint32_t bits = 0;
        xTaskNotifyWait(0, UINT32_MAX, &bits,
                        sweepWanted_ ? pdMS_TO_TICKS(20) : listening_ ? pdMS_TO_TICKS(kNoiseEveryMs) : portMAX_DELAY);
        if (probeWanted_.exchange(false)) runProbe();
        if (!present_) continue;
        if (sweepWanted_) {  // a Sweep has the radio: the Sniffer waits, its packets kept (Q99)
            sweepPass();
            continue;
        }
        if (sweeping_) endSweep();
        bool want = clients_ != 0;
        if (want && !listening_) startListening();
        else if (!want && listening_) stopListening();
        if (listening_ && configChanged_.exchange(false)) startListening();  // again, with the new settings
        if (listening_ && (bits & kIrq)) readPacket();
        if (listening_ && millis() - lastNoiseMs_ >= kNoiseEveryMs) sampleNoise();
    }
}

void RadioService::startListening() {
    if (!ring_) {
        ring_.reset(new (std::nothrow) RadioPacket[kRing]);
        ringFirst_ = seq_ + 1;
        if (!ring_) {
            console.println("radio: not enough memory to listen");
            clients_ = 0;
            return;
        }
    }
    // A full begin (with a reset) each time: the chip comes back from sleep with nothing assumed.
    if (!beginRadio(nullptr)) {
        ++radioErrors_;
        return;
    }
    radio_->setPacketReceivedAction(onDio1);
    int16_t state = receive();
    if (state != RADIOLIB_ERR_NONE) {
        ++radioErrors_;
        console.printf("radio: can't receive (error %d)\n", state);
        return;
    }
    if (!listening_) console.printf("radio: listening, %s\n", config().name);
    listening_ = true;
    lastNoiseMs_ = 0;
}

void RadioService::stopListening() {
    radio_->clearPacketReceivedAction();
    radio_->sleep();
    listening_ = false;
    noise_ = 0;
    xSemaphoreTake(lock_, portMAX_DELAY);
    ring_.reset();  // listening again starts a fresh list; idle costs nothing (Q100, Q103)
    xSemaphoreGive(lock_);
    console.println("radio: asleep");
}

void RadioService::readPacket() {
    RadioPacket p;
    size_t len = std::min<size_t>(radio_->getPacketLength(), sizeof p.data);
    int16_t state = radio_->readData(p.data, len);
    if (state == RADIOLIB_ERR_CRC_MISMATCH) {
        p.crcOk = false;
        ++crcErrors_;
    } else if (state != RADIOLIB_ERR_NONE) {  // a header error or a timeout: nothing to keep
        ++radioErrors_;
        if (receive() != RADIOLIB_ERR_NONE) ++radioErrors_;
        ++restarts_;
        return;
    }
    Config c = config();
    p.ms = millis();
    p.len = static_cast<uint8_t>(len);
    p.rx.frequencyHz = c.frequencyHz;
    p.rx.bandwidthKHz = c.bandwidthKHz;
    p.rx.spreadingFactor = c.spreadingFactor;
    p.rx.syncWord = c.syncWord;
    p.rx.rssi = radio_->getRSSI();
    p.rx.snr = radio_->getSNR();
    p.rx.noiseFloor = noise_;
    p.frequencyError = radio_->getFrequencyError();
    if (receive() != RADIOLIB_ERR_NONE) ++radioErrors_;
    ++restarts_;

    store(p);
}

void RadioService::store(RadioPacket& p) {
    xSemaphoreTake(lock_, portMAX_DELAY);
    p.seq = seq_ + 1;
    if (ring_) ring_[p.seq % kRing] = p;
    seq_ = p.seq;
    xSemaphoreGive(lock_);
    ++packets_;
    lastPacketMs_ = p.ms;
}

void RadioService::debugAntenna(bool on) {
    auto& i2c = M5.In_I2C;
    uint8_t out = i2c.readRegister8(kExpander, kOutput, kI2cFreq);
    i2c.writeRegister8(kExpander, kOutput, on ? (out | 1) : (out & ~1), kI2cFreq);
}

void RadioService::inject(const uint8_t* data, size_t len, float rssi, float snr) {
    if (!listening_) return (void)console.println("lora inject: not listening");
    RadioPacket p;
    Config c = config();
    p.ms = millis();
    p.len = static_cast<uint8_t>(std::min(len, sizeof p.data));
    std::copy(data, data + p.len, p.data);
    p.rx = {c.frequencyHz, c.bandwidthKHz, c.spreadingFactor, rssi, snr, noise_, c.syncWord};
    store(p);
    console.printf("lora inject: #%lu, %u B\n", (unsigned long)p.seq, p.len);
}

// Continuous receive. Only RX done raises DIO1; preambles and headers are only recorded in the
// IRQ status, which sampleNoise() reads.
int16_t RadioService::receive() {
    return radio_->startReceive(RADIOLIB_SX126X_RX_TIMEOUT_INF,
                                RADIOLIB_IRQ_RX_DEFAULT_FLAGS | (1UL << RADIOLIB_IRQ_PREAMBLE_DETECTED),
                                RADIOLIB_IRQ_RX_DEFAULT_MASK, 0);
}

void RadioService::sampleNoise() {
    lastNoiseMs_ = millis();
    noise_ = radio_->getRSSI(false);
    constexpr uint16_t kSeen =
        RADIOLIB_SX126X_IRQ_PREAMBLE_DETECTED | RADIOLIB_SX126X_IRQ_HEADER_VALID | RADIOLIB_SX126X_IRQ_HEADER_ERR;
    uint32_t flags = radio_->getIrqFlags();
    if (flags & RADIOLIB_SX126X_IRQ_PREAMBLE_DETECTED) ++preambles_;
    if (flags & RADIOLIB_SX126X_IRQ_HEADER_VALID) ++headers_;
    if (flags & RADIOLIB_SX126X_IRQ_HEADER_ERR) ++headerErrors_;
    if (flags & kSeen) radio_->clearIrqFlags(kSeen);  // never RX done: that one is the task's
}

bool RadioService::packet(uint32_t seq, RadioPacket& out) const {
    if (seq == 0 || seq > seq_ || seq_ - seq >= kRing) return false;
    xSemaphoreTake(lock_, portMAX_DELAY);
    bool ok = ring_ && ring_[seq % kRing].seq == seq;
    if (ok) out = ring_[seq % kRing];
    xSemaphoreGive(lock_);
    return ok;
}

void RadioService::sweep(bool on, uint32_t fromHz, uint32_t toHz, uint32_t stepHz) {
    if (on) {
        stepHz = std::max<uint32_t>(stepHz, 10000);
        toHz = std::max(toHz, fromHz);
        toHz = std::min<uint32_t>(toHz, fromHz + stepHz * (SweepFrame::kMaxSteps - 1));
        sweepFrom_ = fromHz, sweepTo_ = toHz, sweepStep_ = stepHz;
    }
    sweepWanted_ = on;
    wake(kRequest);
}

bool RadioService::sweepFrame(SweepFrame& out) const {
    if (!sweepSeq_) return false;
    xSemaphoreTake(lock_, portMAX_DELAY);
    out = frame_;
    xSemaphoreGive(lock_);
    return true;
}

// One pass across the band. The first one sets the radio up: the Sniffer is paused, not stopped.
void RadioService::sweepPass() {
    if (!sweeping_) {
        if (listening_) {
            radio_->clearPacketReceivedAction();
            listening_ = false;  // paused; startListening() resumes on the same ring
        }
        if (!beginRadio(nullptr)) {
            ++radioErrors_;
            sweepWanted_ = false;
            return;
        }
        radio_->setBandwidth(125);
        radio_->setSpreadingFactor(7);
        sweeping_ = true;
        console.println("radio: sweeping, the Sniffer is paused");
    }
    uint32_t from = sweepFrom_, to = sweepTo_, step = sweepStep_;
    uint16_t steps = static_cast<uint16_t>((to - from) / step + 1);
    int8_t dbm[SweepFrame::kMaxSteps];
    uint32_t t0 = millis();
    for (uint16_t i = 0; i < steps && sweepWanted_; ++i) {
        radio_->standby();
        // Image calibration is per band and slow; one calibration covers EU868, so skip it per step.
        radio_->setFrequency((from + i * step) / 1e6f, i != 0);
        radio_->startReceive();
        delayMicroseconds(1500);  // RSSI settles
        float best = -200;
        for (int k = 0; k < 3; ++k) {
            best = std::max(best, radio_->getRSSI(false));
            delayMicroseconds(300);
        }
        dbm[i] = static_cast<int8_t>(std::clamp(best, -127.0f, 0.0f));
    }
    radio_->standby();
    if (!sweepWanted_) return;
    xSemaphoreTake(lock_, portMAX_DELAY);
    frame_.seq = sweepSeq_ + 1;
    frame_.fromHz = from;
    frame_.stepHz = step;
    frame_.steps = steps;
    frame_.passMs = static_cast<uint16_t>(millis() - t0);
    std::copy(dbm, dbm + steps, frame_.dbm);
    xSemaphoreGive(lock_);
    sweepSeq_ = frame_.seq;
}

void RadioService::endSweep() {
    sweeping_ = false;
    console.println("radio: sweep stopped");
    if (clients_) startListening();  // the Sniffer again, with its settings and its packets
    else {
        radio_->sleep();
        xSemaphoreTake(lock_, portMAX_DELAY);
        ring_.reset();
        xSemaphoreGive(lock_);
    }
}

uint32_t RadioService::available() const {
    uint32_t newest = seq_, first = ringFirst_;
    if (!listening_ || newest < first) return 0;
    return std::min<uint32_t>(newest - first + 1, kRing);
}

void RadioService::listen(Client client, bool on) {
    if (on) clients_ |= client;
    else clients_ &= ~client;
    wake(kRequest);
}

RadioService::Config RadioService::config() const {
    xSemaphoreTake(lock_, portMAX_DELAY);
    Config c = config_;
    xSemaphoreGive(lock_);
    return c;
}

void RadioService::setPreset(const meshtastic::Preset& p) {
    xSemaphoreTake(lock_, portMAX_DELAY);
    config_ = {meshtastic::eu868FrequencyHz(p), p.bwKHz, p.sf, p.cr, meshtastic::kSyncWord, meshtastic::kPreambleLength,
               p.name};
    xSemaphoreGive(lock_);
    configChanged_ = true;
    wake(kRequest);
}

void RadioService::setConfig(const Config& c) {
    xSemaphoreTake(lock_, portMAX_DELAY);
    config_ = c;
    config_.name = "Custom";
    xSemaphoreGive(lock_);
    configChanged_ = true;
    wake(kRequest);
}

void RadioService::setEcho(bool echo) {
    echo_ = echo;
    echoed_ = seq_;
    listen(Console, echo);
}

void RadioService::tick(uint32_t nowMs) {
    SweepFrame f;
    if (sweepEcho_ && sweeping_ && nowMs - sweepEchoMs_ >= 2000 && sweepFrame(f)) {
        sweepEchoMs_ = nowMs;
        lora::SweepStats st = lora::summarize(f.dbm, f.steps, f.fromHz, f.stepHz);
        console.printf("sweep: #%lu floor %d, top %d dBm", (unsigned long)f.seq, st.floor, st.top);
        for (auto& p : st.peaks) console.printf(", %.1f MHz %d", p.hz / 1e6, p.dbm);
        console.printf(" (%u ms a pass)\n", f.passMs);
    }
    if (!echo_) return;
    for (uint32_t s = std::max(echoed_ + 1, seq_ > kRing ? seq_ - kRing + 1 : 1u); s <= seq_; ++s) {
        RadioPacket p;
        if (!packet(s, p)) continue;
        console.printf("lora: #%lu %u B %.1f dBm SNR %.1f dB%s", (unsigned long)p.seq, p.len, p.rx.rssi, p.rx.snr,
                       p.crcOk ? "" : " (bad CRC)");
        meshtastic::PacketHeader h;
        if (meshtastic::parseHeader(p.data, p.len, h))
            console.printf(" | %s > %s, ch 0x%02X, hops %d/%u%s", meshtastic::nodeId(h.from).c_str(),
                           meshtastic::nodeId(h.to).c_str(), h.channelHash, h.hopsAway(), h.hopStart(),
                           h.viaMqtt() ? ", via MQTT" : "");
        console.println();
    }
    echoed_ = seq_;
}

void RadioService::printSweep(Print& out) const {
    SweepFrame f;
    if (!sweepFrame(f)) return (void)out.println("sweep: none yet (lora sweep on)");
    out.printf("sweep: #%lu, %u steps of %lu kHz from %.3f MHz, %u ms\n", (unsigned long)f.seq, f.steps,
               (unsigned long)(f.stepHz / 1000), f.fromHz / 1e6, f.passMs);
    for (uint16_t i = 0; i < f.steps; i += 8) {
        out.printf("%8.3f", (f.fromHz + i * f.stepHz) / 1e6);
        for (uint16_t k = i; k < f.steps && k < i + 8; ++k) out.printf(" %4d", f.dbm[k]);
        out.println();
    }
}

void RadioService::printStatus(Print& out) const {
    Config c = config();
    Counters n = counters();
    out.printf("radio: %s, %s, antenna switch %s\n", present_ ? "SX1262" : "no radio",
               tcxo_ > 0 ? "TCXO 1.8 V" : "crystal", expander_ ? "on (expander 0x43 P0)" : "expander missing");
    out.printf("radio: %s, %s %.3f MHz, BW %.0f kHz, SF %u, CR 4/%u, sync 0x%02X, preamble %u\n",
               sweeping_ ? "sweeping" : listening_ ? "listening" : "asleep", c.name, c.frequencyHz / 1e6, c.bandwidthKHz, c.spreadingFactor,
               c.codingRate, c.syncWord, c.preamble);
    out.printf("radio: clients%s%s%s%s\n", clients_ ? "" : " none", clients_ & App ? " App" : "",
               clients_ & Capture ? " Capture" : "", clients_ & Console ? " Console" : "");
    out.printf("radio: %lu packets, %lu bad CRC, %lu radio errors, %lu receive restarts\n", (unsigned long)n.packets,
               (unsigned long)n.crcErrors, (unsigned long)n.radioErrors, (unsigned long)n.restarts);
    out.printf("radio: activity (500 ms samples): %lu preambles, %lu headers, %lu bad headers\n",
               (unsigned long)n.preambles, (unsigned long)n.headers, (unsigned long)n.headerErrors);
    if (listening_)
        out.printf("radio: noise floor %.1f dBm%s\n", noise_.load(),
                   noise_ < kDeafBelow ? " (deaf? the antenna path looks open)" : "");
    if (task_) out.printf("radio: task stack %u B free\n", (unsigned)uxTaskGetStackHighWaterMark(task_));
}

void RadioService::probe(Print& out) {
    // The I2C part here, on the main loop; the radio part on the radio task.
    bool found[120] = {};
    M5.In_I2C.scanID(found, kI2cFreq);
    out.print("lora probe: i2c (internal bus, 8/9):");
    for (int a = 8; a < 120; ++a)
        if (found[a]) out.printf(" 0x%02X", a);
    out.println();
    if (found[kExpander]) {
        auto r = [](uint8_t reg) { return M5.In_I2C.readRegister8(kExpander, reg, kI2cFreq); };
        out.printf("lora probe: expander 0x43 dir %02X out %02X highz %02X in %02X: antenna %s\n", r(kDirection),
                   r(kOutput), r(kHighZ), r(kInput), r(kOutput) & 1 && r(kDirection) & 1 ? "on" : "OFF");
    } else {
        out.println("lora probe: no expander at 0x43: the antenna switch can't be enabled");
    }
    probeOut_ = &out;
    probeWanted_ = true;
    wake(kRequest);
}

void RadioService::runProbe() {
    Print& out = *probeOut_;
    uint32_t t0 = millis();
    if (!beginRadio(&out)) return;
    char version[17] = {};
    module_->SPIreadRegisterBurst(RADIOLIB_SX126X_REG_VERSION_STRING, 16, reinterpret_cast<uint8_t*>(version));
    out.printf("lora probe: %s, %s, ready in %u ms\n", version, tcxo_ > 0 ? "TCXO 1.8 V" : "crystal (no TCXO)",
               (unsigned)(millis() - t0));
    // DIO1 to the task, with no transmitter needed: a 100 ms receive timeout, routed to DIO1.
    radio_->setPacketReceivedAction(onDio1);
    uint32_t bits = 0;
    xTaskNotifyWait(0, UINT32_MAX, &bits, 0);  // drop anything pending
    uint32_t sent = millis();
    radio_->startReceive(6400, RADIOLIB_IRQ_RX_DEFAULT_FLAGS,
                         (1UL << RADIOLIB_IRQ_RX_DONE) | (1UL << RADIOLIB_IRQ_TIMEOUT), 0);  // 6400 x 15.625 us
    bits = 0;
    xTaskNotifyWait(0, UINT32_MAX, &bits, pdMS_TO_TICKS(500));
    uint32_t flags = radio_->getIrqFlags();
    if (bits & kIrq)
        out.printf("lora probe: DIO1 interrupt works (receive timeout after %u ms)\n", (unsigned)(millis() - sent));
    else
        out.printf("lora probe: DIO1 interrupt never came (IRQ status 0x%04X)\n", (unsigned)flags);
    radio_->standby();
    radio_->clearIrqFlags(RADIOLIB_SX126X_IRQ_ALL);
    if (!listening_) radio_->clearPacketReceivedAction();
    receive();
    delay(20);
    float sum = 0, low = 0, high = -200;
    for (int i = 0; i < 64; ++i) {
        float r = radio_->getRSSI(false);
        sum += r, low = std::min(low, r), high = std::max(high, r);
        delay(3);
    }
    float mean = sum / 64;
    out.printf("lora probe: noise %.1f dBm (%.1f to %.1f) at %.3f MHz: %s\n", mean, low, high, config().frequencyHz / 1e6,
               mean < kDeafBelow ? "deaf, the antenna path looks open" : "the antenna is connected");
    // Back to what it was doing.
    if (listening_) startListening();
    else radio_->sleep();
    out.println("lora probe: done");
}

}  // namespace roro