#include "CameraDriver.h"

extern "C"{

// // ***********开始： 这部分处理与SDK操作相机无关，用于显示设备列表 ***********
// // ***********BEGIN: These functions are not related to API call and used to display device info***********
// // 数据帧回调函数
// // Data frame callback function
// static void onGetFrame(IMV_Frame* pFrame, void* pUser)
// {
// 	if (pFrame == NULL)
// 	{
// 		printf("pFrame is NULL\n");
// 		return;
// 	}

// 	printf("Get frame blockId = %llu\n", pFrame->frameInfo.blockId);

// 	return;
// }

static void displayDeviceInfo(IMV_DeviceList deviceInfoList)
{
	IMV_DeviceInfo*	pDevInfo = NULL;
	unsigned int cameraIndex = 0;
	char vendorNameCat[11];
	char cameraNameCat[16];

	// 打印Title行 
	// Print title line 
	printf("\nIdx Type Vendor     Model      S/N             DeviceUserID    IP Address    \n");
	printf("------------------------------------------------------------------------------\n");

	for (cameraIndex = 0; cameraIndex < deviceInfoList.nDevNum; cameraIndex++)
	{
		pDevInfo = &deviceInfoList.pDevInfo[cameraIndex];
		// 设备列表的相机索引  最大表示字数：3
		// Camera index in device list, display in 3 characters 
		printf("%-3d", cameraIndex + 1);

		// 相机的设备类型（GigE，U3V，CL，PCIe）
		// Camera type 
		switch (pDevInfo->nCameraType)
		{
			case typeGigeCamera:printf(" GigE");break;
			case typeU3vCamera:printf(" U3V ");break;
			case typeCLCamera:printf(" CL  ");break;
			case typePCIeCamera:printf(" PCIe");break;
			default:printf("     ");break;
		}

		// 制造商信息  最大表示字数：10 
		// Camera vendor name, display in 10 characters 
		if (strlen(pDevInfo->vendorName) > 10)
		{
			memcpy(vendorNameCat, pDevInfo->vendorName, 7);
			vendorNameCat[7] = '\0';
			strcat(vendorNameCat, "...");
			printf(" %-10.10s", vendorNameCat);
		}
		else
		{
			printf(" %-10.10s", pDevInfo->vendorName);
		}

		// 相机的型号信息 最大表示字数：10 
		// Camera model name, display in 10 characters 
		printf(" %-10.10s", pDevInfo->modelName);

		// 相机的序列号 最大表示字数：15 
		// Camera serial number, display in 15 characters 
		printf(" %-15.15s", pDevInfo->serialNumber);

		// 自定义用户ID 最大表示字数：15 
		// Camera user id, display in 15 characters 
		if (strlen(pDevInfo->cameraName) > 15)
		{
			memcpy(cameraNameCat, pDevInfo->cameraName, 12);
			cameraNameCat[12] = '\0';
			strcat(cameraNameCat, "...");
			printf(" %-15.15s", cameraNameCat);
		}
		else
		{
			printf(" %-15.15s", pDevInfo->cameraName);
		}

		// GigE相机时获取IP地址 
		// IP address of GigE camera 
		if (pDevInfo->nCameraType == typeGigeCamera)
		{
			printf(" %s", pDevInfo->DeviceSpecificInfo.gigeDeviceInfo.ipAddress);
		}

		printf("\n");
	}

	return;
}

static char* trim(char* pStr)
{
	char* pDst = pStr;
	char* pTemStr = NULL;

	if (pDst != NULL)
	{
		pTemStr = pDst + strlen(pStr) - 1;
		while (*pDst == ' ')
		{
			pDst++;
		}
		while ((pTemStr > pDst) && (*pTemStr == ' '))
		{
			*pTemStr-- = '\0';
		}
	}
	return pDst;
}

static int isInputValid(char* pInpuStr)
{
	char numChar;
	char* pStr = pInpuStr;
	while (*pStr != '\0')
	{
		numChar = *pStr;
		if ((numChar > '9') || (numChar < '0'))
		{
			return -1;
		}
		pStr++;
	}
	return 0;
}

static unsigned int selectDevice(unsigned int cameraCnt)
{
	char inputStr[256];
	char* pTrimStr;
	int inputIndex = -1;
	int ret = -1;
	char* find = NULL;

	printf("\nPlease input the camera index: ");
	while (1)
	{
		memset(inputStr, 0, sizeof(inputStr));
		fgets(inputStr, sizeof(inputStr), stdin);

		// 清空输入缓存
		// clear flush
		fflush(stdin);

		// fgets比gets多吃一个换行符号，取出换行符号 
		// fgets eats one more line feed symbol than gets, and takes out the line feed symbol
		find = strchr(inputStr, '\n');
		if (find) { *find = '\0'; }

		pTrimStr = trim(inputStr);
		ret = isInputValid(pTrimStr);
		if (ret == 0)
		{
			inputIndex = atoi(pTrimStr);
			// 输入的序号从1开始 
			// Input index starts from 1 
			inputIndex -= 1;
			if ((inputIndex >= 0) && (inputIndex < (int)cameraCnt))
			{
				break;
			}
		}

		printf("Input invalid! Please input the camera index: ");
	}
	return (unsigned int)inputIndex;
}

// ***********结束： 这部分处理与SDK操作相机无关，用于显示设备列表 ***********
// ***********END: These functions are not related to API call and used to display device info***********
static int setSoftTriggerConf(void* libHandle, IMV_HANDLE devHandle)
{
	int ret = IMV_OK;
  	// 获取设置触发源为软触发函数地址 
	DLL_SetEnumFeatureSymbol DLLSetEnumFeatureSymbol = (DLL_SetEnumFeatureSymbol)dlsym(libHandle, "IMV_SetEnumFeatureSymbol");
	if (NULL == DLLSetEnumFeatureSymbol)
	{
		printf("Get IMV_SetEnumFeatureSymbol address failed!\n");
		return ret;
	}

	// 设置触发源为软触发 
	ret = DLLSetEnumFeatureSymbol(devHandle, "TriggerSource", "Software");
	if (IMV_OK != ret)
	{
		printf("Set triggerSource value failed! ErrorCode[%d]\n", ret);
		return ret;
	}

	// 设置触发器 
	ret = DLLSetEnumFeatureSymbol(devHandle, "TriggerSelector", "FrameStart");
	if (IMV_OK != ret)
	{
		printf("Set triggerSelector value failed! ErrorCode[%d]\n", ret);
		return ret;
	}

	// 设置触发模式 
	ret = DLLSetEnumFeatureSymbol(devHandle, "TriggerMode", "On");
	if (IMV_OK != ret)
	{
		printf("Set triggerMode value failed! ErrorCode[%d]\n", ret);
		return ret;
	}

	return ret;
}

static int setUnSoftTriggerConf(void* libHandle, IMV_HANDLE devHandle)
{
	int ret = IMV_OK;
  	// 获取设置触发源为软触发函数地址 
	DLL_SetEnumFeatureSymbol DLLSetEnumFeatureSymbol = (DLL_SetEnumFeatureSymbol)dlsym(libHandle, "IMV_SetEnumFeatureSymbol");
	if (NULL == DLLSetEnumFeatureSymbol)
	{
		printf("Get IMV_SetEnumFeatureSymbol address failed!\n");
		return ret;
	}

	// 设置触发源为软触发 
	ret = DLLSetEnumFeatureSymbol(devHandle, "TriggerSource", "Software");
	if (IMV_OK != ret)
	{
		printf("Set triggerSource value failed! ErrorCode[%d]\n", ret);
		return ret;
	}

	// 设置触发器 
	ret = DLLSetEnumFeatureSymbol(devHandle, "TriggerSelector", "FrameStart");
	if (IMV_OK != ret)
	{
		printf("Set triggerSelector value failed! ErrorCode[%d]\n", ret);
		return ret;
	}

	// 设置触发模式 
	ret = DLLSetEnumFeatureSymbol(devHandle, "TriggerMode", "Off");
	if (IMV_OK != ret)
	{
		printf("Set triggerMode value failed! ErrorCode[%d]\n", ret);
		return ret;
	}

	return ret;
}

// Image convert
static int imageConvert(void* libHandle, IMV_HANDLE devHandle, IMV_Frame frame, IMV_EPixelType convertFormat,void** data, int& w, int& h)
{
	IMV_PixelConvertParam stPixelConvertParam;
	unsigned char*			pDstBuf = NULL;
	unsigned int			nDstBufSize = 0;
	int						ret = IMV_OK;
	FILE*					hFile = NULL;
	const char*				pFileName = NULL;
	const char*				pConvertFormatStr = NULL;

  // 获取设置触发源为软触发函数地址 
  DLL_PixelConvert DLLPixelConvert = (DLL_PixelConvert)dlsym(libHandle, "IMV_PixelConvert");
  if (NULL == DLLPixelConvert)
  {
    printf("Get IMV_PixelConvert address failed!\n");
    return -999;
  }

	switch (convertFormat)
	{
		case gvspPixelRGB8:
			nDstBufSize = sizeof(unsigned char) * frame.frameInfo.width * frame.frameInfo.height * 3;
			pFileName = (const char*)"convertRGB8.bmp";
			pConvertFormatStr = (const char*)"RGB8";
			break;
		case gvspPixelBGR8:
			nDstBufSize = sizeof(unsigned char) * frame.frameInfo.width * frame.frameInfo.height * 3;
			pFileName = (const char*)"convertBGR8.bmp";
			pConvertFormatStr = (const char*)"BGR8";
			break;
		case gvspPixelBGRA8:
			nDstBufSize = sizeof(unsigned char) * frame.frameInfo.width * frame.frameInfo.height * 4;
			pFileName = (const char*)"convertBGRA8.bmp";
			pConvertFormatStr = (const char*)"BGRA8";
			break;
		case gvspPixelMono8:
		default:
			nDstBufSize = sizeof(unsigned char) * frame.frameInfo.width * frame.frameInfo.height;
			pFileName = (const char*)"convertMono8.bmp";
			pConvertFormatStr = (const char*)"Mono8";
			break;
	}

	pDstBuf = (unsigned char*)malloc(nDstBufSize);
	if (NULL == pDstBuf)
	{
		printf("malloc pDstBuf failed!\n");
		return -998;
	}

	// 图像转换成BGR8
	// convert image to BGR8
	memset(&stPixelConvertParam, 0, sizeof(stPixelConvertParam));
	stPixelConvertParam.nWidth = frame.frameInfo.width;
	stPixelConvertParam.nHeight = frame.frameInfo.height;
	stPixelConvertParam.ePixelFormat = frame.frameInfo.pixelFormat;
	stPixelConvertParam.pSrcData = frame.pData;
	stPixelConvertParam.nSrcDataLen = frame.frameInfo.size;
	stPixelConvertParam.nPaddingX = frame.frameInfo.paddingX;
	stPixelConvertParam.nPaddingY = frame.frameInfo.paddingY;
	stPixelConvertParam.eBayerDemosaic = demosaicNearestNeighbor;
	stPixelConvertParam.eDstPixelFormat = convertFormat;
	stPixelConvertParam.pDstBuf = pDstBuf;
	stPixelConvertParam.nDstBufSize = nDstBufSize;

	ret = DLLPixelConvert(devHandle, &stPixelConvertParam);
	if (IMV_OK == ret)
	{
			printf("image convert to %s successfully! nDstDataLen (%u)\n", 
				pConvertFormatStr, stPixelConvertParam.nDstBufSize);
			memcpy(*data, stPixelConvertParam.pDstBuf, stPixelConvertParam.nHeight * stPixelConvertParam.nWidth * 3);
			printf("memcpy success! %d - %d - %d \n", *data, stPixelConvertParam.nHeight, stPixelConvertParam.nWidth);
			w = stPixelConvertParam.nWidth;
			h = stPixelConvertParam.nHeight;
			// cv::Mat im(h, w, CV_8UC3, pDstBuf);
		    // cv::imwrite("/home/caowei/catkin_ws/output1.png", im);
    		// cv::imwrite("output1.png", im);
	}
	else
	{
		printf("image convert to %s failed! ErrorCode[%d]\n", pConvertFormatStr, ret);
	}

	if (pDstBuf)
	{
		free(pDstBuf);
		pDstBuf = NULL;
	}
	return ret;
}

static void sendToRos(IMV_Frame frame)
{
	IMV_FlipImageParam		stFlipImageParam;
	unsigned int			nChannelNum = 0;
	int						ret = IMV_OK;
	FILE*					hFile = NULL;

	memset(&stFlipImageParam, 0, sizeof(stFlipImageParam));

  if (gvspPixelBGR8 == frame.frameInfo.pixelFormat)
	{
		stFlipImageParam.pSrcData = frame.pData;
		stFlipImageParam.nSrcDataLen = frame.frameInfo.width * frame.frameInfo.height * BGR_CHANNEL_NUM;
		stFlipImageParam.ePixelFormat = frame.frameInfo.pixelFormat;
		nChannelNum = BGR_CHANNEL_NUM;
	}
	else
	{
		printf("image convert to BGR8 failed! ErrorCode[%d]\n", ret);
	}

  // 向ros发送/image消息
	do
	{
    
	} while (false);

}

static int ret = IMV_OK;
static unsigned int cameraIndex = 0;
static IMV_HANDLE devHandle = NULL;
static void* libHandle = NULL;
static IMV_Frame frame;


static DLL_EnumDevices DLLEnumDevices = NULL;
static DLL_CreateHandle DLLCreateHandle = NULL;
static DLL_DestroyHandle DLLDestroyHandle = NULL;
static DLL_Open DLLOpen = NULL;
static DLL_AttachGrabbing DLLAttachGrabbing = NULL;
static DLL_StartGrabbing DLLStartGrabbing = NULL;
static DLL_StopGrabbing DLLStopGrabbing = NULL;
static DLL_Close DLLClose = NULL;
static DLL_GetFrame DLLGetFrame = NULL;
static DLL_ReleaseFrame DLLReleaseFrame = NULL;
static DLL_ClearFrameBuffer DLLClearFrameBuffer = NULL;
static DLL_ExecuteCommandFeature DLLExecuteCommandFeature = NULL;
static DLL_SetIntFeatureValue DLLSetIntFeatureValue = NULL;
static DLL_SetDoubleFeatureValue DLLSetDoubleFeatureValue = NULL;

// static int setGrabMode(IMV_HANDLE devHandle, bool isContious)
// {
// 	int ret = IMV_OK;

// 	// 设置触发器 
// 	// Set trigger selector to FrameStart 
// 	ret = IMV_SetEnumFeatureSymbol(devHandle, "TriggerSelector", "FrameStart");
// 	if (IMV_OK != ret)
// 	{
// 		printf("Set triggerSelector value failed! ErrorCode[%d]\n", ret);
// 		return ret;
// 	}

// 	if (isContious)
// 	{
// 		// 关闭触发模式 
// 		// Set trigger mode to Off 
// 		ret = IMV_SetEnumFeatureSymbol(devHandle, "TriggerMode", "Off");
// 		if (IMV_OK != ret)
// 		{
// 			printf("Set triggerMode value failed! ErrorCode[%d]\n", ret);
// 			return ret;
// 		}
// 	}
// 	else
// 	{
// 		// 设置触发源为软触发 
// 		// Set trigger source to Software 
// 		ret = IMV_SetEnumFeatureSymbol(devHandle, "TriggerSource", "Software");
// 		if (IMV_OK != ret)
// 		{
// 			printf("Set triggerSource value failed! ErrorCode[%d]\n", ret);
// 			return ret;
// 		}

// 		// 设置触发模式 
// 		// Set trigger mode to On 
// 		ret = IMV_SetEnumFeatureSymbol(devHandle, "TriggerMode", "On");
// 		if (IMV_OK != ret)
// 		{
// 			printf("Set triggerMode value failed! ErrorCode[%d]\n", ret);
// 			return ret;
// 		}
// 	}

// 	return ret;
// }
#include <unistd.h>
#include <pthread.h>
#define sleep(ms)	usleep(1000 * ms)

void* executeSoftTriggerProc(void* pUserData)
{
	int ret = IMV_OK;
	IMV_HANDLE devHandle = (IMV_HANDLE)pUserData;
	if (NULL == devHandle)
	{
		return NULL;
	}

	while (true)
	{
		ret = DLLExecuteCommandFeature(devHandle, "TriggerSoftware");
		if (IMV_OK != ret)
		{
			printf("Execute TriggerSoftware failed! ErrorCode[%d]\n", ret);
			// 通过睡眠时间来调节帧率
			// Adjust the frame rate by sleep time
			sleep(200);
			continue;
		}
		return NULL;
	}
	return NULL;
}


// 数据帧回调函数
// Data frame callback function
static IMV_Frame* g_pFrame = nullptr;
static void onGetFrame(IMV_Frame* pFrame, void* pUser)
{
	int ret = IMV_OK;
	// unsigned int chunkDataIndex = 0;
	unsigned int paramIndex = 0;
	// IMV_ChunkDataInfo chunkDataInfo;
	IMV_HANDLE devHandle = (IMV_HANDLE)pUser;

	if (pFrame == NULL)
	{
		printf("pFrame is NULL\n");
		return;
	}

	printf("Get frame blockId = %llu\n", pFrame->frameInfo.blockId);

	if (!devHandle)
	{
		printf("devHandle is NULL!\n");
		return;
	}

	g_pFrame = pFrame;

	// // 获取ChunkData数据
	// // Get ChunkData
	// for (chunkDataIndex = 0; chunkDataIndex < pFrame->frameInfo.chunkCount; chunkDataIndex++)
	// {
	// 	ret = IMV_GetChunkDataByIndex(devHandle, pFrame, chunkDataIndex, &chunkDataInfo);
	// 	if (IMV_OK != ret)
	// 	{
	// 		printf("Get ChunkData failed! ErrorCode[%d]\n", ret);
	// 		continue;
	// 	}

	// 	printf("chunkID = %u\n", chunkDataInfo.chunkID);
	// 	for (paramIndex = 0; paramIndex < chunkDataInfo.nParamCnt; paramIndex++)
	// 	{
	// 		printf("paramName = %s\n", chunkDataInfo.pParamNameList[paramIndex].str);
	// 	}
	// 	printf("\n");
	// }

	return;
}


int camera_init()
{
	int ret = IMV_OK;
	// Load SDK library
#ifdef _WIN32
	libHandle = 0;
#else
	printf("Load MVSDKmd.so!\n");
	libHandle = dlopen("libMVSDK.so", RTLD_LAZY);
#endif
	if (NULL == libHandle)
	{
		printf("Load MVSDKmd.so library failed!\n");
		return IMV_ERROR;
	}

	// 获取发现设备接口函数地址 
	// Get discover camera interface address
	DLLEnumDevices = (DLL_EnumDevices)dlsym(libHandle, "IMV_EnumDevices");
	if (NULL == DLLEnumDevices)
	{
		printf("Get IMV_EnumDevices address failed!\n");
		return IMV_ERROR;
	}

	
	// 获取发现设备接口函数地址 
  // Get discover camera interface address
	//DLL_EnumDevices DLLEnumDevices = (DLL_EnumDevices)dlsym(libHandle, "IMV_EnumDevices");
	//if (NULL == DLLEnumDevices)
	//{
	//	printf("Get IMV_EnumDevices address failed!\n");
	//	return 0;
	//}
	
	// 获取创建设备句柄接口函数地址 
	// Get create Device Handle interface address
	DLLCreateHandle = (DLL_CreateHandle)dlsym(libHandle, "IMV_CreateHandle");
	if (NULL == DLLCreateHandle)
	{
		printf("Get IMV_CreateHandle address failed!\n");
		return IMV_ERROR;
	}
	
	// 获取销毁设备句柄接口函数地址 
	// Get destroy Device Handle interface address
	DLLDestroyHandle = (DLL_DestroyHandle)dlsym(libHandle, "IMV_DestroyHandle");
	if (NULL == DLLDestroyHandle)
	{
		printf("Get IMV_DestroyHandle address failed!\n");
		return IMV_ERROR;
	}

	// 获取打开相机接口函数地址 
	// Get open camera interface address
	DLLOpen = (DLL_Open)dlsym(libHandle, "IMV_Open");
	if (NULL == DLLOpen)
	{
		printf("Get IMV_Open address failed!\n");
		return IMV_ERROR;
	}
	
	// 获取注册数据帧回调接口函数地址 
	// Get register data frame callback interface address
	DLLAttachGrabbing = (DLL_AttachGrabbing)dlsym(libHandle, "IMV_AttachGrabbing");
	if (NULL == DLLAttachGrabbing)
	{
		printf("Get IMV_AttachGrabbing address failed!\n");
		return IMV_ERROR;
	}
	
	// 获取开始拉流接口函数地址 
	// Get start grabbing interface address
	DLLStartGrabbing = (DLL_StartGrabbing)dlsym(libHandle, "IMV_StartGrabbing");
	if (NULL == DLLStartGrabbing)
	{
		printf("Get IMV_StartGrabbing address failed!\n");
		return IMV_ERROR;
	}
	
	// 获取停止拉流接口函数地址 
	// Get stop grabbing interface address
	DLLStopGrabbing = (DLL_StopGrabbing)dlsym(libHandle, "IMV_StopGrabbing");
	if (NULL == DLLStopGrabbing)
	{
		printf("Get IMV_StopGrabbing address failed!\n");
		return IMV_ERROR;
	}

	// 获取
	// 获取关闭相机接口函数地址 
	// Get close camera interface address
	DLLClose = (DLL_Close)dlsym(libHandle, "IMV_Close");
	if (NULL == DLLClose)
	{
		printf("Get IMV_Close address failed!\n");
		return IMV_ERROR;
	}
	
	// 获取获取一帧图像函数地址 
	DLLGetFrame = (DLL_GetFrame)dlsym(libHandle, "IMV_GetFrame");
	if (NULL == DLLGetFrame)
	{
		printf("Get IMV_GetFrame address failed!\n");
		return IMV_ERROR;
	}
	
	DLLReleaseFrame = (DLL_ReleaseFrame)dlsym(libHandle, "IMV_ReleaseFrame");
	if (NULL == DLLReleaseFrame)
	{
		printf("Get IMV_ReleaseFrame address failed!\n");
		return IMV_ERROR;
	}
	
	DLLClearFrameBuffer = (DLL_ClearFrameBuffer)dlsym(libHandle, "IMV_ClearFrameBuffer");
	if (NULL == DLLClearFrameBuffer)
	{
		printf("Get IMV_ClearFrameBuffer address failed!\n");
		return IMV_ERROR;
	}
	
	DLLExecuteCommandFeature = (DLL_ExecuteCommandFeature)dlsym(libHandle, "IMV_ExecuteCommandFeature");
	if (NULL == DLLExecuteCommandFeature)
	{
		printf("Get IMV_ExecuteCommandFeature address failed!\n");
		return IMV_ERROR;
	}
	
	DLLSetIntFeatureValue = (DLL_SetIntFeatureValue)dlsym(libHandle, "IMV_SetIntFeatureValue");
	if (NULL == DLLSetIntFeatureValue)
	{
		printf("Get IMV_SetIntFeatureValue address failed!\n");
		return IMV_ERROR;
	}
	
	DLLSetDoubleFeatureValue = (DLL_SetDoubleFeatureValue)dlsym(libHandle, "IMV_SetDoubleFeatureValue");
	if (NULL == DLLSetDoubleFeatureValue)
	{
		printf("Get IMV_SetDoubleFeatureValue address failed!\n");
		return IMV_ERROR;
	}

	////////////////////// 检查接口结束
	// 发现设备 
	// discover camera 
	IMV_DeviceList deviceInfoList;
	ret = DLLEnumDevices(&deviceInfoList, interfaceTypeAll);
	if (IMV_OK != ret)
	{
		printf("Enumeration devices failed! ErrorCode[%d]\n", ret);
		return ret;
	}

	if (deviceInfoList.nDevNum < 1)
	{
		printf("no camera\n");
		return IMV_ERROR;
	}

	// 打印相机基本信息（序号,类型,制造商信息,型号,序列号,用户自定义ID,IP地址） 
	// Print camera info (Index, Type, Vendor, Model, Serial number, DeviceUserID, IP Address) 

	displayDeviceInfo(deviceInfoList);
	// 选择需要连接的相机 
	// Select one camera to connect to  
	// cameraIndex = selectDevice(deviceInfoList.nDevNum);
	cameraIndex = 0; // 第一个相机

	// 创建设备句柄
	// Create Device Handle
	ret = DLLCreateHandle(&devHandle, modeByIndex, (void*)&cameraIndex);
	if (IMV_OK != ret)
	{
		printf("Create devHandle failed! ErrorCode[%d]\n", ret);
		return IMV_ERROR;
	}

	return ret;
}

int camera_start_ansyc(int epx_time, void** data, int& w, int& h, int time_out)
{
	int ret = IMV_OK;
	pthread_t threadID = 0;

	// 打开相机 
	ret = DLLOpen(devHandle);
	if (IMV_OK != ret)
	{
		printf("Open camera failed! ErrorCode[%d]\n", ret);
		return ret;
	}

	//设置软触发模式
	ret = setSoftTriggerConf(libHandle, devHandle);
	if (IMV_OK != ret)
	{
		printf("setSoftTriggerConf failed! ErrorCode[%d]\n", ret);
		return ret;
	}


	ret = DLLSetIntFeatureValue(devHandle, "Width", 9344);
	if (IMV_OK != ret)
	{
		printf("Set feature value Width failed! ErrorCode[%d]\n", ret);
		return ret;
	}

	ret = DLLSetIntFeatureValue(devHandle, "Height", 7000);
	if (IMV_OK != ret)
	{
		printf("Set feature value Height failed! ErrorCode[%d]\n", ret);
		return ret;
	}

	ret = DLLSetIntFeatureValue(devHandle, "OffsetX", 0);
	if (IMV_OK != ret)
	{
		printf("Set feature value OffsetX failed! ErrorCode[%d]\n", ret);
		return ret;
	}

	ret = DLLSetIntFeatureValue(devHandle, "OffsetY", 0);
	if (IMV_OK != ret)
	{
		printf("Set feature value OffsetY failed! ErrorCode[%d]\n", ret);
		return ret;
	}

	// 设置属性值曝光
	  // Set feature value 
	ret = DLLSetDoubleFeatureValue(devHandle, "ExposureTime", epx_time);
	if (IMV_OK != ret)
	{
		printf("Set feature value failed! ErrorCode[%d]\n", ret);
		return ret;
	}

	// 注册数据帧回调函数
	// Register data frame callback function
	ret = DLLAttachGrabbing(devHandle, onGetFrame, (void*)devHandle);
	if (IMV_OK != ret)
	{
		printf("Attach grabbing failed! ErrorCode[%d]\n", ret);
		return ret;
	}

	ret = DLLClearFrameBuffer(devHandle);
	if (IMV_OK != ret)
	{
		printf("DLLClearFrameBuffer failed! ErrorCode[%d]\n", ret);
		return ret;
	}

	// 开始拉流 
	ret = DLLStartGrabbing(devHandle);
	if (IMV_OK != ret)
	{
		printf("Start grabbing failed! ErrorCode[%d]\n", ret);
		return ret;
	}

	// 创建软触发线程 
	// Create soft trigger thread 
	if (pthread_create(&threadID, 0, executeSoftTriggerProc, (void*)devHandle) != 0)
	{
		printf("Failed to create soft trigger thread!\n");
		return -1;
	}

	// 取图2秒
	// get frame 2 seconds
	sleep(500);
	pthread_join(threadID, NULL);

	while (g_pFrame==nullptr)
	{
		printf("waiting frame %d \n", 0);
		sleep(500);
	}

	printf("width %d\n", g_pFrame->frameInfo.width);
	printf("Height %d\n", g_pFrame->frameInfo.height);

	ret = imageConvert(libHandle, devHandle, *g_pFrame, gvspPixelBGR8, data, w, h);
	if (IMV_OK != ret)
	{
		printf("imageConvert failed! ErrorCode[%d]\n", ret);
		return ret;
	}

	ret = DLLStopGrabbing(devHandle);
	if (IMV_OK != ret)
	{
		printf("Stop grabbing failed! ErrorCode[%d]\n", ret);
		return ret;
	}

  	// 关闭相机
  	// Close camera 
  	ret = DLLClose(devHandle);
  	if (IMV_OK != ret)
  	{
    	printf("Close camera failed! ErrorCode[%d]\n", ret);
    	return ret;
  	}
	printf("Close camera success! ErrorCode[%d]\n", ret);

	// Destroy Device Handle
	if (NULL != devHandle)
	{
		// 销毁设备句柄
		// Destroy Device Handle
		DLLDestroyHandle(devHandle);
	}
	printf("DLLDestroyHandle success! [%d]\n", ret);

	if (NULL != libHandle)
	{
		dlclose(libHandle);
	}
	printf("dlclose success! [%d]\n", 0);

	// printf("end...\n");
	return ret;
}

int camera_start(int epx_time)
{
	int ret = IMV_OK;

	// 打开相机 
	ret = DLLOpen(devHandle);
	if (IMV_OK != ret)
	{
		printf("Open camera failed! ErrorCode[%d]\n", ret);
		return ret;
	}

	//设置软触发模式 !!!
	ret = setSoftTriggerConf(libHandle, devHandle);
	if (IMV_OK != ret)
	{
		printf("setSoftTriggerConf failed! ErrorCode[%d]\n", ret);
		return ret;
	}

	ret = DLLSetIntFeatureValue(devHandle, "Width", 9344);
	if (IMV_OK != ret)
	{
		printf("Set feature value Width failed! ErrorCode[%d]\n", ret);
		return ret;
	}

	ret = DLLSetIntFeatureValue(devHandle, "Height", 7000);
	if (IMV_OK != ret)
	{
		printf("Set feature value Height failed! ErrorCode[%d]\n", ret);
		return ret;
	}

	ret = DLLSetIntFeatureValue(devHandle, "OffsetX", 0);
	if (IMV_OK != ret)
	{
		printf("Set feature value OffsetX failed! ErrorCode[%d]\n", ret);
		return ret;
	}

	ret = DLLSetIntFeatureValue(devHandle, "OffsetY", 0);
	if (IMV_OK != ret)
	{
		printf("Set feature value OffsetY failed! ErrorCode[%d]\n", ret);
		return ret;
	}

	// 设置属性值曝光
	// Set feature value 
	ret = DLLSetDoubleFeatureValue(devHandle, "ExposureTime", epx_time);
	if (IMV_OK != ret)
	{
		printf("Set feature value failed! ErrorCode[%d]\n", ret);
		return ret;
	}

	//设置软触发模式 !!!
	ret = setUnSoftTriggerConf(libHandle, devHandle);
	if (IMV_OK != ret)
	{
		printf("setSoftTriggerConf failed! ErrorCode[%d]\n", ret);
		return ret;
	}

	// 开始拉流 
	ret = DLLStartGrabbing(devHandle);
	if (IMV_OK != ret)
	{
		printf("Start grabbing failed! ErrorCode[%d]\n", ret);
		return ret;
	}

	return ret;
}


int camera_cap(void** data, int& w, int& h, int time_out)
{
	int ret = IMV_OK;
	

	// 清除帧数据缓存
	ret = DLLClearFrameBuffer(devHandle);
	if (IMV_OK != ret)
	{
		printf("Clear frame buffer failed! ErrorCode[%d]\n", ret);
		return ret;
	}
	sleep(200);

	// //设置软触发模式
	// ret = setSoftTriggerConf(libHandle, devHandle);
	// if (IMV_OK != ret)
	// {
	// 	printf("setSoftTriggerConf failed! ErrorCode[%d]\n", ret);
	// 	return ret;
	// }
	// sleep(100);

	//软触发
	// while (true)
	// {
	// 	ret = DLLExecuteCommandFeature(devHandle, "TriggerSoftware");
	// 	if (IMV_OK != ret)
	// 	{
	// 		printf("Execute TriggerSoftware failed! ErrorCode[%d]\n", ret);
	// 		sleep(200);
	// 		continue;
	// 	}
	// 	printf("Execute TriggerSoftware success! [%d]\n", ret);
	// 	break;
	// }
	sleep(2500);

	// 获取一帧图像, TIMEOUT 5000ms
	ret = DLLGetFrame(devHandle, &frame, time_out);
	if (IMV_OK != ret)
	{
		printf("Get frame failed! ErrorCode[%d]\n", ret);
		return ret;
	}

	printf("width %d\n", frame.frameInfo.width);
	printf("Height %d\n", frame.frameInfo.height);

	ret = imageConvert(libHandle, devHandle, frame, gvspPixelBGR8, data, w, h);
	if (IMV_OK != ret)
	{
		printf("imageConvert failed! ErrorCode[%d]\n", ret);
		return ret;
	}

	// 释放图像缓存
	ret = DLLReleaseFrame(devHandle, &frame);
	if (IMV_OK != ret)
	{
		printf("Release frame failed! ErrorCode[%d]\n", ret);
		return ret;
	}

	
	return ret;
}

int camera_stop()
{
	int ret = IMV_OK;
	//设置软触发模式
	ret = setUnSoftTriggerConf(libHandle, devHandle);
	if (IMV_OK != ret)
	{
		printf("setSoftTriggerConf failed! ErrorCode[%d]\n", ret);
		return ret;
	}
	sleep(100);

	ret = DLLStopGrabbing(devHandle);
	if (IMV_OK != ret)
	{
		printf("Stop grabbing failed! ErrorCode[%d]\n", ret);
		return ret;
	}

  	// 关闭相机
  	// Close camera 
  	ret = DLLClose(devHandle);
  	if (IMV_OK != ret)
  	{
    	printf("Close camera failed! ErrorCode[%d]\n", ret);
    	return ret;
  	}
	printf("Close camera success! [%d]\n", ret);

	if (NULL != devHandle)
	{
		// 销毁设备句柄
		// Destroy Device Handle
		DLLDestroyHandle(devHandle);
	}
	printf("DLLDestroyHandle success! [%d]\n", ret);

	if (NULL != libHandle)
	{
		dlclose(libHandle);
	}
	printf("dlclose success! [%d]\n", 0);
	return ret;
}

#include <cstdlib> // 包含atoi函数
int main(int argc, char *argv[])
{
	if (argc == 0)
	{
		printf("argc == 0 [%d]\n", -1);
	}

	// std::string s(argv[0]);
	// std::cout << "Usage: " << argv[1] << " <parameter>" << std::endl;
	int ext = atoi(argv[1]);
	if (ext < 10000)
	{
		printf("ex_time is too small == [%d]\n", ext);
		return -1;
	}
	printf("ex_time ==  [%d]\n", ext);

	char* data = (char*)malloc(9344*7000*3);
	int w, h;

	ret = camera_init();
	if (ret != IMV_OK) return ret;

	// ret = camera_start_ansyc(ext, (void**) &data, w, h, 5000);
	// if (ret != IMV_OK) return ret;
	
	ret = camera_start(ext);
	if (ret != IMV_OK) return ret;

	int count = 0;
	while (true)
	{
		ret = camera_cap((void**) &data, w,  h,  5000);
		// if (ret != IMV_OK) return ret;
		if (ret != IMV_OK) 
		{
			count++;
			continue;
		}
		cv::Mat im(h,w, CV_8UC3, data);
		cv::imwrite("output.png", im);
		
		printf("cap times ==  [%d]\n", count);
		break;
	}

	camera_stop();

	
	if (data)
		free(data);
	return ret;
}

  
}