#include "shmparser.h"

#include <stdlib.h>
#include <string.h>
#include <stdint.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,
    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;
    memset(&img, 0, sizeof(img));

    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));
    // no sdk content

    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 %d But Now %d\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
    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);

        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);
}