#include <stdint.h>
#include <float.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/time.h>

#include <libavformat/avformat.h>
#include <libavcodec/avcodec.h>
#include <libavutil/pixdesc.h>
#include <libswscale/swscale.h>
#include <libavutil/imgutils.h>

#include "rknn_api.h"

#define LABEL_NALE_TXT_PATH "./coco_80_labels_list.txt"
#define OBJ_NAME_MAX_SIZE 6
#define OBJ_NUMB_MAX_SIZE 15
#define OBJ_CLASS_NUM 37
#define PROP_BOX_SIZE (5 + OBJ_CLASS_NUM)
#define NMS_THRESH 0.55f
#define BOX_THRESH 0.75f
#define MODEL_PATH "./model.rknn"
#define RKNN_WIDTH 640
#define RKNN_HEIGHT 640
#define MAX_OUTPUT_NUM 3

rknn_context ctx = 0;
rknn_input_output_num io_num = {0};
rknn_tensor_attr *input_attrs = NULL;
rknn_tensor_attr *output_attrs = NULL;
rknn_tensor_mem *input_mems[1] = {NULL};
rknn_tensor_mem **output_mems = NULL;

const char *model_path = MODEL_PATH;
const float nms_threshold = NMS_THRESH;
const float box_conf_threshold = BOX_THRESH;

int img_width = 640;
int img_height = 640;

typedef struct _BOX_RECT {
    int left;
    int right;
    int top;
    int bottom;
} BOX_RECT;

typedef struct __detect_result_t {
    char name[OBJ_NAME_MAX_SIZE];
    BOX_RECT box;
    float prop;
    int id;
} detect_result_t;

typedef struct _detect_result_group_t {
    int count;
    detect_result_t results[OBJ_NUMB_MAX_SIZE];
} detect_result_group_t;

static inline int64_t getCurrentTimeUs() {
    struct timeval tv;
    gettimeofday(&tv, NULL);
    return tv.tv_sec * 1000000 + tv.tv_usec;
}

static int rknn_init_model() {
    int ret = rknn_init(&ctx, model_path, 0, 0, NULL);
    if (ret < 0) {
        printf("rknn_init fail! ret=%d\n", ret);
        return -1;
    }

    ret = rknn_query(ctx, RKNN_QUERY_IN_OUT_NUM, &io_num, sizeof(io_num));
    if (ret != RKNN_SUCC) {
        printf("rknn_query fail! ret=%d\n", ret);
        rknn_destroy(ctx);
        return -1;
    }

    input_attrs = (rknn_tensor_attr *)malloc(io_num.n_input * sizeof(rknn_tensor_attr));
    if (!input_attrs) return -1;
    for (uint32_t i = 0; i < io_num.n_input; i++) {
        input_attrs[i].index = i;
        ret = rknn_query(ctx, RKNN_QUERY_INPUT_ATTR, &input_attrs[i], sizeof(rknn_tensor_attr));
        if (ret < 0) return -1;
    }

    output_attrs = (rknn_tensor_attr *)malloc(io_num.n_output * sizeof(rknn_tensor_attr));
    if (!output_attrs) return -1;
    for (uint32_t i = 0; i < io_num.n_output; i++) {
        output_attrs[i].index = i;
        ret = rknn_query(ctx, RKNN_QUERY_NATIVE_NHWC_OUTPUT_ATTR, &output_attrs[i], sizeof(rknn_tensor_attr));
        if (ret != RKNN_SUCC) return -1;
    }

    input_mems[0] = rknn_create_mem(ctx, input_attrs[0].size_with_stride);
    if (!input_mems[0]) return -1;

    output_mems = (rknn_tensor_mem **)malloc(io_num.n_output * sizeof(rknn_tensor_mem *));
    if (!output_mems) return -1;
    for (uint32_t i = 0; i < io_num.n_output; i++) {
        output_mems[i] = rknn_create_mem(ctx, output_attrs[i].size_with_stride);
        if (!output_mems[i]) return -1;
    }

    return 0;
}

static void rknn_release() {
    if (input_mems[0]) rknn_destroy_mem(ctx, input_mems[0]);
    if (output_mems) {
        for (uint32_t i = 0; i < io_num.n_output; i++)
            rknn_destroy_mem(ctx, output_mems[i]);
        free(output_mems);
    }
    if (input_attrs) free(input_attrs);
    if (output_attrs) free(output_attrs);
    if (ctx) rknn_destroy(ctx);
}

static float sigmoid(float x) {
    return 1.0f / (1.0f + expf(-x));
}

static int process_native_nhwc(int8_t *input, int *anchor, int grid_h, int grid_w, int height, int width, int stride,
                               float *boxes, float *boxScores, int *classId, float threshold, int32_t zp, float scale,
                               int *box_count, int max_box_num) {
    int validCount = 0;
    int8_t thres_i8 = (int8_t)(threshold / scale + zp);
    int anchor_per_branch = 3;
    int align_c = PROP_BOX_SIZE * anchor_per_branch;

    for (int h = 0; h < grid_h; h++) {
        for (int w = 0; w < grid_w; w++) {
            for (int a = 0; a < anchor_per_branch; a++) {
                int hw_offset = h * grid_w * align_c + w * align_c + a * PROP_BOX_SIZE;
                int8_t *hw_ptr = input + hw_offset;
                int8_t box_confidence = hw_ptr[4];
                if (box_confidence >= thres_i8) {
                    int8_t maxClassProbs = hw_ptr[5];
                    int maxClassId = 0;
                    for (int k = 1; k < OBJ_CLASS_NUM; k++) {
                        if (hw_ptr[5 + k] > maxClassProbs) {
                            maxClassId = k;
                            maxClassProbs = hw_ptr[5 + k];
                        }
                    }

                    float box_conf_f32 = (box_confidence - zp) * scale;
                    float class_prob_f32 = (maxClassProbs - zp) * scale;
                    float limit_score = box_conf_f32 * class_prob_f32;

                    if (limit_score > threshold) {
                        if (*box_count >= max_box_num) return validCount;

                        float box_x = (hw_ptr[0] - zp) * scale * 2.0f - 0.5f;
                        float box_y = (hw_ptr[1] - zp) * scale * 2.0f - 0.5f;
                        float box_w = (hw_ptr[2] - zp) * scale * 2.0f;
                        float box_h = (hw_ptr[3] - zp) * scale * 2.0f;

                        box_x = (box_x + w) * stride;
                        box_y = (box_y + h) * stride;
                        box_w *= anchor[a * 2];
                        box_h *= anchor[a * 2 + 1];

                        box_x -= box_w / 2.0f;
                        box_y -= box_h / 2.0f;

                        int idx = (*box_count) * 4;
                        boxes[idx] = box_x;
                        boxes[idx + 1] = box_y;
                        boxes[idx + 2] = box_w;
                        boxes[idx + 3] = box_h;
                        boxScores[*box_count] = limit_score;
                        classId[*box_count] = maxClassId;
                        (*box_count)++;
                        validCount++;
                    }
                }
            }
        }
    }
    return validCount;
}

static int post_process(int8_t *input0, int8_t *input1, int8_t *input2, int model_in_h, int model_in_w,
                        float conf_threshold, float nms_threshold, float scale_w, float scale_h,
                        int32_t *qnt_zps, float *qnt_scales, detect_result_group_t *group) {
    const int max_detections = OBJ_NUMB_MAX_SIZE * 10;
    float boxes[max_detections * 4];
    float boxScores[max_detections];
    int classId[max_detections];
    int box_count = 0;

    // Process stride 8
    int stride0 = 8;
    int grid_h0 = model_in_h / stride0;
    int grid_w0 = model_in_w / stride0;
    const int anchor0[6] = {10, 13, 16, 30, 33, 23};
    process_native_nhwc(input0, (int *)anchor0, grid_h0, grid_w0, model_in_h, model_in_w, stride0,
                        boxes, boxScores, classId, conf_threshold, qnt_zps[0], qnt_scales[0],
                        &box_count, max_detections);

    // Process stride 16
    int stride1 = 16;
    int grid_h1 = model_in_h / stride1;
    int grid_w1 = model_in_w / stride1;
    const int anchor1[6] = {30, 61, 62, 45, 59, 119};
    process_native_nhwc(input1, (int *)anchor1, grid_h1, grid_w1, model_in_h, model_in_w, stride1,
                        boxes + box_count * 4, boxScores + box_count, classId + box_count, conf_threshold, qnt_zps[1], qnt_scales[1],
                        &box_count, max_detections);

    // Process stride 32
    int stride2 = 32;
    int grid_h2 = model_in_h / stride2;
    int grid_w2 = model_in_w / stride2;
    const int anchor2[6] = {116, 90, 156, 198, 373, 326};
    process_native_nhwc(input2, (int *)anchor2, grid_h2, grid_w2, model_in_h, model_in_w, stride2,
                        boxes + box_count * 4, boxScores + box_count, classId + box_count, conf_threshold, qnt_zps[2], qnt_scales[2],
                        &box_count, max_detections);

    if (box_count <= 0) return 0;

    // Non-Maximum Suppression (NMS)
    int *indices = (int *)malloc(box_count * sizeof(int));
    for (int i = 0; i < box_count; i++) indices[i] = i;
    quick_sort_indice_inverse(boxScores, 0, box_count - 1, indices);

    int unique_classes[OBJ_CLASS_NUM] = {0};
    int unique_count = 0;
    for (int i = 0; i < box_count; i++) {
        int cid = classId[indices[i]];
        int found = 0;
        for (int j = 0; j < unique_count; j++) {
            if (unique_classes[j] == cid) {
                found = 1;
                break;
            }
        }
        if (!found) unique_classes[unique_count++] = cid;
    }

    for (int i = 0; i < unique_count; i++) {
        int class_id = unique_classes[i];
        for (int j = 0; j < box_count; j++) {
            if (classId[indices[j]] == class_id && indices[j] != -1) {
                int n = indices[j];
                for (int k = j + 1; k < box_count; k++) {
                    if (classId[indices[k]] == class_id && indices[k] != -1) {
                        int m = indices[k];
                        float x1 = boxes[n * 4];
                        float y1 = boxes[n * 4 + 1];
                        float w1 = boxes[n * 4 + 2];
                        float h1 = boxes[n * 4 + 3];
                        float x2 = boxes[m * 4];
                        float y2 = boxes[m * 4 + 1];
                        float w2 = boxes[m * 4 + 2];
                        float h2 = boxes[m * 4 + 3];
                        float iou = (w1 * h1 + w2 * h2 - (w1 * h1 + w2 * h2 - (x1 - x2) * (x1 - x2) - (y1 - y2) * (y1 - y2))) / (w1 * h1 + w2 * h2 - (x1 - x2) * (x1 - x2) - (y1 - y2) * (y1 - y2));
                        if (iou > nms_threshold) indices[k] = -1;
                    }
                }
            }
        }
    }

    int last_count = 0;
    for (int i = 0; i < box_count; i++) {
        if (indices[i] != -1 && last_count < OBJ_NUMB_MAX_SIZE) {
            int n = indices[i];
            group->results[last_count].box.left = (int)(boxes[n * 4] / scale_w);
            group->results[last_count].box.top = (int)(boxes[n * 4 + 1] / scale_h);
            group->results[last_count].box.right = (int)((boxes[n * 4] + boxes[n * 4 + 2]) / scale_w);
            group->results[last_count].box.bottom = (int)((boxes[n * 4 + 1] + boxes[n * 4 + 3]) / scale_h);
            group->results[last_count].prop = boxScores[n];
            group->results[last_count].id = classId[n];
            last_count++;
        }
    }
    group->count = last_count;

    free(indices);
    return 0;
}

int rknn_infer_image(unsigned char *input_data, detect_result_group_t *detect_result_group) {
    int ret;
    int width = input_attrs[0].dims[2];
    int stride = input_attrs[0].w_stride;

    if (width == stride) {
        memcpy(input_mems[0]->virt_addr, input_data, width * input_attrs[0].dims[1] * input_attrs[0].dims[3]);
    } else {
        int height = input_attrs[0].dims[1];
        int channel = input_attrs[0].dims[3];
        uint8_t *src_ptr = input_data;
        uint8_t *dst_ptr = (uint8_t *)input_mems[0]->virt_addr;
        for (int h = 0; h < height; h++) {
            memcpy(dst_ptr, src_ptr, width * channel);
            src_ptr += width * channel;
            dst_ptr += stride * channel;
        }
    }

    ret = rknn_set_io_mem(ctx, input_mems[0], &input_attrs[0]);
    if (ret < 0) return -1;

    for (uint32_t i = 0; i < io_num.n_output; i++) {
        ret = rknn_set_io_mem(ctx, output_mems[i], &output_attrs[i]);
        if (ret < 0) return -1;
    }

    ret = rknn_run(ctx, NULL);
    if (ret < 0) return -1;

    float scale_w = (float)RKNN_WIDTH / img_width;
    float scale_h = (float)RKNN_HEIGHT / img_height;

    float *out_scales = (float *)malloc(io_num.n_output * sizeof(float));
    int32_t *out_zps = (int32_t *)malloc(io_num.n_output * sizeof(int32_t));
    for (uint32_t i = 0; i < io_num.n_output; i++) {
        out_scales[i] = output_attrs[i].scale;
        out_zps[i] = output_attrs[i].zp;
    }

    post_process((int8_t *)output_mems[0]->virt_addr,
                 (int8_t *)output_mems[1]->virt_addr,
                 (int8_t *)output_mems[2]->virt_addr,
                 RKNN_HEIGHT, RKNN_WIDTH,
                 box_conf_threshold, nms_threshold,
                 scale_w, scale_h,
                 out_zps, out_scales,
                 detect_result_group);

    free(out_scales);
    free(out_zps);
    return 0;
}









еще быстрее это 1 потчный арм