#include "gvideoisp1.h"
#include <fcntl.h> /* low-level i/o */
#include <unistd.h>
#include <sys/stat.h>
#include <sys/mman.h>
#include <sys/ioctl.h>
#include "linux/videodev2.h"
#include "linux/v4l2-subdev.h"
#include <dlfcn.h>
#include "string.h"
#include <iostream>
#include <malloc.h>

#include <base/utils.h>


static const std::string loggername = "GVideoISP1";

#define ARRAY_SIZE(a)	(sizeof(a)/sizeof((a)[0]))
#define CLEAR(x) memset(&(x), 0, sizeof(x))

#define 	V4L2_BUF_TYPE_META_OUTPUT	   14
#define V4L2_CAP_META_OUTPUT		0x08000000  /* Is a metadata output device */

static struct {
	enum v4l2_buf_type type;
	bool supported;
	const char *name;
	const char *string;
} buf_types[] = {
	{ V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE, 1, "Video capture mplanes", "capture-mplane", },
	{ V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE, 1, "Video output", "output-mplane", },
	{ V4L2_BUF_TYPE_VIDEO_CAPTURE, 1, "Video capture", "capture", },
	{ V4L2_BUF_TYPE_VIDEO_OUTPUT, 1, "Video output mplanes", "output", },
	{ V4L2_BUF_TYPE_VIDEO_OVERLAY, 0, "Video overlay", "overlay" },
	//{ V4L2_BUF_TYPE_META_CAPTURE, 1, "Meta-data capture", "meta-capture", }//,
};

static struct v4l2_format_info {
	const char *name;
	unsigned int fourcc;
	unsigned char n_planes;
} pixel_formats[] = {
	{ "RGB332", V4L2_PIX_FMT_RGB332, 1 },
	{ "RGB444", V4L2_PIX_FMT_RGB444, 1 },
	{ "ARGB444", V4L2_PIX_FMT_ARGB444, 1 },
	{ "XRGB444", V4L2_PIX_FMT_XRGB444, 1 },
	{ "RGB555", V4L2_PIX_FMT_RGB555, 1 },
	{ "ARGB555", V4L2_PIX_FMT_ARGB555, 1 },
	{ "XRGB555", V4L2_PIX_FMT_XRGB555, 1 },
	{ "RGB565", V4L2_PIX_FMT_RGB565, 1 },
	{ "RGB555X", V4L2_PIX_FMT_RGB555X, 1 },
	{ "RGB565X", V4L2_PIX_FMT_RGB565X, 1 },
	{ "BGR666", V4L2_PIX_FMT_BGR666, 1 },
	{ "BGR24", V4L2_PIX_FMT_BGR24, 1 },
	{ "RGB24", V4L2_PIX_FMT_RGB24, 1 },
	{ "BGR32", V4L2_PIX_FMT_BGR32, 1 },
	{ "ABGR32", V4L2_PIX_FMT_ABGR32, 1 },
	{ "XBGR32", V4L2_PIX_FMT_XBGR32, 1 },
	{ "RGB32", V4L2_PIX_FMT_RGB32, 1 },
	{ "ARGB32", V4L2_PIX_FMT_ARGB32, 1 },
	{ "XRGB32", V4L2_PIX_FMT_XRGB32, 1 },
	//{ "HSV24", V4L2_PIX_FMT_HSV24, 1 },
	//{ "HSV32", V4L2_PIX_FMT_HSV32, 1 },
	{ "Y8", V4L2_PIX_FMT_GREY, 1 },
	{ "Y10", V4L2_PIX_FMT_Y10, 1 },
	{ "Y12", V4L2_PIX_FMT_Y12, 1 },
	{ "Y16", V4L2_PIX_FMT_Y16, 1 },
	{ "UYVY", V4L2_PIX_FMT_UYVY, 1 },
	{ "VYUY", V4L2_PIX_FMT_VYUY, 1 },
	{ "YUYV", V4L2_PIX_FMT_YUYV, 1 },
	{ "YVYU", V4L2_PIX_FMT_YVYU, 1 },
	{ "NV12", V4L2_PIX_FMT_NV12, 1 },
	{ "NV12M", V4L2_PIX_FMT_NV12M, 2 },
	{ "NV21", V4L2_PIX_FMT_NV21, 1 },
	{ "NV21M", V4L2_PIX_FMT_NV21M, 2 },
	{ "NV16", V4L2_PIX_FMT_NV16, 1 },
	{ "NV16M", V4L2_PIX_FMT_NV16M, 2 },
	{ "NV61", V4L2_PIX_FMT_NV61, 1 },
	{ "NV61M", V4L2_PIX_FMT_NV61M, 2 },
	{ "NV24", V4L2_PIX_FMT_NV24, 1 },
	{ "NV42", V4L2_PIX_FMT_NV42, 1 },
	{ "YUV420M", V4L2_PIX_FMT_YUV420M, 3 },
	{ "YUV422M", V4L2_PIX_FMT_YUV422M, 3 },
	{ "YUV444M", V4L2_PIX_FMT_YUV444M, 3 },
	{ "YVU420M", V4L2_PIX_FMT_YVU420M, 3 },
	{ "YVU422M", V4L2_PIX_FMT_YVU422M, 3 },
	{ "YVU444M", V4L2_PIX_FMT_YVU444M, 3 },
	{ "SBGGR8", V4L2_PIX_FMT_SBGGR8, 1 },
	{ "SGBRG8", V4L2_PIX_FMT_SGBRG8, 1 },
	{ "SGRBG8", V4L2_PIX_FMT_SGRBG8, 1 },
	{ "SRGGB8", V4L2_PIX_FMT_SRGGB8, 1 },
	{ "SBGGR10_DPCM8", V4L2_PIX_FMT_SBGGR10DPCM8, 1 },
	{ "SGBRG10_DPCM8", V4L2_PIX_FMT_SGBRG10DPCM8, 1 },
	{ "SGRBG10_DPCM8", V4L2_PIX_FMT_SGRBG10DPCM8, 1 },
	{ "SRGGB10_DPCM8", V4L2_PIX_FMT_SRGGB10DPCM8, 1 },
	{ "SBGGR10", V4L2_PIX_FMT_SBGGR10, 1 },
	{ "SGBRG10", V4L2_PIX_FMT_SGBRG10, 1 },
	{ "SGRBG10", V4L2_PIX_FMT_SGRBG10, 1 },
	{ "SRGGB10", V4L2_PIX_FMT_SRGGB10, 1 },
	{ "SBGGR10P", V4L2_PIX_FMT_SBGGR10P, 1 },
	{ "SGBRG10P", V4L2_PIX_FMT_SGBRG10P, 1 },
	{ "SGRBG10P", V4L2_PIX_FMT_SGRBG10P, 1 },
	{ "SRGGB10P", V4L2_PIX_FMT_SRGGB10P, 1 },
	{ "SBGGR12", V4L2_PIX_FMT_SBGGR12, 1 },
	{ "SGBRG12", V4L2_PIX_FMT_SGBRG12, 1 },
	{ "SGRBG12", V4L2_PIX_FMT_SGRBG12, 1 },
	{ "SRGGB12", V4L2_PIX_FMT_SRGGB12, 1 },
	{ "DV", V4L2_PIX_FMT_DV, 1 },
	{ "MJPEG", V4L2_PIX_FMT_MJPEG, 1 },
	{ "MPEG", V4L2_PIX_FMT_MPEG, 1 },
};

static const struct {
	const char *name;
	enum v4l2_field field;
} fields[] = {
	{ "any", V4L2_FIELD_ANY },
	{ "none", V4L2_FIELD_NONE },
	{ "top", V4L2_FIELD_TOP },
	{ "bottom", V4L2_FIELD_BOTTOM },
	{ "interlaced", V4L2_FIELD_INTERLACED },
	{ "seq-tb", V4L2_FIELD_SEQ_TB },
	{ "seq-bt", V4L2_FIELD_SEQ_BT },
	{ "alternate", V4L2_FIELD_ALTERNATE },
	{ "interlaced-tb", V4L2_FIELD_INTERLACED_TB },
	{ "interlaced-bt", V4L2_FIELD_INTERLACED_BT },
};

GVideoISP1::GVideoISP1()
{
	dev_name = "/dev/video0";
}

GVideoISP1::~GVideoISP1()
{
	
}

void* GVideoISP1::read_frame(int timeout, size_t& size, int& ind) {
	size = 0;
	ind = -1;
	if (!wait(timeout)){
		utils::to_log(LOG_LEVEL_FATAL, "GVideoISP1::read_frame time out!!!\n" );
		return 0;
	}
	struct v4l2_plane 	planes[VIDEO_MAX_PLANES];
	v4l2_buffer 		buf;
	
	CLEAR(buf);
	CLEAR(planes);
	buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
	buf.memory = V4L2_MEMORY_USERPTR;
	buf.length = VIDEO_MAX_PLANES;
	buf.m.planes = planes;
	if (ioctl(fd, VIDIOC_DQBUF, &buf) < 0) {
		LOG_TRACE(utils::format_string("Unable to dequeue buffer: %s (%d).\n",strerror(errno), errno));
		return 0;
	}
	
	// move to add_v4l2_memory method ...
	// ret = ioctl(fd, VIDIOC_QBUF, &buf);
	// if (ret < 0) {
	// 	LOG_ERROR(utils::format_string("Unable to requeue buffer: %s (%d).\n",strerror(errno), errno));
	// }
	ind = buf.index;
	size = buffers[ind].length;
	utils::to_log(LOG_LEVEL_DEBUG, "\tvideo buf[%d] = %p + %d\n", ind, buffers[ind].start, size);

	return buffers[ind].start;
}


void GVideoISP1::start_capturing(void) {
	unsigned int i;
	int ret;
	enum v4l2_buf_type type;

	for (i = 0; i < n_buffers; ++i) {
		struct v4l2_buffer buf;
		struct v4l2_plane planes[VIDEO_MAX_PLANES];
		CLEAR(buf);
		CLEAR(planes);
		for(int j = 0; j <  VIDEO_MAX_PLANES; j++)
		{
			//planes[j].length = buffers[i].length;
			planes[j].length = buf_size_;
			planes[j].m.userptr =(unsigned long)buffers[i].start;
		}
		buf.index = i;
		buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
		buf.memory = V4L2_MEMORY_USERPTR;//V4L2_MEMORY_MMAP;
		buf.m.planes = planes;
		buf.length = 1;
		
		ret = ioctl(fd, VIDIOC_QBUF, &buf);
		if (ret < 0)
			LOG_ERROR(utils::format_string("Unable to queue buffer: %s (%d).\n",strerror(errno), errno));
		else
			LOG_TRACE(utils::format_string("buf.index = %d, length = %d, VIDIOC_QBUF sucess.\n", i, buf_size_));
	}
	
	type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
	ret = ioctl(fd, VIDIOC_STREAMON, &type);
	if (ret < 0) {
		LOG_ERROR(utils::format_string("Unable to %s streaming: %s (%d).\n","start", strerror(errno), errno));
	}
	else {
		LOG_TRACE(utils::format_string("VIDIOC_STREAMON sucess.\n"));
	}
}

void GVideoISP1::stop_capturing(void) {
	enum v4l2_buf_type type;

	type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
	if (-1 == ioctl(fd, VIDIOC_STREAMOFF, &type))
		LOG_ERROR(utils::format_string("stream off\n"));

	LOG_TRACE(utils::format_string("gVideo stop_capturing\n"));
}

static void get_ts_flags(uint32_t flags, const char **ts_type, const char **ts_source)
{
	switch (flags & V4L2_BUF_FLAG_TIMESTAMP_MASK) {
	case V4L2_BUF_FLAG_TIMESTAMP_UNKNOWN:
		*ts_type = "unk";
		break;
	case V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC:
		*ts_type = "mono";
		break;
	case V4L2_BUF_FLAG_TIMESTAMP_COPY:
		*ts_type = "copy";
		break;
	default:
		*ts_type = "inv";
	}
	switch (flags & V4L2_BUF_FLAG_TSTAMP_SRC_MASK) {
	case V4L2_BUF_FLAG_TSTAMP_SRC_EOF:
		*ts_source = "EoF";
		break;
	case V4L2_BUF_FLAG_TSTAMP_SRC_SOE:
		*ts_source = "SoE";
		break;
	default:
		*ts_source = "inv";
	}
}

void GVideoISP1::init_mmap(void) {
	// struct v4l2_requestbuffers req;
	// int ret;
	// unsigned int length;
	// unsigned int offset;
	// unsigned int page_size;
	// int buffer_size = v4l_width * v4l_height * 3;
	// page_size = getpagesize();
	// buffer_size = (buffer_size + page_size - 1) & ~(page_size - 1);

	// CLEAR(req);

	// //6.VIDIOC_REQBUFS
	// req.count = v4l_buffer_count;
	// req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
	// req.memory = V4L2_MEMORY_USERPTR;//V4L2_MEMORY_MMAP;

	// ret = ioctl(fd, VIDIOC_REQBUFS, &req);
	// if (ret < 0) {
	// 	LOG_ERROR(utils::format_string("Unable to request buffers: %s (%d).", strerror(errno),errno));
	// }
	// else {
	// 	LOG_TRACE(utils::format_string("%s VIDIOC_REQBUFS sucess",dev_name.c_str()));
	// }
	// LOG_TRACE(utils::format_string("%u buffers requested.", req.count));

	// buffers.resize(req.count);

	
	// for (n_buffers = 0; n_buffers < buffers.size(); ++n_buffers)
	// {
	// 	buffers[n_buffers].length = buffer_size;
	// 	buffers[n_buffers].start = memalign(/* boundary */ page_size, buffer_size);

	// 	if (!buffers[n_buffers].start)
	// 		LOG_ERROR("Out of memory");
	// }

	struct v4l2_requestbuffers req;
	int ret;
	unsigned int length;
	unsigned int offset;
	unsigned int page_size;
	int buffer_size = v4l_width * v4l_height * 3;
	LOG_TRACE(utils::format_string("init_mmap::buduiqi:changdu: buffer_size= %d\n", buffer_size));
	page_size = getpagesize();
	buffer_size = (buffer_size + page_size - 1) & ~(page_size - 1);
	LOG_TRACE(utils::format_string("init_mmap::shijichangdu:v4l_width = %d,v4l_height = %d\n", v4l_width,v4l_height));
	LOG_TRACE(utils::format_string("init_mmap::duiqi:page_size = %d,buffer_size = %d\n", page_size,buffer_size));

	CLEAR(req);

	//6.VIDIOC_REQBUFS
	req.count = v4l_buffer_count;
	//req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
	//req.memory = V4L2_MEMORY_MMAP;
	req.memory = V4L2_MEMORY_USERPTR;
	LOG_TRACE(utils::format_string("set v4l_buffer_count = %d\n", req.count));
	LOG_TRACE(utils::format_string("set v4l_buffer_type = %d\n", req.type));
	LOG_TRACE(utils::format_string("set v4l_buffer_memory = %d\n", req.memory));

	ret = ioctl(fd, VIDIOC_REQBUFS, &req);
	if (ret < 0) {
		LOG_ERROR(utils::format_string("Unable to request buffers: %s (%d).\n", strerror(errno),errno));
	}
	else {
		LOG_TRACE(utils::format_string("%s VIDIOC_REQBUFS sucess\n",dev_name.c_str()));
	}
	LOG_TRACE(utils::format_string("%u buffers requested.\n", req.count));

	buffers.resize(req.count);
	if (buffers.empty()) {
		utils::to_log(LOG_LEVEL_FATAL, "GVideoISP1::init_mmap: Out of memory\n");
	}
	
	//7.VIDIOC_QUERYBUF
	for (n_buffers = 0; n_buffers < req.count; ++n_buffers) {
		const char *ts_type, *ts_source;
		struct v4l2_buffer buf;
		struct v4l2_plane planes[VIDEO_MAX_PLANES];
		CLEAR(buf);
		CLEAR(planes);
		buf.index = n_buffers;
		buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
		//buf.memory = V4L2_MEMORY_MMAP;
		buf.memory = V4L2_MEMORY_USERPTR;
		buf.length = VIDEO_MAX_PLANES;
		buf.m.planes = planes;

		ret = ioctl(fd, VIDIOC_QUERYBUF, &buf);
		if (ret < 0) {
			LOG_ERROR(utils::format_string("Unable to query buffer %u: %s (%d).\n", n_buffers,strerror(errno), errno));

		}
		else {
			LOG_TRACE(utils::format_string("index %d VIDIOC_QUERYBUF sucess.\n",n_buffers));

		}
		get_ts_flags(buf.flags, &ts_type, &ts_source);
		LOG_TRACE(utils::format_string("length: %u offset: %u timestamp type/source: %s/%s\n",
		       buf.length, buf.m.offset, ts_type, ts_source));

		//mmap
		unsigned int length;
		unsigned int offset;
		length = buf.m.planes[0].length;
		offset = buf.m.planes[0].m.mem_offset;
		buf_size_ = buf.m.planes[0].length;
		LOG_TRACE(utils::format_string("VIDIOC_QUERYBUF：Buffer %u length  = %d.\n",n_buffers,length));
		LOG_TRACE(utils::format_string("VIDIOC_QUERYBUF：Buffer %u offset  = %d.\n",n_buffers,offset));
		LOG_TRACE(utils::format_string("VIDIOC_QUERYBUF：Buffer %u querybuf_length  = %d.\n",n_buffers,buf_size_));
	}
	
	for (n_buffers = 0; n_buffers < buffers.size(); ++n_buffers)
	{
		buffers[n_buffers].length = buf_size_;
		buffers[n_buffers].start = memalign(/* boundary */ 1024, buf_size_);
		LOG_TRACE(utils::format_string("memalign:buffers[%d].length = %d\n", n_buffers,buffers[n_buffers].length));
		LOG_TRACE(utils::format_string("memalign:buffers[%d].start = %p\n", n_buffers,buffers[n_buffers].start));

		if (!buffers[n_buffers].start)
		{
			utils::to_log(LOG_LEVEL_FATAL, "GVideoISP1::init_mmap: Out of memory,exit application\n");
			exit(EXIT_FAILURE);
		}
	}
}

static const char *v4l2_buf_type_name(__u32 type)
{
	unsigned int i;

	for (i = 0; i < ARRAY_SIZE(buf_types); ++i) {
		if (buf_types[i].type == type)
			return buf_types[i].name;
	}

	if (type & V4L2_BUF_TYPE_PRIVATE)
		return "Private";
	else
		return "Unknown";
}

static const struct v4l2_format_info *v4l2_format_by_fourcc(unsigned int fourcc)
{
	unsigned int i;

	for (i = 0; i < ARRAY_SIZE(pixel_formats); ++i) {
		if (pixel_formats[i].fourcc == fourcc)
			return &pixel_formats[i];
	}

	return NULL;
}


static const char *v4l2_format_name(unsigned int fourcc)
{
	const struct v4l2_format_info *info;
	static char name[5];
	unsigned int i;

	info = v4l2_format_by_fourcc(fourcc);
	if (info)
		return info->name;

	for (i = 0; i < 4; ++i) {
		name[i] = fourcc & 0xff;
		fourcc >>= 8;
	}

	name[4] = '\0';
	return name;
}

static std::string video_enum_frame_intervals(int fd, __u32 pixelformat,
	unsigned int width, unsigned int height)
{
	struct v4l2_frmivalenum ival;
	unsigned int i;
	int ret;
	std::string fsdesc("");

	for (i = 0; ; ++i) {
		memset(&ival, 0, sizeof ival);
		ival.index = i;
		ival.pixel_format = pixelformat;
		ival.width = width;
		ival.height = height;
		ret = ioctl(fd, VIDIOC_ENUM_FRAMEINTERVALS, &ival);
		if (ret < 0)
			break;

		if (i != ival.index)
			LOG_TRACE(utils::format_string("Warning: driver returned wrong ival index " "%u.\n", ival.index));
		if (pixelformat != ival.pixel_format)
			LOG_TRACE(utils::format_string("Warning: driver returned wrong ival pixel " "format %08x.\n", ival.pixel_format));
		if (width != ival.width)
			LOG_TRACE(utils::format_string("Warning: driver returned wrong ival width " "%u.\n", ival.width));
		if (height != ival.height)
			LOG_TRACE(utils::format_string("Warning: driver returned wrong ival height " "%u.\n", ival.height));

		if (i != 0)
			utils::to_log(LOG_LEVEL_DEBUG, ", ");

		switch (ival.type) {
		case V4L2_FRMIVAL_TYPE_DISCRETE:
			fsdesc = utils::format_string("%u/%u", ival.discrete.numerator, ival.discrete.denominator);
			return std::move(fsdesc);

		case V4L2_FRMIVAL_TYPE_CONTINUOUS:
			fsdesc = utils::format_string("%u/%u - %u/%u", ival.stepwise.min.numerator, ival.stepwise.min.denominator, ival.stepwise.max.numerator, ival.stepwise.max.denominator);
			return std::move(fsdesc);

		case V4L2_FRMIVAL_TYPE_STEPWISE:
			fsdesc = utils::format_string("%u/%u - %u/%u (by %u/%u)", ival.stepwise.min.numerator, ival.stepwise.min.denominator, ival.stepwise.max.numerator, ival.stepwise.max.denominator, ival.stepwise.step.numerator, ival.stepwise.step.denominator);
			return std::move(fsdesc);

		default:
			break;
		}
	}

	return std::move(fsdesc);
}

static void video_enum_frame_sizes(int fd, __u32 pixelformat)
{
	struct v4l2_frmsizeenum frame;
	unsigned int i;
	int ret;

	for (i = 0; ; ++i) {
		memset(&frame, 0, sizeof frame);
		frame.index = i;
		frame.pixel_format = pixelformat;
		ret = ioctl(fd, VIDIOC_ENUM_FRAMESIZES, &frame);
		if (ret < 0)
			break;

		if (i != frame.index)
			LOG_TRACE(utils::format_string("Warning: driver returned wrong frame index " "%u.\n", frame.index));
		if (pixelformat != frame.pixel_format)
			LOG_TRACE(utils::format_string("Warning: driver returned wrong frame pixel " "format %08x.\n", frame.pixel_format));

		switch (frame.type) {
		case V4L2_FRMSIZE_TYPE_DISCRETE:
			LOG_TRACE(utils::format_string("\tFrame size: %ux%u (%s)\n", frame.discrete.width, frame.discrete.height,
				video_enum_frame_intervals(fd, frame.pixel_format, frame.discrete.width, frame.discrete.height).c_str()));
			break;

		case V4L2_FRMSIZE_TYPE_CONTINUOUS:
			LOG_TRACE(utils::format_string("\tFrame size: %ux%u - %ux%u (%s)\n", frame.stepwise.min_width, frame.stepwise.min_height, frame.stepwise.max_width, frame.stepwise.max_height,
				video_enum_frame_intervals(fd, frame.pixel_format, frame.stepwise.max_width, frame.stepwise.max_height).c_str()));
			break;

		case V4L2_FRMSIZE_TYPE_STEPWISE:
			LOG_TRACE(utils::format_string("\tFrame size: %ux%u - %ux%u (by %ux%u) (%s)\n", frame.stepwise.min_width, frame.stepwise.min_height, frame.stepwise.max_width, frame.stepwise.max_height, frame.stepwise.step_width, frame.stepwise.step_height,
			 	video_enum_frame_intervals(fd, frame.pixel_format, frame.stepwise.max_width,  frame.stepwise.max_height).c_str()));
			break;

		default:
			break;
		}
	}
}


static void video_enum_formats(int fd, enum v4l2_buf_type type)
{
	struct v4l2_fmtdesc fmt;
	unsigned int i;
	int ret;

	for (i = 0; ; ++i) {
		memset(&fmt, 0, sizeof fmt);
		fmt.index = i;
		fmt.type = type;
		ret = ioctl(fd, VIDIOC_ENUM_FMT, &fmt);
		if (ret < 0)
			break;

		if (i != fmt.index)
			LOG_TRACE(utils::format_string("Warning: driver returned wrong format index " "%u.\n", fmt.index));
		if (type != fmt.type)
			LOG_TRACE(utils::format_string("Warning: driver returned wrong format type " "%u.\n", fmt.type));

		LOG_TRACE(utils::format_string("Format %u: %s (%08x)\n", i, v4l2_format_name(fmt.pixelformat), fmt.pixelformat));
		LOG_TRACE(utils::format_string("\tType: %s (%u)\n", v4l2_buf_type_name(fmt.type), fmt.type));
		LOG_TRACE(utils::format_string("\tName: %.32s\n", fmt.description));
		video_enum_frame_sizes(fd, fmt.pixelformat);
	}
}

static void video_enum_inputs(int fd)
{
	struct v4l2_input input;
	unsigned int i;
	int ret;

	for (i = 0; ; ++i) {
		memset(&input, 0, sizeof input);
		input.index = i;
		ret = ioctl(fd, VIDIOC_ENUMINPUT, &input);
		if (ret < 0)
			break;

		if (i != input.index)
			LOG_TRACE(utils::format_string("Warning: driver returned wrong input index " "%u.\n", input.index));

		LOG_TRACE(utils::format_string("\tInput %u: %s.\n", i, input.name));
	}

}

static const char *v4l2_field_name(__u32 field)
{
	unsigned int i;

	for (i = 0; i < ARRAY_SIZE(fields); ++i) {
		if (fields[i].field == field)
			return fields[i].name;
	}

	return "unknown";
}


static void rkisp_sd_set_crop(const char *ispsd, int fd, int pad, int *w, int *h)
{
    struct v4l2_subdev_selection sel;
    int ret;

    memset(&sel, 0, sizeof(sel));
    sel.which = V4L2_SUBDEV_FORMAT_ACTIVE;
    sel.pad = pad;
    sel.r.width = *w;
    sel.r.height = *h;
    sel.r.left = 0;
    sel.r.top = 0;
    sel.target = V4L2_SEL_TGT_CROP;
    sel.flags = V4L2_SEL_FLAG_LE;
    ret = ioctl(fd, VIDIOC_SUBDEV_S_SELECTION, &sel);
    if (ret) {
        LOG_ERROR(utils::format_string("subdev %s pad %d crop failed, ret = %d\n", ispsd, sel.pad, ret));
    }
	else {
		LOG_TRACE(utils::format_string("VIDIOC_SUBDEV_S_SELECTION sucess.\n"));		
	}
	
    *w = sel.r.width;
    *h = sel.r.height;
}

static void rkisp_video_set_crop(int fd, int x, int y, int w, int h)
{
    struct v4l2_selection sel;
    int ret;
    memset(&sel, 0, sizeof(sel));
    sel.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
    sel.r.width = w;
    sel.r.height = h;
    sel.r.left = x;
    sel.r.top = y;
    sel.target = V4L2_SEL_TGT_CROP;
    sel.flags = 0;
    ret = ioctl(fd, VIDIOC_S_SELECTION, &sel);
    if (ret) {
        LOG_ERROR(utils::format_string("VIDIOC_S_SELECTION::set output crop(0,0/%dx%d) failed, %s\n", w, h, strerror(errno)));
    }
	else {
		LOG_TRACE(utils::format_string("sVIDIOC_S_SELECTION scuess\n"));
	}

}
void GVideoISP1::init_device(void) {
	struct v4l2_capability cap;
	struct v4l2_format fmt;

	enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    struct v4l2_fmtdesc fmt_1;
    struct v4l2_frmsizeenum frmsize;
    struct v4l2_frmivalenum frmival;
	struct v4l2_subdev_format subdev_fmt;
	int subdev_fd;

	unsigned int caps;
	bool has_video;
	bool has_meta;
	bool has_capture;
	bool has_output;
	bool has_mplane;
	
	int ret;
	int i;
	//---------------------------set /dev/v4l2-subdev*-------------------------------------//
	//----------------------1.v4l2_subdev2 set format----------------------//
	subdev_fd = ::open(subdev2_name.c_str(),O_RDWR,0);
	if (-1 == subdev_fd) {
		LOG_ERROR(utils::format_string("Cannot open '%s'\n", subdev2_name.c_str()));
	}
	else {
		LOG_TRACE(utils::format_string("open %s success.fd = %d.\n",subdev2_name.c_str(),subdev_fd));
	}
	memset(&subdev_fmt, 0, sizeof subdev_fmt);
	subdev_fmt.pad = 0;
	subdev_fmt.which = V4L2_SUBDEV_FORMAT_ACTIVE;
	ret = ioctl(subdev_fd, VIDIOC_SUBDEV_G_FMT, &subdev_fmt);
	if (ret < 0) {
		LOG_ERROR(utils::format_string("VIDIOC_SUBDEV_G_FMT failed.\n"));	
	}
	else {
		LOG_TRACE(utils::format_string("VIDIOC_SUBDEV_G_FMT SUCESS\n"));
	}
	LOG_TRACE(utils::format_string("VIDIOC_SUBDEV_G_FMT:height = %d;width = %d;code = %d.\n", subdev_fmt.format.height,subdev_fmt.format.width,subdev_fmt.format.code));
	
	subdev_fmt.format.width = v4l_width;
	subdev_fmt.format.height = v4l_height;
	LOG_TRACE(utils::format_string("set height = %d\n",subdev_fmt.format.height));
	LOG_TRACE(utils::format_string("set width = %d\n",subdev_fmt.format.width));
    ret = ioctl(subdev_fd, VIDIOC_SUBDEV_S_FMT, &subdev_fmt);
	if (ret < 0) {
		LOG_ERROR(utils::format_string("VIDIOC_SUBDEV_S_FMT failed.\n"));	
	}
	else {
		LOG_TRACE(utils::format_string("VIDIOC_SUBDEV_S_FMT SUCESS\n"));
	}
	LOG_TRACE(utils::format_string("VIDIOC_SUBDEV_S_FMT:height = %d;width = %d;code = %d.\n", subdev_fmt.format.height,subdev_fmt.format.width,subdev_fmt.format.code));
	::close(subdev_fd);
	//-------------------------------------------------------------//

	//----------------------2.v4l2_subdev0 set format::pad0----------------------//
	subdev_fd = ::open(subdev0_name.c_str(),O_RDWR,0);
	if (-1 == subdev_fd) {
		LOG_ERROR(utils::format_string("Cannot open '%s'\n", subdev0_name.c_str()));
	}
	else {
		LOG_TRACE(utils::format_string("open %s success.fd = %d.\n",subdev0_name.c_str(),subdev_fd));
	}
	memset(&subdev_fmt, 0, sizeof subdev_fmt);
	subdev_fmt.pad = 0;
	subdev_fmt.which = V4L2_SUBDEV_FORMAT_ACTIVE;
	ret = ioctl(subdev_fd, VIDIOC_SUBDEV_G_FMT, &subdev_fmt);
	if (ret < 0) {
		LOG_ERROR(utils::format_string("VIDIOC_SUBDEV_G_FMT failed.\n"));	
	}
	else {
		LOG_TRACE(utils::format_string("VIDIOC_SUBDEV_G_FMT SUCESS\n"));
	}
	LOG_TRACE(utils::format_string("VIDIOC_SUBDEV_G_FMT:pad = %d;height = %d;width = %d;code = %d.\n", subdev_fmt.pad,subdev_fmt.format.height,subdev_fmt.format.width,subdev_fmt.format.code));
	subdev_fmt.format.width = v4l_width;
	subdev_fmt.format.height = v4l_height;
	LOG_TRACE(utils::format_string("set height = %d;width = %d\n",subdev_fmt.format.height,subdev_fmt.format.width));
		
    ret = ioctl(subdev_fd, VIDIOC_SUBDEV_S_FMT, &subdev_fmt);
	if (ret < 0) {
		LOG_ERROR(utils::format_string("VIDIOC_SUBDEV_S_FMT failed.\n"));	
	}
	else {
		LOG_TRACE(utils::format_string("VIDIOC_SUBDEV_S_FMT SUCESS\n"));
	}
	LOG_TRACE(utils::format_string("VIDIOC_SUBDEV_S_FMT:pad = %d;height = %d;width = %d;code = %d.\n", subdev_fmt.pad,subdev_fmt.format.height,subdev_fmt.format.width,subdev_fmt.format.code));
	rkisp_sd_set_crop(subdev0_name.c_str(),subdev_fd, 0 , &v4l_width, &v4l_height);
	LOG_TRACE(utils::format_string("v4l_set height = %d;width = %d\n",v4l_width,v4l_height));
	//----------------------3.v4l2_subdev0 set format::pad2----------------------//
	rkisp_sd_set_crop(subdev0_name.c_str(),subdev_fd, 2 , &v4l_width, &v4l_height);
	LOG_TRACE(utils::format_string("v4l_set height = %d;width = %d\n",v4l_width,v4l_height));
	memset(&subdev_fmt, 0, sizeof subdev_fmt);
	subdev_fmt.pad = 2;
	subdev_fmt.which = V4L2_SUBDEV_FORMAT_ACTIVE;
	ret = ioctl(subdev_fd, VIDIOC_SUBDEV_G_FMT, &subdev_fmt);
	if (ret < 0)
	{
		LOG_TRACE(utils::format_string("VIDIOC_SUBDEV_G_FMT failed.\n"));	
	}
	else
	{
		LOG_TRACE(utils::format_string("VIDIOC_SUBDEV_G_FMT SUCESS\n"));
	}
	LOG_TRACE(utils::format_string("VIDIOC_SUBDEV_G_FMT:pad = %d\n",subdev_fmt.pad));
	LOG_TRACE(utils::format_string("VIDIOC_SUBDEV_G_FMT:subdev_fmt.format.height = %d\n",subdev_fmt.format.height));
	LOG_TRACE(utils::format_string("VIDIOC_SUBDEV_G_FMT:subdev_fmt.format.width = %d\n",subdev_fmt.format.width));
	LOG_TRACE(utils::format_string("VIDIOC_SUBDEV_G_FMT:subdev_fmt.format.code = %d\n",subdev_fmt.format.code));
	subdev_fmt.format.width = v4l_width;
	subdev_fmt.format.height = v4l_height;
	LOG_TRACE(utils::format_string("set height = %d\n",subdev_fmt.format.height));
	LOG_TRACE(utils::format_string("set width = %d\n",subdev_fmt.format.width));

	ret = ioctl(subdev_fd, VIDIOC_SUBDEV_S_FMT, &subdev_fmt);
	if (ret < 0)
	{
		LOG_TRACE(utils::format_string("VIDIOC_SUBDEV_S_FMT failed.\n"));	
	}
	else
	{
		LOG_TRACE(utils::format_string("VIDIOC_SUBDEV_S_FMT SUCESS\n"));
	}
	LOG_TRACE(utils::format_string("VIDIOC_SUBDEV_G_FMT:pad = %d\n",subdev_fmt.pad));
	LOG_TRACE(utils::format_string("VIDIOC_SUBDEV_S_FMT:subdev_fmt.format.height = %d\n",subdev_fmt.format.height));
	LOG_TRACE(utils::format_string("VIDIOC_SUBDEV_S_FMT:subdev_fmt.format.width = %d\n",subdev_fmt.format.width));
	LOG_TRACE(utils::format_string("VIDIOC_SUBDEV_S_FMT:subdev_fmt.format.code = %d\n",subdev_fmt.format.code)); 
	//-------------------------------------------------------------//

	//-------4.set video selection(crop) because ispsd size changed------------------//
	rkisp_video_set_crop(fd, 0, 0, v4l_width, v4l_height);
	//----------------------1.VIDIOC_QUERYCAP----------------------//
	memset(&cap, 0, sizeof cap);
	if (-1 == ioctl(fd, VIDIOC_QUERYCAP, &cap)) 

	ret = ioctl(fd, VIDIOC_QUERYCAP, &cap);
	if (ret < 0)
	{
		LOG_TRACE(utils::format_string("VIDIOC_QUERYCAP failed.\n"));	
	}
	else
	{
		LOG_TRACE(utils::format_string("cap.capabilities = %x .\n",cap.capabilities));
		LOG_TRACE(utils::format_string("cap.device_caps = %x .\n",cap.device_caps));	
	}
	caps = cap.capabilities & V4L2_CAP_DEVICE_CAPS
	     ? cap.device_caps : cap.capabilities;	
	
	 has_video = caps & (V4L2_CAP_VIDEO_CAPTURE_MPLANE |
				 V4L2_CAP_VIDEO_CAPTURE |
				 V4L2_CAP_VIDEO_OUTPUT_MPLANE |
				 V4L2_CAP_VIDEO_OUTPUT);
	//  has_meta = caps & (V4L2_CAP_META_CAPTURE |
	// 			V4L2_CAP_META_OUTPUT);
	//  has_capture = caps & (V4L2_CAP_VIDEO_CAPTURE_MPLANE |
	// 			   V4L2_CAP_VIDEO_CAPTURE |
	// 			   V4L2_CAP_META_CAPTURE);
	 has_output = caps & (V4L2_CAP_VIDEO_OUTPUT_MPLANE |
				  V4L2_CAP_VIDEO_OUTPUT |
				  V4L2_CAP_META_OUTPUT);
	 has_mplane = caps & (V4L2_CAP_VIDEO_CAPTURE_MPLANE |
				  V4L2_CAP_VIDEO_OUTPUT_MPLANE);
	 
	 LOG_TRACE(utils::format_string("Device `%s' on `%s' (driver '%s') supports%s%s%s%s %s mplanes.\n", cap.card, cap.bus_info, cap.driver, has_video ? " video," : "", has_meta ? " meta-data," : "", has_capture ? " capture," : "", has_output ? " output," : "", has_mplane ? "with" : "without"));


	if (!(cap.capabilities & V4L2_CAP_STREAMING)) {
		LOG_ERROR(utils::format_string("%s does not support streaming i/o\n", dev_name.c_str()));
	}
	else {
		LOG_TRACE(utils::format_string("%s support streaming i/o\n", dev_name.c_str()));
	}
	//-------------------------------------------------------------//

	//----------------------2.VIDIOC_ENUM_FMT----------------------//
	LOG_TRACE(utils::format_string("- Available formats:\n"));
	LOG_TRACE(utils::format_string("------------------------------------------------------\n"));
	LOG_TRACE(utils::format_string("fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE = %d\n", V4L2_BUF_TYPE_VIDEO_CAPTURE));
	video_enum_formats(fd, V4L2_BUF_TYPE_VIDEO_CAPTURE);
	
	LOG_TRACE(utils::format_string("------------------------------------------------------\n"));
	LOG_TRACE(utils::format_string("fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE = %d\n", V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE));
	video_enum_formats(fd, V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE);
	
	LOG_TRACE(utils::format_string("------------------------------------------------------\n"));
	LOG_TRACE(utils::format_string("fmt.type = V4L2_BUF_TYPE_VIDEO_OUTPUT = %d\n", V4L2_BUF_TYPE_VIDEO_OUTPUT));
	video_enum_formats(fd, V4L2_BUF_TYPE_VIDEO_OUTPUT);

	LOG_TRACE(utils::format_string("------------------------------------------------------\n"));
	LOG_TRACE(utils::format_string("fmt.type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE = %d\n", V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE));
	video_enum_formats(fd, V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE);

	LOG_TRACE(utils::format_string("------------------------------------------------------\n"));
	LOG_TRACE(utils::format_string("fmt.type = V4L2_BUF_TYPE_VIDEO_OVERLAY = %d\n", V4L2_BUF_TYPE_VIDEO_OVERLAY));
	video_enum_formats(fd, V4L2_BUF_TYPE_VIDEO_OVERLAY);

	LOG_TRACE(utils::format_string("------------------------------------------------------\n"));
	//LOG_TRACE(utils::format_string("fmt.type = V4L2_BUF_TYPE_META_CAPTURE = %d\n", V4L2_BUF_TYPE_META_CAPTURE));
	//video_enum_formats(fd, V4L2_BUF_TYPE_META_CAPTURE);
	
	LOG_TRACE(utils::format_string("------------------------------------------------------\n"));
	LOG_TRACE(utils::format_string("fmt.type = V4L2_BUF_TYPE_META_OUTPUT = %d\n", V4L2_BUF_TYPE_META_OUTPUT));
	//video_enum_formats(fd, V4L2_BUF_TYPE_META_OUTPUT);
	LOG_TRACE(utils::format_string("------------------------------------------------------\n"));
	//--------------------------------------------------------------//

	//----------------------3.VIDIOC_ENUMINPUT----------------------//
	LOG_TRACE(utils::format_string("- Available inputs:\n"));
	video_enum_inputs(fd);
	//-------------------------------------------------------------//

	//----------------------4.VIDIOC_S_FMT-------------------------//
	LOG_TRACE(utils::format_string("..........................................................................\n"));
	CLEAR(fmt);
	//fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
	fmt.fmt.pix.width = v4l_width;
	fmt.fmt.pix.height = v4l_height;
	//fmt.fmt.pix.width = 2112;
	//fmt.fmt.pix.height = 1568;
	LOG_TRACE(utils::format_string("v4l2 VIDIOC_S_FMT width:%d, height:%d\n", fmt.fmt.pix.width, fmt.fmt.pix.height));
	//fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_RGB24;
	//fmt.fmt.pix.field = V4L2_FIELD_INTERLACED;
	//fmt.fmt.pix_mp.width = 1728;
	//fmt.fmt.pix_mp.height = 1568;
	fmt.fmt.pix_mp.pixelformat = V4L2_PIX_FMT_RGB24;
	fmt.fmt.pix_mp.field = 0;
	fmt.fmt.pix_mp.num_planes = 1;
	fmt.fmt.pix_mp.flags = 0;
	fmt.fmt.pix_mp.plane_fmt[0].bytesperline = 0;
	fmt.fmt.pix_mp.plane_fmt[0].sizeimage = 0;

	ret = ioctl(fd, VIDIOC_S_FMT, &fmt);
	if (ret < 0) {
		LOG_TRACE(utils::format_string("Unable to set format: %s (%d).\n", strerror(errno),errno));

	}
	else {
		LOG_TRACE(utils::format_string("VIDIOC_S_FMT sucess.\n"));
	}
	
	LOG_TRACE(utils::format_string("Video format set: %s (%08x) %ux%u field %s, %u planes: \n", v4l2_format_name(fmt.fmt.pix_mp.pixelformat), fmt.fmt.pix_mp.pixelformat, fmt.fmt.pix_mp.width, fmt.fmt.pix_mp.height, v4l2_field_name(fmt.fmt.pix_mp.field), fmt.fmt.pix_mp.num_planes));
	
	for (i = 0; i < fmt.fmt.pix_mp.num_planes; i++) {
		LOG_TRACE(utils::format_string(" * bytesperline %u, buffer size %u\n", fmt.fmt.pix_mp.plane_fmt[i].bytesperline, fmt.fmt.pix_mp.plane_fmt[i].sizeimage));
	}
	//-------------------------------------------------------------//

	//----------------------5.VIDIOC_G_FMT-------------------------//
	memset(&fmt, 0, sizeof fmt);
	fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
	ret = ioctl(fd, VIDIOC_G_FMT, &fmt);
	if (ret < 0) {
		LOG_TRACE(utils::format_string("Unable to get format: %s (%d).\n", strerror(errno),errno));
	}
	else {
		utils::to_log(LOG_LEVEL_DEBUG, "VIDIOC_G_FMT sucess.\n");
	}
	LOG_TRACE(utils::format_string("Video format: %s (%08x) %ux%u field %s, %u planes: \n", v4l2_format_name(fmt.fmt.pix_mp.pixelformat), fmt.fmt.pix_mp.pixelformat, fmt.fmt.pix_mp.width, fmt.fmt.pix_mp.height, v4l2_field_name(fmt.fmt.pix_mp.field), fmt.fmt.pix_mp.num_planes));
	for (i = 0; i < fmt.fmt.pix_mp.num_planes; i++)
		LOG_TRACE(utils::format_string(" * bytesperline %u, buffer size %u\n", fmt.fmt.pix_mp.plane_fmt[i].bytesperline, fmt.fmt.pix_mp.plane_fmt[i].sizeimage));

	::close(subdev_fd);
	//-------------------------------------------------------------//

	//6.VIDIOC_REQBUFS	//7.VIDIOC_QUERYBUF mmmap
	init_mmap();
	//-------------------------------------------------------------//
}