#include "cc110l.h"

#define CONFIG0 0x00
#define PATABLE 0x3E

#define SRES 0x30
#define SRX 0x34
#define STX 0x35
#define SNOP 0x3D
#define FIFO 0x3F

#define STATE_MASK 0x70
#define STATE_IDLE (0x00 << 4)
#define STATE_RX   (0x01 << 4)
#define STATE_TX   (0x02 << 4)

#define WRITE 0x00
#define BURST 0x40
#define READ 0x80

uint8_t debug[16] = {0xFF};
uint8_t spiret;
uint8_t spitran;

uint8_t spistatus;
uint8_t testdata;
uint8_t marcstate, pktstatus, rxbytes, txbytes;

/*
static uint8_t config_regs[] = {
    0x29, 0x2E, 0x06, 0x07, (RADIO_SYNC >> 8) & 0xFF, RADIO_SYNC & 0xFF, 0xFF, 0x04,
    0x05, RADIO_ADDR, RADIO_CHANNEL, 0x06, 0x00, 0x22, 0xB6, 0x27,
    0x2A, 0x83, 0x03, 0x21, 0xEE, 0x65, 0x07, 0x30,
    0x18, 0x16, 0x6C, 0x43, 0x40, 0x91, 0x87, 0x6B,
    0xF8, 0x56, 0x10, 0xE9, 0x2A, 0x00, 0x1F, 0x41,
    0x00, 0x59, 0x7F, 0x3F, 0x81, 0x35, 0x09
};
*/

static uint8_t config_regs[] = {
    0x2E, 0x2E, 0x06, 0x07, (RADIO_SYNC >> 8) & 0xFF, RADIO_SYNC & 0xFF, 0xFF, 0x04,
    0x05, RADIO_ADDR, RADIO_CHANNEL, 0x06, 0x00, 0x22, 0xB6, 0x27,
    0x2A, 0x83, 0x03, 0x21, 0xEE, 0x65, 0x07, 0x30,
    0x18, 0x16, 0x6C, 0x43, 0x40, 0x91, 0x87, 0x6B,
    0xF8, 0x56, 0x10, 0xE9, 0x2A, 0x00, 0x1F, 0x41,
    0x00, 0x59, 0x7F, 0x3F, 0x81, 0x35, 0x09
};

static void spi_begin() {
    RADIO_CLK_PORT &= ~RADIO_CLK_BIT;
    RADIO_CS_PORT &= ~RADIO_CS_BIT;
    __nop();

    while (RADIO_MISO_PORT & RADIO_MISO_BIT)
        __nop();
}

static inline void spi_end() {
    __nop();

    RADIO_CS_PORT |= RADIO_CS_BIT;
}

static uint8_t spi_comm(uint8_t byte) {
    uint8_t bit;

    for (bit = 8; bit; bit--) {
        if (byte & 0x80)
            RADIO_MOSI_PORT |= RADIO_MOSI_BIT;
        else
            RADIO_MOSI_PORT &= ~RADIO_MOSI_BIT;

        RADIO_CLK_PORT |= RADIO_CLK_BIT;
        __nop();

        byte <<= 1;
        
        if (RADIO_MISO_PORT & RADIO_MISO_BIT)
            byte |= 0x01;

        RADIO_CLK_PORT &= ~RADIO_CLK_BIT;
    }

    spiret = byte;
    spitran++;

    __nop();

    return byte;
}

static void delay_ms(uint16_t ms) {
    while (ms--)
        __delay_cycles(1000);
}

uint8_t patable;
uint8_t first_status;

static void spi_state_wait(uint8_t status) {
    __nop();
    first_status = spi_comm(SNOP);

    while ((spi_comm(SNOP) & STATE_MASK) != status)
        __nop();

    while (((spistatus = spi_comm(SNOP | READ)) & STATE_MASK) != STATE_IDLE) {
        P1OUT |= 0x10;

        spi_comm(0x35 | BURST | READ);
        marcstate = spi_comm(0x00);
        spi_comm(0x38 | BURST | READ);
        pktstatus = spi_comm(0x00);
        spi_comm(0x3A | BURST | READ);
        txbytes = spi_comm(0x00);
        spi_comm(0x3B | BURST | READ);
        rxbytes = spi_comm(0x00);

        spi_comm(0x31 | BURST | READ);
        testdata = spi_comm(0x00);

        delay_ms(100);
        P1OUT &= ~0x10;
        delay_ms(500);

        __nop();
    }

    spi_comm(0x3A | BURST | READ);
    txbytes = spi_comm(0x00);
    spi_comm(0x3B | BURST | READ);
    rxbytes = spi_comm(0x00);
}

uint8_t radio_get_status() {
    uint8_t status;

    spi_begin();
    status = spi_comm(0 | READ);
    spi_end();

    return status;
}

void radio_init() {
    uint8_t n;

    RADIO_CS_PORT |= RADIO_CS_BIT;
    __delay_cycles(30);
    RADIO_CS_PORT &= ~RADIO_CS_BIT;
    __delay_cycles(30);
    RADIO_CS_PORT |= RADIO_CS_BIT;
    __delay_cycles(45);

    spi_begin();
    spi_comm(SRES);
    spi_end();
    
    spi_begin();
    spi_comm(CONFIG0 | BURST | WRITE);
    for (n = 0; n < sizeof(config_regs); n++)
        spi_comm(config_regs[n]);
    spi_end();

    spi_begin();
    spi_comm(PATABLE | WRITE);
    spi_comm(0x60);
    spi_end();
}

uint8_t txb, txa, wstat1 = 0xFF, wstat2 = 0xFF, ldebug;

void radio_send(uint8_t *buffer, uint8_t length) {
    uint8_t i;

    ldebug = length;

    spi_begin();
    spi_comm(FIFO | BURST | WRITE);
    spi_comm(length);
    //spi_comm(RADIO_ADDR);
    for (i = 0; i < length; i++) {
        wstat2 = wstat1;
        wstat1 = spi_comm(buffer[i]);
    }
    spi_end();

    spi_begin();

    spi_comm(0x3A | BURST | READ);
    txb = spi_comm(0x00);

    spi_comm(STX);
    spi_state_wait(STATE_TX);

    spi_comm(0x3A | BURST | READ);
    txa = spi_comm(0x00);

    spi_end();
}

uint8_t state2 = 0xFF, state3 = 0xFF, state4 = 0xFF;
uint8_t where = 0;

uint8_t radio_recv(uint8_t *buffer) {
    uint8_t i, length;

    spi_begin();

    spi_comm(SRX);

    where = 1;
    
    while (1) {
        while (!(RADIO_PI_PORT & RADIO_PI_BIT)) ;
        where = 2;
        while (RADIO_PI_PORT & RADIO_PI_BIT) ;
        where = 3;
        if (spi_comm(SNOP | READ) & 0x0F)
            break;
        
        P1OUT ^= 0x01;
    }
    where = 4;

    //spi_state_wait(STATE_RX);
    where = 5;
    spi_comm(FIFO | BURST | READ);
    where = 6;
    length = spi_comm(0);
    //spi_comm(0x00); // address

    for (i = 0; i < length; i++)
        buffer[i] = spi_comm(0);

    spi_comm(0); // status 1
    spi_comm(0); // status 2

    spi_end();

    return length;
}