#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <fcntl.h>		/* low-level i/o */
#include <unistd.h>
#include <errno.h>
#include <sys/types.h>
#include <sys/time.h>
#include <sys/mman.h>
#include <sys/ioctl.h>
#include <fcntl.h>
#include <linux/fb.h>
#include <asm/types.h>		/* for videodev2.h */
#include <pthread.h>
#include <time.h>
#include <linux/videodev.h>
#include <media/davinci/davinci_vpfe.h>	/*kernel header file, prefix path comes from
				   makefile */
//#include "omnivision_capt.h"
#include <media/davinci/ccdc_dm355.h>


//#include "mt9t001_capture.h"
//#include "display.h"


#define CLEAR(x) memset (&(x), 0, sizeof (x))
#define CAP_FORMAT		22//(V4L2_STD_OV9710_WVGA_30FPS)
#define MIN_BUFFERS	3

struct buffer {
	void *start;
	int offset;
	int length;
	int index;
};

static struct buffer *buffers = NULL;
static int nBuffers = 0;
int fdCapture;
int *temp;

static int configCCDCraw();
static int SetDataFormat(v4l2_std_id std, int width, int height );
static int InitCaptureBuffers(void);
void CaptureFrame(int *phyaddr);
int StartStreaming(void);

int main(void)
{
   fdCapture = -1;
   struct v4l2_capability cap;
   int ret = -1;
   int *capbuf_phyaddr;
   int fdOut = -1;
   int test;
   struct v4l2_buffer *arg;

   if ((fdCapture = open("/dev/video0", O_RDWR | O_NONBLOCK, 0)) <= -1)
   {
      perror("InitDevice:open::");
		exit(0);
	}
	else
	{
	   printf("\nOpen Done\n");
	}
	
	/*Is capture supported? */
	if (-1 == ioctl(fdCapture, VIDIOC_QUERYCAP, &cap))
	{
		perror("InitDevice:ioctl:VIDIOC_QUERYCAP:");
		exit(0);
	}
	else
	{
		printf("\nQUERYCAP Done\n");
	}

	if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE))
	{
		printf("InitDevice:capture is not supported on /dev/video0");
		exit(0);
	}
	
	/*is MMAP-IO supported? */
	if (!(cap.capabilities & V4L2_CAP_STREAMING))
	{
		printf("InitDevice:IO method MMAP is not supported on /dev/video0");
		exit(0);
	}
	
	printf("\nCalling configCCDCraw()\n");
	ret = configCCDCraw();
	if (ret < 0) {
		perror("configCCDCraw");
		exit(0);
	} else {
		printf("\nconfigCCDCraw Done\n");
	}
	SetDataFormat(CAP_FORMAT, 640, 400);
	
	printf("initializing capture buffers\n");
	InitCaptureBuffers();

	printf("starting streaming\n");
	StartStreaming();
	
	test = 0;
	
	
	while(1)
	{
	   CaptureFrame(capbuf_phyaddr);
	   
		printf("ADDR: %u\n", *capbuf_phyaddr);
		
//******************************************************
// Here I want to read the buffer and write it to a file
//******************************************************

		//fdOut = open("pic.raw", O_RDWR, 0);
		//if(fdOut < 0)
		//{
		//   printf("ERROR OPEN");
		//}
		
		//printf("write to file...\n");
		//test = write(fdOut, (*capbuf_phyaddr+(*temp)), sizeof(640*400*2));
		//printf("Bytes written: %i\n", test);
		
		//close(fdOut);
	}
	
   return 0;
}

static int SetDataFormat(v4l2_std_id std, int width, int height)
{
	v4l2_std_id cur_std;
	struct v4l2_format fmt;

	cur_std = std;


	printf("SetDataFormat:setting std to %d\n", (int)cur_std);

	//if (-1 == ioctl(fdCapture, VIDIOC_S_STD, &cur_std))
	//{
	//	printf("SetDataFormat:unable to set standard automatically\n");
	//	exit(0);
	//}
	//else
	//{
	   printf("\nS_STD Done\n");
	//}

	sleep(1);		/* wait until device is fully locked */
	
	cur_std = 0;
	if (-1 == ioctl(fdCapture, VIDIOC_G_STD, &cur_std))
	{
		perror("SetDataFormat:ioctl:VIDIOC_G_STD:");
	}
	else
	{
		printf("\nGetSTD Done WITH std = %u\n", (int) cur_std);
	}

	printf("SetDataFormat:requesting width:%d height:%d\n", width, height);
	CLEAR(fmt);
	fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	fmt.fmt.pix.width = width;
	fmt.fmt.pix.height = height;
	fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_SBGGR8;

	/* the field can be either interlaced together or
	   separated in a top, bottom fashion */
	fmt.fmt.pix.field = V4L2_FIELD_NONE;
	if (-1 == ioctl(fdCapture, VIDIOC_S_FMT, &fmt))
	{
		perror("SetDataFormat:ioctl:VIDIOC_S_FMT");
		exit(0);
	}
	else
	{
		printf("\nS_FMT Done\n");
	}
		

	return 0;
}


static int InitCaptureBuffers(void)
{
	struct v4l2_requestbuffers req;
	int nIndex = 0;

	CLEAR(req);
	req.count = MIN_BUFFERS;
	req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	req.memory = V4L2_MEMORY_MMAP;

	if (-1 == ioctl(fdCapture, VIDIOC_REQBUFS, &req))
	{
		perror("InitCaptureBuffers:ioctl:VIDIOC_REQBUFS");
		exit(0);
	}
	else
	{
		printf("\nREQBUF Done\n");
   }

	if (req.count < MIN_BUFFERS)
	{
		printf("InitCaptureBuffers only:%d buffers avilable,\n", req.count);
		exit(0);
	}

	nBuffers = req.count;
	
	buffers = (struct buffer *) calloc(req.count, sizeof(struct buffer));
	
	if (!buffers)
	{
		perror("InitCaptureBuffers:calloc:");
		exit(0);
	}

	for (nIndex = 0; nIndex < req.count; ++nIndex)
	{
		struct v4l2_buffer buf;
		CLEAR(buf);
		buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
		buf.memory = V4L2_MEMORY_MMAP;
		buf.index = nIndex;
      

		if (-1 == ioctl(fdCapture, VIDIOC_QUERYBUF, &buf))
		{
			perror("InitCaptureBuffers:ioctl:VIDIOC_QUERYBUF:\n");
			exit(0);
		}
		else
		{
			printf("\nQUERYBUF Done\n");
		}

		buffers[nIndex].length = buf.length;
		
		printf("buf.length= %i\n", buf.length);

      temp = mmap(NULL, buf.length, PROT_READ | PROT_WRITE, MAP_SHARED, fdCapture, buf.m.offset);
		buffers[nIndex].start = temp;

		buffers[nIndex].offset = buf.m.offset;

      printf("temp: %i\n", *temp);
      printf("buf.m.offset: %i\n", buf.m.offset);

		if (MAP_FAILED == buffers[nIndex].start)
		{
			perror("InitCaptureBuffers:mmap:");
			exit(0);
		}
	}

	return 0;
}

static int configCCDCraw()
{
	//struct mt9t001_params mtparams;
	ccdc_config_params_raw raw_params = {
                            .pix_fmt = CCDC_PIXFMT_RAW,
                            .frm_fmt = CCDC_FRMFMT_PROGRESSIVE,
                            .win = VPFE_WIN_WVGA,
                            .fid_pol = CCDC_PINPOL_POSITIVE,
                            .vd_pol = CCDC_PINPOL_POSITIVE,
                            .hd_pol = CCDC_PINPOL_POSITIVE,
                            .image_invert_enable = FALSE,
                            .data_sz = _12BITS,
                            .med_filt_thres = 0,
                            .mfilt1 = NO_MEDIAN_FILTER1,
                            .mfilt2 =  NO_MEDIAN_FILTER2,
                            .ccdc_offset = 0,
                            .lpf_enable = FALSE,
                            .datasft = 2,
                            .alaw = {
                                    .b_alaw_enable = FALSE,
                                     .gama_wd = 0},
                            .blk_clamp = {
                                          .b_clamp_enable = FALSE,
                                          .sample_pixel = 1,
                                          .start_pixel = 0,
                                          .dc_sub = 0},
                            .blk_comp = {
                                         .b_comp = 0,
                                         .gb_comp = 0,
                                         .gr_comp = 0,
                                         .r_comp = 0},
                            .vertical_dft = {
                                          .ver_dft_en = FALSE},
                            .lens_sh_corr = {
                                          .lsc_enable = FALSE},
			                   .data_formatter_r = {
                                          	.fmt_enable = FALSE},
                            .color_space_con = {
                                          	.csc_enable = FALSE}
                      };


	if (-1 == ioctl(fdCapture, VPFE_CMD_CONFIG_CCDC_RAW, &raw_params))
	{
		perror("InitDevice:ioctl:VPFE_CMD_CONFIG_CCDC_RAW:");
		exit(0);
	}
	//if (-1 == ioctl(fdCapture, VPFE_CMD_G_MT9T001_PARAMS, &mtparams)) {
	//	perror("InitDevice:ioctl: VPFE_CMD_G_MT9T001_PARAMS");
	//	exit(0);
	//}

	//printf("the col size is : %d \n", mtparams.format.col_size);
	
	return 0;
}

int StartStreaming(void)
{
	int i = 0;
	enum v4l2_buf_type type;

	for (i = 0; i < nBuffers; i++)
	{
		struct v4l2_buffer buf;
		CLEAR(buf);
		buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
		buf.memory = V4L2_MEMORY_MMAP;
		buf.index = i;
		printf("Queing buffer:%d\n", i);

		if (-1 == ioctl(fdCapture, VIDIOC_QBUF, &buf))
		{
			perror("StartStreaming:ioctl:VIDIOC_QBUF:");
		}
		else
		{
			printf("\nQ_BUF Done\n");
      }
      
	}
	/* all done , get set go */
	type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	if (-1 == ioctl(fdCapture, VIDIOC_STREAMON, &type))
	{
		perror("StartStreaming:ioctl:VIDIOC_STREAMON:");
	}
	else
	{
		printf("\nSTREAMON Done\n");
	}

	return 0;
}

void CaptureFrame(int *phyaddr)
{
	
	static struct v4l2_buffer buf;
	enum v4l2_buf_type type;
	static int init_flag = 0;
	type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	if(!init_flag)
	{
		CLEAR(buf);
		buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
		buf.memory = V4L2_MEMORY_MMAP;
		init_flag = 1;
	}
	else
	{
		/* requeue the buffer */
		if (-1 == ioctl(fdCapture, VIDIOC_QBUF, &buf))
		{
			perror("StartCameraCaputre:ioctl:VIDIOC_QBUF");
		}
	}

	/*determine ready buffer */
	if (-1 == ioctl(fdCapture, VIDIOC_DQBUF, &buf))
	{
		if (EAGAIN == errno)
		{
			return ;
		}
		perror("StartCameraCaputre:ioctl:VIDIOC_DQBUF");
		exit(0);
	}
   *phyaddr = buffers[buf.index].offset;
	
}