#include "lip/lip.h"
#if LIP_OPCUA_UADP_SUB_ENABLE
#include "lip_opcua_uadp_proto_defs.h"
#include "lip_opcua_uadp_sub.h"
#include "lip/app/opcua/binary/lip_opcua_binary_types_parse.h"
#include "lbitfield.h"


typedef struct {
    t_lip_uadp_pubid pubid;
    t_lip_uadp_sub_msg_proc_cb cb;
    void *cb_ctx;
} t_lip_opcua_sub_ctx;

static t_lip_opcua_sub_ctx f_sub_ctx[LIP_OPCUA_UADP_SUB_PUBID_MAX_CNT];
static int f_sub_cnt;


static bool f_pubid_cmp(const t_lip_uadp_pubid *id1, const t_lip_uadp_pubid *id2) {
    return id1->is_valid && id2->is_valid && (id1->is_str == id2->is_str) &&
           (id1->is_str ? ((id1->len == id2->len) && (memcmp(id1->str, id2->str, id1->len)==0))
                                                                                          : (id1->intval == id2->intval));
}

static int f_get_sub_ctx_idx(const t_lip_uadp_pubid *pubid) {
    int ret = -1;
    for (int i = 0; (i < f_sub_cnt) && (ret < 0); ++i) {
        if (f_pubid_cmp(pubid, &f_sub_ctx[i].pubid)) {
            ret = i;
        }
    }
    return ret;
}

static t_lip_opcua_sub_ctx *f_get_sub_ctx(const t_lip_uadp_pubid *pubid) {
    int idx = f_get_sub_ctx_idx(pubid);
    return idx < 0 ? NULL : &f_sub_ctx[idx];
}



t_lip_errs lip_uadp_sub_add(const t_lip_uadp_pubid *pubid, t_lip_uadp_sub_msg_proc_cb cb, void *cb_ctx) {
    t_lip_errs err = LIP_ERR_SUCCESS;
    if (f_sub_cnt == LIP_OPCUA_UADP_SUB_PUBID_MAX_CNT) {
        err = LIP_ERR_OPCUA_SUB_MAX_REC;
    } else {
        t_lip_opcua_sub_ctx *ctx = f_get_sub_ctx(pubid);
        if (!ctx) {
            ctx = &f_sub_ctx[f_sub_cnt];
            ctx->pubid = *pubid;
            ++f_sub_cnt;
        }
        ctx->cb = cb;
        ctx->cb_ctx = cb_ctx;
    }
    return err;
}

void lip_uadp_sub_rem(const t_lip_uadp_pubid *pubid) {
    int idx = f_get_sub_ctx_idx(pubid);
    if (idx >= 0) {
        if (idx < (f_sub_cnt - 1)) {
            memmove(&f_sub_ctx[idx], &f_sub_ctx[idx+1], (f_sub_cnt - idx - 1) * sizeof(f_sub_ctx[0]));
        }
        --f_sub_cnt;
    }
}

void lip_uadp_sub_clear(void) {
    f_sub_cnt = 0;
}




t_lip_errs lip_opcua_uadp_process_packet(t_lip_payload *pl, const t_lip_eth_rx_packet *eth_packet) {
    t_lip_errs err = LIP_ERR_SUCCESS;
    const uint8_t *data_ptr = pl->data;
    uint8_t ver_falgs1, ex_flags1 = 0, ex_flags2 = 0;

    /* --------------------- NetworkMessage Header --------------------------*/
    ver_falgs1 = *data_ptr++;
    if (LBITFIELD_GET(ver_falgs1, LIP_UADP_NETMSG_FLAGS1_VERSION) == 1) {
        if (ver_falgs1 & LIP_UADP_NETMSG_FLAGS1_EXFLAGS1_EN) {
            ex_flags1 = *data_ptr++;
            if (ex_flags1 & LIP_UADP_NETMSG_EXFLAGS1_EXFLAGS2_EN) {
                ex_flags2 = *data_ptr++;
            }
        }

        t_lip_uadp_pubid pubid;
        pubid.is_valid = 0;

        if (ver_falgs1 & LIP_UADP_NETMSG_FLAGS1_PUBID_EN) {
            uint8_t pubid_type = LBITFIELD_GET(ex_flags1, LIP_UADP_NETMSG_EXFLAGS1_PUBID_TYPE);
            if (pubid_type == LIP_UADP_NETMSG_EXFLAGS1_PUBID_TYPE_BYTE) {
                pubid.intval = *data_ptr++;
                pubid.len = 1;
                pubid.is_valid = 1;
                pubid.is_str = 0;
            } else if (pubid_type == LIP_UADP_NETMSG_EXFLAGS1_PUBID_TYPE_UINT16) {
                LIP_OPCUA_GET_UINT16(pubid.intval, data_ptr);
                pubid.len = 2;
                pubid.is_valid = 1;
                pubid.is_str = 0;
            } else if (pubid_type == LIP_UADP_NETMSG_EXFLAGS1_PUBID_TYPE_UINT32) {
                LIP_OPCUA_GET_UINT32(pubid.intval, data_ptr);
                pubid.len = 4;
                pubid.is_valid = 1;
                pubid.is_str = 0;
            } else if (pubid_type == LIP_UADP_NETMSG_EXFLAGS1_PUBID_TYPE_UINT64) {
                LIP_OPCUA_GET_UINT64(pubid.intval, data_ptr);
                pubid.len = 8;
                pubid.is_valid = 1;
                pubid.is_str = 0;
            } else if (pubid_type == LIP_UADP_NETMSG_EXFLAGS1_PUBID_TYPE_STR) {
                int32_t str_len;
                LIP_OPCUA_GET_INT32(str_len, data_ptr);
                if (str_len > 0) {
                    pubid.len = str_len < 0xFFFF ? str_len : 0xFFFF;
                    pubid.str = data_ptr;
                    data_ptr += str_len;
                } else {
                    pubid.len = 0;
                    pubid.str = NULL;
                }
                pubid.is_valid = 1;
                pubid.is_str = 1;
            }
        }

        if (pubid.is_valid) {
            const t_lip_opcua_sub_ctx *ctx = f_get_sub_ctx(&pubid);
            if (ctx) {
                t_lip_uadp_group_info group;
                memset(&group, 0, sizeof(group));

                if (ex_flags1 & LIP_UADP_NETMSG_EXFLAGS1_DSET_CLASSID_EN) {
                    data_ptr += 16;
                }

                if (ver_falgs1 & LIP_UADP_NETMSG_FLAGS1_GROUPHDR_EN) {
                    /* ------------------ Group Header --------------------------------*/
                    uint8_t group_flags = *data_ptr++;


                    if (group_flags & LIP_UADP_GRPHDR_FLAGS_WR_GROUP_ID_EN) {
                        LIP_OPCUA_GET_UINT16(group.id, data_ptr);
                        group.flags |= LIP_UADP_GROUP_FLAG_ID_VALID;
                    }
                    if (group_flags & LIP_UADP_GRPHDR_FLAGS_GROUP_VER_EN) {
#if LIP_OPCUA_UADP_GROUP_VERSION
                        LIP_OPCUA_GET_UINT32(group.version, data_ptr);
                        group.flags |= LIP_UADP_GROUP_FLAG_VERSION_VALID;
#else
                        data_ptr += 4;
#endif
                    }
                    if (group_flags & LIP_UADP_GRPHDR_FLAGS_NETMSG_NUM_EN) {
#if LIP_OPCUA_UADP_GROUP_NETMSGNUM
                        LIP_OPCUA_GET_UINT16(group.netmsg_num, data_ptr);
                        group.flags |= LIP_UADP_GROUP_FLAG_NETMSG_NUM_VALID;
#else
                        data_ptr += 2;
#endif
                    }
                    if (group_flags & LIP_UADP_GRPHDR_FLAGS_SEQNUM_EN) {
#if LIP_OPCUA_UADP_GROUP_SEQNUM
                        LIP_OPCUA_GET_UINT16(group.seqnum, data_ptr);
                        group.flags |= LIP_UADP_GROUP_FLAG_SEQNUM_VALID;
#else
                        data_ptr += 2;
#endif
                    }
                }



                /* -------------------- Payload Header ------------------------------*/
                uint8_t save_ds_cnt = 0;
                uint8_t rx_ds_cnt = 0;
                t_lip_uadp_ds_info ds_list[LIP_OPCUA_UADP_SUB_DS_MAX_CNT];
                uint16_t ds_sizes[LIP_OPCUA_UADP_SUB_DS_MAX_CNT];
                uint32_t drop_ds_size = 0;
                memset(ds_list,0 , sizeof(ds_list));

                if (ver_falgs1 & LIP_UADP_NETMSG_FLAGS1_PLHDR_EN) {
                    rx_ds_cnt = *data_ptr++;
                    for (uint8_t i = 0; i < rx_ds_cnt; ++i) {
                        if (i < LIP_OPCUA_UADP_SUB_DS_MAX_CNT) {
                            LIP_OPCUA_GET_UINT16(ds_list[save_ds_cnt].writer_id, data_ptr);
                            save_ds_cnt++;
                        } else {
                            data_ptr += 2;
                        }
                    }
                }

                /* ----------------- Extended Network MessageHeader ------------------*/
                if (ex_flags1 & LIP_UADP_NETMSG_EXFLAGS1_TSTMP_EN) {
#if LIP_OPCUA_UADP_GROUP_TSTMP
                    LIP_OPCUA_GET_TSTMP(group.tstmp, data_ptr, ex_flags1 & LIP_UADP_NETMSG_EXFLAGS1_PICOSEC_EN);
                    group.flags |= LIP_UADP_GROUP_FLAG_TSTMP_VALID;
#else
                    LIP_UADP_DROP_TSTMP(data_ptr, ex_flags1 & LIP_UADP_NETMSG_EXFLAGS1_PICOSEC_EN);
#endif
                }



                if (ex_flags2 & LIP_UADP_NETMSG_EXFLAGS2_PROMOTED_EN) {
                    uint16_t prom_size;
                    LIP_OPCUA_GET_UINT16(prom_size, data_ptr);
                    data_ptr += prom_size;
                }
                /* ----------------- Security Header ---------------------------------*/
                if (ex_flags1 & LIP_UADP_NETMSG_EXFLAGS1_SECHDR_EN) {

                }


                /* ----------------- Payload -----------------------------------------*/
                if (rx_ds_cnt > 1) {
                    if (ver_falgs1 & LIP_UADP_NETMSG_FLAGS1_PLHDR_EN) {
                        for (uint8_t i = 0; i < rx_ds_cnt; ++i) {
                            uint16_t ds_size;
                            LIP_OPCUA_GET_UINT16(ds_size, data_ptr);
                            if (i < save_ds_cnt) {
                                ds_sizes[i] = ds_size;
                            } else {
                                drop_ds_size += ds_size;
                            }
                        }
                    } else {
                        /** @todo err */
                    }
                } else if (rx_ds_cnt == 1) {
                    ds_sizes[0] = pl->size - (data_ptr - pl->data);
                }



                for (uint8_t i = 0; i < save_ds_cnt; ++i) {
                    t_lip_uadp_ds_info *ds = &ds_list[i];
                    const uint8_t *hdr_start_ptr = data_ptr;
                    uint8_t ds_flags1 = *data_ptr++;
                    uint8_t ds_flags2 = 0;

                    if (ds_flags1 & LIP_UADP_DSHDR_FLAGS1_DSFLAGS2_EN) {
                        ds_flags2 = *data_ptr++;
                    }

                    if (ds_flags1 & (LIP_UADP_DSHDR_FLAGS1_SEQNUM_EN
                                     | LIP_UADP_DSHDR_FLAGS2_TSTMP_EN
                                     | LIP_UADP_DSHDR_FLAGS1_STATUS_EN
                                     | LIP_UADP_DSHDR_FLAGS1_CFGVER_MJ_EN
                                     | LIP_UADP_DSHDR_FLAGS1_CFGVER_MN_EN)) {
                        if (ds_flags1 & LIP_UADP_DSHDR_FLAGS1_SEQNUM_EN) {
#if LIP_OPCUA_UADP_DS_SEQNUM
                            LIP_OPCUA_GET_UINT16(ds->seqnum, data_ptr);
                            ds->flags |= LIP_UADP_DS_FLAG_SEQNUM_VALID;
#else
                            data_ptr += 2;
#endif
                        }
                        if (ds_flags2 & LIP_UADP_DSHDR_FLAGS2_TSTMP_EN) {
#if LIP_OPCUA_UADP_DS_TSTMP
                            LIP_OPCUA_GET_TSTMP(ds->ts, data_ptr, ds_flags2 & LIP_UADP_DSHDR_FLAGS2_PICOSEC_EN);
                            ds->flags |= LIP_UADP_DS_FLAG_TSTMP_VALID;
#else
                            LIP_UADP_DROP_TSTMP(data_ptr, ds_flags2 & LIP_UADP_DSHDR_FLAGS2_PICOSEC_EN);
#endif
                        }
                        if (ds_flags1 & LIP_UADP_DSHDR_FLAGS1_STATUS_EN) {
#if LIP_OPCUA_UADP_DS_STATUS
                            LIP_OPCUA_GET_UINT16(ds->status, data_ptr);
                            ds->flags |= LIP_UADP_DS_FLAG_STATUS_VALID;
#else
                            data_ptr += 2;
#endif
                        }
                        if (ds_flags1 & LIP_UADP_DSHDR_FLAGS1_CFGVER_MJ_EN) {
#if LIP_OPCUA_UADP_DS_VERSION
                            LIP_OPCUA_GET_UINT32(ds->version.mj, data_ptr);
                            ds->flags |= LIP_UADP_DS_FLAG_VERSION_MJ_VALID;
#else
                            data_ptr += 4;
#endif
                        }
                        if (ds_flags1 & LIP_UADP_DSHDR_FLAGS1_CFGVER_MN_EN) {
#if LIP_OPCUA_UADP_DS_VERSION
                            LIP_OPCUA_GET_UINT32(ds->version.mn, data_ptr);
                            ds->flags |= LIP_UADP_DS_FLAG_VERSION_MN_VALID;
#else
                            data_ptr += 4;
#endif
                        }
                    }
                    ds->data_ptr = data_ptr;
                    ds->data_size = ds_sizes[i] - (data_ptr - hdr_start_ptr);
                    data_ptr += ds->data_size;
                }
                /* ----------------- Security Footer ---------------------------------*/

                /* ----------------- Signature ---------------------------------------*/


                if ((err == LIP_ERR_SUCCESS) && ctx->cb) {
                    ctx->cb(&pubid, &group, ds_list, save_ds_cnt, ctx->cb_ctx, eth_packet->ll_frame);
                }
            }
        }
    }


    return err;
}

#endif
