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

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

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

	// 设置触发模式 
	// Set trigger mode to On 
	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;
// }

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(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 = setGrabMode(devHandle, false);
	// if (IMV_OK != ret)
	// {
	// 	printf("Set grabbing mode 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;
	sleep(500);
	// 清除帧数据缓存
	ret = DLLClearFrameBuffer(devHandle);
	if (IMV_OK != ret)
	{
		printf("Clear frame buffer failed! ErrorCode[%d]\n", ret);
		return ret;
	}

	/////////////////////////获取一帧图像////////////////////////////
	// 执行软触发
	// ret = DLLExecuteCommandFeature(devHandle, "TriggerSoftware");
	// if (IMV_OK != ret)
	// {
	// 	printf("Execute TriggerSoftware failed! ErrorCode[%d]\n", ret);
	// 	return ret;
	// }

	// sleep(500);

	// 获取一帧图像, TIMEOUT 5000ms
	ret = DLLGetFrame(devHandle, &frame, time_out);
	// ret = DLLGetFrame(devHandle, &frame, 5000);
	// if (IMV_OK != ret && -101 != ret)
	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;
	}

	// printf("11111111111111\n", ret);
	// cv::Mat im(frame.frameInfo.height, frame.frameInfo.width, CV_8UC3, frame.pData);
	// cv::imwrite("/home/caowei/catkin_ws/output.png", im);
	// cv::imwrite("output.png", im);
	// printf("22222222222222\n", ret);
	// // 向ros送 /image消息
	// sendToRos(frame);
	// 释放图像缓存
	ret = DLLReleaseFrame(devHandle, &frame);
	if (IMV_OK != ret)
	{
		printf("Release frame failed! ErrorCode[%d]\n", ret);
		return ret;
	}
	sleep(500);
	////////////////////////////////////////////////////////////////

	// // 取图2秒
	// // get frame 2 seconds
	// sleep(2000);
	// 停止拉流 
	// Stop grabbing 
	
	// 等待流碎片走完
	// sleep(2000);
}

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


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

	// 待修改

	// 销毁设备句柄
	// Destroy Device Handle
	if (NULL != devHandle)
	{
		// 销毁设备句柄
		// Destroy Device Handle
		DLLDestroyHandle(devHandle);
	}

	if (NULL != libHandle)
	{
		dlclose(libHandle);
	}

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

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

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

	ret = camera_start(12 * 10000);
	if (ret != IMV_OK) return ret;

	int index = 5;
	while (true)
	{
		ret = camera_cap((void**) &data, w,  h,  5000);
		if (ret != IMV_OK) return ret;

		cv::Mat im(h,w, CV_8UC3, data);
		// cv::imwrite("/home/caowei/catkin_ws/output.png", im);
		cv::imwrite("output.png", im);
		index --;
		if (index == 0)
			break;
	}

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

	// cv::Mat im(h,w, CV_8UC3, data);
	// // cv::imwrite("/home/caowei/catkin_ws/output.png", im);
	// cv::imwrite("output.png", im);

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

	// cv::Mat im2(h,w, CV_8UC3, data);
	// // cv::imwrite("/home/caowei/catkin_ws/output2.png", im2);
	// cv::imwrite("output2.png", im2);

	camera_stop();

	if (data)
		free(data);
	return ret;
}
//
//
//int main()
//{
//	int ret = IMV_OK;
//	unsigned int cameraIndex = 0;
//	IMV_HANDLE devHandle = NULL;
//	void* libHandle = NULL;
//	DLL_DestroyHandle DLLDestroyHandle = NULL;
//  IMV_Frame frame;
//
//  // 加载SDK库  
//  // Load SDK library
//#ifdef _WIN32
//  libHandle = 0;
//#else
//  libHandle = dlopen("libMVSDK.so", RTLD_LAZY);
//#endif
//
//  if (NULL == libHandle)
//  {
//    printf("Load MVSDKmd.dll library failed!\n");
//    return 0;
//  }
//
//  // 获取发现设备接口函数地址 
//  // Get discover camera interface address
//  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 0;
//  }
//
//  // 获取销毁设备句柄接口函数地址 
//  // Get destroy Device Handle interface address
//  DLLDestroyHandle = (DLL_DestroyHandle)dlsym(libHandle, "IMV_DestroyHandle");
//  if (NULL == DLLDestroyHandle)
//  {
//    printf("Get IMV_DestroyHandle address failed!\n");
//    return 0;
//  }
//
//  // 获取打开相机接口函数地址 
//  // Get open camera interface address
//  DLLOpen = (DLL_Open)dlsym(libHandle, "IMV_Open");
//  if (NULL == DLLOpen)
//  {
//    printf("Get IMV_Open address failed!\n");
//    return 0;
//  }
//
//  // 获取注册数据帧回调接口函数地址 
//  // 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 0;
//  }
//
//  // 获取开始拉流接口函数地址 
//  // Get start grabbing interface address
//  DLLStartGrabbing = (DLL_StartGrabbing)dlsym(libHandle, "IMV_StartGrabbing");
//  if (NULL == DLLStartGrabbing)
//  {
//    printf("Get IMV_StartGrabbing address failed!\n");
//    return 0;
//  }
//
//  // 获取停止拉流接口函数地址 
//  // Get stop grabbing interface address
//  DLLStopGrabbing = (DLL_StopGrabbing)dlsym(libHandle, "IMV_StopGrabbing");
//  if (NULL == DLLStopGrabbing)
//  {
//    printf("Get IMV_StopGrabbing address failed!\n");
//    return 0;
//  }
//
//  // 获取
//
//  // 获取关闭相机接口函数地址 
//  // Get close camera interface address
//  DLLClose = (DLL_Close)dlsym(libHandle, "IMV_Close");
//  if (NULL == DLLClose)
//  {
//    printf("Get IMV_Close address failed!\n");
//    return 0;
//  }
//
//  // 获取获取一帧图像函数地址 
//  DLLGetFrame = (DLL_GetFrame)dlsym(libHandle, "IMV_GetFrame");
//  if (NULL == DLLGetFrame)
//  {
//    printf("Get IMV_GetFrame address failed!\n");
//    return 0;
//  }
//
//  DLLReleaseFrame = (DLL_ReleaseFrame)dlsym(libHandle, "IMV_ReleaseFrame");
//  if (NULL == DLLReleaseFrame)
//  {
//    printf("Get IMV_ReleaseFrame address failed!\n");
//    return 0;
//  }
//
//  DLLClearFrameBuffer = (DLL_ClearFrameBuffer)dlsym(libHandle, "IMV_ClearFrameBuffer");
//  if (NULL == DLLClearFrameBuffer)
//  {
//    printf("Get IMV_ClearFrameBuffer address failed!\n");
//    return 0;
//  }
//
//  DLLExecuteCommandFeature = (DLL_ExecuteCommandFeature)dlsym(libHandle, "IMV_ExecuteCommandFeature");
//  if (NULL == DLLExecuteCommandFeature)
//  {
//    printf("Get IMV_ExecuteCommandFeature address failed!\n");
//    return 0;
//  }
//
//  DLLSetIntFeatureValue = (DLL_SetIntFeatureValue)dlsym(libHandle, "IMV_SetIntFeatureValue");
//  if (NULL == DLLSetIntFeatureValue)
//  {
//    printf("Get IMV_SetIntFeatureValue address failed!\n");
//    return 0;
//  }
//
//  DLLSetDoubleFeatureValue = (DLL_SetDoubleFeatureValue)dlsym(libHandle, "IMV_SetDoubleFeatureValue");
//  if (NULL == DLLSetDoubleFeatureValue)
//  {
//    printf("Get IMV_SetDoubleFeatureValue address failed!\n");
//    return 0;
//  }
//  
//
//////////////////////// 检查接口结束
//		// 发现设备 
//		// discover camera 
//  IMV_DeviceList deviceInfoList;
//  ret = DLLEnumDevices(&deviceInfoList, interfaceTypeAll);
//  if (IMV_OK != ret)
//  {
//    printf("Enumeration devices failed! ErrorCode[%d]\n", ret);
//    return 0;
//  }
//
//  if (deviceInfoList.nDevNum < 1)
//  {
//    printf("no camera\n");
//    return 0;
//  }
//
//  // 打印相机基本信息（序号,类型,制造商信息,型号,序列号,用户自定义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 0;
//  }
//
//  // 打开相机 
//  ret = DLLOpen(devHandle);
//  if (IMV_OK != ret)
//  {
//    printf("Open camera failed! ErrorCode[%d]\n", ret);
//    return 0;
//  }
//
//  // 设置软触发模式
//  ret = setSoftTriggerConf(libHandle, devHandle);
//  if (IMV_OK != ret)
//  {
//    printf("setSoftTriggerConf failed! ErrorCode[%d]\n", ret);
//    return 0;
//  }
//
//  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", 12*10000);
//	if (IMV_OK != ret)
//	{
//		printf("Set feature value failed! ErrorCode[%d]\n", ret);
//		return ret;
//	}
//
//  // // 非多线程，不需要注册回调
//  // // 注册数据帧回调函数
//  // // Register data frame callback function
//  // ret = DLLAttachGrabbing(devHandle, onGetFrame, NULL);
//  // if (IMV_OK != ret)
//  // {
//  //   printf("Attach grabbing failed! ErrorCode[%d]\n", ret);
//  //   return 0;
//  // }
//
//  // 开始拉流 
//  // Start grabbing 
//  ret = DLLStartGrabbing(devHandle);
//  if (IMV_OK != ret)
//  {
//    printf("Start grabbing failed! ErrorCode[%d]\n", ret);
//    return 0;
//  }
//
//  /////////////////////////获取一帧图像////////////////////////////
//
//  // 清除帧数据缓存
//	// Clear frame buffer
//	ret = DLLClearFrameBuffer(devHandle);
//	if (IMV_OK != ret)
//	{
//		printf("Clear frame buffer failed! ErrorCode[%d]\n", ret);
//		return ret;
//	}
//
//	// 执行软触发
//	// Execute soft trigger 
//	ret = DLLExecuteCommandFeature(devHandle, "TriggerSoftware");
//	if (IMV_OK != ret)
//	{
//		printf("Execute TriggerSoftware failed! ErrorCode[%d]\n", ret);
//		return ret;
//	}
//
//
//  // 获取一帧图像, TIMEOUT 5000ms
//  ret = DLLGetFrame(devHandle, &frame, 5000);
//  if (IMV_OK != ret)
//  {
//    printf("Get frame failed! ErrorCode[%d]\n", ret);
//    return 0;
//  }
//
//  printf("width %d\n", frame.frameInfo.width);
//  printf("Height %d\n", frame.frameInfo.height);
//
//  ret = imageConvert(libHandle, devHandle, frame, gvspPixelBGR8);
//  if (IMV_OK != ret)
//  {
//    printf("imageConvert failed! ErrorCode[%d]\n", ret);
//    return 0;
//  }
//  // printf("11111111111111\n", ret);
//  // cv::Mat im(frame.frameInfo.width, frame.frameInfo.height, CV_8UC3, frame.pData);
//  // cv::imwrite("/home/caowei/catkin_ws/output.png", im);
//  // cv::imwrite("output.png", im);
//  // printf("22222222222222\n", ret);
//  // // 向ros送 /image消息
//  // sendToRos(frame);
//
//  // 释放图像缓存
//  ret = DLLReleaseFrame(devHandle, &frame);
//  if (IMV_OK != ret)
//  {
//    printf("Release frame failed! ErrorCode[%d]\n", ret);
//    return 0;
//  }
//  ////////////////////////////////////////////////////////////////
//
//  // // 取图2秒
//  // // get frame 2 seconds
//  // sleep(2000);
//
//  // 停止拉流 
//  // Stop grabbing 
//  ret = DLLStopGrabbing(devHandle);
//  if (IMV_OK != ret)
//  {
//    printf("Stop grabbing failed! ErrorCode[%d]\n", ret);
//    return 0;
//  }
//
//  // 关闭相机
//  // Close camera 
//  ret = DLLClose(devHandle);
//  if (IMV_OK != ret)
//  {
//    printf("Close camera failed! ErrorCode[%d]\n", ret);
//    return 0;
//  }
//
//	// 销毁设备句柄
//	// Destroy Device Handle
//	if (NULL != devHandle)
//	{
//		// 销毁设备句柄
//		// Destroy Device Handle
//		DLLDestroyHandle(devHandle);
//	}
//
//	if (NULL != libHandle)
//	{
//		dlclose(libHandle);
//	}
//
//	printf("end...\n");
//	// getchar();
//
//	return 0;
//}
  
}