#include "lip/lip.h"
#if LIP_OPCUA_UADP_PUB_ENABLE
#include "lip_opcua_uadp_proto_defs.h"
#include "lip_opcua_uadp_pub.h"
#include "lip/app/opcua/binary/lip_opcua_binary_types_parse.h"
#include "lbitfield.h"



t_lip_errs lip_uadp_pub_group_init(t_lip_uadp_pub_group_ctx *pubgr_ctx, const t_lip_uadp_pub_transport *transp,
                                   const t_lip_uadp_pubid *pubid, uint16_t group_id, uint16_t group_flags) {
    t_lip_errs err = lip_uadp_pubid_is_valid(pubid) ? LIP_ERR_SUCCESS : LIP_ERR_OPCUA_INVALID_PUBID;
    if (err == LIP_ERR_SUCCESS) {
        memset(pubgr_ctx, 0, sizeof(*pubgr_ctx));
        pubgr_ctx->pubid = *pubid;
        pubgr_ctx->transp = transp;
        pubgr_ctx->group_info.flags = group_flags;
        pubgr_ctx->group_info.id = group_id;

        uint8_t pubtype = pubid->is_str ? LIP_UADP_NETMSG_EXFLAGS1_PUBID_TYPE_STR :
                          pubid->len == 1 ? LIP_UADP_NETMSG_EXFLAGS1_PUBID_TYPE_BYTE :
                          pubid->len == 2 ? LIP_UADP_NETMSG_EXFLAGS1_PUBID_TYPE_UINT16 :
                          pubid->len == 4 ? LIP_UADP_NETMSG_EXFLAGS1_PUBID_TYPE_UINT32 :
                                                      LIP_UADP_NETMSG_EXFLAGS1_PUBID_TYPE_UINT64;
        pubgr_ctx->internal.exflags1 = LBITFIELD_SET(LIP_UADP_NETMSG_EXFLAGS1_PUBID_TYPE, pubtype)
                                       | LBITFIELD_SET(LIP_UADP_NETMSG_EXFLAGS1_DSET_CLASSID_EN, 0)
                                       | LBITFIELD_SET(LIP_UADP_NETMSG_EXFLAGS1_SECHDR_EN, 0)
                                       | LBITFIELD_SET(LIP_UADP_NETMSG_EXFLAGS1_TSTMP_EN, (group_flags & LIP_UADP_GROUP_FLAG_TSTMP_VALID) ? 1 : 0)
#if LIP_OPCUA_TSTMP_PICOSEC_EN
                                       | LBITFIELD_SET(LIP_UADP_NETMSG_EXFLAGS1_PICOSEC_EN, group_flags & LIP_UADP_GROUP_FLAG_TSTMP_VALID)
#endif
                                       | LBITFIELD_SET(LIP_UADP_NETMSG_EXFLAGS1_EXFLAGS2_EN, 0);

        pubgr_ctx->internal.flags1 = LBITFIELD_SET(LIP_UADP_NETMSG_FLAGS1_VERSION, LIP_UADP_NETMSG_VERSION_NUM)
                                     | LBITFIELD_SET(LIP_UADP_NETMSG_FLAGS1_PUBID_EN, 1)
                                     | LBITFIELD_SET(LIP_UADP_NETMSG_FLAGS1_GROUPHDR_EN, 1)
                                     | LBITFIELD_SET(LIP_UADP_NETMSG_FLAGS1_PLHDR_EN, 1)
                                     | LBITFIELD_SET(LIP_UADP_NETMSG_FLAGS1_EXFLAGS1_EN, pubgr_ctx->internal.exflags1 != 0);
        pubgr_ctx->internal.groupflags = LBITFIELD_SET(LIP_UADP_GRPHDR_FLAGS_WR_GROUP_ID_EN, (group_flags & LIP_UADP_GROUP_FLAG_ID_VALID) ? 1 : 0)
#if LIP_OPCUA_UADP_GROUP_VERSION
                                    | LBITFIELD_SET(LIP_UADP_GRPHDR_FLAGS_GROUP_VER_EN, (group_flags & LIP_UADP_GROUP_FLAG_VERSION_VALID) ? 1 : 0)
#endif
#if LIP_OPCUA_UADP_GROUP_NETMSGNUM
                                    | LBITFIELD_SET(LIP_UADP_GRPHDR_FLAGS_NETMSG_NUM_EN, (group_flags & LIP_UADP_GROUP_FLAG_NETMSG_NUM_VALID) ? 1 : 0)
#endif
#if LIP_OPCUA_UADP_GROUP_SEQNUM
                                    | LBITFIELD_SET(LIP_UADP_GRPHDR_FLAGS_SEQNUM_EN, (group_flags & LIP_UADP_GROUP_FLAG_SEQNUM_VALID) ? 1 : 0)
#endif
            ;


    }
    return err;
}

t_lip_errs lip_uadp_pub_group_send(t_lip_uadp_pub_group_ctx *pubgr_ctx, const t_lip_uadp_ds_info *ds_list, uint8_t ds_cnt) {
    t_lip_errs err = LIP_ERR_SUCCESS;
    t_lip_tx_frame *tx_frame = lip_eth_frame_prepare(&pubgr_ctx->transp->frame_info, LIP_ETHTYPE_OPC_UADP, pubgr_ctx->transp->da);
    if (tx_frame) {
        uint8_t *data_ptr = tx_frame->buf.cur_ptr;
        const uint8_t ver_falgs1 = pubgr_ctx->internal.flags1;
        const uint8_t ex_flags1 = pubgr_ctx->internal.exflags1;
        *data_ptr++ = ver_falgs1;
        if (ver_falgs1 & LIP_UADP_NETMSG_FLAGS1_EXFLAGS1_EN) {
            *data_ptr++ = ex_flags1;
        }

        if (ver_falgs1 & LIP_UADP_NETMSG_FLAGS1_PUBID_EN) {
            const int32_t len = pubgr_ctx->pubid.len;
            if (pubgr_ctx->pubid.is_str) {
                LIP_OPCUA_PUT_INT32(len, data_ptr);
                memcpy(data_ptr, pubgr_ctx->pubid.str, len);
                data_ptr += len;
            } else if (len == 1) {
                *data_ptr++ = pubgr_ctx->pubid.intval & 0xFF;
            } else if (len == 2) {
                LIP_OPCUA_PUT_UINT16(pubgr_ctx->pubid.intval, data_ptr);
            } else if (len == 4) {
                LIP_OPCUA_PUT_UINT32(pubgr_ctx->pubid.intval, data_ptr);
            } else if (len == 8) {
                LIP_OPCUA_PUT_UINT64(pubgr_ctx->pubid.intval, data_ptr);
            }
        }

        if (ver_falgs1 & LIP_UADP_NETMSG_FLAGS1_GROUPHDR_EN) {
            const uint8_t group_flags = pubgr_ctx->internal.groupflags;
            *data_ptr++ = group_flags;
            if (group_flags & LIP_UADP_GRPHDR_FLAGS_WR_GROUP_ID_EN) {
                LIP_OPCUA_PUT_UINT16(pubgr_ctx->group_info.id, data_ptr);
            }
#if LIP_OPCUA_UADP_GROUP_VERSION
            if (group_flags & LIP_UADP_GRPHDR_FLAGS_GROUP_VER_EN) {
                LIP_OPCUA_PUT_UINT32(pubgr_ctx->group_info.version, data_ptr);
            }
#endif
#if LIP_OPCUA_UADP_GROUP_NETMSGNUM
            if (group_flags & LIP_UADP_GRPHDR_FLAGS_NETMSG_NUM_EN) {
                LIP_OPCUA_PUT_UINT16(pubgr_ctx->group_info.netmsg_num, data_ptr);
            }
#endif
#if LIP_OPCUA_UADP_GROUP_SEQNUM
            if (group_flags & LIP_UADP_GRPHDR_FLAGS_SEQNUM_EN) {
                LIP_OPCUA_PUT_UINT16(pubgr_ctx->group_info.seqnum, data_ptr);
            }
#endif
        }

        /* -------------------- Payload Header ------------------------------*/
        if (ver_falgs1 & LIP_UADP_NETMSG_FLAGS1_PLHDR_EN) {
            *data_ptr++ = ds_cnt;
            for (uint8_t i = 0; i < ds_cnt; ++i) {
                LIP_OPCUA_PUT_UINT16(ds_list[i].writer_id, data_ptr);
            }
        }

        /* ----------------- Extended Network MessageHeader ------------------*/
#if LIP_OPCUA_UADP_GROUP_TSTMP
        if (ex_flags1 & LIP_UADP_NETMSG_EXFLAGS1_TSTMP_EN) {
            LIP_OPCUA_PET_TSTMP(pubgr_ctx->group_info.tstmp, data_ptr, ex_flags1 & LIP_UADP_NETMSG_EXFLAGS1_PICOSEC_EN);
        }
#endif
        /* ----------------- Promoted Header ---------------------------------*/
        /* ----------------- Security Header ---------------------------------*/

        /* ----------------- Payload -----------------------------------------*/
        uint8_t *ds_size_put_ptr = NULL;
        if (ds_cnt > 1) {
            if (ver_falgs1 & LIP_UADP_NETMSG_FLAGS1_PLHDR_EN) {
                /* Так как размер зависит как от размера данных, так и от размера заголовка,
                 * то до формирования заголовка мы еще не знаем нужное значение.
                 * Чтобы два раза не рассчитывать, сохраняем указатель на место сохранения размеров,
                 * а запись будем делать уже после сохранения заголовков */
                ds_size_put_ptr = data_ptr;
                data_ptr += 2*ds_cnt;
            }
        }


        for (uint8_t i = 0; i < ds_cnt; ++i) {
            const t_lip_uadp_ds_info *ds = &ds_list[i];
            uint8_t *ds_start_ptr = data_ptr;


            uint8_t ds_flags2 = 0
#if LIP_OPCUA_UADP_DS_TSTMP
#if LIP_OPCUA_TSTMP_PICOSEC_EN
                        | (ds->flags & LIP_UADP_DS_FLAG_TSTMP_VALID) ? (LIP_UADP_DSHDR_FLAGS2_TSTMP_EN |LIP_UADP_DSHDR_FLAGS2_PICOSEC_EN) : 0
#else
                        | (ds->flags & LIP_UADP_DS_FLAG_TSTMP_VALID) ? LIP_UADP_DSHDR_FLAGS2_TSTMP_EN : 0
#endif
#endif
                    ;
            uint8_t ds_flags1 = (ds_flags2 != 0 ? LIP_UADP_DSHDR_FLAGS1_DSFLAGS2_EN : 0)
#if LIP_OPCUA_UADP_DS_SEQNUM
                                | (ds->flags & LIP_UADP_DS_FLAG_SEQNUM_VALID ? LIP_UADP_DSHDR_FLAGS1_SEQNUM_EN : 0)
#endif
#if LIP_OPCUA_UADP_DS_STATUS
                                | (ds->flags & LIP_UADP_DS_FLAG_STATUS_VALID ? LIP_UADP_DSHDR_FLAGS1_STATUS_EN : 0)
#endif
#if LIP_OPCUA_UADP_DS_VERSION
                                | (ds->flags & LIP_UADP_DS_FLAG_VERSION_MJ_VALID ? LIP_UADP_DSHDR_FLAGS1_CFGVER_MJ_EN : 0)
                                | (ds->flags & LIP_UADP_DS_FLAG_VERSION_MN_VALID ? LIP_UADP_DSHDR_FLAGS1_CFGVER_MN_EN : 0)
#endif
                ;
            *data_ptr++ = ds_flags1;
            if (ds_flags2 != 0) {
                *data_ptr++ = ds_flags2;
            }

#if LIP_OPCUA_UADP_DS_SEQNUM
            if (ds_flags1 & LIP_UADP_DSHDR_FLAGS1_SEQNUM_EN) {
                LIP_OPCUA_PUT_UINT16(ds->seqnum, data_ptr);
            }
#endif
#if LIP_OPCUA_UADP_DS_TSTMP
            if (ds_flags2 & LIP_UADP_DSHDR_FLAGS2_TSTMP_EN) {
                LIP_OPCUA_PET_TSTMP(ds->ts, data_ptr, ds_flags2 & LIP_UADP_DSHDR_FLAGS2_PICOSEC_EN);
            }
#endif
#if LIP_OPCUA_UADP_DS_STATUS
            if (ds_flags1 & LIP_UADP_DSHDR_FLAGS1_STATUS_EN) {
                LIP_OPCUA_PUT_UINT16(ds->status, data_ptr);
            }
#endif
#if LIP_OPCUA_UADP_DS_VERSION
            if (ds_flags1 & LIP_UADP_DSHDR_FLAGS1_CFGVER_MJ_EN) {                
                LIP_OPCUA_PUT_UINT32(ds->version.mj, data_ptr);
            }
            if (ds_flags1 & LIP_UADP_DSHDR_FLAGS1_CFGVER_MN_EN) {
                LIP_OPCUA_PUT_UINT32(ds->version.mn, data_ptr);
            }
#endif

            memcpy(data_ptr, ds->data_ptr, ds->data_size);
            data_ptr += ds->data_size;
            if (ds_size_put_ptr != NULL) {
                LIP_OPCUA_PUT_UINT16(data_ptr - ds_start_ptr, ds_size_put_ptr);
            }
        }

        tx_frame->buf.cur_ptr = data_ptr;
        lip_eth_frame_flush(tx_frame);

    } else {
        err = LIP_ERR_TXBUF_NOT_FOUND;
    }
    return err;
}



#endif

