
#include "grifo_down_converter_if.h"

#include "grifo_ctrlink_common.h"

using namespace grifo_fw;


static inline ctrlink_board_address_t dc2ba(dc_board_t board)
{
    return static_cast<ctrlink_board_address_t>(static_cast<unsigned int>(ctrlink_dc_1)+static_cast<unsigned int>(board));
}

template<typename T_> static T_* msg_alloc_size(ctrlink_blob_t* ld, dc_board_t board, function_id_t fid, unsigned int size, T_* msg=0, ctrlink_pingpong_t pingpong=ctrlink_pingpong_auto, ctrlink_append_mode_t mode=ctrlink_append_write)
{
    unsigned int payload_size=size; //sizeof(T_);
    uint32_t dst=ctrlink_address(dc2ba(board), fid, pingpong); //ctrlink_pingpong_auto);

    T_* m=reinterpret_cast<T_*>(ctrlink_append_raw_message(ld, dst, mode, msg, payload_size));
    return m;
}

template<typename T_> static T_* msg_alloc_size_fw_synch(bool fw_synch, ctrlink_blob_t* ld, dc_board_t board, function_id_t fid, unsigned int size, T_* msg=0, ctrlink_pingpong_t pingpong=ctrlink_pingpong_auto, ctrlink_append_mode_t mode=ctrlink_append_write)
{
    unsigned int payload_size=size; //sizeof(T_);
    uint32_t dst=ctrlink_address(dc2ba(board), fid, pingpong); //ctrlink_pingpong_auto);

    T_* m=reinterpret_cast<T_*>(ctrlink_append_raw_message(ld, dst, mode, msg, payload_size, fw_synch));
    return m;
}

template<typename T_> static T_* msg_alloc(ctrlink_blob_t* ld, dc_board_t board, function_id_t fid, T_* msg=0, ctrlink_append_mode_t mode=ctrlink_append_write)
{
    unsigned int payload_size=sizeof(T_);
    uint32_t dst=ctrlink_address(dc2ba(board), fid, ctrlink_pingpong_auto);

    T_* m=reinterpret_cast<T_*>(ctrlink_append_raw_message(ld, dst, mode, msg, payload_size));
    return m;
}

template<typename T_> static T_* msg_alloc(ctrlink_blob_t* ld, dc_board_t board, function_id_t fid, ctrlink_pingpong_t pp, T_* msg=0, ctrlink_append_mode_t mode=ctrlink_append_write)
{
    unsigned int payload_size=sizeof(T_);
    uint32_t dst=ctrlink_address(dc2ba(board), fid, pp);

    T_* m=reinterpret_cast<T_*>(ctrlink_append_raw_message(ld, dst, mode, msg, payload_size));
    return m;
}


int grifo_fw::dc_append_status_request(ctrlink_blob_t* ld, dc_board_t board)
{
	return ctrlink_append_status_request(ld, dc2ba(board));
}

struct dc_ad_msg_t_
{
    uint16_t cfg;
};

int grifo_fw::dc_append_ad_control_message(ctrlink_blob_t* ld, dc_board_t board, dc_ad_ctrl_t mode)
{

    dc_ad_msg_t_* msg=msg_alloc<dc_ad_msg_t_>(ld, board, fid_ad_ctrl, ctrlink_pingpong_aynch);
    if (!msg)
        return 1;
    msg->cfg=mode & 0x7;
    msg=msg_alloc<dc_ad_msg_t_>(ld, board, fid_ad_enable, ctrlink_pingpong_aynch);
    msg->cfg=1;
    return 0;
}

struct dc_sw_msg_t_
{
    uint16_t cfg;
};

int grifo_fw::dc_append_sw_asynch_message(ctrlink_blob_t* ld, dc_board_t board, unsigned int sw)
{
    return dc_append_sw_operative_message(ld, board, sw, ctrlink_pingpong_aynch);
}
int grifo_fw::dc_append_sw_operative_message(ctrlink_blob_t* ld, dc_board_t board, unsigned int sw, ctrlink_pingpong_t mode)
{

    dc_sw_msg_t_* msg=msg_alloc<dc_sw_msg_t_>(ld, board, fid_sw_dc, mode); //mode ? ctrlink_pingpong_aynch : ctrlink_pingpong_auto);
    if (!msg)
        return 1;
    msg->cfg=sw & 0x1F;
    return 0;
}

struct dc_sct_msg_t_
{
    uint16_t num;
    uint8_t stc0;
    uint8_t spare;
    uint8_t stc[2044];
};

int grifo_fw::dc_append_stc_operative_message_synch_send(ctrlink_blob_t* ld, dc_board_t board, unsigned int num_values, const unsigned int* values, ctrlink_pingpong_t pingpong)
{
    if (num_values>=2045)
        num_values=2044;
    if (num_values<1)
        return 1;
    //Calculate real size
    unsigned int size_values=num_values-1;
    unsigned int size=size_values+4;
    size=((size+3)>>2)<<2;; //round to 32 bits. Is it really required?

    dc_sct_msg_t_* msg=msg_alloc_size_fw_synch<dc_sct_msg_t_>(true, ld, board, fid_stc, size, 0, pingpong);
    if (!msg)
        return 1;
    msg->num=num_values;
    msg->spare=values[0];
    msg->stc0=values[0];
    for(unsigned int i=1; i<num_values; ++i)
    {
        msg->stc[i-1]=values[i];
    }
    return 0;
}

int grifo_fw::dc_append_stc_operative_message(ctrlink_blob_t* ld, dc_board_t board, unsigned int num_values, const unsigned int* values, ctrlink_pingpong_t pingpong)
{

    if (num_values>=2045)
        num_values=2044;
    if (num_values<1)
        return 1;
    //Calculate real size
    unsigned int size_values=num_values-1;
    unsigned int size=size_values+4;
    size=((size+3)>>2)<<2;; //round to 32 bits. Is it really required?

    dc_sct_msg_t_* msg=msg_alloc_size<dc_sct_msg_t_>(ld, board, fid_stc, size, 0, pingpong);
    if (!msg)
        return 1;
    msg->num=num_values;
    msg->spare=values[0];
    msg->stc0=values[0];
    for(unsigned int i=1; i<num_values; ++i)
    {
        msg->stc[i-1]=values[i];
    }
    return 0;
}

struct dc_sct_asynch_msg_t_
{
    uint8_t stc0;
    uint8_t spare;
};

int grifo_fw::dc_append_stc_asynch_message(ctrlink_blob_t* ld, dc_board_t board, unsigned int values)
{

    unsigned int payload_size=sizeof(dc_sct_asynch_msg_t_);

    uint32_t dst=ctrlink_address(dc2ba(board), fid_stc, ctrlink_pingpong_aynch);

    dc_sct_asynch_msg_t_* msg=reinterpret_cast<dc_sct_asynch_msg_t_*>(ctrlink_append_raw_message(ld, dst, ctrlink_append_write, 0, payload_size));
    if (!msg)
        return 1;
    msg->stc0=values;
    msg->spare=values;

    return 0;
}

int grifo_fw::dc_append_nacg_asynch_message(ctrlink_blob_t* ld, dc_board_t board, unsigned int ch0, unsigned int ch1, unsigned int ch2, unsigned int ch3)
{
    struct nagc_msg_t
    {
        uint8_t nagc[4];
    };

    unsigned int payload_size=sizeof(nagc_msg_t);

    uint32_t dst=ctrlink_address(dc2ba(board), fid_nagc, ctrlink_pingpong_aynch);

    nagc_msg_t* msg=reinterpret_cast<nagc_msg_t*>(ctrlink_append_raw_message(ld, dst, ctrlink_append_write, 0, payload_size));
    if (!msg)
        return 1;
    msg->nagc[0]=ch0;
    msg->nagc[1]=ch1;
    msg->nagc[2]=ch2;
    msg->nagc[3]=ch3;

    return 0;
}

int grifo_fw::dc_append_nacg_asynch_message(ctrlink_blob_t* ld, dc_board_t board, unsigned int ch[4])
{
    return grifo_fw::dc_append_nacg_asynch_message(ld, board, ch[0], ch[1], ch[2], ch[3]);
}

int grifo_fw::dc_append_nacg_operative_message(ctrlink_blob_t* ld, dc_board_t board, unsigned int num_values, const unsigned int *ch, ctrlink_pingpong_t pingpong)
{
    struct nagc_msg_t
    {
        enum { k_max_values=2047};

        uint16_t num_values;
        uint16_t spare;
        uint8_t nagc[k_max_values][4];
    };

    //unsigned int payload_size=sizeof(nagc_msg_t);
    //Calculate real size
    unsigned int size_values=num_values;
    unsigned int size=(size_values*4)+4;
    unsigned int payload_size=((size+3)>>2)<<2;; //round to 32 bits. Is it really required?

    uint32_t dst=ctrlink_address(dc2ba(board), fid_nagc, pingpong); //ctrlink_pingpong_auto);

    nagc_msg_t* msg=reinterpret_cast<nagc_msg_t*>(ctrlink_append_raw_message(ld, dst, ctrlink_append_write, 0, payload_size));
    if (!msg)
        return 1;
    msg->num_values=num_values;
    msg->spare=0;

    for(unsigned int i=0; i<num_values; ++i, ch+=4)
    {
        msg->nagc[i][0]=ch[0];
        msg->nagc[i][1]=ch[1];
        msg->nagc[i][2]=ch[2];
        msg->nagc[i][3]=ch[3];
    }
#if 0
    for(unsigned int i=num_values; i<nagc_msg_t::k_max_values; ++i)
    {
        msg->nagc[i][0]=msg->nagc[num_values-1][0];
        msg->nagc[i][1]=msg->nagc[num_values-1][1];
        msg->nagc[i][2]=msg->nagc[num_values-1][2];
        msg->nagc[i][3]=msg->nagc[num_values-1][3];
    }
#endif
    return 0;
}

int grifo_fw::dc_append_frequency_operative_message(ctrlink_blob_t* ld, dc_board_t board, unsigned int num_values, const unsigned int *frq /*[num_values]*/, ctrlink_pingpong_t pingpong)
{
    return ctrlink_append_lo_frequency_operative_message(ld, dc2ba(board), num_values, frq, pingpong);
 #if 0
    struct frq_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];
    };

    frq_msg_t* msg=msg_alloc<frq_msg_t>(ld, board, fid_lo_frq);
    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<frq_msg_t::k_max_words; ++i)
    {
        msg->frq[i]=msg->frq[n-1];
    }

    return 0;
#endif
}

int grifo_fw::dc_append_frequency_asynch_message(ctrlink_blob_t* ld, dc_board_t board, unsigned int frq)
{
    return ctrlink_append_lo_frequency_asynch_message(ld, dc2ba(board), frq);
#if 0
    struct frq_msg_t
    {
        uint8_t frq;
    };

    frq_msg_t* msg=msg_alloc<frq_msg_t>(ld, board, fid_lo_frq);
    if (!msg)
        return 1;
    msg->frq=frq;
    return 0;
#endif
}

int grifo_fw::dc_append_test_port_command_message(ctrlink_blob_t* ld, dc_board_t board, unsigned int tp)
{
    uint32_t* msg=msg_alloc<uint32_t>(ld, board, fid_test_port, ctrlink_pingpong_aynch);
    if (!msg)
        return 1;
    *msg=tp;
    return 0;
}

int grifo_fw::dc_append_simple_test_pattern(ctrlink_blob_t* ld, dc_board_t board, unsigned int pattern, bool asynch)
{
    static unsigned int values[2048];

    //ld=validate_blob(ld);

    ctrlink_pingpong_t pingpong=asynch ? ctrlink_pingpong_aynch : ctrlink_pingpong_auto;

    if (pattern & 2)
    {
        dc_ad_ctrl_t ad=pattern & 1 ? dc_ad_ramp_test : dc_ad_operative;
        dc_append_ad_control_message(ld, board, ad);
    }

    if (pattern & 4)
        dc_append_sw_operative_message(ld, board, 0, pingpong);
    if (pattern & 8)
    {
        unsigned int tmp=0;
        for(unsigned int i=0; i<2048; ++i)
        {
            values[i]=tmp;
            tmp=(tmp+1) & 0x7;
        }
        if (asynch)
            dc_append_frequency_asynch_message(ld, board, values[0]);
        else
            dc_append_frequency_operative_message(ld, board, 32, values);
    }

    if (pattern & 16)
    {
        unsigned int tmp=255;
        for(unsigned int i=0; i<2048; ++i)
        {
            values[i]=tmp;
            if (tmp>0)
                --tmp;
        }
        if (asynch)
            dc_append_stc_asynch_message(ld, board, values[0]);
        else
            dc_append_stc_operative_message(ld, board, 128, values);
    }

    if (pattern & 32)
    {
        unsigned int tmp=0;
        for(unsigned int i=0; i<2048; ++i)
        {
            values[i]=tmp;
            if (tmp>=63)
                tmp=0;
            else
                ++tmp;
        }
        if (asynch)
            dc_append_nacg_asynch_message(ld, board, values);
        else
            dc_append_nacg_operative_message(ld, board, 32, values);
    }

    if (pattern & 64)
    {
        ctrlink_append_status_request(ld, dc2ba(board));
    }
    return 0;
}

