#include <stdlib.h>
#include <stdbool.h>
#include <avr/io.h>
#include <util/delay.h>
#include <string.h>

#include "nrf24l01.h"
#include "nrf24l01-mnemonics.h"


static void copy_address(uint8_t *source, uint8_t *destination);
inline static void set_as_output(gpio_pin pin);
inline static void set_high(gpio_pin pin);
inline static void set_low(gpio_pin pin);
static void spi_init(nRF24L01 *rf);
static uint8_t spi_transfer(uint8_t data);


nRF24L01 *nRF24L01_init(void) {
    nRF24L01 *rf = malloc(sizeof(nRF24L01));
    memset(rf, 0, sizeof(nRF24L01));
    return rf;
}

void nRF24L01_begin(nRF24L01 *rf) {
    set_as_output(rf->ss);
    set_as_output(rf->ce);

    set_high(rf->ss);
    set_low(rf->ce);

    spi_init(rf);

    nRF24L01_send_command(rf, FLUSH_RX, NULL, 0);
    nRF24L01_send_command(rf, FLUSH_TX, NULL, 0);
    
    nRF24L01_clear_interrupts(rf);
    
    
    // CONFIG
    uint8_t data;
    data = _BV(EN_CRC) | _BV(CRCO) | _BV(PWR_UP) | _BV(PRIM_RX);
    nRF24L01_write_register(rf, CONFIG, &data, 1);

    // Vypnutie ACK
    //data = _BV(ENAA_P0) | _BV(ENAA_P1) | _BV(ENAA_P2) | _BV(ENAA_P3) | _BV(ENAA_P4) | _BV(ENAA_P5);
    data = 0x00;
    nRF24L01_write_register(rf, EN_AA, &data, 1);
    
    
    //Nastavenie kanála
    data = 0x4C;
    nRF24L01_write_register(rf, RF_CH, &data, 1);
    
    
    // Nastavenie rýchlosti na Imbps
    data = _BV(0) | _BV(1) |_BV(2) | _BV(RF_DR_HIGH);
    nRF24L01_write_register(rf, RF_SETUP, &data, 1);
    
    // Vypnutie dynamic payload na všetkých pipes
    //data = _BV(DPL_P0) | _BV(DPL_P1) | _BV(DPL_P2) | _BV(DPL_P3) | _BV(DPL_P4) | _BV(DPL_P5);
    data = 0;
    nRF24L01_write_register(rf, DYNPD, &data, 1);
    
    // Globalne vypnutie dynamic payload
    //data = _BV(EN_DPL);
    data = 0;
    nRF24L01_write_register(rf, FEATURE, &data, 1);
    
    // Zapnut pipe 0
    data = _BV(0);
    nRF24L01_write_register(rf, EN_RXADDR, &data, 1);

    // Nastavit payload dlzku 32
    data = 0x20;
    nRF24L01_write_register(rf, RX_PW_P0, &data, 1);
}

uint8_t nRF24L01_send_command(nRF24L01 *rf, uint8_t command, void *data, size_t length) {
    set_low(rf->ss);

    rf->status = spi_transfer(command);
    for (unsigned int i = 0; i < length; i++)
        ((uint8_t*)data)[i] = spi_transfer(((uint8_t*)data)[i]);

    set_high(rf->ss);

    return rf->status;
}

uint8_t nRF24L01_write_register(nRF24L01 *rf, uint8_t reg_address, void *data, size_t length) {
    return nRF24L01_send_command(rf, W_REGISTER | reg_address, data, length);
}

uint8_t nRF24L01_read_register(nRF24L01 *rf, uint8_t reg_address, void *data, size_t length) {
    return nRF24L01_send_command(rf, R_REGISTER | reg_address, data, length);
}

uint8_t nRF24L01_no_op(nRF24L01 *rf) {
    return nRF24L01_send_command(rf, NOP, NULL, 0);
}

uint8_t nRF24L01_update_status(nRF24L01 *rf) {
    return nRF24L01_no_op(rf);
}

uint8_t nRF24L01_get_status(nRF24L01 *rf) {
    return rf->status;
}

bool nRF24L01_data_received(nRF24L01 *rf) {
    nRF24L01_update_status(rf);
    return (rf->status & (1<<RX_DR));
}

void nRF24L01_start_listen(nRF24L01 *rf) {
    
    // PRIM_RX = 1
    uint8_t config;
    nRF24L01_read_register(rf, CONFIG, &config, 1);
    config |= _BV(PRIM_RX);
    nRF24L01_write_register(rf, CONFIG, &config, 1);
    
    // Prechod do RX
    set_high(rf->ce);
}

void nRF24L01_stop_listen(nRF24L01 *rf) {
    // Prechod do Standby-I
    set_low(rf->ce);
}

void nRF24L01_set_recieve_address(nRF24L01 *rf, void *address) {
    uint8_t addr[5];
    copy_address((uint8_t *)address, addr);
    nRF24L01_write_register(rf, RX_ADDR_P0, addr, 5);
}

void nRF24L01_set_recieve2_address(nRF24L01 *rf, void *address) {
    uint8_t addr[5];
    copy_address((uint8_t *)address, addr);
    nRF24L01_write_register(rf, RX_ADDR_P1, addr, 5);
}

bool nRF24L01_read_received_data(nRF24L01 *rf, char *message) {

    nRF24L01_clear_receive_interrupt(rf);

    char mess[32];
    nRF24L01_send_command(rf, R_RX_PAYLOAD, &mess, 32);
    
    memcpy(message, mess, 32);
    
    return true;
}

int nRF24L01_pipe_number_received(nRF24L01 *rf) {
    int pipe_number = (rf->status & RX_P_NO_MASK) >> 1;
    return pipe_number <= 5 ? pipe_number : -1;
}

void nRF24L01_set_transmit_address(nRF24L01 *rf, void *address) {
    uint8_t addr[5];
    copy_address((uint8_t *)address, addr);
    nRF24L01_write_register(rf, TX_ADDR, addr, 5);
}

void nRF24L01_transmit(nRF24L01 *rf, void *msg) {
    
    // Správa do FIFO
    nRF24L01_clear_transmit_interrupts(rf);
    nRF24L01_send_command(rf, W_TX_PAYLOAD, msg, 32);
    
    // PRIM_RX = 0
    uint8_t config;
    nRF24L01_read_register(rf, CONFIG, &config, 1);
    config &= ~_BV(PRIM_RX);
    nRF24L01_write_register(rf, CONFIG, &config, 1);
    
    // CE impulz > 10us Prechod do TX
    set_high(rf->ce);
    _delay_us(11);
    set_low(rf->ce);
}

void nRF24L01_flush_transmit_message(nRF24L01 *rf) {
    nRF24L01_send_command(rf, FLUSH_TX, NULL, 0);
}

void nRF24L01_flush_recieve_message(nRF24L01 *rf) {
    nRF24L01_send_command(rf, FLUSH_RX, NULL, 0);
}



void nRF24L01_clear_interrupts(nRF24L01 *rf) {
    uint8_t data = _BV(RX_DR) | _BV(TX_DS) | _BV(MAX_RT);
    nRF24L01_write_register(rf, STATUS, &data, 1);
}

void nRF24L01_clear_transmit_interrupts(nRF24L01 *rf) {
    uint8_t data = _BV(TX_DS) | _BV(MAX_RT);
    nRF24L01_write_register(rf, STATUS, &data, 1);
}

void nRF24L01_clear_receive_interrupt(nRF24L01 *rf) {
    uint8_t data = _BV(RX_DR) | rf->status;
    nRF24L01_write_register(rf, STATUS, &data, 1);
}



static void copy_address(uint8_t *source, uint8_t *destination) {
    for (int i = 0; i < 5; i++)
        destination[i] = source[i];
}



inline static void set_as_output(gpio_pin pin) {
    volatile uint8_t *ddr = pin.port - 1;
    *ddr |= _BV(pin.pin);
}

inline static void set_as_input(gpio_pin pin) {
    volatile uint8_t *ddr = pin.port - 1;
    *ddr &= ~_BV(pin.pin);
}

inline static void set_high(gpio_pin pin) {
    *pin.port |= _BV(pin.pin);
}

inline static void set_low(gpio_pin pin) {
    *pin.port &= ~_BV(pin.pin);
}

static void spi_init(nRF24L01 *rf) {
    // set as master
    SPCR |= _BV(MSTR);
    // enable SPI
    SPCR |= _BV(SPE);
    // MISO pin automatically overrides to input
    set_as_output(rf->sck);
    set_as_output(rf->mosi);
    set_as_input(rf->miso);
    // SPI mode 0: Clock Polarity CPOL = 0, Clock Phase CPHA = 0
    SPCR &= ~_BV(CPOL);
    SPCR &= ~_BV(CPHA);
    // Clock 2X speed
    SPCR &= ~_BV(SPR0);
    SPCR &= ~_BV(SPR1);
    SPSR |= _BV(SPI2X);
    // most significant first (MSB)
    SPCR &= ~_BV(DORD);
}

static uint8_t spi_transfer(uint8_t data) {
    SPDR = data;
    while (!(SPSR & _BV(SPIF)));
    return SPDR;
}
