//#define ENABLE_DBG_LOG

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

#include "kmdw_console.h"
#include "kmdw_memory.h"
#include "kdrv_gdma.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_fifoq_manager.h"
#include "kdp2_ipc_cmd.h"
#include "kdp2_inf_app_yolo.h"
#include "kdp2_usb_companion.h"

#include "kmdw_display.h"
// --- hico mipi include ---
#include "hico_mipi.h"

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

extern uint32_t kdrv_efuse_get_kn_number(void);

#define NUM_MIPI_INIT_BUFS 2 // 2 (ping-pong buffers) or 3 (triple-buffers for skipping frames)
#define NUM_IMAGE_BUF 10
#define IMAGE_BUF_SIZE (1 * 1024 * 1024)
#define NUM_RESULT_BUF 10
#define RESULT_BUF_SIZE (400 * 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

//static osThreadId_t usb_img_tid = 0;
//#define SHOW_SENSOR_IMAGE_ON_LCD//debug purpose: show sensor image directly without inference on LCD

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

typedef struct
{
    uint32_t cam_idx;
    uint32_t buf_addr;
    uint32_t img_width;
    uint32_t img_height;
    uint32_t img_format;
} mipi_img_object_t;

#define FLAG_WAIT_USB_CONNECTION 0x1

static osThreadId_t _usb_cmd_tid = NULL;
static osMessageQueueId_t _img_queue = NULL;
static uint32_t _hico_mode = MODE_LIVE_VIEW;

//static osThreadId_t task_usb_cmd_handle = NULL;
//static osThreadId_t task_usb_image_handle = NULL;
//static osThreadId_t task_usb_result_handle = NULL;
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);

static uint32_t _app_job_id = 0xFFFFFFFF;

// usb link status 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;
    }
}

// vendor-specific control transfer setup packet notify
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;
    }

    default:
        ret = false;
        break;
    }

    return ret;
}

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

// image ISR callback
void image_coming_callback(uint32_t cam_idx, uint32_t img_buf, uint32_t *p_new_img)
{
    osStatus_t sts;
    mipi_img_object_t img_obj;
    uint32_t new_inf_buf;
    int buf_size;
    sts = kmdw_fifoq_manager_image_get_free_buffer(&new_inf_buf, &buf_size, 0, true);
    if (sts != osOK)
    {
        sts = osMessageQueueGet(_img_queue, (void *)&img_obj, NULL, 0);
        if (sts != osOK)
        {
            dbg_log("(ISR) error !! retrieving new buf failed, osMessageQueueGet() ret = %d\n", sts);
            // Due to highest priority ISR has,
            // image input is so fast that fifoq can't provide a free buffer even force_grab is set
            *p_new_img = img_buf;
            return;
        }

        new_inf_buf = img_obj.buf_addr;
    }

    // get inf_buf address by shift back by the size of XXX_inference_header_t
    uint32_t inf_buf = img_buf - sizeof(kdp2_ipc_app_yolo_inf_header_t);

    img_obj.cam_idx = cam_idx;
    img_obj.buf_addr = inf_buf;
    img_obj.img_width = _cams_fmt[cam_idx].width;
    img_obj.img_height = _cams_fmt[cam_idx].height;
    if (_cams_fmt[cam_idx].pixelformat == IMG_FORMAT_RGB565)
        img_obj.img_format = KP_IMAGE_FORMAT_RGB565;
    else if (_cams_fmt[cam_idx].pixelformat == IMG_FORMAT_RAW8)
        img_obj.img_format = KP_IMAGE_FORMAT_RAW8;
    else if (_cams_fmt[cam_idx].pixelformat == IMG_FORMAT_YCBCR)
        img_obj.img_format = KP_IMAGE_FORMAT_YUYV;
#ifdef SHOW_SENSOR_IMAGE_ON_LCD
    if(cam_idx == 0)
        lcdc_kdp2_set_disp_buf(img_buf,0);//show sensor image directly
    //DSG("image_coming_callback, img_buf=0x%x, cam_idx=%d, pixelformat=0x%x, img_width=%d img_height=%d",img_buf, cam_idx, _cams_fmt[cam_idx].pixelformat, _cams_fmt[cam_idx].width, _cams_fmt[cam_idx].height);
#endif
    sts = osMessageQueuePut(_img_queue, (const void *)&img_obj, 0U, 0);
    if (sts != osOK)
    {
        dbg_log("(ISR) error !! osMessageQueuePut() failed, sts = %d\n", sts);
        *p_new_img = img_buf;
        return;
    }

    *p_new_img = new_inf_buf + sizeof(kdp2_ipc_app_yolo_inf_header_t);

    return;
}

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])))//scpu: exception: code=1, object_id=0x102021d4
        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 buf_addr[NUM_MIPI_INIT_BUFS] = {0};

    // mipi needs some buffers at initialization
    for (int i = 0; i < NUM_MIPI_INIT_BUFS; i++)
    {
        kmdw_fifoq_manager_image_get_free_buffer(&buf_addr[i], &buf_size, osWaitForever, false);
        buf_addr[i] += sizeof(kdp2_ipc_app_yolo_inf_header_t);
    }

    if (0 != (ret = kmdw_camera_buffer_init(cam_idx, buf_addr[0], buf_addr[1])))
        return ret;

    if (0 != (ret = kmdw_camera_start(cam_idx, image_coming_callback)))
        return ret;

    return 0;
}


#define CMD_BUF_SIZE (1 * 1024)
//#define CMD_BUF_SIZE (1 * 1024 * 1024)


// this thread receive the command from host SW
void kdp2_hico_mipi_usb_cmd_thread(void *arg)
{
    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
        {
            // customized command to configure mipi camera
            if (header_stamp->job_id == JID_START_HICO_MIPI)
            {
                dbg_log("[%s] configuration command received\n", __FUNCTION__);

                hico_mipi_config_command_t *hico_mipi_cmd = (hico_mipi_config_command_t *)cmd_buf;
                hico_mipi_config_response_t hico_mipi_resp;

                hico_mipi_resp.header_stamp.magic_type = KDP2_MAGIC_TYPE_CUSTOMIZED;
                hico_mipi_resp.header_stamp.job_id = JID_START_HICO_MIPI;
                hico_mipi_resp.header_stamp.status_code = KP_SUCCESS;
                hico_mipi_resp.header_stamp.total_size = sizeof(hico_mipi_resp);
                hico_mipi_resp.num_cam_sensors = 0;

                _hico_mode = hico_mipi_cmd->mode;
                _app_job_id = hico_mipi_cmd->app_job_id;

                // Configure the mipi camera
                if (hico_mipi_cmd->sensor_sel & CAM_SENSOR_0)
                {
                    if (0 == camera_start(0, IMGSRC_0_WIDTH, IMGSRC_0_HEIGHT, IMGSRC_0_FORMAT))
                    {
						dbg_log("[%s] camera_start 0, ok\n",__FUNCTION__);
                        hico_mipi_resp.num_cam_sensors++;
                        hico_mipi_resp.cam_settings[0].img_width = IMGSRC_0_WIDTH;
                        hico_mipi_resp.cam_settings[0].img_height = IMGSRC_0_HEIGHT;
                        hico_mipi_resp.cam_settings[0].img_format = KP_IMAGE_FORMAT_RGB565;
                    }
                    else
                    {
                        hico_mipi_resp.header_stamp.status_code = KP_ERROR_OTHER_99; // FIXME, give it a specific error code
                    }
                }

                if (hico_mipi_cmd->sensor_sel & CAM_SENSOR_1)
                {
                    dbg_log("[%s] camera_start 1, ok\n",__FUNCTION__);

                    if (0 == camera_start(1, IMGSRC_1_WIDTH, IMGSRC_1_HEIGHT, IMGSRC_1_FORMAT))
                    {
                        hico_mipi_resp.num_cam_sensors++;
                        hico_mipi_resp.cam_settings[1].img_width = IMGSRC_1_WIDTH;
                        hico_mipi_resp.cam_settings[1].img_height = IMGSRC_1_HEIGHT;
                        hico_mipi_resp.cam_settings[1].img_format = KP_IMAGE_FORMAT_RAW8;
                    }
                    else
                    {
                        hico_mipi_resp.header_stamp.status_code = KP_ERROR_OTHER_99; // FIXME, give it a specific error code
                    }
                }

                kdrv_status_t usb_sts = usbd_hal_bulk_send(KDP2_USB_ENDPOINT_DATA_IN, (void *)&hico_mipi_resp, sizeof(hico_mipi_resp), 1000);
                if (usb_sts != KDRV_STATUS_OK) // KDRV_STATUS_USBD_TRANSFER_TERMINATED or KDRV_STATUS_USBD_TRANSFER_DISCONNECTED
                {
                    dbg_log("error ! usbd_hal_bulk_send() ret = %d\n", usb_sts);
                    //return;
                }
            }
            else
            {
                dbg_log("error !!! wrong job_id = %d\n", __FUNCTION__, header_stamp->job_id);
            }
        }
        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("[%s] handle legendary kdp command = 0x%x\n",__FUNCTION__, header_stamp->job_id);
            kdp2_cmd_handle_legend_kdp_command(cmd_buf);
        }
        else
        {
            dbg_log("[%s] error ! buffer begin with incorrect magic_type 0x%x, txLen %d\n", __FUNCTION__, header_stamp->magic_type, txLen);
        }
    }
}

// this thread sends inference result to host
void kdp2_hico_mipi_usb_result_thread(void *arg)
{
    kmdw_printf("[%s] start !\n", __FUNCTION__);

    while (1)
    {
        uint32_t result_buf_addr;
        int result_buf_length;

        // get result data from result fifo queue with blocking wait
        kmdw_fifoq_manager_result_dequeue(&result_buf_addr, &result_buf_length, osWaitForever);
        dbg_log("[%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;

        yolo_result->header_stamp.job_id = JID_CAM_0_INF_RESULT + yolo_result->inf_number; // for now it tells host SW that this is an inference result

        #if 1
            //yolo_result->header_stamp.total_size = result_buf_length;
			  #endif
        // send result to the host SW, blocking wait
        dbg_log("[%s] send usb data size %d\n", __FUNCTION__, yolo_result->header_stamp.total_size);
        kdrv_status_t usb_sts = usbd_hal_bulk_send(KDP2_USB_ENDPOINT_DATA_IN, (void *)result_buf_addr, yolo_result->header_stamp.total_size, osWaitForever);
        if (usb_sts != KDRV_STATUS_OK) // KDRV_STATUS_USBD_TRANSFER_TERMINATED or KDRV_STATUS_USBD_TRANSFER_DISCONNECTED
        {
            dbg_log("error ! usbd_hal_bulk_send() ret = %d \n", usb_sts);
        }

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

// this thread sends raw image from image sensor to host SW (and also enqueues it to image fifo queue ?)
void kdp2_hico_mipi_usb_img_send_back_thread(void *arg)
{
    kmdw_printf("[%s] start !\n", __FUNCTION__);

    while (1)
    {
        mipi_img_object_t img_obj;
        osMessageQueueGet(_img_queue, (void *)&img_obj, NULL, osWaitForever);
        dbg_log("[%s] _hico_mode = %d\n",__FUNCTION__, _hico_mode);
        dbg_log("[%s] got inf-buf (cam_idx %d buf_addr 0x%p img_width %d img_height %d img_format %d)\n",
        __FUNCTION__,img_obj.cam_idx, img_obj.buf_addr, img_obj.img_width, img_obj.img_height, img_obj.img_format);

        kdp2_ipc_app_yolo_inf_header_t *inf_header = (kdp2_ipc_app_yolo_inf_header_t *)img_obj.buf_addr;

        int byte_ppix;
        switch (img_obj.img_format)
        {
        case KP_IMAGE_FORMAT_RGB565:
        case KP_IMAGE_FORMAT_YUYV:
            byte_ppix = 2;
            break;
        case KP_IMAGE_FORMAT_RGBA8888:
            byte_ppix = 3;
            break;
        case KP_IMAGE_FORMAT_RAW8:
            byte_ppix = 1;
        }

        inf_header->header_stamp.magic_type = KDP2_MAGIC_TYPE_INFERENCE;
        inf_header->header_stamp.total_size = sizeof(kdp2_ipc_app_yolo_inf_header_t) + (img_obj.img_width * img_obj.img_height * byte_ppix);
        inf_header->header_stamp.job_id = JID_CAM_0_IMAGE + img_obj.cam_idx;
        inf_header->header_stamp.status_code = KP_SUCCESS;

        inf_header->inf_number = img_obj.cam_idx;
        inf_header->width = img_obj.img_width;
        inf_header->height = img_obj.img_height;
        inf_header->channel = (img_obj.img_format == KP_IMAGE_FORMAT_RAW8) ? 1 : 3;
    #if 0//KL720_Scott
        inf_header->model_id = KNERON_YOLOV5S_COCO80_640_640_3;//
    #else
        inf_header->model_id = TINY_YOLO_V3_224_224_3;
    #endif
        inf_header->image_format = img_obj.img_format;
        inf_header->model_normalize = KP_NORMALIZE_KNERON;

        dbg_log("[%s] send usb data size %d\n", __FUNCTION__, inf_header->header_stamp.total_size);
        kdrv_status_t usb_sts = usbd_hal_bulk_send(KDP2_USB_ENDPOINT_DATA_IN, (void *)inf_header, inf_header->header_stamp.total_size, osWaitForever);

        if (usb_sts != KDRV_STATUS_OK) // KDRV_STATUS_USBD_TRANSFER_TERMINATED or KDRV_STATUS_USBD_TRANSFER_DISCONNECTED
        {
            dbg_log("error ! usbd_hal_bulk_send() ret = %d\n", usb_sts);
            return;
        }

        if (_hico_mode == MODE_LIVE_VIEW)
        {
            dbg_log("[%s] kmdw_fifoq_manager_image_put_free_buffer %d\n",__FUNCTION__);
            kmdw_fifoq_manager_image_put_free_buffer((uint32_t)inf_header, IMAGE_BUF_SIZE, osWaitForever);
        }
        else // MODE_LIVE_VIEW_INF
        {
            dbg_log("[%s] kmdw_fifoq_manager_image_enqueue %d\n",__FUNCTION__);
            inf_header->header_stamp.job_id = _app_job_id;
            kmdw_fifoq_manager_image_enqueue(1, 0, (uint32_t)inf_header, IMAGE_BUF_SIZE, osWaitForever, false);
        }
    }
}

////////////////////////////////////////////////////////////
#define RECOVERY_MARK_POS (SdRAM_MEM_BASE + SdRAM_MEM_SIZE - 64)

// KDP2 Inference Interface for HICO MIPI code
// image input
// image + inference output
int kdp2_hico_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;
    }

    // HICO Mode
    uint16_t bcdDevice = KP_KDP2_FW_HICO_MODE;

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

    // this is about recovery mode
    *(uint32_t *)RECOVERY_MARK_POS = 0;

    usbd_hal_initialize(kn_num_string, bcdDevice, usb_user_link_status_callback, usb_user_control_callback);

    usbd_hal_set_enable(true);

    /* Allocate memory for image and result buffers */
    uint32_t buf_addr = kmdw_ddr_reserve(NUM_IMAGE_BUF * IMAGE_BUF_SIZE + NUM_RESULT_BUF * RESULT_BUF_SIZE);
    if (buf_addr == 0)
    {
        dbg_log("error !!! kmdw_ddr_reserve() failed for image/result buffers\n");
        return -1;
    }

    // queue buffers into image free-queue
    for (uint32_t i = 0; i < NUM_IMAGE_BUF; i++)
    {
        kmdw_fifoq_manager_image_put_free_buffer(buf_addr, IMAGE_BUF_SIZE, osWaitForever);
        buf_addr += IMAGE_BUF_SIZE;
    }

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

    // prepare an internal image queue between ISR callback and image-processing thread
    _img_queue = osMessageQueueNew(NUM_IMAGE_BUF, sizeof(mipi_img_object_t), NULL);
    if (_img_queue == NULL)
    {
        dbg_log("error !!! osMessageQueueNew() failed\n");
    }

    kmdw_fifoq_manager_store_fifoq_config(NUM_IMAGE_BUF, IMAGE_BUF_SIZE, NUM_RESULT_BUF, RESULT_BUF_SIZE);

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

#ifdef SHOW_SENSOR_IMAGE_ON_LCD
    display_init(V2K_PIX_FMT_RGB565, IMGSRC_0_WIDTH, IMGSRC_0_HEIGHT, 0);
#endif
    return 0;
}
