
#include "grifo_aesa_fw_if.h"

#include "grifo_fw_aesa_blob.h"

#include "grifo_fw_tag.h"


#include <string.h>

using namespace aesa;

namespace grifo_fw
{

static aesa_blob_t* aesa_default_memory_;

}

using namespace grifo_fw;

static inline aesa_blob_t* aesa_blob(aesa_blob_t* b)
{
    if (b)
        return b;
    return reinterpret_cast<aesa_blob_t*>(aesa_default_memory_);
}

/*
 * Local data
 */

static TagGenerator aesaTagGenerator;

//static uint8_t out_msg_counter=0;
//static uint8_t batch_counter;
static aesa_receive_callback_t aesa_rx_cb=NULL;

/*
 * Datatypes definition
 */
struct message_opaque_t
{
    aesa_tx_header_t header;
    uint32_t data[0];
};

static uint8_t gen_next_cookie()
{
    return aesaTagGenerator.getNextTag();
    //++out_msg_counter;
    //return (out_msg_counter & 0x7F) | batch_counter;
}

class AesaStream
{
public:
    uint8_t* d;
    unsigned int s;
    unsigned int pos;

    AesaStream(void* p, unsigned int max_size):
        d(reinterpret_cast<uint8_t*>(p)),
        s(max_size),
        pos(3)
    {
    }

    void push(uint8_t v)
    {
        d[pos]=v;
        if (pos==0)
        {
            pos=3;
            d+=4;
        }
        else
            --pos;
    }
    void push32(uint32_t v)
    {
        push(v & 0xFF);
        push((v>>8) & 0xFF);
        push((v>>16) & 0xFF);
        push((v>>24) & 0xFF);
    }
    void push16(uint16_t v)
    {
        push(v & 0xFF);
        push((v>>8) & 0xFF);
    }
    void push16(int16_t v)
    {
        push16(static_cast<uint16_t>(v));
    }
};

static message_opaque_t* aesa_alloc(aesa_blob_t* ld, unsigned int size_bytes, unsigned int synch, unsigned int reply_required, unsigned int timeout_us=0)
{
    ld=aesa_blob(ld);

    unsigned int cursor=ld->header.work_size;

    grifo_fw_block_raw_helper_t helper(ld->payload.data, sizeof ld->payload, cursor);

    unsigned int alloc_size=size_bytes;

    message_opaque_t* p=reinterpret_cast<message_opaque_t*>(helper.alloc_raw_block((unsigned int)sizeof(aesa_tx_header_t), alloc_size));
    if (!p)
        return 0;

    ld->header.work_size=helper.cursor;

    p->header.cw.raw=0;
    p->header.cw.fields.synch=synch;
    p->header.cw.fields.response=reply_required;
    p->header.cw.fields.payload_len=(size_bytes);

    if (timeout_us>0)
    {
        if (timeout_us<=2550)
        {
            const unsigned int lsb=10;
            unsigned int tmp=timeout_us/lsb;
            if ((tmp*lsb)<timeout_us)
                ++tmp;
            p->header.cw.fields.timeout=tmp;
            p->header.cw.fields.timeout_weight=0;
        }
        else
        {
            const unsigned int lsb=100000;
            unsigned int tmp=timeout_us/lsb;  //lsb=100ms
            if ((tmp*lsb)<timeout_us)
                ++tmp;
            p->header.cw.fields.timeout=tmp;
            p->header.cw.fields.timeout_weight=1;
        }
    }

    unsigned int wlen=(size_bytes+3)>>2;
    if (wlen>0)
        p->data[wlen-1]=0;

    return p;
}

void grifo_fw::aesa_payload_swap(uint8_t *dst, const void* payload, unsigned int size_bytes)
{
    unsigned int b=3;
    const uint8_t *src=reinterpret_cast<const uint8_t *>(payload);
    for(unsigned int i=0; i<size_bytes; ++i)
    {
        dst[b]=src[i];
        if (b==0)
        {
            dst+=4;
            b=3;
        }
        else
            --b;
    }
}

int grifo_fw::aesa_append_raw_message(aesa_blob_t* ld, unsigned int synch, unsigned int num_responses, const void* payload, unsigned int size_bytes)
{
    message_opaque_t* p=aesa_alloc(ld, size_bytes, synch, num_responses, 0);
    if (!p)
        return 1;
    aesa_payload_swap(
            reinterpret_cast<uint8_t *>(p->data[0]),
            reinterpret_cast<const uint8_t *>(payload),
            size_bytes);
    return 0;
#if 0
    ld=aesa_blob(ld);

    unsigned int cursor=ld->header.work_size;

    grifo_fw_block_raw_helper_t helper(ld->payload.data, sizeof ld->payload, cursor);

    unsigned int alloc_size=size_bytes;

    message_opaque_t* p=reinterpret_cast<message_opaque_t*>(helper.alloc_raw_block(sizeof(aesa_tx_header_t), alloc_size));
    if (!p)
        return 1;

    p->header.cw.raw=0;
    p->header.cw.fields.synch=synch;
    p->header.cw.fields.response=num_responses;
    p->header.cw.fields.payload_len=(size_bytes);
    unsigned int wlen=(size_bytes+3)>>2;
    if (wlen>0)
        p->data[wlen-1]=0;

    //const uint8_t *tmp=reinterpret_cast<const uint8_t *>(payload);
    //out_msg_counter=tmp[1];

    //endianess swap
    unsigned int b=3;
    const uint8_t *src=reinterpret_cast<const uint8_t *>(payload);
    uint8_t *dst=reinterpret_cast<uint8_t *>(&p->data[0]);
    for(unsigned int i=0; i<size_bytes; ++i)
    {
        dst[b]=src[i];
        if (b==0)
        {
            dst+=4;
            b=3;
        }
        else
            --b;
    }
    ld->header.work_size=helper.cursor;

    return 0;
#endif
}

int grifo_fw::aesa_append_aesa_fw_status(aesa_blob_t* ld, sync_option_t synch, unsigned int timeout)
{
    message_opaque_t* p=aesa_alloc(ld, AESA_READ_FIRMWARE_STATUS_CMD_SIZE, synch, 1, timeout);
    if (!p)
        return 0;

    unsigned char* payload=reinterpret_cast<unsigned char*>(p->data);

    AesaStream s(payload, AESA_READ_FIRMWARE_STATUS_CMD_SIZE);
    s.push(AESA_READ_FIRMWARE_STATUS_CMD);
    s.push(gen_next_cookie());

    return 0;
}

template<typename T_> struct message_template3_t
{
    uint8_t cmd;
    uint8_t counter;
    uint8_t destination;
    T_ payload;
};

void grifo_fw::aesa_blob_initialize(aesa_blob_t* ld)
{
    ld=aesa_blob(ld);
    ld->initialize();
}

unsigned int grifo_fw::aesa_blob_used_size(aesa_blob_t* ld)
{
    ld=aesa_blob(ld);
    unsigned int size=ld->header.work_size*4+sizeof(ld->header);
    return size;
}

int grifo_fw::aesa_blob_clear(aesa_blob_t* ld)
{
    ld=aesa_blob(ld);
    ld->clear();
    return 0;
}


int grifo_fw::aesa_append_calc_beam_pos(aesa_blob_t* ld, beam_calculation_t* beam_data, sync_option_t synch, unsigned int timeout)
{
    int16_t tmp;
    message_opaque_t* p=aesa_alloc(ld, BEAM_CALCULATION_CMD_SIZE, synch, 2, timeout);
    if (!p)
        return 0;

    unsigned char* payload=reinterpret_cast<unsigned char*>(p->data);

    AesaStream s(payload, BEAM_CALCULATION_CMD_SIZE);
    s.push(BEAM_CALCULATION_CMD);
    s.push(gen_next_cookie());
    s.push(0); // destination id: broadcast to all planks
    s.push16(beam_data->tx_freq);
    s.push16(beam_data->rx_freq);
 
    tmp=static_cast<int16_t>(beam_data->tx_u/BEAM_DIRECTION_FACTOR_LSB);
    s.push16(static_cast<uint16_t>(tmp));
    tmp=static_cast<int16_t>(beam_data->tx_v/BEAM_DIRECTION_FACTOR_LSB);
    s.push16(static_cast<uint16_t>(tmp));
    tmp=static_cast<int16_t>(beam_data->rx_u/BEAM_DIRECTION_FACTOR_LSB);
    s.push16(static_cast<uint16_t>(tmp));
    tmp=static_cast<int16_t>(beam_data->rx_v/BEAM_DIRECTION_FACTOR_LSB);
    s.push16(static_cast<uint16_t>(tmp));

    s.push16(beam_data->tx_ctrl_state_index);
    s.push(beam_data->tx_enable_pattern_index);
    s.push16(beam_data->rx_ctrl_state_index);
    s.push(beam_data->rx_enable_pattern_index);
 
    tmp=static_cast<int16_t>(beam_data->guard_rx_u/BEAM_DIRECTION_FACTOR_LSB);
    s.push16(static_cast<uint16_t>(tmp));
    tmp=static_cast<int16_t>(beam_data->guard_rx_v/BEAM_DIRECTION_FACTOR_LSB);
    s.push16(static_cast<uint16_t>(tmp));   

    s.push16(beam_data->guard_rx_ctrl_state_index);
    s.push(beam_data->guard_rx_enable_pattern_index);
    s.push16(beam_data->sync_divisor);
    s.push(beam_data->duty_ratio);
    return 0;
}


int grifo_fw::aesa_append_save_beam_pos(aesa_blob_t* ld, beam_calculation_t* beam_data, uint8_t index, sync_option_t synch, unsigned int timeout)
{
    int16_t tmp;
    message_opaque_t* p=aesa_alloc(ld, SAVE_BEAM_CALCULATION_CMD_SIZE, synch, 2, timeout);
    if (!p)
        return 0;

    unsigned char* payload=reinterpret_cast<unsigned char*>(p->data);

    AesaStream s(payload, SAVE_BEAM_CALCULATION_CMD_SIZE);
    s.push(SAVE_BEAM_CALCULATION_CMD);
    s.push(gen_next_cookie());
    s.push(0); // destination id: broadcast to all planks
    s.push(index);
    s.push16(beam_data->tx_freq);
    s.push16(beam_data->rx_freq);
    tmp=static_cast<uint16_t>(beam_data->tx_u/BEAM_DIRECTION_FACTOR_LSB);
    s.push16(static_cast<uint16_t>(tmp));
    tmp=static_cast<uint16_t>(beam_data->tx_v/BEAM_DIRECTION_FACTOR_LSB);
    s.push16(static_cast<uint16_t>(tmp));
    tmp=static_cast<uint16_t>(beam_data->rx_u/BEAM_DIRECTION_FACTOR_LSB);
    s.push16(static_cast<uint16_t>(tmp));
    tmp=static_cast<uint16_t>(beam_data->rx_v/BEAM_DIRECTION_FACTOR_LSB);
    s.push16(static_cast<uint16_t>(tmp));

    s.push16(beam_data->tx_ctrl_state_index);
    s.push(beam_data->tx_enable_pattern_index);
    s.push16(beam_data->rx_ctrl_state_index);
    s.push(beam_data->rx_enable_pattern_index);
    tmp=static_cast<int16_t>(beam_data->guard_rx_u/BEAM_DIRECTION_FACTOR_LSB);
    s.push16(static_cast<uint16_t>(tmp));
    tmp=static_cast<int16_t>(beam_data->guard_rx_v/BEAM_DIRECTION_FACTOR_LSB);
    s.push16(static_cast<uint16_t>(tmp));
    s.push16(beam_data->guard_rx_ctrl_state_index);
    s.push(beam_data->guard_rx_enable_pattern_index);
    s.push16(beam_data->sync_divisor);
    s.push(beam_data->duty_ratio);
    return 0;
}

int grifo_fw::aesa_append_index_beam_pos(aesa_blob_t* ld, uint8_t index, sync_option_t synch, unsigned int timeout)
{
    message_opaque_t* p=aesa_alloc(ld, INDEX_BEAM_CALCULATION_CMD_SIZE, synch, 1, timeout);
    if (!p)
        return 0;

    unsigned char* payload=reinterpret_cast<unsigned char*>(p->data);

    AesaStream s(payload, INDEX_BEAM_CALCULATION_CMD_SIZE);
    s.push(INDEX_BEAM_CALCULATION_CMD);
    s.push(gen_next_cookie());
    s.push(0); // destination id: broadcast to all planks
    s.push(index);

    return 0;
}

int grifo_fw::aesa_append_cti_read_status(aesa_blob_t* ld, sync_option_t synch, unsigned int timeout)
{
    message_opaque_t* p=aesa_alloc(ld, CTI_READ_STATUS_CMD_SIZE, synch, 0, timeout);
    if (!p)
        return 0;

    unsigned char* payload=reinterpret_cast<unsigned char*>(p->data);

    AesaStream s(payload, CTI_READ_STATUS_CMD_SIZE);
    s.push(CTI_READ_STATUS_CMD);
    s.push(gen_next_cookie());

    return 0;
}

int grifo_fw::aesa_append_cti_read_extend_status(aesa_blob_t* ld, sync_option_t synch, unsigned int timeout)
{
    message_opaque_t* p=aesa_alloc(ld, CTI_READ_EXTENDED_STATUS_CMD_SIZE, synch, 1, timeout);
    if (!p)
        return 0;

    unsigned char* payload=reinterpret_cast<unsigned char*>(p->data);

    AesaStream s(payload, CTI_READ_EXTENDED_STATUS_CMD_SIZE);
    s.push(CTI_READ_EXTENDED_STATUS_CMD);
    s.push(gen_next_cookie());

    return 0;
}

int grifo_fw::aesa_append_rfif_eti_readback(aesa_blob_t* ld, sync_option_t synch, unsigned int timeout)
{
    message_opaque_t* p=aesa_alloc(ld, RFIF_ETI_READ_BACK_CMD_SIZE, synch, 1, timeout);
    if (!p)
        return 0;

    unsigned char* payload=reinterpret_cast<unsigned char*>(p->data);

    AesaStream s(payload, RFIF_ETI_READ_BACK_CMD_SIZE);
    s.push(RFIF_ETI_READ_BACK_CMD);
    s.push(gen_next_cookie());

    return 0;
}

int grifo_fw::aesa_append_rfif_nvm_read_data(aesa_blob_t* ld, uint32_t start_address, uint16_t num_bytes, sync_option_t synch, unsigned int timeout)
{
    message_opaque_t* p=aesa_alloc(ld, RFIF_NVM_READ_DATA_CMD_SIZE, synch, 1, timeout);
    if (!p)
        return 0;

    unsigned char* payload=reinterpret_cast<unsigned char*>(p->data);

    AesaStream s(payload, RFIF_NVM_READ_DATA_CMD_SIZE);
    s.push(RFIF_NVM_READ_DATA_CMD);
    s.push(gen_next_cookie());
    s.push32(start_address);
    s.push16(num_bytes);

    return 0;
}

int grifo_fw::aesa_append_rfif_nvm_erase_data(aesa_blob_t* ld, uint32_t start_address, uint16_t num_bytes, sync_option_t synch, unsigned int timeout)
{
    message_opaque_t* p=aesa_alloc(ld, RFIF_NVM_ERASE_DATA_CMD_SIZE, synch, 1, timeout);
    if (!p)
        return 0;

    unsigned char* payload=reinterpret_cast<unsigned char*>(p->data);

    AesaStream s(payload, RFIF_NVM_ERASE_DATA_CMD_SIZE);
    s.push(RFIF_NVM_ERASE_DATA_CMD);
    s.push(gen_next_cookie());
    s.push32(start_address);
    s.push16(num_bytes);

    return 0;
}

int grifo_fw::aesa_append_rfif_nvm_write_data(aesa_blob_t* ld, uint32_t start_address, uint16_t num_bytes, const char* data, sync_option_t synch, unsigned int timeout)
{
    message_opaque_t* p=aesa_alloc(ld, RFIF_NVM_WRITE_DATA_CMD_SIZE, synch, 1, timeout);
    if (!p)
        return 0;

    unsigned char* payload=reinterpret_cast<unsigned char*>(p->data);

    if (num_bytes>8)
        num_bytes=8;

    AesaStream s(payload, RFIF_NVM_WRITE_DATA_CMD_SIZE);
    s.push(RFIF_NVM_WRITE_DATA_CMD);
    s.push(gen_next_cookie());
    s.push32(start_address);
    s.push16(num_bytes);
    for (int i=num_bytes-1; i>=0; i--)
        s.push(data[i]);

    return 0;
}

int grifo_fw::aesa_append_plank_read_extended_status(aesa_blob_t* ld, uint8_t plank_id, sync_option_t synch, unsigned int timeout)
{
    message_opaque_t* p=aesa_alloc(ld, PLANK_READ_EXTENDED_STATUS_CMD_SIZE, synch, 1, timeout);
    if (!p)
        return 0;

    unsigned char* payload=reinterpret_cast<unsigned char*>(p->data);

    AesaStream s(payload, PLANK_READ_EXTENDED_STATUS_CMD_SIZE);
    s.push(PLANK_READ_EXTENDED_STATUS_CMD);
    s.push(gen_next_cookie());
    s.push(plank_id);

    return 0;
}

int grifo_fw::aesa_append_plank_vtx_control(aesa_blob_t* ld, uint8_t plank_id, plank_vtx_control_t data, sync_option_t synch, unsigned int timeout)
{
    message_opaque_t* p=aesa_alloc(ld, PLANK_VTX_CONTROL_CMD_SIZE, synch, 1, timeout);
    if (!p)
        return 0;

    unsigned char* payload=reinterpret_cast<unsigned char*>(p->data);

    AesaStream s(payload, PLANK_VTX_CONTROL_CMD_SIZE);
    s.push(PLANK_VTX_CONTROL_CMD);
    s.push(gen_next_cookie());
    s.push(plank_id);
    s.push(data.byte_0.raw);

    return 0;
}

int grifo_fw::aesa_append_plank_trc_initialise(aesa_blob_t* ld, uint8_t plank_id, sync_option_t synch, unsigned int timeout)
{
    message_opaque_t* p=aesa_alloc(ld, PLANK_TRC_INITIALISE_CMD_SIZE, synch, 1, timeout);
    if (!p)
        return 0;

    unsigned char* payload=reinterpret_cast<unsigned char*>(p->data);

    AesaStream s(payload, PLANK_TRC_INITIALISE_CMD_SIZE);
    s.push(PLANK_TRC_INITIALISE_CMD);
    s.push(gen_next_cookie());
    s.push(plank_id);

    return 0;
}

int grifo_fw::aesa_append_plank_trc_read_status(aesa_blob_t* ld, uint8_t plank_id, uint8_t trcn, sync_option_t synch, unsigned int timeout)
{
    message_opaque_t* p=aesa_alloc(ld, 4, synch, 1, timeout);
    if (!p)
        return 0;

    unsigned char* payload=reinterpret_cast<unsigned char*>(p->data);

    AesaStream s(payload, 4);
    s.push(0xe1);
    s.push(gen_next_cookie());
    s.push(plank_id);
    s.push(trcn);

    return 0;
}

int grifo_fw::aesa_append_plank_read_status(aesa_blob_t* ld, uint8_t plank_id, sync_option_t synch, unsigned int timeout)
{
    message_opaque_t* p=aesa_alloc(ld, PLANK_READ_STATUS_CMD_SIZE, synch, 1, timeout);
    if (!p)
        return 0;

    unsigned char* payload=reinterpret_cast<unsigned char*>(p->data);

    AesaStream s(payload, PLANK_READ_STATUS_CMD_SIZE);
    s.push(PLANK_READ_STATUS_CMD);
    s.push(gen_next_cookie());
    s.push(plank_id);

    return 0;
}

int grifo_fw::aesa_append_plank_ibit_start(aesa_blob_t* ld, uint8_t plank_id, sync_option_t synch, unsigned int timeout)
{
    message_opaque_t* p=aesa_alloc(ld, PLANK_IBIT_START_CMD_SIZE, synch, 1, timeout);
    if (!p)
        return 0;

    unsigned char* payload=reinterpret_cast<unsigned char*>(p->data);

    AesaStream s(payload, PLANK_IBIT_START_CMD_SIZE);
    s.push(PLANK_IBIT_START_CMD);
    s.push(gen_next_cookie());
    s.push(plank_id);

    return 0;
}

int grifo_fw::aesa_append_rfif_read_status(aesa_blob_t* ld, sync_option_t synch, unsigned int timeout)
{
    message_opaque_t* p=aesa_alloc(ld, RFIF_READ_STATUS_CMD_SIZE, synch, 1, timeout);
    if (!p)
        return 0;

    unsigned char* payload=reinterpret_cast<unsigned char*>(p->data);

    AesaStream s(payload, RFIF_READ_STATUS_CMD_SIZE);
    s.push(RFIF_READ_STATUS_CMD);
    s.push(gen_next_cookie());

    return 0;
}

int grifo_fw::aesa_append_rfif_read_fw_status(aesa_blob_t* ld, sync_option_t synch, unsigned int timeout)
{
    message_opaque_t* p=aesa_alloc(ld, RFIF_READ_FW_STATUS_CMD_SIZE, synch, 1, timeout);
    if (!p)
        return 0;

    unsigned char* payload=reinterpret_cast<unsigned char*>(p->data);

    AesaStream s(payload, RFIF_READ_FW_STATUS_CMD_SIZE);
    s.push(RFIF_READ_FW_STATUS_CMD);
    s.push(gen_next_cookie());

    return 0;
}

int grifo_fw::aesa_append_rfif_read_bit_status(aesa_blob_t* ld, sync_option_t synch, unsigned int timeout)
{
    message_opaque_t* p=aesa_alloc(ld, RFIF_READ_BIT_STATUS_CMD_SIZE, synch, 1, timeout);
    if (!p)
        return 0;

    unsigned char* payload=reinterpret_cast<unsigned char*>(p->data);

    AesaStream s(payload, RFIF_READ_BIT_STATUS_CMD_SIZE);
    s.push(RFIF_READ_BIT_STATUS_CMD);
    s.push(gen_next_cookie());

    return 0;
}

int grifo_fw::aesa_append_aesa_download_map_all(aesa_blob_t* ld, sync_option_t synch, unsigned int timeout)
{
    message_opaque_t* p=aesa_alloc(ld, DOWNLOAD_MAP_CMD_SIZE, synch, 1, timeout);
    if (!p)
        return 0;

    unsigned char* payload=reinterpret_cast<unsigned char*>(p->data);

    AesaStream s(payload, DOWNLOAD_MAP_CMD_SIZE);
    s.push(DOWNLOAD_MAP_CMD);
    s.push(gen_next_cookie());
    s.push(1);
    s.push(MAX_NUMBER_OF_PLANKS);

    return 0;
}

int grifo_fw::aesa_append_aesa_read_status(aesa_blob_t* ld, uint8_t start_plank, uint8_t end_plank, sync_option_t synch, unsigned int timeout)
{
    message_opaque_t* p=aesa_alloc(ld, AESA_READ_STATUS_CMD_SIZE, synch, 1, timeout);
    if (!p)
        return 0;

    unsigned char* payload=reinterpret_cast<unsigned char*>(p->data);

    AesaStream s(payload, AESA_READ_STATUS_CMD_SIZE);
    s.push(AESA_READ_STATUS_CMD);
    s.push(gen_next_cookie());
    s.push(start_plank);
    s.push(end_plank);

    return 0;
}

//static aesa_cti_comm_status_t aesa_cti_comm_status;
static aesa_fw_comm_status_t aesa_fw_comm_status;

const grifo_fw::aesa_fw_comm_status_t& grifo_fw::aesa_get_fw_comm_status()
{
    return aesa_fw_comm_status;
}

struct grifo_fw::grifo_fw_block_raw_deserializer_t
{
    enum { k_som=0x5A5A5A5A, k_eob=0xA5A5A5A5};

    const uint32_t* memory;
    unsigned int max_size;
    unsigned int cursor;

    grifo_fw_block_raw_deserializer_t(const void* mem, unsigned int size, unsigned int crs=0):
        memory(reinterpret_cast<const uint32_t*>(mem)),
        max_size(size),
        cursor(crs)
    {
    }
    void attach(const void* mem, unsigned int size, unsigned int crs=0)
    {
        memory=reinterpret_cast<const uint32_t*>(mem);
        max_size=size;
        cursor=crs;
    }

    aesa_message_descriptor_t extract()
    {
        aesa_message_descriptor_t msg;
        msg.data=reinterpret_cast<const uint32_t*>(extract(&msg.len_word));
        if (msg.len_word>0)
        {
            msg.status_word=msg.data[msg.len_word-1];
            msg.payload_len=msg.status_word>>16;

            if (msg.payload_len>(msg.len_word-1)*4)
            {
                msg.sts=aesa_message_descriptor_t::len_error;
            }
            else if (msg.status_word & aesa_message_descriptor_t::timeout_on_rx_res)
                msg.sts=aesa_message_descriptor_t::timeout;
            else if (msg.status_word & aesa_message_descriptor_t::crc_mismatch_res)
                msg.sts=aesa_message_descriptor_t::crc_error;
            else
                msg.sts=aesa_message_descriptor_t::ok;
        }
        return msg;
    }

    const void* extract(unsigned int* len)
    {
        const uint32_t* p=memory;

        *len=0;
        if (p[cursor]!=grifo_fw_block_raw_helper_t::k_som)
            return 0;

        unsigned int start=++cursor;

        for(; cursor<max_size; ++cursor)
        {
            if ((p[cursor]==grifo_fw_block_raw_helper_t::k_som) || (p[cursor]==grifo_fw_block_raw_helper_t::k_eob))
            {
                *len=cursor-start;
                return &p[start];
            }
        }
        return 0;
    }
};

static grifo_fw_block_raw_deserializer_t global_deserializer(0,0);

aesa_deserializer::aesa_deserializer(const void* mem, unsigned int size)
{
    raw_deserializer=&global_deserializer;
    raw_deserializer->attach(mem, size);
}

aesa_message_descriptor_t aesa_deserializer::extract()
{
    aesa_message_descriptor_t d=raw_deserializer->extract();
    if (d.sts==aesa_message_descriptor_t::ok)
        ++aesa_fw_comm_status.good_message_counter;
    else
    {
        if (d.sts==aesa_message_descriptor_t::crc_error)
            ++aesa_fw_comm_status.crc_error_counter;
        if (d.sts==aesa_message_descriptor_t::timeout)
            ++aesa_fw_comm_status.tm_error_counter;
    }
    aesa_fw_comm_status.last_msg_status=d.sts;

    return d;
}

uint8_t grifo_fw::aesa_get_msg_counter()
{
    return aesaTagGenerator.getLastTag(); //out_msg_counter;
}

void grifo_fw::aesa_set_msg_counter(uint8_t counter)
{
    aesaTagGenerator.reset(counter); //out_msg_counter=counter;
}

int grifo_fw::aesa_append_cti_init_link(aesa_blob_t* ld,
                                        bool link_1_or_2,
                                        uint8_t init_msg_id,
                                        const char* init_bit_code1,
                                        const char* init_bit_code2,
                                        sync_option_t synch,
										unsigned int timeout)
{
    message_opaque_t* p=aesa_alloc(ld, CTI_INIT_LINK_CMD_SIZE, synch, 1, timeout);
    if (!p)
        return 0;

    unsigned char* payload=reinterpret_cast<unsigned char*>(p->data);

    AesaStream s(payload, CTI_INIT_LINK_CMD_SIZE);
    if (link_1_or_2)
        s.push(CTI_INIT_LINK_1_CMD);
    else
        s.push(CTI_INIT_LINK_2_CMD);
    s.push(gen_next_cookie());
    s.push(init_msg_id);
    for (int i=0; i<CTI_INIT_LINK_PATTERN_SIZE; i++)
        s.push(init_bit_code1[i]);
    for (int i=0; i<CTI_INIT_LINK_PATTERN_SIZE; i++)
        s.push(init_bit_code2[i]);

    return 0;
}

int grifo_fw::aesa_append_cti_nvm_write_enable(aesa_blob_t* ld, uint8_t key, uint8_t target, sync_option_t synch, unsigned int timeout)
{
    message_opaque_t* p=aesa_alloc(ld, CTI_NVM_WRITE_ENABLE_CMD_SIZE, synch, 1, timeout);
    if (!p)
        return 0;

    unsigned char* payload=reinterpret_cast<unsigned char*>(p->data);

    AesaStream s(payload, CTI_NVM_WRITE_ENABLE_CMD_SIZE);
    s.push(CTI_NVM_WRITE_ENABLE_CMD);
    s.push(gen_next_cookie());
    s.push(key);
    s.push(target);

    return 0;
}
/* data = 0 to disable all*/
int grifo_fw::aesa_append_cti_imu_control(aesa_blob_t* ld, uint8_t data, sync_option_t synch, unsigned int timeout)
{
    message_opaque_t* p=aesa_alloc(ld, 3, synch, 0, timeout);
    if (!p)
        return 0;

    unsigned char* payload=reinterpret_cast<unsigned char*>(p->data);

    AesaStream s(payload, 3);
    s.push(0x19);
    s.push(gen_next_cookie());
    s.push(data);
    return 0;
}


int grifo_fw::aesa_append_rfif_time_sync(aesa_blob_t* ld, const char* data, sync_option_t synch, unsigned int timeout)
{
    message_opaque_t* p=aesa_alloc(ld, RFIF_TIME_SYNC_CMD_SIZE, synch, 1, timeout);
    if (!p)
        return 0;

    unsigned char* payload=reinterpret_cast<unsigned char*>(p->data);

    AesaStream s(payload, RFIF_TIME_SYNC_CMD_SIZE);
    s.push(RFIF_TIME_SYNC_CMD);
    s.push(gen_next_cookie());
    for (int i=0; i<RFIF_TIME_SYNC_PAYLOAD_SIZE; i++)
        s.push(data[i]);

    return 0;
}

struct test_rfif_mode_t
{
    int ovr;
    rfif_mode_command_t cmd;
};

static test_rfif_mode_t autoMonitorable_rfifmode;

int grifo_fw::aesa_append_rfif_mode(aesa_blob_t* ld, rfif_mode_command_t* rfif_mode, sync_option_t synch, unsigned int timeout)
{
    message_opaque_t* p=aesa_alloc(ld, RFIF_MODE_CMD_SIZE, synch, 1, timeout);
    if (!p)
        return 0;

    if(autoMonitorable_rfifmode.ovr)
    {
        rfif_mode=&autoMonitorable_rfifmode.cmd;
    }else
    {
        memcpy(&autoMonitorable_rfifmode.cmd,rfif_mode,(unsigned int)sizeof(autoMonitorable_rfifmode.cmd));
    }
    

    unsigned char* payload=reinterpret_cast<unsigned char*>(p->data);

    AesaStream s(payload, RFIF_MODE_CMD_SIZE);
    s.push(RFIF_MODE_CMD);
    s.push(gen_next_cookie());
    // payload byte 0
    s.push(rfif_mode->mode               |
           rfif_mode->guard_sel<<4       |
           rfif_mode->radiation_on<<7);
    // payload byte 1
    s.push(rfif_mode->tx_sample          |
           rfif_mode->tx_rx_guard_sel<<1 |
           rfif_mode->guard_cal_sel<<2   |
           rfif_mode->cal_sel_a<<3       |
           rfif_mode->cal_sel_b<<4       |
           rfif_mode->detector_on_off<<5 |
           rfif_mode->amp_cal<<7);
    // payload byte 2
    s.push(rfif_mode->attenuator/RFIF_ATTENUATION_DB_LSB);
    // payload byte 3-4
    s.push16(rfif_mode->tx_frequency);
    // payload byte 5-6
    s.push16(rfif_mode->rx_frequency);

    return 0;
}

int grifo_fw::aesa_append_rf_end_to_end_test(aesa_blob_t* ld, rf_end_to_end_t* rf_etet, sync_option_t synch, unsigned int timeout)
{
    message_opaque_t* p=aesa_alloc(ld, RF_END_TO_END_TEST_CMD_SIZE, synch, 1, timeout);
    if (!p)
        return 0;

    unsigned char* payload=reinterpret_cast<unsigned char*>(p->data);

    AesaStream s(payload, RF_END_TO_END_TEST_CMD_SIZE);
    s.push(RF_END_TO_END_TEST_CMD);
    s.push(gen_next_cookie());
    s.push(rf_etet->test_mode);
    s.push(rf_etet->test_plank_id);
    s.push(rf_etet->test_trc_id);
    s.push(rf_etet->test_trc_gain);
    s.push(rf_etet->test_trc_phase);
    s.push(rf_etet->first_list_plank_id);
    s.push(rf_etet->first_list_trc_id);
    s.push(rf_etet->last_list_plank_id);
    s.push(rf_etet->last_list_trc_id);
    s.push(rf_etet->list_trc_gain);
    s.push(rf_etet->list_trc_phase);
    s.push(rf_etet->timeout & 0x00FF);
    s.push(rf_etet->timeout >> 8);
    s.push(rf_etet->plank_vtx_bcm_ctrl);

    return 0;
}

int grifo_fw::aesa_append_cti_array_error_log_write(aesa_blob_t* ld, uint8_t num_words, uint8_t* data, sync_option_t synch, unsigned int timeout)
{
    unsigned int size=3+num_words*2;
    message_opaque_t* p=aesa_alloc(ld, size, synch, 1, timeout);
    if (!p)
        return 0;

    unsigned char* payload=reinterpret_cast<unsigned char*>(p->data);

    AesaStream s(payload, size);
    s.push(CTI_ARRAY_ERROR_LOG_WRITE_CMD);
    s.push(gen_next_cookie());
    s.push(num_words-1); // 0 means 1 word
    for (int i=0; i<num_words*2; i++)
        s.push((data[i]));

    return 0;
}

int grifo_fw::aesa_append_cti_time_sync(aesa_blob_t* ld, uint8_t* data, sync_option_t synch, unsigned int timeout)
{
    message_opaque_t* p=aesa_alloc(ld, CTI_TIME_SYNC_CMD_SIZE, synch, 1, timeout);
    if (!p)
        return 0;

    unsigned char* payload=reinterpret_cast<unsigned char*>(p->data);

    AesaStream s(payload, CTI_TIME_SYNC_CMD_SIZE);
    s.push(CTI_TIME_SYNC_CMD);
    s.push(gen_next_cookie());
    for (int i=0; i<6; i++)
        s.push(data[i]);

    return 0;
}

void grifo_fw::aesa_memory_updated(const void *src, unsigned int max_size)
{
    static uint8_t buffer[AESA_RX_MESSAGE_MAX_SIZE];
    struct aesa_receive_info_t aesa_rx_info;

    aesa_deserializer deserializer(src, max_size);
    for (aesa_message_descriptor_t msg=deserializer.extract(); msg.len_word>0; msg=deserializer.extract())
    {
        aesa_payload_swap(buffer, msg.data, (msg.len_word-1)*4);
        if (aesa_rx_cb)
        {
            // fill parameters
            aesa_rx_info.rx_buffer=buffer;
            aesa_rx_info.length=(msg.len_word-1)*4;
            aesa_rx_info.payload_len=msg.payload_len;
            aesa_rx_info.status=msg.sts;
            // activate callback
            aesa_rx_cb(&aesa_rx_info);
        }
    }
}

int grifo_fw::aesa_register_rx_callback(aesa_receive_callback_t rx_cb)
{
    aesa_rx_cb=rx_cb;

    return 0;
}

int grifo_fw::aesa_deregister_rx_callback()
{
    if (aesa_rx_cb)
        aesa_rx_cb=NULL;

    return 0;
}

void grifo_fw::aesa_initialize_raw_default_blob(void* p)
{
    aesa_default_memory_=reinterpret_cast<aesa_blob_t*>(p);
}

grifo_fw::aesa_blob_t* grifo_fw::aesa_get_default_blob()
{
    return aesa_default_memory_;
}

int grifo_fw::aesa_append_rfif_cfgmem_read_data(aesa_blob_t* ld, uint32_t start_address, uint16_t num_bytes, sync_option_t synch, unsigned int timeout)
{
    message_opaque_t* p=aesa_alloc(ld, RFIF_CFGMEM_READ_DATA_CMD_SIZE, synch, 1, timeout);
    if (!p)
        return 0;

    unsigned char* payload=reinterpret_cast<unsigned char*>(p->data);

    AesaStream s(payload, RFIF_CFGMEM_READ_DATA_CMD_SIZE);
    s.push(RFIF_CFGMEM_READ_DATA_CMD);
    s.push(gen_next_cookie());
    s.push32(start_address);
    s.push16(num_bytes);

    return 0;
}

int grifo_fw::aesa_append_rfif_cfgmem_write_data(aesa_blob_t* ld, uint32_t start_address, uint16_t num_bytes, const char* data, sync_option_t synch, unsigned int timeout)
{
    message_opaque_t* p=aesa_alloc(ld, RFIF_CFGMEM_WRITE_DATA_CMD_SIZE, synch, 1, timeout);
    if (!p)
        return 0;

    unsigned char* payload=reinterpret_cast<unsigned char*>(p->data);

    /*if (num_bytes>8)
        num_bytes=8;*/

    AesaStream s(payload, RFIF_CFGMEM_WRITE_DATA_CMD_SIZE);
    s.push(RFIF_CFGMEM_WRITE_DATA_CMD);
    s.push(gen_next_cookie());
    s.push32(start_address);
    s.push16(num_bytes);
    for (int i=num_bytes-1; i>=0; i--)
        s.push(data[i]);

    return 0;
}

int grifo_fw::aesa_append_rfif_cfgmem_erase_data(aesa_blob_t* ld, uint32_t start_address, uint16_t num_bytes, sync_option_t synch, unsigned int timeout)
{
    message_opaque_t* p=aesa_alloc(ld, RFIF_CFGMEM_ERASE_DATA_CMD_SIZE, synch, 1, timeout);
    if (!p)
        return 0;

    unsigned char* payload=reinterpret_cast<unsigned char*>(p->data);

    AesaStream s(payload, RFIF_CFGMEM_ERASE_DATA_CMD_SIZE);
    s.push(RFIF_CFGMEM_ERASE_DATA_CMD);
    s.push(gen_next_cookie());
    s.push32(start_address);
    s.push16(num_bytes);

    return 0;
}

