#if defined(USE_SX1262)

#define RADIOLIB_STATIC_ONLY 1
#include "PollingSX1262Wrapper.h"

#include <Arduino.h>

#define NUM_NOISE_FLOOR_SAMPLES  64
#define SAMPLING_THRESHOLD  14

extern "C" void MeshCoreWatchdogStage(uint8_t stage) __attribute__((weak));
extern "C" void MeshCoreWatchdogStage(uint8_t stage) {
  (void)stage;
}

namespace {
const uint16_t kRxIrqMask =
    RADIOLIB_SX126X_IRQ_RX_DONE |
    RADIOLIB_SX126X_IRQ_CRC_ERR |
    RADIOLIB_SX126X_IRQ_HEADER_ERR |
    RADIOLIB_SX126X_IRQ_TIMEOUT;

const uint16_t kTxIrqMask =
    RADIOLIB_SX126X_IRQ_TX_DONE |
    RADIOLIB_SX126X_IRQ_TIMEOUT;

const unsigned long kIdleIrqPollMillis = 50;
}

PollingSX1262Wrapper::PollingSX1262Wrapper(CustomSX1262& radio, mesh::MainBoard& board)
  : CustomSX1262Wrapper(radio, board),
    _state(STATE_IDLE),
    _pending_irq(0),
    _last_irq_poll(0) {
}

CustomSX1262* PollingSX1262Wrapper::sx1262() const {
  return (CustomSX1262*)_radio;
}

void PollingSX1262Wrapper::begin() {
  _state = STATE_IDLE;
  _pending_irq = 0;
  _last_irq_poll = 0;

  _noise_floor = 0;
  _threshold = 0;
  _num_floor_samples = 0;
  _floor_sample_sum = 0;

  sx1262()->clearIrqFlags(RADIOLIB_SX126X_IRQ_ALL);
  startReceiveIfNeeded();
}

void PollingSX1262Wrapper::pollIrq(bool force) {
  MeshCoreWatchdogStage('q');

  if (!force && _state == STATE_RX) {
#if defined(P_LORA_DIO_1)
    if (!digitalRead(P_LORA_DIO_1)) {
      unsigned long now = millis();
      if ((unsigned long)(now - _last_irq_poll) < kIdleIrqPollMillis) {
        return;
      }
      _last_irq_poll = now;
    }
#endif
  }

  uint16_t irq = (uint16_t)sx1262()->getIrqFlags();
  irq &= (_state == STATE_TX_WAIT) ? kTxIrqMask : kRxIrqMask;
  if (irq) {
    _pending_irq |= irq;
  }
}

void PollingSX1262Wrapper::startReceiveIfNeeded() {
  if (_state == STATE_RX || _state == STATE_TX_WAIT) {
    return;
  }

  MeshCoreWatchdogStage('X');
  int err = _radio->startReceive();
  if (err == RADIOLIB_ERR_NONE) {
    _state = STATE_RX;
  } else {
    MESH_DEBUG_PRINTLN("PollingSX1262Wrapper: error: startReceive(%d)", err);
    _state = STATE_IDLE;
  }
}

void PollingSX1262Wrapper::loop() {
  MeshCoreWatchdogStage('R');
  pollIrq(false);

  if (_state == STATE_RX && _pending_irq == 0 && _num_floor_samples < NUM_NOISE_FLOOR_SAMPLES) {
    if (!isReceivingPacket()) {
      int rssi = getCurrentRSSI();
      if (rssi < _noise_floor + SAMPLING_THRESHOLD) {
        _num_floor_samples++;
        _floor_sample_sum += rssi;
      }
    }
  } else if (_num_floor_samples >= NUM_NOISE_FLOOR_SAMPLES && _floor_sample_sum != 0) {
    _noise_floor = _floor_sample_sum / NUM_NOISE_FLOOR_SAMPLES;
    if (_noise_floor < -120) {
      _noise_floor = -120;
    }
    _floor_sample_sum = 0;

    MESH_DEBUG_PRINTLN("PollingSX1262Wrapper: noise_floor = %d", (int)_noise_floor);
  }
}

int PollingSX1262Wrapper::recvRaw(uint8_t* bytes, int sz) {
  int len = 0;

  if (_state == STATE_TX_WAIT) {
    return 0;
  }

  pollIrq(false);

  if (_pending_irq & RADIOLIB_SX126X_IRQ_RX_DONE) {
    MeshCoreWatchdogStage('L');
    len = (int)_radio->getPacketLength();
    if (len > 0) {
      if (len > sz) {
        len = sz;
      }
      MeshCoreWatchdogStage('r');
      int err = _radio->readData(bytes, len);
      if (err != RADIOLIB_ERR_NONE) {
        MESH_DEBUG_PRINTLN("PollingSX1262Wrapper: error: readData(%d)", err);
        len = 0;
        n_recv_errors++;
      } else {
        n_recv++;
      }
    }
    _pending_irq = 0;
    _state = STATE_IDLE;
  } else if (_pending_irq & kRxIrqMask) {
    sx1262()->clearIrqFlags(RADIOLIB_SX126X_IRQ_ALL);
    _pending_irq = 0;
    _state = STATE_IDLE;
    n_recv_errors++;
  }

  startReceiveIfNeeded();
  return len;
}

bool PollingSX1262Wrapper::startSendRaw(const uint8_t* bytes, int len) {
  MeshCoreWatchdogStage('t');
  _board->onBeforeTransmit();

  _pending_irq = 0;
  sx1262()->standby();
  sx1262()->clearIrqFlags(RADIOLIB_SX126X_IRQ_ALL);

  int err = _radio->startTransmit((uint8_t*)bytes, len);
  if (err == RADIOLIB_ERR_NONE) {
    _state = STATE_TX_WAIT;
    return true;
  }

  MESH_DEBUG_PRINTLN("PollingSX1262Wrapper: error: startTransmit(%d)", err);
  sx1262()->standby();
  _state = STATE_IDLE;
  _board->onAfterTransmit();
  return false;
}

bool PollingSX1262Wrapper::isSendComplete() {
  if (_state != STATE_TX_WAIT) {
    return false;
  }

  pollIrq(true);
  if (_pending_irq & RADIOLIB_SX126X_IRQ_TX_DONE) {
    _state = STATE_TX_DONE;
    n_sent++;
    return true;
  }

  return false;
}

void PollingSX1262Wrapper::onSendFinished() {
  MeshCoreWatchdogStage('f');
  sx1262()->finishTransmit();
  _pending_irq = 0;
  _state = STATE_IDLE;
  _board->onAfterTransmit();
}

bool PollingSX1262Wrapper::isInRecvMode() const {
  return _state == STATE_RX;
}

void PollingSX1262Wrapper::resetAGC() {
  if (_pending_irq != 0 || _state != STATE_RX || isReceivingPacket()) {
    return;
  }

  doResetAGC();
  _state = STATE_IDLE;
  _noise_floor = 0;
  _num_floor_samples = 0;
  _floor_sample_sum = 0;
}

#endif