#include "shmparser.h"

#include <stdlib.h>
#include <string.h>
#include <stdint.h>
#include <stdio.h>

enum{
    IMG_DATA = 1,
    IMG_WIDTH,
    IMG_HEIGHT,
    IMG_TIMESTAMP,
    IMG_ID,
    IMG_CAMERA_ID,
    IMG_CAMERA_NAME,

    RULE_HANDLETRACK,
    RULE_DATATYPE,
    RULE_SDK_COUNT,
    HIDDEN_SDK_SIZE,
    RULE_SDK_TYPE,
    RULE_SDK_ID,
    RULE_SDK_NAME,
    RULE_SDK_TIMESTAMP,
    RULE_SDK_TGT_COUNT,

    RULE_SDK_TGT_ID,
    RULE_SDK_TGT_TYPE,
    RULE_SDK_TGT_CONFIDENCE,
    RULE_SDK_TGT_RECT_L,
    RULE_SDK_TGT_RECT_T,
    RULE_SDK_TGT_RECT_R,
    RULE_SDK_TGT_RECT_B,
    RULE_SDK_TGT_FEAT,
    RULE_SDK_TGT_ATTR,
};


static const int cmdBits = 16;
static const int sizeBits = 48;

static inline uint64_t mask(const int nbits) { return ((uint64_t)(1) << nbits) - 1; }
static inline uint64_t max64() { return ((((uint64_t)(1)<<63)-1)<<1)+1; }
static inline uint64_t cmdMask() { return mask(cmdBits); }
static inline uint64_t sizeMask() { return max64() & (~(cmdMask() << sizeBits)); }
static inline uint64_t packHeader(const uint64_t cmd, const uint64_t value){
    return ((cmd & cmdMask()) << sizeBits) | (value & sizeMask());
}
static inline void unpackHeader(const uint64_t h, uint64_t* cmd, uint64_t* value){
    *cmd = h >> sizeBits;
    *value = h & sizeMask();
}

static inline uint16_t Get8(const void* p){
    return ((const uint8_t*)p)[0];
}
static inline void Put8(void* p, const uint8_t u){
    ((uint8_t*)p)[0] = u;
}

static inline uint16_t Get16(const void *p)
{
    const uint8_t * ptr = (const uint8_t *)p;
    return (((uint16_t) ptr[0]) << 8u) |
           (((uint16_t) ptr[1]));
}
static inline void Put16(void *p, uint16_t u)
{
    uint8_t* ptr = (uint8_t *)p;
    ptr[0] = (uint8_t) (u >> 8u);
    ptr[1] = (uint8_t) (u);
}

static inline uint32_t Get32(const void *p)
{
    const uint8_t * ptr = (const uint8_t *)p;
    return (((uint32_t) ptr[0]) << 24u) |
           (((uint32_t) ptr[1]) << 16u) |
           (((uint32_t) ptr[2]) << 8u) |
           (((uint32_t) ptr[3]));
}
static inline void Put32(void *p, uint32_t u)
{
    uint8_t* ptr = (uint8_t *)p;
    ptr[0] = (uint8_t) (u >> 24u);
    ptr[1] = (uint8_t) (u >> 16u);
    ptr[2] = (uint8_t) (u >> 8u);
    ptr[3] = (uint8_t) (u);
}

static inline uint64_t Get64(const void *p)
{
    const uint8_t * ptr = (const uint8_t *)p;
    return (((uint64_t) ptr[0]) << 56u) |
           (((uint64_t) ptr[1]) << 48u) |
           (((uint64_t) ptr[2]) << 40u) |
           (((uint64_t) ptr[3]) << 32u) |
           (((uint64_t) ptr[4]) << 24u) |
           (((uint64_t) ptr[5]) << 16u) |
           (((uint64_t) ptr[6]) << 8u) |
           ((uint64_t) ptr[7]);
}
static inline void Put64(void *p, uint64_t u)
{
    uint8_t* ptr = (uint8_t *)p;
    ptr[0] = (uint8_t) (u >> 56);
    ptr[1] = (uint8_t) (u >> 48);
    ptr[2] = (uint8_t) (u >> 40u);
    ptr[3] = (uint8_t) (u >> 32);
    ptr[4] = (uint8_t) (u >> 24u);
    ptr[5] = (uint8_t) (u >> 16u);
    ptr[6] = (uint8_t) (u >> 8u);
    ptr[7] = (uint8_t) (u);
}

struct stimg* make_image_ref(const uint64_t id, const unsigned char* data, const uint32_t size,
                            const uint32_t width, const uint32_t height,
                            const char* timestamp, const uint32_t ts_size,
                            const char* camera_id, const uint32_t cid_size,
                            const char* camera_name,const uint32_t cname_size)
{
    struct stimg* img = (struct stimg*)calloc(1, sizeof(struct stimg));

    img->data = (unsigned char*)data;
    img->data_size = size;
    img->width = width;
    img->height = height;
    img->id = id;
    img->timestamp = (char*)timestamp;
    img->timestamp_size = ts_size;
    img->camera_id = (char*)camera_id;
    img->camera_id_size = cid_size;
    img->camera_name = (char*)camera_name;
    img->camera_name_size = cname_size;

    return img;
}

static inline unsigned char* put_memory(unsigned char* p, const int cmd,
                                        const void* data, const uint64_t size)
{
    Put64(p, packHeader(cmd, size));
    p += sizeof(uint64_t);
    memcpy(p, data, size);
    p += size;

    return p;
}
// 多存储一个0
static inline unsigned char* put_string(unsigned char* p, const int cmd,
                                        const void* data, const uint32_t size)
{
    p = put_memory(p, cmd, data, size);
    return p;
}
static inline unsigned char* put_number(unsigned char* p, const int cmd, const uint32_t number){
    Put64(p, packHeader(cmd, number));
    p += sizeof(uint64_t);
    return p;
}
// 由于一个 uint64_t 分为 cmd 和 size，可能放不下一个 uint64_t 的数字
// 使用另一个field存储
static inline unsigned char* put_number_64(unsigned char* p, const int cmd, const uint64_t number){
    Put64(p, packHeader(cmd, sizeof(number)));
    p += sizeof(uint64_t);
    Put64(p, number);
    p += sizeof(number);
    return p;
}

void image2shm(void* shm, struct stimg* img){
    /*
        data
        width
        height
        id
        timestamp
        camera id
        camera name
    */
    unsigned char* tmp = (unsigned char*)shm;

    /*
        63     48               0
        -------------------------
        |  cmd |    size        |
        -------------------------
        |        data           |
        -------------------------
    */
    const size_t hsize = sizeof(uint64_t);
    tmp = put_memory(tmp, IMG_DATA, img->data, img->data_size);

    /*
        63     48               0
        -------------------------
        |  cmd |    data        |
        -------------------------
    */
    // put width
    tmp = put_number(tmp, IMG_WIDTH, img->width);
    // put height
    tmp = put_number(tmp, IMG_HEIGHT, img->height);
    // put id, id bits maybe > sizeBits, use another field keep it
    tmp = put_number_64(tmp, IMG_ID, img->id);
    // put timestamp
    tmp = put_string(tmp, IMG_TIMESTAMP, img->timestamp, img->timestamp_size);
    // put camera id
    tmp = put_string(tmp, IMG_CAMERA_ID, img->camera_id, img->camera_id_size);
    tmp = put_string(tmp, IMG_CAMERA_NAME, img->camera_name, img->camera_name_size);
}

static inline unsigned char* get_memory(unsigned char* p, int* cmd,
                                        unsigned char** data, uint64_t* size)
{
    uint64_t header = Get64(p);
    p += sizeof(uint64_t);

    uint64_t tmpc = 0, tmpn = 0;
    unpackHeader(header, &tmpc, &tmpn);
    *cmd = tmpc;
    *size = tmpn;
    // get data
    *data = p;
    // next field
    p += tmpn;

    return p;
}
// 多存储一个0
static inline unsigned char* get_string(unsigned char* p, int* cmd,
                                        char** data, uint32_t* size)
{
    uint64_t size64 = 0;
    p = get_memory(p, cmd, (unsigned char**)data, &size64);
    *size = size64;
    return p;
}
static inline unsigned char* get_number(unsigned char* p, int* cmd, uint32_t* value){
    uint64_t header = Get64(p);
    p += sizeof(uint64_t);
    // get width
    uint64_t tmpc = 0, tmpn = 0;
    unpackHeader(header, &tmpc, &tmpn);
    *cmd = tmpc;
    *value = tmpn;

    return p;
}
// 由于一个 uint64_t 分为 cmd 和 size，可能放不下一个 uint64_t 的数字
// 使用另一个field存储
static inline unsigned char* get_number_64(unsigned char* p, int* cmd, uint64_t* value){
    uint64_t header = Get64(p);
    p += sizeof(uint64_t);
    // get width
    uint64_t tmpc = 0, tmpn = 0;
    unpackHeader(header, &tmpc, &tmpn);
    *cmd = tmpc;
    *value = Get64(p);
    p += sizeof(uint64_t);

    return p;
}

void free_stimg(struct stimg* img){
    free(img);
}

struct stimg* shm2image(void* shm){
    /*
        data
        width
        height
        id
        timestamp
        camera id
        camera name
    */
    unsigned char* tmp = (unsigned char*)shm;

    struct stimg* img = (struct stimg*)calloc(1, sizeof(struct stimg));

    /*
        63     48               0
        -------------------------
        |  cmd |    size        |
        -------------------------
        |        data           |
        -------------------------
    */

    int cmd = 0;
    // get data
    tmp = get_memory(tmp, &cmd, &img->data, &img->data_size);
    // get width
    tmp = get_number(tmp, &cmd, &img->width);
    // get height
    tmp = get_number(tmp, &cmd, &img->height);
    // get id, skip [cmd,size]
    tmp = get_number_64(tmp, &cmd, &img->id);
    // get timestamp
    tmp = get_string(tmp, &cmd, &img->timestamp, &img->timestamp_size);
    // get camera id
    tmp = get_string(tmp, &cmd, &img->camera_id, &img->camera_id_size);
    // get camera name
    get_string(tmp, &cmd, &img->camera_name, &img->camera_name_size);

    return img;
}

/////////////////////////////////////////////////////////////

// 由于 handletrack 会在运行中变化，目前使用固定 256 字节存储
static const uint64_t SIZEHT = 256;

void rule2shm(void* shm, const char* handletrack, const uint32_t ht_size){
    unsigned char* tmp = (unsigned char*)shm;
    int cmd = 0;
    // handle track
    tmp = put_memory(tmp, RULE_HANDLETRACK, handletrack, ht_size);
    // 虽然保存的是 ht_size, 但为后续扩展内存的容量是 SIZEHT 将 tmp 调整到 SIZEHT
    tmp += SIZEHT - ht_size;

    // datatype, 此字段目前貌似没用
    tmp = put_memory(tmp, RULE_DATATYPE, "sdk", 3);

    // sdk count
    Put64(tmp, packHeader(RULE_SDK_COUNT, 0));
    tmp += sizeof(uint64_t);
    // no sdk content size = 0
    Put64(tmp, 0);
}

static unsigned char* put_targets(unsigned char* p, const TResult* res, uint64_t* size){
    /*
        id
        type
        confidence
        struct strc rect
        feature
        attribute
    */
    const uint64_t hsize = sizeof(uint64_t);
    uint64_t s = 0;
    unsigned char* pheader = p;
    for(int i = 0; i < res->count; i++){
        Target* tgt = &res->targets[i];

        // 放入 id 是 uint64_t 需要两个 field
        p = put_number_64(p, RULE_SDK_TGT_ID, tgt->id);
        // type
        // p = put_memory(p, RULE_SDK_TGT_TYPE, tgt->type,
        //     strlen(tgt->type) < sizeof(tgt->type) ? strlen(tgt->type) : sizeof(tgt->type));
        p = put_memory(p, RULE_SDK_TGT_TYPE, tgt->type, sizeof(tgt->type));
        // confidence
        p = put_number(p, RULE_SDK_TGT_CONFIDENCE, tgt->confidence);
        // rect
        p = put_number(p, RULE_SDK_TGT_RECT_L, tgt->rect.left);
        p = put_number(p, RULE_SDK_TGT_RECT_T, tgt->rect.top);
        p = put_number(p, RULE_SDK_TGT_RECT_R, tgt->rect.right);
        p = put_number(p, RULE_SDK_TGT_RECT_B, tgt->rect.bottom);
        // feature
        p = put_memory(p, RULE_SDK_TGT_FEAT, tgt->feature, tgt->feature_size);
        // attribute
        p = put_memory(p, RULE_SDK_TGT_ATTR, tgt->attribute, tgt->attribute_size);
    }

    *size += p - pheader;
    return p;
}
static unsigned char* get_targets(unsigned char* p, struct sttgt* tgts, const uint32_t tgtcnt){
    /*
        id
        type
        confidence
        struct strc rect
        feature
        attribute
    */
    int cmd = 0;
    for(int i = 0; i < tgtcnt; i++){
        struct sttgt tgt;
        memset(&tgt, 0, sizeof(tgt));
        p = get_number_64(p, &cmd, &tgt.id);
        p = get_string(p, &cmd, &tgt.type, &tgt.type_size);
        p = get_number(p, &cmd, (uint32_t*)&tgt.confidence);
        p = get_number(p, &cmd, (uint32_t*)&tgt.rect.left);
        p = get_number(p, &cmd, (uint32_t*)&tgt.rect.top);
        p = get_number(p, &cmd, (uint32_t*)&tgt.rect.right);
        p = get_number(p, &cmd, (uint32_t*)&tgt.rect.bottom);
        uint64_t dummy = 0;
        p = get_memory(p, &cmd, &tgt.feature, &dummy);
        tgt.feature_size = dummy;
        p = get_memory(p, &cmd, &tgt.attribute, &dummy);
        tgt.attribute_size = dummy;
        tgts[i] = tgt;
    }
    return p;
}

void add_result2rule_in_shm(void* shm, const TResult* res,
                            const char* sdktype, const uint32_t st_size,
                            const char* timestamp, const uint32_t ts_size)
{
/*
    handletrack
    datatype
    sdkmsg count
    [sdkmsg array whole size]  --> 隐藏的，不出现在stsdk结构中
    sdkmsg[s]
*/
    unsigned char* tmp = (unsigned char*)shm;
    const size_t hsize = sizeof(uint64_t);

    int cmd = 0;
    // 首先修改 handletrack
    unsigned char* pheader = tmp;
    // 获取 handletrack
    unsigned char* dummy = NULL;
    uint64_t dummy_val = 0;
    tmp = get_memory(tmp, &cmd, &dummy, &dummy_val);
    // 虽然保存的是 ht_size, 但为后续扩展内存的容量是 SIZEHT 将 tmp 调整到 SIZEHT
    // tmp 目前指向 handletrack 字符串最后一位的下一位，后面还有 SIZEHT - dummy_val 空间

    // 修改 handletrack
    memcpy(tmp, "___", 3);
    memcpy(tmp+3, sdktype, st_size);

    // 修改 handletrack header
    Put64(pheader, packHeader(RULE_HANDLETRACK, dummy_val + 3 + st_size));

    if (dummy_val + 3 + st_size > SIZEHT)
        printf("handletrack_size Must Less Than %lu But Now %lu\n", SIZEHT, dummy_val + 3 + st_size);

    // 下一个 field
    tmp += SIZEHT - dummy_val;
    // datatype simple skip
    tmp = get_memory(tmp, &cmd, &dummy, &dummy_val);

    // 获取 sdkmessage count
    pheader = tmp;
    uint32_t count = 0;
    tmp = get_number(tmp, &cmd, &count);
    // 修改 sdkmessage count
    put_number(pheader, cmd, count+1);

    // 紧接着 count 的是整个sdk数组的内存占用size
    pheader = tmp;
    // 当前保存的 sdk 数组的长度
    uint64_t cursize = Get64(pheader);
    // 跳过一个 unint64_t 用于存放修改后整个sdk的长度
    tmp += hsize;

    if (count > 0){
        // 跳过现有的 sdkmessage 添加新的
        tmp += cursize;
    }
    /*
        type
        id
        name
        timestamp
        target count
        targets
    */
    unsigned char* old = tmp;
    // sdk type
    tmp = put_memory(tmp, RULE_SDK_TYPE, sdktype, st_size);
    // id name
    tmp = put_memory(tmp, RULE_SDK_ID, "fake_id", 7);
    tmp = put_memory(tmp, RULE_SDK_NAME, "fake_name", 9);
    // timestamp
    tmp = put_memory(tmp, RULE_SDK_TIMESTAMP, timestamp, ts_size);
    // targets count
    tmp = put_number(tmp, RULE_SDK_TGT_COUNT, res->count);
    cursize += tmp - old;

    // targets
    if (res->count > 0)
        tmp = put_targets(tmp, res, &cursize);

    // 最后修改 psdksize 内容
    Put64(pheader, cursize);
}

struct strule* shm2rule(void* shm){
    unsigned char* tmp = (unsigned char*)shm;

    struct strule* rule = (struct strule*)calloc(1, sizeof(struct strule));

    /*
    handletrack
    datatype
    sdkmsg count
    [sdkmsg array whole size]  --> 隐藏的，不出现在stsdk结构中
    sdkmsg[s]
    */

    int cmd = 0;
    // handletrack
    tmp = get_string(tmp, &cmd, &rule->handletrack, &rule->handletrack_size);
    // skip SIZEHT
    tmp += SIZEHT - rule->handletrack_size;
    // datatype
    tmp = get_string(tmp, &cmd, &rule->datatype, &rule->datatype_size);
    // sdkmsg count
    tmp = get_number(tmp, &cmd, &rule->sdk_count);
    if (rule->sdk_count == 0) return rule;

    // skip hidden sdkmsg size
    uint64_t sdksize = Get64(tmp);

    tmp += sizeof(uint64_t);
    // sdkmsg create array
    unsigned char* pheader = tmp;
    struct stsdk* sdkarray = (struct stsdk*)calloc(rule->sdk_count, sizeof(struct stsdk));
    for(int i = 0; i < rule->sdk_count; i++){
        /*
            type
            id
            name
            timestamp
            target count
            targets
        */
        struct stsdk sdk;
        memset(&sdk, 0, sizeof(sdk));
        tmp = get_string(tmp, &cmd, &sdk.sdktype, &sdk.sdktype_size);
        tmp = get_string(tmp, &cmd, &sdk.sdkid, &sdk.sdkid_size);
        tmp = get_string(tmp, &cmd, &sdk.sdkname, &sdk.sdkname_size);
        tmp = get_string(tmp, &cmd, &sdk.timestamp, &sdk.timestamp_size);
        tmp = get_number(tmp, &cmd, &sdk.tgt_count);

        if (sdk.tgt_count > 0){
            sdk.tgt = (struct sttgt*)calloc(sdk.tgt_count, sizeof(struct sttgt));
            tmp = get_targets(tmp, sdk.tgt, sdk.tgt_count);
        }

        sdkarray[i] = sdk;
    }

    rule->sdk = sdkarray;
    return rule;
}

void free_strule(struct strule* rule){
    for(int i = 0; i < rule->sdk_count; i++){
        free(rule->sdk[i].tgt);
    }
    free(rule->sdk);
    free(rule);
}

///////////////////////////////////////////////////////////////
// go need function

void goImage2Shm(void* shm, void* img){
    struct stimg* cimg = (struct stimg*)img;
    image2shm(shm, cimg);
}

void goImageGet(void* img, void** data, uint64_t* size,
                void** ts, uint32_t* tss,
                void** cid, uint32_t* cids,
                void** cname, uint32_t* cnames,
                uint64_t* id, uint32_t* w, uint32_t* h)
{
    struct stimg* cimg = (struct stimg*)img;
    *data = cimg->data;
    *size = cimg->data_size;

    *ts = cimg->timestamp;
    *tss = cimg->timestamp_size;

    *cid = cimg->camera_id;
    *cids = cimg->camera_id_size;

    *cname = cimg->camera_name;
    *cnames = cimg->camera_name_size;

    *id = cimg->id;
    *w = cimg->width;
    *h = cimg->height;
}

void goTargetsDel(void* tgts){
    if (tgts) {
        struct sttgt* t = (struct sttgt*)tgts;
        free(t);
    }
}

void* goTargetsNew(const uint32_t size){
    struct sttgt* t = (struct sttgt*)calloc(size, sizeof(struct sttgt));
    return t;
}

void goTargetAdd(void* tgts, int idx, uint64_t id, char* type, uint32_t tsize, int32_t confidence,
                    int left, int top, int right, int bottom,
                    unsigned char* feat, uint32_t featsize,
                    unsigned char* attr, uint32_t attrsize)
{
    struct sttgt* ts = (struct sttgt*)tgts;
    struct sttgt t;
    memset(&t, 0, sizeof(t));

    t.id = id;
    t.type = type;
    t.type_size = tsize;
    t.confidence = confidence;
    t.rect.left = left;
    t.rect.top = top;
    t.rect.right = right;
    t.rect.bottom = bottom;
    t.feature = feat;
    t.feature_size = featsize;
    t.attribute = attr;
    t.attribute_size = attrsize;

    ts[idx] = t;
}

void goSdkPut(void* shm, void* tgts, uint32_t tgtcnt,
                    char* type, uint32_t tsize,
                    char* timestamp, uint32_t tmsize)
{
    TResult* res = (TResult*)calloc(1, sizeof(TResult));
    res->count = tgtcnt;
    res->targets = (Target*)calloc(tgtcnt, sizeof(Target));

    struct sttgt* ts = (struct sttgt*)tgts;

    for(uint32_t i = 0; i < tgtcnt; i++){
        Target t;
        memset(&t, 0, sizeof(t));
        t.id = ts[i].id;
        t.confidence = ts[i].confidence;
        memcpy(&t.rect, &ts[i].rect, sizeof(t.rect));
        memcpy(t.type, ts[i].type, ts[i].type_size);
        t.feature = (char*)ts[i].feature;
        t.feature_size = ts[i].feature_size;
        t.attribute = (char*)ts[i].attribute;
        t.attribute_size = ts[i].attribute_size;

        res->targets[i] = t;
    }

    add_result2rule_in_shm(shm, res, type, tsize, timestamp, tmsize);
    free(res->targets);
    free(res);
}

//////////////////// unserialize

void goHandletrackGet(void* rule, void** data, uint32_t* size){
    struct strule* r = (struct strule*)rule;
    *data = r->handletrack;
    *size = r->handletrack_size;
}

void goDatatypeGet(void* rule, void** data, uint32_t* size){
    struct strule* r = (struct strule*)rule;
    *data = r->datatype;
    *size = r->datatype_size;
}

void goSdkArrayGet(void* rule, void** data, uint32_t* size){
    struct strule* r = (struct strule*)rule;
    *data = r->sdk;
    *size = r->sdk_count;
}

void goSdkGet(void* sdk, int idx, void** t, uint32_t* ts,
                    void** i, uint32_t* is, void** n, uint32_t* ns,
                    void** tm, uint32_t* tms, void** data, uint32_t* size)
{
    struct stsdk* ss = (struct stsdk*)sdk;
    struct stsdk* s = &ss[idx];

    *t = s->sdktype;
    *ts = s->sdktype_size;

    *i = s->sdkid;
    *is = s->sdkid_size;

    *n = s->sdkname;
    *ns = s->sdkname_size;

    *tm = s->timestamp;
    *tms = s->timestamp_size;
    *data = s->tgt;
    *size = s->tgt_count;
}

void goTargetGet(void* tgt, int idx, uint64_t* id, int32_t* c,
                        int32_t* l,int32_t* t,int32_t* r,int32_t* b,
                        void** tp, uint32_t* tps,
                        void** feat, uint32_t* fs, void** attr, uint32_t* attrs)
{
    struct sttgt* ta = (struct sttgt*)tgt;
    struct sttgt* tt = &ta[idx];

    *id = tt->id;
    *c = tt->confidence;
    *l = tt->rect.left;
    *t = tt->rect.top;
    *r = tt->rect.right;
    *b = tt->rect.bottom;
    *tp = tt->type;
    *tps = tt->type_size;
    *feat = tt->feature;
    *fs = tt->feature_size;
    *attr = tt->attribute;
    *attrs = tt->attribute_size;
}