
#include <string.h>
#include "cmsis_os2.h"
#include "project.h"
#include "kmdw_power_manager.h"

#include "kmdw_console.h"
#include "kdrv_power.h"
#include "kdrv_scu_ext.h"
#include "kdp_system.h"

#include "usbd_hal.h"

#include "model_type.h"
#include "buffer_object.h"

#include "kmdw_camera.h"//review merge with 720 code base, kdrv_camera.h
#include "kmdw_fifoq_manager.h"
#include "kdp2_ipc_cmd.h"
#include "kdp2_inf_app_yolo.h"
#include "kdp2_usb_companion.h"

//for display format, e.g. #define V2K_PIX_FMT_RGB565  v2k_fourcc('R', 'G', 'B', 'P')
#include "kmdw_display.h"

#define FW_BUILD // for host_mipi.h
#include "host_mipi.h"

#include "kmdw_camera.h"
#include "kdrv_timer.h"
#include "kdrv_i2c.h"


#define BUF_HEADER_SIZE sizeof(buf_header_t)
#define OFFSET_INF_TO_HEAD_BUF (sizeof(buf_header_t) - sizeof(kdp2_ipc_app_yolo_inf_header_t))

#define IMG_BUF_TO_HEAD_BUF(img_buf) \
    (uint32_t)((uint32_t)img_buf - BUF_HEADER_SIZE)

#define NUM_MIPI_INIT_BUFS 2 // 2 (ping-pong buffers) or 3 (triple-buffers for skipping frames)

#define NUM_DISPLAY_QUEUE_DEPTH 3

#define NUM_IMG_BUF 12
#define NUM_RESULT_BUF 12

#define IMG_BUF_SIZE (1 * 1024 * 1024)
#define RESULT_BUF_SIZE (400 * 1024)
#define CMD_BUF_SIZE (1 * 1024)

#if 0//KL720_Scott
#define JTAG_MAGIC_ADDRESS 0x1FFFFFFC
#define JTAG_MAGIC_VALUE 0xFEDCBA01
#define USB_BOOT_MAGIC_HB 0xaabbccdd
#define USB_BOOT_MAGIC_LB 0x11223344

#else
#define JTAG_MAGIC_ADDRESS 0x10100000
#define KDP2_BOOT_CONFIG_ADDRESS 0x10100100

#define JTAG_MAGIC_VALUE 0xFEDCBA01
#define BOOT_FROM_FLASH 0xA
typedef struct
{
    uint32_t boot_type;     // 0xA = flash-boot, others = usb-boot
    uint8_t loader_ver[4];  // fw loader version numbers
    uint8_t scpu_fw_ver[4]; // SCPU fw version numbers
    uint8_t ncpu_fw_ver[4]; // NCPU fw version numbers
} kdp2_boot_config_t;
#endif

//#define ENABLE_DBG_LOG
#ifdef ENABLE_DBG_LOG
#define dbg_log(__format__, ...) kmdw_printf("[kp host mipi]"__format__, ##__VA_ARGS__)
#else
#define dbg_log(__format__, ...)
#endif

//#define SHOW_SENSOR_IMAGE_ON_LCD
//debug: show sensor image directly without inference

//#define SHOW_INF_RESULT
#ifdef SHOW_INF_RESULT
static char* classmap[] = {"person","bicycle","car","motorbike","aeroplane","bus","train","truck","boat","traffic_light",
                           "fire_hydrant","stop_sign","parking_meter","bench","bird","cat","dog","horse","sheep","cow",
                           "elephant","bear","zebra","giraffe","backpack","umbrella","handbag","tie","suitcase","frisbee",
                           "skis","snowboard","sports_ball","kite","baseball_bat","baseball_glove","skateboard","surfboard","tennis_racket","bottle",
                           "wine_glass","cup","fork","knife","spoon","bowl","banana","apple","sandwich","orange",
                           "broccoli","carrot","hot_dog","pizza","donut","cake","chair","sofa","pottedplant","bed",
                           "diningtable","toilet","tvmonitor","laptop","mouse","remote","keyboard","cell_phone","microwave","oven",
                           "toaster","sink","refrigerator","book","clock","vase","scissors","teddy_bear","hair_drier","toothbrush"};
#endif

typedef struct
{
    uint32_t buf_address;
    uint32_t buf_size;
} kimg_buf_object_t;

typedef struct
{
    uint32_t img_width;
    uint32_t img_height;
    uint32_t img_format;
    uint32_t img_channel;
    uint32_t img_buf_size;
} mipi_cam_settings_t;

#define NUM_HOST_SENSOR 2

enum
{
    CAM_RGB_IDX = 0,
    CAM_NIR_IDX = 1,
};

camera_settings_t _cam_settings[NUM_HOST_SENSOR] = {0};

//static osMessageQueueId_t _img_queue = NULL;

//static bool _fifoq_mem_allocated = false;
//static bool _do_reset_queue = false;
//static bool _enable_inf_droppable = false;

#define FLAG_WAIT_USB_CONNECTION 0x1

static osThreadId_t _usb_cmd_tid = NULL;
static osMessageQueueId_t _display_img_queue = NULL;

static int _display_cam = CAM_RGB_IDX; // default send RGB images
static int dbg_mipi_cnt = 0;
static uint32_t _img_buf_start_addr_backup = 0;
static osThreadId_t _update_display_tid = NULL;

static osMutexId_t _mutex_result;
static kp_app_yolo_result_t *_p_yolo_data[NUM_HOST_SENSOR] = {NULL};

extern int32_t kmdw_model_load_model(int8_t model_info_index_p);
extern void display_init(uint32_t input_fmt, uint16_t xres, uint16_t yres, uint8_t cam_idx);
extern void display_exit(void);
extern uint32_t lcdc_kdp2_get_disp_idx(int *read_done_idx);
extern uint32_t lcdc_kdp2_set_disp_buf(uint32_t buf_addr, int write_done_idx);
extern uint32_t lcdc_kdp2_get_disp_buf(int cam_idx, int *disp_idx);
// vendor-specific control transfer setup packet notify
static void usb_user_link_status_callback(usbd_hal_link_status_t link_status)
{
    switch (link_status)
    {
    case USBD_STATUS_DISCONNECTED:
        kmdw_printf("USB is disconnected\n");
        break;

    case USBD_STATUS_CONFIGURED:
        kmdw_printf("USB is connected\n");
        osThreadFlagsSet(_usb_cmd_tid, FLAG_WAIT_USB_CONNECTION);
        break;
    }
}
static bool usb_user_control_callback(usbd_hal_setup_packet_t *setup)
{
    bool ret = false;

    dbg_log("control bRequest = 0x%x\n", setup->bRequest);

    switch (setup->bRequest)
    {
    case KDP2_CONTROL_REBOOT:
    {
        dbg_log("control reboot\n");
        kdrv_power_sw_reset();
        break;
    }
    case KDP2_CONTROL_SHUTDOWN:
    {
        dbg_log("control shutdown\n");
        kmdw_power_manager_shutdown();
        break;
    }
    case KDP2_CONTROL_FIFOQ_RESET:
    {
        dbg_log("control fifoq reset\n");

        // FIXME
        kmdw_printf("wakeup USB cmd thread\n");
        osThreadFlagsSet(_usb_cmd_tid, FLAG_WAIT_USB_CONNECTION);
        //osThreadFlagsSet(image_thread_id, 0x1);

        ret = true;
        break;
    }
#if 0
    case KDP2_CONTROL_FIFOQ_CONFIGURE:
    {
        if (_fifoq_mem_allocated)
            break; // already inited

        uint16_t arg1_image = setup->wValue;
        uint16_t arg2_result = setup->wIndex;

        uint32_t image_count = (arg1_image & 0x7) + 1;                         // lower 3 bits for number of image, 1~8
        uint32_t image_size = (10 * 1024) * (uint32_t)((arg1_image >> 3) + 1); // higher 13 bits for image buffer size in 10KB, 10KB~80MB

        uint32_t result_count = (arg2_result & 0x7) + 1;                         // lower 3 bits for number of image, 1~8
        uint32_t result_size = (10 * 1024) * (uint32_t)((arg2_result >> 3) + 1); // higher 13 bits for image buffer size in 10KB, 10KB~80MB

        ret = allocate_memory_for_inference_queue(image_count, image_size, result_count, result_size);

        break;
    }
    case KDP2_CONTROL_FIFOQ_ENABLE_DROPPABLE:
    {
        _enable_inf_droppable = (setup->wValue == 1);
        ret = true;
    }
#endif
    default:
        ret = false;
        break;
    }

    return ret;
}

static void send_to_inference_queue(uint32_t head_buf, int cam_idx)
{
    buf_header_t *bufHdr = (buf_header_t *)head_buf;
    kdp2_ipc_app_yolo_inf_header_t *infHdr = &bufHdr->inf_header;

    infHdr->header_stamp.magic_type = KDP2_MAGIC_TYPE_INFERENCE;
    infHdr->header_stamp.total_size = sizeof(kdp2_ipc_app_yolo_inf_header_t) + _cam_settings[cam_idx].img_buf_size;
    infHdr->header_stamp.job_id = KDP2_INF_ID_APP_YOLO;
    infHdr->header_stamp.status_code = KP_SUCCESS;
    infHdr->inf_number = cam_idx;
    infHdr->width = _cam_settings[cam_idx].img_width;
    infHdr->height = _cam_settings[cam_idx].img_height;
    infHdr->channel = _cam_settings[cam_idx].img_channel;//3
    infHdr->model_id = TINY_YOLO_V3_224_224_3;//211;//KNERON_YOLOV5S_COCO80_640_640_3;//
    infHdr->image_format = _cam_settings[cam_idx].img_format;
    infHdr->model_normalize = KP_NORMALIZE_KNERON;
//kmdw_printf("send_to_inference_queue,[infHdr] channel = %d, model_id=%d, image_format=%d\n", infHdr->channel, infHdr->model_id, infHdr->image_format);
    // non-blocking inference enqueue
    osStatus_t sts = kmdw_fifoq_manager_image_enqueue(1, 0, (uint32_t)infHdr, IMG_BUF_SIZE, 0, false);
    if (sts != osOK)
        kmdw_printf("(ISR) error !! kmdw_fifoq_manager_image_enqueue() failed, sts = %d\n", sts);
}

static void send_to_display_queue(uint32_t hico_buf_addr, int cam_idx)
{
    // send to host thru USB (bottom-half mechanism)
    buf_header_t *bufHdr = (buf_header_t *)hico_buf_addr;
    kimg_buf_object_t bufobj;
    bufobj.buf_address = hico_buf_addr;
    bufobj.buf_size = BUF_HEADER_SIZE + _cam_settings[cam_idx].img_buf_size;

    osStatus_t sts = osMessageQueuePut(_display_img_queue, (const void *)&bufobj, NULL, 0);
    if (sts != osOK)
        return;

    bufHdr->read_ref_count++;
}

static uint32_t retrieve_new_buffer_for_mipi()
{
    uint32_t new_head_buf_addr = 0;
    int buf_size;

    for (int i = 0; i < NUM_IMG_BUF; i++) // MAX try, not really a for-loop
    {
        uint32_t new_inf_buf_addr = 0;

        osStatus_t sts = kmdw_fifoq_manager_image_get_free_buffer(&new_inf_buf_addr, &buf_size, 0, true);
        if (sts != osOK) {
            kmdw_printf("(ISR %d) error !! kmdw_fifoq_manager_image_get_free_buffer() failed, sts = %d\n", dbg_mipi_cnt, sts);
            return 0;
        }

        new_head_buf_addr = new_inf_buf_addr - OFFSET_INF_TO_HEAD_BUF;
        buf_header_t *new_bufHdr = (buf_header_t *)new_head_buf_addr;

        if (new_bufHdr->read_ref_count == 0)
            break; // found available one
        else
        {
            // this buf is still in use, return it back to free buf queue
            sts = kmdw_fifoq_manager_image_put_free_buffer(new_inf_buf_addr, IMG_BUF_SIZE, 0);
            if (sts != osOK)
                kmdw_printf("(ISR %d) error !! kmdw_fifoq_manager_image_put_free_buffer() failed, sts = %d\n", dbg_mipi_cnt, sts);

            new_head_buf_addr = 0;
        }
    }

    if (new_head_buf_addr == 0)
    {
        kmdw_printf("(ISR %d) error !! cannot retrieve available free buffers\n", dbg_mipi_cnt);
        return 0;
    }

    return (new_head_buf_addr + BUF_HEADER_SIZE);
}

//cam_format cams_fmt = {0};
static struct cam_format _cams_fmt[2] = {0};

// image ISR callback
void image_coming_callback(uint32_t cam_idx, uint32_t img_buf_addr, uint32_t *pNew_img_buf)
{
#ifndef SHOW_SENSOR_IMAGE_ON_LCD// 1//send to inference queue
    dbg_mipi_cnt++;

    // retrieve a new free buf (from the inference free buffer queue)
    *pNew_img_buf = retrieve_new_buffer_for_mipi();
    if (NULL == pNew_img_buf) {
        // Due to highest priority ISR has,
        // image input is so fast that fifoq can't provide a free buffer even force_grab is set
        *pNew_img_buf = img_buf_addr;
        return;
    }

    if(cam_idx == CAM_RGB_IDX)
    {
        _cam_settings[0].img_width = IMGSRC_0_WIDTH;
        _cam_settings[0].img_height = IMGSRC_0_HEIGHT;
        _cam_settings[0].img_format = KP_IMAGE_FORMAT_RGB565;
        _cam_settings[0].img_channel = 3;
        _cam_settings[0].img_buf_size = IMGSRC_0_WIDTH * IMGSRC_0_HEIGHT * 2;
    }
    else if(cam_idx == CAM_NIR_IDX)
    {
        _cam_settings[1].img_width = IMGSRC_1_WIDTH;
        _cam_settings[1].img_height = IMGSRC_1_HEIGHT;
        _cam_settings[1].img_format = KP_IMAGE_FORMAT_RAW8;
        _cam_settings[1].img_channel = 1;
        _cam_settings[1].img_buf_size = IMGSRC_1_WIDTH * IMGSRC_1_HEIGHT;
    }


    // // FIXME: reduce NIR camera FPS to 1/2
    // if (cam_idx == CAM_NIR_IDX)
    // {
    //     static bool nir_skip = false;
    //     nir_skip = !nir_skip;
    //     if (!nir_skip)
    //     {
    //         *pNew_img_buf = img_buf_addr;
    //         return;
    //     }
    // }

    uint32_t head_buf = img_buf_addr - BUF_HEADER_SIZE;

    // initialize hico img header
    buf_header_t *bufHdr = (buf_header_t *)head_buf;
    bufHdr->magic_type = HICO_IMAGE_RECEIVER_HEADER_MAGIC;
    bufHdr->img_buf_offset = BUF_HEADER_SIZE;
    bufHdr->read_ref_count = 0;
    bufHdr->cam_index = cam_idx;

    //kmdw_printf("send_to_inference_queue, cam_idx=0%d, _display_cam=%d",cam_idx,_display_cam);

    // send both RGB and NIR images to infernece
    send_to_inference_queue(head_buf, cam_idx);

    // send only selected image to display thread
    if (_display_cam == cam_idx)
        send_to_display_queue(head_buf, cam_idx);

    return;
#else//show sensor image directly

    //DSG("image_coming_callback,img_buf_addr=0x%x",img_buf_addr);
    lcdc_kdp2_set_disp_buf(img_buf_addr,0);//show sensor image directly

    return;
#endif
}


uint32_t camera_start(uint8_t cam_idx, uint32_t width, uint32_t height, uint32_t pixelformat)
{
    uint32_t ret;

    struct cam_capability cap;

    _cams_fmt[cam_idx].width = width;
    _cams_fmt[cam_idx].height = height;
    _cams_fmt[cam_idx].pixelformat = pixelformat;

    char fmtstr[8];
    memset(&cap, 0, sizeof(cap));

    if (0 != (ret = kmdw_camera_open(cam_idx)))
        return ret;

    if (0 != (ret = kmdw_camera_get_device_info(cam_idx, &cap)))
        return ret;

    if (0 != (ret = kmdw_camera_set_frame_format(cam_idx, &_cams_fmt[cam_idx])))
        return ret;

    if (0 != (ret = kmdw_camera_get_frame_format(cam_idx, &_cams_fmt[cam_idx])))
        return ret;

    memset(fmtstr, 0, 8);
    memcpy(fmtstr, &_cams_fmt[cam_idx].pixelformat, 4);

    int buf_size;
    uint32_t mipi_buf_addr[NUM_MIPI_INIT_BUFS] = {0};
    kmdw_printf("%s kmdw_fifoq_manager_image_get_free_buffer\n", __FUNCTION__);
    // mipi needs some buffers at initialization
    for (int i = 0; i < NUM_MIPI_INIT_BUFS; i++)
    {
        uint32_t head_buf;
        // retrieve free hico buffer
        kmdw_fifoq_manager_image_get_free_buffer(&head_buf, &buf_size, osWaitForever, false);
        // and use its img buffer part for MIPI input
        mipi_buf_addr[i] = head_buf + BUF_HEADER_SIZE;
    }
    kmdw_printf("%s kmdw_camera_buffer_init\n", __FUNCTION__);
    if (0 != (ret = kmdw_camera_buffer_init(cam_idx, mipi_buf_addr[0], mipi_buf_addr[1])))
        return ret;
    kmdw_printf("%s kmdw_camera_start\n", __FUNCTION__);
    if (0 != (ret = kmdw_camera_start(cam_idx, image_coming_callback)))
        return ret;

    return 0;
}

// this thread receive the command from host SW
void kdp2_host_recv_cmd_thread(void *arg)
{
    dbg_log("[%s] start !\n", __FUNCTION__);
    kmdw_printf("[%s] start !\n", __FUNCTION__);
    _usb_cmd_tid = osThreadGetId();
    if (_usb_cmd_tid == NULL)
        kmdw_printf("%s creation failed !\n", __FUNCTION__);
    // wait until usb connection is established
    osThreadFlagsWait(FLAG_WAIT_USB_CONNECTION, osFlagsWaitAny, osWaitForever);
    kmdw_printf("[%s] FLAG_WAIT_USB_CONNECTION OK !\n", __FUNCTION__);
    uint32_t cmd_buf = kmdw_ddr_reserve(CMD_BUF_SIZE);

    while (1)
    {
        uint32_t txLen = CMD_BUF_SIZE;
        kdrv_status_t usb_sts = usbd_hal_bulk_receive(KDP2_USB_ENDPOINT_DATA_OUT, (void *)cmd_buf, &txLen, osWaitForever);
        if (usb_sts != KDRV_STATUS_OK) // KDRV_STATUS_USBD_TRANSFER_TERMINATED or KDRV_STATUS_USBD_TRANSFER_DISCONNECTED
        {
            dbg_log("[%s] bulk receive is terminated, sts %d\n", __FUNCTION__, usb_sts);
            continue;
        }

        dbg_log("[%s] usb recv addr 0x%x len %d\n", __FUNCTION__, (void *)cmd_buf, txLen);

        kp_inference_header_stamp_t *header_stamp = (kp_inference_header_stamp_t *)cmd_buf;

        if (header_stamp->magic_type == KDP2_MAGIC_TYPE_COMMAND) // standard KDP2 commands
        {
            // borrow image fifo to handle KDP2 commands as well
            dbg_log("[%s] handle kdp2 command = 0x%x\n", __FUNCTION__, header_stamp->job_id);
            // handle kdp2 commands ...
            kdp2_cmd_handle_kp_command(cmd_buf);
            continue;
        }
        else if (header_stamp->magic_type == KDP2_MAGIC_TYPE_CUSTOMIZED) // customized commands
        {
            dbg_log("[%s] configuration command received\n", __FUNCTION__);

            start_camera_cmd_t *scCmd = (start_camera_cmd_t *)cmd_buf;

            if (scCmd->cmd_id == COMMAND_START_CAMERA)
            {
                // start mipi cameras

                _cam_settings[0].img_width = IMGSRC_0_WIDTH;
                _cam_settings[0].img_height = IMGSRC_0_HEIGHT;
                _cam_settings[0].img_format = KP_IMAGE_FORMAT_RGB565;
                _cam_settings[0].img_channel = 3;
                _cam_settings[0].img_buf_size = IMGSRC_0_WIDTH * IMGSRC_0_HEIGHT * 2;

                if (0 != camera_start(0, IMGSRC_0_WIDTH, IMGSRC_0_HEIGHT, IMGSRC_0_FORMAT))
                    kmdw_printf("error !! camera_start 0 failed\n");

                _cam_settings[1].img_width = IMGSRC_1_WIDTH;
                _cam_settings[1].img_height = IMGSRC_1_HEIGHT;
                _cam_settings[1].img_format = KP_IMAGE_FORMAT_RAW8;
                _cam_settings[1].img_channel = 1;
                _cam_settings[1].img_buf_size = IMGSRC_1_WIDTH * IMGSRC_1_HEIGHT * 1;

                if (0 != camera_start(1, IMGSRC_1_WIDTH, IMGSRC_1_HEIGHT, IMGSRC_1_FORMAT))
                    kmdw_printf("error !! camera_start 1 failed\n");

                start_camera_resp_t scResp;

                scResp.header_stamp.magic_type = KDP2_MAGIC_TYPE_CUSTOMIZED;
                scResp.header_stamp.status_code = KP_SUCCESS;
                scResp.header_stamp.total_size = sizeof(scResp);
                scResp.num_cam_sensors = NUM_HOST_SENSOR;

                for (int i = 0; i < NUM_HOST_SENSOR; i++)
                    memcpy(&scResp.cam_set[i], &_cam_settings[i], sizeof(camera_settings_t));

                // send command-response back to user
                usbd_hal_bulk_send(KDP2_USB_ENDPOINT_DATA_IN, (void *)&scResp, sizeof(scResp), 1000);
            }
            else if (scCmd->cmd_id == COMMAND_SELECT_CAMERA)
            {
                select_camera_cmd_t *cmd = (select_camera_cmd_t *)cmd_buf;
                kmdw_printf("select camera %d\n", cmd->sel_camera);
                _display_cam = cmd->sel_camera;
            }
        }
        else if ((header_stamp->magic_type & 0xFFFF) == KDP_MSG_HDR_CMD) // very speical case for old arch. fw update
        {
            // handle legendary kdp commands, should be as few as possible
            dbg_log("handle legendary kdp command = 0x%x\n", header_stamp->job_id);
            kdp2_cmd_handle_legend_kdp_command(cmd_buf);
        }
        else
        {
            dbg_log("error ! buffer begin with incorrect magic_type 0x%x, txLen %d\n", __FUNCTION__, header_stamp->magic_type, txLen);
        }
    }
}
//TINY_YOLO_V3_224_224_3;//KNERON_YOLOV5S_COCO80_640_640_3;//
enum {
    UART_CMD_START_TINY_YOLO_V3_INF_RGB = 1,
    UART_CMD_START_TINY_YOLO_V3_INF_NIR,
    UART_CMD_START_TINY_YOLO_V3_INF_RGB_NIR,
    UART_CMD_STOP_TINY_YOLO_V3_INF,
    UART_CMD_QUIT,
    UART_CMD_SET_SNAPSHOT,
    UART_CMD_SET_REVIEW_SNAPSHOT,
    UART_CMD_NUM
};

// this thread receive the command from UART or power button ?

void kdp2_host_recv_uart_cmd_thread(void *arg)
{
    int id = 0;
    char buf[64];
    bool cam_started[2] = {false};
    bool quit_thread = false;

    kmdw_printf("[%s] start !\n", __FUNCTION__);
    // Implement it if needed ? Press power button to toggle inference
    // power_button_register(power_btn_handler);
    //kmdw_console_echo_gets(buf, sizeof(buf));
    memset(buf, 0, sizeof(buf));
    kmdw_printf("[%s] clear uart buf !\n", __FUNCTION__);

    while(1)
    {
        //kmdw_printf("\n === Menu === id=%d\n",id);
        if (id)
            goto cmd_prompt;

        kmdw_printf("\n === Menu === \n");
        kmdw_printf("( 1) Start Tiny YoloV3 Inference with RGB camera\n");
        kmdw_printf("( 2) Start Tiny YoloV3 Inference with NIR camera\n");
        kmdw_printf("( 3) Start Tiny YoloV3 Inference with RGB and NIR camera\n");
        kmdw_printf("( 4) Stop Inference\n");
        kmdw_printf("( 5) Quit\n");
cmd_prompt:
        DSG_NOLF(" command >> ");
        kmdw_console_echo_gets(buf, sizeof(buf));

        id = atoi(strtok(buf, " \r\n\t"));

        if (id > 0 && id <= UART_CMD_NUM)
        {
            // FIXME: add mechanism of releasing buffers used by mipi driver to mipi driver and kmdw_camera levels
            // following code is just a workaround
            if (UART_CMD_START_TINY_YOLO_V3_INF_RGB <= id && id <= UART_CMD_START_TINY_YOLO_V3_INF_RGB_NIR)
            {
                if (true == cam_started[0] || true == cam_started[1])
                {
                    DSG("Please stop the current running camera first");
                    goto cmd_prompt;
                }
            #if 1
                kmdw_printf("[%s] Wait until all image buffers are not used, cmd id=%d\n", __FUNCTION__,id);
                // Wait until all image buffers are not used
                while (osThreadBlocked != osThreadGetState(_update_display_tid))
                    osDelay(1);

                uint32_t temp_buf_addr;
                int temp_buf_size;

                kmdw_printf("[%s] clean up image buffers, cmd id=%d\n", __FUNCTION__,id);
                // clean up image buffers
                while (osErrorResource != kmdw_fifoq_manager_image_get_free_buffer(&temp_buf_addr, &temp_buf_size, 0, true));

                uint32_t img_buf_addr = _img_buf_start_addr_backup;

                // requeue buffers into image free-queue
                for (uint32_t i = 0; i < NUM_IMG_BUF; i++)
                {
                    buf_header_t *bufHdr = (buf_header_t *)(img_buf_addr);
                    bufHdr->read_ref_count = 0;

                    kmdw_fifoq_manager_image_put_free_buffer((uint32_t)&bufHdr->inf_header, IMG_BUF_SIZE, osWaitForever);
                    img_buf_addr += IMG_BUF_SIZE; // next one
                }
            #endif
            }

            switch(id)
            {
            case UART_CMD_START_TINY_YOLO_V3_INF_RGB:
                _display_cam = CAM_RGB_IDX;
                if (0 != camera_start(0, IMGSRC_0_WIDTH, IMGSRC_0_HEIGHT, IMGSRC_0_FORMAT))
                {
                    DSG("Error ! RGB camera start failed !");
                }
                else
                    cam_started[0] = true;

                display_init(V2K_PIX_FMT_RGB565, IMGSRC_0_WIDTH, IMGSRC_0_HEIGHT, _display_cam);
                break;
            case UART_CMD_START_TINY_YOLO_V3_INF_NIR:
                _display_cam = CAM_NIR_IDX;
                if (0 != camera_start(1, IMGSRC_1_WIDTH, IMGSRC_1_HEIGHT, IMGSRC_1_FORMAT))
                {
                    DSG("Error ! NIR camera start failed !");
                }
                else
                    cam_started[1] = true;

                display_init(V2K_PIX_FMT_RAW8, IMGSRC_1_WIDTH, IMGSRC_1_HEIGHT, _display_cam);
                break;
            case UART_CMD_START_TINY_YOLO_V3_INF_RGB_NIR:
                _display_cam = CAM_RGB_IDX;
                if (0 != camera_start(0, IMGSRC_0_WIDTH, IMGSRC_0_HEIGHT, IMGSRC_0_FORMAT) || 0 != camera_start(1, IMGSRC_1_WIDTH, IMGSRC_1_HEIGHT, IMGSRC_1_FORMAT))
                {
                    DSG("Error ! RGB or NIR camera start failed !");
                }
                else
                    cam_started[0] = cam_started[1] = true;

                display_init(V2K_PIX_FMT_RGB565, IMGSRC_0_WIDTH, IMGSRC_0_HEIGHT, _display_cam);
                kmdw_printf("LCD:default show RGB camera\n");
                break;
            case UART_CMD_STOP_TINY_YOLO_V3_INF:
            case UART_CMD_QUIT:
                if (true == cam_started[0])
                {
                    kmdw_printf("kmdw_camera_stop 0\n");
                    if (KMDW_STATUS_OK != kmdw_camera_stop(0))
                    {
                        DSG("Error ! RGB camera stop failed !");
                    }
                    kmdw_printf("kmdw_camera_close 0\n");
                    if (KMDW_STATUS_OK != kmdw_camera_close(0))
                    {
                        DSG("Error ! RGB camera close failed !");
                    }
                    cam_started[0] = false;
                    //kdrv_gpio_write_pin(RGB_LED, false); // switch off the LED
                }
                if (true == cam_started[1])
                {
                    if (KMDW_STATUS_OK != kmdw_camera_stop(1))
                    {
                        DSG("Error ! NIR camera stop failed !");
                    }
                    if (KMDW_STATUS_OK != kmdw_camera_close(1))
                    {
                        DSG("Error ! NIR camera close failed !");
                    }
                    cam_started[1] = false;
                }
                display_exit();
                if (id == UART_CMD_QUIT)
                {
                    DSG("Quit");
                    quit_thread = true;
                }
                break;
            }
        }
        else {
            if (id)
                err_msg("Invalid command: %d\n", id);
            continue;
        }

        if (quit_thread)
        {
            kmdw_power_manager_shutdown();
            break;
        }

    }
}

void kdp2_host_update_display_thread(void *arg)
{
    dbg_log("[%s] start !\n", __FUNCTION__);
    kmdw_printf("start kdp2_host_update_display_thread\n");
    kimg_buf_object_t bufobj;

    _update_display_tid = osThreadGetId();

    uint32_t display_buf[2];//PingPong buffer for display
    static int display_idx = 0;

    for(int i = 0; i < 2; i++)
    {
        display_buf[i] = kmdw_ddr_reserve(IMG_BUF_SIZE);
        if (display_buf[i] == 0)
            kmdw_printf("error !! display_buf[%d] memory allocation failed\n",i);
    }

    while (1)
    {
        // waiting for image from ISR though internal image queue
        osMessageQueueGet(_display_img_queue, (void *)&bufobj, NULL, osWaitForever);

        int buf_size = bufobj.buf_size;

#if 0 // this make HW crash whatever GDMA or GDMA3 !! FIXME FIXME !!
        kdrv_status_t dma_sts = kdrv_gdma_memcpy(display_buf, bufobj.buf_address, buf_size, NULL, NULL);
        if (dma_sts != KDRV_STATUS_OK)
        {
            kmdw_printf("error !! kdrv_gdma_memcpy() failed, err = %d\n", dma_sts);
            break;
        }
#else
        //display_idx++;
        //display_idx&=1;
        lcdc_kdp2_get_disp_idx(&display_idx);
        memcpy((void *)display_buf[display_idx], (void *)bufobj.buf_address, buf_size);
#endif
        buf_header_t *bufHdr = (buf_header_t *)(bufobj.buf_address);
        int cam_idx = bufHdr->cam_index;
        // FIXME: need mutex
        bufHdr->read_ref_count--;

        int img_width = _cam_settings[cam_idx].img_width;
        int img_bpp = (cam_idx == CAM_RGB_IDX) ? 2 : 1;

        // drawing bounding box
        osMutexAcquire(_mutex_result, osWaitForever);
        {
            kp_app_yolo_result_t *yd = _p_yolo_data[cam_idx];

            char *img_begin_addr = (char *)(display_buf[display_idx] + BUF_HEADER_SIZE);
            //char *img_begin_addr = (char *)(display_buf + BUF_HEADER_SIZE);
            //char *img_begin_addr = (char *)(bufobj.buf_address + BUF_HEADER_SIZE);

        #ifdef SHOW_INF_RESULT//inference result
            if(yd->box_count >= 1)
            {
                if (cam_idx == 0)
                    kmdw_printf("RGB image inference result :\n");
                else if (cam_idx == 1)
                    kmdw_printf("NIR image inference result :\n");
                kmdw_printf("box count : %d\n", yd->box_count);
                for (int i = 0; i < yd->box_count; i++)
                {
                    kmdw_printf("Box %d (x1, y1, x2, y2, score, class) = %.1f, %.1f, %.1f, %.1f, %f, %d\n",
                    i,
                    yd->boxes[i].x1, yd->boxes[i].y1,
                    yd->boxes[i].x2, yd->boxes[i].y2,
                    yd->boxes[i].score, yd->boxes[i].class_num);

                    kmdw_printf("%s \n", classmap[yd->boxes[i].class_num]);
                }
            }
            //DSG("kdp2_host_update_display_thread,box_count=%d",yd->box_count);
        #endif
            for (int i = 0; i < yd->box_count; i++)
            {
                int px1 = (int)yd->boxes[i].x1;
                int py1 = (int)yd->boxes[i].y1;
                int px2 = (int)yd->boxes[i].x2 - 1;
                int py2 = (int)yd->boxes[i].y2 - 1;

                int tl_pos = img_bpp * (py1 * img_width + px1); // top-left pos
                int bl_pos = img_bpp * (py2 * img_width + px1); // bottom-left pos
                int tr_pos = img_bpp * (py1 * img_width + px2); // top-right pos
                int x_diff = px2 - px1 + 1;
                int y_diff = py2 - py1 + 1;

                char *draw_pos;

                if (cam_idx == CAM_RGB_IDX) // RGB565
                {
                    uint16_t box_color = 0x7E0; // green

                    draw_pos = img_begin_addr + tl_pos;
                    for (int j = 0; j < x_diff; j++)
                    {
                        *(uint16_t *)draw_pos = box_color;
                        *(uint16_t *)(draw_pos+img_bpp*img_width) = box_color;
                        *(uint16_t *)(draw_pos+2*img_bpp*img_width) = box_color;
                        *(uint16_t *)(draw_pos+3*img_bpp*img_width) = box_color;

                        draw_pos += img_bpp;
                    }

                    draw_pos = img_begin_addr + bl_pos;
                    for (int j = 0; j < x_diff; j++)
                    {
                        *(uint16_t *)draw_pos = box_color;
                        *(uint16_t *)(draw_pos-img_bpp*img_width) = box_color;
                        *(uint16_t *)(draw_pos-2*img_bpp*img_width) = box_color;
                        *(uint16_t *)(draw_pos-3*img_bpp*img_width) = box_color;

                        draw_pos += img_bpp;
                    }

                    draw_pos = img_begin_addr + tl_pos;
                    for (int j = 0; j < y_diff; j++)
                    {
                        *(uint16_t *)draw_pos = box_color;
                        *(uint16_t *)(draw_pos+2) = box_color;
                        *(uint16_t *)(draw_pos+4) = box_color;
                        *(uint16_t *)(draw_pos+6) = box_color;

                        draw_pos += (img_width * img_bpp);
                    }

                    draw_pos = img_begin_addr + tr_pos;
                    for (int j = 0; j < y_diff; j++)
                    {
                        *(uint16_t *)draw_pos = box_color;
                        *(uint16_t *)(draw_pos-2) = box_color;
                        *(uint16_t *)(draw_pos-4) = box_color;
                        *(uint16_t *)(draw_pos-6) = box_color;

                        draw_pos += (img_width * img_bpp);
                    }
                }
                else if (cam_idx == CAM_NIR_IDX) // grayscale
                {
                    uint8_t box_color = 0xFF; // white

                    draw_pos = img_begin_addr + tl_pos;
                    for (int j = 0; j < x_diff; j++)
                    {
                        *(uint8_t *)draw_pos = box_color;
                        draw_pos += img_bpp;
                    }

                    draw_pos = img_begin_addr + bl_pos;
                    for (int j = 0; j < x_diff; j++)
                    {
                        *(uint8_t *)draw_pos = box_color;
                        draw_pos += img_bpp;
                    }

                    draw_pos = img_begin_addr + tl_pos;
                    for (int j = 0; j < y_diff; j++)
                    {
                        *(uint8_t *)draw_pos = box_color;
                        draw_pos += (img_width * img_bpp);
                    }

                    draw_pos = img_begin_addr + tr_pos;
                    for (int j = 0; j < y_diff; j++)
                    {
                        *(uint8_t *)draw_pos = box_color;
                        draw_pos += (img_width * img_bpp);
                    }
                }
            }
        }
        osMutexRelease(_mutex_result);
        // FIXME, this is just a simulation via remote display
        // replace code to send to a local display !!
	    #if 0
        kdrv_status_t usb_sts = usbd_hal_bulk_send(KDP2_USB_ENDPOINT_DATA_IN, (void *)display_buf, buf_size, osWaitForever);
        if (usb_sts != KDRV_STATUS_OK) // KDRV_STATUS_USBD_TRANSFER_TERMINATED or KDRV_STATUS_USBD_TRANSFER_DISCONNECTED
            kmdw_printf("[usb_image] error ! usbd_hal_bulk_send() ret = %d\n", usb_sts);
		#else
            lcdc_kdp2_set_disp_buf((uint32_t)(display_buf[display_idx] + BUF_HEADER_SIZE),display_idx);
            //kmdw_printf("[%s] display=0x%x\n", __FUNCTION__,bufobj.buf_address);
			//display_addr = bufobj.buf_address + BUF_HEADER_SIZE;//bufobj.buf_address: from image queue directly
        #endif
    }
}
// this thread sends inference result to host
void kdp2_host_update_result_thread(void *arg)
{
    dbg_log("[%s] start !\n", __FUNCTION__);
    kmdw_printf("start kdp2_host_update_result_thread\n");
    while (1)
    {
        uint32_t result_buf_addr;
        int result_buf_length;

        // get result data from result fifo queue, blocking wait
        kmdw_fifoq_manager_result_dequeue(&result_buf_addr, &result_buf_length, osWaitForever);
        //kmdw_printf("[%s] result_buf_addr=0x%x result_buf_length = %d\n",__FUNCTION__,result_buf_addr,result_buf_length);
        // then send inference result
        kdp2_ipc_app_yolo_result_t *yolo_result = (kdp2_ipc_app_yolo_result_t *)result_buf_addr;

        osMutexAcquire(_mutex_result, osWaitForever);
        {
            // copy needed size
            int copy_size = 8 + (yolo_result->yolo_data.box_count * sizeof(kp_bounding_box_t));
            //kmdw_printf("[%s] copy_size = %d\n",__FUNCTION__,copy_size);
            //kmdw_printf("[%s] box_count = %d\n",__FUNCTION__,yolo_result->yolo_data.box_count);

            memcpy(_p_yolo_data[yolo_result->inf_number], &yolo_result->yolo_data, copy_size);

        }
        osMutexRelease(_mutex_result);

        // return free buf back to queue
        kmdw_fifoq_manager_result_put_free_buffer(result_buf_addr, result_buf_length, osWaitForever);
    }
}

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


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

int kdp2_host_mipi_init()
{
    // retrieve real serial number here from efuse
    // then convert it to hex string format

    uint32_t uid = 0;

    uid = kdp_sys_get_kn_number();

    int32_t sidx = 0;
    uint8_t kn_num_string[32] = {0};
    for (int i = 7; i >= 0; i--)
    {
        uint32_t hex = (uid >> i * 4) & 0xF;
        kn_num_string[sidx] = (hex < 10) ? '0' + hex : 'A' + (hex - 10);
        sidx += 2;
    }

    // Host Mode
    uint16_t bcdDevice = KP_KDP2_FW_HOST_MODE;

    kmdw_printf("%s\n",__FUNCTION__);
#if 0//KL720_Scott
    if (*((uint32_t *)JTAG_MAGIC_ADDRESS) == JTAG_MAGIC_VALUE)
    {
        kmdw_printf("FW is running in JTAG mode\n");
        bcdDevice |= KP_KDP2_FW_JTAG_TYPE;
    }
    else
    {
        uint32_t magic_lb, magic_hb;
        magic_lb = (*(uint32_t *)(DDR_MAGIC_BASE));
        magic_hb = (*(uint32_t *)(DDR_MAGIC_BASE + 0x04));

        if ((magic_lb == USB_BOOT_MAGIC_LB) && (magic_hb == USB_BOOT_MAGIC_HB))
        {
            kmdw_printf("KDP2 FW is running in usb-boot mode\n");
            bcdDevice |= KP_KDP2_FW_USB_TYPE;
        }
        else
        {
            kmdw_printf("KDP2 FW is running in flash-boot mode\n");
            bcdDevice |= KP_KDP2_FW_FLASH_TYPE;
        }
    }
    usbd_hal_initialize(kn_num_string, bcdDevice, usb_user_link_status_callback, usb_user_control_callback);
#else/*
    if (*((uint32_t *)JTAG_MAGIC_ADDRESS) == JTAG_MAGIC_VALUE)
    {
        kmdw_printf("FW is running in JTAG mode\n");
        bcdDevice |= KP_KDP2_FW_JTAG_TYPE;
    }
    else*/
    {
        kdp2_boot_config_t *bConfig = (kdp2_boot_config_t *)KDP2_BOOT_CONFIG_ADDRESS;
        if (bConfig->boot_type == BOOT_FROM_FLASH)
        {
            kmdw_printf("KDP2 FW is running in flash-boot mode\n");
            bcdDevice |= KP_KDP2_FW_FLASH_TYPE;

            kmdw_printf("boot ncpu fw from flash\n");
            SCU_EXTREG_CM4_NCPU_CTRL_SET_wakeup(1); // run ncpu
        }
        else
        {
            kmdw_printf("KDP2 FW is running in usb-boot mode\n");
            bcdDevice |= KP_KDP2_FW_USB_TYPE;
        }
    }
    //usbd_hal_initialize(kn_num_string, bcdDevice, usb_user_control_callback);
    usbd_hal_initialize(kn_num_string, bcdDevice, usb_user_link_status_callback, usb_user_control_callback);
#endif
    // this is about recovery mode
//    *(uint32_t *)RECOVERY_MARK_POS = 0;

    //usbd_hal_initialize(kn_num_string, bcdDevice, usb_user_control_callback);
    usbd_hal_initialize(kn_num_string, bcdDevice, usb_user_link_status_callback, usb_user_control_callback);
    usbd_hal_set_enable(true);

    // wow ! fifoq can also handle command
    kdp2_cmd_handler_initialize();



    /* Allocate memory for image and result buffers */
    uint32_t img_buf_addr = kmdw_ddr_reserve(NUM_IMG_BUF * IMG_BUF_SIZE);
    if (img_buf_addr == 0)
    {
        dbg_log("error !!! kmdw_ddr_reserve() failed for image/result buffers\n");
        return -1;
    }

    // back up image buffer start address for reopening the camera
    // FIXME: this is just a workaround
    // temporary sync with 720 code base
    _img_buf_start_addr_backup = img_buf_addr;
    // queue buffers into image free-queue
    for (uint32_t i = 0; i < NUM_IMG_BUF; i++)
    {
        buf_header_t *bufHdr = (buf_header_t *)(img_buf_addr);
        bufHdr->read_ref_count = 0;

        kmdw_fifoq_manager_image_put_free_buffer((uint32_t)&bufHdr->inf_header, IMG_BUF_SIZE, osWaitForever);
        img_buf_addr += IMG_BUF_SIZE; // next one
    }

    uint32_t result_buf_addr = kmdw_ddr_reserve(NUM_RESULT_BUF * RESULT_BUF_SIZE);

    kmdw_printf("result_buf_addr = 0x%x\n", result_buf_addr);

    // queue buffers into result free-queue
    for (uint32_t i = 0; i < NUM_RESULT_BUF; i++)
    {
        kmdw_fifoq_manager_result_put_free_buffer(result_buf_addr, RESULT_BUF_SIZE, osWaitForever);
        result_buf_addr += RESULT_BUF_SIZE;
    }

    _mutex_result = osMutexNew(NULL);
    if (_mutex_result == NULL)
        kmdw_printf("error !! _mutex_result creation failed\n");

    for (int i = 0; i < NUM_HOST_SENSOR; i++)
    {
        _p_yolo_data[i] = (kp_app_yolo_result_t *)kmdw_ddr_reserve(sizeof(kp_app_yolo_result_t));
        if (_p_yolo_data[i] == NULL)
            kmdw_printf("error !! _p_yolo_data memory allocation failed\n");

        _p_yolo_data[i]->box_count = 0;
    }

    _display_img_queue = osMessageQueueNew(NUM_DISPLAY_QUEUE_DEPTH, sizeof(kimg_buf_object_t), NULL);
    if (_display_img_queue == NULL)
        kmdw_printf("error !! _display_img_queue creation failed\n");

    int32_t load_model_sts = kmdw_model_load_model(-1);//load_model from flash, -1:means to load all models
                                                       //return 0: failes;  1: OK(means 1 model is loaded)
    kmdw_printf("%s: load_model_sts=%d\n",__FUNCTION__,load_model_sts);

    return 0;
}
