/*
 * /linux-3.0.8/drivers/media/video/vcam_test.c
 */
 
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <string.h>
#include <getopt.h>
#include <pthread.h>
#include <errno.h>
#include <signal.h>
#include <fcntl.h>

#include <sys/types.h>
#include <sys/stat.h>
#include <sys/time.h>
#include <sys/ioctl.h>
#include <sys/mman.h>


//#include <linux/videodev.h>
#include <linux/videodev2.h>
#include <linux/fb.h>

#include "vcam_test.h"

int stop=0;


/***** help message function *****/
static void vcam_help(char *progname)
{
  printf("-------------------------\n");
  printf("Usage: %s\n" \
                  " [-h ]:	help message\n" \
                  " [-i ]:	input device\n" \
                  " [-o ]:	output device\n" \
                  " [-r ]:	resolution\n" \
                  " [-f ]:	frames per second\n" \
                  " [-m ]:	grap method\n" \
                  " [-v ]:	software version\n" \
                  " [-b ]:	background run\n",progname);
  printf("-------------------------\n");
}



/***** init device function *****/
static int vcam_map_buffer(struct vcam_control_info *info)
{
	int i = 0;
	int ret = 0;
	for (i = 0; i < NB_BUFFER; i++) {
		memset(&info->in.buffer, 0, sizeof(struct v4l2_buffer));
		info->in.buffer.index = i;
		info->in.buffer.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
		info->in.buffer.memory = V4L2_MEMORY_MMAP;
		ret = ioctl(info->in.fd, VIDIOC_QUERYBUF, &info->in.buffer);
		if (ret < 0) {
			printf("failed to query buffer : error = %d\n", ret);
			goto exit_1;
		}else{
			printf("good to query buffer\n");
		}
		info->in.mem[i] = mmap(0 /* start anywhere */ ,
                	info->in.buffer.length, PROT_READ, MAP_SHARED, info->in.fd,
                 	info->in.buffer.m.offset);
		if (info->in.mem[i] == MAP_FAILED) {
			printf("failed to map buffer : error = %d\n", ret);
			goto exit_1;
		}else{
			printf("good to map buffer: addr=0x%08x, len=%d\n", 
				info->in.mem[i], info->in.buffer.length);
		}
	}
	return ret;
	
exit_1:
	while(--i >= 0){
		munmap(info->in.mem[i], info->in.buffer.length);
		info->in.mem[i] = NULL;
	}
	return ret;

}
static int vcam_unmap_buffer(struct vcam_control_info *info)
{
	int i = NB_BUFFER;
	int ret = 0;
	while(--i >= 0){
		munmap(info->in.mem[i], info->in.buffer.length);
		info->in.mem[i] = NULL;
	}
	return ret;
}

static int vcam_init_in(struct vcam_control_info *info)
{	printf("vcam_init_in()\n");
	int ret = 0;
	int i=0;
	if (info == NULL)
		return -1;
	ret = open(info->in.dev_name, O_RDWR);
	if (ret < 0) {
		printf("failed to open in device: %s\n", info->in.dev_name);
		return -1;
	}else{
		printf("good to open in device: %s\n", info->in.dev_name);
	}
	info->in.fd = ret;

	
	memset(&info->in.capability, 0, sizeof(struct v4l2_capability));
	ret = ioctl(info->in.fd, VIDIOC_QUERYCAP, &info->in.capability);
	if (ret < 0) {
		printf("failed to query capability: error = %d\n", ret);
		goto exit_1;
	}else{
		printf("good to query capability\n");
	}
	
	if ((info->in.capability.capabilities & V4L2_CAP_VIDEO_CAPTURE) == 0) {
		printf("failed to support video capture\n");
		goto exit_1;
	}else{
		printf("good to support video capture\n");
	}
	if (info->in.grab_method) {
		if (!(info->in.capability.capabilities & V4L2_CAP_STREAMING)) {
	  		printf("failed to support video streaming\n");
	 		goto exit_1;
		}else{
			printf("good to support video streaming\n");
		}	
	} else {
		if (!(info->in.capability.capabilities & V4L2_CAP_READWRITE)) {
	  		printf("failed to support video read\n");
	  		goto exit_1;
		}else{
			printf("good to support video read\n");
		}
	}

	
	memset(&info->in.format, 0, sizeof(struct v4l2_format));
	info->in.format.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	info->in.format.fmt.pix.width = info->in.width;
	info->in.format.fmt.pix.height = info->in.height;
	info->in.format.fmt.pix.pixelformat = info->in.in_format;
	info->in.format.fmt.pix.field = V4L2_FIELD_ANY;
	ret = ioctl(info->in.fd, VIDIOC_S_FMT, &info->in.format);
	if (ret < 0) {
		printf("failed to set input format: error = %d\n", ret);
		goto exit_1;
	}else{
		printf("good to set input format: %s\n", 
			info->in.in_format == V4L2_PIX_FMT_MJPEG ? "V4L2_PIX_FMT_MJPEG": 
			( info->in.in_format == V4L2_PIX_FMT_YUYV ? "V4L2_PIX_FMT_YUYV":
			( info->in.in_format == V4L2_PIX_FMT_RGB565X ? "V4L2_PIX_FMT_RGB565X":
				" Other format ")));
	}
	if ((info->in.format.fmt.pix.width != info->in.width) ||
	  (info->in.format.fmt.pix.height != info->in.height)) {
		printf("failed to ask unavailable get width=%d, height=%d \n",
	      		info->in.format.fmt.pix.width, 
	      		info->in.format.fmt.pix.height);
		
		info->in.width = info->in.format.fmt.pix.width;
		info->in.height = info->in.format.fmt.pix.height;
	}else{
		printf("good to get width=%d, height=%d \n",
	      		info->in.format.fmt.pix.width, 
	      		info->in.format.fmt.pix.height);
	}

	/*
	memset(&info->in.streamparm, 0, sizeof(struct v4l2_streamparm));
	info->in.streamparm.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	info->in.streamparm.parm.capture.timeperframe.numerator = 1;
	info->in.streamparm.parm.capture.timeperframe.denominator = info->in.fps;
	ret = ioctl(info->in.fd, VIDIOC_S_PARM, &info->in.streamparm);
	if (ret < 0) {
		printf("failed to set stream parm : error = %d\n", ret);
		goto exit_1;
	}
	*/
	
	memset(&info->in.requestbuffers, 0, sizeof(struct v4l2_requestbuffers));
	info->in.requestbuffers.count = NB_BUFFER;
	info->in.requestbuffers.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	info->in.requestbuffers.memory = V4L2_MEMORY_MMAP;
	ret = ioctl(info->in.fd, VIDIOC_REQBUFS, &info->in.requestbuffers);
	if (ret < 0) {
		printf("failed to request buffer : error = %d\n", ret);
		goto exit_1;
	}else{
		printf("good to request buffer: count=%d\n", info->in.requestbuffers.count);
	}
	vcam_map_buffer(info);

	for (i = 0; i < NB_BUFFER; ++i) {
		memset(&info->in.buffer, 0, sizeof(struct v4l2_buffer));
		info->in.buffer.index = i;
		info->in.buffer.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
		info->in.buffer.memory = V4L2_MEMORY_MMAP;
		ret = ioctl(info->in.fd, VIDIOC_QBUF, &info->in.buffer);
		if (ret < 0) {
			printf("failed to queue buffer : error = %d\n", ret);
			goto exit_2;
		}else{
			printf("good to queue buffer: index=%d\n", info->in.buffer.index);
		}
	}
	
	info->in.in_framesize = (info->in.width * info->in.height << 1);
	info->in.tmp_buffer = NULL;
	info->in.frame_buffer = NULL;
	switch (info->in.in_format) {
	case V4L2_PIX_FMT_MJPEG:
		info->in.tmp_buffer = 
			(unsigned char *) calloc(1, (size_t) info->in.in_framesize);
		if (info->in.tmp_buffer == NULL){
			printf("failed to allocate MJPEG tmp buffer\n");
			goto exit_2;
		}else{
			printf("good to allocatetmp MJPEG buffer: addr=0x%08x, size=%d\n", 
				info->in.tmp_buffer, info->in.in_framesize);
		}
		/*
		info->in.frame_buffer =
	    		(unsigned char *) calloc(1, (size_t) info->in.width * (info->in.height + 8) * 2);
		if (info->in.frame_buffer == NULL){
			printf(stderr,"failed to allocate MJPEG frame buffer\n");
			goto exit_2;
		}
		*/
		break;
	case V4L2_PIX_FMT_YUYV:
		info->in.tmp_buffer = 
			(unsigned char *) calloc(1, (size_t) info->in.in_framesize);
		if (info->in.tmp_buffer == NULL){
			printf("failed to allocate YUYV frame buffer\n");
			goto exit_2;
		}else{
			printf("good to allocatetmp YUYV buffer: addr=0x%08x, size=%d\n", 
				info->in.tmp_buffer, info->in.in_framesize);
		}
		break;

	case V4L2_PIX_FMT_RGB565X:
		info->in.tmp_buffer = 
			(unsigned char *) calloc(1, (size_t) info->in.in_framesize);
		if (info->in.tmp_buffer == NULL){
			printf("failed to allocate RGB565X frame buffer\n");
			goto exit_2;
		}else{
			printf("good to allocate RGB565X tmp buffer: addr=0x%08x, size=%d\n", 
				info->in.tmp_buffer, info->in.in_framesize);
		}
		break;
	default:
		printf("failed to support format type\n");
		goto exit_2;
		break;
	}
	return 0;

exit_2:
	vcam_unmap_buffer(info);
exit_1:
	close(info->in.fd);
	return -1;
}

static int vcam_stop(struct vcam_control_info *info)
{
	int type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	int ret;

	ret = ioctl(info->in.fd, VIDIOC_STREAMOFF, &type);
	if (ret < 0) {
		printf("failed to stop capture: %d.\n", ret);
		return ret;
	}else{
		printf("good to stop capture\n");
	}
	info->in.is_streaming = 0;
	return 0;
}
static int vcam_deinit_in(struct vcam_control_info *info)
{	printf("vcam_deinit_in()\n");
	int ret = 0;
	vcam_stop(info);
	if(info->in.tmp_buffer != NULL){
		free(info->in.tmp_buffer);
		info->in.tmp_buffer = NULL;
	}
	if(info->in.frame_buffer != NULL){
		free(info->in.frame_buffer);
		info->in.frame_buffer = NULL;
	}
	vcam_unmap_buffer(info);
	close(info->in.fd);
	return ret;
}


static int vcam_init_out(struct vcam_control_info *info)
{	printf("vcam_init_out()\n");
	int ret = 0;
	if (info == NULL)
		return -1;
	ret = open(info->out.dev_name, O_RDWR);
	if (ret < 0) {
		printf("failed to open out device: %s\n", info->out.dev_name);
		return -1;
	}else{
		printf("good to open out device: %s\n", info->out.dev_name);
	}
	info->out.fd = ret;
	memset(&info->out.var_screeninfo, 0, sizeof(struct fb_var_screeninfo));
	ret = ioctl(info->out.fd, FBIOGET_VSCREENINFO, &info->out.var_screeninfo);
	if (ret < 0) {
		printf("failed to query var screeninfo: error = %d\n", ret);
		goto exit_1;
	}else{
		printf("good to query var screeninfo: xres=%d, yres=%d, bpp=%d\n", 
			info->out.var_screeninfo.xres, 
			info->out.var_screeninfo.yres,
			info->out.var_screeninfo.bits_per_pixel);
	}
	
	memset(&info->out.fix_screeninfo, 0, sizeof(struct fb_fix_screeninfo));
	ret = ioctl(info->out.fd, FBIOGET_FSCREENINFO, &info->out.fix_screeninfo);
	if (ret < 0) {
		printf("failed to query fix screeninfo: error = %d\n", ret);
		goto exit_1;
	}else{
		printf("good to query fix screeninfo: start=0x%08x, len=%d\n", 
			info->out.fix_screeninfo.smem_start, 
			info->out.fix_screeninfo.smem_len);
	}
	info->out.fb_size = 
		(info->out.var_screeninfo.xres * info->out.var_screeninfo.yres) * 
		(info->out.var_screeninfo.bits_per_pixel/8);
	info->out.fb_buffer = mmap(0, info->out.fb_size, PROT_READ | PROT_WRITE,
							MAP_SHARED, info->out.fd, 0);
	if(info->out.fb_buffer == (void *) -1){
		printf("failed to map fb buffer: error = %d\n", ret);
		goto exit_1;
	}else{
		printf("good to map fb buffer: addr=0x%08x, size=%d\n", 
			info->out.fb_buffer, info->out.fb_size);
	}
	return ret;
	
exit_1:
	close(info->out.fd);
	return -1;	
	
}
static int vcam_deinit_out(struct vcam_control_info *info)
{	printf("vcam_deinit_out()\n");
	munmap(info->out.fb_buffer, info->out.fb_size);
	close(info->out.fd);
}


static int vcam_start(struct vcam_control_info *info)
{
	int type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	int ret;

	ret = ioctl(info->in.fd, VIDIOC_STREAMON, &type);
	if (ret < 0) {
		printf("failed to start capture: %d.\n", ret);
		return ret;
	}
	info->in.is_streaming = 1;
	return 0;
}

static int vcam_grab_frame(struct vcam_control_info *info)
{
#define HEADERFRAME1 0xaf
	int ret;

	if (!info->in.is_streaming)
		if (vcam_start(info))
	 		 goto exit_1;
	  
	memset(&info->in.buffer, 0, sizeof(struct v4l2_buffer));
	info->in.buffer.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	info->in.buffer.memory = V4L2_MEMORY_MMAP;

	ret = ioctl(info->in.fd, VIDIOC_DQBUF, &info->in.buffer);
	if (ret < 0) {
		printf("failed to dequeue buffer: error = %d\n", ret);
		goto exit_1;
	}

	switch (info->in.in_format){
	case V4L2_PIX_FMT_MJPEG:
	case V4L2_PIX_FMT_YUYV:
	case V4L2_PIX_FMT_RGB565X:
		if (info->in.buffer.bytesused <= HEADERFRAME1) {
	  		printf("failed to get used buffer: error = %d\n", ret);
	  		goto exit_1;
		}
		/*
		memcpy(info->in.tmp_buffer, 
				info->in.mem[info->in.buffer.index], 
				info->in.buffer.bytesused);
				*/
		memcpy(info->out.fb_buffer, 
				info->in.mem[info->in.buffer.index], 
				info->in.buffer.bytesused);				
				
		break;
	
	default:
		goto exit_1;
		break;
	}
	ret = ioctl(info->in.fd, VIDIOC_QBUF, &info->in.buffer);
	if (ret < 0) {
		printf("failed to requeue buffer: error = %d\n", ret);
		goto exit_1;
	}
	return 0;

exit_1:
	
	info->in.signal_quit = 0;
	return -1;
}



static void vcam_signal_handler(int sigm) 
{	printf("vcam_signal_handler()\n");
	stop = 1;
	usleep(1000*1000);
	return;
}



/***** main function *****/
int main(int argc, char *argv[])
{	printf("main()\n");

	struct vcam_control_info info;
	//char *in_device = "/dev/video0";
	char *in_device = "/dev/video1";
	char *out_device = "/dev/fb0";
	int width=320;
	int height=240;
	int fps=5; 
	//int grab_method = 0;
	int grab_method = 1;
	int daemon=0;
	
	int on=1; 
	int ret = 0;
	while(1) {
		int option_index = 0, c=0;
		static struct option long_options[] = \
		{
		  {"h", no_argument, 0, 0},		/* 0: help message */
		  
		  {"i", required_argument, 0, 0},	/* 1: input device */
		  {"o", required_argument, 0, 0},	/* 2: output device */
		  
		  {"r", required_argument, 0, 0},	/* 3: input resolution */
		  {"f", required_argument, 0, 0},	/* 4: input fps */
		  {"m", required_argument, 0, 0},	/* 5: input grab mode */
		  
		  {"v", no_argument, 0, 0},		/* 6: software version */
		  {"b", no_argument, 0, 0},		/* 7: background run */

		  {0, 0, 0, 0}
		};
		c = getopt_long_only(argc, argv, "", long_options, &option_index);
		if (c == -1) 
			break;
		if(c=='?'){
			vcam_help(argv[0]); 
			return 0; 
		}
		switch (option_index) {
		case 0:
			vcam_help(argv[0]);
		  	return 0;
		 	break;
		case 1:
			in_device = strdup(optarg);
			break;
		case 2:
			out_device = strdup(optarg);
			break;
		case 3:
			if ( strcmp("960x720", optarg) == 0 ) { 
				width=960; 
				height=720; 
			}else if ( strcmp("640x480", optarg) == 0 ) { 
				width=640; 
				height=480; 
			}else if ( strcmp("320x240", optarg) == 0 ) { 
				width=320; 
				height=240; 
			}else if ( strcmp("160x120", optarg) == 0 ) { 
				width=160; 
				height=120; 
			}else { 
		      		printf("failed to support resolution: %s\n", optarg);
		 	}      
			break;
		case 4:
			fps=atoi(optarg);
			break;
		case 5:
			grab_method=atoi(optarg);
			break;
		case 6:
			printf("Virtual Camera Test Version: %s\n" \
		      		"Date....: %s\n" \
		      		"Time....: %s\n", 
		        	VCAM_TEST_VERSION, __DATE__, __TIME__);
			return 0;
			break;
		case 7:
		 	daemon=1;
			break;
		default:
		    vcam_help(argv[0]);
		    return 0;
		}
	}
	memset(&info.in.dev_name, 0, DEVICE_NAME_LEN);
	memset(&info.out.dev_name, 0, DEVICE_NAME_LEN);
	memcpy(&info.in.dev_name[0], in_device, strlen(in_device));
	memcpy(&info.out.dev_name[0], out_device, strlen(out_device));
	info.in.height = height;
	info.in.width = width;
	info.in.fps= fps;
	info.in.grab_method= grab_method;
	info.in.in_format= V4L2_PIX_FMT_RGB565X;

	
	signal(SIGPIPE, SIG_IGN);
	if (signal(SIGINT, vcam_signal_handler) == SIG_ERR) {
		printf("failed to register signal handler\n");
		exit(1);
	}
	if ( daemon ) {
	//	daemon_mode();
	}

	ret = vcam_init_in(&info);
	if (ret < 0) {
		printf("failed to init in device: %s\n", info.in.dev_name);
		exit(1);
	}else{
		printf("good to init in device: %s\n", info.in.dev_name);
	}
	ret = vcam_init_out(&info);
	if (ret < 0) {
		printf("failed to init out device: %s\n", info.out.dev_name);
		exit(1);
	}else{
		printf("good to init out device: %s\n", info.out.dev_name);
	}
	printf("start to grab frame from %s and display image to %s\n",
		info.in.dev_name ,info.out.dev_name);
	while( !stop ) {
		ret = vcam_grab_frame(&info) ;
		if(ret < 0 ) {
			printf("failed to grab frame: error = %d\n", ret);
			//exit(1);
		}else{
			//memcpy(info.out.fb_buffer, info.in.tmp_buffer, info.in.buffer.bytesused);
			if ( info.in.fps < 5 ) {
		 		usleep(1000*1000/info.in.fps);
			}
		}

	}
	vcam_deinit_in(&info);
	vcam_deinit_out(&info);
	return 0;
}