#include <avr/interrupt.h>
#define HARDWARERADIO_CPP (1)
#include "HardwareRadio.h"

#define BUFFER_FREE (0)
#define BUFFER_USED (0x80)

#define BUFFER_POOL_SIZE (8)

radio_buffer_t buffer_pool[BUFFER_POOL_SIZE];
radio_buffer_t *tx_queue;
radio_buffer_t *rx_queue;


extern "C"
{
    void push_begin(radio_buffer_t **qhead, radio_buffer_t *buf);
    void push_end(radio_buffer_t **qhead, radio_buffer_t *buf);
    radio_buffer_t * pop_begin(radio_buffer_t **qhead);
    radio_buffer_t * pop_end(radio_buffer_t **qhead);
}



HardwareRadio::HardwareRadio(void)
{}

void HardwareRadio::begin(void)
{
    this->begin(17, STATE_RX);
}


void HardwareRadio::begin(uint8_t channel, uint8_t idlestate)
{

uint8_t i;

    for(i=0; i<BUFFER_POOL_SIZE; i++)
    {
        /* Mark buffer as empty */
        buffer_pool[i].len = 0;
        buffer_pool[i].next = NULL;
    }

    tx_queue = NULL;
    txbuf = NULL;
    pwrbuf = alloc_buffer();
    txbuffer_init(pwrbuf);
    txseq = 0;
    tx_in_progress  = 0;

    rx_queue = NULL;
    prdbuf = NULL;
    rxbuf = alloc_buffer();
    radio_init(rxbuf->frm, 127);

    radio_set_param(RP_CHANNEL(channel));
    radio_set_param(RP_IDLESTATE((radio_state_t)idlestate));
    radio_set_state((radio_state_t)idlestate);
}


radio_buffer_t * HardwareRadio::alloc_buffer(void)
{
    radio_buffer_t *pret = NULL;
    uint8_t i;

    for(i=0; i< BUFFER_POOL_SIZE; i++)
    {
        if(buffer_pool[i].len == 0)
        {
            buffer_pool[i].len = BUFFER_USED;
            pret = &buffer_pool[i];
            break;
        }
    }
    return pret;
}

void HardwareRadio::free_buffer(radio_buffer_t * pbuf)
{
    if (pbuf != NULL)
    {
        pbuf->len = 0;
        pbuf->next = NULL;
    }
}


void HardwareRadio::write(uint8_t byte)
{

    this->pwrbuf->frm[this->pwrbuf->idx] = byte;

    this->pwrbuf->idx++;
    if (this->pwrbuf->idx > 125)
    {
        this->flush();
    }
}

void HardwareRadio::write(char * str)
{
uint8_t slen, i;
char * p;
    slen = strlen(str);
    p = str;
    for(i=0;i<slen;i++)
    {
        pwrbuf->frm[pwrbuf->idx] = *p++;
        pwrbuf->idx++;
        if (pwrbuf->idx > 125)
        {
            flush();
        }
    }
}

void HardwareRadio::put(int16_t val)
{
    typedef struct {
        uint8_t id;
        int16_t val;
    } ser_int_t;
    ser_int_t *p;

    if ((pwrbuf->idx + 3) > 125)
    {
        flush();
    }
    p = (ser_int_t *) & (pwrbuf->frm[pwrbuf->idx]);
    p->id = 0x81;
    p->val = val;
    pwrbuf->idx += 3;
    

}


void HardwareRadio::get_int(int16_t& value)
{
    value = *((int16_t*)&prdbuf->frm[prdbuf->idx]);
    prdbuf->idx += 2;
}


void HardwareRadio::flush(void)
{

    if (pwrbuf->idx > 3)
    {
        pwrbuf->len = pwrbuf->idx + 2;
        push_end(&tx_queue, pwrbuf);

        cli();
        pwrbuf = alloc_buffer();
        txbuffer_init(pwrbuf);
        sei();
    }


    if (tx_queue != NULL && tx_in_progress == 0)
    {
        tx_in_progress = 1;
        txbuf = pop_begin(&tx_queue);
        radio_set_state((radio_state_t) STATE_TXAUTO);
        radio_send_frame(txbuf->idx+2, txbuf->frm, 0);
    }
}


void HardwareRadio::txbuffer_init(radio_buffer_t *pbuf)
{
    pbuf->frm[0] = 1;
    pbuf->frm[1] = 0;
    pbuf->frm[2] = txseq++;
    pbuf->idx = 3;
}

int HardwareRadio::available(void)
{
    int ret = 0;

    /* try to get the next buffer from rx queue. */
    if (prdbuf == NULL)
    {
        cli();
        prdbuf = pop_end(&rx_queue);
        sei();
    }

    if (prdbuf != NULL)
    {
        ret = (prdbuf->len - prdbuf->idx - 2);
        if (ret <= 0)
        {
            cli();
            free_buffer(prdbuf);
            prdbuf = NULL;
            sei();
            ret = 0;
        }
    }
    return ret;
}

int HardwareRadio::read(void)
{
int ret = -1; /*EOF*/

    /* try to get the next buffer from rx queue. */
    if (prdbuf == NULL)
    {
        cli();
        prdbuf = pop_end(&rx_queue);
        sei();
    }

    /* is a buffer available ? */
    if (prdbuf != NULL)
    {
        if ((this->prdbuf->idx+2) < this->prdbuf->len)
        {
            ret = this->prdbuf->frm[this->prdbuf->idx];
            this->prdbuf->idx ++;
        }
        else
        {
            free_buffer(prdbuf);
            prdbuf = NULL;
        }
    }
    return ret;
}


uint8_t decode_154headerlen(uint8_t *frm)
{
uint8_t ret = 3;
    if (frm[1] & 8)
    {
        ret += 4;
    }
    if (frm[1] & 4)
    {
        ret += 6;
    }
    if (frm[1] & 0x80)
    {
        ret += 4;
    }
    if (frm[1] & 0x40)
    {
        ret += 6;
    }
    if ((frm[1] & 0xc0) != 0 && (frm[0] & 0x40) != 0)
    {
        ret -= 2;
    }
    return ret;

}

HardwareRadio Radio;

extern "C" uint8_t * usr_radio_receive_frame(uint8_t len, uint8_t *frm, uint8_t lqi, int8_t ed, uint8_t crc)
{
    radio_buffer_t * pbuf;
    if ((len > 5))
    {
        pbuf = Radio.alloc_buffer();
        if (pbuf != NULL)
        {
            /* another buffer is available, update the current one and
             * add it to the queue. */
            Radio.rxbuf->len = len;
            Radio.rxbuf->idx = decode_154headerlen(frm);
            push_begin(&rx_queue, Radio.rxbuf);
            Radio.rxbuf = pbuf;
            frm = pbuf->frm;
        }
        else
        {
            /* no more buffers, we drop this frame and reuse
             * the last buffer. */
            frm = Radio.rxbuf->frm;

        }
    }
    return frm;
}

/* This function is called from the Radio IRQ Context */
extern "C" void usr_radio_tx_done(radio_tx_done_t status)
{
radio_buffer_t * pbuf;
    /* remove buffer from list and free it. */
    /* tx_status could handled here, at least with some counters. */
    Radio.tx_in_progress = 0;
    Radio.free_buffer(Radio.txbuf);
}


extern "C" void usr_radio_error(radio_error_t err)
{}

/**
 */
extern "C" void push_begin(radio_buffer_t **qhead, radio_buffer_t *buf)
{
    if (*qhead == NULL)
    {
        *qhead = buf;
        buf->next = NULL;
    }
    else
    {
        buf->next = *qhead;
        *qhead = buf;
    }
}

extern "C" radio_buffer_t * pop_begin(radio_buffer_t **qhead)
{
    radio_buffer_t * ret = NULL;
    if (*qhead == NULL)
    {
        ret = NULL;
    }
    else
    {
        ret = *qhead;
        *qhead = (radio_buffer_t *)ret->next;
    }
    if (ret != NULL)
    {
        ret->next = NULL;
    }
    return ret;
}

extern "C" void push_end(radio_buffer_t **qhead, radio_buffer_t *buf)
{

    radio_buffer_t * next, *prev;
    buf->next = NULL;

    if (*qhead == NULL)
    {
        *qhead = buf;
    }
    else
    {

        for(prev = NULL, next = *qhead;
            next != NULL;
            prev = next, next = next->next)
        {
            /* run to end of queue */
        }
        prev->next = buf;
    }

}

extern "C" radio_buffer_t * pop_end(radio_buffer_t **qhead)
{
    radio_buffer_t * next, *prev, *ret = NULL;

    if (*qhead == NULL)
    {
        ret = NULL;
    }
    else
    {
        for(prev = NULL, next = *qhead;
            next->next != NULL;
            prev = next, next = next->next)
        {
            /* run to end of queue */
        }

        ret = next;

        if (ret == *qhead)
        {
            *qhead = NULL;
        }
        else
        {
            prev->next = NULL;
        }
    }

    if (ret != NULL)
    {
        ret->next = NULL;
    }
    return ret;
}


extern "C" void flush_queue(radio_buffer_t **qhead)
{
    radio_buffer_t * p;

    do
    {
        p = pop_begin(qhead);
        Radio.free_buffer(p);
    }
    while(p!=NULL);

}
