#include "HCamDevice.h"
#include <stdint.h>
#include <thread>
#include <chrono>
#include <string>
#include <poll.h>
#include <sys/epoll.h>
#include <unistd.h>
#include <fcntl.h>
#include <sys/mman.h>
#include <sys/ioctl.h>
#include <string.h>
//#include "../cameraConfig.h"
//#include "../CameraParams.h"
#include <linux/v4l2-subdev.h>
//#include "camconfig.h"
#include <iostream>
#include <errno.h>

#define ADC_82V48

#define HT_CAM_REG_CR_STARTSAMPLE_MASK 0x00000001
#define HT_CAM_REG_CR_STOPSAMPLE_MASK 0x00000002
#define HT_CAM_REG_CR_CHANGE_MASK 0x00020000


#define camera_print(...) (printf("L%d(%s):", __LINE__, __FILE__), \
						   printf(__VA_ARGS__))

#define camera_dbg(...) (printf("L%d(%s):", __LINE__, __FILE__), \
						 printf(__VA_ARGS__))

#define camera_err(...) (printf("L%d(%s):", __LINE__, __FILE__), \
						 printf(__VA_ARGS__))

#define CLEAR(x) memset(&(x), 0, sizeof(x))

#pragma pack(1)

#ifdef ADC_82V38
#pragma pack(1)
typedef struct ADC_82V38_Timing
{
	uint32_t regData : 9;
	uint32_t : 3;
	uint32_t regAddr : 3;
	uint32_t rwbit : 1;
	uint32_t regData1 : 9;
	uint32_t : 3;
	uint32_t regAddr1 : 3;
	uint32_t rwbit1 : 1;
} adcTiming;
#endif

#ifdef ADC_82V48
struct adcTiming
{
	uint32_t regData : 8;
	uint32_t : 2;
	uint32_t regAddr : 5;
	uint32_t rwbit : 1;
	uint32_t regData1 : 8;
	uint32_t : 2;
	uint32_t regAddr1 : 5;
	uint32_t rwbit1 : 1;
};
#endif

#define V4L2_EVENT_HTCAMMOD_CLASS (V4L2_EVENT_PRIVATE_START | 0x1000)
#define V4L2_EVENT_HTCAMMOD_OVERFREP (V4L2_EVENT_HTCAMMOD_CLASS | 0X1)

extern int errno;

HCamDevice::HCamDevice()
{
	virBaseAddr = NULL;
	memfd = 0;
	subDeviceName = "/dev/v4l-subdev0";
	subDevicefd = 0;
	videofd = 0;
	v4lWidth = 3100;
	v4lHeight = 3100;
	v4lBufferCount = 3;
	v4l2buftype = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
	nplanes = 1; // only use one plane
	drivertype = V4L2_CAP_VIDEO_CAPTURE_MPLANE;
	v4l2memtype = V4L2_MEMORY_MMAP;

	captureBufers = NULL;

	init_ps_regs();
	init_dev();
	uint8_t val = 0;
	for (int i = 0; i < 20; i++)
	{
		HtCamReadADCReg(i, &val);
		camera_print("ADDR: 0x%x, Value: 0x%x\n", i, val);
	}

	this->event_thread.reset(new std::thread(&HCamDevice::HtCamEventWorkThread, this));
}

int HCamDevice::open_video(int width,int height)
{
	int fd = 0;
	if ((fd = open(videoDevName.c_str(), O_RDWR, 0)) == -1)
	{
		camera_dbg("open video fial\n");
		return -1;
	}
	videofd = fd;
	camera_dbg("open video succeed\n");

	set_size(width,height);
	return 0;
}
int HCamDevice::close_video()
{
	if (!videofd)
	{
		return -1;
	}
	close(videofd);
	camera_dbg("close video succeed\n");
	return 0;
}
HCamDevice::~HCamDevice()
{
	HtCamExitVideoCapturing();
	if (this->event_thread->joinable())
		this->event_thread->join();
}

int HCamDevice::HtCamEventWorkThread(void)
{
	struct pollfd pfd;
	pfd.fd = subDevicefd;
	pfd.events = POLLPRI;

	struct v4l2_event ev;

	while (true)
	{
		if (pfd.fd > 0)
		{
			// camera_print("POLLING ON SUB-DEV \n");
			if (poll(&pfd, 1, -1) > 0)
			{
				while (!ioctl(subDevicefd, VIDIOC_DQEVENT, &ev))
				{
					if (this->getCameraEvents)
					{
						this->getCameraEvents(ev.type);
					}
					else
					{
						switch (ev.type)
						{
						case V4L2_EVENT_HTCAMMOD_OVERFREP:
							camera_print("Get Sub-dev event:0x%08x \n", ev.type);
							break;
						default:
							break;
						}
					}
				}
			}
		}
	}

	return 0;
}

// void HCamDevice::HtCamStartVideoCapturing()
// {
// 	uint8_t n_buffers;
// 	struct v4l2_buffer buf;
// 	if (((CAM_INFO_REG *)&pPsReg[CAM_INFO])->cam_run_status)
// 	{
// 		printf("Run the scan multiple times %d\n", ((CAM_INFO_REG *)&pPsReg[CAM_INFO])->cam_run_status);
// 		return ;
// 	}
// 	for (n_buffers = 0; n_buffers < v4lBufferCount; n_buffers++)
// 	{
// 		memset(&buf, 0, sizeof(buf));
// 		if (drivertype == V4L2_CAP_VIDEO_CAPTURE_MPLANE)
// 			buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
// 		else
// 			buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
// 		buf.memory = V4L2_MEMORY_MMAP;
// 		buf.index = n_buffers;
// 		if (drivertype == V4L2_CAP_VIDEO_CAPTURE_MPLANE)
// 		{
// 			buf.length = nplanes;
// 			buf.m.planes = (struct v4l2_plane *)calloc(buf.length, sizeof(struct v4l2_plane));
// 		}

// 		if (ioctl(videofd, VIDIOC_QBUF, &buf) == -1)
// 		{
// 			((CAM_INFO_REG *)&pPsReg[CAM_INFO])->cam_run_status = 0;
// 			camera_err(" VIDIOC_QBUF error\n");

// 			if (drivertype == V4L2_CAP_VIDEO_CAPTURE_MPLANE)
// 				free(buf.m.planes);
// 			free(captureBufers);
// 			close(videofd);
// 			return;
// 		}
// 		if (drivertype == V4L2_CAP_VIDEO_CAPTURE_MPLANE)
// 			free(buf.m.planes);
// 	}

// 	enum v4l2_buf_type type;
// 	if (drivertype == V4L2_CAP_VIDEO_CAPTURE_MPLANE)
// 		type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
// 	else
// 		type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
// 	if (ioctl(videofd, VIDIOC_STREAMON, &type) == -1)
// 	{
// 		((CAM_INFO_REG *)&pPsReg[CAM_INFO])->cam_run_status = 0;
// 		camera_err(" VIDIOC_STREAMON error! %s\n", strerror(errno));
// 		return;
// 	}
// 	else
// 	{
// 		((CAM_INFO_REG *)&pPsReg[CAM_INFO])->cam_run_status = 1;
// 		camera_print(" stream on succeed\n");
// 	}
// 	return;
// }

// void HCamDevice::HtCamStopVideoCapturing()
// {
// 	enum v4l2_buf_type type;
// 	((CAM_INFO_REG *)&pPsReg[CAM_INFO])->cam_run_status = 0;
// 	if (drivertype == V4L2_CAP_VIDEO_CAPTURE_MPLANE)
// 		type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
// 	else
// 		type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
// 	if (ioctl(videofd, VIDIOC_STREAMOFF, &type) == -1)
// 		camera_err(" VIDIOC_STREAMOFF error! %s\n", strerror(errno));
// }

void HCamDevice::HtCamExitVideoCapturing()
{
	//HtCamStopVideoCapturing();

	uint8_t i;
	for (i = 0; i < v4lBufferCount; ++i)
	{
		if (-1 == munmap(captureBufers[i].start, captureBufers[i].length))
			printf("munmap \n");
	}
	if(close(videofd)<0)
	{
		camera_err("close video fd error \n");
	}
	if(close(memfd)<0)
	{
		camera_err("close mem fd error \n");
	}
	if(close(subDevicefd)<0)
	{
		camera_err("close sub Device fd error \n");
	}
}

int HCamDevice::HtCamWaitVideoCapture(int msTimeout)
{
	struct pollfd pfd;
	pfd.fd = videofd;
	pfd.events = POLLIN | POLLRDNORM;

	if (poll(&pfd, 1, msTimeout) > 0)
		return pfd.revents;

	return 0;
}

int HCamDevice::HtCamReadCaptureFrame(void **pbuf, int timeout)
{
	if (!HtCamWaitVideoCapture(timeout))
	{
		camera_err("read frame time out\n");
		if ((lastSucceedBufferIndex + 1) >= v4lBufferCount)
			*pbuf = captureBufers[0].start;
		else
			*pbuf = captureBufers[lastSucceedBufferIndex + 1].start;
		return -1;
	}
	struct v4l2_buffer buf;
	CLEAR(buf);
	if (drivertype == V4L2_CAP_VIDEO_CAPTURE_MPLANE)
		buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
	else
		buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;

	buf.memory = V4L2_MEMORY_MMAP;

	if (drivertype == V4L2_CAP_VIDEO_CAPTURE_MPLANE)
	{
		buf.length = nplanes;
		buf.m.planes = (struct v4l2_plane *)calloc(nplanes, sizeof(struct v4l2_plane));
	}

	if (ioctl(videofd, VIDIOC_DQBUF, &buf) == 0)
		camera_dbg("*****DQBUF[%d] FINISH*****\n", buf.index);
	else
	{
		camera_err("****DQBUF FAIL*****\n");
		return -2;
	}

	if (ioctl(videofd, VIDIOC_QBUF, &buf) == 0)
		camera_dbg("************QBUF[%d] FINISH**************\n", buf.index);
	else
	{
		return -2;
	}
	lastSucceedBufferIndex = buf.index;
	*pbuf = captureBufers[buf.index].start;

	if (drivertype == V4L2_CAP_VIDEO_CAPTURE_MPLANE)
	{
		free(buf.m.planes);
	}
	return buf.index;
}

void HCamDevice::HtCamChangeAdjustSpTime(uint32_t sp_time_gap, uint32_t sp_time_rw)
{
	uint32_t *pCamCtrlReg = virBaseAddr;

	sp_time_gap = (sp_time_gap << 16);
	pCamCtrlReg[3] |= (uint32_t)(sp_time_rw);
	pCamCtrlReg[3] |= (uint32_t)(sp_time_gap);
}

void HCamDevice::HtCamChangeAdjustPhase(uint16_t times, uint8_t dir)
{
	uint32_t *pCamCtrlReg = virBaseAddr;
	int i;
	if (dir)
		pCamCtrlReg[10] |= (1 << 9);
	else
		pCamCtrlReg[10] &= ~(1 << 9);

	for (i = 0; i < times; i++)
	{
		pCamCtrlReg[10] &= ~(1 << 8);
		pCamCtrlReg[10] |= (1 << 8);
		std::this_thread::sleep_for(std::chrono::milliseconds(1));
		pCamCtrlReg[10] &= ~(1 << 10);
		pCamCtrlReg[10] |= (1 << 10);
	}
}

// This function is prohibited
int HCamDevice::HtCamStartSampling()
{
	uint32_t *CamReg;
	if (virBaseAddr == NULL)
		return -1;
	CamReg = (uint32_t *)virBaseAddr;

	CamReg[10] &= ~(HT_CAM_REG_CR_STARTSAMPLE_MASK);
	CamReg[10] |= (HT_CAM_REG_CR_STARTSAMPLE_MASK);

	return 0;
}

// This function is prohibited
int HCamDevice::HtCamStopSampling()
{
	uint32_t *CamReg;
	if (virBaseAddr == NULL)
		return -1;
	CamReg = (uint32_t *)virBaseAddr;

	CamReg[10] |= (HT_CAM_REG_CR_STOPSAMPLE_MASK);
	CamReg[10] &= ~(HT_CAM_REG_CR_STOPSAMPLE_MASK);

	return 0;
}

int HCamDevice::set_size(int width, int height)
{
	v4lWidth = width;
	v4lHeight = height;
	return 0;
}

int HCamDevice::init_fd()
{
	int fd;
	void *map_base;

	if ((fd = open("/dev/mem", O_RDWR | O_SYNC)) < 0)
	{
		camera_dbg("Cannot open /dev/mem \n");
		return -1;
	}
	memfd = fd;

	map_base = mmap(NULL, AddrMapSize, PROT_READ | PROT_WRITE, MAP_SHARED, memfd, phyBaseAddr);
	if (map_base == NULL)
	{
		camera_dbg("Cannot mmap addr \n");
		close(fd);
		return -1;
	}
	virBaseAddr = (uint32_t *)map_base;


	if ((fd = open(subDeviceName.c_str(), O_RDWR, 0)) == -1)
	{
		camera_dbg("t Cam Cannot open subdevice file\n");
		return -1;
	}
	subDevicefd = fd;
	return 1;
}

int HCamDevice::init_sample()
{
	struct v4l2_subdev_format subdev_fmt;
	int ret;
	memset(&subdev_fmt, 0, sizeof(subdev_fmt));

	subdev_fmt.pad = 0;
	subdev_fmt.which = V4L2_SUBDEV_FORMAT_ACTIVE;
	subdev_fmt.format.width = v4lWidth;
	subdev_fmt.format.height = v4lHeight;

	ret = ioctl(subDevicefd, VIDIOC_SUBDEV_S_FMT, &subdev_fmt);
	if (ret < 0)
		printf("VIDIOC_SUBDEV_S_FMT failed.\n");

	struct v4l2_event_subscription sub;
	memset(&sub, 0, sizeof(sub));
	sub.type = V4L2_EVENT_HTCAMMOD_OVERFREP;
	ret = ioctl(subDevicefd, VIDIOC_SUBSCRIBE_EVENT, &sub);
	if (ret < 0)
		printf("VIDIOC_SUBSCRIBE_EVENT failed.\n");

	return ret;
}

int HCamDevice::init_capture()
{
	struct v4l2_format fmt;
	struct v4l2_control ctrl;
	CLEAR(fmt);
	if (drivertype == V4L2_CAP_VIDEO_CAPTURE_MPLANE)
	{
		fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
		fmt.fmt.pix_mp.width = v4lWidth;
		fmt.fmt.pix_mp.height = v4lHeight;
		fmt.fmt.pix_mp.pixelformat = V4L2_PIX_FMT_GREY;
		fmt.fmt.pix_mp.field = V4L2_FIELD_NONE;
		fmt.fmt.pix_mp.num_planes = nplanes;
	}
	else
	{
		fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
		fmt.fmt.pix.width = v4lWidth;
		fmt.fmt.pix.height = v4lHeight;
		fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_GREY;
		fmt.fmt.pix.field = V4L2_FIELD_NONE;
	}
	int ioctlRet = ioctl(videofd, VIDIOC_S_FMT, &fmt) ;
	if ( ioctlRet < 0)
	{
		camera_err("error %s\n" , strerror(errno));
		camera_err(" setting the data format failed!width %d height %d planes %d \n" , fmt.fmt.pix_mp.width,fmt.fmt.pix_mp.height , fmt.fmt.pix_mp.num_planes );
		camera_print(" fmt.type = %d\n", fmt.type);
		camera_print(" fmt.fmt.pix.width = %d\n", fmt.fmt.pix_mp.width);
		camera_print(" fmt.fmt.pix.height = %d\n", fmt.fmt.pix_mp.height);
		camera_print(" fmt.fmt.pix.field = %d\n", fmt.fmt.pix_mp.field);
		// close(videofd);
		return -1;
	}

	if (drivertype == V4L2_CAP_VIDEO_CAPTURE_MPLANE)
	{
		if (v4lWidth != fmt.fmt.pix_mp.width || v4lHeight != fmt.fmt.pix_mp.height)
			camera_err(" does not support %u * %u\n", v4lWidth, v4lHeight);

		v4lWidth = fmt.fmt.pix_mp.width;
		v4lHeight = fmt.fmt.pix_mp.height;
		camera_print(" VIDIOC_S_FMT succeed\n");
		camera_print(" fmt.type = %d\n", fmt.type);
		camera_print(" fmt.fmt.pix.width = %d\n", fmt.fmt.pix_mp.width);
		camera_print(" fmt.fmt.pix.height = %d\n", fmt.fmt.pix_mp.height);
		camera_print(" fmt.fmt.pix.field = %d\n", fmt.fmt.pix_mp.field);

		if (ioctl(videofd, VIDIOC_G_FMT, &fmt) < 0)
			camera_err(" get the data format failed!\n");

		nplanes = fmt.fmt.pix_mp.num_planes;
	}
	else
	{
		if (v4lWidth != fmt.fmt.pix.width || v4lHeight != fmt.fmt.pix.height)
			camera_err(" does not support %u * %u\n", v4lWidth, v4lHeight);

		v4lWidth = fmt.fmt.pix.width;
		v4lHeight = fmt.fmt.pix.height;
		camera_print(" VIDIOC_S_FMT succeed\n");
		camera_print(" fmt.type = %d\n", fmt.type);
		camera_print(" fmt.fmt.pix.width = %d\n", fmt.fmt.pix.width);
		camera_print(" fmt.fmt.pix.height = %d\n", fmt.fmt.pix.height);
		camera_print(" fmt.fmt.pix.field = %d\n", fmt.fmt.pix.field);
	}

	CLEAR(ctrl);
	ctrl.id = (V4L2_CID_USER_BASE + 0xc121); // V4L2_CID_XILINX_LOW_LATENCY
	ctrl.value = (1 << 2);					 // XVIP_LOW_LATENCY_DISABLE
	if (ioctl(videofd, VIDIOC_S_CTRL, &ctrl) < 0)
	{
		camera_err("Fail to set control:%s.\n", strerror(errno));
	}
	return 0;
}

int HCamDevice::init_video_buffer()
{
	struct v4l2_requestbuffers req;
	struct v4l2_buffer buf;
	CLEAR(req);
	req.count = v4lBufferCount;
	if (drivertype == V4L2_CAP_VIDEO_CAPTURE_MPLANE)
		req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
	else
		req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	req.memory = v4l2memtype;
	if (ioctl(videofd, VIDIOC_REQBUFS, &req) < 0)
	{
		camera_err(" VIDIOC_REQBUFS failed\n");
		close(videofd);
		return -1;
	}

	v4lBufferCount = req.count;
	camera_dbg(" reqbuf number is %d\n", v4lBufferCount);

	if (captureBufers != NULL)
		free(captureBufers);
	captureBufers = (captureBuffer *)calloc(v4lBufferCount, sizeof(struct captureBuffer));

	uint8_t n_buffers;
	for (n_buffers = 0; n_buffers < req.count; ++n_buffers)
	{
		CLEAR(buf);
		if (drivertype == V4L2_CAP_VIDEO_CAPTURE_MPLANE)
			buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
		else
			buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
		buf.memory = v4l2memtype;
		buf.index = n_buffers;
		if (drivertype == V4L2_CAP_VIDEO_CAPTURE_MPLANE)
		{
			buf.length = nplanes;
			buf.m.planes = (struct v4l2_plane *)calloc(buf.length, sizeof(struct v4l2_plane));
		}

		if (ioctl(videofd, VIDIOC_QUERYBUF, &buf) == -1)
		{
			camera_err(" VIDIOC_QUERYBUF error\n");
			if (drivertype == V4L2_CAP_VIDEO_CAPTURE_MPLANE)
				free(buf.m.planes);
			free(captureBufers);
			close(videofd);
			return -1;
		}

		if (drivertype == V4L2_CAP_VIDEO_CAPTURE_MPLANE)
		{

			captureBufers[n_buffers].length = buf.m.planes[0].length;
			captureBufers[n_buffers].start = mmap(NULL,
												  buf.m.planes[0].length,
												  PROT_READ /*|PROT_WRITE*/,
												  MAP_SHARED, videofd,
												  buf.m.planes[0].m.mem_offset);

			camera_dbg(" map buffer index: %d, mem: %p, len: %x, offset: %x\n",
					   n_buffers, captureBufers[n_buffers].start, buf.m.planes[0].length,
					   buf.m.planes[0].m.mem_offset);
			free(buf.m.planes);
		}
		else
		{
			captureBufers[n_buffers].length = buf.length;
			captureBufers[n_buffers].start = mmap(NULL,
												  buf.length,
												  PROT_READ /*| PROT_WRITE*/,
												  MAP_SHARED, videofd,
												  buf.m.offset);
			camera_dbg(" map buffer index: %d, mem: %p, len: %x, offset: %x\n",
					   n_buffers, captureBufers[n_buffers].start, buf.length, buf.m.offset);
		}
	}
	lastSucceedBufferIndex = (n_buffers - 1);
	return 1;
}

int HCamDevice::init_dev()
{
	init_fd();
	init_sample();
	init_capture();
	
	init_video_buffer();
	// TestPattern 1
	HtCamSwitchSampleModes(0);
	// HtCamChangeExposureValue(500);
	// start sample
	// camera_dbg("ST SP : %d , VSNP : %d \r\n" ,CamProperty::ST_SP  ,CamProperty::VSNP);
	// if(CamProperty::ST_SP != 0  ||  CamProperty::ST_SP_VSNP != 0)
	// 	HtCamChangeMonoStartSample(CamProperty::ST_SP * 3 + CamProperty::ST_SP_VSNP);
	// if(CamProperty::VSNP != 0)
	// {
	// 	unsigned int value = HtCamReadFpgaRegs(16);
	// 	value = value & 0xffffff00;
	// 	HtCamWriteFpgaRegs(16, CamProperty::VSNP | value);
	// }	
	// ex_trigger = 0, int_trigger = 1
	HtCamChangeTriggerInAndEXt(1);

	HtCamInitADCReg();
	HtCamWriteAllADC();
	return 0;
}

void HCamDevice::HtCamSwitchSampleModes(uint8_t mode)
{
	uint32_t *pCamCtrlReg = virBaseAddr;

	if (!mode)
		pCamCtrlReg[4] &= ~(0x00020000);
	else
		pCamCtrlReg[4] |= (0x00020000);
}

void HCamDevice::HtCamChangeExposureValue(uint32_t value)
{
	uint32_t *pCamCtrlReg = virBaseAddr;

	//  pCamCtrlReg[3] |= (uint32_t)(0x00006000);
	pCamCtrlReg[5] = 0x00000000;
	pCamCtrlReg[6] = 0x00000000;

	pCamCtrlReg[5] |= (uint32_t)(value); // RED
	pCamCtrlReg[5] |= (uint32_t)(value << 16);
	pCamCtrlReg[6] |= (uint32_t)(value);	   // GREEN
	pCamCtrlReg[6] |= ((uint32_t)value << 16); // BLUE
}

void HCamDevice::HtCamWriteADCReg(uint8_t addr, uint8_t data)
{
	uint32_t *pCamCtrlReg = virBaseAddr;
	uint32_t AdcRegFrame = 0x0000;
	uint32_t EnableAdcWrite = 0x4000;
	adcTiming *pAdcRegFrame = (adcTiming *)&AdcRegFrame;

	// if (addr > 20)
	// 	return;
	pADCReg[addr] = data;
	pAdcRegFrame->rwbit = 0;
	pAdcRegFrame->regAddr = addr;
	pAdcRegFrame->regData = data;

	pAdcRegFrame->rwbit1 = 0;
	pAdcRegFrame->regAddr1 = addr;
	pAdcRegFrame->regData1 = data;

	pCamCtrlReg[7] = (uint32_t)AdcRegFrame;

	pCamCtrlReg[4] &= ~(EnableAdcWrite);
	std::this_thread::sleep_for(std::chrono::milliseconds(1));
	pCamCtrlReg[4] |= (EnableAdcWrite);
	std::this_thread::sleep_for(std::chrono::milliseconds(1));
}

void HCamDevice::HtCamReadADCReg(uint8_t addr, uint8_t *data)
{
	uint32_t *pCamCtrlReg = virBaseAddr;
	uint32_t AdcRegFrame = 0x0000;
	uint32_t EnableAdcWrite = (1 << 14);
	uint32_t tempData;
	adcTiming *pAdcRegFrame = (adcTiming *)&AdcRegFrame;

	// if (addr > 0x14)
	// 	return;
	pAdcRegFrame->rwbit = 1;
	pAdcRegFrame->regAddr = addr;
	pAdcRegFrame->regData = 0;
	pAdcRegFrame->rwbit1 = 1;
	pAdcRegFrame->regAddr1 = addr;
	pAdcRegFrame->regData1 = 0;

	pCamCtrlReg[7] = (uint32_t)AdcRegFrame;
	pCamCtrlReg[4] &= ~(EnableAdcWrite);
	std::this_thread::sleep_for(std::chrono::milliseconds(5));
	pCamCtrlReg[4] |= (EnableAdcWrite);
	std::this_thread::sleep_for(std::chrono::milliseconds(10));

	tempData = pCamCtrlReg[2];
	uint8_t value = (tempData >> 4) & (0xFF);
	(*data) = value;
}

uint8_t HCamDevice::getADCReg(int addr)
{
	if(addr > adcRegSize)
		return 0 ;
	return pADCReg[addr];
}

void HCamDevice::setADCReg(int addr , uint8_t value)
{
	if(addr > adcRegSize)
		return ;
	pADCReg[addr] = value ;
}

void HCamDevice::saveADCReg()
{
	// CamADCConfig adcConfig(CamProperty::_ADCPATH);
	// int size = adcConfig.getSize();
	// size = adcRegSize < size ? adcRegSize : size ;
	// std::vector<int> adcValue(size) ;
	// for(int index =0 ; index < size ; index++)
	// {
	// 	adcValue[index] = pADCReg[index];
	// }
	// if(!adcConfig.saveValue(adcValue))
	// {
	// 	std::cout << "Save adc Value Error!" << std::endl ;
	// }
}

uint32_t HCamDevice::HtCamReadFpgaRegs(uint8_t reg_addr)
{
	uint32_t *pCamCtrlReg = virBaseAddr;
	unsigned int u =  pCamCtrlReg[reg_addr] ;
	// FILE *fp;
	// fp = fopen("/home/root/logF", "a+");
	// fprintf(fp , "read reg %d , value :  %d\r\n" ,reg_addr , u );
	// fclose(fp);
	// outFile << "PC read reg  " << (int)reg_addr <<" value : " << u << std::endl;
	return pCamCtrlReg[reg_addr] ;
}

void HCamDevice::HtCamWriteFpgaRegs(uint8_t reg_addr, uint32_t reg_value)
{
	uint32_t reg4 = 0;
	// FILE *fp;
	// fp = fopen("/home/root/logW", "a+");
	// fprintf(fp , "W reg %d , value :  %d\r\n" ,reg_addr , reg_value );
	// fclose(fp);
	// switch (reg_addr)
	// {
	// case 0x04:
	// 	reg4 = HtCamReadFpgaRegs(0x04);
	// 	if (((CamZ_Reg_4 *)&reg4)->color_mode != ((CamZ_Reg_4 *)&reg_value)->color_mode)
	// 	{
	// 		getImgFormat(((CamZ_Reg_4 *)&reg_value)->color_mode);
	// 	}
	// 	break;
	// default:
	// 	break;
	// }
	uint32_t *pCamCtrlReg = virBaseAddr;
	pCamCtrlReg[reg_addr] = reg_value;
}

void HCamDevice::HtCamChangeMonoStartSample(int start_sample)
{
	uint32_t *pCamCtrlReg = virBaseAddr;
	uint32_t ss = start_sample;

	pCamCtrlReg[13] &= ~(0xFFFF0000); // clear
	pCamCtrlReg[13] |= (uint32_t)(ss << 16);
}

void HCamDevice::HtCamChangeTriggerInAndEXt(int in_out)
{
	uint32_t *pCamCtrlReg = virBaseAddr;

	if (in_out)
	{
		pCamCtrlReg[10] |= (0x00000040);
	}
	else
	{
		pCamCtrlReg[10] &= ~(0x00000040);
	}
}

void HCamDevice::HtCamChangeDpi(int dpi)
{
	uint32_t *pCamCtrlReg = virBaseAddr;
	if (dpi)
	{
		pCamCtrlReg[10] |= (0x00000800);
	}
	else
	{
		pCamCtrlReg[10] &= ~(0x00000800);
	}
}

void HCamDevice::HtCamSetTriggerMode(int val)
{
	uint32_t *pCamCtrlReg = virBaseAddr;
	if (val)
		pCamCtrlReg[10] |= 0x00000040;  //40是16进制 转换为二进制0100 0000 刚好就是第十个寄存器的第七位
	else
		pCamCtrlReg[10] &= ~(0x00000040);
}

int HCamDevice::HtCamGetTriggerMode()
{
	uint32_t *pCamCtrlReg = virBaseAddr;
	return ((pCamCtrlReg[10] & (0x00000040)) >> 6);
}

int HCamDevice::HtCamGetColorMode()
{
	uint32_t *pCamCtrlReg = virBaseAddr;
	return ((pCamCtrlReg[4] & (0x00000004)) >> 2);
}

// PERJECT_VERSION getByName(std::string name)
// {
// 	if(name == "PYTHONZ_1200")
// 		return PERJECT_VERSION::PythonZ1200 ;
// 	if(name == "PYTHONZ_1800")
// 		return PERJECT_VERSION::PythonZ1800 ;
// 	if(name == "PYTHONZ_WSS")
// 		return PERJECT_VERSION::PythonZWSS;
// 	if(name == "PYTHONZ_1206" )
// 		return PERJECT_VERSION::PythonZ1206;
// 	if(name == "PYTHONZ_1602" )
// 		return PERJECT_VERSION::PythonZ1602;
// 	return PERJECT_VERSION::PythonZ1200; //return a default var
// }

void HCamDevice::init_ps_regs()
{
	//CamPSConfig psConfig(CONFIG_PATH);
	// memset(pPsReg, 0, sizeof(pPsReg));
	// //pPsReg[PS_VERSION] = 100000;
	// pPsReg[BUF_WIDTH] = v4lWidth;
	// pPsReg[BUF_HEIGHT] = CamProperty::_V4LHEIGHT;
	// pPsReg[IMG_WIDTH] = CamProperty::_IMG_BUF_WIDTH;
	// std::cout << "v4lBufferCount "  << v4lBufferCount << std::endl;
	// pPsReg[BUF_NUM] = v4lBufferCount;
	// pPsReg[CAM_STATUS] = psConfig.getReg(CAM_STATUS);
	// CAM_INFO_REG *camInfo = (CAM_INFO_REG *)&pPsReg[CAM_INFO];
	// camInfo->CIS_Width = pPsReg[IMG_WIDTH] / CamProperty::_CIS_NUM;
	// camInfo->CIS_count = CamProperty::_CIS_NUM;
	// camInfo->ProjectVersion = getByName(CamProperty::_CAM_TYPE);
	// camInfo->vidio_status = 0;
	// pPsReg[RLS_CHECK_CODE] = Alpha;
	// pPsReg[DATA_STATUS] = 0;
	// pPsReg[HEARTBAT] = 0x80000001;

	// CamPSConfig psConfig(CONFIG_PATH);
	// pPsReg[PS_VERSION] = psConfig.getReg(PS_VERSION);
	// pPsReg[CAM_STATUS] = psConfig.getReg(CAM_STATUS);
	// std::cout << "pPsReg[BUF_NUM]  "  << pPsReg[BUF_NUM]  << std::endl;
	// std::cout << "CAM_STATUS "  << pPsReg[CAM_STATUS] << std::endl;
	
	// pPsReg[MOTOR_SPEED] = psConfig.getReg(MOTOR_SPEED);
	// pPsReg[CAM_DPI]		= psConfig.getReg(CAM_DPI);
	// pPsReg[RLS_CHECK_CODE] = psConfig.getReg(RLS_CHECK_CODE);
	//pPsReg[HEARTBAT]	= psConfig.getReg(HEARTBAT);
}

uint32_t HCamDevice::HtCamReadPsRegs(uint8_t reg_addr)
{
	return pPsReg[reg_addr];
}

#include <iostream>
#include <fstream>
void HCamDevice::HtCamWritePsRegs(uint8_t reg_addr, uint32_t reg_value)
{
	printf("reg addr %d\r\n" ,reg_addr );
	std::cout << "Write Camera  PS reg addr " << reg_addr << "value " << reg_value << std::endl;
	if (reg_addr >= REG_NUM)
		return;
	pPsReg[reg_addr] = reg_value;
	switch (reg_addr)
	{
	// case BUF_WIDTH:
	// case BUF_HEIGHT:
	// case BUF_NUM:
	// 	HtCamResizeBuffer(pPsReg[BUF_WIDTH], pPsReg[BUF_HEIGHT], pPsReg[BUF_NUM]);
	// 	break;
	// case CAM_DPI:
	// 	HtCamPsDpiChange(reg_value);
	// 	break;
	// case CAM_STATUS:
	// {
	// 	// HtCamImageProcessChange();
	// 	// if (getImgProcSize)
	// 	// 	getImgProcSize(reg_value);
	// }
	break;
	default:
		break;
	}
}

void HCamDevice::savePsReg()
{
	// CamPSConfig psConfig(CONFIG_PATH);
	// psConfig.setReg(PS_VERSION 	, pPsReg[PS_VERSION] );
	// psConfig.setReg(CAM_STATUS 	, pPsReg[CAM_STATUS] );
	// psConfig.setReg(MOTOR_SPEED , pPsReg[MOTOR_SPEED]);
	// psConfig.setReg(CAM_DPI 	, pPsReg[CAM_DPI]	 );
	// psConfig.setReg(HEARTBAT 	, pPsReg[HEARTBAT]   );
	// psConfig.setReg(RLS_CHECK_CODE, pPsReg[RLS_CHECK_CODE]);
	// psConfig.save();
}

void HCamDevice::HtCamWriteAllADC()
{
	for (int index = 0; index < adcRegSize; index++)
	{
		HtCamWriteADCReg(index, pADCReg[index]);
	}
}

void HCamDevice::HtCamInitADCReg()
{
	
#ifdef ADC_82V38
	pADCReg[0] = 0x23;
	pADCReg[1] = 0xF3;
	pADCReg[2] = 40;
	pADCReg[3] = 40;
	pADCReg[4] = 40;
	pADCReg[5] = 255;
	pADCReg[6] = 255;
	pADCReg[7] = 255;
	return ;
#endif

#ifdef ADC_82V48
	//CamADCConfig adcConfig(CamProperty::_ADCPATH);
	//int size = adcConfig.getSize();
	//std::cout << " read ADC size : " << size << std::endl ;
	//if (size > 0)
	{
		//for (int index = 0; index < size; index++)
		{
			//pADCReg[index] = adcConfig.getReg(index);
		}
		return;
	}

	pADCReg[0] = 0x07;
	pADCReg[1] = 0x50;

	// /* 1200 cis */
	// if (CamProperty::_CAM_TYPE == "PYTHONZ_1200")
	// {
	// 	/* gain */
	// 	pADCReg[2] = 0x90;
	// 	pADCReg[3] = 0x00;
	// 	pADCReg[4] = 0x90;
	// 	pADCReg[5] = 0x00;
	// 	pADCReg[6] = 0x90;
	// 	pADCReg[7] = 0x00;
	// 	pADCReg[8] = 0x90;
	// 	pADCReg[9] = 0x00;
	// 	pADCReg[0xa] = 0x90;
	// 	pADCReg[0xb] = 0x00;
	// 	pADCReg[0xc] = 0x90;
	// 	pADCReg[0xd] = 0x00;
	// 	/* offset */
	// 	pADCReg[0xe] = 0x58;
	// 	pADCReg[0xf] = 0x5b;
	// 	pADCReg[0x10] = 0x55;
	// 	pADCReg[0x11] = 0x55;
	// 	pADCReg[0x12] = 0x50;
	// 	pADCReg[0x13] = 0x55;
	// }
	// else
	// {		
	// 	/* offset */
	// 	if (CamProperty::_CAM_TYPE == "PYTHONZ_WSS")
	// 	{
	// 		pADCReg[2] = 0x55;
	// 		pADCReg[3] = 0x00;
	// 		pADCReg[4] = 0x4a;
	// 		pADCReg[5] = 0x00;
	// 		pADCReg[6] = 0x50;
	// 		pADCReg[7] = 0x00;
	// 		pADCReg[8] = 0x4e;
	// 		pADCReg[9] = 0x00;
	// 		pADCReg[0xa] = 0x40;
	// 		pADCReg[0xb] = 0x00;
	// 		pADCReg[0xc] = 0x50;
	// 		pADCReg[0xd] = 0x00;
	// 		pADCReg[0xe] = 0x65;
	// 		pADCReg[0xf] = 0x65;
	// 		pADCReg[0x10] = 0x65;
	// 		pADCReg[0x11] = 0x65;
	// 		pADCReg[0x12] = 0x65;
	// 		pADCReg[0x13] = 0x65;
	// 	}
	// 	else
	// 	{
	// 		pADCReg[2] = 0xA0;
	// 		pADCReg[3] = 0x00;
	// 		pADCReg[4] = 0xA0;
	// 		pADCReg[5] = 0x00;
	// 		pADCReg[6] = 0xA0;
	// 		pADCReg[7] = 0x00;
	// 		pADCReg[8] = 0xA0;
	// 		pADCReg[9] = 0x00;
	// 		pADCReg[0xa] = 0xA0;
	// 		pADCReg[0xb] = 0x00;
	// 		pADCReg[0xc] = 0xA0;
	// 		pADCReg[0xd] = 0x00;
	// 		pADCReg[0xe] = 0x20;
	// 		pADCReg[0xf] = 0x20;
	// 		pADCReg[0x10] = 0x20;
	// 		pADCReg[0x11] = 0x20;
	// 		pADCReg[0x12] = 0x20;
	// 		pADCReg[0x13] = 0x20;
	// 	}
	// }
#endif
}

void HCamDevice::HtCamResizeBuffer(int width, int height, int number)
{
	v4lWidth = width;
	v4lHeight = height;
	v4lBufferCount = number;

	HtCamExitVideoCapturing();
	init_fd();
	init_sample();
	init_capture();
	init_video_buffer();

	camera_dbg("v4lWidth = %d, v4lHeight = %d, v4lBufferCount = %d\n", v4lWidth, v4lHeight, v4lBufferCount);
}

void HCamDevice::set_get_imgSize_event(GetImgProcSize _getImgProcSize)
{
	this->getImgProcSize = _getImgProcSize;
}

void HCamDevice::set_get_imgFormat_event(GetImgFormat _getImgFormat)
{
	this->getImgFormat = _getImgFormat;
}

void HCamDevice::set_get_CameraEvents_event(GetCameraEvents _getCameraEvents)
{
	this->getCameraEvents = _getCameraEvents;
}

void HCamDevice::set_reload_correct_event(ReLoadCorrecEvent _reLoadCorrectEvent)
{
	this->reLoadCorrectEvent = _reLoadCorrectEvent;
}

// void HCamDevice::HtCamImageProcessChange()
// {
// 	CAM_STATUS_REG* status = (CAM_STATUS_REG*)&pPsReg[CAM_STATUS];
// 	if(status->doImageProcess)
// 	{
// 		HtCamPsDpiChange(pPsReg[CAM_DPI]);
// 	}else
// 	{
// 		pPsReg[IMG_WIDTH] = pPsReg[BUF_WIDTH];
// 	}
// }

// DPI切换-暂只适配1200/1800
// bool HCamDevice::HtCamPsDpiChange(int dpi)
// {
// 	std::cout << "DPI Change for " << dpi <<std::endl; 
// 	for (auto i = CamProperty::_DPI_WIDTH.begin(); i != CamProperty::_DPI_WIDTH.end(); i++)
// 	{
// 		if (dpi == i->first)
// 		{
// 			CamProperty::_N_DPI = dpi;
// 			CamProperty::_IMG_BUF_WIDTH = i->second;
			

// 			pPsReg[BUF_WIDTH] = CamProperty::_DPI_V4LWIDTH[dpi];
			
// 			if (dpi > 300)
// 			{
// 				HtCamChangeDpi(0);
// 				pPsReg[BUF_HEIGHT] = CamProperty::_V4LHEIGHT/2;
// 			}
// 			else
// 			{
// 				HtCamChangeDpi(1);	
// 				pPsReg[BUF_HEIGHT] = CamProperty::_V4LHEIGHT;		
// 			}
// 			std::cout <<  "image process " <<  ((CAM_STATUS_REG*)&pPsReg[CAM_STATUS])->doImageProcess << std::endl ;
// 			if(((CAM_STATUS_REG*)&pPsReg[CAM_STATUS])->doImageProcess)
// 				pPsReg[IMG_WIDTH] = CamProperty::_IMG_BUF_WIDTH  ;
// 			else
// 				pPsReg[IMG_WIDTH] = pPsReg[BUF_WIDTH];
// 			((CAM_INFO_REG *)&pPsReg[CAM_INFO])->CIS_Width = pPsReg[IMG_WIDTH] / ((CAM_INFO_REG *)&pPsReg[CAM_INFO])->CIS_count;
// 			// 更改数据缓存区大小

// 			std::cout << "LL " << v4lWidth << pPsReg[BUF_WIDTH] << v4lHeight << pPsReg[BUF_HEIGHT] << v4lBufferCount << pPsReg[BUF_NUM] << std::endl ;
// 			if(v4lWidth != pPsReg[BUF_WIDTH] || v4lHeight != pPsReg[BUF_HEIGHT] || v4lBufferCount != pPsReg[BUF_NUM])
// 			{
// 				HtCamResizeBuffer(pPsReg[BUF_WIDTH], pPsReg[BUF_HEIGHT] , pPsReg[BUF_NUM]);
// 				camera_print(" fmt.fmt.pix.width = %d\n", v4lWidth);
// 				camera_print(" fmt.fmt.pix.height = %d\n", v4lHeight);
// 				camera_print(" fmt.fmt.pix.field = %d\n", pPsReg[BUF_NUM]);
// 				if(reLoadCorrectEvent)
// 					reLoadCorrectEvent(CamProperty::_IMG_BUF_WIDTH);
// 			}
// 			if(config != nullptr)
// 				config->saveConfig();
// 			// Camconfig h_config;    
// 			// h_config.saveConfig();
// 			return true;
// 		}
// 	}
// 	return false;
// }

void HCamDevice::HtCamOverClockClear()
{
	uint32_t *pCamCtrlReg = virBaseAddr;
	// camera_print("val0 = 0x%08x\n", pCamCtrlReg[10]);
	pCamCtrlReg[10] |= (0x00001000);
	// camera_print("val1 = 0x%08x\n", pCamCtrlReg[10]);
	std::this_thread::sleep_for(std::chrono::milliseconds(1));
	pCamCtrlReg[10] &= ~(0x00001000);
	// camera_print("val2 = 0x%08x\n", pCamCtrlReg[10]);
}