
#include "grifo_ctrlink_if.h"

#include "grifo_fw_tag.h"
#include "grifo_core_fw_cx.h"

#include "grifo_ctrlink_common.h"

#include <string.h>

using namespace grifo_fw;

static grifo_fw::TagGenerator ctrlinkTagGenerator;
 
// static fpga_info_t autoMonitorable_fpga_infos[(ctrlink_exciter+1)-ctrlink_dc_1];
struct autoMonitorable_fpga_info_t
{
    fpga_info_t fpga_info[(ctrlink_exciter+1)-ctrlink_dc_1];
    ctrlink_reply_decode_t::msg_sts_t link_status;
    bool board_fail[(ctrlink_exciter+1)-ctrlink_dc_1];
};
static autoMonitorable_fpga_info_t autoMonitorable_fpga_info; 
ctrlink_reply_decode_t::msg_sts_t grifo_fw::ctrlink_status_payload(ctrlink_board_address_t board,fpga_info_t& fpga_info,bool& fail)
{
    fpga_info = autoMonitorable_fpga_info.fpga_info[board-ctrlink_dc_1];
    fail = autoMonitorable_fpga_info.board_fail[board-ctrlink_dc_1];
    return autoMonitorable_fpga_info.link_status; 
}

void grifo_fw::ctrlink_bite_prepare()
{
    for(size_t i=0;i<sizeof(autoMonitorable_fpga_info.board_fail)/sizeof(autoMonitorable_fpga_info.board_fail[0]);++i)
    {
        autoMonitorable_fpga_info.board_fail[i]=true;
    }
    autoMonitorable_fpga_info.link_status=ctrlink_reply_decode_t::msg_err;
}
grifo_fw::ctrlink_blob_t* grifo_fw::ctrlink_default_blob_[ctrlink_num_instance];

ctrlink_blob_t*& grifo_fw::ctrlink_receiver_bus=grifo_fw::ctrlink_default_blob_[ctrlink_receiver_instance];
ctrlink_blob_t*& grifo_fw::ctrlink_exciter_bus=grifo_fw::ctrlink_default_blob_[ctrlink_exciter_instance];

static ctrlink_blob_t* validate_blob(ctrlink_blob_t* ld)
{
    return ctrlink_validate_blob(ld);
}

#if 0
enum ctrlink_pingpong_t
{
    ctrlink_pingpong_aynch=0,
    ctrlink_pingpong_a=1,
    ctrlink_pingpong_b=2,
    ctrlink_pingpong_auto
};

enum function_id_t
{
    fid_status=0x10,

    fid_lo_frq=0x50,
    fid_stc=0x60,
    fid_nagc=0x70

};

static inline unsigned int ctrlink_address(unsigned int board, unsigned int funct, ctrlink_pingpong_t pingpong_mode)
{
    unsigned int pingpong=0;
    if (pingpong_mode<ctrlink_pingpong_auto)
        pingpong=static_cast<unsigned int>(pingpong_mode);
    else
    {
        unsigned int area=static_cast<unsigned int>(GrifoCoreFwContext::freeArea());
        pingpong=area+1;
    }
    uint32_t d=funct & 0x03FF;  //9 bits
    d|=(board & 0x0FF)<<12;
    d|=(pingpong & 0x3);
    return d;
}
#endif


void grifo_fw::ctrlink_initialize_default_blob(grifo_fw::ctrlink_instance_id_t id, ctrlink_blob_t* ld)
{
    ctrlink_default_blob_[id]=ld;
    if (ld)
    {
        ld->initialize();
        ld->clear();
    }
}

void grifo_fw::ctrlink_rcv_initialize_raw_default_blob(void* p)
{
   ctrlink_initialize_default_blob(ctrlink_receiver_instance, reinterpret_cast<ctrlink_blob_t*>(p));
}

void grifo_fw::ctrlink_exc_initialize_raw_default_blob(void* p)
{
   ctrlink_initialize_default_blob(ctrlink_exciter_instance, reinterpret_cast<ctrlink_blob_t*>(p));
}

static void bclear(ctrlink_blob_t* ld)
{
    ld=validate_blob(ld);
    ld->header.work_size=0;
    ld->payload.data[0]=grifo_fw_block_raw_helper_t::k_eob;
    ld->payload.data[1]=grifo_fw_block_raw_helper_t::k_eob;
    ld->payload.data[2]=grifo_fw_block_raw_helper_t::k_eob;
    ld->payload.data[3]=grifo_fw_block_raw_helper_t::k_eob;
}

unsigned int grifo_fw::ctrlink_used_size(ctrlink_blob_t* ld)
{
    ld=validate_blob(ld);
    unsigned int size=ld->header.work_size*4+sizeof(ld->header);
    return size;
}

int grifo_fw::ctrlink_clear(ctrlink_blob_t* ld)
{
    if (ld==0)
    {
        ld=grifo_fw::ctrlink_receiver_bus;
        bclear(ld);
        ld=grifo_fw::ctrlink_exciter_bus;
        bclear(ld);
    }
    else
        bclear(ld);
/*
    ld=validate_blob(ld);
    ld->header.work_size=0;
    ld->payload.data[0]=grifo_fw_block_raw_helper_t::k_eob;
    ld->payload.data[1]=grifo_fw_block_raw_helper_t::k_eob;
    ld->payload.data[2]=grifo_fw_block_raw_helper_t::k_eob;
    ld->payload.data[3]=grifo_fw_block_raw_helper_t::k_eob;
*/
    return 0;
}

struct message_opaque_t
{
    ctrlink_tx_header_t header;
    uint32_t data[0];
};

void* grifo_fw::ctrlink_append_raw_message(ctrlink_blob_t* ld, uint32_t dst, ctrlink_append_mode_t append_mode, const void* payload, unsigned int size_bytes, bool fw_synch)
{
    ld=validate_blob(ld);
    if (!ld)
        return 0;

    unsigned int cursor=ld->header.work_size;

    bool read=append_mode==ctrlink_append_read;

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

    unsigned int alloc_size=read ? 0 : size_bytes;

    message_opaque_t* p=reinterpret_cast<message_opaque_t*>(helper.alloc_raw_block((unsigned int)sizeof(ctrlink_tx_header_t), alloc_size));

    if (!p)
        return 0;

    p->header.destination=dst;
    p->header.cw.raw=0;
    p->header.cw.fields.tag=ctrlinkTagGenerator.getNextTag(); //tag++;
    p->header.cw.fields.rw=read;
    p->header.cw.fields.synch=fw_synch ? 1 : 0; //WARNING: this should be always false. It is about the FPGA FW operation, not about the destination board operation
    p->header.cw.fields.payload_len=(size_bytes+3)>>2;
    if (!read && payload)
        memcpy(p->data, payload, size_bytes);

    ld->header.work_size=helper.cursor;

    return p->data;
}

int grifo_fw::ctrlink_append_status_request(ctrlink_blob_t* ld, ctrlink_board_address_t board)
{
    unsigned int b_start=static_cast<unsigned int>(board);
    unsigned int b_stop=b_start+1;
    if (board==ctrlink_all)
    {
        b_start=ctrlink_dc_1;
        b_stop=ctrlink_synth+1;
    }
    void* r=0;
    for(unsigned int n=b_start; n<b_stop; ++n)
    {
        uint32_t dst=ctrlink_address(n, fid_status, ctrlink_pingpong_aynch);
        r=ctrlink_append_raw_message(ld, dst, ctrlink_append_read, 0, sizeof(fpga_info_t)); //0x40*4);
    }
    return r!=0;
}

struct grifo_fw_common_frq_opr_msg_t
{
    enum { k_max_values=121, k_max_words=(k_max_values+3)>>2};

    uint16_t num_values;
    uint16_t frq[k_max_words];
};

int grifo_fw::ctrlink_append_lo_frequency_operative_message(ctrlink_blob_t* ld, unsigned int board, unsigned int num_values, const unsigned int *frq /*[num_values]*/, ctrlink_pingpong_t pingpong)
{

	grifo_fw_common_frq_opr_msg_t* msg=ctrlink_msg_alloc<grifo_fw_common_frq_opr_msg_t>(ld, board, fid_lo_frq, pingpong); //ctrlink_pingpong_auto);
    if (!msg)
        return 1;

    msg->num_values=num_values;

    msg->frq[0]=frq[0];
    unsigned int n=(num_values+3)>>2;
    unsigned int fp=1;
    for(unsigned int i=1; i<n; ++i, fp+=4)
    {
        const unsigned int* f=&frq[fp];
        uint16_t tmp=
                f[0]|(f[1]<<4)|(f[2]<<8)|(f[3]<<12);
        msg->frq[i]=tmp;
    }
    for(unsigned int i=n; i<grifo_fw_common_frq_opr_msg_t::k_max_words; ++i)
    {
        msg->frq[i]=msg->frq[n-1];
    }

    return 0;
}

struct grifo_fw_common_frq_msg_t
{
    uint8_t frq;
    uint8_t spare;
    uint16_t spare2;
};

int grifo_fw::ctrlink_append_lo_frequency_asynch_message(ctrlink_blob_t* ld, unsigned int board, unsigned int frq)
{
	grifo_fw_common_frq_msg_t* msg=ctrlink_msg_alloc<grifo_fw_common_frq_msg_t>(ld, board, fid_lo_frq, ctrlink_pingpong_aynch);
    if (!msg)
        return 1;
    msg->frq=frq;

    msg->spare=0;
    msg->spare2=0;

    return 0;
}

struct ctrlink_rx_cw_fileds_t
{
    unsigned int rw: 1;    //0=read
    unsigned int synch: 1; //0=asynch
    unsigned int spare: 2; //0=no skip
    unsigned int adr_err: 1;
    unsigned int last_ch: 1;
    unsigned int broadcast: 1;
    unsigned int spare2: 1;

    unsigned int tag: 8;

    unsigned int payload_len: 16;
};

union ctrlink_rx_cw_t
{
    uint32_t raw;
    ctrlink_tx_cw_fileds_t fields;
};

struct ctrlink_rx_header_t
{
    uint32_t destination;
    ctrlink_rx_cw_t cw;
};
#if 0
int grifo_fw::ctrlink_check_reply(ctrlink_reply_decode_t* result, const void* p)
{
    //static unsigned int last_tag;

    const uint32_t* base=reinterpret_cast<const uint32_t*>(p);

    //memset(result, 0, sizeof *result);
    result->sts=ctrlink_reply_decode_t::msg_err;
    result->err_msg="???";
    ++result->checked;

    if (base[0]!=grifo_fw_block_raw_helper_t::k_som)
    {
        result->tag=0;
        result->err_msg="missed SOM";
        result->sts=ctrlink_reply_decode_t::msg_missed;
        ++result->total_fails;
        ++result->con_fails;
    }
    else
    {
        const ctrlink_rx_header_t* h=reinterpret_cast<const ctrlink_rx_header_t*>(&base[1]);
        if (h->cw.raw==result->tag)
        {
            result->sts=ctrlink_reply_decode_t::msg_err;
            result->err_msg="TAG";
            ++result->total_fails;
            ++result->con_fails;
        }
        else
        {
            result->tag=h->cw.raw;
            result->sts=ctrlink_reply_decode_t::msg_ok;
            result->err_msg="OK";
            result->con_fails=0;
        }
    }
    return result->sts;
}
#endif

int grifo_fw::ctrlink_check_reply(ctrlink_reply_decode_t* result, const void* p)
{
    //static unsigned int last_tag;

    const uint32_t* base=reinterpret_cast<const uint32_t*>(p);
    bool flag_exit=false;
    //memset(result, 0, sizeof *result);
    result->sts=ctrlink_reply_decode_t::msg_err;
    result->err_msg="???";
    ++result->checked;
    if(result->checked==1)
    {
        return 0;
    }
    int i=0;
    for(i=0;!flag_exit && i<1024;)
    {
        if (base[i]!=grifo_fw_block_raw_helper_t::k_som)
        {
            result->tag=0;
            result->err_msg="missed SOM";
            result->sts=ctrlink_reply_decode_t::msg_missed;
            ++result->total_fails;
            ++result->con_fails;
            flag_exit=true;
        }
        else
        {
            const ctrlink_rx_header_t* h=reinterpret_cast<const ctrlink_rx_header_t*>(&base[i+1]);
            if (h->cw.raw==result->tag)
            {
                result->sts=ctrlink_reply_decode_t::msg_err;
                result->err_msg="TAG";
                ++result->total_fails;
                ++result->con_fails;
                flag_exit=true;
            }
            else
            {
                result->tag=h->cw.raw;
                result->sts=ctrlink_reply_decode_t::msg_ok;
                result->err_msg="OK";
                result->con_fails=0;
                if(h->cw.fields.rw==0 && (h->destination & 0x3FF0000)==0x100000) /* extract status */
                {
                    uint32_t board = (h->destination>>28) & 0xf;
                    if(board<=ctrlink_exciter && board>=ctrlink_dc_1 )
                    {
                        board-=ctrlink_dc_1;
                        memcpy(&autoMonitorable_fpga_info.fpga_info[board],&base[i+3],sizeof(autoMonitorable_fpga_info.fpga_info[0]));
                        autoMonitorable_fpga_info.board_fail[board]=false;
                    } 
                }

                if(h->cw.fields.rw)
                {
                    i+=4;    
                }
                else
                {
                    i+=4;
                    i+=h->cw.fields.payload_len;
                }

            }
        }
        if(base[i]==grifo_fw_block_raw_helper_t::k_eob)
        {
            break;
        }
    }
    if(i>=1024)
    {
        result->sts=ctrlink_reply_decode_t::msg_err;
        result->err_msg="TAG";
        ++result->total_fails;
        ++result->con_fails;
    }
    autoMonitorable_fpga_info.link_status=static_cast<ctrlink_reply_decode_t::msg_sts_t>(result->sts);
    return result->sts;
}

