#include <fcntl.h>
#include <malloc.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>

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

#include <linux/videodev2.h>

#ifndef V4L2_PIX_FMT_VYUY
#define V4L2_PIX_FMT_VYUY     v4l2_fourcc('V','Y','U','Y') /* 16 YUV 4:2:2 */
#endif

struct image {
	unsigned int width;
	unsigned int height;
	unsigned int bytes_per_line;
	unsigned int size;
	void *buf;
};

static void perror_exit(const char *str)
{
	perror(str);
	exit(EXIT_FAILURE);
}

static unsigned char clip(int value)
{
	if (value < 0)
		value = 0;
	else if (value > 255)
		value = 255;
	return value;
}

static void yuv422_to_rgb(unsigned char *src, unsigned char *dst,
			  unsigned int nr_pixels)
{
	unsigned int i;
	int y1, y2, u, v;

	for (i = 0; i < nr_pixels; i += 2) {
		/* Input format is Cr(i)Y(i + 1)Cb(i)Y(i) */
		y2 = *src++;
		v = *src++;
		y1 = *src++;
		u = *src++;
		y1 -= 16;
		u -= 128;
		v -= 128;
		y2 -= 16;

		*dst++ = clip(( 298 * y1           + 409 * v + 128) >> 8);
		*dst++ = clip(( 298 * y1 - 100 * u - 208 * v + 128) >> 8);
		*dst++ = clip(( 298 * y1 + 516 * u           + 128) >> 8);
		*dst++ = clip(( 298 * y2           + 409 * v + 128) >> 8);
		*dst++ = clip(( 298 * y2 - 100 * u - 208 * v + 128) >> 8);
		*dst++ = clip(( 298 * y2 + 516 * u           + 128) >> 8);
	}
}

static void dump_frame(int fd, const struct image *image)
{
	unsigned char *line_rgb;
	unsigned char *line_yuv;
	unsigned char *p;
	unsigned int nlines = image->height;
	unsigned int nbytes;
	ssize_t ret;
	char ppm_header[256];

	line_rgb = malloc(image->width * 3);
	line_yuv = image->buf;

	sprintf(ppm_header, "P6\n%u %u 255\n", image->width, image->height);
	for (nbytes = strlen(ppm_header), p = ppm_header; nbytes;
			p += ret, nbytes -= ret) {
		ret = write(fd, p, nbytes);
		if (ret < 0)
			perror_exit("write error");
	}

	while (nlines--) {
		yuv422_to_rgb(line_yuv, line_rgb, image->width);
		for (nbytes = image->width * 3, p = line_rgb; nbytes;
				p += ret, nbytes -= ret) {
			ret = write(fd, p, nbytes);
			if (ret < 0)
				perror_exit("write error");
		}
		line_yuv += image->bytes_per_line;
	}

	free(line_rgb);
}

static void grab_frame(int fd, struct image *image)
{
	unsigned int nbytes = image->size;
	ssize_t ret;

	while (nbytes > 0) {
		ret = read(fd, image->buf, nbytes);
		if (ret < 0)
			perror_exit("read error");
		nbytes -= ret;
	}
}

static void init_videodev(int fd, struct image *image)
{
	struct v4l2_capability cap;
	struct v4l2_format fmt;

	if (ioctl(fd, VIDIOC_QUERYCAP, &cap) < 0)
		perror_exit("VIDIOC_QUERYCAP failed");

	if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE)) {
		fprintf(stderr, "No video capture capability present\n");
		exit(EXIT_FAILURE);
	}
	if (!(cap.capabilities & V4L2_CAP_READWRITE)) {
		fprintf(stderr, "read() interface not supported by driver\n");
		exit(EXIT_FAILURE);
	}

	/* Select video format */
	memset(&fmt, 0, sizeof(fmt));
	fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	fmt.fmt.pix.width = 320;
	fmt.fmt.pix.height = 240;
	fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_VYUY;

	if (ioctl(fd, VIDIOC_S_FMT, &fmt))
		perror_exit("VIDIOC_S_FMT failed");

	image->width = fmt.fmt.pix.width;
	image->height = fmt.fmt.pix.height;
	image->bytes_per_line = fmt.fmt.pix.bytesperline;
	image->size = fmt.fmt.pix.sizeimage;
	image->buf = malloc(image->size);
	if (!image->buf) {
		fprintf(stderr, "Out of memory\n");
		exit(EXIT_FAILURE);
	}
}

static void show_usage(const char *progname)
{
	fprintf(stderr, "Usage: %s [-d videodev] [-o outfile]\n\n",
		progname);
	fprintf(stderr,
		"If no options are given, one frame is grabbed from\n"
		"/dev/video0 and written to stdout in PPM format.\n");
}

int main(int argc, char **argv)
{
	struct image image;
	char *input_name = "/dev/video0";
	char *output_name = NULL;
	int infd, outfd;

	while (1) {
		int c;

		c = getopt(argc, argv, "d:ho:");
		if (c == -1)
			break;

		switch (c) {
		case 'd':
			input_name = strdup(optarg);
			break;
		case 'o':
			output_name = strdup(optarg);
			break;
		case 'h':
		case '?':
			show_usage(argv[0]);
			return EXIT_FAILURE;
		default:
			abort();
		}
	}

	infd = open(input_name, O_RDONLY);
	if (infd < 0) {
		perror("Failed to open video device");
		return EXIT_FAILURE;
	}

	outfd = STDOUT_FILENO;
	if (output_name) {
		outfd = open(output_name, O_WRONLY | O_CREAT | O_TRUNC,
			     0666);
		if (outfd < 0) {
			perror("Failed to open output file");
			return EXIT_FAILURE;
		}
	}

	init_videodev(infd, &image);
	grab_frame(infd, &image);
	dump_frame(outfd, &image);

	return EXIT_SUCCESS;
}