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

/*PRIVATE API*/

/*MACROS*/
#define _BV(X) (1 << (X))
#define assert_not_bool(what, assert_not_val, ret_val) do \
{ \
    if((what) == (assert_not_val)) \
    { \
        debug_printf("Failure : ["#what"] is ["#assert_not_val"] in file ["__FILE__"] in function [%s] at line [%d]", __FUNCTION__, __LINE__); \
        return (bool)(ret_val); \
    } \
} while(0)

#define assert_not_uint8_t(what, assert_not_val, ret_val) do \
{ \
    if((what) == (assert_not_val)) \
    { \
        debug_printf("Failure : ["#what"] is ["#assert_not_val"] in file ["__FILE__"] in function [%s] at line [%d]", __FUNCTION__, __LINE__); \
        return (uint8_t)(ret_val); \
    } \
} while(0)

#define assert_not_void(what, assert_not_val) do \
{ \
    if((what) == (assert_not_val)) \
    { \
        debug_printf("Failure : ["#what"] is ["#assert_not_val"] in file ["__FILE__"] in function [%s] at line [%d]", __FUNCTION__, __LINE__); \
        return; \
    } \
} while(0)

/*REGISTER LIST*/
typedef enum NRF_REGISTER
{
    CONFIG = 0x00,
    EN_AA,
    EN_RXADDR,
    SETUP_AW,
    SETUP_RETR,
    RF_CH,
    RF_SETUP,
    STATUS,
    OBSERVE_TX,
    RPD,
    RX_ADDR_P0,
    RX_ADDR_P1,
    RX_ADDR_P2,
    RX_ADDR_P3,
    RX_ADDR_P4,
    RX_ADDR_P5,
    TX_ADDR,
    RX_PW_P0,
    RX_PW_P1,
    RX_PW_P2,
    RX_PW_P3,
    RX_PW_P4,
    RX_PW_P5,
    FIFO_STATUS,
    DYNPD = 0x1C,
    FEATURE
} NRF_REGISTER_e;

/*REGISTER BITS*/
typedef enum NRF_CONFIG_REGISTER_BIT
{
    CONFIG_PRIM_RX_BIT = 0,
    CONFIG_PWR_UP_BIT,
    CONFIG_CRCO_BIT,
    CONFIG_EN_CRC_BIT,
    CONFIG_MASK_MAX_RT_BIT,
    CONFIG_MASK_TX_DS_BIT,
    CONFIG_MASK_RX_DR_BIT
} NRF_CONFIG_REGISTER_BIT_e;

typedef enum NRF_RF_SETUP_REGISTER_BIT
{
    RF_SETUP_LNA_HCURR_BIT = 0,
    RF_SETUP_RF_PWR_LOW_BIT,
    RF_SETUP_RF_PWR_HIGH_BIT,
    RF_SETUP_RF_DR_HIGH_BIT,
    RF_SETUP_PLL_LOCK_BIT,
    RF_SETUP_RF_DR_LOW_BIT,
    RF_SETUP_CONT_WAVE_BIT = 7,
} NRF_RF_SETUP_REGISTER_BIT_e;

typedef enum NRF_STATUS_REGISTER_BIT
{
    STATUS_TX_FULL_BIT = 0,
    STATUS_RX_P_NO_BIT,
    STATUS_MAX_RT_BIT = 4,
    STATUS_TX_DS_BIT,
    STATUS_RX_DR_BIT
} NRF_STATUS_REGISTER_BIT_e;

typedef enum NRF_FEATURE_REGISTER_BIT
{
    FEATURE_EN_DYN_ACK_BIT = 0,
    FEATURE_EN_ACK_PAY_BIT,
    FEATURE_EN_DPL_BIT
} NRF_FEATURE_REGISTER_BIT_e;

typedef enum NRF_FIFO_STATUS_REGISTER_BIT
{
    FIFO_STATUS_RX_EMPTY_BIT = 0,
    FIFO_STATUS_RX_FULL_BIT,
    FIFO_STATUS_TX_EMPTY_BIT = 4,
    FIFO_STATUS_TX_FULL_BIT,
    FIFO_STATUS_TX_REUSE_BIT
} NRF_FIFO_STATUS_REGISTER_BIT_e;

typedef enum NRF_DYNPD_REGISTER_BIT
{
    DYNPD_DPL_P0_BIT = 0,
    DYNPD_DPL_P1_BIT,
    DYNPD_DPL_P2_BIT,
    DYNPD_DPL_P3_BIT,
    DYNPD_DPL_P4_BIT,
    DYNPD_DPL_P5_BIT,
} NRF_DYNPD_REGISTER_BIT_e;

/*COMMAND LIST*/
typedef enum NRF_COMMAND
{
    R_REGISTER = 0x00,
    W_REGISTER = 0x20,
    R_RX_PAYLOAD = 0x61,
    W_TX_PAYLOAD = 0xA0,
    FLUSH_TX = 0xE1,
    FLUSH_RX = 0xE2,
    REUSE_TX_PL = 0xE3,
    ACTIVATE = 0x50,
    R_RX_PL_WID = 0x60,
    W_ACK_PAYLOAD = 0xA8,
    W_TX_PAYLOAD_NOACK = 0xB0,
    NRF_NOP = 0xFF
} NRF_COMMAND_e;

static void read_registers(NRF24L01P_t *NRF, NRF_REGISTER_e reg, uint8_t *buffer, uint8_t length)
{
    assert_not_void(NRF, NULL);
    assert_not_void(buffer, NULL);
    assert_not_void(length, 0);

    //We first read the STATUS reg because there is no combo write/read SPI function.
    uint8_t command = R_REGISTER | STATUS;
    tls_spi_read_with_cmd(&command, sizeof command, &NRF->status, sizeof NRF->status);

    command = R_REGISTER | reg;
    tls_spi_read_with_cmd(&command, sizeof command, buffer, length);
}

static uint8_t read_register(NRF24L01P_t *NRF, NRF_REGISTER_e reg)
{
    assert_not_uint8_t(NRF, NULL, 0);

    //We first read the STATUS reg because there is no combo write/read SPI function.
    uint8_t command = R_REGISTER | STATUS, reg_value = 0;
    tls_spi_read_with_cmd(&command, sizeof command, &NRF->status, sizeof NRF->status);

    command = R_REGISTER | reg;
    tls_spi_read_with_cmd(&command, sizeof command, &reg_value, sizeof reg_value);

    return reg_value;
}

static void write_registers(NRF24L01P_t *NRF, NRF_REGISTER_e reg, const uint8_t *buffer, uint8_t length)
{
    assert_not_void(NRF, NULL);
    assert_not_void(buffer, NULL);
    assert_not_void(length, 0);

    //We first read the STATUS reg because there is no combo write/read SPI function.
    uint8_t command = R_REGISTER | STATUS;
    tls_spi_read_with_cmd(&command, sizeof command, &NRF->status, sizeof NRF->status);

    command = W_REGISTER | reg;
    tls_spi_write_with_cmd(&command, sizeof command, buffer, length);
}

static void write_register(NRF24L01P_t *NRF, NRF_REGISTER_e reg, uint8_t value)
{
    assert_not_void(NRF, NULL);

    //We first read the STATUS reg because there is no combo write/read SPI function.
    uint8_t command = R_REGISTER | STATUS;
    tls_spi_read_with_cmd(&command, sizeof command, &NRF->status, sizeof NRF->status);

    command = W_REGISTER | reg;
    tls_spi_write_with_cmd(&command, sizeof command, &value, sizeof value);
}

static void write_payload(NRF24L01P_t *NRF, const void *payload, uint8_t length, NRF_COMMAND_e type)
{
    assert_not_void(NRF, NULL);
    assert_not_void(payload, NULL);
    assert_not_void(length, 0);

    //We first read the STATUS reg because there is no combo write/read SPI function.
    uint8_t command = R_REGISTER | STATUS;
    tls_spi_read_with_cmd(&command, sizeof command, &NRF->status, sizeof NRF->status);

    if(!NRF->dynamic_payloads_enabled)
    {
        length = length < NRF->payload_size ? length : NRF->payload_size;
    }
    else
    {
        length = length < 32 ? length : 32;
    }

    command = type;
    tls_spi_write_with_cmd(&command, sizeof command, payload, length);
}

static void read_payload(NRF24L01P_t *NRF, void *payload, uint8_t length)
{
    assert_not_void(NRF, NULL);
    assert_not_void(payload, NULL);

    //We first read the STATUS reg because there is no combo write/read SPI function.
    uint8_t command = R_REGISTER | STATUS, full_payload[32] = {0};
    tls_spi_read_with_cmd(&command, sizeof command, &NRF->status, sizeof NRF->status);

    if(!NRF->dynamic_payloads_enabled)
    {
        length = length < NRF->payload_size ? length : NRF->payload_size;
    }
    else
    {
        length = length < 32 ? length : 32;
    }

    command = R_RX_PAYLOAD;

    tls_spi_read_with_cmd(&command, sizeof command, full_payload, sizeof full_payload);
    memcpy(payload, full_payload, length);
}

static void reuse_tx_payload(NRF24L01P_t *NRF)
{
    assert_not_void(NRF, NULL);

    //We first read the STATUS reg because there is no combo write/read SPI function.
    uint8_t command = R_REGISTER | STATUS;
    tls_spi_read_with_cmd(&command, sizeof command, &NRF->status, sizeof NRF->status);

    command = REUSE_TX_PL;
    tls_spi_write(&command, sizeof command);
}

static uint8_t r_rx_pl_wid(NRF24L01P_t *NRF)
{
    assert_not_uint8_t(NRF, NULL, 0);

    //We first read the STATUS reg because there is no combo write/read SPI function.
    uint8_t command = R_REGISTER | STATUS, pl_wid = 0;
    tls_spi_read_with_cmd(&command, sizeof command, &NRF->status, sizeof NRF->status);

    command = R_RX_PL_WID;
    tls_spi_read_with_cmd(&command, sizeof command, &pl_wid, sizeof pl_wid);

    return pl_wid;
}

static void toggle_features(NRF24L01P_t *NRF)
{
    assert_not_void(NRF, NULL);

    uint8_t command = ACTIVATE, activate_magic_value = 0x73;
    tls_spi_write_with_cmd(&command, sizeof command, &activate_magic_value, sizeof activate_magic_value);
}

static uint8_t get_status(NRF24L01P_t *NRF)
{
    return read_register(NRF, STATUS);
}

static void err_notify(NRF24L01P_t *NRF)
{
    assert_not_void(NRF, NULL);

    debug_printf("RF24 HARDWARE FAIL: Radio not responding, verify pin connections, wiring, etc.");

    NRF->failure_detected = true;
}
/*PUBLIC API*/
bool NRF24L01P_begin(NRF24L01P_t *NRF)
{
    assert_not_bool(NRF, NULL, false);

    memset(NRF, 0, sizeof *NRF);

    CE_LOW();
    delay_ms(5);
    NRF24L01P_setRetries(NRF, 5, 15);
    NRF24L01P_setDataRate(NRF, RF24_1MBPS);

    //Detect if the chip is a plus variant & use old toggle features command accordingly
    uint8_t before_toggle = read_register(NRF, FEATURE);
    toggle_features(NRF);
    uint8_t after_toggle = read_register(NRF, FEATURE);
    NRF->is_p_variant = before_toggle == after_toggle;
    if(after_toggle)
    {
        if(NRF->is_p_variant)
        {
            toggle_features(NRF);
        }

        write_register(NRF, FEATURE, 0);
    }

    NRF->ack_payloads_enabled = false;
    write_register(NRF, DYNPD, 0);
    NRF->dynamic_payloads_enabled = false;
    write_register(NRF, EN_AA, 0x3F);
    write_register(NRF, EN_RXADDR, 0);
    NRF24L01P_setPayloadSize(NRF, 32);
    NRF24L01P_setAddressWidth(NRF, 5);

    NRF24L01P_setChannel(NRF, 76);

    write_register(NRF, STATUS, _BV(STATUS_MAX_RT_BIT) | _BV(STATUS_TX_DS_BIT) | _BV(STATUS_RX_DR_BIT));

    NRF24L01P_flush_rx(NRF);
    NRF24L01P_flush_tx(NRF);

    write_register(NRF, CONFIG, _BV(CONFIG_EN_CRC_BIT) | _BV(CONFIG_CRCO_BIT));
    NRF->config_reg = read_register(NRF, CONFIG);

    NRF24L01P_powerUp(NRF);

    return NRF->config_reg == (_BV(CONFIG_EN_CRC_BIT) | _BV(CONFIG_CRCO_BIT) | _BV(CONFIG_PWR_UP_BIT));
}

bool NRF24L01P_isChipConnected(NRF24L01P_t *NRF)
{
    assert_not_bool(NRF, NULL, false);

    uint8_t value = read_register(NRF, SETUP_AW);

    return value >= 1 && value <= 3;
}

void NRF24L01P_startListening(NRF24L01P_t *NRF)
{
    assert_not_void(NRF, NULL);

    NRF24L01P_powerUp(NRF);
    NRF->config_reg |= _BV(CONFIG_PRIM_RX_BIT);
    write_register(NRF, CONFIG, NRF->config_reg);
    write_register(NRF, STATUS, _BV(STATUS_MAX_RT_BIT) | _BV(STATUS_TX_DS_BIT) | _BV(STATUS_RX_DR_BIT));

    CE_HIGH();

    if(NRF->pipe0_reading_address[0] > 0)
    {
        write_registers(NRF, RX_ADDR_P0, NRF->pipe0_reading_address, NRF->addr_width);
    }
    else
    {
        NRF24L01P_closeReadingPipe(NRF, 0);
    }
}

void NRF24L01P_stopListening(NRF24L01P_t *NRF)
{
    assert_not_void(NRF, NULL);

    CE_LOW();

    delay_ms(1);
    if(NRF->ack_payloads_enabled)
    {
        NRF24L01P_flush_tx(NRF);
    }

    NRF->config_reg &= ~_BV(CONFIG_PRIM_RX_BIT);
    write_register(NRF, CONFIG, NRF->config_reg);

    uint8_t en_rxaddr = read_register(NRF, EN_RXADDR);
    en_rxaddr |= _BV(0);
    write_register(NRF, EN_RXADDR, en_rxaddr);
}

bool NRF24L01P_available(NRF24L01P_t *NRF)
{
    assert_not_bool(NRF, NULL, false);

    return NRF24L01P_availablePipe(NRF, NULL);
}

void NRF24L01P_powerDown(NRF24L01P_t *NRF)
{
    assert_not_void(NRF, NULL);

    uint8_t reg = read_register(NRF, CONFIG);
    reg &= ~_BV(CONFIG_PWR_UP_BIT);
    write_register(NRF, CONFIG, reg);
}

void NRF24L01P_powerUp(NRF24L01P_t *NRF)
{
    assert_not_void(NRF, NULL);

    if(!(NRF->config_reg & _BV(CONFIG_PWR_UP_BIT)))
    {
        NRF->config_reg = read_register(NRF, CONFIG);
        NRF->config_reg |= _BV(CONFIG_PWR_UP_BIT);
        write_register(NRF, CONFIG, NRF->config_reg);

        delay_ms(5);
    }
}

bool NRF24L01P_writeMulticast(NRF24L01P_t *NRF, const void *payload, uint8_t length, const bool multicast)
{
    assert_not_bool(NRF, NULL, false);
    assert_not_bool(payload, NULL, false);
    assert_not_bool(length, 0, false);

    NRF24L01P_startFastWrite(NRF, payload, length, multicast, true);

    uint32_t timer = elapsed_ms();

    while( !(get_status(NRF) & (_BV(STATUS_TX_DS_BIT) | _BV(STATUS_MAX_RT_BIT))) )
    {
        if(elapsed_ms() - timer > 95)
        {
            err_notify(NRF);
            return false;
        }
    }

    CE_LOW();

    write_register(NRF, STATUS, _BV(STATUS_MAX_RT_BIT) | _BV(STATUS_TX_DS_BIT) | _BV(STATUS_RX_DR_BIT));

    if(NRF->status & _BV(STATUS_MAX_RT_BIT))
    {
        NRF24L01P_flush_tx(NRF);
        return false;
    }

    return true;
}

bool NRF24L01P_write(NRF24L01P_t *NRF, const void *payload, uint8_t length)
{
    assert_not_bool(NRF, NULL, false);
    assert_not_bool(payload, NULL, false);
    assert_not_bool(length, 0, false);
    
    return NRF24L01P_writeMulticast(NRF, payload, length, false);
}

bool NRF24L01P_writeBlocking(NRF24L01P_t *NRF, const void *payload, uint8_t length, uint32_t timeout)
{
    assert_not_bool(NRF, NULL, false);
    assert_not_bool(payload, NULL, false);
    assert_not_bool(length, 0, false);

    uint32_t timer = elapsed_ms();

    while(get_status(NRF) & _BV(STATUS_TX_FULL_BIT))
    {
        if(NRF->status & _BV(STATUS_MAX_RT_BIT))
        {
            NRF24L01P_reUseTX(NRF);
            if(elapsed_ms() - timer > timeout)
            {
                return false;
            }
        }

        if(elapsed_ms() - timer > timeout + 95)
        {
            err_notify(NRF);
        }
    }

    NRF24L01P_startFastWrite(NRF, payload, length, false, true);

    return true;
}

void NRF24L01P_reUseTX(NRF24L01P_t *NRF)
{
    assert_not_void(NRF, NULL);

    write_register(NRF, STATUS, _BV(STATUS_MAX_RT_BIT));
    reuse_tx_payload(NRF);

    CE_LOW();
    delay_ms(1);
    CE_HIGH();
}

bool NRF24L01P_writeFastMulticast(NRF24L01P_t *NRF, const void *payload, uint8_t length, const bool multicast)
{
    assert_not_bool(NRF, NULL, false);
    assert_not_bool(payload, NULL, false);
    assert_not_bool(length, 0, false);

    uint32_t timer = elapsed_ms();

    while(get_status(NRF) & _BV(STATUS_TX_FULL_BIT))
    {
        if(NRF->status & _BV(STATUS_MAX_RT_BIT))
        {
            return false;
        }

        if(elapsed_ms() - timer > 95)
        {
            err_notify(NRF);
            return false;
        }
    }

    NRF24L01P_startFastWrite(NRF, payload, length, multicast, true);

    return true;
}

bool NRF24L01P_writeFast(NRF24L01P_t *NRF, const void *payload, uint8_t length)
{
    assert_not_bool(NRF, NULL, false);
    assert_not_bool(payload, NULL, false);
    assert_not_bool(length, 0, false);

    return NRF24L01P_writeFastMulticast(NRF, payload, length, false);
}

void NRF24L01P_startFastWrite(NRF24L01P_t *NRF, const void* payload, uint8_t length, const bool multicast, bool start_tx)
{
    assert_not_void(NRF, NULL);
    assert_not_void(payload, NULL);
    assert_not_void(length, 0);

    write_payload(NRF, payload, length, multicast ? W_TX_PAYLOAD_NOACK : W_TX_PAYLOAD);

    if(start_tx)
    {
        CE_HIGH();
    }
}

bool NRF24L01P_startWrite(NRF24L01P_t *NRF, const void *payload, uint8_t length, const bool multicast)
{
    assert_not_bool(NRF, NULL, false);
    assert_not_bool(payload, NULL, false);
    assert_not_bool(length, 0, false);

    write_payload(NRF, payload, length,  multicast ? W_TX_PAYLOAD_NOACK : W_TX_PAYLOAD);
    CE_HIGH();
    delay_ms(1);
    CE_LOW();

    return !(get_status(NRF) & _BV(STATUS_TX_FULL_BIT));
}

bool NRF24L01P_rxFifoFull(NRF24L01P_t *NRF)
{
    assert_not_bool(NRF, NULL, false);

    return read_register(NRF, FIFO_STATUS) & _BV(FIFO_STATUS_RX_FULL_BIT) ? true : false;
}

bool NRF24L01P_txFifoFull(NRF24L01P_t *NRF)
{
    assert_not_bool(NRF, NULL, false);

    return read_register(NRF, FIFO_STATUS) & _BV(FIFO_STATUS_TX_FULL_BIT) ? true : false;
}

bool NRF24L01P_txStandBy(NRF24L01P_t *NRF)
{
    assert_not_bool(NRF, NULL, false);

    uint32_t timeout = elapsed_ms();

    while(!(read_register(NRF, FIFO_STATUS) & _BV(FIFO_STATUS_TX_EMPTY_BIT)))
    {
        if(NRF->status & _BV(STATUS_MAX_RT_BIT))
        {
            write_register(NRF, STATUS, _BV(STATUS_MAX_RT_BIT));
            CE_LOW();
            NRF24L01P_flush_tx(NRF);

            return false;
        }

        if(elapsed_ms() - timeout > 95)
        {
            err_notify(NRF);
            return false;
        }
    }

    CE_LOW();
    return true;
}

bool NRF24L01P_txStandByTimeout(NRF24L01P_t *NRF, uint32_t timeout, bool start_tx)
{
    assert_not_bool(NRF, NULL, false);

    if(start_tx)
    {
        NRF24L01P_stopListening(NRF);
        CE_HIGH();
    }

    uint32_t start = elapsed_ms();

    while(!(read_register(NRF, FIFO_STATUS) & _BV(FIFO_STATUS_TX_EMPTY_BIT)))
    {
        if(NRF->status & _BV(STATUS_MAX_RT_BIT))
        {
            write_register(NRF, STATUS, _BV(STATUS_MAX_RT_BIT));
            CE_LOW();
            delay_ms(1);
            CE_HIGH();
            if(elapsed_ms() - start >= timeout)
            {
                CE_LOW();
                NRF24L01P_flush_tx(NRF);
                return false;
            }
        }

        if(elapsed_ms() - start > timeout + 95)
        {
            err_notify(NRF);
            return false;
        }
    }

    CE_LOW();

    return true;
}

void NRF24L01P_maskIRQ(NRF24L01P_t *NRF, bool tx, bool fail, bool rx)
{
    assert_not_void(NRF, NULL);

    NRF->config_reg &= ~(1 << CONFIG_MASK_MAX_RT_BIT | 1 << CONFIG_MASK_TX_DS_BIT | 1 << CONFIG_MASK_RX_DR_BIT);
    NRF->config_reg |= fail << CONFIG_MASK_MAX_RT_BIT | tx << CONFIG_MASK_TX_DS_BIT | rx << CONFIG_MASK_RX_DR_BIT;
    write_register(NRF, CONFIG, NRF->config_reg);
}

uint8_t NRF24L01P_getDynamicPayloadSize(NRF24L01P_t *NRF)
{
    assert_not_uint8_t(NRF, NULL, 0);

    uint8_t pl_wid = r_rx_pl_wid(NRF);

    if(pl_wid > 32)
    {
        NRF24L01P_flush_rx(NRF);
        delay_ms(2);
        return 0;
    }

    return pl_wid;
}

bool NRF24L01P_availablePipe(NRF24L01P_t *NRF, uint8_t *pipe)
{
    assert_not_bool(NRF, NULL, false);

    uint8_t get_pipe = (get_status(NRF) >> STATUS_RX_P_NO_BIT) & 0x07;

    if(get_pipe > 5) return false;

    if(pipe) *pipe = get_pipe;

    return true;
}

void NRF24L01P_read(NRF24L01P_t *NRF, void *payload, uint8_t length)
{
    assert_not_void(NRF, NULL);

    read_payload(NRF, payload, length);

    write_register(NRF, STATUS, _BV(STATUS_RX_DR_BIT));
}

void NRF24L01P_whatHappened(NRF24L01P_t *NRF, bool *tx_ok, bool *tx_fail, bool *rx_ready)
{
    assert_not_void(NRF, NULL);

    write_register(NRF, STATUS, _BV(STATUS_MAX_RT_BIT) | _BV(STATUS_TX_DS_BIT) | _BV(STATUS_RX_DR_BIT));

    if(tx_ok) *tx_ok = NRF->status & _BV(STATUS_TX_DS_BIT) ? true : false;
    if(tx_fail) *tx_fail = NRF->status & _BV(STATUS_MAX_RT_BIT) ? true : false;
    if(rx_ready) *rx_ready = NRF->status & _BV(STATUS_RX_DR_BIT) ? true : false;
}

void NRF24L01P_openWritingPipe(NRF24L01P_t *NRF, const uint8_t* address)
{
    assert_not_void(NRF, NULL);
    
    write_registers(NRF, RX_ADDR_P0, address, NRF->addr_width);
    write_registers(NRF, TX_ADDR, address, NRF->addr_width);
}

void NRF24L01P_setAddressWidth(NRF24L01P_t *NRF, uint8_t a_width)
{
    assert_not_void(NRF, NULL);
    if(a_width < 3 || a_width > 5) return;

    NRF->addr_width = a_width;
    write_register(NRF, SETUP_AW, NRF->addr_width - 2);
}

void NRF24L01P_openReadingPipe(NRF24L01P_t *NRF, uint8_t pipe, const uint8_t *address)
{
    assert_not_void(NRF, NULL);
    assert_not_void(address, NULL);

    if(pipe == 0)
    {
        memcpy(NRF->pipe0_reading_address, address, NRF->addr_width);
    }
    
    if(pipe <= 5)
    {
        if(pipe < 2)
        {
            write_registers(NRF, RX_ADDR_P0 + pipe, address, NRF->addr_width);
        }
        else
        {
            write_register(NRF, RX_ADDR_P0 + pipe, address[0]);
        }

        uint8_t en_rxaddr = read_register(NRF, EN_RXADDR);
        en_rxaddr |= _BV(pipe);
        write_register(NRF, EN_RXADDR, en_rxaddr);
    }
}

void NRF24L01P_closeReadingPipe(NRF24L01P_t *NRF, uint8_t pipe)
{
    assert_not_void(NRF, NULL);

    if(pipe > 5) return;

    uint8_t en_rxaddr = read_register(NRF, EN_RXADDR);
    en_rxaddr &= ~_BV(pipe);
    write_register(NRF, EN_RXADDR, en_rxaddr);
}

void NRF24L01P_enableDynamicPayloads(NRF24L01P_t *NRF)
{
    assert_not_void(NRF, NULL);

    write_register(NRF, FEATURE, read_register(NRF, FEATURE) | _BV(FEATURE_EN_DPL_BIT));
    write_register(NRF, DYNPD, read_register(NRF, DYNPD) | _BV(DYNPD_DPL_P0_BIT) | _BV(DYNPD_DPL_P1_BIT) | _BV(DYNPD_DPL_P2_BIT) | _BV(DYNPD_DPL_P3_BIT) | _BV(DYNPD_DPL_P4_BIT) | _BV(DYNPD_DPL_P5_BIT));

    NRF->dynamic_payloads_enabled = true;
}

void NRF24L01P_disableDynamicPayloads(NRF24L01P_t *NRF)
{
    assert_not_void(NRF, NULL);

    write_register(NRF, FEATURE, 0);
    write_register(NRF, DYNPD, 0);

    NRF->dynamic_payloads_enabled = false;
    NRF->ack_payloads_enabled = false;
}

void NRF24L01P_enableAckPayload(NRF24L01P_t *NRF)
{
    assert_not_void(NRF, NULL);

    if(!NRF->ack_payloads_enabled)
    {
        write_register(NRF, FEATURE, read_register(NRF, FEATURE) | _BV(FEATURE_EN_ACK_PAY_BIT) | _BV(FEATURE_EN_DPL_BIT));
        write_register(NRF, DYNPD, read_register(NRF, DYNPD) | _BV(DYNPD_DPL_P0_BIT));

        NRF->dynamic_payloads_enabled = true;
        NRF->ack_payloads_enabled = true;
    }
}

void NRF24L01P_disableAckPayload(NRF24L01P_t *NRF)
{
    assert_not_void(NRF, NULL);

    if(NRF->ack_payloads_enabled)
    {
        write_register(NRF, FEATURE, read_register(NRF, FEATURE) | ~_BV(FEATURE_EN_ACK_PAY_BIT));

        NRF->ack_payloads_enabled = true;
    }
}

void NRF24L01P_enableDynamicAck(NRF24L01P_t *NRF)
{
    assert_not_void(NRF, NULL);

    write_register(NRF, FEATURE, read_register(NRF, FEATURE) | _BV(FEATURE_EN_DYN_ACK_BIT));
}

bool NRF24L01P_writeAckPayload(NRF24L01P_t *NRF, uint8_t pipe, const void *payload, uint8_t length)
{
    assert_not_bool(NRF, NULL, false);
    assert_not_bool(payload, NULL, false);

    if(pipe > 5) return false;

    if(NRF->ack_payloads_enabled)
    {
        write_payload(NRF, payload, length, W_ACK_PAYLOAD | pipe);
        return !(NRF->status & _BV(STATUS_TX_FULL_BIT));
    }
    return false;
}

bool NRF24L01P_isAckPayloadAvailable(NRF24L01P_t *NRF)
{
    assert_not_bool(NRF, NULL, false);

    return NRF24L01P_available(NRF);
}

bool NRF24L01P_isPVariant(NRF24L01P_t *NRF)
{
    assert_not_bool(NRF, NULL, false);

    return NRF->is_p_variant;
}

void NRF24L01P_setAutoAck(NRF24L01P_t *NRF, bool enable)
{
    assert_not_void(NRF, NULL);

    if(enable)
    {
        write_register(NRF, EN_AA, 0x3F);
    }
    else
    {
        write_register(NRF, EN_AA, 0);
        // accomodate ACK payloads feature
        if(NRF->ack_payloads_enabled)
        {
            NRF24L01P_disableAckPayload(NRF);
        }
    }
}

void NRF24L01P_setAutoAckPipe(NRF24L01P_t *NRF, uint8_t pipe, bool enable)
{
    assert_not_void(NRF, NULL);

    if(pipe < 6)
    {
        uint8_t en_aa = read_register(NRF, EN_AA);
        if(enable)
        {
            en_aa |= _BV(pipe);
        }
        else
        {
            en_aa &= ~_BV(pipe);
            if(NRF->ack_payloads_enabled && !pipe)
            {
               NRF24L01P_disableAckPayload(NRF);
            }
        }
        write_register(NRF, EN_AA, en_aa);
    }
}

bool NRF24L01P_testCarrier(NRF24L01P_t *NRF)
{
    assert_not_bool(NRF, NULL, false);

    return read_register(NRF, RPD) & 1;
}

bool NRF24L01P_testRPD(NRF24L01P_t *NRF)
{
    assert_not_bool(NRF, NULL, false);

    return NRF24L01P_testCarrier(NRF);
}

void NRF24L01P_setPALevel(NRF24L01P_t *NRF, rf24_pa_dbm_e level, bool lna_enable)
{
    assert_not_void(NRF, NULL);

    uint8_t setup = read_register(NRF, RF_SETUP) & 0xF8;

    if (level > 3) {                            // If invalid level, go to max PA
        level = (RF24_PA_MAX << 1) + lna_enable; // +1 to support the SI24R1 chip extra bit
    } else {
        level = (level << 1) + lna_enable;       // Else set level as requested
    }

    write_register(NRF, RF_SETUP, setup |= level);   // Write it to the chip
}

rf24_pa_dbm_e NRF24L01P_getPALevel(NRF24L01P_t *NRF)
{
    assert_not_uint8_t(NRF, NULL, RF24_PA_MIN);

    return (read_register(NRF, RF_SETUP) & (_BV(RF_SETUP_RF_PWR_LOW_BIT) | _BV(RF_SETUP_RF_PWR_HIGH_BIT))) >> 1;
}

uint8_t NRF24L01P_getARC(NRF24L01P_t *NRF)
{
    assert_not_uint8_t(NRF, NULL, 0);

    return read_register(NRF, OBSERVE_TX) & 0x0F;
}

void NRF24L01P_printDetails(NRF24L01P_t *NRF)
{
    assert_not_void(NRF, NULL);

    shell_printf("\
+-------------------------+"NEW_LINE"\
|      NRF REGISTERS      |"NEW_LINE"\
+-------------------------+"NEW_LINE);
    shell_printf("CONFIG(%u) : 0x%02X"NEW_LINE, CONFIG, read_register(NRF, CONFIG));
    shell_printf("EN_RXADDR(%u) : 0x%02X"NEW_LINE, EN_RXADDR, read_register(NRF, EN_RXADDR));
    shell_printf("SETUP_AW(%u) : 0x%02X"NEW_LINE, SETUP_AW, read_register(NRF, SETUP_AW));
    shell_printf("SETUP_RETR(%u) : 0x%02X"NEW_LINE, SETUP_RETR, read_register(NRF, SETUP_RETR));
    shell_printf("RF_CH(%u) : 0x%02X"NEW_LINE, RF_CH, read_register(NRF, RF_CH));
    shell_printf("RF_SETUP(%u) : 0x%02X"NEW_LINE, RF_SETUP, read_register(NRF, RF_SETUP));
    shell_printf("STATUS(%u) : 0x%02X"NEW_LINE, STATUS, read_register(NRF, STATUS));
    uint8_t addr[5] = {0};
    read_registers(NRF, RX_ADDR_P0, addr, sizeof addr);
    shell_printf("RX_ADDR_P0(%u) : 0x%02X%02X%02X%02X%02X"NEW_LINE, RX_ADDR_P0, addr[0], addr[1], addr[2], addr[3], addr[4]);
    read_registers(NRF, RX_ADDR_P1, addr, sizeof addr);
    shell_printf("RX_ADDR_P1(%u) : 0x%02X%02X%02X%02X%02X"NEW_LINE, RX_ADDR_P1, addr[0], addr[1], addr[2], addr[3], addr[4]);
    read_registers(NRF, TX_ADDR, addr, sizeof addr);
    shell_printf("TX_ADDR(%u) : 0x%02X%02X%02X%02X%02X"NEW_LINE, TX_ADDR, addr[0], addr[1], addr[2], addr[3], addr[4]);
    shell_printf("RX_PW_P0(%u) : %u"NEW_LINE, RX_PW_P0, read_register(NRF, RX_PW_P0));
    shell_printf("RX_PW_P1(%u) : %u"NEW_LINE, RX_PW_P1, read_register(NRF, RX_PW_P1));
    shell_printf("RX_PW_P2(%u) : %u"NEW_LINE, RX_PW_P2, read_register(NRF, RX_PW_P2));
    shell_printf("RX_PW_P3(%u) : %u"NEW_LINE, RX_PW_P3, read_register(NRF, RX_PW_P3));
    shell_printf("RX_PW_P4(%u) : %u"NEW_LINE, RX_PW_P4, read_register(NRF, RX_PW_P4));
    shell_printf("RX_PW_P5(%u) : %u"NEW_LINE, RX_PW_P5, read_register(NRF, RX_PW_P5));
}

void NRF24L01P_setRetries(NRF24L01P_t *NRF, uint8_t delay, uint8_t count)
{
    assert_not_void(NRF, NULL);

    write_register(NRF, SETUP_RETR, (delay << 4) | (count & 0xF));
}

uint8_t NRF24L01P_flush_tx(NRF24L01P_t *NRF)
{
    write_register(NRF, FLUSH_TX, NRF_NOP);

    return read_register(NRF, STATUS);
}

uint8_t NRF24L01P_flush_rx(NRF24L01P_t *NRF)
{
    write_register(NRF, FLUSH_RX, NRF_NOP);

    return read_register(NRF, STATUS);
}

bool NRF24L01P_setDataRate(NRF24L01P_t *NRF, rf24_datarate_e data_rate)
{
    assert_not_bool(NRF, NULL, false);

    uint8_t rf_setup = read_register(NRF, RF_SETUP);

    rf_setup &= ~(_BV(RF_SETUP_RF_DR_LOW_BIT) | _BV(RF_SETUP_RF_DR_HIGH_BIT));

    if(data_rate == RF24_250KBPS)
    {
        rf_setup |= _BV(RF_SETUP_RF_DR_LOW_BIT);
    }
    else if(data_rate == RF24_2MBPS)
    {
        rf_setup |= _BV(RF_SETUP_RF_DR_HIGH_BIT);
    }
    write_register(NRF, RF_SETUP, rf_setup);

    if(read_register(NRF, RF_SETUP) == rf_setup)
    {
        return true;
    }
    return false;
}

rf24_datarate_e NRF24L01P_getDataRate(NRF24L01P_t *NRF)
{
    assert_not_uint8_t(NRF, NULL, RF24_1MBPS);

    uint8_t dr = read_register(NRF, RF_SETUP);

    if(dr == _BV(RF_SETUP_RF_DR_LOW_BIT))
    {
        // '10' = 250KBPS
        return RF24_250KBPS;
    }
    else if(dr == _BV(RF_SETUP_RF_DR_HIGH_BIT))
    {
        // '01' = 2MBPS
        return RF24_2MBPS;
    }
    else
    {
        // '00' = 1MBPS
        return RF24_1MBPS;
    }
}

void NRF24L01P_setCRCLength(NRF24L01P_t *NRF, rf24_crclength_e length)
{
    assert_not_void(NRF, NULL);

    NRF->config_reg &= ~(_BV(CONFIG_CRCO_BIT) | _BV(CONFIG_EN_CRC_BIT));

    if(length == RF24_CRC_8)
    {
        NRF->config_reg |= _BV(CONFIG_EN_CRC_BIT);
    }
    else if(length == RF24_CRC_16)
    {
        NRF->config_reg |= _BV(CONFIG_EN_CRC_BIT) | _BV(CONFIG_CRCO_BIT);
    }

    write_register(NRF, CONFIG, NRF->config_reg);
}

rf24_crclength_e NRF24L01P_getCRCLength(NRF24L01P_t *NRF)
{
    assert_not_uint8_t(NRF, NULL, RF24_CRC_DISABLED);

    rf24_crclength_e result = RF24_CRC_DISABLED;
    uint8_t AA = read_register(NRF, EN_AA);
    NRF->config_reg = read_register(NRF, CONFIG);

    if(NRF->config_reg & _BV(CONFIG_EN_CRC_BIT) || AA)
    {
        if(NRF->config_reg & _BV(CONFIG_CRCO_BIT))
        {
            result = RF24_CRC_16;
        }
        else
        {
            result = RF24_CRC_8;
        }
    }

    return result;
}

void NRF24L01P_disableCRC(NRF24L01P_t *NRF)
{
    assert_not_void(NRF, NULL);

    NRF->config_reg &= ~_BV(CONFIG_EN_CRC_BIT);
    write_register(NRF, CONFIG, NRF->config_reg);
}

void NRF24L01P_setPayloadSize(NRF24L01P_t *NRF, uint8_t size)
{
    assert_not_void(NRF, NULL);
    if(size < 1 || size > 32) return;

    NRF->payload_size = size;

    for(uint8_t i = 0; i < 6; i++)
    {
        write_register(NRF, RX_PW_P0 + i, NRF->payload_size);
    }
}

uint8_t NRF24L01P_getPayloadSize(NRF24L01P_t *NRF)
{
    assert_not_uint8_t(NRF, NULL, 0);

    return NRF->payload_size;
}

void NRF24L01P_setChannel(NRF24L01P_t *NRF, uint8_t channel)
{
    assert_not_void(NRF, NULL);
    if(channel > 125) return;

    write_register(NRF, RF_CH, channel);
}

uint8_t NRF24L01P_getChannel(NRF24L01P_t *NRF)
{
    assert_not_uint8_t(NRF, NULL, 0);

    return read_register(NRF, RF_CH);
}

void NRF24L01P_startConstCarrier(NRF24L01P_t *NRF, rf24_pa_dbm_e level, uint8_t channel)
{
    assert_not_void(NRF, NULL);

    NRF24L01P_stopListening(NRF);
    write_register(NRF, RF_SETUP, read_register(NRF, RF_SETUP) | _BV(RF_SETUP_CONT_WAVE_BIT) | _BV(RF_SETUP_PLL_LOCK_BIT));
    if(NRF24L01P_isPVariant(NRF))
    {
        NRF24L01P_setAutoAck(NRF, false);
        NRF24L01P_setRetries(NRF, 0, 0);
        uint8_t dummy_buf[32];
        memset(dummy_buf, 0xFF, sizeof dummy_buf);

        write_registers(NRF, TX_ADDR, dummy_buf, 5);
        NRF24L01P_flush_tx(NRF);

        write_payload(NRF, dummy_buf, sizeof dummy_buf, W_TX_PAYLOAD);

        NRF24L01P_disableCRC(NRF);
    }

    NRF24L01P_setPALevel(NRF, level, false);
    NRF24L01P_setChannel(NRF, channel);

    CE_HIGH();

    if(NRF24L01P_isPVariant(NRF))
    {
        delay_ms(1);
        CE_LOW();
        NRF24L01P_reUseTX(NRF);
    }
}

void NRF24L01P_stopConstCarrier(NRF24L01P_t *NRF)
{
    assert_not_void(NRF, NULL);

    NRF24L01P_powerDown(NRF);
    write_register(NRF, RF_SETUP, read_register(NRF, RF_SETUP) & ~_BV(RF_SETUP_CONT_WAVE_BIT) & ~_BV(RF_SETUP_PLL_LOCK_BIT));
    CE_LOW();
}