#include <stdio.h>
#include <stdbool.h>
#include <string.h>
#include <stdlib.h>
#include <unistd.h>
#include <sys/stat.h>
#include <signal.h>
#include <pthread.h>
#include <sys/time.h>

#include <MemBroker/mem_broker.h>
#include <MsgBroker/msg_broker.h>
#include <vmf/video_source.h>
#include <vmf/video_bind.h>
#include <comm/video_buf.h>
#include <vmf/video_encoder.h>
#include <vmf/video_display_mechanism.h>
#include <vmf/sync_shared_memory.h>
#include <vmf/ssm_info.h>
#include <comm/vmf_log.h>
#include <vmf/vector_dma.h>
#include <iniparser/iniparser.h>
#include <linux/videodev2.h>
#include "vmf_nnm_inference_app.h"
#include "vmf_nnm_fifoq_manager.h"
#include "vmf_nnm_ipc_cmd.h"

#include "application_init.h"

#include "kdp2_host_stream.h"
#include "kp_struct.h"
#include "kdp2_inf_app_yolo.h"
#include "fec_api.h"
#include "stdc_post_process.h"

//#define ENABLE_DBG_LOG

#ifdef ENABLE_DBG_LOG
#define dbg_log(__format__, ...) printf("[kp host_stream]"__format__, ##__VA_ARGS__)
#else
#define dbg_log(__format__, ...)
#endif


#define VENC_VSRC_PIN       "vsrc_ssm"					//! VMF_VSRC Output pin
#define VENC_VSRC_C_PIN     "vsrc_ssm_c_0"					//! VMF_VSRC Customer Output pin
#define VENC_VSRC_B_PIN     "vsrc_ssm_b_0"				//! VMF_VSRC Draw Box Customer Output pin
#define VENC_RESOURCE_DIR   "./Resource/"               //! directory contains ISP, AE, AWB, AutoScene sub directory
#define VENC_CMD_FIFO       "/tmp/venc/c0/command.fifo"	//! communicate with rtsps, vrec, etc.
#define SRB_DEFAULT_PREFIX		"venc_srb"

/**
 * @struct	YUV_BUFF_INFO_T
 * @brief	The structure for YUV_BUFF_INFO_T
 */
typedef struct
{
    unsigned char *pcYuvBuff;
    unsigned int dwWidth;
    unsigned int dwHeight;
} YUV_BUFF_INFO_T;

/**
 * @struct	POINT_INFO_T
 * @brief	The structure for POINT_INFO_T
 */
typedef struct
{
    unsigned int dwX;
    unsigned int dwY;
}POINT_INFO_T;

/**
 * @struct	DRAW_LINE_INFO_T
 * @brief	The structure for DRAW_LINE_INFO_T
 */
typedef struct
{
    POINT_INFO_T tStartPoint;
    POINT_INFO_T tEndPoint;
    unsigned int dwLineWidth;
} DRAW_LINE_INFO_T;

void ssm_clear_header(unsigned char*, unsigned int, void*);

/********************************************************************************************/
static void *g_pm_buf = NULL;
static VMF_BIND_CONTEXT_T* g_ptBind = NULL;
static unsigned int g_dwInitBind = 0;

VMF_SRC_CONNECT_INFO_T connect_info;
extern unsigned int g_dwVocPixFmt;
VMF_LAYOUT_T g_tLayout;
VMF_VENC_CODEC_TYPE g_eCodecType = VMF_VENC_CODEC_TYPE_H264;
SSM_HANDLE_T *gptSsmHandle = NULL;
VMF_VSRC_HANDLE_T* g_ptVsrcHandle = NULL;
float g_fWidthRatio = 1.0;
float g_fHeightRatio = 1.0;
EIS_T g_tEis;
unsigned int g_dwDrawBoxType = 0; //! 0: isp primary mask, 1: dma
static unsigned int g_dwDrawOnResize = 0;

int g_iTerminate = 0;
bool _blRecvRunning = true;
bool _blResultRunning = true;

static bool _enable_inf_droppable = false;

static VMF_VDISP_HANDLE_T* g_ptDisplayHandle;
static unsigned int g_dwRoiX = 0, g_dwRoiY = 0;

DMA_INFO_T* dma2d_init()
{
    DMA_INFO_T* dma_info;
    dma_info = (DMA_INFO_T*)MemBroker_GetMemory(sizeof(DMA_INFO_T), VMF_ALIGN_TYPE_DEFAULT);
    if (NULL == dma_info) {
        printf("dma2d_init MemBroker_GetMemory failed\n");
        return NULL;
    }
    dma_info->ptDmaHandle = VMF_DMA_Init(1,128);
    if (NULL == dma_info->ptDmaHandle) {
        if(dma_info)
            MemBroker_FreeMemory(dma_info);
        printf("dma2d_init failed\n");
        return NULL;
    }
    VMF_DMA_2DCF_INIT_T init;
    memset(&init, 0, sizeof(VMF_DMA_2DCF_INIT_T));
    init.dwProcessCbCr = 1;
    dma_info->ptDmaDesc = VMF_DMA_Descriptor_Create(DMA_2D, &init);
    if (NULL == dma_info->ptDmaDesc) {
        if(dma_info->ptDmaHandle)
            VMF_DMA_Release(dma_info->ptDmaHandle);
        if(dma_info)
            MemBroker_FreeMemory(dma_info);
        printf("dma_desc init failed\n");
    }
    return dma_info;
}

void dma2d_release(DMA_INFO_T* dma_info)
{
    VMF_DMA_Descriptor_Destroy(dma_info->ptDmaDesc);
    VMF_DMA_Release(dma_info->ptDmaHandle);
    MemBroker_FreeMemory(dma_info);
}

int dma2d_copy(VMF_DMA_HANDLE_T* dma_handle, VMF_DMA_DESCRIPTOR_T* dma_desc, void* dest, void* source, VMF_VSRC_SSM_OUTPUT_INFO_T* vsrc_ssm_info)
{
    int ret = 0;
    VMF_DMA_ADDR_T dma_addr;
    memset(&dma_addr, 0, sizeof(VMF_DMA_ADDR_T));
    //MemBroker_CacheCopyBack(source, vsrc_ssm_info->dwWidth * vsrc_ssm_info->dwHeight * 1.5);//Reading source data from ssm buffer doesn't need using CacheCopyBack.
    dma_addr.dwCopyWidth = vsrc_ssm_info->dwWidth;
    dma_addr.dwCopyHeight = vsrc_ssm_info->dwHeight;
    dma_addr.pbySrcYPhysAddr = (unsigned char*)MemBroker_GetPhysAddr(source) + vsrc_ssm_info->dwOffset[0];
    dma_addr.pbySrcCbPhysAddr = (unsigned char*)MemBroker_GetPhysAddr(source) + vsrc_ssm_info->dwOffset[1];
    dma_addr.pbySrcCrPhysAddr = (unsigned char*)MemBroker_GetPhysAddr(source) + vsrc_ssm_info->dwOffset[2];
    dma_addr.dwSrcStride = vsrc_ssm_info->dwYStride;

    dma_addr.pbyDstYPhysAddr = (unsigned char*)MemBroker_GetPhysAddr(dest);
    dma_addr.pbyDstCbPhysAddr = dma_addr.pbyDstYPhysAddr + vsrc_ssm_info->dwWidth* vsrc_ssm_info->dwHeight;
    dma_addr.pbyDstCrPhysAddr = dma_addr.pbyDstCbPhysAddr + vsrc_ssm_info->dwWidth* vsrc_ssm_info->dwHeight/4;
    dma_addr.dwDstStride = vsrc_ssm_info->dwWidth;
    MemBroker_CacheInvalidate(dest, vsrc_ssm_info->dwWidth * vsrc_ssm_info->dwHeight * 1.5);
    ret |= VMF_DMA_Descriptor_Update_Addr(dma_desc, &dma_addr);
    ret |= VMF_DMA_Setup(dma_handle, &dma_desc, 1);
    ret |= VMF_DMA_Process(dma_handle);
    return ret;
}

/******************************* Local functions Implementation ************************************/
void calculate_rs_size(unsigned int dwInferenceWidth, unsigned int dwInferenceHeight, unsigned int dwDrawWidth, unsigned int dwDrawHeight, float *pWidthRatio, float *pHeightRatio, unsigned int dwKeepRatio)
{
    float fWidthRatio = 1.0, fHeightRatio = 1.0;
    unsigned int dwRsWidth, dwRsHeight = 0;

    if (dwKeepRatio) {
        if ((dwDrawWidth == dwInferenceWidth) && (dwDrawHeight == dwInferenceHeight)) {
            dwRsWidth = dwInferenceWidth;
            dwRsHeight = dwInferenceHeight;
        } else {
            fWidthRatio = (float)dwInferenceWidth / (float)dwDrawWidth;
            if ((dwDrawHeight * fWidthRatio) <= dwInferenceHeight) { // Padding bottom
                dwRsWidth = dwInferenceWidth;
                dwRsHeight = (int)(dwDrawHeight * fWidthRatio);
            } else {
                fHeightRatio = (float)dwInferenceHeight / (float)dwDrawHeight;
                dwRsWidth = (int)(dwDrawWidth * fHeightRatio);
                dwRsHeight = dwInferenceHeight;
            }
        }
    } else {
        dwRsWidth = dwInferenceWidth;
        dwRsHeight = dwInferenceHeight;
    }

    if ((dwRsHeight % 2) != 0)
        dwRsHeight = dwRsHeight - 1;

    dwRsWidth = VMF_16_ALIGN(dwRsWidth);
    *pWidthRatio = (float)dwRsWidth / (float)dwDrawWidth;
    *pHeightRatio = (float)dwRsHeight / (float)dwDrawHeight;
    printf("Inference: (%u,%u) DrawScreen: (%u, %u) Resize: (%u, %u) Ratio: (%.3f, %.3f)\n",
        dwInferenceWidth, dwInferenceHeight, dwDrawWidth, dwDrawHeight,
        dwRsWidth, dwRsHeight, *pWidthRatio, *pHeightRatio);
}

void calculate_bbox_postion(DETECT_INFO *ptResults, float fLeftX, float fTopY, float fRightX, float fBottomY, float fScore, int iClass)
{
    ptResults->dwStartX = g_dwRoiX + (unsigned int)(fLeftX / g_fWidthRatio);
    ptResults->dwStartY = g_dwRoiY + (unsigned int)(fTopY / g_fHeightRatio);
    ptResults->dwWidth = (unsigned int)((fRightX - fLeftX) / g_fWidthRatio);
    ptResults->dwHeight = (unsigned int)((fBottomY - fTopY) / g_fHeightRatio);
    ptResults->fScore = fScore;
    ptResults->dwClass = iClass;
}

void setup_isp_privacy_mask(unsigned int dwEnable, unsigned int dwStartX, unsigned int dwStartY, unsigned int dwWidth, unsigned int dwHeight)
{
	VMF_ISP_OPTION_T tIspOption;
	VMF_ISP_PM_CONFIG_T tIspPmOpt;

	if(dwWidth == 0 || dwHeight == 0)
		return ;

	memset(&tIspOption, 0, sizeof(VMF_ISP_OPTION_T));
	memset(&tIspPmOpt, 0, sizeof(VMF_ISP_PM_CONFIG_T));
	tIspOption.option_flag = ISP_OPTION_PRIVACY_MASK_SETTINGS;
	tIspOption.adwData[0] = (unsigned int)&tIspPmOpt;

	unsigned int dwPmMapStride = 0, dwPmMaskSize = 0, dwFirstSet = 0;
    unsigned int dwInWidth = g_tLayout.dwVideoWidth;
    unsigned int dwInHeight = g_tLayout.dwVideoHeight;
	unsigned char *pcTmpMaskBuffer = NULL;

	tIspPmOpt.bEnable = dwEnable;
	tIspPmOpt.abyColorYUV[0] = 75;
	tIspPmOpt.abyColorYUV[1] = 37;
	tIspPmOpt.abyColorYUV[2] = 66;

	dwPmMapStride = (dwInWidth + 15) >> 4;	//2*2 block use 1bit represetation, and minimum byte unit
 	dwPmMapStride = (dwPmMapStride + 15) & (~15);	//ISP PM need align to 16
	dwPmMaskSize = dwPmMapStride * (dwInHeight >> 1);
	tIspPmOpt.dwMaskStride = dwPmMapStride;
    if (g_dwDrawOnResize) {
        tIspPmOpt.bMainMaskDisable = 1;
        tIspPmOpt.bResizeMaskDisable = 0;
    } else {
        tIspPmOpt.bMainMaskDisable = 0;
        tIspPmOpt.bResizeMaskDisable = 1;
    }
	if (dwStartX + dwWidth >= dwInWidth)
		dwWidth = dwInWidth - dwStartX -1;
	if (dwStartY + dwHeight >= dwInHeight)
		dwHeight = dwInHeight - dwStartY -1;

	unsigned int dwPmx = dwStartX >> 1;
	unsigned int dwPmxE = (dwStartX + dwWidth) >> 1;
	unsigned int dwPmxByte = dwPmx / 8;
	unsigned int dwPmxEByte = dwPmxE / 8;
	unsigned int dwPmy = dwStartY >> 1;
	unsigned int dwPmyE = (dwStartY + dwHeight) >> 1;
	unsigned int dwPmyByte = dwPmy / 8;
	unsigned int dwPmyEByte = dwPmyE / 8;
	unsigned char ucLineS = 0x00, ucLineE = 0x00, ucLineM = 0x00;

	if (dwEnable == 1){
		ucLineS = 0x03;
		ucLineE = 0xC0;
		ucLineM = 0xFF;
	}

	if (g_pm_buf == NULL){
		g_pm_buf = MemBroker_GetMemory(sizeof(unsigned char)*dwPmMaskSize, VMF_ALIGN_TYPE_DEFAULT);
		if (g_pm_buf == NULL) {
			printf("[%s] malloc failed \n", __func__);
			return ;
		}
		memset(g_pm_buf, 0, dwPmMaskSize);
		dwFirstSet = 1;
	}
	tIspPmOpt.pMaskBuffer = (unsigned char*)g_pm_buf;
	if ((dwPmxEByte < dwPmxByte) || (dwPmyEByte < dwPmyByte)){
		printf("[%s] range failed \n", __func__);
		MemBroker_FreeMemory(g_pm_buf);
		g_pm_buf = NULL;
		return;
	}

	//upper line
	pcTmpMaskBuffer = (unsigned char*)(tIspPmOpt.pMaskBuffer + tIspPmOpt.dwMaskStride * dwPmy);
	if ( pcTmpMaskBuffer != NULL ) {
	    memset(pcTmpMaskBuffer + dwPmxByte, ucLineM, dwPmxEByte - dwPmxByte + 1);
		MemBroker_CacheCopyBack(pcTmpMaskBuffer + dwPmxByte, dwPmxEByte - dwPmxByte + 1);
	}
	pcTmpMaskBuffer = (unsigned char*)(tIspPmOpt.pMaskBuffer + tIspPmOpt.dwMaskStride * (dwPmy + 1));
	if ( pcTmpMaskBuffer != NULL ) {
	    memset(pcTmpMaskBuffer + dwPmxByte, ucLineM, dwPmxEByte - dwPmxByte + 1);
		MemBroker_CacheCopyBack(pcTmpMaskBuffer + dwPmxByte, dwPmxEByte - dwPmxByte + 1);
	}

	//lower line
	pcTmpMaskBuffer = (unsigned char*)(tIspPmOpt.pMaskBuffer + tIspPmOpt.dwMaskStride * dwPmyE);
	if ( pcTmpMaskBuffer != NULL ) {
	    memset(pcTmpMaskBuffer + dwPmxByte, ucLineM, dwPmxEByte - dwPmxByte + 1);
		MemBroker_CacheCopyBack(pcTmpMaskBuffer + dwPmxByte, dwPmxEByte - dwPmxByte + 1);
	}

	pcTmpMaskBuffer = (unsigned char*)(tIspPmOpt.pMaskBuffer + tIspPmOpt.dwMaskStride * (dwPmyE - 1));
	if ( pcTmpMaskBuffer != NULL ) {
	    memset(pcTmpMaskBuffer + dwPmxByte, ucLineM, dwPmxEByte - dwPmxByte + 1);
		MemBroker_CacheCopyBack(pcTmpMaskBuffer + dwPmxByte, dwPmxEByte - dwPmxByte + 1);
	}

	//edge
	for (unsigned int i = dwPmy + 1; i <= dwPmyE - 1 ;i++){
		pcTmpMaskBuffer = (unsigned char*)(tIspPmOpt.pMaskBuffer + tIspPmOpt.dwMaskStride * i);
		if (dwEnable == 1){
			pcTmpMaskBuffer[dwPmxByte] |= ucLineS;
			pcTmpMaskBuffer[dwPmxEByte] |= ucLineE;
		} else {
			pcTmpMaskBuffer[dwPmxByte] = ucLineS;
			pcTmpMaskBuffer[dwPmxEByte] = ucLineE;
		}
	}

	if (dwFirstSet == 1) {
		if (VMF_VSRC_ConfigISP(g_ptVsrcHandle, 0, 0, 0, &tIspOption) != 0){	//only one stream, only one layer of isp config , only one isp.
			printf("[%s] VMF_VSRC_ConfigISP failed\n", __func__);
		}
	} else {
		if ( g_pm_buf != NULL ) {
			MemBroker_CacheCopyBack(g_pm_buf, sizeof(unsigned char) * dwPmMaskSize);
		}
	}
}

static void release_video_source(VMF_VSRC_HANDLE_T* hVsrc)
{
	VMF_VSRC_Stop(hVsrc);
	VMF_VSRC_Release(hVsrc);
}

static void setup_spec(VMF_VSRC_SPEC_CONFIG_T* ptSpec, HOST_STREAM_INIT_OPT_T* pHostStreamInit)
{
    unsigned int i = 0;
	ptSpec->bEnableSpec = 1;

    /* Match SDK logic: FEC_C + bOthers=1 only when VOC, ROI, or 1R mode needs it.
     * Otherwise use VMF_ISP_MODE_FEC (standard pipeline). */
    if (0 != pHostStreamInit->bRoiEnable || 1 == pHostStreamInit->dwVocEnable ||
        FEC_MODE_1R == (FEC_MODE)pHostStreamInit->dwFecMode) {
        ptSpec->dwIspMode = VMF_ISP_MODE_FEC_C;
        ptSpec->tIspEncSpec.bOthers = 1;
    } else {
        ptSpec->dwIspMode = VMF_ISP_MODE_FEC;
    }

    if (pHostStreamInit->dwEisEnable) {
        ptSpec->dwIspMode = VMF_ISP_MODE_EIS;
    }

    for (i=0;i<pHostStreamInit->dwEncodeStreamCount;i++) {
        if (VMF_VENC_CODEC_TYPE_H265 == pHostStreamInit->tVEncoder[i].eCodecType){
            ptSpec->tIspEncSpec.bEncH265 = 1;
        } else if (VMF_VENC_CODEC_TYPE_H264 == pHostStreamInit->tVEncoder[i].eCodecType){
            ptSpec->tIspEncSpec.bEncH264 = 1;
        } else if (VMF_VENC_CODEC_TYPE_MJPG == pHostStreamInit->tVEncoder[i].eCodecType){
            ptSpec->tIspEncSpec.bEncJPEG = 1;
        }
    }

    if (pHostStreamInit->dwVocEnable && pHostStreamInit->dwEncodeStreamCount == 0) {
        ptSpec->tIspEncSpec.bEncH264 = 1;
    }
}

/* This function is called after video source is initialized and first video frame is captured in buffer */
static void vsrc_init_callback(unsigned int width, unsigned int height)
{
	memset(&g_tLayout, 0, sizeof(VMF_LAYOUT_T));
	g_tLayout.dwCanvasWidth = width;
	g_tLayout.dwCanvasHeight = height;
	g_tLayout.dwVideoPosX = 0;
	g_tLayout.dwVideoPosY = 0;
	g_tLayout.dwVideoWidth = width;
	g_tLayout.dwVideoHeight = height;
	dbg_log("vsrc_init_callback(): width:%d, height:%d \n", width, height);
}

void set_eis(VMF_EIS_INIT_T* ptEisInit)
{
	EIS_T* ptEis = &g_tEis;

    if(!ptEisInit){
        printf("[%s] Err: eis init is null.\n", __func__);
        return;
    }

	ptEisInit->pszLensCurveNodesPath = ptEis->pszCurveNodesPath;
	ptEisInit->pszLogPath = NULL;
	ptEisInit->fGyroDataGain = ptEis->fGyroDataGain;
	ptEisInit->dwGridSection = ptEis->dwGridSection;
	ptEisInit->dwMaxGridSection = (ptEis->dwMaxGridSection >= ptEis->dwGridSection)?ptEis->dwMaxGridSection:ptEis->dwGridSection;
	ptEisInit->fCropRatio = ptEis->fCropRatio;
	ptEisInit->dwImageType = ptEis->dwImageType;
	ptEisInit->dwProcessMode = ptEis->dwProcessMode;
	ptEisInit->dwCoordinateTransform[0] = ptEis->dwCoordinateTransform[0];
	ptEisInit->dwCoordinateTransform[1] = ptEis->dwCoordinateTransform[1];
	ptEisInit->dwCoordinateTransform[2] = ptEis->dwCoordinateTransform[2];
	ptEisInit->sqwTimeOffset = ptEis->sqwTimeOffset;
	ptEisInit->bImageRotate180 = ptEis->bImageRotate180;
	ptEisInit->sdwReadoutTimeOffset = ptEis->sdwReadoutTimeOffset;
	ptEisInit->fReadoutTimeRatio = ptEis->fReadoutTimeRatio;
	ptEisInit->bForceOriRs = ptEis->bForceOriRs;
    memcpy(&ptEisInit->tGyroConfig, &ptEis->tGyroConfig, sizeof(VMF_VSRC_GYRO_CONFIG_T));
}

static int init_video_source(HOST_STREAM_INIT_OPT_T* pHostStreamInit)
{
	VMF_VSRC_INITOPT_T vsrc_initopt;
	VMF_VSRC_FRONTEND_CONFIG_T tFrontConfig;

	printf("[DBG] init_video_source: sizeof(initopt)=%zu sizeof(frontcfg)=%zu\n",
	       sizeof(VMF_VSRC_INITOPT_T), sizeof(VMF_VSRC_FRONTEND_CONFIG_T));
	fflush(stdout);
	memset(&vsrc_initopt, 0, sizeof(VMF_VSRC_INITOPT_T));
	memset(&tFrontConfig, 0, sizeof(VMF_VSRC_FRONTEND_CONFIG_T));


    if(pHostStreamInit->dwFecMode != 0){
		if(loadCalibrateConfig(pHostStreamInit, &tFrontConfig) == -1){
			printf("[%s] Err: no calibrate config\n", __func__);
			return -1;
		}
        if(loadFECConfig(pHostStreamInit, &tFrontConfig) == -1){
			printf("[%s] Err: no fec config\n", __func__);
			return -1;
		}
    } else {
        /* FEC passthrough (no lens correction). Load FEC config for gFecDefValue defaults only.
         * Pass NULL for ptFrontConfig — skips lens curve node loading (not needed for passthrough).
         * Do NOT set ptFecConfig here — SDK sets only eFecMethod=GTR (below) for this case. */
        if(loadFECConfig(pHostStreamInit, NULL) == -1){
            printf("[%s] No fec config file, using defaults\n", __func__);
        }
        printf("[%s] FEC passthrough mode (dwFecMode=0)\n", __func__);
	}
    /* SDK: eFecMethod=GTR is set unconditionally after the if-else, ptFecConfig stays NULL */
    tFrontConfig.tFecInitConfig.eFecMethod = VMF_FEC_METHOD_GTR;

    //! Fusion or Normal mode
    if (pHostStreamInit->pszFusionConfigPath) {
        tFrontConfig.dwSensorConfigCount = 2;
        tFrontConfig.apszSensorConfig[0] = pHostStreamInit->pszSensorConfigPath;
        tFrontConfig.apszSensorConfig[1] = pHostStreamInit->pszFusionConfigPath;
        vsrc_initopt.eAppMode = VMF_VSRC_APP_MODE_FUSION;
        printf("[%s] Fusion Mode\n", __func__);
    }
    else {
        tFrontConfig.dwSensorConfigCount = 1;
        tFrontConfig.apszSensorConfig[0] = pHostStreamInit->pszSensorConfigPath;
        vsrc_initopt.eAppMode = VMF_VSRC_APP_MODE_NORMAL;
        printf("[%s] Normal Mode\n", __func__);
    }

	vsrc_initopt.dwFrontConfigCount = 1;
	vsrc_initopt.ptFrontConfig = &tFrontConfig;
	vsrc_initopt.pszOutPinPrefix = VENC_VSRC_PIN;
	vsrc_initopt.bShared = 1;
	vsrc_initopt.fnInitCallback = vsrc_init_callback;
	vsrc_initopt.pszResourceDir = VENC_RESOURCE_DIR;
	vsrc_initopt.pszAutoSceneConfig = pHostStreamInit->pszAutoSceneConfigPath;
	setup_spec(&vsrc_initopt.tSpecConfig, pHostStreamInit);

	//! Fill EIS configuration
	if(pHostStreamInit->dwEisEnable == 1) {
		tFrontConfig.tFecInitConfig.ptEisInit = calloc(1, sizeof(VMF_EIS_INIT_T));
		if(!tFrontConfig.tFecInitConfig.ptEisInit) {
			printf("[%s] calloc VMF_EIS_INIT_T fail.\n", __func__);
			pHostStreamInit->dwEisEnable = 0;
		} else {
			set_eis(tFrontConfig.tFecInitConfig.ptEisInit);
			if( tFrontConfig.tFecInitConfig.ptEisInit->dwGridSection > 1 ){
                // Using dma to draw box
				g_dwDrawBoxType = 1;
			}
		}
	}

	printf("[DBG] Before VMF_VSRC_Init: eAppMode=%d frontCnt=%d prefix=%s res=%s\n",
	       vsrc_initopt.eAppMode, vsrc_initopt.dwFrontConfigCount,
	       vsrc_initopt.pszOutPinPrefix, vsrc_initopt.pszResourceDir);
	fflush(stdout);
	g_ptVsrcHandle = VMF_VSRC_Init(&vsrc_initopt);
	printf("[DBG] After VMF_VSRC_Init: handle=%p\n", (void*)g_ptVsrcHandle);
	if (!g_ptVsrcHandle) {
		printf("[%s] VMF_VSRC_Init failed!\n", __func__);
		return -1;
	}

	if (VMF_VSRC_Start(g_ptVsrcHandle, NULL) != 0) {
		printf("[%s] VMF_VSRC_Start failed!\n", __func__);
		release_video_source(g_ptVsrcHandle);
		return -1;
	}

    return setup_fec_mode(g_ptVsrcHandle, &g_tLayout, (FEC_MODE)pHostStreamInit->dwFecMode, pHostStreamInit->dwEisEnable);
}

static int set_video_data(VMF_VDISP_INITOPT_T *ptVideoInit, HOST_STREAM_INIT_OPT_T* pHostStreamInit)
{
	if (!ptVideoInit || !pHostStreamInit) {
		printf("%s failed \n", __func__);
		return -1;
	}
    /* Use ini-configured VOC pixel format. Default to NV12 when not configured. */
    ptVideoInit->dwInPixFormat = g_dwVocPixFmt ? g_dwVocPixFmt : v4l2_fourcc('N', 'V', '1', '2');
	ptVideoInit->dwMaxInWidth  = pHostStreamInit->dwVocWidth;
    ptVideoInit->dwMaxInHeight = pHostStreamInit->dwVocHeight;
    printf("[%s] VOC VDISP pixfmt=0x%08X w=%u h=%u\n", __func__,
        ptVideoInit->dwInPixFormat, ptVideoInit->dwMaxInWidth, ptVideoInit->dwMaxInHeight);
	return 0;
}

void set_data_to_yuv(unsigned char* pucYBuff, unsigned int dwWidth, unsigned int dwHeight, POINT_INFO_T tDrawPoint, int iColor)
{
    unsigned int dwOffsetY = 0, dwOffsetU = 0, dwOffsetV = 0, dwPlaneSize = 0;

    dwHeight = VMF_16_ALIGN(dwHeight);
    dwPlaneSize = ((dwWidth * dwHeight) >> 2);
    dwOffsetY = tDrawPoint.dwY * dwWidth + tDrawPoint.dwX;
    dwOffsetU =  dwWidth * dwHeight + (((tDrawPoint.dwY >> 1) * dwWidth) >> 1) + (tDrawPoint.dwX >> 1);
    dwOffsetV = dwOffsetU + dwPlaneSize ;

    if(iColor){
        pucYBuff[dwOffsetY] = 0x00;
        pucYBuff[dwOffsetU] = 0x00;
        pucYBuff[dwOffsetV] = 0xFF;
    }else{
        pucYBuff[dwOffsetY] = 0xFF;
        pucYBuff[dwOffsetU] = 0x00;
        pucYBuff[dwOffsetV] = 0x00;
    }

}

void draw_line(YUV_BUFF_INFO_T *ptYuvBuffInfo, DRAW_LINE_INFO_T *ptDrawLineInfo, int iColor)
{
    unsigned char *pucBuffY = NULL;
    unsigned int x0 = 0, y0 = 0;
    unsigned int x1 = 0, y1 = 0;
    unsigned int dx = 0, dy = 0;
    int xstep = 0, ystep = 0, nstep = 0, eps = 0;

    if(!ptYuvBuffInfo || !ptYuvBuffInfo->pcYuvBuff)
        return;

    x0 = ptDrawLineInfo->tStartPoint.dwX,
    y0 = ptDrawLineInfo->tStartPoint.dwY;
    x1 = ptDrawLineInfo->tEndPoint.dwX;
    y1 = ptDrawLineInfo->tEndPoint.dwY;

    if(ptDrawLineInfo->dwLineWidth == 0)
        ptDrawLineInfo->dwLineWidth = 1;

    x0 = (x0 >= ptYuvBuffInfo->dwWidth) ? (x0 - ptDrawLineInfo->dwLineWidth) : x0;
    x1 = (x1 >= ptYuvBuffInfo->dwWidth) ? (x1 - ptDrawLineInfo->dwLineWidth) : x1;
    y0 = (y0 >= ptYuvBuffInfo->dwHeight) ? (y0 - ptDrawLineInfo->dwLineWidth) : y0;
    y1 = (y1 >= ptYuvBuffInfo->dwHeight) ? (y1 - ptDrawLineInfo->dwLineWidth) : y1;

    dx = (x0 > x1) ? (x0 - x1) : (x1 - x0);
    dy = (y0 > y1) ? (y0 - y1) : (y1 - y0);

    xstep = (x0 < x1) ? 1 : -1;
    ystep = (y0 < y1) ? 1 : -1;

    POINT_INFO_T draw_point;
    draw_point.dwX = x0;
    draw_point.dwY = y0;

    pucBuffY = ptYuvBuffInfo->pcYuvBuff;

    if(dx > dy){
        while(nstep <= (int)dx){
            set_data_to_yuv(pucBuffY, ptYuvBuffInfo->dwWidth, ptYuvBuffInfo->dwHeight, draw_point, iColor);
            eps += dy;
            if((eps << 1) >= (int)dx){
                draw_point.dwY += ystep;
                eps -= dx;
            }
            draw_point.dwX += xstep;
            nstep++;
        }
    }else{
        while(nstep <= (int)dy){
            set_data_to_yuv(pucBuffY, ptYuvBuffInfo->dwWidth, ptYuvBuffInfo->dwHeight, draw_point, iColor);
            eps += dx;
            if((eps << 1) >= (int)dy){
                draw_point.dwX += xstep;
                eps -= dy;
            }
            draw_point.dwY += ystep;
            nstep++;
        }
    }
}

void draw_rect(YUV_BUFF_INFO_T* yuvBuffInfo, DETECT_INFO *ptDetInfo, int iColor)
{
    DRAW_LINE_INFO_T drawLineInfo;
    int iLineEndX = 0, iLineEndY = 0;
    int iStartX = 0, iStartY = 0, iWidth = 0, iHeight = 0;

    iStartX = (int)ptDetInfo->dwStartX;
    iStartY = (int)ptDetInfo->dwStartY;
    iWidth = (int)ptDetInfo->dwWidth;
    iHeight = (int)ptDetInfo->dwHeight;

    iLineEndX = iStartX + iWidth;
    if(iLineEndX > (int)yuvBuffInfo->dwWidth)
        iLineEndX = (int)yuvBuffInfo->dwWidth;

    iLineEndY = iStartY + iHeight;
    if(iLineEndY > (int)yuvBuffInfo->dwHeight)
        iLineEndY = (int)yuvBuffInfo->dwHeight;

    drawLineInfo.dwLineWidth = 1;
    drawLineInfo.tStartPoint.dwX = iStartX;
    drawLineInfo.tStartPoint.dwY = iStartY;
    drawLineInfo.tEndPoint.dwX = iLineEndX;
    drawLineInfo.tEndPoint.dwY = iStartY;
    draw_line(yuvBuffInfo, &drawLineInfo, iColor);

    drawLineInfo.tStartPoint.dwX = iStartX;
    drawLineInfo.tStartPoint.dwY = iLineEndY;
    drawLineInfo.tEndPoint.dwX = iLineEndX;
    drawLineInfo.tEndPoint.dwY = iLineEndY;
    draw_line(yuvBuffInfo, &drawLineInfo, iColor);

    drawLineInfo.tStartPoint.dwX = iStartX;
    drawLineInfo.tStartPoint.dwY = iStartY;
    drawLineInfo.tEndPoint.dwX = iStartX;
    drawLineInfo.tEndPoint.dwY = iLineEndY;
    draw_line(yuvBuffInfo, &drawLineInfo, iColor);

    drawLineInfo.tStartPoint.dwX = iLineEndX;
    drawLineInfo.tStartPoint.dwY = iStartY;
    drawLineInfo.tEndPoint.dwX = iLineEndX;
    drawLineInfo.tEndPoint.dwY = iLineEndY;
    draw_line(yuvBuffInfo, &drawLineInfo, iColor);
}

/*
 * Per-class YCbCr colours for STDC segmentation overlay (BT.601 full-range).
 * Order matches STDC_CLASS_* indices:
 *   0=bunker  1=car  2=grass  3=greenery  4=person  5=pond  6=road  7=tree
 * road (class 6) is left transparent — it occupies most of the frame and adds
 * visual noise rather than information.
 */
static const uint8_t s_stdc_yuv[STDC_NUM_CLASSES][3] = {
    {161, 141, 119},  /* bunker   #94a3b8 */
    {119,  99, 214},  /* car      #ef4444 */
    {200, 112,  81},  /* grass    #86efac */
    {137, 104,  55},  /* greenery #22c55e */
    {144,  59, 203},  /* person   #f97316 */
    {154, 182,  87},  /* pond     #60a5fa */
    {  0,   0,   0},  /* road     — transparent (unused) */
    { 88, 113,  80},  /* tree     #15803d */
};

/*
 * Paint a 50%-alpha segmentation colour overlay into a YM12 frame.
 * Called once per frame inside the draw-box thread after the DMA copy.
 */
static void stdc_paint_seg_overlay(unsigned char *base_buf,
                                   VMF_VSRC_SSM_OUTPUT_INFO_T *info,
                                   unsigned int frame_w, unsigned int frame_h)
{
    uint8_t  local_map[STDC_SEG_MAP_MAX];
    uint32_t seg_w, seg_h;

    pthread_mutex_lock(&g_stdc_seg_mutex);
    seg_w = g_stdc_seg_w;
    seg_h = g_stdc_seg_h;
    if (seg_w == 0 || seg_h == 0 || seg_w * seg_h > STDC_SEG_MAP_MAX) {
        pthread_mutex_unlock(&g_stdc_seg_mutex);
        return;
    }
    memcpy(local_map, g_stdc_seg_map, seg_w * seg_h);
    pthread_mutex_unlock(&g_stdc_seg_mutex);

    uint8_t  *y_buf    = base_buf + info->dwOffset[0];
    uint8_t  *cb_buf   = base_buf + info->dwOffset[1];
    uint8_t  *cr_buf   = base_buf + info->dwOffset[2];
    uint32_t  y_stride = info->dwYStride;
    uint32_t  uv_stride = info->dwYStride >> 1;

    /* Pre-compute column mapping: output x → seg col index */
    uint8_t col_lut[1920];
    uint32_t fx_max = (frame_w < 1920) ? frame_w : 1920;
    for (uint32_t fx = 0; fx < fx_max; fx++)
        col_lut[fx] = (uint8_t)((fx * seg_w) / frame_w);

    for (uint32_t fy = 0; fy < frame_h; fy++) {
        uint32_t seg_r = (fy * seg_h) / frame_h;
        const uint8_t *seg_row = local_map + seg_r * seg_w;
        uint8_t *y_row = y_buf + fy * y_stride;

        /* Y plane — every pixel */
        for (uint32_t fx = 0; fx < fx_max; fx++) {
            uint8_t cls = seg_row[col_lut[fx]];
            if (cls == STDC_CLASS_ROAD || cls >= STDC_NUM_CLASSES) continue;
            const uint8_t *col = s_stdc_yuv[cls];
            y_row[fx] = (uint8_t)(((uint16_t)y_row[fx] + col[0]) >> 1);
        }

        /* UV plane — every other row, every other column */
        if ((fy & 1) == 0) {
            uint8_t *cb_row = cb_buf + (fy >> 1) * uv_stride;
            uint8_t *cr_row = cr_buf + (fy >> 1) * uv_stride;
            for (uint32_t fx = 0; fx < fx_max; fx += 2) {
                uint8_t cls = seg_row[col_lut[fx]];
                if (cls == STDC_CLASS_ROAD || cls >= STDC_NUM_CLASSES) continue;
                const uint8_t *col = s_stdc_yuv[cls];
                uint32_t uv_x = fx >> 1;
                cb_row[uv_x] = (uint8_t)(((uint16_t)cb_row[uv_x] + col[1]) >> 1);
                cr_row[uv_x] = (uint8_t)(((uint16_t)cr_row[uv_x] + col[2]) >> 1);
            }
        }
    }
}

void* kdp2_host_stream_draw_box(void *arg)
{
	HOST_STREAM_INIT_OPT_T* pHostStreamInit = (HOST_STREAM_INIT_OPT_T*)arg;
	unsigned int i = 0;
	YUV_BUFF_INFO_T tYuvInfo;
	SSM_WRITER_INIT_OPTION_T ssm_opt;
	SSM_HANDLE_T* ptSsmWriterHandle = NULL;
	SSM_HANDLE_T* ptSsmReaderHandle = NULL;
	SSM_BUFFER_T tWriterSsmBuffer, tReaderSsmBuffer;
	VMF_VSRC_SSM_OUTPUT_INFO_T vsrc_ssm_reader_info, vsrc_ssm_writer_info;

	VMF_DMA_HANDLE_T* hDma = NULL;
	VMF_DMA_ADDR_T dma_addr;
	VMF_DMA_2DCF_INIT_T Dma2DInit;
	VMF_DMA_DESCRIPTOR_T* ptDma2DDesc = NULL;

	unsigned int dwWidth = pHostStreamInit->tVEncoder[0].dwWidth;
	unsigned int dwHeight = pHostStreamInit->tVEncoder[0].dwHeight;
	char szVsrcPinName[32];

	memset(&ssm_opt, 0, sizeof(SSM_WRITER_INIT_OPTION_T));
	memset(&vsrc_ssm_writer_info, 0, sizeof(VMF_VSRC_SSM_OUTPUT_INFO_T));

	if(!pHostStreamInit->dwNnmSource){
		if(g_tLayout.dwVideoWidth == dwWidth && g_tLayout.dwVideoHeight == dwHeight){
			ptSsmReaderHandle = SSM_Reader_Init(VENC_VSRC_PIN"_0", 1);
		} else {
			snprintf(szVsrcPinName, 32, "vsrc_ssm_0_%d_%d", dwWidth, dwHeight);
			ptSsmReaderHandle = SSM_Reader_Init(szVsrcPinName, 1);
		}
	}else{
		ptSsmReaderHandle = SSM_Reader_Init(pHostStreamInit->pszSsmName, 1);
	}
	if(!ptSsmReaderHandle){
		printf("ERROR: %s() SSM_Reader_Init failed.\n", __func__);
		goto CUSTOMER_RELEASE;
	}

	SSM_Reader_ReturnReceiveBuff(ptSsmReaderHandle, &tReaderSsmBuffer);
	VMF_VSRC_SSM_GetInfo(tReaderSsmBuffer.buffer, &vsrc_ssm_reader_info);
	memcpy(&vsrc_ssm_writer_info, &vsrc_ssm_reader_info, sizeof(VMF_VSRC_SSM_OUTPUT_INFO_T));

	if(pHostStreamInit->dwNnmSource){
		dwWidth = vsrc_ssm_reader_info.dwWidth;
		dwHeight = vsrc_ssm_reader_info.dwHeight;
	}
	ssm_opt.name = VENC_VSRC_B_PIN;
	ssm_opt.buf_size = ((dwWidth * VMF_16_ALIGN(dwHeight) * 3) >> 1) + VMF_MAX_SSM_HEADER_SIZE;
	ssm_opt.alignment = VMF_ALIGN_TYPE_DEFAULT;
	ssm_opt.pshared = 1;
	ssm_opt.pUserData = &vsrc_ssm_writer_info;
	ssm_opt.fp_setup_buffer = ssm_clear_header;
	ptSsmWriterHandle = SSM_Writer_Init(&ssm_opt);
	if(!ptSsmWriterHandle){
		printf("ERROR: %s() SSM_Writer_Init failed.\n", __func__);
		goto CUSTOMER_RELEASE;
	}
	memset(&tWriterSsmBuffer, 0, sizeof(SSM_BUFFER_T));
	SSM_Writer_SendGetBuff(ptSsmWriterHandle, &tWriterSsmBuffer);

	memset(&Dma2DInit, 0, sizeof(VMF_DMA_2DCF_INIT_T));
	Dma2DInit.dwProcessCbCr = 1;
	Dma2DInit.dwSrcFormatFlag = vsrc_ssm_reader_info.dwType;
	ptDma2DDesc = VMF_DMA_Descriptor_Create(DMA_2D, &Dma2DInit);
	if(!ptDma2DDesc){
		printf("ERROR: %s() VMF_DMA_Descriptor_Create failed.\n", __func__);
		goto CUSTOMER_RELEASE;
	}

	hDma = VMF_DMA_Init(1,128);
	if(!hDma){
		printf("ERROR: %s() VMF_DMA_Init failed.\n", __func__);
		goto CUSTOMER_RELEASE;
	}

	while(!g_iTerminate)
	{
		SSM_Reader_ReturnReceiveBuff(ptSsmReaderHandle, &tReaderSsmBuffer);
		VMF_VSRC_SSM_GetInfo(tReaderSsmBuffer.buffer, &vsrc_ssm_reader_info);

        //memcpy(&vsrc_ssm_writer_info, &vsrc_ssm_reader_info, sizeof(VMF_VSRC_SSM_OUTPUT_INFO_T));
		memcpy(tWriterSsmBuffer.buffer, tReaderSsmBuffer.buffer, sizeof(VMF_FRAME_INFO_T));
		memset(&dma_addr, 0, sizeof(VMF_DMA_ADDR_T));

		dma_addr.dwCopyWidth = dwWidth;
		dma_addr.dwCopyHeight = dwHeight;
		dma_addr.pbySrcYPhysAddr  = tReaderSsmBuffer.buffer_phys_addr + vsrc_ssm_reader_info.dwOffset[0];
		dma_addr.pbySrcCbPhysAddr = tReaderSsmBuffer.buffer_phys_addr + vsrc_ssm_reader_info.dwOffset[1];
		dma_addr.pbySrcCrPhysAddr = tReaderSsmBuffer.buffer_phys_addr + vsrc_ssm_reader_info.dwOffset[2];
		dma_addr.dwSrcStride = vsrc_ssm_reader_info.dwYStride;

        /* Use the SSM-reported offsets for destination planes.
         * Writer SSM header is a copy of reader's (see memcpy above), so the encoder
         * reads Cb/Cr from dwOffset[1/2]. DMA must write there, not at
         * VMF_MAX_SSM_HEADER_SIZE+dwYSize which ignores stride-alignment padding. */
		dma_addr.pbyDstYPhysAddr  = tWriterSsmBuffer.buffer_phys_addr + vsrc_ssm_reader_info.dwOffset[0];
		dma_addr.pbyDstCbPhysAddr = tWriterSsmBuffer.buffer_phys_addr + vsrc_ssm_reader_info.dwOffset[1];
		dma_addr.pbyDstCrPhysAddr = tWriterSsmBuffer.buffer_phys_addr + vsrc_ssm_reader_info.dwOffset[2];
		dma_addr.dwDstStride = vsrc_ssm_reader_info.dwYStride;
		VMF_DMA_Descriptor_Update_Addr(ptDma2DDesc, &dma_addr);

		if(0 != VMF_DMA_Setup(hDma, &ptDma2DDesc, 1)){
			printf("ERROR: %s() VMF_DMA_Setup failed.\n", __func__);
			goto CUSTOMER_RELEASE;
		}
		VMF_DMA_Process(hDma);

        int need_flush = 0;

        if (g_dwResultCounts != 0) {
            tYuvInfo.pcYuvBuff = tWriterSsmBuffer.buffer + vsrc_ssm_reader_info.dwOffset[0];
            tYuvInfo.dwWidth = dwWidth;
            tYuvInfo.dwHeight = dwHeight;
            for (i = 0; i < g_dwResultCounts; i++)
                draw_rect(&tYuvInfo, &g_atDrawInfo[i], 1);
            need_flush = 1;
        }

        /* Paint STDC segmentation colour overlay (50% alpha blend into YM12) */
        if (g_stdc_seg_w > 0) {
            stdc_paint_seg_overlay(tWriterSsmBuffer.buffer, &vsrc_ssm_reader_info,
                                   dwWidth, dwHeight);
            need_flush = 1;
        }

        if (need_flush) {
            unsigned int flush_start = vsrc_ssm_reader_info.dwOffset[0];
            unsigned int flush_end   = vsrc_ssm_reader_info.dwOffset[2] + vsrc_ssm_reader_info.dwUVSize;
            MemBroker_CacheFlush(tWriterSsmBuffer.buffer + flush_start, flush_end - flush_start);
        }
		SSM_Writer_SendGetBuff(ptSsmWriterHandle, &tWriterSsmBuffer);
	}

CUSTOMER_RELEASE:

	if(hDma)
		VMF_DMA_Release(hDma);

	if(ptDma2DDesc)
		VMF_DMA_Descriptor_Destroy(ptDma2DDesc);

	if(ptSsmReaderHandle){
		SSM_Reader_ReturnBuff(ptSsmReaderHandle, &tReaderSsmBuffer);
		SSM_Release(ptSsmReaderHandle);
	}

	if (ptSsmWriterHandle)
		SSM_Release(ptSsmWriterHandle);

	return NULL;
}

int init_video_display(HOST_STREAM_INIT_OPT_T* pHostStreamInit)
{
	VMF_VDISP_INITOPT_T tDisplayOpt;
	memset(&tDisplayOpt, 0, sizeof(VMF_VDISP_INITOPT_T));
	set_video_data(&tDisplayOpt, pHostStreamInit);

    if(pHostStreamInit->dwVocWidth != pHostStreamInit->tVEncoder[0].dwWidth ||
       pHostStreamInit->dwVocHeight != pHostStreamInit->tVEncoder[0].dwHeight){
		printf("[WARNING] voc resolution doesn't match main stream!! (%ux%u)!=(%ux%u)\n", pHostStreamInit->tVEncoder[0].dwWidth, pHostStreamInit->tVEncoder[0].dwHeight, pHostStreamInit->dwVocWidth, pHostStreamInit->dwVocHeight);
    }
	g_ptDisplayHandle = VMF_VDISP_Init(&tDisplayOpt);
	if (!g_ptDisplayHandle) {
		printf("%s init vdisp failed \n", __func__);
		return -1;
	}
	VMF_VDISP_SetEarlyInterrupt(g_ptDisplayHandle, 2);
	return 0;
}

static VMF_BIND_CONTEXT_T* init_bind(void* hVideoSource)
{
	//! init video bind
	VMF_BIND_INITOPT_T tBindOpt;
	memset(&tBindOpt, 0, sizeof(VMF_BIND_INITOPT_T));
	tBindOpt.dwSrcOutputIndex = 0;
	tBindOpt.ptSrcHandle = hVideoSource;
	tBindOpt.pfnQueryFunc = (VMF_BIND_QUERY_FUNC) VMF_VSRC_GetInfo;
	tBindOpt.pfnIspFunc = (VMF_BIND_CONFIG_ISP_FUNC) VMF_VSRC_ConfigISP;
	return VMF_BIND_Init(&tBindOpt);
}

static void release_bind(VMF_BIND_CONTEXT_T* pBind)
{
	VMF_BIND_Release(pBind);
}

static int Custom_Roi_BIND_Request(VMF_BIND_CONTEXT_T* ptContext, unsigned int dwWidth, unsigned int dwHeight,
	unsigned int dwStride __attribute__((unused)), unsigned int dwFps __attribute__((unused)), VMF_SRC_CONNECT_INFO_T* ptConnectInfo) 
{
	if (!ptContext) {
		return -1;
	}
	
	ptConnectInfo->bConnectIfp = 0;
	ptConnectInfo->bDisableSharedOsd = 0;
	ptConnectInfo->bIsSsmShared = 1;
	ptConnectInfo->bUnregister = 0;
	ptConnectInfo->bUseResizedSrc = 0;
	ptConnectInfo->dwCodecType = g_eCodecType;
	ptConnectInfo->dwDataType = 0;
	ptConnectInfo->dwSrcWidth = dwWidth;
	ptConnectInfo->dwSrcHeight = dwHeight;
	ptConnectInfo->dwSrcYStride = dwWidth;
	ptConnectInfo->dwSrcUVStride = ptConnectInfo->dwSrcYStride>>1;
	memcpy(ptConnectInfo->szSrcPin, VENC_VSRC_C_PIN, strlen(VENC_VSRC_C_PIN));

	return 0;
}

static int Custom_Main_BIND_Request(VMF_BIND_CONTEXT_T* ptContext, unsigned int dwWidth, unsigned int dwHeight,
	unsigned int dwStride __attribute__((unused)), unsigned int dwFps __attribute__((unused)), VMF_SRC_CONNECT_INFO_T* ptConnectInfo)
{
	if (!ptContext) {
		return -1;
	}

	ptConnectInfo->bConnectIfp = 0;
	ptConnectInfo->bDisableSharedOsd = 0;
	ptConnectInfo->bIsSsmShared = 1;
	ptConnectInfo->bUnregister = 0;
	ptConnectInfo->bUseResizedSrc = 0;
	ptConnectInfo->dwCodecType = g_eCodecType;
	ptConnectInfo->dwDataType = 0;
	ptConnectInfo->dwSrcWidth = dwWidth;
	ptConnectInfo->dwSrcHeight = dwHeight;
	ptConnectInfo->dwSrcYStride = dwWidth;
	ptConnectInfo->dwSrcUVStride = ptConnectInfo->dwSrcYStride>>1;
	memcpy(ptConnectInfo->szSrcPin, VENC_VSRC_B_PIN, strlen(VENC_VSRC_B_PIN));
	return 0;
}

static int init_output_srb(const char* pcName, unsigned int dwBufNumber, unsigned int dwBufSize, VMF_VENC_OUT_SRB_T** pptVencOutputSRB)
{
    VMF_VENC_OUT_SRB_INITOPT_T tSrbInitOpt;
    memset(&tSrbInitOpt, 0, sizeof(VMF_VENC_OUT_SRB_INITOPT_T));

    tSrbInitOpt.pszSrbName = pcName;
    tSrbInitOpt.dwSrbNum = dwBufNumber;
    tSrbInitOpt.dwSrbSize = dwBufSize;

    if(0 != VMF_VENC_OUT_SRB_Init(pptVencOutputSRB, &tSrbInitOpt)){
        printf("[%s] VMF_VENC_OUT_SRB_Init failed!!\n", __func__);
        return -1;
    }

    return 0;
}

static void release_video_encoder(VMF_VENC_HANDLE_T* ptVencHandle, VMF_VENC_OUT_SRB_T** pptVencOutputSRB)
{
    VMF_VENC_Release(ptVencHandle);
    VMF_VENC_OUT_SRB_Release(pptVencOutputSRB);
}

static VMF_VENC_HANDLE_T* init_video_encoder(VEncoder tStream, VMF_VENC_OUT_SRB_T* ptVencOutputSRB, 
    bool bKeepRatio, VMF_BIND_CONTEXT_T *ptBind, unsigned int dwBindMode)
{
    VMF_VENC_HANDLE_T* ptVencHandle = NULL;
    VMF_VENC_CONFIG_T tVencConfig;
    VMF_H4E_CONFIG_T tH4e_config = {
        tStream.dwQp,					// dwQp
        tStream.dwBitrate,				// dwBitrate
        (double)tStream.dwFps,			// dbFps
        tStream.dwGop,					// dwGop
        VMF_H4E_PROFILE_HIGH,			// eProfile
        0,								// iSliceQualityStrategy
        VMF_ADMODE_MEET_FPS,			// eAdMode
        0,								// dwMinQp
        0,								// dwMaxQp
        0,								// dwMinFps
        tStream.dwVirtIFrameInterval,	// dwVirtIFrameInterval
        tStream.dwPiq					// dwPIQ
    };

    VMF_H5E_CONFIG_T tH5e_config = {
        tStream.dwQp,					// dwQp
        tStream.dwBitrate,				// dwBitrate
        tStream.dwFps,					// dwFps
        tStream.dwGop,					// dwGop
        0,								// iSliceQualityStrategy
        0,								// dwMinQp
        0,								// dwMaxQp
        tStream.dwVirtIFrameInterval,	// Virtual I-frame interval
        tStream.dwPiq,					// dwPIQ
        VMF_ADMODE_MEET_FPS,			// eAdMode
        0                               // Complex map control in VBR mode
    };

    VMF_JE_CONFIG_T tJe_config = {
        tStream.dwQp,					// dwQp
        0,								// bEnableThumbnail
        0,								// dwThumbnailQp
        0,								// bJfifHdr
        tStream.dwBitrate,				// dwBitrate
        tStream.dwFps					// dbFps
    };

    memset(&tVencConfig, 0, sizeof(VMF_VENC_CONFIG_T));
    tVencConfig.dwEncWidth = tStream.dwWidth;
    tVencConfig.dwEncHeight = tStream.dwHeight;
    tVencConfig.dwFps = tStream.dwFps;
    tVencConfig.bConnectIfp = 0;
    printf("[%s] enc :%d, %d \n", __func__, tStream.dwWidth, tStream.dwHeight);
    switch(tStream.eCodecType){
        case VMF_VENC_CODEC_TYPE_H264:
            tVencConfig.eCodecType = VMF_VENC_CODEC_TYPE_H264;
            tVencConfig.pCodecConfig = &tH4e_config;
            break;

        case VMF_VENC_CODEC_TYPE_H265:
            tVencConfig.eCodecType = VMF_VENC_CODEC_TYPE_H265;
            tVencConfig.pCodecConfig = &tH5e_config;
            break;

        case VMF_VENC_CODEC_TYPE_MJPG:
            tVencConfig.eCodecType = VMF_VENC_CODEC_TYPE_MJPG;
            tVencConfig.pCodecConfig = &tJe_config;
            break;

        default:
            printf("[%s] Invalid Codec Type\n", __func__);
            return NULL;
    }

    VMF_VENC_OUT_SRB_Setup_Config(&tVencConfig, tVencConfig.eCodecType, tVencConfig.pCodecConfig, ptVencOutputSRB);

    tVencConfig.pBind = ptBind;
    if (dwBindMode == 1) {
        tVencConfig.fnSrcConnectFunc = (VMF_SRC_CONNECT_FUNC) Custom_Roi_BIND_Request;
    } else if(dwBindMode == 2){
        tVencConfig.fnSrcConnectFunc = (VMF_SRC_CONNECT_FUNC) Custom_Main_BIND_Request;
    }
    else {
        tVencConfig.fnSrcConnectFunc = (VMF_SRC_CONNECT_FUNC) VMF_BIND_Request;
        tVencConfig.bKeepRatio = bKeepRatio;
    }
    ptVencHandle = VMF_VENC_Init(&tVencConfig);
    if(!ptVencHandle){
        printf("[%s] VMF_VENC_Init() failed\n", __func__);
        return NULL;
    }

    VMF_VENC_ProduceStreamHdr(ptVencHandle);

    return ptVencHandle;
}

static int init_output_scm(const char* pcName, unsigned int dwBufSize, VMF_VENC_OUT_SCM_T** pptVencOutputSCM)
{
	VMF_VENC_OUT_SCM_INITOPT_T tScmInitOpt;
    memset(&tScmInitOpt, 0, sizeof(VMF_VENC_OUT_SCM_INITOPT_T));
    tScmInitOpt.szScmName = pcName;
    tScmInitOpt.dwChkSize = dwBufSize/3;
    tScmInitOpt.dwBufSize = dwBufSize;
    tScmInitOpt.bBlock = 1;
    tScmInitOpt.bLimit = 1;
    if (0 != VMF_VENC_OUT_SCM_Init(pptVencOutputSCM, &tScmInitOpt)) {
        printf("[%s] VMF_VENC_OUT_SCM_Init failed!!\n", __func__);
        return -1;
    }
    return 0;
}

static void release_video_encoder_scm(VMF_VENC_HANDLE_T* ptVencHandle, VMF_VENC_OUT_SCM_T** pptVencOutputSCM)
{
    VMF_VENC_Release(ptVencHandle);
    VMF_VENC_OUT_SCM_Release(pptVencOutputSCM);
}

static VMF_VENC_HANDLE_T* init_video_encoder_scm(VEncoder tStream, VMF_VENC_OUT_SCM_T* ptVencOutputSCM, 
    bool bKeepRatio, VMF_BIND_CONTEXT_T *ptBind, unsigned int dwBindMode)
{
    VMF_VENC_HANDLE_T* ptVencHandle = NULL;
    VMF_VENC_CONFIG_T tVencConfig;
    VMF_H4E_CONFIG_T tH4e_config = {
        tStream.dwQp,					// dwQp
        tStream.dwBitrate,				// dwBitrate
        (double)tStream.dwFps,			// dbFps
        tStream.dwGop,					// dwGop
        VMF_H4E_PROFILE_HIGH,			// eProfile
        0,								// iSliceQualityStrategy
        VMF_ADMODE_MEET_FPS,			// eAdMode
        0,								// dwMinQp
        0,								// dwMaxQp
        0,								// dwMinFps
        tStream.dwVirtIFrameInterval,	// dwVirtIFrameInterval
        tStream.dwPiq					// dwPIQ
    };

    VMF_H5E_CONFIG_T tH5e_config = {
        tStream.dwQp,					// dwQp
        tStream.dwBitrate,				// dwBitrate
        tStream.dwFps,					// dwFps
        tStream.dwGop,					// dwGop
        0,								// iSliceQualityStrategy
        0,								// dwMinQp
        0,								// dwMaxQp
        tStream.dwVirtIFrameInterval,	// Virtual I-frame interval
        tStream.dwPiq,					// dwPIQ
        VMF_ADMODE_MEET_FPS,			// eAdMode
        0                               // Complex map control in VBR mode
    };

    VMF_JE_CONFIG_T tJe_config = {
        tStream.dwQp,					// dwQp
        0,								// bEnableThumbnail
        0,								// dwThumbnailQp
        0,								// bJfifHdr
        tStream.dwBitrate,				// dwBitrate
        tStream.dwFps					// dbFps
    };

    memset(&tVencConfig, 0, sizeof(VMF_VENC_CONFIG_T));
    tVencConfig.dwEncWidth = tStream.dwWidth;
    tVencConfig.dwEncHeight = tStream.dwHeight;
    tVencConfig.dwFps = tStream.dwFps;
    tVencConfig.bConnectIfp = 0;
    printf("[%s] enc :%d, %d \n", __func__, tStream.dwWidth, tStream.dwHeight);
    switch(tStream.eCodecType){
        case VMF_VENC_CODEC_TYPE_H264:
            tVencConfig.eCodecType = VMF_VENC_CODEC_TYPE_H264;
            tVencConfig.pCodecConfig = &tH4e_config;
            break;

        case VMF_VENC_CODEC_TYPE_H265:
            tVencConfig.eCodecType = VMF_VENC_CODEC_TYPE_H265;
            tVencConfig.pCodecConfig = &tH5e_config;
            break;

        case VMF_VENC_CODEC_TYPE_MJPG:
            tVencConfig.eCodecType = VMF_VENC_CODEC_TYPE_MJPG;
            tVencConfig.pCodecConfig = &tJe_config;
            break;

        default:
            printf("[%s] Invalid Codec Type\n", __func__);
            return NULL;
    }

    VMF_VENC_OUT_SCM_Setup_Config(&tVencConfig, tVencConfig.eCodecType, tVencConfig.pCodecConfig, ptVencOutputSCM);

    tVencConfig.pBind = ptBind;
    if (dwBindMode == 1) {
        tVencConfig.fnSrcConnectFunc = (VMF_SRC_CONNECT_FUNC) Custom_Roi_BIND_Request;
    } else if(dwBindMode == 2){
        tVencConfig.fnSrcConnectFunc = (VMF_SRC_CONNECT_FUNC) Custom_Main_BIND_Request;
    }
    else {
        tVencConfig.fnSrcConnectFunc = (VMF_SRC_CONNECT_FUNC) VMF_BIND_Request;
        tVencConfig.bKeepRatio = bKeepRatio;
    }
    ptVencHandle = VMF_VENC_Init(&tVencConfig);
    if(!ptVencHandle){
        printf("[%s] VMF_VENC_Init() failed\n", __func__);
        return NULL;
    }

    VMF_VENC_ProduceStreamHdr(ptVencHandle);

    return ptVencHandle;
}

void ssm_clear_header(unsigned char* virt_addr, unsigned int buf_size, void* pUserData)
{
	VMF_VSRC_SSM_OUTPUT_INFO_T* vsrc_ssm_writer_info = (VMF_VSRC_SSM_OUTPUT_INFO_T*) pUserData;

	if (buf_size > VMF_MAX_SSM_HEADER_SIZE)
		memset(virt_addr, 0, VMF_MAX_SSM_HEADER_SIZE);

	VMF_VSRC_SSM_SetInfo(virt_addr, vsrc_ssm_writer_info);	
}

static void msg_callback(MsgContext *ptMsgContext, void *pUserData)
{
    HOST_STREAM_INIT_OPT_T *pHostStreamInit = (HOST_STREAM_INIT_OPT_T*)pUserData;
    printf("ptMsgContext->pszHost=%s, ptMsgContext->pszCmd=%s \n", ptMsgContext->pszHost, ptMsgContext->pszCmd);

    if(!strncasecmp(ptMsgContext->pszHost, "encoder", 7)){
        int stream_num = atoi((ptMsgContext->pszHost+7));
        printf("[streamer_msg] msg_context->host= %s, number: %d\n", ptMsgContext->pszHost, stream_num);

        if(!strcasecmp(ptMsgContext->pszCmd, "start")){
            if(++pHostStreamInit->tVEncoder[stream_num].dwAliveCount == 1)
                VMF_VENC_Start(pHostStreamInit->tVEncoder[stream_num].ptHandle);
        }else if(!strcasecmp(ptMsgContext->pszCmd, "stop")){
            if(pHostStreamInit->tVEncoder[stream_num].dwAliveCount){
                if(--pHostStreamInit->tVEncoder[stream_num].dwAliveCount == 0)
                    VMF_VENC_Stop(pHostStreamInit->tVEncoder[stream_num].ptHandle);
            }
        }else if(!strcasecmp(ptMsgContext->pszCmd, "forceCI")){
            VMF_VENC_ProduceStreamHdr(pHostStreamInit->tVEncoder[stream_num].ptHandle);
        }else if(!strcasecmp(ptMsgContext->pszCmd, "forceIntra")){
            VMF_VENC_SetIntra(pHostStreamInit->tVEncoder[stream_num].ptHandle);
        }
	} else if (!strcasecmp(ptMsgContext->pszHost, "live_vdisp")) {
        if(g_dwVocEnable){
            if (!strcasecmp(ptMsgContext->pszCmd, "mirror")) {
                unsigned int bMirrorEn = *(unsigned int *) ptMsgContext->pbyData;
                VMF_VDISP_SetMirror(g_ptDisplayHandle, bMirrorEn);
            }
            if (!strcasecmp(ptMsgContext->pszCmd, "flip")) {
                unsigned int bFlipEn = *(unsigned int *) ptMsgContext->pbyData;
                VMF_VDISP_SetFlip(g_ptDisplayHandle, bFlipEn);
            }
            if (!strcasecmp(ptMsgContext->pszCmd, "setContrast")) {
                int sdwContrast = *(int *) ptMsgContext->pbyData;
                int sdwGetContrast = 0;
                VMF_VDISP_YUVOutput_SetContrast(g_ptDisplayHandle, sdwContrast);
                VMF_VDISP_YUVOutput_GetContrast(g_ptDisplayHandle, &sdwGetContrast);
                if(sdwContrast == sdwGetContrast){
                    printf("Setting contrast value %d successfully.\n", sdwContrast);
                } else {
                    printf("Setting contrast value %d out of range. (set to %d).\n", sdwContrast, sdwGetContrast);
                }
            }
            if (!strcasecmp(ptMsgContext->pszCmd, "setBrightness")) {
                int sdwBrightness = *(int *) ptMsgContext->pbyData;
                int sdwGetBrightness = 0;
                VMF_VDISP_YUVOutput_SetBrightness(g_ptDisplayHandle, sdwBrightness);
                VMF_VDISP_YUVOutput_GetBrightness(g_ptDisplayHandle, &sdwGetBrightness);
                if(sdwBrightness == sdwGetBrightness){
                    printf("Setting brightness value %d successfully.\n", sdwBrightness);
                } else {
                    printf("Setting brightness value %d out of range. (set to %d).\n", sdwBrightness, sdwGetBrightness);
                }
            }
            if (!strcasecmp(ptMsgContext->pszCmd, "setSaturation")) {
                unsigned int dwSaturation = *(unsigned int *) ptMsgContext->pbyData;
                unsigned int dwGetSaturation = 0;
                VMF_VDISP_YUVOutput_SetSaturation(g_ptDisplayHandle, dwSaturation);
                VMF_VDISP_YUVOutput_GetSaturation(g_ptDisplayHandle, &dwGetSaturation);
                if(dwSaturation == dwGetSaturation){
                    printf("Setting saturation value %d successfully.\n", dwSaturation);
                } else {
                    printf("Setting saturation value %d out of range. (set to %d).\n", dwSaturation, dwGetSaturation);
                }
            }
        }
    } else if( !strcasecmp(ptMsgContext->pszHost, SR_MODULE_NAME) ){
		if( !strcasecmp(ptMsgContext->pszCmd, SUSPEND_CMD) ) {
			pthread_mutex_lock(&pHostStreamInit->tSuspendpMutex);
			pHostStreamInit->bSuspend = 1;
			pthread_mutex_unlock(&pHostStreamInit->tSuspendpMutex);

            for (unsigned int i=0;i<pHostStreamInit->dwEncodeStreamCount;i++) {
			    VMF_VENC_Suspend(pHostStreamInit->tVEncoder[i].ptHandle);
            }
			VMF_VSRC_Suspend(g_ptVsrcHandle);
            VMF_NNM_Fifoq_Manager_Suspend();

			if (!pHostStreamInit->dwVocEnable) 
                MsgBroker_SuspendAckMsg();
		}else if( !strcasecmp(ptMsgContext->pszCmd, RESUME_CMD) ) {
			VMF_VSRC_Resume(g_ptVsrcHandle);
            for (unsigned int i=0;i<pHostStreamInit->dwEncodeStreamCount;i++) {
			    VMF_VENC_Resume(pHostStreamInit->tVEncoder[i].ptHandle);
            }
            VMF_NNM_Fifoq_Manager_Resume();

			pthread_mutex_lock(&pHostStreamInit->tSuspendpMutex);
			pHostStreamInit->bSuspend = 0;
			pthread_cond_signal(&pHostStreamInit->tSuspendCond);
			pthread_mutex_unlock(&pHostStreamInit->tSuspendpMutex);
		}
	}

    if(ptMsgContext->bHasResponse)
        ptMsgContext->dwDataSize = 0;
}

void *kdp2_host_video_thread(void *arg)
{
	HOST_STREAM_INIT_OPT_T* pHostStreamInit = (HOST_STREAM_INIT_OPT_T*)arg;
	MsgBroker_RegisterMsg(VENC_CMD_FIFO);
    MsgBroker_Run(VENC_CMD_FIFO, msg_callback, pHostStreamInit, &g_iTerminate);
	MsgBroker_UnRegisterMsg();
    return NULL;
}

void* kdp2_host_roi_image_thread(void *arg)
{
	HOST_STREAM_INIT_OPT_T* pHostStreamInit = (HOST_STREAM_INIT_OPT_T*)arg;
    
    unsigned int dwRoiImageWidth = pHostStreamInit->tVEncoder[pHostStreamInit->dwInferenceStream].dwWidth;
    unsigned int dwRoiImageHeight = pHostStreamInit->tVEncoder[pHostStreamInit->dwInferenceStream].dwHeight;
	unsigned int dwRoiStartX = pHostStreamInit->dwRoiX;
	unsigned int dwRoiStartY = pHostStreamInit->dwRoiY;

	//! init ssm reader & writer
	SSM_WRITER_INIT_OPTION_T ssm_opt;
	SSM_HANDLE_T *ptSsmWriterHandle = NULL, *ptSsmReaderHandle = NULL;
	SSM_BUFFER_T tWriterSsmBuffer, tReaderSsmBuffer;
	VMF_VSRC_SSM_OUTPUT_INFO_T vsrc_ssm_reader_info, vsrc_ssm_writer_info;

	//! init DMA hander, 2D descriptor
	VMF_DMA_HANDLE_T* hDma = NULL;
	VMF_DMA_ADDR_T dma_addr;
	VMF_DMA_2DCF_INIT_T Dma2DInit;
	VMF_DMA_DESCRIPTOR_T* ptDma2DDesc = NULL;

	// memset the uninitialized data structures here to avoid UNINIT case warned by Coverity. 
	memset(&ssm_opt, 0, sizeof(SSM_WRITER_INIT_OPTION_T));
	memset(&tWriterSsmBuffer, 0, sizeof(SSM_BUFFER_T));
	memset(&vsrc_ssm_writer_info, 0, sizeof(VMF_VSRC_SSM_OUTPUT_INFO_T));
	memset(&vsrc_ssm_reader_info, 0, sizeof(VMF_VSRC_SSM_OUTPUT_INFO_T));
	memset(&Dma2DInit, 0, sizeof(VMF_DMA_2DCF_INIT_T));

	vsrc_ssm_writer_info.dwYStride = dwRoiImageWidth;
	vsrc_ssm_writer_info.dwYSize = vsrc_ssm_writer_info.dwYStride*dwRoiImageHeight;
	vsrc_ssm_writer_info.dwUVSize = vsrc_ssm_writer_info.dwYSize>>2;
	vsrc_ssm_writer_info.dwOffset[0] = VMF_MAX_SSM_HEADER_SIZE;
	vsrc_ssm_writer_info.dwOffset[1] = vsrc_ssm_writer_info.dwOffset[0] + vsrc_ssm_writer_info.dwYSize;
	vsrc_ssm_writer_info.dwOffset[2] = vsrc_ssm_writer_info.dwOffset[1] + vsrc_ssm_writer_info.dwUVSize;
	vsrc_ssm_writer_info.dwWidth = dwRoiImageWidth;
	vsrc_ssm_writer_info.dwHeight = dwRoiImageHeight;
	
	ssm_opt.name      = VENC_VSRC_C_PIN;
	ssm_opt.buf_size  = dwRoiImageWidth*dwRoiImageHeight*3/2 + VMF_MAX_SSM_HEADER_SIZE;
	ssm_opt.alignment = VMF_ALIGN_TYPE_DEFAULT;
	ssm_opt.pshared   = 1;
	ssm_opt.pUserData = &vsrc_ssm_writer_info;
	ssm_opt.fp_setup_buffer = ssm_clear_header;
	ptSsmWriterHandle = SSM_Writer_Init(&ssm_opt);
	if (!ptSsmWriterHandle) {
		printf("%s() failed, SSM_Reader_Init failed!\n", __func__);
		goto EXIT_ROI_IMAGE_THREAD;
	}
	SSM_Writer_SendGetBuff(ptSsmWriterHandle, &tWriterSsmBuffer);

   	ptSsmReaderHandle = SSM_Reader_Init(VENC_VSRC_PIN"_0", 1);
	if (!ptSsmReaderHandle) {
		printf("%s() failed, SSM_Reader_Init failed!\n", __func__);
		goto EXIT_ROI_IMAGE_THREAD;
	}
	SSM_Reader_ReturnReceiveBuff(ptSsmReaderHandle, &tReaderSsmBuffer);
	VMF_VSRC_SSM_GetInfo(tReaderSsmBuffer.buffer, &vsrc_ssm_reader_info);
    if ((vsrc_ssm_reader_info.dwWidth < (dwRoiImageWidth+dwRoiStartX)) ||
        (vsrc_ssm_reader_info.dwHeight < (dwRoiImageHeight+dwRoiStartY))) {
		printf("%s() failed, ROI window w %u h %u larger than main stream w %u h %u! \n", 
            __func__, dwRoiImageWidth+dwRoiStartX, dwRoiImageHeight+dwRoiStartY, vsrc_ssm_reader_info.dwWidth, vsrc_ssm_reader_info.dwHeight);;
		goto EXIT_ROI_IMAGE_THREAD;
    }

	//create a DMA Descriptor with specified DMA mode
	Dma2DInit.dwProcessCbCr = 1;
	Dma2DInit.dwSrcFormatFlag = vsrc_ssm_reader_info.dwType;
	ptDma2DDesc = VMF_DMA_Descriptor_Create(DMA_2D, &Dma2DInit);
	if (!ptDma2DDesc) {
		printf("%s() failed, VMF_DMA_Descriptor_Create failed!\n", __func__);
		goto EXIT_ROI_IMAGE_THREAD;
	}

	//initilizing a DMA handle 	
	hDma = VMF_DMA_Init(1,128);
	if (!hDma) {
		printf("%s() failed, VMF_DMA_Init failed!\n", __func__);
		goto EXIT_ROI_IMAGE_THREAD;
	}

	dwRoiStartX = VMF_16_ALIGN(dwRoiStartX);
	dwRoiStartY = VMF_8_ALIGN(dwRoiStartY);
	
	while (!g_iTerminate) 
	{
		SSM_Reader_ReturnReceiveBuff(ptSsmReaderHandle, &tReaderSsmBuffer);
		VMF_VSRC_SSM_GetInfo(tReaderSsmBuffer.buffer, &vsrc_ssm_reader_info);	

		memcpy(tWriterSsmBuffer.buffer, tReaderSsmBuffer.buffer, sizeof(VMF_FRAME_INFO_T));

		/*2D DMA*/
		//update source and destination address 
		memset(&dma_addr, 0, sizeof(VMF_DMA_ADDR_T));
		dma_addr.dwCopyWidth = dwRoiImageWidth;
		dma_addr.dwCopyHeight = dwRoiImageHeight;
		dma_addr.pbySrcYPhysAddr  = tReaderSsmBuffer.buffer_phys_addr + vsrc_ssm_reader_info.dwOffset[0] + dwRoiStartY*vsrc_ssm_reader_info.dwYStride + dwRoiStartX;
		dma_addr.pbySrcCbPhysAddr = tReaderSsmBuffer.buffer_phys_addr + vsrc_ssm_reader_info.dwOffset[1] + dwRoiStartY*vsrc_ssm_reader_info.dwYStride/4 + dwRoiStartX/2;
		dma_addr.pbySrcCrPhysAddr = tReaderSsmBuffer.buffer_phys_addr + vsrc_ssm_reader_info.dwOffset[2] + dwRoiStartY*vsrc_ssm_reader_info.dwYStride/4 + dwRoiStartX/2;
		dma_addr.dwSrcStride = vsrc_ssm_reader_info.dwYStride;
	
		dma_addr.pbyDstYPhysAddr = tWriterSsmBuffer.buffer_phys_addr + VMF_MAX_SSM_HEADER_SIZE;
		dma_addr.pbyDstCbPhysAddr = dma_addr.pbyDstYPhysAddr  + dwRoiImageWidth* dwRoiImageHeight;
		dma_addr.pbyDstCrPhysAddr = dma_addr.pbyDstCbPhysAddr + dwRoiImageWidth* dwRoiImageHeight/4;
		dma_addr.dwDstStride = dwRoiImageWidth;
		VMF_DMA_Descriptor_Update_Addr(ptDma2DDesc, &dma_addr);

		if ( 0 != VMF_DMA_Setup(hDma, &ptDma2DDesc, 1)) {
			fprintf(stderr, "VMF_DMA_Setup failed!\n");
		}

		//start processing
		VMF_DMA_Process(hDma);

		SSM_Writer_SendGetBuff(ptSsmWriterHandle, &tWriterSsmBuffer);
	}

EXIT_ROI_IMAGE_THREAD:

	//free DMA handle
	if (hDma)
		VMF_DMA_Release(hDma);
	//free DMA descriptor
	if (ptDma2DDesc)
		VMF_DMA_Descriptor_Destroy(ptDma2DDesc);

	if (ptSsmReaderHandle) {
		SSM_Reader_ReturnBuff(ptSsmReaderHandle, &tReaderSsmBuffer);
		SSM_Release(ptSsmReaderHandle);
	}
	if (ptSsmWriterHandle) {
		SSM_Release(ptSsmWriterHandle);
	}
	return NULL;
}

void *kdp2_host_voc_thread(void *arg)
{
    HOST_STREAM_INIT_OPT_T* pHostStreamInit = (HOST_STREAM_INIT_OPT_T*)arg;
	VMF_FRAME_BUF_T abuf[VMF_VIDEO_DISPLAY_MIN_QUEUE_SIZE];
	ssm_handle_t *ptSsmHandle = NULL;
	unsigned int dwHeight = (pHostStreamInit->dwVocHeight + 15 ) & (~15);
	unsigned int dwYSize = pHostStreamInit->dwVocWidth*dwHeight;
	unsigned int q_idx = 0;
	ssm_buffer_t ssm_buf[VMF_VIDEO_DISPLAY_MIN_QUEUE_SIZE];
	int ret;
	VMF_SRC_CONNECT_INFO_T tSrcConnectInfo;

	memset(abuf, 0, sizeof(VMF_FRAME_BUF_T)*VMF_VIDEO_DISPLAY_MIN_QUEUE_SIZE);
	memset(ssm_buf, 0, sizeof(ssm_buffer_t)*VMF_VIDEO_DISPLAY_MIN_QUEUE_SIZE);
	memset(&tSrcConnectInfo, 0, sizeof(VMF_SRC_CONNECT_INFO_T));
	tSrcConnectInfo.dwCodecType = pHostStreamInit->tVEncoder[0].eCodecType;
	//request memory
	//! The video display device uses "triple-buffering mechanism"
	//! You need to prepare three buffers at least.
    if(init_video_display(pHostStreamInit) < 0){
		printf("[%s] init_video_display failed!!\n", __func__);
		return NULL;
    }

    if(pHostStreamInit->dwNnmSource){
        // NnmSource=1: no bind handle; read directly from the external streamer SSM
        ptSsmHandle = SSM_Reader_Init(pHostStreamInit->pszSsmName, 1);
    } else {
        while(!g_dwInitBind){
            // wait g_ptBind init
            usleep(2000);
        }
        ret = VMF_BIND_Request(g_ptBind, pHostStreamInit->dwVocWidth, pHostStreamInit->dwVocHeight, pHostStreamInit->dwVocWidth, 0, &tSrcConnectInfo);
        if(-1 == ret) {
            printf("[%s] VMF_BIND_Request failed!!\n", __func__);
            return NULL;
        }
        printf("[%s] BIND pin='%s' codec=%u srcW=%u srcH=%u srcYStride=%u srcUVStride=%u dataType=%u\n",
            __func__, tSrcConnectInfo.szSrcPin, tSrcConnectInfo.dwCodecType,
            tSrcConnectInfo.dwSrcWidth, tSrcConnectInfo.dwSrcHeight,
            tSrcConnectInfo.dwSrcYStride, tSrcConnectInfo.dwSrcUVStride, tSrcConnectInfo.dwDataType);
        //! init ssm reader
        if (g_dwDrawBoxType && strncmp(tSrcConnectInfo.szSrcPin, "vsrc_ssm_0", strlen("vsrc_ssm_0")) == 0) {
            ptSsmHandle = SSM_Reader_Init(VENC_VSRC_B_PIN, 1);
        } else {
            ptSsmHandle = SSM_Reader_Init(tSrcConnectInfo.szSrcPin, tSrcConnectInfo.bIsSsmShared);
        }
    }

	if (!ptSsmHandle)
		printf("%s() failed, SSM_Reader_Init failed!\n", __func__);

	if(-1 == VMF_VDISP_All_Setting_Reset(g_ptDisplayHandle)) {
		printf("[%s] Some of VDISP effects reset failed!!\n", __func__);
	}
	if(-1 == VMF_VDISP_SetCompress(g_ptDisplayHandle, 0)) {
		printf("[%s] Set compress failed!!\n", __func__);
	}

	//! Currently, the video display device shows "blue screen" with text overlay on the third frame buffer.
	while (true == _blResultRunning) {
        VMF_VSRC_SSM_OUTPUT_INFO_T vsrc_ssm_info;

		pthread_mutex_lock(&pHostStreamInit->tSuspendpMutex);
		if(pHostStreamInit->bSuspend) {
			VMF_VDISP_Stop(g_ptDisplayHandle);
			SSM_Reader_ReturnBuff(ptSsmHandle, &ssm_buf[q_idx]);
			q_idx = 0;
			MsgBroker_SuspendAckMsg();
			pthread_cond_wait(&pHostStreamInit->tSuspendCond, &pHostStreamInit->tSuspendpMutex);
		}
		pthread_mutex_unlock(&pHostStreamInit->tSuspendpMutex);

		if(0 != SSM_Reader_ReturnReceiveBuff(ptSsmHandle, &ssm_buf[q_idx])) {
			printf("%s error \n", __func__);
		} else {
            memset(&vsrc_ssm_info, 0, sizeof(VMF_VSRC_SSM_OUTPUT_INFO_T));
            VMF_VSRC_SSM_GetInfo(ssm_buf[q_idx].buffer, &vsrc_ssm_info);

            if (vsrc_ssm_info.dwOffset[0] != 0 && vsrc_ssm_info.dwOffset[1] != 0) {
                abuf[q_idx].apdwData[0] = ssm_buf[q_idx].buffer + vsrc_ssm_info.dwOffset[0];
                abuf[q_idx].apdwData[1] = ssm_buf[q_idx].buffer + vsrc_ssm_info.dwOffset[1];
                /* NV12 (semi-planar): offset[2]==0 means no separate Cr plane → NULL.
                 * YM12 (planar): offset[2]!=0 → Cr plane pointer. */
                abuf[q_idx].apdwData[2] = vsrc_ssm_info.dwOffset[2]
                    ? (ssm_buf[q_idx].buffer + vsrc_ssm_info.dwOffset[2]) : NULL;
            } else {
                printf("[%s] Error: Invalid SSM offsets! off=(%u,%u,%u)\n", __func__,
                    vsrc_ssm_info.dwOffset[0], vsrc_ssm_info.dwOffset[1], vsrc_ssm_info.dwOffset[2]);
            }

            static int frame_logged = 0;
            static int voc_frame_count = 0;
            voc_frame_count++;
            if (!frame_logged && (vsrc_ssm_info.dwWidth != 0)) {
                frame_logged = 1;
                printf("[%s] VOC SSM type=%u w=%u h=%u yStride=%u ySize=%u uvSize=%u off=(%u,%u,%u)\n",
                    __func__, vsrc_ssm_info.dwType, vsrc_ssm_info.dwWidth, vsrc_ssm_info.dwHeight,
                    vsrc_ssm_info.dwYStride, vsrc_ssm_info.dwYSize, vsrc_ssm_info.dwUVSize,
                    vsrc_ssm_info.dwOffset[0], vsrc_ssm_info.dwOffset[1], vsrc_ssm_info.dwOffset[2]);
                printf("[%s] apdwData[0]=%p [1]=%p [2]=%p (phys)\n",
                    __func__, (void*)abuf[q_idx].apdwData[0],
                    (void*)abuf[q_idx].apdwData[1], (void*)abuf[q_idx].apdwData[2]);
                /* Peek first 16 Y bytes via virtual address */
                unsigned char *py = (unsigned char*)(ssm_buf[q_idx].buffer + VMF_MAX_SSM_HEADER_SIZE);
                printf("[%s] Y[0..15]: %02X %02X %02X %02X %02X %02X %02X %02X "
                       "%02X %02X %02X %02X %02X %02X %02X %02X\n",
                    __func__, py[0],py[1],py[2],py[3],py[4],py[5],py[6],py[7],
                    py[8],py[9],py[10],py[11],py[12],py[13],py[14],py[15]);
                     if (vsrc_ssm_info.dwOffset[1] != 0) {
                          unsigned char *pUV = (unsigned char*)(ssm_buf[q_idx].buffer + vsrc_ssm_info.dwOffset[1]);
                          printf("[%s] UV[0..15]: %02X %02X %02X %02X %02X %02X %02X %02X "
                                    "%02X %02X %02X %02X %02X %02X %02X %02X\n",
                                __func__, pUV[0],pUV[1],pUV[2],pUV[3],pUV[4],pUV[5],pUV[6],pUV[7],
                                pUV[8],pUV[9],pUV[10],pUV[11],pUV[12],pUV[13],pUV[14],pUV[15]);
                          if (vsrc_ssm_info.dwOffset[2] == 0) {
                              /* NV12: even=Cb, odd=Cr */
                              printf("[%s] NV12: Cb=%02X,%02X,%02X,%02X,%02X,%02X,%02X,%02X "
                                     "Cr=%02X,%02X,%02X,%02X,%02X,%02X,%02X,%02X\n",
                                  __func__, pUV[0],pUV[2],pUV[4],pUV[6],pUV[8],pUV[10],pUV[12],pUV[14],
                                  pUV[1],pUV[3],pUV[5],pUV[7],pUV[9],pUV[11],pUV[13],pUV[15]);
                          }
                     }
                     if (vsrc_ssm_info.dwOffset[2] != 0) {
                          unsigned char *pCr = (unsigned char*)(ssm_buf[q_idx].buffer + vsrc_ssm_info.dwOffset[2]);
                          printf("[%s] YM12 Cr[0..15]: %02X %02X %02X %02X %02X %02X %02X %02X "
                                    "%02X %02X %02X %02X %02X %02X %02X %02X\n",
                                __func__, pCr[0],pCr[1],pCr[2],pCr[3],pCr[4],pCr[5],pCr[6],pCr[7],
                                pCr[8],pCr[9],pCr[10],pCr[11],pCr[12],pCr[13],pCr[14],pCr[15]);
                     }
            }
            /* Periodic Cb/Cr average: every 150 frames (~6s) to watch AWB convergence.
             * NV12 (semi-planar, offset[2]==0): UV plane has interleaved Cb0,Cr0,Cb1,Cr1...
             * YM12 (planar, offset[2]!=0): separate Cb and Cr planes. */
            if (voc_frame_count % 150 == 0 && vsrc_ssm_info.dwOffset[1] != 0) {
                unsigned char *pUV = (unsigned char*)(ssm_buf[q_idx].buffer + vsrc_ssm_info.dwOffset[1]);
                unsigned int cb_sum = 0, cr_sum = 0;
                if (vsrc_ssm_info.dwOffset[2] != 0) {
                    /* YM12: separate planes */
                    unsigned char *pCr = (unsigned char*)(ssm_buf[q_idx].buffer + vsrc_ssm_info.dwOffset[2]);
                    for (int ci = 0; ci < 16; ci++) cb_sum += pUV[ci];
                    for (int ci = 0; ci < 16; ci++) cr_sum += pCr[ci];
                } else {
                    /* NV12: Cb at even bytes, Cr at odd bytes */
                    for (int ci = 0; ci < 16; ci += 2) { cb_sum += pUV[ci]; cr_sum += pUV[ci+1]; }
                    cb_sum = cb_sum * 2; cr_sum = cr_sum * 2; /* scale to /16 denominator */
                }
                printf("[%s] frame=%d Cb_avg=%u Cr_avg=%u (neutral=128, pink=Cr>>128)\n",
                    __func__, voc_frame_count, cb_sum/16, cr_sum/16);
            }
            static int vdisp_err_count = 0;
            int vdisp_ret = VMF_VDISP_ProcessOneFrame(g_ptDisplayHandle, &abuf[q_idx], &q_idx);
            if (vdisp_ret != 0 && vdisp_err_count < 5) {
                printf("[%s] VMF_VDISP_ProcessOneFrame ret=%d (err#%d)\n", __func__, vdisp_ret, ++vdisp_err_count);
            }
		}
		usleep(1000);
	}

	for (int i = 0; i < VMF_VIDEO_DISPLAY_MIN_QUEUE_SIZE; ++i)
		SSM_Reader_ReturnBuff(ptSsmHandle, &ssm_buf[i]);

	SSM_Release(ptSsmHandle);
    VMF_VDISP_Release(g_ptDisplayHandle);
	return NULL;
}

void *kdp2_host_stream_image_thread(void *arg)
{
    HOST_STREAM_INIT_OPT_T* pHostStreamInit = (HOST_STREAM_INIT_OPT_T*)arg;

    dbg_log("[%s] starting ..\n", __FUNCTION__);

    struct timeval tGetTime,tGetTime1,tGetTime2;
    float dwLoopUsedTime = 0, fpsTime = 1000000 / pHostStreamInit->fFps;
    int dwCount = 0;
    float spend = 0.0, sleepTime = 0.0;
    kp_inference_header_stamp_t *header_stamp = NULL;
    uint32_t buf_addr = 0;      // contains a inference image or a command
    uint32_t phy_buf_addr = 0;  // contains a inference image or a command
    int buf_size = 0;           // buffer size should bigger than inference image size
    int sts = 0;
    pthread_t task_draw_box_handle;
    //Video source initialization
    unsigned int dwInferenceWidth = pHostStreamInit->tVEncoder[pHostStreamInit->dwInferenceStream].dwWidth;
    unsigned int dwInferenceHeight = pHostStreamInit->tVEncoder[pHostStreamInit->dwInferenceStream].dwHeight;
    ssm_handle_t *ptSsmHandle = NULL;
	ssm_buffer_t ssm_buf;
    const unsigned int getyuv_retry_cnt = 500;
    unsigned int wait_cnt = 0;
    char azReaderSsmName[64];
    char tmpName[30] = {0};

    g_dwDrawOnResize = pHostStreamInit->dwDrawOnResize;
    VMF_SSM_READER_SCHEME eImageBufMode = (VMF_SSM_READER_SCHEME)pHostStreamInit->dwGetImageBufMode;
    if(!pHostStreamInit->dwNnmSource){
        if (init_video_source(pHostStreamInit)) {
            dbg_log("init_video_source failed!!\n");
            goto EXIT_MIPI_IMAGE_THREAD;
        }

        /* initializing the binder associated with the video source */
        if ((g_ptBind = init_bind(g_ptVsrcHandle)) == NULL) {
            dbg_log("init_bind failed!!\n");
            release_video_source(g_ptVsrcHandle);
            goto EXIT_MIPI_IMAGE_THREAD;
        }
        /* Set g_dwDrawBoxType BEFORE g_dwInitBind=1.
         * VOC thread unblocks on g_dwInitBind and immediately checks g_dwDrawBoxType
         * to decide which SSM pin to read from.  If draw box is enabled on stream 0,
         * it must see g_dwDrawBoxType=1 so it reads from VENC_VSRC_B_PIN (overlay
         * output) rather than the raw ISP SSM — fixes intermittent HDMI no-overlay. */
        if (pHostStreamInit->bDrawBoxEnable && pHostStreamInit->dwEncodeStreamCount > 0)
            g_dwDrawBoxType = 1;
        g_dwInitBind = 1;

        if (((dwInferenceWidth == g_tLayout.dwVideoWidth) && (dwInferenceHeight == g_tLayout.dwVideoHeight)) || (pHostStreamInit->bRoiEnable)) {
            g_fWidthRatio = 1.0;
            g_fHeightRatio = 1.0;
        } else {
            calculate_rs_size(dwInferenceWidth, dwInferenceHeight, g_tLayout.dwVideoWidth, g_tLayout.dwVideoHeight, 
                        &g_fWidthRatio, &g_fHeightRatio, pHostStreamInit->tVEncoder[pHostStreamInit->dwInferenceStream].dwKeepRatio);
        }

        for (unsigned int i=0; i < pHostStreamInit->dwEncodeStreamCount; i++) {
            snprintf(tmpName, 30, SRB_DEFAULT_PREFIX"_%d", i+1);

            if (pHostStreamInit->eMemType == SCM) {
                if (init_output_scm(tmpName, pHostStreamInit->tVEncoder[i].dwEncodeBufferSize, &pHostStreamInit->tVEncoder[i].ptSCM)) {
                    printf("ERROR: init_output_scm() fail .\n");
                    goto EXIT_MIPI_IMAGE_THREAD;
                }
                if (pHostStreamInit->bDrawBoxEnable && i == 0) {
                    g_dwDrawBoxType = 1;
                    if ((pHostStreamInit->tVEncoder[i].ptHandle = init_video_encoder_scm(pHostStreamInit->tVEncoder[i], pHostStreamInit->tVEncoder[i].ptSCM, pHostStreamInit->tVEncoder[i].dwKeepRatio, g_ptBind, 2)) == NULL) {
                        printf("ERROR: init_video_encoder_scm() fail .\n");
                        goto EXIT_MIPI_IMAGE_THREAD;
                    }
                    pthread_create(&task_draw_box_handle, NULL, kdp2_host_stream_draw_box, pHostStreamInit);
                    pthread_setname_np(task_draw_box_handle, "inf_drawbox");
                } else {
                    if ((pHostStreamInit->tVEncoder[i].ptHandle = init_video_encoder_scm(pHostStreamInit->tVEncoder[i], pHostStreamInit->tVEncoder[i].ptSCM, pHostStreamInit->tVEncoder[i].dwKeepRatio, g_ptBind, 0)) == NULL) {
                        printf("ERROR: init_video_encoder_scm() fail .\n");
                        goto EXIT_MIPI_IMAGE_THREAD;
                    }
                }
            } else {
                if (init_output_srb(tmpName, pHostStreamInit->tVEncoder[i].dwEncodeBufferAmount, pHostStreamInit->tVEncoder[i].dwEncodeBufferSize, &pHostStreamInit->tVEncoder[i].ptSRB)){
                    printf("ERROR: init_output_srb() fail .\n");
                    goto EXIT_MIPI_IMAGE_THREAD;
                }
                if (pHostStreamInit->bDrawBoxEnable && i == 0) {
                    g_dwDrawBoxType = 1;
                    if ((pHostStreamInit->tVEncoder[i].ptHandle = init_video_encoder(pHostStreamInit->tVEncoder[i], pHostStreamInit->tVEncoder[i].ptSRB, pHostStreamInit->tVEncoder[i].dwKeepRatio, g_ptBind, 2)) == NULL) {
                        printf("ERROR: init_video_encoder() fail .\n");
                        goto EXIT_MIPI_IMAGE_THREAD;
                    }
                    pthread_create(&task_draw_box_handle, NULL, kdp2_host_stream_draw_box, pHostStreamInit);
                    pthread_setname_np(task_draw_box_handle, "inf_drawbox");
                } else {
                    if ((pHostStreamInit->tVEncoder[i].ptHandle = init_video_encoder(pHostStreamInit->tVEncoder[i], pHostStreamInit->tVEncoder[i].ptSRB, pHostStreamInit->tVEncoder[i].dwKeepRatio, g_ptBind, 0)) == NULL) {
                        printf("ERROR: init_video_encoder() fail .\n");
                        goto EXIT_MIPI_IMAGE_THREAD;
                    }
                }
            }
        }

        if (pHostStreamInit->bRoiEnable)
            snprintf(azReaderSsmName, 64, VENC_VSRC_C_PIN);
        else {
            if (pHostStreamInit->dwEncodeStreamCount == 0) {
                // In VOC-only / no-encoder mode, read frames directly from IFP output.
                snprintf(azReaderSsmName, 64, "vsrc_ssm_ifp_0");
            } else if (pHostStreamInit->dwInferenceStream == 0) {
                snprintf(azReaderSsmName, 64, "vsrc_ssm_0");
            } else {
                if (pHostStreamInit->tVEncoder[pHostStreamInit->dwInferenceStream].dwKeepRatio) {
                    snprintf(azReaderSsmName, 64, "vsrc_ssm_0_%d_%d_k", dwInferenceWidth, dwInferenceHeight);
                } else {
                    snprintf(azReaderSsmName, 64, "vsrc_ssm_0_%d_%d", dwInferenceWidth, dwInferenceHeight);
                }
            }
            pHostStreamInit->dwRoiX = 0;
            pHostStreamInit->dwRoiY = 0;
        }

        printf("[%s] Reader SSM: %s\n", __func__, azReaderSsmName);

        gptSsmHandle = ptSsmHandle = SSM_Reader_Init(azReaderSsmName, 1);   //! 1:inter-process , 0:intra-process
    }else{
        if(pHostStreamInit->bDrawBoxEnable){
            pthread_create(&task_draw_box_handle, NULL, kdp2_host_stream_draw_box, pHostStreamInit);
            pthread_setname_np(task_draw_box_handle, "inf_drawbox");
            g_dwDrawBoxType = 1;
        }
        printf("[%s] NnmSource=1: SSM_Reader_Init(%s) ...\n", __func__, pHostStreamInit->pszSsmName);
        gptSsmHandle = ptSsmHandle = SSM_Reader_Init(pHostStreamInit->pszSsmName, 1);   //! 1:inter-process , 0:intra-process
        printf("[%s] NnmSource=1: SSM_Reader_Init returned %p\n", __func__, (void*)ptSsmHandle);
    }
    if (!ptSsmHandle) {
        printf("%s() failed, SSM_Reader_Init failed!\n", __func__);
        goto EXIT_MIPI_IMAGE_THREAD;
    }
    //! wait for video source being ready.
    memset(&ssm_buf, 0, sizeof(ssm_buffer_t));

    while (SSM_Reader_ReturnReceiveNewestBuff(ptSsmHandle, &ssm_buf, eImageBufMode) < 0) {
        usleep(100000); // 0.1 sec wait between retries
        if ((getyuv_retry_cnt < wait_cnt++) || (false == _blRecvRunning)) {
            printf("%s err: get video yuv failed, exit\n", __func__);
            goto EXIT_MIPI_IMAGE_THREAD;
        }
    }

    // run infinitely
    while (true == _blRecvRunning) {
        if (dwCount == 0) {
            gettimeofday(&tGetTime, NULL);
            dwLoopUsedTime = tGetTime.tv_sec * 1000000 + tGetTime.tv_usec;
        }
        gettimeofday(&tGetTime1, NULL);
        // take a free buffer to receive a inf image or a command
        if (true == VMF_NNM_Fifoq_Manager_Get_Fifoq_Allocated()) {
            sts = VMF_NNM_Fifoq_Manager_Image_Get_Free_Buffer(&buf_addr, &phy_buf_addr, &buf_size, -1, _enable_inf_droppable);

            if (KP_FW_FIFOQ_ACCESS_FAILED_125 == sts)
                continue;
            else if (KP_SUCCESS != sts)
                goto EXIT_MIPI_IMAGE_THREAD;
        } else {
            printf("[%s] Error: FIFO queue is not allocated.\n", __FUNCTION__);
            goto EXIT_MIPI_IMAGE_THREAD;
        }

        dbg_log("[%s] free queue --> buf 0x%X size %d\n", __FUNCTION__, buf_addr, buf_size);

        SSM_Reader_ReturnReceiveNewestBuff(ptSsmHandle, &ssm_buf, eImageBufMode);//VMF_SSM_READER_BLOCK / VMF_SSM_READER_NONBLOCK
        VMF_VSRC_SSM_OUTPUT_INFO_T vsrc_ssm_info;
        VMF_VSRC_SSM_GetInfo(ssm_buf.buffer, &vsrc_ssm_info);
        if (!ssm_buf.buffer) {
            printf("[%s] ssm_buf.buffer = %p, EXIT_MIPI_IMAGE_THREAD \n", __func__, ssm_buf.buffer);
            goto EXIT_MIPI_IMAGE_THREAD;
        }

        sts = app_hdr_send_inf_cb(buf_addr, &_blRecvRunning, (void*)pHostStreamInit, ssm_buf.buffer, &vsrc_ssm_info);
        if (KP_SUCCESS != sts) {
            printf("[%s] Error: callback function error (%d)\n", __FUNCTION__, sts);
            goto EXIT_MIPI_IMAGE_THREAD;
        }

        header_stamp = (kp_inference_header_stamp_t *)buf_addr;

        if (header_stamp->magic_type == KDP2_MAGIC_TYPE_INFERENCE) {
            if (header_stamp->total_size > (uint32_t)buf_size) {
                printf("[%s] Error: Inference image size (%d) is bigger than buffer size (%d)\n", __FUNCTION__, header_stamp->total_size, buf_size);
                goto EXIT_MIPI_IMAGE_THREAD;
            }

            dbg_log("[%s] buf 0x%x -- > inference queue\n", __FUNCTION__, buf_addr);
            sts = VMF_NNM_Fifoq_Manager_Image_Enqueue(header_stamp->total_image, header_stamp->image_index, buf_addr, phy_buf_addr, buf_size, 0, false);
            if (KP_FW_FIFOQ_ACCESS_FAILED_125 == sts)
                continue;
            else if (KP_SUCCESS != sts)
                goto EXIT_MIPI_IMAGE_THREAD;
        } else {
            printf("[%s] Error: Invalid magic type %u\n", __FUNCTION__, header_stamp->magic_type);
            sts = VMF_NNM_Fifoq_Manager_Image_Put_Free_Buffer(buf_addr, phy_buf_addr, buf_size, 0);
            if (KP_FW_FIFOQ_ACCESS_FAILED_125 == sts)
                continue;
            else if (KP_SUCCESS != sts)
                goto EXIT_MIPI_IMAGE_THREAD;
        }

        gettimeofday(&tGetTime2, NULL);
        spend = tGetTime2.tv_sec * 1000000 + tGetTime2.tv_usec - tGetTime1.tv_sec * 1000000 - tGetTime1.tv_usec;
        sleepTime = fpsTime - spend;
        if((sleepTime < 0.0) || (sleepTime > 1000000))
            sleepTime = 0.0;

        usleep(sleepTime);

        dwCount++;
        if (dwCount == 10) {
            gettimeofday(&tGetTime, NULL);
            dwLoopUsedTime = tGetTime.tv_sec * 1000000 + tGetTime.tv_usec - dwLoopUsedTime;
            printf("[%s] %f fps !!!\n",__FUNCTION__, 10000000 / dwLoopUsedTime);
            dbg_log("sleepTime = %.3f us, fpsTime = %.3f us\n", sleepTime, fpsTime);
            dwCount = 0;
        }
    }

EXIT_MIPI_IMAGE_THREAD:
    dbg_log("[%s] exit ..\n", __FUNCTION__);

    if (ptSsmHandle) {
        SSM_Reader_ReturnBuff(ptSsmHandle, &ssm_buf);
        SSM_Release(ptSsmHandle);
    }
    if(!pHostStreamInit->dwNnmSource){
        for (unsigned int i=0; i < pHostStreamInit->dwEncodeStreamCount; i++) {
            if (pHostStreamInit->eMemType == SCM)
                release_video_encoder_scm(pHostStreamInit->tVEncoder[i].ptHandle, &pHostStreamInit->tVEncoder[i].ptSCM);
            else
                release_video_encoder(pHostStreamInit->tVEncoder[i].ptHandle, &pHostStreamInit->tVEncoder[i].ptSRB);
        }

        if (g_ptBind)
            release_bind(g_ptBind);

        if (g_ptVsrcHandle)
            release_video_source(g_ptVsrcHandle);
    }

    dbg_log("[%s] exit ..\n", __FUNCTION__);
    return NULL;
}

void *kdp2_host_update_result_thread(void *arg)
{
    HOST_STREAM_INIT_OPT_T* pHostStreamInit = (HOST_STREAM_INIT_OPT_T*)arg;
    uint32_t buf_addr = 0;
    uint32_t phy_buf_addr = 0;
    int buf_size = 0;
    int sts = 0;

    dbg_log("[%s] starting ..\n", __FUNCTION__);
    if (app_hdr_recv_inf_cb == NULL) {
        printf("[%s] recv_inf_cb is NULL \n", __FUNCTION__);
        goto EXIT_UPDATE_RESULT_THREAD;
    }

    g_dwRoiX = pHostStreamInit->dwRoiX;
    g_dwRoiY = pHostStreamInit->dwRoiY;
    g_dwDrawBoxEnable = pHostStreamInit->bDrawBoxEnable;
    g_dwOnlyPerson = pHostStreamInit->dwOnlyPerson;

    while (true == _blResultRunning) {
        // get result data from queue blocking wait
        int ret = VMF_NNM_Fifoq_Manager_Result_Dequeue(&buf_addr, &phy_buf_addr, &buf_size, -1);

        if (KP_FW_FIFOQ_ACCESS_FAILED_125 == ret) {
            continue;
        } else if (KP_SUCCESS != ret) {
            printf("[%s] Error: FIFO queue error %d.\n", __FUNCTION__, sts);
            goto EXIT_UPDATE_RESULT_THREAD;
        }

        //dbg_log("[%s] buf 0x%x len %d -- usb --> host\n", __FUNCTION__, buf_addr, header_stamp->total_size);
        sts = app_hdr_recv_inf_cb(buf_addr, &_blResultRunning); //printf result
        if (KP_SUCCESS != sts) {
            printf("[%s] Error: callback function error (%d)\n", __FUNCTION__, sts);
            goto EXIT_UPDATE_RESULT_THREAD;
        }

        // return free buf back to queue
        ret = VMF_NNM_Fifoq_Manager_Result_Put_Free_Buffer(buf_addr, phy_buf_addr, buf_size, -1);
        if (KP_FW_FIFOQ_ACCESS_FAILED_125 == ret) {
            continue;
        } else if (KP_SUCCESS != ret) {
            goto EXIT_UPDATE_RESULT_THREAD;
        }
    }

EXIT_UPDATE_RESULT_THREAD:
    if (NULL != g_pm_buf) {
		MemBroker_FreeMemory(g_pm_buf);
		g_pm_buf = NULL;
    }

    dbg_log("[%s] exit ..\n", __FUNCTION__);
    return NULL;
}
