/*
 *******************************************************************************
 *  Copyright (c) 2010-2022 VATICS(KNERON) Inc. All rights reserved.
 *
 *  +-----------------------------------------------------------------+
 *  | THIS SOFTWARE IS FURNISHED UNDER A LICENSE AND MAY ONLY BE USED |
 *  | AND COPIED IN ACCORDANCE WITH THE TERMS AND CONDITIONS OF SUCH  |
 *  | A LICENSE AND WITH THE INCLUSION OF THE THIS COPY RIGHT NOTICE. |
 *  | THIS SOFTWARE OR ANY OTHER COPIES OF THIS SOFTWARE MAY NOT BE   |
 *  | PROVIDED OR OTHERWISE MADE AVAILABLE TO ANY OTHER PERSON. THE   |
 *  | OWNERSHIP AND TITLE OF THIS SOFTWARE IS NOT TRANSFERRED.        |
 *  |                                                                 |
 *  | THE INFORMATION IN THIS SOFTWARE IS SUBJECT TO CHANGE WITHOUT   |
 *  | ANY PRIOR NOTICE AND SHOULD NOT BE CONSTRUED AS A COMMITMENT BY |
 *  | VATICS(KNERON) INC.                                             |
 *  +-----------------------------------------------------------------+
 *
 *******************************************************************************
 */
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <signal.h>
#include <fcntl.h>
#include <errno.h>
#include <getopt.h>
#include <unistd.h>
#include <stdarg.h>
#include <sys/stat.h>
#include <pthread.h>
#include <sys/ioctl.h>
#include <sys/time.h>
#include <linux/spi/spidev.h>
#include <MemBroker/mem_broker.h>
#include <MsgBroker/msg_broker.h>
#include <vmf/sync_shared_memory.h>
#include <vmf/video_encoder_output_srb.h>
#include <vmf/video_source.h>
#include <vmf/video_encoder.h>
#include <vmf/video_bind.h>
#include <vmf/resize.h>
#include <comm/frame_info.h>
#include <vmf/ssm_info.h>

#define MODULE_NAME			 "vospi"
#define VENC_OUTPUT_BUF_NUM  3
#define VENC_VSRC_PIN        "vsrc_ssm"						//! VMF_VSRC Output pin
#define WRITER_PIN       	 "vsrc_ssm_tc"
#define VENC_OUTPUT_PIN      "venc_srb_1"					//! VMF_VideoEnc Output pin
#define VENC_TH_OUTPUT_PIN   "venc_srb_2"					//! VMF_VideoEnc Output pin
#define VENC_CMD_FIFO        "/tmp/venc/c0/command.fifo" 	//! communicate with rtsps, vrec, etc.
#define VENC_RESOURCE_DIR    "./Resource/"                  //! directory contains ISP, AE, AWB, AutoScene sub directory
#define VENC_ENCODE_BUF_SIZE (4*1024*1024)	                //! 4*1024*1024

#define VENC_ENCODE_WIDTH   320
#define VENC_ENCODE_HEIGHT  240
#define LEPTON_SPI_FPS      26
#define THERMAL_FPS         9	// about LEPTON_SPI_FPS/3
#define COLOR_STEPS         256
#define PACKET_SIZE         164
#define READ_PACKET_NUM     60
#define VENC_TH_ENCODE_BUF_SIZE (4*VENC_ENCODE_WIDTH*VENC_ENCODE_HEIGHT)	//! 4*Width*Height
//#define DEBUG_WRITE_YUV_FILE 1

#define THERMAL_I2C_DEVICE  "/dev/i2c-0"
#define I2C_TIMEOUT         0x0702
#define I2C_RETRIES         0x0701
#define I2C_RDWR            0x0707
#define THERMAL_DEVICE      "/dev/lepton3.0"

struct i2c_msg
{
	unsigned short addr;
	unsigned short flags;
	unsigned short len;
	unsigned char *buf;
};

struct i2c_rdwr_ioctl_data
{
	struct i2c_msg *msgs;
	int nmsgs;
};

// YUV Palette
const unsigned char SPECTRAL_Y[COLOR_STEPS]= {
92, 93, 94, 95, 96, 97, 98, 99, 100, 101, 102, 103, 103, 104, 105, 106,
107, 108, 109, 110, 111, 112, 113, 114, 114, 115, 117, 119, 120, 122, 124, 126,
128, 130, 131, 133, 135, 137, 139, 141, 142, 144, 146, 148, 150, 152, 153, 155,
157, 159, 161, 163, 164, 166, 167, 168, 170, 171, 173, 174, 176, 177, 178, 180,
181, 183, 184, 186, 187, 188, 190, 191, 193, 194, 195, 197, 198, 200, 201, 202,
203, 204, 206, 207, 208, 209, 210, 212, 213, 214, 215, 216, 218, 219, 220, 221,
222, 223, 225, 226, 227, 228, 229, 230, 231, 232, 232, 233, 234, 234, 235, 236,
236, 237, 238, 238, 239, 240, 241, 241, 242, 243, 243, 244, 245, 245, 246, 247,
247, 246, 245, 244, 243, 242, 241, 240, 239, 238, 237, 236, 235, 234, 233, 232,
231, 230, 229, 229, 228, 227, 226, 225, 224, 223, 221, 220, 219, 217, 216, 215,
213, 212, 211, 209, 208, 207, 205, 204, 203, 201, 200, 198, 197, 196, 194, 193,
192, 190, 189, 187, 186, 184, 182, 181, 179, 177, 175, 174, 172, 170, 168, 167,
165, 163, 161, 160, 158, 156, 154, 153, 151, 149, 148, 146, 144, 143, 141, 140,
139, 137, 136, 134, 133, 132, 130, 129, 127, 126, 125, 123, 122, 120, 119, 118,
116, 115, 113, 112, 111, 109, 108, 105, 103, 101, 99, 97, 95, 93, 91, 89,
86, 84, 82, 80, 78, 76, 74, 72, 70, 67, 65, 63, 61, 59, 57, 55
};

const unsigned char SPECTRAL_U[COLOR_STEPS]= {
166, 166, 166, 166, 166, 166, 166, 167, 167, 167, 167, 167, 167, 167, 167, 167,
167, 167, 167, 167, 168, 168, 168, 168, 168, 168, 167, 166, 164, 163, 161, 159,
158, 156, 155, 153, 152, 150, 148, 147, 145, 144, 142, 141, 139, 137, 136, 134,
133, 131, 130, 128, 127, 126, 126, 125, 124, 123, 122, 121, 121, 120, 119, 118,
117, 116, 116, 115, 114, 113, 112, 111, 111, 110, 109, 108, 107, 106, 106, 105,
104, 103, 102, 101, 100, 99, 98, 97, 96, 95, 94, 93, 92, 91, 91, 90,
89, 88, 87, 86, 85, 84, 83, 84, 84, 84, 85, 85, 86, 86, 87, 87,
88, 88, 89, 89, 90, 90, 91, 91, 91, 92, 92, 93, 93, 94, 94, 95,
95, 94, 93, 93, 92, 92, 91, 90, 90, 89, 89, 88, 87, 87, 86, 86,
85, 84, 84, 83, 82, 82, 81, 81, 80, 79, 79, 79, 79, 79, 79, 78,
78, 78, 78, 78, 78, 77, 77, 77, 77, 77, 77, 76, 76, 76, 76, 76,
76, 75, 75, 75, 76, 76, 76, 77, 77, 77, 78, 78, 78, 78, 79, 79,
79, 80, 80, 80, 81, 81, 81, 82, 82, 82, 83, 83, 83, 84, 85, 86,
87, 88, 90, 91, 92, 93, 94, 95, 96, 97, 98, 99, 100, 101, 102, 103,
104, 105, 106, 107, 108, 109, 110, 111, 112, 113, 114, 115, 116, 117, 118, 119,
119, 120, 121, 122, 123, 124, 125, 126, 127, 128, 129, 129, 130, 131, 132, 133
};

const unsigned char SPECTRAL_V[COLOR_STEPS]= {
128, 126, 124, 122, 120, 118, 116, 115, 113, 111, 109, 107, 105, 103, 101, 99,
98, 96, 94, 92, 90, 88, 86, 84, 83, 81, 80, 80, 80, 80, 80, 80,
81, 81, 81, 81, 81, 81, 81, 82, 82, 82, 82, 82, 82, 82, 83, 83,
83, 83, 83, 83, 84, 85, 86, 87, 88, 89, 90, 91, 92, 92, 93, 94,
95, 96, 97, 98, 99, 100, 101, 102, 103, 103, 104, 105, 106, 107, 108, 109,
109, 110, 111, 112, 113, 113, 114, 115, 116, 117, 117, 118, 119, 120, 121, 121,
122, 123, 124, 125, 125, 126, 127, 127, 127, 128, 128, 128, 128, 128, 129, 129,
129, 129, 129, 130, 130, 130, 130, 130, 131, 131, 131, 131, 131, 132, 132, 132,
133, 133, 134, 134, 135, 136, 136, 137, 138, 138, 139, 140, 140, 141, 142, 142,
143, 144, 144, 145, 146, 146, 147, 148, 148, 149, 150, 151, 152, 153, 154, 155,
155, 156, 157, 158, 159, 160, 161, 162, 163, 164, 165, 166, 167, 168, 169, 169,
170, 171, 172, 173, 174, 175, 176, 177, 178, 179, 180, 181, 182, 183, 184, 185,
186, 187, 188, 189, 190, 191, 192, 193, 194, 195, 196, 197, 198, 198, 198, 198,
198, 199, 199, 199, 199, 199, 199, 199, 199, 200, 200, 200, 200, 200, 200, 200,
200, 201, 201, 201, 201, 201, 201, 201, 201, 201, 201, 201, 201, 201, 201, 201,
201, 201, 201, 201, 201, 201, 201, 200, 200, 200, 200, 200, 200, 200, 200, 200
};

VMF_LAYOUT_T g_tLayout;
static unsigned short *g_adwThermalValue = NULL;

static int g_bTerminate = 0;
static unsigned int g_dwStreamingNum = 0;
static unsigned int g_dwStreamingThNum = 0;
static unsigned int g_dwLeptonType = 3;
static unsigned int g_dwResetSleepType = 1;
static char* g_szAutoSceneConfig = NULL;
static char* g_szSensorConfig = NULL;
static char* g_szSensorConfigFusion = NULL;
VMF_VSRC_HANDLE_T* g_ptVsrcHandle = NULL;
VMF_BIND_CONTEXT_T* g_ptBind = NULL;
VMF_BIND_CONTEXT_T* g_ptThBind = NULL;
VMF_VENC_OUT_SRB_T* g_ptVencOutputSRB = NULL;
VMF_VENC_OUT_SRB_T* g_ptVencThOutputSRB = NULL;
VMF_VENC_HANDLE_T* g_ptVencHandle = NULL;
VMF_VENC_HANDLE_T* g_ptVencThHandle = NULL;
VMF_RS_HANDLE_T* g_ptResizeHandle = NULL;
VMF_VENC_CODEC_TYPE g_eCodecType = VMF_VENC_CODEC_TYPE_H264;
static pthread_mutex_t g_tThemalDataMutex = PTHREAD_MUTEX_INITIALIZER;
static char *g_ptThermalDevice = NULL;
static char *g_ptI2cDevice = NULL;

static int g_bInitialized = 0;
static unsigned int g_dwPreFrameCount = 0;
static unsigned int g_dwCurFrameCount = 0;
static unsigned int g_dwThermalWidth = 80;
static unsigned int g_dwThermalHeight = 60;
static unsigned int g_dwSpiSpeed = 17000000;
static int g_dwSpiFd = -1;

static VMF_H4E_CONFIG_T g_tH4e_config = {
	25,							// dwQp
	4000000,					// dwBitrate
	30,							// dbFps
	60,							// dwGop
	VMF_H4E_PROFILE_HIGH,		// eProfile
	0,							// iSliceQualityStrategy
	VMF_ADMODE_MEET_FPS,		// eAdMode
	0,							// dwMinQp
	0,							// dwMaxQp
	0,							// dwMinFps
	0,                          // dwVirtIFrameInterval
	0                           // dwPIQ
};

static VMF_H5E_CONFIG_T g_tH5e_config = {
	25,							// dwQp
	4000000,					// dwBitrate
	30,							// dwFps
	30,							// dwGop
	0,                          // iSliceQualityStrategy
	0,							// dwMinQp
	0,							// dwMaxQp
	0,                          // Virtual I-frame interval
	0,							// dwPIQ
	VMF_ADMODE_MEET_FPS,		// eAdMode
	0                           // Complex map control in VBR mode
};

static VMF_JE_CONFIG_T g_tJep_config = {
	50,							// dwQp
	0,							// bEnableThumbnail
	0,							// dwThumbnailQp
	0,							// bJfifHdr
	1024*1024,					// dwBitrate
	25
};

void print_msg(const char *fmt, ...)
{
	va_list ap;
	va_start(ap, fmt);
	fprintf(stderr, "[%s] ", MODULE_NAME);
	vfprintf(stderr, fmt, ap);
	va_end(ap);
}

static void release_bind(void)
{
	VMF_BIND_Release(g_ptBind);
}

static void release_thbind(void)
{
	VMF_BIND_Release(g_ptThBind);
}

static void release_video_source(void)
{
	if(g_ptVsrcHandle) {
		VMF_VSRC_Stop(g_ptVsrcHandle);
		VMF_VSRC_Release(g_ptVsrcHandle);
	}
}

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;
	print_msg("[%s]: width:%d, height:%d \n", __func__, width, height);
}

static void setup_spec(VMF_VSRC_SPEC_CONFIG_T* ptSpec, VMF_VENC_CODEC_TYPE eCodecType)
{
	ptSpec->bEnableSpec = 1;
	ptSpec->dwIspMode = VMF_ISP_MODE_FEC_C;

	if (eCodecType == VMF_VENC_CODEC_TYPE_H265){
		ptSpec->tIfpEncSpec.bEncH265 = 1;
	} else if (eCodecType == VMF_VENC_CODEC_TYPE_H264){
		ptSpec->tIfpEncSpec.bEncH264 = 1;
	} else if (eCodecType == VMF_VENC_CODEC_TYPE_MJPG){
		ptSpec->tIfpEncSpec.bEncJPEG = 1;
	}

	if (eCodecType == VMF_VENC_CODEC_TYPE_H265){
		ptSpec->tIspEncSpec.bEncH265 = 1;
	} else if (eCodecType == VMF_VENC_CODEC_TYPE_H264){
		ptSpec->tIspEncSpec.bEncH264 = 1;
	} else if (eCodecType == VMF_VENC_CODEC_TYPE_MJPG){
		ptSpec->tIspEncSpec.bEncJPEG = 1;
	}
}

static void setup_fec(VMF_FEC_P180_CONFIG_T *ptFecConfig)
{
	ptFecConfig->fZoom = 1.5;
	ptFecConfig->fFocalLength = 0.630;
	ptFecConfig->eModeType = VMF_FEC_MODE_PANO_180_TWO_DIRECTION;
	ptFecConfig->fDstOffsetX = 0.0;
	ptFecConfig->fDstOffsetY = 0.0;
	ptFecConfig->fDstXYRatio = 1.00;
	ptFecConfig->fRectCurvature = 0.35;
	ptFecConfig->fRectSlope = 0.35;
	ptFecConfig->eLensType = VMF_FEC_LENS_EQUIDISTANT;
}

static int init_video_source(VMF_VENC_CODEC_TYPE eCodecType)
{
	VMF_VSRC_INITOPT_T tVsrcInitOpt;
	VMF_VSRC_FRONTEND_CONFIG_T tVsrcFrontendConfig;
	VMF_FEC_P180_CONFIG_T tFecP180Config;
	memset(&tVsrcInitOpt, 0, sizeof(VMF_VSRC_INITOPT_T));
	memset(&tVsrcFrontendConfig, 0, sizeof(VMF_VSRC_FRONTEND_CONFIG_T));
	memset(&tFecP180Config, 0, sizeof(VMF_FEC_P180_CONFIG_T));

	setup_fec(&tFecP180Config);
	tVsrcFrontendConfig.tFecInitConfig.ptFecConfig = &tFecP180Config;
	tVsrcFrontendConfig.tFecInitConfig.eCoeffMode = VMF_FEC_COEF_MODE_P180;
	tVsrcFrontendConfig.tFecInitConfig.eFecMethod = VMF_FEC_METHOD_GTR;
	tVsrcFrontendConfig.tFecInitConfig.eGridSize = VMF_FEC_GRID_8X8;
	tVsrcFrontendConfig.apszSensorConfig[0] = g_szSensorConfig;
	tVsrcFrontendConfig.apszSensorConfig[1] = g_szSensorConfigFusion;

	if(tVsrcFrontendConfig.apszSensorConfig[1] != NULL) {
		print_msg("[%s] -d detected, start with Fusion mode.\n", __func__);
		tVsrcFrontendConfig.dwSensorConfigCount = 2;
		tVsrcInitOpt.eAppMode = VMF_VSRC_APP_MODE_FUSION;
	} else {
		print_msg("[%s] Not using -d, start with Normal mode.\n", __func__);
		tVsrcFrontendConfig.dwSensorConfigCount = 1;
		tVsrcInitOpt.eAppMode = VMF_VSRC_APP_MODE_NORMAL;
	}
	tVsrcInitOpt.dwFrontConfigCount = 1;
	tVsrcInitOpt.ptFrontConfig = &tVsrcFrontendConfig;
	tVsrcInitOpt.pszAutoSceneConfig = g_szAutoSceneConfig;
	tVsrcInitOpt.pszOutPinPrefix = VENC_VSRC_PIN;
	tVsrcInitOpt.bShared = 1;
	tVsrcInitOpt.fnInitCallback = vsrc_init_callback;
	tVsrcInitOpt.pszResourceDir = VENC_RESOURCE_DIR;
	setup_spec(&tVsrcInitOpt.tSpecConfig, eCodecType);

	g_ptVsrcHandle =  VMF_VSRC_Init(&tVsrcInitOpt);
	if (!g_ptVsrcHandle) {
		print_msg("[%s] VMF_VSRC_Init failed!\n", __func__);
		return -1;
	}

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

	return 0;
}

static int init_bind(void)
{
	VMF_BIND_INITOPT_T tBindOpt;
	memset(&tBindOpt, 0, sizeof(VMF_BIND_INITOPT_T));

	tBindOpt.dwSrcOutputIndex = 0;
	tBindOpt.ptSrcHandle = g_ptVsrcHandle;
	tBindOpt.pfnQueryFunc = (VMF_BIND_QUERY_FUNC) VMF_VSRC_GetInfo;
	tBindOpt.pfnIspFunc = (VMF_BIND_CONFIG_ISP_FUNC) VMF_VSRC_ConfigISP;

	g_ptBind = VMF_BIND_Init(&tBindOpt);
	if (!g_ptBind){
		print_msg("[%s] VMF_BIND_Init failed!!\n", __func__);
		release_video_source();
		return -1;
	}

	return 0;
}

static int init_thbind(void)
{
	VMF_BIND_INITOPT_T tBindOpt;
	memset(&tBindOpt, 0, sizeof(VMF_BIND_INITOPT_T));

	tBindOpt.dwSrcOutputIndex = 0;
	tBindOpt.ptSrcHandle = g_ptVsrcHandle;
	tBindOpt.pfnQueryFunc = (VMF_BIND_QUERY_FUNC) VMF_VSRC_GetInfo;
	tBindOpt.pfnIspFunc = (VMF_BIND_CONFIG_ISP_FUNC) VMF_VSRC_ConfigISP;

	g_ptThBind = VMF_BIND_Init(&tBindOpt);
	if (!g_ptThBind){
		print_msg("[%s] VMF_BIND_Init failed!!\n", __func__);
		release_video_source();
		return -1;
	}

	return 0;
}

static int init_resize_hanle(void)
{
	VMF_RS_INITOPT_T init_opt;
	VMF_RS_CONFIG_T Config_Opt;
	memset(&init_opt, 0, sizeof(VMF_RS_INITOPT_T));
	memset(&Config_Opt, 0, sizeof(VMF_RS_CONFIG_T));
	init_opt.dwSrcWidth  = g_dwThermalWidth;
	init_opt.dwSrcHeight = g_dwThermalHeight;
	init_opt.dwSrcStride = g_dwThermalWidth;
	init_opt.dwFormatFlag = 0;
	init_opt.pszParamsDir = "./Resource/ISP/0/";
	Config_Opt.dwDstWidth  = VENC_ENCODE_WIDTH;
	Config_Opt.dwDstHeight = VENC_ENCODE_HEIGHT;
	Config_Opt.dwDstStride = VENC_ENCODE_WIDTH;
	Config_Opt.dwSharpness = 0;
	g_ptResizeHandle = VMF_RS_Init(&init_opt,&Config_Opt);

	if (!g_ptResizeHandle)
		return -1;

	return 0;
}

static void release_output_srb(void)
{
	VMF_VENC_OUT_SRB_Release(&g_ptVencOutputSRB);
}

static void release_thoutput_srb(void)
{
	VMF_VENC_OUT_SRB_Release(&g_ptVencThOutputSRB);
}

static void release_video_encoder(void)
{
	if(g_ptVencHandle)
		VMF_VENC_Release(g_ptVencHandle);

	if(g_ptVencOutputSRB)
		VMF_VENC_OUT_SRB_Release(&g_ptVencOutputSRB);
}

static void release_thvideo_encoder(void)
{
	if(g_ptVencThHandle)
		VMF_VENC_Release(g_ptVencThHandle);

	if(g_ptVencThOutputSRB)
		VMF_VENC_OUT_SRB_Release(&g_ptVencThOutputSRB);
}

static int init_output_srb(const char* name, unsigned int buf_number, unsigned int buf_size)
{
	VMF_VENC_OUT_SRB_INITOPT_T tSrbInitOpt;
	memset(&tSrbInitOpt, 0, sizeof(VMF_VENC_OUT_SRB_INITOPT_T));

	tSrbInitOpt.pszSrbName = name;
	tSrbInitOpt.dwSrbNum  = buf_number;
	tSrbInitOpt.dwSrbSize = buf_size;

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

	return 0;
}

static int init_thoutput_srb(const char* name, unsigned int buf_number, unsigned int buf_size)
{
	VMF_VENC_OUT_SRB_INITOPT_T tSrbInitOpt;
	memset(&tSrbInitOpt, 0, sizeof(VMF_VENC_OUT_SRB_INITOPT_T));

	tSrbInitOpt.pszSrbName = name;
	tSrbInitOpt.dwSrbNum  = buf_number;
	tSrbInitOpt.dwSrbSize = buf_size;

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

	return 0;
}

static int init_video_encoder(VMF_VENC_CODEC_TYPE eCodecType, int dwVideoWidth, int dwVideoHeight)
{
	VMF_VENC_CONFIG_T tVencConfig;
	memset(&tVencConfig, 0, sizeof(VMF_VENC_CONFIG_T));

	tVencConfig.dwEncWidth = dwVideoWidth;
	tVencConfig.dwEncHeight = dwVideoHeight;
	if(g_szSensorConfigFusion){
		tVencConfig.dwFps = 20;
	} else {
		tVencConfig.dwFps = 30;
	}

	switch(eCodecType){
		case VMF_VENC_CODEC_TYPE_H264:
			tVencConfig.eCodecType = VMF_VENC_CODEC_TYPE_H264;
			tVencConfig.pCodecConfig = &g_tH4e_config;
		    break;

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

		case VMF_VENC_CODEC_TYPE_MJPG:
			tVencConfig.eCodecType = VMF_VENC_CODEC_TYPE_MJPG;
			tVencConfig.pCodecConfig = &g_tJep_config;
			tVencConfig.dwFps = 10;
			break;

		default:
			print_msg("[%s] Invalid Codec Type\n", __func__);
			return -1;
	}

	VMF_VENC_OUT_SRB_Setup_Config(&tVencConfig, tVencConfig.eCodecType, tVencConfig.pCodecConfig, g_ptVencOutputSRB);
	tVencConfig.fnSrcConnectFunc = (VMF_SRC_CONNECT_FUNC) VMF_BIND_Request;
	tVencConfig.pBind = g_ptBind;

	g_ptVencHandle = VMF_VENC_Init(&tVencConfig);
	if (!g_ptVencHandle) {
		print_msg("[%s] VMF_VENC_Init() failed\n", __func__);
		release_video_source();
		release_bind();
		release_output_srb();
		return -1;
	}

	/* start the video encoder engine */
	VMF_VENC_ProduceStreamHdr(g_ptVencHandle);
	return 0;
}

static int Custom_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, READER_PIN, strlen(READER_PIN));
	memcpy(ptConnectInfo->szSrcPin, WRITER_PIN, strlen(WRITER_PIN));
	return 0;
}

static int init_thvideo_encoder(VMF_VENC_CODEC_TYPE eCodecType, int dwVideoWidth, int dwVideoHeight)
{
	VMF_VENC_CONFIG_T tVencConfig;
	memset(&tVencConfig, 0, sizeof(VMF_VENC_CONFIG_T));

	tVencConfig.dwEncWidth = dwVideoWidth;
	tVencConfig.dwEncHeight = dwVideoHeight;

	switch(eCodecType){
		case VMF_VENC_CODEC_TYPE_H264:
			tVencConfig.eCodecType = VMF_VENC_CODEC_TYPE_H264;
			tVencConfig.pCodecConfig = &g_tH4e_config;
		    break;

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

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

		default:
			print_msg("[%s] Invalid Codec Type\n", __func__);
			return -1;
	}
	tVencConfig.dwFps = THERMAL_FPS;

	VMF_VENC_OUT_SRB_Setup_Config(&tVencConfig, tVencConfig.eCodecType, tVencConfig.pCodecConfig, g_ptVencThOutputSRB);
	tVencConfig.fnSrcConnectFunc = (VMF_SRC_CONNECT_FUNC) Custom_BIND_Request;
	tVencConfig.pBind = g_ptThBind;

	g_ptVencThHandle = VMF_VENC_Init(&tVencConfig);
	if (!g_ptVencThHandle) {
		print_msg("[%s] VMF_VENC_Init() failed\n", __func__);
		release_thbind();
		release_thoutput_srb();
		return -1;
	}

	/* start the video encoder engine */
	VMF_VENC_ProduceStreamHdr(g_ptVencThHandle);
	return 0;
}

static void msg_callback(MsgContext* msg_context, void* user_data)
{
	(void) user_data;
	print_msg("[%s] msg_context->pszHost=%s, msg_context->pszCmd=%s \n",
			__func__, msg_context->pszHost, msg_context->pszCmd);

	while(!g_bInitialized) {
		usleep(1000);
	}
	if (!strcasecmp(msg_context->pszHost, "encoder0")) {
		if (!strcasecmp(msg_context->pszCmd, "start")) {
			if (++g_dwStreamingNum == 1) {
				VMF_VENC_Start(g_ptVencHandle);
			}
		} else if (!strcasecmp(msg_context->pszCmd, "stop")) {
			if (g_dwStreamingNum) {
				if (--g_dwStreamingNum == 0) {
					VMF_VENC_Stop(g_ptVencHandle);
				}
			}
		} else if (!strcasecmp(msg_context->pszCmd, "forceCI")) {
			VMF_VENC_ProduceStreamHdr(g_ptVencHandle);
		} else if (!strcasecmp(msg_context->pszCmd, "forceIntra")) {
			VMF_VENC_SetIntra(g_ptVencHandle);
		}
	}
	else if (!strcasecmp(msg_context->pszHost, "encoder1")) {
		if (!strcasecmp(msg_context->pszCmd, "start")) {
			if (++g_dwStreamingThNum == 1) {
				VMF_VENC_Start(g_ptVencThHandle);
			}
		} else if (!strcasecmp(msg_context->pszCmd, "stop")) {
			if (g_dwStreamingThNum) {
				if (--g_dwStreamingThNum == 0) {
					VMF_VENC_Stop(g_ptVencThHandle);
				}
			}
		} else if (!strcasecmp(msg_context->pszCmd, "forceCI")) {
			VMF_VENC_ProduceStreamHdr(g_ptVencThHandle);
		} else if (!strcasecmp(msg_context->pszCmd, "forceIntra")) {
			VMF_VENC_SetIntra(g_ptVencThHandle);
		}
	}
	else if( !strcasecmp(msg_context->pszHost, SR_MODULE_NAME) ){
		if( !strcasecmp(msg_context->pszCmd, SUSPEND_CMD) ) {
			VMF_VSRC_Suspend(g_ptVsrcHandle); //! suspend
			VMF_VENC_Suspend(g_ptVencHandle);
			VMF_VENC_Suspend(g_ptVencThHandle);
			MsgBroker_SuspendAckMsg();
		}else if( !strcasecmp(msg_context->pszCmd, RESUME_CMD) ) {
			VMF_VSRC_Resume(g_ptVsrcHandle); //! resume
			VMF_VENC_Resume(g_ptVencHandle);
			VMF_VENC_Resume(g_ptVencThHandle);
		}
	}
	if (msg_context->bHasResponse) {
		msg_context->dwDataSize = 0;
	}
}

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

int i2c_write_reg(int slave_2_byte, char *dev, unsigned char *buf, unsigned char slave_address, unsigned int reg_address, int len)
{
	struct i2c_rdwr_ioctl_data work_queue;
	unsigned char *w_buf = NULL;
	int ret;
	int offset;

	w_buf = (unsigned char *)calloc(1, sizeof(unsigned char)*(len+2));
	if(w_buf == NULL){
		print_msg("[%s] Allocate memory fail!\n", __func__);
		return 0;
	}

	if(slave_2_byte == 1)
	{
		w_buf[0] = (reg_address & 0xFF00) >> 8;
		w_buf[1] = (reg_address & 0x00FF) >> 0;
		offset = 2;
	}
	else
	{
		w_buf[0] = (reg_address & 0x00FF) >> 0;
		offset = 1;
	}

	int fd = open(dev, O_RDWR);
	if (fd < 0)
	{
		print_msg("[%s] Error on opening the device file\n", __func__);
		if(w_buf){
			free(w_buf);
			w_buf = NULL;
		}
		return 0;
	}

	work_queue.nmsgs = 1;
	work_queue.msgs = (struct i2c_msg*)malloc(work_queue.nmsgs *sizeof(struct i2c_msg));
	if (!work_queue.msgs)
	{
		print_msg("[%s] Memory alloc error\n", __func__);
		close(fd);
		if(w_buf){
			free(w_buf);
			w_buf = NULL;
		}
		return 0;
	}

	ioctl(fd, I2C_TIMEOUT, 2);
	ioctl(fd, I2C_RETRIES, 1);

	(work_queue.msgs[0]).len = offset + len;
	(work_queue.msgs[0]).addr = slave_address;
	(work_queue.msgs[0]).buf = w_buf;

	memcpy(w_buf + offset, buf, len);
	ret = ioctl(fd, I2C_RDWR, (unsigned long) &work_queue);
	if (ret < 0)
	{
		print_msg("[%s] Error during I2C_RDWR ioctl with error code: %d\n", __func__, ret);
		close(fd);
		if(w_buf){
			free(w_buf);
			w_buf = NULL;
		}
		if(work_queue.msgs){
			free(work_queue.msgs);
			work_queue.msgs = NULL;
		}
		return 0;
	}
	else
	{
		//printf("write salve:%02x reg:%02x\n", slave_address, reg_address);
		close(fd);
		if(w_buf){
			free(w_buf);
			w_buf = NULL;
		}
		if(work_queue.msgs){
			free(work_queue.msgs);
			work_queue.msgs = NULL;
		}
		return len;
	}
}

void lepton_reboot_via_i2c()
{
	unsigned char buf[2];
	int slave_2_byte = 1;
	char *dev = strdup(g_ptI2cDevice);
	unsigned char slave_address = 0x2a;
	unsigned int reg_address;
	int value;
	int len = 2;

	//i2c /dev/i2c-0 2a 1 0008 0
	reg_address = 0x0008;
	value = 0x0000;
	buf[1] = (value & 0x00ff) >> 0;
	buf[0] = (value & 0xff00) >> 8;
	i2c_write_reg(slave_2_byte, dev, buf, slave_address, reg_address, len);

	//i2c /dev/i2c-0 2a 1 0004 4842
	reg_address = 0x0004;
	value = 0x4842;
	buf[1] = (value & 0x00ff) >> 0;
	buf[0] = (value & 0xff00) >> 8;
	i2c_write_reg(slave_2_byte, dev, buf, slave_address, reg_address, len);

	if(dev){
		free(dev);
		dev = NULL;
	}
}

int SpiOpenPort( void )
{
	int status_value = -1;
	//int *spi_cs_fd;
	unsigned char spi_mode = SPI_MODE_3;
	unsigned char spi_bitsPerWord = 8;

	//----- SET SPI MODE -----
	//SPI_MODE_0 (0,0)  CPOL=0 (Clock Idle low level), CPHA=0 (SDO transmit/change edge active to idle)
	//SPI_MODE_1 (0,1)  CPOL=0 (Clock Idle low level), CPHA=1 (SDO transmit/change edge idle to active)
	//SPI_MODE_2 (1,0)  CPOL=1 (Clock Idle high level), CPHA=0 (SDO transmit/change edge active to idle)
	//SPI_MODE_3 (1,1)  CPOL=1 (Clock Idle high level), CPHA=1 (SDO transmit/change edge idle to active)
	spi_mode = SPI_MODE_3;

	//----- SET BITS PER WORD -----
	spi_bitsPerWord = 8;

	//----- SET SPI BUS SPEED -----
	//g_dwSpiSpeed = 17000000;				//1000000 = 1MHz (1uS per bit)

	g_dwSpiFd = open(g_ptThermalDevice, O_RDWR);
	if (g_dwSpiFd < 0)
	{
		printf("Error - Could not open SPI device");
		return -1;
	}

	status_value = ioctl(g_dwSpiFd, SPI_IOC_WR_MODE, &spi_mode);
	if(status_value < 0)
	{
		printf("Could not set SPIMode (WR)...ioctl fail");
		return -1;
	}

	status_value = ioctl(g_dwSpiFd, SPI_IOC_RD_MODE, &spi_mode);
	if(status_value < 0)
	{
		printf("Could not set SPIMode (RD)...ioctl fail");
		return -1;
	}

	status_value = ioctl(g_dwSpiFd, SPI_IOC_WR_BITS_PER_WORD, &spi_bitsPerWord);
	if(status_value < 0)
	{
		printf("Could not set SPI bitsPerWord (WR)...ioctl fail");
		return -1;
	}

	status_value = ioctl(g_dwSpiFd, SPI_IOC_RD_BITS_PER_WORD, &spi_bitsPerWord);
	if(status_value < 0)
	{
		printf("Could not set SPI bitsPerWord(RD)...ioctl fail");
		return -1;
	}

	status_value = ioctl(g_dwSpiFd, SPI_IOC_WR_MAX_SPEED_HZ, &g_dwSpiSpeed);
	if(status_value < 0)
	{
		printf("Could not set SPI speed (WR)...ioctl fail");
		return -1;
	}

	status_value = ioctl(g_dwSpiFd, SPI_IOC_RD_MAX_SPEED_HZ, &g_dwSpiSpeed);
	if(status_value < 0)
	{
		printf("Could not set SPI speed (RD)...ioctl fail");
		return -1;
	}
	printf("spi_mode %d, SpiSpeed is %d, spi_bitsPerWord %d\n", spi_mode, g_dwSpiSpeed, spi_bitsPerWord);
	return(status_value);
}

int SpiClosePort( void )
{
	int status_value = -1;
	status_value = close(g_dwSpiFd);
	if(status_value < 0) {
		printf("Error - Could not close SPI device");
		return -1;
	}
	return(status_value);
}

void *vatics_lepton_thread_lptdrv(void *arg __attribute__((unused)))
{
	int fd = -1;
	unsigned int i = 0, j = 0, h = 0;
	unsigned int cur_frame_count = 0;
	unsigned int fps_frame_count = 0;
	unsigned int segment_id = 1;
	unsigned int pre_segment_id = 1;
	unsigned int az_segment[4] = {0, 0, 0, 0};
	unsigned int segment_shift = 0;
	int good_packet = 0;
	int fail_frame = 0;
	int reset_count = 0;
	int read_return = 0;
	struct timeval now, prev;
	struct timeval reset_now, reset_prev;
	struct timeval timeout;
	fd_set set;
	unsigned char  achSpiBuff[PACKET_SIZE*READ_PACKET_NUM];
	unsigned int dwGetAllSeg = 1;
	unsigned dwDiffTime = 0;

	memset(achSpiBuff, 0, sizeof(unsigned char) * PACKET_SIZE * READ_PACKET_NUM);

	gettimeofday(&prev, NULL);
	gettimeofday(&reset_prev, NULL);

	fd = open(g_ptThermalDevice, O_RDWR);
	if (fd < 0){
		print_msg("[%s] can't open device %s", __func__, g_ptThermalDevice);
		return NULL;
	}

	FD_ZERO(&set); /* clear the set */
	FD_SET(fd, &set); /* add our file descriptor to the set */

	timeout.tv_sec = 1;
	timeout.tv_usec = 0;

	while(g_bTerminate == 0)
	{
		read_return = select(fd + 1, &set, NULL, NULL, &timeout);
		if(read_return == -1){
			print_msg("select error\n"); // an error accured
			usleep(3000);
			continue;
		} else if(read_return == 0) {
			print_msg("read timeout\n"); // a timeout occured
			continue;
		} else {
			read_return = read(fd, achSpiBuff, 0); // there was data to read
		}

		if(read_return != (int)(PACKET_SIZE*READ_PACKET_NUM) ){
			print_msg("[%s] read buffer size Fail, ret: %d\n", __func__, read_return);
			usleep(3000);
			continue;
		}

		if(g_dwLeptonType != 3){
			if(cur_frame_count % 3 != 0){
				cur_frame_count++;
				usleep(3000);
				continue;
			}
		}

		good_packet = 0;
		for(i = 0 ; i < READ_PACKET_NUM ; i++)
		{
			if(achSpiBuff[i*PACKET_SIZE+1] == i)
			{
				good_packet++;
			} else {
				break;
			}
		}

		if (good_packet == READ_PACKET_NUM)
		{
			cur_frame_count++;
			fail_frame = 0;
			reset_count = 0;
			segment_id = (achSpiBuff[20*PACKET_SIZE]&0xF0)>>4;
		}
		else
		{
			fail_frame++;
			//if(fail_frame >= 750){
			if(fail_frame >= 50){
				// sometimes the device is always send fail frame, and it can't recover from calling reset
				// need to ask lepton how to reset exactly or make reset from outer circuit
				//usleep(750000);
				usleep(200000);
				fail_frame = 0;
				reset_count++;
				print_msg("reset_count: %d\n", reset_count);
				if(reset_count >= 5){
					gettimeofday(&reset_now, NULL);
					print_msg("[%s] Reset lepton via i2c, Period: %d s !!!\n", __func__, reset_now.tv_sec-reset_prev.tv_sec);
					reset_count = 0;
					lepton_reboot_via_i2c();
					if(g_dwResetSleepType){
						// sleep 4 sec
						print_msg("Sleep 4 sec\n");
						for(int k = 0 ; k < 20 ; k++){
							g_dwCurFrameCount++;
							usleep(200000);
						}
					} else {
						print_msg("Sleep 750 ms\n");
						usleep(750000);
					}
					reset_prev = reset_now;
				}
			//} else {
			//	if(fail_frame % 10 == 0) {
			//		print_msg("Fail Frame: %d\n", fail_frame);
			//	}
			}
			continue;
		}

		if(g_dwLeptonType == 3){
			switch(segment_id)
			{
				case 1:
					az_segment[0] = 1;
					pre_segment_id = segment_id;
					break;
				case 2:
				case 3:
				case 4:
					if(pre_segment_id == segment_id-1){
						az_segment[segment_id-1] = 1;
						pre_segment_id = segment_id;
					}
					break;
				default:
					usleep(3000);
					continue;
			}

			h = 0;
			segment_shift = (segment_id-1)*( (g_dwThermalWidth * g_dwThermalHeight) >> 2);
			pthread_mutex_lock(&g_tThemalDataMutex);
			for(j = 0 ; j < READ_PACKET_NUM ; j++) {
				for(i = 4 ; i < PACKET_SIZE ; i = i + 2 ){
					g_adwThermalValue[h+segment_shift] = (unsigned short)(achSpiBuff[(j*PACKET_SIZE)+i] << 8) + achSpiBuff[(j*PACKET_SIZE)+i+1];
					h++;
				}
			}
			pthread_mutex_unlock(&g_tThemalDataMutex);

			dwGetAllSeg = 1;
			for(int j = 0; j < 4 ; j++){
				if(az_segment[j] == 0){
					dwGetAllSeg = 0;
					break;
				}
			}

			if(dwGetAllSeg){
				memset(&az_segment, 0, sizeof(unsigned int)*4);
				if(g_dwCurFrameCount % 100 == 99){
					gettimeofday(&now, NULL);
					dwDiffTime = (now.tv_sec * 1000000 + now.tv_usec) - (prev.tv_sec * 1000000 + prev.tv_usec);
					print_msg("Segment numbers: %d, Thermal fps:\t%.2f\n", cur_frame_count - fps_frame_count, (float)1000/((float)dwDiffTime/100000) );
					fps_frame_count = cur_frame_count;
					prev = now;
				}
				g_dwCurFrameCount++;
			}
		} else {
			if(g_dwCurFrameCount % 100 == 99){
				gettimeofday(&now, NULL);
				dwDiffTime = (now.tv_sec * 1000000 + now.tv_usec) - (prev.tv_sec * 1000000 + prev.tv_usec);
				print_msg("frames: %d, Thermal fps:\t%.2f\n", cur_frame_count - fps_frame_count, (float)1000/((float)dwDiffTime/100000) );

				fps_frame_count = cur_frame_count;
				prev = now;
			}

			h = 0;
			pthread_mutex_lock(&g_tThemalDataMutex);
			for(j = 0 ; j < g_dwThermalHeight ; j++) {
				for(i = 4 ; i < PACKET_SIZE ; i = i + 2 ){
					g_adwThermalValue[h] = (unsigned short)(achSpiBuff[(j*PACKET_SIZE)+i] << 8) + achSpiBuff[(j*PACKET_SIZE)+i+1];
					h++;
				}
			}
			pthread_mutex_unlock(&g_tThemalDataMutex);
			g_dwCurFrameCount++;
		}
	}
	close(fd);

	return NULL;
}

int lepton_Init( void ){
	int ret = 0;

	ret = SpiOpenPort();
	if (ret == -1) {
		printf("Open SPI Port Error\n");
		return -1;
	}

	return 0;
}

void * vatics_lepton_thread_spidev(void *arg __attribute__((unused)))
{
	unsigned int i = 0, j = 0, h = 0;
	unsigned int iNFStartCopy = 0, iNFCopySize = 0;
	int iNFStartPKID = -99;
	int iNFCurPKID = 0;
	unsigned int cur_frame_count = 0;
	unsigned int fps_frame_count = 0;
	struct timeval now = {0 , 0}, prev = {0 , 0};
	unsigned int segment_id = 1;
	unsigned int pre_segment_id = 1;
	unsigned int az_segment[4] = {0, 0, 0, 0};
	unsigned int segment_shift = 0;
	unsigned int dwGetAllSeg = 1;
	unsigned dwDiffTime = 0;
	unsigned char  achSpiBuffTemp[PACKET_SIZE*READ_PACKET_NUM];
	unsigned char  achSpiBuffCurr[PACKET_SIZE*READ_PACKET_NUM];
	unsigned char  achSpiBuffNext[PACKET_SIZE*READ_PACKET_NUM];

	if(lepton_Init() != 0){
		print_msg("[%s] Err: lepton init ERROR\n", __func__);
		return NULL;
	}

	while(g_bTerminate == 0)
	{
		//read data packets from lepton over SPI
		int resets = 0;
		h = 0;
		int isGetAllPacket = 0;
		int iStartPKID = -99;
		int iCurPKID = 0;
		unsigned int iStartCopy = 0, iCopySize = 0;

		memset(achSpiBuffTemp, 0, sizeof(unsigned char) * PACKET_SIZE * READ_PACKET_NUM);
		memset(achSpiBuffCurr, 0, sizeof(unsigned char) * PACKET_SIZE * READ_PACKET_NUM);

		if(iNFStartCopy > 0){
			// copy data of next frame to cur frame
			iStartPKID = iNFCopySize;
			iCurPKID = iNFCopySize - 1;
			memcpy(achSpiBuffCurr, achSpiBuffNext, PACKET_SIZE*(iNFCopySize));
		}
		memset(achSpiBuffNext, 0, sizeof(unsigned char) * PACKET_SIZE * READ_PACKET_NUM);

		iNFStartCopy = 0, iNFCopySize = 0;
		iNFStartPKID = -99;
		iNFCurPKID = 0;

		while (!isGetAllPacket) {
			int iGetDiscard = 0;
			int iNFGetDiscard = 0;
			int iGetError = 0;
			unsigned int chkidx = 0;

			int ret = read(g_dwSpiFd, achSpiBuffTemp, sizeof(unsigned char)*PACKET_SIZE * READ_PACKET_NUM);
			if(ret != PACKET_SIZE * READ_PACKET_NUM ){
				printf("SPI Read Error, ret:%d\n", ret);
				break;
			}

			iStartCopy = 0, iCopySize = 0;
			for(chkidx = 0; chkidx < READ_PACKET_NUM ; chkidx++){
				unsigned char *pchPacketID = achSpiBuffTemp + chkidx * PACKET_SIZE;
				int packetID = (int)pchPacketID[1];
				if(isGetAllPacket == 1){
					// Get Next frame
					if (packetID == 0 ) {
						// id = 0, First packet
						iNFStartCopy = chkidx;
						iNFStartPKID = (0-chkidx);
						iNFCurPKID = 0;
						iNFCopySize = 1;
					} else if (packetID == iNFCurPKID + 1) {
						// id = 1 ~ 59 packet, must follow the sequence
						iNFCopySize++;
						if(packetID != READ_PACKET_NUM - 1){
							iNFCurPKID++;
						}
					} else {
						if( ((pchPacketID[0] & 0x0F)== 0x0F) && ((pchPacketID[1] & 0xF0)== 0xF0) ){
							// buf[0][1] = xFFx, discard packet
							iNFGetDiscard++;
						} else {
							// not follow the id sequence, restart get the first packet and get error, jump the loop
							iNFCurPKID = 0;
							iNFCopySize = 0;
							break;
						}
					}
				} else {
					// Get First frame
					if (packetID == 0 ) {
						// id = 0, First packet
						iStartCopy = chkidx;
						iStartPKID = (0-chkidx);
						iCurPKID = 0;
						iCopySize = 1;
					} else if (packetID == iCurPKID + 1) {
						// id = 1 ~ 59 packet, must follow the sequence
						if(packetID == READ_PACKET_NUM - 1){
							// id = 59 packet, get all packet(one frame)
							if(isGetAllPacket == 0){
								isGetAllPacket = 1;
								iCopySize++;
							}
						} else {
							iCopySize++;
							iCurPKID++;
						}
					} else {
						if( ((pchPacketID[0] & 0x0F)== 0x0F) && ((pchPacketID[1] & 0xF0)== 0xF0) ){
							// buf[0][1] = xFFx, discard packet
							iGetDiscard++;
						} else {
							// not follow the id sequence, restart get the first packet and get error, jump the loop
							iGetError = 1;
							iCurPKID = 0;
							iStartPKID = -99;
							break;
						}
					}
				}
			}

			if(iGetDiscard == READ_PACKET_NUM || iGetError){
				// N packets are all discard packets, sleep
				resets += 1;
				usleep(1000);

				if(resets % 50 == 0){
					printf("[%s:%u] iGetDiscard: %u !!!\n", __func__, __LINE__, resets);
					usleep(200000);
				}

				//Note: we've selected 750 resets as an arbitrary limit, since there should never be 750 "null" packets between two valid transmissions at the current poll rate
				//By polling faster, developers may easily exceed this count, and the down period between frames may then be flagged as a loss of sync
				if(resets >= 750) {
					lepton_reboot_via_i2c();
					//usleep(750000);
					usleep(3000000);
					printf("[%s:%u] Reset !!!\n", __func__, __LINE__);
					resets = 0;
				}
			} else {
				// get data packets, copy to result
				if (iStartPKID < 0){
					iStartPKID = 0;
				}

				if(iStartPKID + iCopySize <= READ_PACKET_NUM){
					// id must on 0 ~ 59
					memcpy(achSpiBuffCurr+(iStartPKID*PACKET_SIZE), achSpiBuffTemp+(iStartCopy*PACKET_SIZE), PACKET_SIZE*(iCopySize));
					iStartPKID = iStartPKID + iCopySize;
				}
				if(iNFCopySize > 0){
					if (iNFStartPKID < 0){
						iNFStartPKID = 0;
					}
					memcpy(achSpiBuffNext+(iNFStartPKID*PACKET_SIZE), achSpiBuffTemp+(iNFStartCopy*PACKET_SIZE), PACKET_SIZE*(iNFCopySize));
					iNFStartPKID = iNFStartPKID + iNFCopySize;
				}
			}
		}

		cur_frame_count++;

		if(g_dwLeptonType == 3){
			// Lepton 3.5
			segment_id = (achSpiBuffCurr[20*PACKET_SIZE]&0xF0)>>4;
			switch(segment_id)
			{
				case 1:
					az_segment[0] = 1;
					pre_segment_id = segment_id;
					break;
				case 2:
				case 3:
				case 4:
					if(pre_segment_id == segment_id-1){
						az_segment[segment_id-1] = 1;
						pre_segment_id = segment_id;
					}
					if(segment_id == 4)
						usleep(2000);
					break;
				default:
					usleep(1000);
					continue;
			}

			h = 0;
			segment_shift = (segment_id-1)*( (g_dwThermalWidth * g_dwThermalHeight) >> 2);
			pthread_mutex_lock(&g_tThemalDataMutex);
			for(j = 0 ; j < READ_PACKET_NUM ; j++) {
				for(i = 4 ; i < PACKET_SIZE ; i = i + 2 ){
					g_adwThermalValue[h+segment_shift] = (unsigned short)(achSpiBuffCurr[(j*PACKET_SIZE)+i] << 8) + achSpiBuffCurr[(j*PACKET_SIZE)+i+1];
					h++;
				}
			}
			pthread_mutex_unlock(&g_tThemalDataMutex);

			dwGetAllSeg = 1;
			for(int j = 0; j < 4 ; j++){
				if(az_segment[j] == 0){
					dwGetAllSeg = 0;
					break;
				}
			}

			if(dwGetAllSeg){
				memset(&az_segment, 0, sizeof(unsigned int)*4);
				if(g_dwCurFrameCount % 100 == 99){
					gettimeofday(&now, NULL);
					if(prev.tv_sec == 0 && prev.tv_usec == 0){
						prev = now;
					} else {
						dwDiffTime = (now.tv_sec * 1000000 + now.tv_usec) - (prev.tv_sec * 1000000 + prev.tv_usec);
						print_msg("segments: %d, Thermal fps:\t%.2f\n", cur_frame_count - fps_frame_count, (float)1000/((float)dwDiffTime/100000) );

						fps_frame_count = cur_frame_count;
						prev = now;
					}
				}
				g_dwCurFrameCount++;
			}
		} else {
			// Lepton 2.5
			if(cur_frame_count % 3 != 0){
				usleep(3000);
				continue;
			}

			if(g_dwCurFrameCount % 100 == 99){
				gettimeofday(&now, NULL);
				if(prev.tv_sec == 0 && prev.tv_usec == 0){
					prev = now;
				} else {
					dwDiffTime = (now.tv_sec * 1000000 + now.tv_usec) - (prev.tv_sec * 1000000 + prev.tv_usec);
					print_msg("CurFrames: %d, Thermal fps:\t%.2f\n", g_dwCurFrameCount, (float)1000/((float)dwDiffTime/100000) );

					fps_frame_count = cur_frame_count;
					prev = now;
				}
			}

			h = 0;
			pthread_mutex_lock(&g_tThemalDataMutex);
			for(j = 0 ; j < g_dwThermalHeight ; j++) {
				for(i = 4 ; i < PACKET_SIZE ; i = i + 2 ){
					g_adwThermalValue[h] = (unsigned short)(achSpiBuffCurr[(j*PACKET_SIZE)+i] << 8) + achSpiBuffCurr[(j*PACKET_SIZE)+i+1];
					h++;
				}
			}
			pthread_mutex_unlock(&g_tThemalDataMutex);
			g_dwCurFrameCount++;
		}
	}

	if(SpiClosePort() != 0){
		print_msg("[%s] Err: Cannot close SPI device.\n", __func__);
	}
	printf("[%s] exit thread\n", __func__);

	return NULL;
}

void *thermal_loop (void *arg __attribute__((unused)))
{
	int iMinValue = -1, iMaxValue = -1;
	float fValueStep = 0;
	struct timeval tNowFrameTime, tPreFrameTime = {0, 0};
	unsigned int dwSize = VMF_32_ALIGN(VENC_ENCODE_WIDTH)*VMF_16_ALIGN(VENC_ENCODE_HEIGHT);
	//! init writer
	SSM_WRITER_INIT_OPTION_T tSsmWriterInit;
	SSM_HANDLE_T *ptSsmWriterHandle = NULL;
	SSM_BUFFER_T tOutWriterSsmBuffer;
	VMF_VIDEO_BUF_T tRsFrameInfo;
	VMF_VIDEO_BUF_T	tRsOutbuf;
	VMF_VSRC_SSM_OUTPUT_INFO_T tSsmWriterOutInfo;
	static unsigned char* pBrsRsFrame = NULL;
	static unsigned char* pOutRsFrame = NULL;
	int iDiffTime = 0, iSleepTime = 0;
	struct timespec tFrameTime;
	unsigned short *ptTermalData = NULL;

#ifdef DEBUG_WRITE_YUV_FILE
	unsigned int iCountTmp = 0;
#endif

	memset(&tOutWriterSsmBuffer, 0, sizeof(SSM_BUFFER_T));
	memset(&tSsmWriterInit, 0, sizeof(SSM_WRITER_INIT_OPTION_T));
	memset(&tRsFrameInfo, 0, sizeof(VMF_VIDEO_BUF_T));
	memset(&tRsOutbuf, 0, sizeof(VMF_VIDEO_BUF_T));

	tSsmWriterOutInfo.dwYStride = VMF_32_ALIGN(VENC_ENCODE_WIDTH);
	tSsmWriterOutInfo.dwYSize = tSsmWriterOutInfo.dwYStride * VENC_ENCODE_HEIGHT;
	tSsmWriterOutInfo.dwUVSize = tSsmWriterOutInfo.dwYSize >> 2;
	tSsmWriterOutInfo.dwOffset[0] = VMF_MAX_SSM_HEADER_SIZE;
	tSsmWriterOutInfo.dwOffset[1] = tSsmWriterOutInfo.dwOffset[0] + tSsmWriterOutInfo.dwYSize;
	tSsmWriterOutInfo.dwOffset[2] = tSsmWriterOutInfo.dwOffset[1] + tSsmWriterOutInfo.dwUVSize;
	tSsmWriterOutInfo.dwWidth = VMF_32_ALIGN(VENC_ENCODE_WIDTH);
	tSsmWriterOutInfo.dwHeight = VMF_16_ALIGN(VENC_ENCODE_HEIGHT);

	tSsmWriterInit.name      = WRITER_PIN;
	tSsmWriterInit.buf_size  = ((dwSize*3)>>1) + VMF_MAX_SSM_HEADER_SIZE;
	tSsmWriterInit.alignment = VMF_ALIGN_TYPE_DEFAULT;
	tSsmWriterInit.pshared   = 1;
	tSsmWriterInit.pUserData = &tSsmWriterOutInfo;
	tSsmWriterInit.fp_setup_buffer = ssm_clear_header;

	ptSsmWriterHandle = SSM_Writer_Init(&tSsmWriterInit);
	if (!ptSsmWriterHandle) {
		print_msg("%s() failed, SSM_Writer_Init failed!\n", __func__);
		goto RELEASE;
	}
	SSM_Writer_SendGetBuff(ptSsmWriterHandle, &tOutWriterSsmBuffer);

	pBrsRsFrame = (unsigned char *)MemBroker_GetMemory(g_dwThermalWidth * g_dwThermalHeight * 3 >> 1, VMF_ALIGN_TYPE_DEFAULT);
	if (!pBrsRsFrame) {
		print_msg("[%s] Allocate resize output frame buffer fail !!\n",__func__);
		goto RELEASE;
	}
//	memset(pBrsRsFrame, 0, sizeof(unsigned char)*g_dwThermalWidth * g_dwThermalHeight * 3 >> 1);

	pOutRsFrame = (unsigned char *)MemBroker_GetMemory(VENC_ENCODE_WIDTH * VENC_ENCODE_HEIGHT * 3 >> 1, VMF_ALIGN_TYPE_DEFAULT);
	if (!pOutRsFrame) {
		print_msg("[%s] Allocate resize output frame buffer fail !!\n",__func__);
		goto RELEASE;
	}
//	memset(pOutRsFrame, 0, sizeof(unsigned char)*VENC_ENCODE_WIDTH * VENC_ENCODE_HEIGHT * 3 >> 1);

	while(g_bTerminate == 0){
		unsigned short Y, U, V;
		int iOffset = 0;
		unsigned int i = 0;
		unsigned int iBrsYOffset = 0; //VMF_MAX_SSM_HEADER_SIZE;
		unsigned int iBrsUOffset = iBrsYOffset + (g_dwThermalWidth*g_dwThermalHeight);
		unsigned int iBrsVOffset = iBrsUOffset + (iBrsUOffset >> 2);

		if( g_dwPreFrameCount != g_dwCurFrameCount){
			ptTermalData = g_adwThermalValue;
			g_dwPreFrameCount = g_dwCurFrameCount;
		} else {
			usleep(3000);
			continue;
		}

		memset(pBrsRsFrame, 0, sizeof(unsigned char)*g_dwThermalWidth * g_dwThermalHeight * 3 >> 1);
		iMinValue = ptTermalData[0];
		iMaxValue = ptTermalData[0];
		for(unsigned int i = 0 ; i < g_dwThermalWidth*g_dwThermalHeight ; i++){
			if(ptTermalData[i] < iMinValue && ptTermalData[i] != 0 ){
				iMinValue = ptTermalData[i];
			}
			if(ptTermalData[i] > iMaxValue){
				iMaxValue = ptTermalData[i];
			}
		}

		if( (iMaxValue - iMinValue) > COLOR_STEPS){
			fValueStep = (float)(iMaxValue - iMinValue) / COLOR_STEPS;
		} else {
			fValueStep = 1;
		}

		tSsmWriterOutInfo.dwYStride = VMF_32_ALIGN(VENC_ENCODE_WIDTH);
		tSsmWriterOutInfo.dwYSize = tSsmWriterOutInfo.dwYStride * VENC_ENCODE_HEIGHT;
		tSsmWriterOutInfo.dwUVSize = tSsmWriterOutInfo.dwYSize >> 2;
		tSsmWriterOutInfo.dwOffset[0] = VMF_MAX_SSM_HEADER_SIZE;
		tSsmWriterOutInfo.dwOffset[1] = tSsmWriterOutInfo.dwOffset[0] + tSsmWriterOutInfo.dwYSize;
		tSsmWriterOutInfo.dwOffset[2] = tSsmWriterOutInfo.dwOffset[1] + tSsmWriterOutInfo.dwUVSize;

		i = 0;
		for(unsigned int h = 0 ; h < g_dwThermalHeight ; h++){
			for(unsigned int w = 0 ; w < g_dwThermalWidth ; w++){
				int iDiff = 0;
				iDiff = (ptTermalData[i] - iMinValue) / fValueStep;

				if(iDiff > COLOR_STEPS - 1){
					iDiff = COLOR_STEPS - 1;
				}
				Y = SPECTRAL_Y[iDiff];
				U = SPECTRAL_U[iDiff];
				V = SPECTRAL_V[iDiff];

				pBrsRsFrame[iBrsYOffset + i] = Y;
				if(h % 2 == 0){
					if(i % 2 == 0) {
						pBrsRsFrame[iBrsUOffset + iOffset] = U;
						pBrsRsFrame[iBrsVOffset + iOffset] = V;
						iOffset += 1;
					}
				}
				i++;
			}
		}

		MemBroker_CacheCopyBack(pBrsRsFrame, sizeof(unsigned char)*g_dwThermalWidth * g_dwThermalHeight * 3 >> 1);

		tRsFrameInfo.apbyVirtAddr[0] = pBrsRsFrame;
		tRsFrameInfo.apbyVirtAddr[1] = tRsFrameInfo.apbyVirtAddr[0] + (g_dwThermalWidth * g_dwThermalHeight);
		tRsFrameInfo.apbyVirtAddr[2] = tRsFrameInfo.apbyVirtAddr[1] + ((g_dwThermalWidth * g_dwThermalHeight) >> 2);
		tRsFrameInfo.apbyPhysAddr[0] = (unsigned char *)MemBroker_GetPhysAddr((unsigned char *)tRsFrameInfo.apbyVirtAddr[0]);
		tRsFrameInfo.apbyPhysAddr[1] = (unsigned char *)MemBroker_GetPhysAddr((unsigned char *)tRsFrameInfo.apbyVirtAddr[1]);
		tRsFrameInfo.apbyPhysAddr[2] = (unsigned char *)MemBroker_GetPhysAddr((unsigned char *)tRsFrameInfo.apbyVirtAddr[2]);

		if (tRsFrameInfo.apbyVirtAddr[0] != 0) {
			tRsOutbuf.apbyVirtAddr[0] = pOutRsFrame;
			tRsOutbuf.apbyVirtAddr[1] = tRsOutbuf.apbyVirtAddr[0] + (VENC_ENCODE_WIDTH * VENC_ENCODE_HEIGHT);
			tRsOutbuf.apbyVirtAddr[2] = tRsOutbuf.apbyVirtAddr[1] + ((VENC_ENCODE_WIDTH * VENC_ENCODE_HEIGHT) >> 2);
			tRsOutbuf.apbyPhysAddr[0] = (unsigned char *)MemBroker_GetPhysAddr(tRsOutbuf.apbyVirtAddr[0]);
			tRsOutbuf.apbyPhysAddr[1] = (unsigned char *)MemBroker_GetPhysAddr(tRsOutbuf.apbyVirtAddr[1]);
			tRsOutbuf.apbyPhysAddr[2] = (unsigned char *)MemBroker_GetPhysAddr(tRsOutbuf.apbyVirtAddr[2]);
			VMF_RS_ProcessOneFrame(g_ptResizeHandle, &tRsOutbuf, &tRsFrameInfo);
		}

		memcpy(tOutWriterSsmBuffer.buffer+VMF_MAX_SSM_HEADER_SIZE, pOutRsFrame, sizeof(unsigned char)*(VENC_ENCODE_WIDTH * VENC_ENCODE_HEIGHT * 3) >> 1);
		MemBroker_CacheCopyBack(tOutWriterSsmBuffer.buffer, sizeof(unsigned char)*tSsmWriterInit.buf_size);

#ifdef DEBUG_WRITE_YUV_FILE
		iCountTmp++;
		if(iCountTmp >= 600){
			iCountTmp = 0;

			char			path[128]    = {0};
			char			isp_output_format[128] = {0};
			strcpy(isp_output_format, "out_rs_%dx%d_420.yuv");
			FILE         *fp      = NULL;
			FILE         *brs_fp      = NULL;
			sprintf(path, isp_output_format, VENC_ENCODE_WIDTH, VENC_ENCODE_HEIGHT);

			print_msg("[%s] write yuv file : %s ...\n", __func__, path);

			int ys = VENC_ENCODE_WIDTH * VENC_ENCODE_HEIGHT;
			int uv = ys >> 2;
			fp = fopen(path, "wb");
			if (fp) {
				fwrite(pOutRsFrame, 1, (ys * 3) >> 1, fp);
				fclose(fp);
			} else {
				print_msg("[%s] open %s fail\n", __func__, path);
			}

			sprintf(path, "brs_rs_%dx%d_420.yuv", g_dwThermalWidth, g_dwThermalHeight);
			ys = g_dwThermalWidth * g_dwThermalHeight;
			uv = ys >> 2;
			brs_fp = fopen(path, "wb");
			if (brs_fp) {
				fwrite(pBrsRsFrame, 1, ys * 3 >> 1, brs_fp);
				fclose(brs_fp);
			} else {
				print_msg("[%s] open %s fail\n", __func__, path);
			}
		}
#endif

		gettimeofday(&tNowFrameTime, NULL);
		if(tPreFrameTime.tv_sec != 0){
			iDiffTime = (tNowFrameTime.tv_sec - tPreFrameTime.tv_sec) * 1000000 + (tNowFrameTime.tv_usec - tPreFrameTime.tv_usec);
			iSleepTime = (1000000/THERMAL_FPS) - iDiffTime;
			if(iSleepTime > 0) {
				usleep(iSleepTime);
			}
		}
		tPreFrameTime = tNowFrameTime;

		VMF_FRAME_INFO_T* black_frame_info = (VMF_FRAME_INFO_T*) tOutWriterSsmBuffer.buffer;
		clock_gettime(CLOCK_MONOTONIC_RAW, &tFrameTime);
		unsigned int time_gap = (1000000/THERMAL_FPS)*2;
		if((unsigned int)tFrameTime.tv_nsec/1000 < time_gap) {
			black_frame_info->dwSec = (unsigned int)tFrameTime.tv_sec - 1;
			black_frame_info->dwUSec = (unsigned int)(tFrameTime.tv_nsec/1000) + 1000000 - time_gap;
		} else {
			black_frame_info->dwSec = (unsigned int)tFrameTime.tv_sec;
			black_frame_info->dwUSec = (unsigned int)(tFrameTime.tv_nsec/1000) - time_gap;
		}

		MemBroker_CacheCopyBack(tOutWriterSsmBuffer.buffer, sizeof(unsigned char)*tSsmWriterInit.buf_size);
		SSM_Writer_SendGetBuff(ptSsmWriterHandle, &tOutWriterSsmBuffer);
	}

RELEASE:
	if (pBrsRsFrame){
		MemBroker_FreeMemory(pBrsRsFrame);
		pBrsRsFrame = NULL;
	}

	if (pOutRsFrame){
		MemBroker_FreeMemory(pOutRsFrame);
		pOutRsFrame = NULL;
	}

	if (ptSsmWriterHandle) {
		SSM_Release(ptSsmWriterHandle);
		ptSsmWriterHandle = NULL;
	}
	return NULL;
}

static void sig_kill(int signo)
{
	print_msg("[%s] receive SIGNAL: %d\n",__func__, signo);
	g_bTerminate = 1;
}

int main(int argc, char* argv[])
{
	int ch, ret = 0;
	pthread_attr_t attr;
	struct sched_param param;
	pthread_t lept_pid;
	pthread_t thermal_pid;
	char *pStrstrRet = NULL;

	VMF_VENC_CODEC_TYPE eCodecType = VMF_VENC_CODEC_TYPE_H264;

	while ((ch = getopt(argc, argv, "c:d:i:s:C:t:r:p:a:")) != -1)
	{
		switch(ch)
		{
			case 'c':
				g_szSensorConfig = strdup(optarg);
				break;

			case 'd':
				g_szSensorConfigFusion= strdup(optarg);
				break;

			case 'i':
				g_ptI2cDevice = strdup(optarg);
				break;

			case 's':
				g_ptThermalDevice = strdup(optarg);
				break;

			case 'C':
				eCodecType = (VMF_VENC_CODEC_TYPE) atoi(optarg);
				break;

			case 't':
				g_dwLeptonType = atoi(optarg);
				break;

			case 'r':
				g_dwResetSleepType = atoi(optarg);
				break;

			case 'p':
				g_dwSpiSpeed = atoi(optarg);
				break;

			case 'a':
				g_szAutoSceneConfig = strdup(optarg);
				break;
			default:
				print_msg("Usage: %s [-c<sensor_config_file>] [-d<fusion_sensor_config_file>] [-i<I2C device name, default:/dev/i2c-0>]\n"
				"\t [-s<Lepton SPI device name, default:/dev/lepton3.0>] [-C<codec_type>] [-a autosecne_config_file] \n"
				"\t [-r <reset sleep time(0:750 ms, 1: 4 sec)>] [-t Lepton type(2: Lepton2.5, 3:Lepton 3.5)]\r\n", argv[0]);
				goto FAILURE;
		}
	}

	g_eCodecType = eCodecType;
	if(g_ptI2cDevice == NULL){
		g_ptI2cDevice = strdup(THERMAL_I2C_DEVICE);
	}
	if(g_ptThermalDevice == NULL){
		g_ptThermalDevice = strdup(THERMAL_DEVICE);
	}

	print_msg("Thermal Device: %s, I2C Device: %s\n", g_ptThermalDevice, g_ptI2cDevice);

	/* check sensor config */
	if (!g_szSensorConfig) {
		print_msg("[%s] Err: no sensor config\n", __func__);
		goto FAILURE;
	}

	if(g_dwLeptonType == 3){
		g_dwThermalWidth  = 160;
		g_dwThermalHeight = 120;
	} else {
		g_dwThermalWidth  = 80;
		g_dwThermalHeight = 60;
	}

	g_adwThermalValue = (unsigned short *)calloc(1, sizeof(unsigned short) * g_dwThermalWidth * g_dwThermalHeight);
	if(g_adwThermalValue == NULL){
		print_msg("[%s] allocate lepton buffer faild \n", __func__);
		goto FAILURE;
	}

	/* initialized with default attributes */
	pthread_attr_init(&attr);
	/* safe to get existing scheduling param */
	pthread_attr_getschedparam (&attr, &param);
	/* set the police and the priority */
	pthread_attr_setschedpolicy(&attr, SCHED_RR);
	param.sched_priority = 50;
	/* setting the new scheduling param */
	pthread_attr_setschedparam(&attr, &param);
	/* it make the new attr working.*/
	pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED);

	pthread_mutex_init(&g_tThemalDataMutex, NULL);

    pStrstrRet = strstr(g_ptThermalDevice, "spidev");
    if (pStrstrRet) {
		print_msg("[%s] use spi device.\n", __func__);
		if (0 != pthread_create(&lept_pid, &attr, vatics_lepton_thread_spidev, NULL)) {
			print_msg("[%s] create lepton spidev thread faild \n", __func__);
			goto FAILURE;
		} else {
			pthread_setname_np(lept_pid, "lepton_spi");
		}
    } else {
		print_msg("[%s] use lepton driver.\n", __func__);
		if (0 != pthread_create(&lept_pid, &attr, vatics_lepton_thread_lptdrv, NULL)) {
			print_msg("[%s] create lepton driver thread faild \n", __func__);
			goto FAILURE;
		} else {
			pthread_setname_np(lept_pid, "lepton_drv");
		}
	}

	//! init thread
	if (0 != pthread_create(&thermal_pid, NULL, thermal_loop, NULL)) {
		print_msg("[%s] create thermal thread faild\n", __func__);
		goto FAILURE;
	} else {
		/* set video source thread name */
		pthread_setname_np(thermal_pid, "thermal");
	}

	/* register signal */
	signal(SIGTERM, sig_kill);
	signal(SIGINT, sig_kill);

	if (init_video_source(eCodecType)) {
		goto FAILURE;
	}

	/* initializing the binder associated with the video source */
	if (init_bind()) {
		goto FAILURE;
	}

	/* initializing the binder associated with the video source */
	if (init_thbind()) {
		goto FAILURE;
	}

	/* initialize the SRB for primary encoder output buffer */
	if (init_output_srb(VENC_OUTPUT_PIN,
			VENC_OUTPUT_BUF_NUM,
 			VENC_ENCODE_BUF_SIZE)) {
		goto FAILURE;
	}

	if (init_thoutput_srb(VENC_TH_OUTPUT_PIN,
			VENC_OUTPUT_BUF_NUM,
 			VENC_TH_ENCODE_BUF_SIZE)) {
		goto FAILURE;
	}

	if (init_video_encoder(eCodecType, g_tLayout.dwVideoWidth, g_tLayout.dwVideoHeight)) {
		goto FAILURE;
	}

	if (init_thvideo_encoder(eCodecType, VENC_ENCODE_WIDTH, VENC_ENCODE_HEIGHT)) {
		goto FAILURE;
	}

	ret = init_resize_hanle();
	if (ret) {
		print_msg("[%s] Initial resize handle failed !!\n", __func__);
		goto FAILURE;
	}

	g_bInitialized = 1;

	MsgBroker_RegisterMsg(VENC_CMD_FIFO);
	MsgBroker_Run(VENC_CMD_FIFO, msg_callback, NULL, &g_bTerminate);
	MsgBroker_UnRegisterMsg();

	if(pthread_join(thermal_pid, NULL)) {
		print_msg("[%s] Thermal thread join failed !!\n", __func__);
		goto FAILURE;
	}

	if(pthread_join(lept_pid, NULL)) {
		print_msg("[%s] Lepton SPI thread join failed !!\n", __func__);
		goto FAILURE;
	}

FAILURE:
	if (g_ptResizeHandle){
		VMF_RS_Release(g_ptResizeHandle);
	}

	release_video_encoder();
	release_thvideo_encoder();
	release_output_srb();
	release_thoutput_srb();

	release_bind();
	release_thbind();
	release_video_source();

	if(g_szAutoSceneConfig){
		free(g_szAutoSceneConfig);
		g_szAutoSceneConfig = NULL;
	}

	if(g_szSensorConfig){
		free(g_szSensorConfig);
		g_szSensorConfig = NULL;
	}

	if(g_szSensorConfigFusion){
		free(g_szSensorConfigFusion);
		g_szSensorConfigFusion = NULL;
	}

	if(g_ptI2cDevice){
		free(g_ptI2cDevice);
		g_ptI2cDevice = NULL;
	}

	if(g_ptThermalDevice){
		free(g_ptThermalDevice);
		g_ptThermalDevice = NULL;
	}

	if (g_adwThermalValue){
		free(g_adwThermalValue);
		g_adwThermalValue = NULL;
	}

	if(pStrstrRet){
		free(pStrstrRet);
		pStrstrRet = NULL;
	}
	print_msg("[%s] terminated successfully!\n", __func__);

	return 0;
}
