#include "RadioLink.h"
#include "Config.h"
#include <RF24.h>
#include <SPI.h>

static_assert(HXP_RADIO_DATA_RATE == RF24_250KBPS && HXP_RADIO_PA_LEVEL == RF24_PA_LOW,
              "HexapodProtocol.h radio enum values out of sync with RF24.h");

static RF24 radio(RADIO_CE_PIN, RADIO_CSN_PIN);

bool RadioLink::begin() {
  present_ = radio.begin();
  if (present_) {
    radio.setPayloadSize(HXP_RADIO_PAYLOAD);   // must match the robot exactly (static payloads)
    radio.setPALevel((rf24_pa_dbm_e)HXP_RADIO_PA_LEVEL);
    radio.setDataRate((rf24_datarate_e)HXP_RADIO_DATA_RATE);
    radio.setChannel(HXP_RADIO_CHANNEL);
    radio.setRetries(RADIO_RETRY_DELAY, RADIO_RETRY_COUNT);
    radio.openWritingPipe(HXP_RADIO_ADDRESS);
    radio.stopListening();
  }
  return present_;
}

bool RadioLink::send(const CmdPacket& p) {
  if (!present_) return false;
  bool acked = radio.write(&p, sizeof(p));
  history_ = (history_ << 1) | (acked ? 1u : 0u);
  if (acked) {
    last_ack_ms_ = millis();
    ever_acked_ = true;
  }
  return acked;
}

void RadioLink::update(uint32_t now_ms) {
  link_ok_ = present_ && ever_acked_ && (now_ms - last_ack_ms_) < LINK_LOST_MS;
}

uint8_t RadioLink::qualityPct() const {
  return (uint8_t)(__builtin_popcount(history_) * 100 / 32);
}