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

#include <fcntl.h>
#include <unistd.h>
#include <linux/types.h>
#include <linux/videodev.h>

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#define RGB_VALID(x) ((x) < 0)? 0 :(((x)>255)? 255: (x)) 

unsigned char *
yuv2rgb(unsigned char *yuv,unsigned char *rgb_buf,int width,int height){
	int i,j;
	unsigned char *y=yuv;
	unsigned char *u=y + width * height;
	unsigned char *v=u + width * height / 4;
	unsigned char *d = rgb_buf;
	unsigned char *us,*vs;

	for(i = 0; height > i; i++){
		// us,vsはrgbの４画素に使用する
		us = u; vs = v; 
		for(j = 0; width > j; j += 2){ // step by 2pixel
			int r, g, b, u0, v0, y0, cr, cg, cb;
			u0 = -128 + *u++;
			v0 = -128 + *v++;

			// 変換式を512倍したもの(整数演算するため)
			cr = 718 * v0;
			cg = -176 * u0 - 366 * v0;
			cb = 906 * u0;
			
			y0 = (*y++) << 9;
			r =(y0 + cr)>> 9;
			g =(y0 + cg)>> 9;
			b =(y0 + cb)>> 9;
			*d++ = RGB_VALID(r);
			*d++ = RGB_VALID(g);
			*d++ = RGB_VALID(b);

			y0 =  (*y++) << 9;
			r =(y0 + cr)>> 9;
			g =(y0 + cg)>> 9;
			b =(y0 + cb)>> 9;
			*d++ = RGB_VALID(r);
			*d++ = RGB_VALID(g);
			*d++ = RGB_VALID(b);
		}
		if( (i % 2) ==0){
			u = us; v = vs;
		}
	}
	
	return rgb_buf;
}

int main(int argc,char *argv[]){
	int WIDTH=640,HEIGHT=480;
	int n, fd;
	char *map;
	struct video_picture vp;
	struct video_mbuf vm;
	struct video_mmap vmm;
	FILE *fp;
	unsigned char rgb_buf[WIDTH*HEIGHT*3];

	if (argc!=3){
		fprintf(stderr,"usage %s <video-device> <outputfile>\n"
			,argv[0]);
		exit(0);
	}

	if( (fd=open(argv[1],O_RDWR))<0){
		perror("error:open device");
		return -1;
	}
	
	/* 明るさ、色、コントラスト、ホワイトネスを決め打ちする*/
	vp.brightness=32767;
	vp.hue=32767;
	vp.colour=32767;
	vp.contrast=32767;
	vp.whiteness=32767;
	vp.depth=24;			/* color depth */
	vp.palette=VIDEO_PALETTE_YUV420P; /* パレット形式 */
	if(ioctl(fd, VIDIOCSPICT, &vp)) {
		perror("ioctl(VIDIOCSPICT)");
		return -1;
	}

	/* mmap 情報の取得 */
	if(ioctl(fd, VIDIOCGMBUF, &vm)<0) {
		perror("ioctl(VIDIOCGMBUF)");
		return -1;
	}

	/* mmap */
	if((map=(char *)mmap(0, vm.size, 
			     PROT_READ|PROT_WRITE, 
			     MAP_SHARED, 
			     fd, 0))==(char *)-1) {
		perror("mmap");
		return -1;
	}


	/* キャプチャ開始 */
	vmm.frame=0;
	vmm.width=WIDTH;
	vmm.height=HEIGHT;
	vmm.format=VIDEO_PALETTE_YUV420P;
	if(ioctl(fd, VIDIOCMCAPTURE, &vmm)<0) {
		perror("ioctl(VIDIOCMCAPTURE)");
		return -1;
	}


	/*  キャプチャ終了待ち */
	n=0;
	if(ioctl(fd, VIDIOCSYNC, &n)<0) {
		perror("ioctl(VIDIOCSYNC)");
		return -1;
	}
	// YUV420PをRGB24に変換
	yuv2rgb(map+vm.offsets[0],rgb_buf,WIDTH,HEIGHT);
	

	/* ppm の書きだし */
	if((fp = fopen(argv[2],"w+"))==NULL){
		perror("error: open file");
		exit(-1);
	}
	printf("write ppm image to %s\n",argv[2]);
	fprintf(fp,"P6 %d %d 255\n", WIDTH, HEIGHT); fflush(stdout);
	fwrite(rgb_buf, sizeof(unsigned char),WIDTH*HEIGHT*3,fp);
	fclose(fp);
	
	close(fd); 

	return 0;
}