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

#include <MemBroker/mem_broker.h>
#include "kp_struct.h"
#include "model_type.h"
#include "kdp2_inf_app_yolo.h"
//#include "demo_customize_inf_single_model.h"
#include "stdc_app.h"
#include "kdp2_host_stream.h"
#include "event_recorder.h"

//#define ENABLE_DBG_LOG

#ifdef ENABLE_DBG_LOG
#define dbg_log(__format__, ...) printf(""__format__, ##__VA_ARGS__)
#else
#define dbg_log(__format__, ...)
#endif

#define FDFR_MODEL_SIZE_ROW         90
#define FDFR_MODEL_SIZE_COL         90
#define MAX_RESULT_BOX              80

static bool _init_config_yolo_params = false;
extern unsigned int g_dwDrawBoxEnable;
extern unsigned int g_dwDrawBoxType;
extern unsigned int g_verbose_log;

/* ── In-place log trimmer ──────────────────────────────────────────────────────
 * Keeps /tmp/fw.log under 2 MB by overwriting in-place via STDOUT_FILENO.
 * Works only when stdout is redirected to a file (nohup ... > /tmp/fw.log).
 */
#define LOG_MAX_BYTES  (2u * 1024u * 1024u)
#define LOG_KEEP_BYTES (1u * 1024u * 1024u)

static void log_trim_if_needed(void)
{
    off_t sz = lseek(STDOUT_FILENO, 0, SEEK_END);
    if (sz < (off_t)LOG_MAX_BYTES) return;

    uint8_t *buf = (uint8_t *)malloc(LOG_KEEP_BYTES);
    if (!buf) return;
    fflush(stdout);
    off_t start = sz - (off_t)LOG_KEEP_BYTES;
    ssize_t got  = pread(STDOUT_FILENO, buf, LOG_KEEP_BYTES, start);
    if (got > 0) {
        lseek(STDOUT_FILENO, 0, SEEK_SET);
        write(STDOUT_FILENO, buf, (size_t)got);
        ftruncate(STDOUT_FILENO, (off_t)got);
        lseek(STDOUT_FILENO, 0, SEEK_END);
        printf("[LOG] trimmed to %u KB\n", LOG_KEEP_BYTES / 1024);
        fflush(stdout);
    }
    free(buf);
}

static kp_app_yolo_post_proc_config_t post_proc_params_v5s = {
    .prob_thresh = 0.15,
    .nms_thresh = 0.5,
    .max_detection_per_class = 100,
    .anchor_row = 3,
    .anchor_col = 6,
    .stride_size = 3,
    .reserved_size = 0,
    .data = {
        // anchors[3][6]
        10, 13, 16, 30, 33, 23,
        30, 61, 62, 45, 59, 119,
        116, 90, 156, 198, 373, 326,
        // strides[3]
        8, 16, 32,
    },
};

static kp_app_yolo_post_proc_config_t post_proc_params_v5_vd = {
    .prob_thresh = 0.15,
    .nms_thresh = 0.5,
    .max_detection_per_class = 100,
    .anchor_row = 3,
    .anchor_col = 6,
    .stride_size = 3,
    .reserved_size = 0,
    .data = {
        // anchors[3][6]
        7, 8, 16, 12, 11, 26,
        27, 20, 48, 36, 28, 69,
        95, 62, 70, 172, 184, 242,
        // strides[3]
        8, 16, 32,
    },
};

static void print_yolo_result(kp_app_yolo_result_t *yolo_data)
{
    unsigned int i = 0;
    static unsigned int dwResultCounts = 0;

    if (dwResultCounts > MAX_RESULT_BOX)
        dwResultCounts = MAX_RESULT_BOX;

    if (g_dwDrawBoxEnable) {
        for (i = 0 ; i < dwResultCounts; i++) {
            if (g_atDrawInfo[i].bDrawFlag && !g_dwDrawBoxType)
                setup_isp_privacy_mask(0, g_atDrawInfo[i].dwStartX, g_atDrawInfo[i].dwStartY, g_atDrawInfo[i].dwWidth, g_atDrawInfo[i].dwHeight);
        }
        memset(&g_atDrawInfo, 0, sizeof(g_atDrawInfo));
        dwResultCounts = yolo_data->box_count;

        g_dwResultCounts = 0;
        for (i = 0; i < yolo_data->box_count; i++) {
            if (g_dwOnlyPerson) {
                if (yolo_data->boxes[i].class_num != 0)
                    continue;
            }
            calculate_bbox_postion(&g_atDrawInfo[i], yolo_data->boxes[i].x1, yolo_data->boxes[i].y1,
                yolo_data->boxes[i].x2, yolo_data->boxes[i].y2, yolo_data->boxes[i].score, yolo_data->boxes[i].class_num);
            g_atDrawInfo[i].bDrawFlag = true;
            if(!g_dwDrawBoxType) {
                setup_isp_privacy_mask(1, g_atDrawInfo[i].dwStartX, g_atDrawInfo[i].dwStartY, g_atDrawInfo[i].dwWidth, g_atDrawInfo[i].dwHeight);
            }
            g_dwResultCounts++;
        }
        if(g_dwResultCounts > MAX_RESULT_BOX) { g_dwResultCounts = MAX_RESULT_BOX; }
    }

    for (i = 0; i < yolo_data->box_count; i++) {
        if (g_dwOnlyPerson) {
            if (yolo_data->boxes[i].class_num != 0)
                continue;
        }

        printf("[%s][%d] [AI coordinate] Count = %d x1 = %f y1 = %f x2 = %f y2 = %f score = %f class_num = %d\n", __func__, i,
            yolo_data->box_count, yolo_data->boxes[i].x1, yolo_data->boxes[i].y1,
            yolo_data->boxes[i].x2, yolo_data->boxes[i].y2, yolo_data->boxes[i].score, yolo_data->boxes[i].class_num);
    }
}

/****************************************************************
 * application header/result callback function (Please override the callback functions for other application verify)
 ****************************************************************/
int app_header_send_inference(uint32_t buf_addr, bool *bl_run_next_inference, void* arg, unsigned char* image_buffer, VMF_VSRC_SSM_OUTPUT_INFO_T* vsrc_ssm_info)
{
    HOST_STREAM_INIT_OPT_T* pInitOpt=(HOST_STREAM_INIT_OPT_T*)arg;
    unsigned int dwWidth = pInitOpt->tVEncoder[pInitOpt->dwInferenceStream].dwWidth;
    unsigned int dwHeight = pInitOpt->tVEncoder[pInitOpt->dwInferenceStream].dwHeight;
    int ret = 0;

    kp_image_format_t kp_image_format = KP_IMAGE_FORMAT_YUV420; //The default image format of host_stream/hico_mipi is KP_IMAGE_FORMAT_YUV420.

    if(pInitOpt->dwJobId == KDP2_INF_ID_APP_YOLO)
    {
        if (false == _init_config_yolo_params) {
            /****************************************************************
             * configure application pre/post-processing params
             ****************************************************************/
            _init_config_yolo_params = true;

            kdp2_ipc_app_yolo_post_proc_config_t *app_yolo_post_proc_config = (kdp2_ipc_app_yolo_post_proc_config_t *)buf_addr;
            kp_inference_header_stamp_t *header_stamp = &app_yolo_post_proc_config->header_stamp;

            header_stamp->magic_type = KDP2_MAGIC_TYPE_INFERENCE;
            header_stamp->total_size = sizeof(kdp2_ipc_app_yolo_post_proc_config_t);
            header_stamp->total_image = 1;
            header_stamp->image_index = 1;
            header_stamp->job_id = KDP2_JOB_ID_APP_YOLO_CONFIG_POST_PROC;

            app_yolo_post_proc_config->model_id = pInitOpt->dwModelId;
            app_yolo_post_proc_config->param_size = sizeof(kp_app_yolo_post_proc_config_t);
            app_yolo_post_proc_config->set_or_get = 1;


            switch (pInitOpt->dwModelId)
            {
            case KNERON_YOLOV5S_COCO80_640_640_3:
            case KNERON_YOLOV5S_PersonBottleChairPottedplant4_288_640_3:
            case KNERON_YOLOV5S_PersonBicycleCarMotorcycleBusTruckCatDog8_256_480_3:
                post_proc_params_v5s.prob_thresh = pInitOpt->fThreshold;
                memcpy((void *)app_yolo_post_proc_config->param_data, (void *)&post_proc_params_v5s, sizeof(kp_app_yolo_post_proc_config_t));
                break;
            case KNERON_YOLOV5S0375_PersonBicycleCarMotorcycleBusTruck6_256_352_3:
                post_proc_params_v5_vd.prob_thresh = pInitOpt->fThreshold;
                memcpy((void *)app_yolo_post_proc_config->param_data, (void *)&post_proc_params_v5_vd, sizeof(kp_app_yolo_post_proc_config_t));
                break;
            default:
                // cannot find matched post-proc config
                printf("%s, model_id = %d, cannot find matched post-proc config\n", __func__, pInitOpt->dwModelId);
                break;
            }

            return KP_SUCCESS;
        }

        kdp2_ipc_app_yolo_inf_header_t *app_yolo_header = (kdp2_ipc_app_yolo_inf_header_t *)buf_addr;
        kp_inference_header_stamp_t *header_stamp = &app_yolo_header->header_stamp;
        static int inf_number = 0;
        inf_number = inf_number+1;
        inf_number &= 65535;

        header_stamp->magic_type = KDP2_MAGIC_TYPE_INFERENCE;
        header_stamp->total_size = sizeof(kdp2_ipc_app_yolo_inf_header_t) + (uint32_t)(vsrc_ssm_info->dwWidth*vsrc_ssm_info->dwHeight*1.5);
        header_stamp->total_image = 1;
        header_stamp->image_index = 1;
        header_stamp->job_id = KDP2_INF_ID_APP_YOLO;

        app_yolo_header->inf_number = inf_number;
        app_yolo_header->model_id = pInitOpt->dwModelId;
        app_yolo_header->width = dwWidth;
        app_yolo_header->height = dwHeight;

        app_yolo_header->image_format = kp_image_format;
        app_yolo_header->model_normalize = KP_NORMALIZE_KNERON;//KP_NORMALIZE_DISABLE;
        printf("[SEND] inf_number = %d \n", inf_number);
        //memcpy((void *)(buf_addr + sizeof(kdp2_ipc_app_yolo_inf_header_t)), image_buffer, image_buffer_size);
        ret = dma2d_copy(pInitOpt->ptDmaInfo->ptDmaHandle, pInitOpt->ptDmaInfo->ptDmaDesc, (void *)(buf_addr + sizeof(kdp2_ipc_app_yolo_inf_header_t)), (void*)image_buffer, vsrc_ssm_info);
        if(ret){
            printf("dma2d_copy failed\n");
        }

        //_inference_number++;

        /****************************************************************
         * finish application - close send thead
         ****************************************************************/
        //if (_loop_time <= _inference_number)
        //    *bl_run_next_inference = false;
    }
/*    
    else if(pInitOpt->dwJobId == DEMO_KL630_CUSTOMIZE_INF_SINGLE_MODEL_JOB_ID)
    {
        demo_customize_inf_single_model_header_t *pAppSingle = (demo_customize_inf_single_model_header_t *)buf_addr;
        kp_inference_header_stamp_t *header_stamp = &pAppSingle->header_stamp;

        header_stamp->magic_type = KDP2_MAGIC_TYPE_INFERENCE;
        header_stamp->total_size = sizeof(demo_customize_inf_single_model_header_t) + (uint32_t)(vsrc_ssm_info->dwWidth*vsrc_ssm_info->dwHeight*1.5);
        header_stamp->total_image = 1;
        header_stamp->image_index = 0;
        header_stamp->job_id = DEMO_KL630_CUSTOMIZE_INF_SINGLE_MODEL_JOB_ID;

        pAppSingle->width = dwWidth;
        pAppSingle->height = dwHeight;

        //memcpy((void *)(buf_addr + sizeof(demo_customize_inf_single_model_header_t)), image_buffer, image_buffer_size);
        ret = dma2d_copy(pInitOpt->ptDmaInfo->ptDmaHandle, pInitOpt->ptDmaInfo->ptDmaDesc, (void *)(buf_addr + sizeof(demo_customize_inf_single_model_header_t)), (void*)image_buffer, vsrc_ssm_info);
        if(ret){
            printf("dma2d_copy failed\n");
        }
    }
*/    
    else if(pInitOpt->dwJobId == STDC_JOB_ID)
    {
        stdc_inf_header_t *pStdcHdr = (stdc_inf_header_t *)buf_addr;
        kp_inference_header_stamp_t *header_stamp = &pStdcHdr->header_stamp;

        header_stamp->magic_type = KDP2_MAGIC_TYPE_INFERENCE;
        header_stamp->total_size = sizeof(stdc_inf_header_t) + (uint32_t)(vsrc_ssm_info->dwWidth*vsrc_ssm_info->dwHeight*1.5);
        header_stamp->total_image = 1;
        header_stamp->image_index = 0;
        header_stamp->job_id = STDC_JOB_ID;

        pStdcHdr->width  = dwWidth;
        pStdcHdr->height = dwHeight;
        pStdcHdr->model_id = pInitOpt->dwModelId;
        pStdcHdr->fps = pInitOpt->fFps;

        static int stdc_send_count = 0;
        if (++stdc_send_count % 30 == 1)
            printf("[STDC SEND] frame=%d %dx%d\n", stdc_send_count, dwWidth, dwHeight);

        ret = dma2d_copy(pInitOpt->ptDmaInfo->ptDmaHandle, pInitOpt->ptDmaInfo->ptDmaDesc, (void *)(buf_addr + sizeof(stdc_inf_header_t)), (void*)image_buffer, vsrc_ssm_info);
        if(ret){
            printf("dma2d_copy failed\n");
        }
    }
    else
    {
        printf("[%s] Error: Job ID %u\n", __FUNCTION__, pInitOpt->dwJobId);
        return KP_FW_ERROR_UNKNOWN_APP;
    }

    /* Take VMF_SNAP if event_recorder has a pending request */
    event_recorder_provide_frame();

    return KP_SUCCESS;
}

int app_header_recv_inference(uint32_t buf_addr, bool *bl_run_next_inference)
{
    kp_inference_header_stamp_t *header_stamp = (kp_inference_header_stamp_t *)buf_addr;

    if (KDP2_INF_ID_APP_YOLO == header_stamp->job_id) {
        /****************************************************************
         * receive & dump application result
         ****************************************************************/
        if (KP_SUCCESS != header_stamp->status_code) {
            printf("[%s] Error: Run application fail %u\n", __func__, header_stamp->status_code);
            return header_stamp->status_code;
        }

        kdp2_ipc_app_yolo_result_t *app_yolo_result = (kdp2_ipc_app_yolo_result_t *)header_stamp;
        kp_app_yolo_result_t *yolo_data = &app_yolo_result->yolo_data;
        printf("[RECV] inf_number = %d \n", app_yolo_result->inf_number);
        print_yolo_result(yolo_data);
        /****************************************************************
         * finish application - close receive thead
         ****************************************************************/
        //if ((_loop_time - 1) <= app_yolo_result->inf_number)
        //    *bl_run_next_inference = false;
    }
    else if (KDP2_JOB_ID_APP_YOLO_CONFIG_POST_PROC == header_stamp->job_id) {
        /****************************************************************
         * receive pre/post-processing configuration respones
         ****************************************************************/
        if (KP_SUCCESS != header_stamp->status_code) {
            printf("[%s] Error: config post-processing params fail %u\n", __func__, header_stamp->status_code);
            return header_stamp->status_code;
        }

        printf("[%s] config post-processing params success.\n", __func__);
    }
/*    
    else if(DEMO_KL630_CUSTOMIZE_INF_SINGLE_MODEL_JOB_ID == header_stamp->job_id)
    {
        printf("[%s] Job ID %u\n", __FUNCTION__, header_stamp->job_id);
    }
*/    
    else if(STDC_JOB_ID == header_stamp->job_id)
    {
        if (KP_SUCCESS != header_stamp->status_code) {
            printf("[%s] STDC inference error %u\n", __func__, header_stamp->status_code);
            return header_stamp->status_code;
        }
        stdc_inf_result_t *stdc_res = (stdc_inf_result_t *)header_stamp;
        stdc_analysis_t   *ana      = &stdc_res->stdc_result.analysis;

        /* Copy seg map to shared globals for draw-box thread */
        if (ana->seg_width > 0 && ana->seg_height > 0 &&
            ana->seg_width * ana->seg_height <= STDC_SEG_MAP_MAX) {
            pthread_mutex_lock(&g_stdc_seg_mutex);
            memcpy(g_stdc_seg_map, stdc_res->stdc_result.seg_map,
                   ana->seg_width * ana->seg_height);
            g_stdc_seg_w = ana->seg_width;
            g_stdc_seg_h = ana->seg_height;
            pthread_mutex_unlock(&g_stdc_seg_mutex);
        }

        /* ── 1. Console log — class ratios + status (at most once per second) ── */
        {
            static struct timeval s_last_log = {0, 0};
            struct timeval now;
            gettimeofday(&now, NULL);
            long elapsed_us = (now.tv_sec  - s_last_log.tv_sec)  * 1000000L
                            + (now.tv_usec - s_last_log.tv_usec);
            if (elapsed_us >= 1000000L) {
                s_last_log = now;
                log_trim_if_needed();   /* keep /tmp/fw.log under LOG_MAX_BYTES */
                if (g_verbose_log) {
                    printf("[STDC] frame=%u mov=%d diff=%.1f "
                           "bunker=%.1f%% car=%.1f%% grass=%.1f%% greenery=%.1f%% "
                           "person=%.1f%% pond=%.1f%% road=%.1f%% tree=%.1f%%\n",
                           ana->frame_count, ana->is_moving, ana->motion_diff,
                           ana->class_ratio[STDC_CLASS_BUNKER]   * 100.0f,
                           ana->class_ratio[STDC_CLASS_CAR]       * 100.0f,
                           ana->class_ratio[STDC_CLASS_GRASS]     * 100.0f,
                           ana->class_ratio[STDC_CLASS_GREENERY]  * 100.0f,
                           ana->class_ratio[STDC_CLASS_PERSON]    * 100.0f,
                           ana->class_ratio[STDC_CLASS_POND]      * 100.0f,
                           ana->class_ratio[STDC_CLASS_ROAD]      * 100.0f,
                           ana->class_ratio[STDC_CLASS_TREE]      * 100.0f);

                    /* ── 2. Grass / motion state ── */
                    if (ana->on_grass)
                        printf("[STDC] ON GRASS: %.1fs%s\n",
                               ana->grass_duration_sec,
                               ana->grass_warning ? " *** GRASS WARNING ***" : "");
                    else
                        printf("[STDC] ON ROAD\n");
                }
            }
        }

        /* ── 3. Warning log (on state change — matches Python sections H/I/J) ── */
        {
            static int s_last_warn = -1;
            int warn_mask = (ana->collision_risk   ? 0x001 : 0)
                          | (ana->person_warning   ? 0x002 : 0)
                          | (ana->car_warning      ? 0x004 : 0)
                          | (ana->grass_warning    ? 0x008 : 0)
                          | (ana->tree_dense       ? 0x010 : 0)
                          | (ana->tree_approaching ? 0x020 : 0)
                          | (ana->bunker_warning   ? 0x040 : 0)
                          | (ana->pond_warning     ? 0x080 : 0)
                          | (ana->greenery_warning ? 0x100 : 0);
            if (warn_mask != s_last_warn) {
                if (ana->collision_risk)
                    printf("[STDC WARN] COLLISION RISK (person=%.1f%% car=%.1f%% "
                           "tree=%.1f%% bunker=%.1f%% pond=%.1f%%)\n",
                           ana->col_person_ratio*100, ana->col_car_ratio*100,
                           ana->col_tree_ratio*100, ana->col_bunker_ratio*100,
                           ana->col_pond_ratio*100);
                if (ana->person_warning)
                    printf("[STDC WARN] PERSON %.1f%%\n",
                           ana->class_ratio[STDC_CLASS_PERSON]*100);
                if (ana->car_warning)
                    printf("[STDC WARN] CAR %.1f%%\n",
                           ana->class_ratio[STDC_CLASS_CAR]*100);
                if (ana->greenery_warning)
                    printf("[STDC WARN] GREENERY AREA %.1f%%\n",
                           ana->class_ratio[STDC_CLASS_GREENERY]*100);
                if (ana->bunker_warning)
                    printf("[STDC WARN] BUNKER %.1f%%\n",
                           ana->class_ratio[STDC_CLASS_BUNKER]*100);
                if (ana->pond_warning)
                    printf("[STDC WARN] POND %.1f%%\n",
                           ana->class_ratio[STDC_CLASS_POND]*100);
                if (ana->tree_approaching)
                    printf("[STDC WARN] TREE APPROACHING (roi_growth=%.1f%%)\n",
                           ana->tree_roi_growth*100);
                if (ana->tree_dense)
                    printf("[STDC WARN] TREE DENSE AREA %.1f%%\n",
                           ana->class_ratio[STDC_CLASS_TREE]*100);
                s_last_warn = warn_mask;
            }
        }

        /* ── 4. Visual overlay via g_atDrawInfo (rendered by draw_box DMA thread) ──
         *
         * Requires DrawBoxEnable=1 in host_stream.ini / demo_rtsp.sh.
         * The draw_box thread calls draw_rect() for each entry 0..g_dwResultCounts-1.
         *
         * Layout on 1920×1080 (stream0):
         *
         * [center] Collision ROI outline  x=480  y=270  w=960  h=486
         *          (always present; turns into a thick double-border on collision)
         * [center] Forward ROI outline    x=576  y=594  w=768  h=432
         *          (outer bbox of trapezoid: bottom-left 30%, bottom-right 70%)
         *
         * [top-left] Warning boxes 200×55px, stride 63px (when warning active):
         *   y=10  collision_risk      y=199 grass_warning
         *   y=73  person_warning      y=262 tree_dense
         *   y=136 car_warning         y=325 tree_approaching
         *
         * [right edge] Class presence 40×55px, stride 63px (when ratio > threshold):
         *   ci=0 bunker   y=10   (thr 1%)
         *   ci=1 car      y=73   (thr 5%)
         *   ci=2 grass    y=136  (thr 10%)
         *   ci=3 greenery y=199  (thr 20%)
         *   ci=4 person   y=262  (thr 1%)
         *   ci=5 pond     y=325  (thr 1%)
         *   ci=6 road     y=388  (thr 50%)
         *   ci=7 tree     y=451  (thr 30%)
         */
        if (g_dwDrawBoxEnable) {
            unsigned int cnt = 0;
            DETECT_INFO *di = g_atDrawInfo;

            /* Collision ROI outline — always drawn */
            di[cnt].dwStartX = 480; di[cnt].dwStartY = 270;
            di[cnt].dwWidth  = 960; di[cnt].dwHeight = 486;
            di[cnt].fScore = 0; di[cnt].dwClass = 0; di[cnt].bDrawFlag = true;
            cnt++;

            /* When collision active: draw a second inner outline for emphasis */
            if (ana->collision_risk) {
                di[cnt].dwStartX = 492; di[cnt].dwStartY = 282;
                di[cnt].dwWidth  = 936; di[cnt].dwHeight = 462;
                di[cnt].fScore = 0; di[cnt].dwClass = 0; di[cnt].bDrawFlag = true;
                cnt++;
            }

            /* Forward ROI outline — always drawn */
            di[cnt].dwStartX = 576; di[cnt].dwStartY = 594;
            di[cnt].dwWidth  = 768; di[cnt].dwHeight = 432;
            di[cnt].fScore = 0; di[cnt].dwClass = 0; di[cnt].bDrawFlag = true;
            cnt++;

            /* Side warning/class panels are intentionally disabled.
             * Keep only central ROI overlays to avoid per-class side boxes. */

            g_dwResultCounts = cnt;
        }

        /* Drive violation event state machine */
        event_recorder_update(ana);
    }
    else {
        printf("[%s] Error: Job ID %u\n", __FUNCTION__, header_stamp->job_id);
        return KP_FW_ERROR_UNKNOWN_APP;
    }

    return KP_SUCCESS;
}

