/*
 *
 * Copyright (C) 2007 Texas Instruments Inc
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 */

#include <stdio.h>
#include <sys/types.h>
#include <sys/ioctl.h>
#include <fcntl.h>
#include <string.h>
#include <stdlib.h>
#include <unistd.h>
#include <sys/mman.h>
#include <getopt.h>
#include <sys/mman.h>
#include <sys/time.h>
#include <linux/videodev.h>
#include <linux/videodev2.h>

#define VIDIOC_G_OMAP2_ROTATION		_IOR ('V', 4,  int)


char dev_name[2][20] = { "/dev/video1", "/dev/video2" };

#define IS_VALID_CHANNEL(ch)  (((ch) == 0) || ((ch) == 1))
#define NUMBUFFERS	3
#define WIDTH		320
#define HEIGHT		240

struct buf_info {
	unsigned int length;
	char *start;
};

int main(int argc, char *argv[])
{
	char shortoptions[] = "c:n:";
	int mode = O_RDWR, c, ret, l, k, j, i;
	int index,m = 1,del_cnt=10;
	int fd, ch_no = 0, numbuffers = NUMBUFFERS, a;
	unsigned int *ui;
	struct v4l2_requestbuffers req;
	struct v4l2_buffer buf;
	struct buf_info *buff_info;
	struct v4l2_format fmt;
	struct timeval before, after, result;
	int rotation=0;
	double red, green, blue;
	char *c_ptr;	

	unsigned char CB;
	unsigned char Y0;
	unsigned char CR;
	

	if (!IS_VALID_CHANNEL(ch_no)) {
		printf("Invalid channel id");
		exit(0);
	}
	if (numbuffers < 0) {
		printf("Invalid numbufers\n");
		exit(0);
	}
	fd = open((const char *)dev_name[ch_no], mode);
	if (fd <= 0) {
		printf("Cannot open = %s device\n", dev_name[ch_no]);
		perror("Error:");
		exit(0);
	}

	fmt.type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
	fmt.fmt.pix.width = WIDTH;
	fmt.fmt.pix.height = HEIGHT;
	fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_UYVY;
        ret = ioctl(fd, VIDIOC_S_FMT, &fmt);
        if (ret < 0) {
                perror("VIDIOC_S_FMT\n");
                close(fd);
                exit(0);
        }


	ret = ioctl(fd, VIDIOC_G_FMT, &fmt);
	if (ret < 0) {
		perror("VIDIOC_G_FMT\n");
		close(fd);
		exit(0);
	}

	printf("Format:\n");
	printf("Height: %i\n"        , fmt.fmt.pix.height);
	printf("Width: %i\n"         , fmt.fmt.pix.width);
	printf("Bytes per line: %i\n", fmt.fmt.pix.bytesperline);


	ret = ioctl(fd, VIDIOC_G_OMAP2_ROTATION, &rotation);
	if(ret < 0) {
		perror("VIDIOC_G_OMAP2_ROTATION\n");
		close(fd);
		exit(0);
	}
	printf("getfmt\n");
	
	req.count = numbuffers;
	req.type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
	req.memory = V4L2_MEMORY_MMAP;
	
	ret = ioctl(fd, VIDIOC_REQBUFS, &req);
	if (ret < 0) {
		perror("cannot allocate memory\n");
		close(fd);
		exit(0);
	}
	printf("reqbufs = %d\n",req.count);

	buff_info =
		(struct buf_info *) malloc(sizeof(struct buf_info) *
				req.count);
	if (!buff_info) {
		printf("cannot allocate memory for buff_info\n");
		close(fd);
		exit(0);
	}

	memset(&buf, 0, sizeof(buf));
	for (i = 0; i < req.count; i++) {
		buf.type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
		buf.index = i;
		buf.memory = V4L2_MEMORY_MMAP;
		ret = ioctl(fd, VIDIOC_QUERYBUF, &buf);
		if (ret < 0) {
			perror("VIDIOC_QUERYCAP\n");
			for (j = 0; j < i; j++)
				munmap(buff_info[j].start,
						buff_info[j].length);
			close(fd);
			exit(0);
		}
		buff_info[i].length = buf.length;

		buff_info[i].start =
			(char*)mmap(NULL, buf.length, PROT_READ | PROT_WRITE, MAP_SHARED, fd, buf.m.offset);

		if ((unsigned int) buff_info[i].start ==
				MAP_SHARED) {
			printf("Cannot mmap = %d buffer\n", i);
			for (j = 0; j < i; j++)
				munmap(buff_info[j].start,
						buff_info[j].length);
			close(fd);
			exit(0);
		}
		memset(buff_info[i].start, 0x80, buff_info[i].length);
		
		printf("mapped = %d\n",i);
	}

	memset(&buf, 0, sizeof(buf));

        /* Enqueue buffers */
        for (i = 0; i < req.count; i++)
 	{
                buf.type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
                buf.index = i;
                buf.memory = V4L2_MEMORY_MMAP;
                ret = ioctl(fd, VIDIOC_QBUF, &buf);
                if (ret < 0) {
                        perror("VIDIOC_QBUF\n");
                        for (j = 0; j < req.count; j++)
                                munmap(buff_info[j].start,
                                                buff_info[j].length);
                        exit(0);
                }
		
		for(k=0; k<fmt.fmt.pix.bytesperline*fmt.fmt.pix.height; k++) 
		{
			buff_info[i].start[k]=0;
		}	
                printf("VIDIOC_QBUF = %d\n",i);
        }

	a = 0;
	ret = ioctl(fd, VIDIOC_STREAMON, &a);
	if (ret < 0) {
		perror("VIDIOC_STREAMON\n");
		for (i = 0; i < req.count; i++ ){
			printf("Freeing buffer %d\n",i);
			munmap(buff_info[i].start, buff_info[i].length);
		}
		close(fd);
		exit(0);
	}

	printf("STREAMON\n");
	memset(&buf, 0, sizeof(buf));
	buf.type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
	buf.index = 0;
	buf.memory = V4L2_MEMORY_MMAP;
	gettimeofday(&before, NULL);

	printf("\n");

	for(i = 0 ; 1 ;i ++) 
	{
		ret = ioctl(fd, VIDIOC_DQBUF, &buf);
		if(ret < 0){
			perror("VIDIOC_DQBUF\n");
			for (j = 0; j < req.count; j++)
				munmap(buff_info[j].start, buff_info[j].length);
			close(fd);
			exit(0);
		}

		for(k=0; k<fmt.fmt.pix.height; k++) 
		{
			c_ptr=buff_info[buf.index].start+k*fmt.fmt.pix.bytesperline;

			red  =0.0;
			green=0.0;
			blue =0.0;

			if(i%2) red  =255.0;
			else    green=255.0;
			
			Y0=16.0 +1.0/256.0*( 65.738*red+129.057*green+ 25.064*blue);
			CB=128.0+1.0/256.0*(-37.945*red- 74.494*green+112.439*blue);
			CR=128.0+1.0/256.0*(112.439*red- 94.154*green- 18.285*blue);

			for(l=0; l<fmt.fmt.pix.bytesperline; l+=4)
			{
				c_ptr[l+0]=CB;
				c_ptr[l+1]=Y0;
				c_ptr[l+2]=CR;
				c_ptr[l+3]=Y0;
			}
		}

		sleep(2);

		ret = ioctl(fd, VIDIOC_QBUF, &buf);
		if(ret < 0){
			perror("VIDIOC_QBUF\n");
			for (j = 0; j < req.count; j++)
				munmap(buff_info[j].start, buff_info[j].length);
			close(fd);
			exit(0);
		}
	}
	//fclose(fp);

	ret = ioctl(fd, VIDIOC_STREAMOFF, &a);
	if (ret < 0) 
	{
		perror("VIDIOC_STREAMOFF\n");
		for (i = 0; i < req.count; i++)
			munmap(buff_info[i].start, buff_info[i].length);
		close(fd);
		exit(0);
	}
	for (i = 0; i < req.count; i++)
		munmap(buff_info[i].start, buff_info[i].length);
	close(fd);

	printf("Success\n");

	return 0;
}