#include <string.h>
#include "radio.h"

extern "C" {
    #include "nrf_delay.h"
}

static uint8_t radio_packet[RADIO_PACKET_TOTAL_SIZE] = { 0 }, radio_state = RADIO_OFF;
static uint16_t radio_frequency = 0;

void radio_init() {
    NRF_TIMER2->TASKS_STOP  = 1;
    NRF_TIMER2->TASKS_CLEAR = 1;
    NRF_TIMER2->MODE        = TIMER_MODE_MODE_Timer << TIMER_MODE_MODE_Pos;
    NRF_TIMER2->BITMODE     = TIMER_BITMODE_BITMODE_16Bit << TIMER_BITMODE_BITMODE_Pos;
    NRF_TIMER2->PRESCALER   = 4;

    radio_state = RADIO_IDLE;
}

void radio_configure() {
    if (radio_state == RADIO_OFF) return;

    NRF_RADIO->PREFIX0     = 0xC0C1C2C3;
    NRF_RADIO->PREFIX1     = 0xC4C5C6C7;
    NRF_RADIO->BASE0       = 0x01234567;
    NRF_RADIO->BASE1       = 0x89ABCDEF;
    NRF_RADIO->TXADDRESS   = 0;
    NRF_RADIO->RXADDRESSES = RADIO_RXADDRESSES_ADDR0_Enabled << RADIO_RXADDRESSES_ADDR0_Pos;
    NRF_RADIO->PCNF0       = (0UL << RADIO_PCNF0_S1LEN_Pos) |
                             (0UL << RADIO_PCNF0_S0LEN_Pos) |
                             (8UL << RADIO_PCNF0_LFLEN_Pos);
    NRF_RADIO->PCNF1       = (RADIO_PCNF1_WHITEEN_Disabled      << RADIO_PCNF1_WHITEEN_Pos) |
                             (RADIO_PCNF1_ENDIAN_Big            << RADIO_PCNF1_ENDIAN_Pos ) |
                             (4UL                               << RADIO_PCNF1_BALEN_Pos  ) |
                             (0UL                               << RADIO_PCNF1_STATLEN_Pos) |
                             (uint32_t(RADIO_PACKET_TOTAL_SIZE) << RADIO_PCNF1_MAXLEN_Pos );
    NRF_RADIO->CRCCNF      = RADIO_CRCCNF_LEN_Three        << RADIO_CRCCNF_LEN_Pos;
    //NRF_RADIO->SHORTS      = RADIO_SHORTS_READY_START_Msk  << RADIO_SHORTS_READY_START_Pos;
    NRF_RADIO->TXPOWER     = RADIO_TXPOWER_TXPOWER_Pos4dBm << RADIO_TXPOWER_TXPOWER_Pos;
    NRF_RADIO->MODE        = RADIO_MODE_MODE_Nrf_2Mbit     << RADIO_MODE_MODE_Pos;
    NRF_RADIO->FREQUENCY   = 40;
}

void radio_disable() {
    if (radio_state == RADIO_OFF || radio_state == RADIO_IDLE) return;

    NRF_RADIO->EVENTS_DISABLED = 0;
    NRF_RADIO->TASKS_DISABLE = 1;
    while (!NRF_RADIO->EVENTS_DISABLED) nrf_delay_us(1);
    radio_state = RADIO_IDLE;
}

bool radio_do_work(uint16_t timeout_us = 0) {
    NRF_RADIO->EVENTS_END = 0;
    NRF_RADIO->PACKETPTR = (uint32_t)radio_packet;
    NRF_RADIO->TASKS_START = 1;
    if (timeout_us) {
        NRF_TIMER2->CC[0]       = timeout_us;
        NRF_TIMER2->TASKS_START = 1;
    }

    while (!NRF_RADIO->EVENTS_END) {
        if (timeout_us && NRF_TIMER2->EVENTS_COMPARE[0]) {
            NRF_RADIO->TASKS_STOP         = 1;
            NRF_TIMER2->EVENTS_COMPARE[0] = 0;
            NRF_TIMER2->TASKS_STOP        = 1;
            NRF_TIMER2->TASKS_CLEAR       = 1;
            return false;
        }
        nrf_delay_us(1);
    }

    return true;
}

inline void radio_set_frequency() {
    NRF_RADIO->FREQUENCY = radio_frequency;
}

void radio_set_channel(uint8_t channel) {
    if (radio_state != RADIO_IDLE) return;

    radio_frequency = channel * RADIO_CHANNEL_WIDTH;
    if (radio_frequency > 80) radio_frequency = 80 - (80 % RADIO_CHANNEL_WIDTH);
    radio_set_frequency();
}

void radio_transmit() {
    if (radio_state != RADIO_TX) {
        NRF_RADIO->EVENTS_READY = 0;
        NRF_RADIO->TASKS_TXEN   = 1;
        while (!NRF_RADIO->EVENTS_READY) nrf_delay_us(1);
        radio_state = RADIO_TX;
    }
    radio_do_work();
}

bool radio_send(uint8_t *data, uint8_t length) {
    if (radio_state == RADIO_OFF || !data || !length || length > RADIO_MAX_PACKET_SIZE) return false;
    radio_packet[RADIO_PACKET_LENGTH_BYTE] = length + RADIO_PACKET_RESERVED_BYTES;
    memcpy(&radio_packet[RADIO_PACKET_RESERVED_BYTES], data, length);
    radio_transmit();
    return true;
}

bool radio_recv(uint8_t *data, uint8_t &length, uint16_t timeout_us) {
    if (radio_state == RADIO_OFF || !data || !length) return false;

    if (radio_state != RADIO_RX) {
        NRF_RADIO->EVENTS_READY = 0;
        NRF_RADIO->TASKS_RXEN   = 1;
        while (!NRF_RADIO->EVENTS_READY) nrf_delay_us(1);
        radio_state = RADIO_RX;
    }

    if (!radio_do_work(timeout_us)) return false;

    if (NRF_RADIO->CRCSTATUS) {
        uint8_t packet_length = radio_packet[RADIO_PACKET_LENGTH_BYTE] - RADIO_PACKET_RESERVED_BYTES;
        if (packet_length < length) length = packet_length;
        memcpy(data, &radio_packet[RADIO_PACKET_RESERVED_BYTES], length);
        return true;
    }

    return false;
}