// Telerobotics Senior Project
// (c) 2000 Bradley University ECE
// -------------------------------
// Frame grabber - prints JPEG data to stdout
// Adapted from:
// vcat.c from V4L2 Sample Apps by Bill Dirks 
// vctrl.c from V4L2 Sample Apps originally by Erik Walthinsen
// example.c from jpeg6-b library by IJG
// Required software packages:
// -- Video for Linux Two
// -- Winnov Videum V4L2 driver, or other driver for other card.
// -- libjpeg-6b

#include <sys/time.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <sys/ioctl.h>
#include <fcntl.h>
#include <unistd.h>
#include <stdlib.h>
#include <stdio.h>
#include <sys/mman.h>
#include <errno.h>

/* These are needed to use the Videum driver */
#include <linux/fs.h>
#include <linux/kernel.h>
#include <linux/videodev.h>  /* Video for Linux Two */

// JPEG library includes

#include "jpeglib.h"
#include <setjmp.h>

int main(int argc, char *argv[])
{
	char			*device	= NULL;
	int			vid = -1;
	int			err;
	int			i;
	int			n;
	struct v4l2_capability	cap;
        struct v4l2_format	fmt;
	int			bgr	= 0;
	int			rgb	= 0;
	int			rgbswap	= 0;
	char			*data;
	char			t;
	char			*jpgfile;
	FILE			*outfile;
	struct jpeg_compress_struct	cinfo;
	struct jpeg_error_mgr 	jerr;
	JSAMPROW 		row_pointer[1];
	int			row_stride;
	int			quality;
	int			input;

	device = "/dev/video";
	quality = 50;

	input = atoi(argv[1]);

	while(vid < 0)
	{
		vid = open(device, O_RDONLY);
	}

	ioctl(vid, VIDIOC_S_INPUT, &input);   // set input

	err = ioctl(vid, VIDIOC_QUERYCAP, &cap);
	if (err)
	{
		fprintf(stderr, "QUERYCAP returned error %d\n", errno);
		return 1;
	}
	if (cap.type != V4L2_TYPE_CAPTURE)
	{
		fprintf(stderr, "Device %s is not a video capture device.\n",
			device);
		return 1;
	}
	if (!(cap.flags & V4L2_FLAG_READ))
	{
		fprintf(stderr, "Device %s doesn't support read().\n", device);
		return 1;
	}

	fmt.type = V4L2_BUF_TYPE_CAPTURE;
	err = ioctl(vid, VIDIOC_G_FMT, &fmt);
	if (err)
	{
		fprintf(stderr, "1.G_FMT returned error %d\n", errno);
		return 1;
	}

	fmt.fmt.pix.width=320;
	fmt.fmt.pix.height=240;
	fmt.fmt.pix.depth=24;
	fmt.fmt.pix.pixelformat=V4L2_PIX_FMT_BGR24;

	err = ioctl(vid, VIDIOC_S_FMT, &fmt);
	if (err)
	{
		fprintf(stderr, "S_FMT returned error %d\n", errno);
		return 1;
	}

	err = ioctl(vid, VIDIOC_G_FMT, &fmt);
	if (err)
	{
		fprintf(stderr, "2.G_FMT returned error %d\n", errno);
		return 1;
	}

	data = malloc(fmt.fmt.pix.sizeimage);
	if (data == NULL)
	{
		fprintf(stderr, "malloc(%d) failed\n", fmt.fmt.pix.sizeimage);
		return 1;
	}

//	some sorta delay here, to stabilize image

	// sleep(1);

	n = read(vid, data, fmt.fmt.pix.sizeimage);
	if (n < 0)
	{
		fprintf(stderr, "read() returned error %d\n", errno);
		return 1;
	}	

//  Dirty hack - reads extra frame because the first might not look perty
//  Figure out why the first frame is so ugly sometimes.

	n = read(vid, data, fmt.fmt.pix.sizeimage);
	if (n < 0)
	{
		fprintf(stderr, "read() returned error %d\n", errno);
		return 1;
	}	

	n = read(vid, data, fmt.fmt.pix.sizeimage);
	if (n < 0)
	{
		fprintf(stderr, "read() returned error %d\n", errno);
		return 1;
	}	

	n = read(vid, data, fmt.fmt.pix.sizeimage);
	if (n < 0)
	{
		fprintf(stderr, "read() returned error %d\n", errno);
		return 1;
	}	

	n = read(vid, data, fmt.fmt.pix.sizeimage);
	if (n < 0)
	{
		fprintf(stderr, "read() returned error %d\n", errno);
		return 1;
	}	

	close(vid);

	// Videum driver only supports BGR, so we have to
	// invert to RGB for use by the JPEG compressor	

	for (i = 0; i < n; i += 3)
	{
		t = data[i];
		data[i] = data[i + 2];
		data[i + 2] = t;
	}

	// if (n)
	//	fwrite(data, (size_t)n, 1, stdout);

	// print_settings(vid);

	cinfo.err = jpeg_std_error(&jerr);
	jpeg_create_compress(&cinfo);	

	jpeg_stdio_dest(&cinfo, stdout);
	cinfo.image_width = 320;
	cinfo.image_height = 240;
	cinfo.input_components = 3;
	cinfo.in_color_space = JCS_RGB;
	jpeg_set_defaults(&cinfo);
	jpeg_set_quality(&cinfo, quality, TRUE);
	jpeg_start_compress(&cinfo, TRUE);
	row_stride = 320 * 3;

	while (cinfo.next_scanline < cinfo.image_height)
	{
		row_pointer[0] = & data[cinfo.next_scanline * row_stride];
		(void) jpeg_write_scanlines(&cinfo, row_pointer, 1);
	}

	jpeg_finish_compress(&cinfo);
	jpeg_destroy_compress(&cinfo);

	return 0;

}