//#include "config.h"
#include <SDL/SDL.h>
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>

#include <string.h>
#include <assert.h>

#include <fcntl.h>
#include <unistd.h>
#include <errno.h>

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

#include <asm/types.h>
#include <linux/videodev2.h>
typedef long long unsigned UINT64_C;
extern "C" {
#include <libavutil/avutil.h>
#include <libswscale/swscale.h>
}

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

void showcaps(int fd);
void showcontrols(int fd);
void show_inputs(int fd);
void set_input(int fd, int idx);
void query_standard(int fd);
void query_imgfmt(int fd, v4l2_pix_format *fmt);
void query_formats(int fd);
bool show_frame(uint8_t *frame, PixelFormat fmt, int stride, int w, int h);
void init_mmap(int fd);
void* acquire_mmap_frame(int fd, struct v4l2_buffer *buffer);
void release_mmap_frame(int fd, struct v4l2_buffer *buffer);
void finalize_mmap(int fd);

uint8_t *framebuf;
uint8_t *rgbbuf;
struct mmap_buffer_item {
	void *start;
	size_t length;
	bool locked;
} *mmap_buffers= NULL;
int mmap_buffer_count= 0;

int main() {
	v4l2_pix_format fmt;
	struct v4l2_buffer buffer;
	//int lines;
	int got= 0;
	
	int fd= open("/dev/video0", O_RDWR);
	if (fd < 0) { perror("open(/dev/video0)"); exit(2); }
	
	showcaps(fd);
	showcontrols(fd);
	show_inputs(fd);
	
	set_input(fd, 1);
	query_standard(fd);
	query_imgfmt(fd, &fmt);
	query_formats(fd);
	printf("End of formats\n");
	
	int framesize= fmt.sizeimage;
	int imglines= fmt.height; // account for interlace
	//framebuf= (uint8_t*) malloc(framesize);
	//if (!framebuf) { perror("malloc"); return 1; }
	rgbbuf= (uint8_t*) malloc(fmt.width * imglines * 3);
	if (!rgbbuf) { perror("malloc"); return 1; }
	
	init_mmap(fd);
	while (1) {
		#if 0
		got= read(fd, framebuf, framesize);
		if (got < 0) { perror("read"); return 2; }
		#else
		framebuf= (uint8_t*) acquire_mmap_frame(fd, &buffer);
		got= framesize= buffer.bytesused;
		#endif
		printf("Received %d bytes\n", got);
	
	
		if (got != framesize) {
			printf("didn't get frame size\n");
		} else if (imglines * (int)fmt.bytesperline > framesize) {
			printf("frame not large enough for image\n");
		} else if (!show_frame(framebuf, PIX_FMT_UYVY422, fmt.bytesperline, fmt.width, imglines)) {
			abort();
		}
		
		release_mmap_frame(fd, &buffer);
	}
	
	//unlink("frame");
	//int out= open("frame", O_RDWR|O_CREAT, 777);
	//if (out < 0) { perror("open(frame)"); exit(2); }
	//write(out, framebuf, got);
	//close(out);

	close(fd);
	return 0;
}

struct SwsContext *convparam= NULL;
SDL_Surface *window= NULL;
SDL_Event event;

bool show_frame(uint8_t *frame, PixelFormat fmt, int stride, int w, int h) {
	if (!window) {
		if (SDL_Init(SDL_INIT_VIDEO) < 0) {
			fprintf(stderr, "Couldn't initialize SDL: %s\n", SDL_GetError());
			return false;
		}
		atexit(SDL_Quit);
		
		window = SDL_SetVideoMode(w, h, 24, SDL_SWSURFACE);
		if (!window) {
			fprintf(stderr, "Couldn't set create window: %s\n", SDL_GetError());
			return false;
		}
	}
	
	convparam= sws_getCachedContext(convparam, w, h, fmt, w, h, PIX_FMT_BGR24, SWS_BICUBIC, NULL, NULL, NULL);
	if (!convparam) {
		printf("Unable to convert image format\n");
		return false;
	}
	
	//void* pixels;
	//int pitch;
	if (SDL_LockSurface(window) < 0) {
		fprintf(stderr, "Couldn't lock texture: %s\n", SDL_GetError());
		return false;
	}
	
	uint8_t* s_data[4]= { frame, NULL, NULL, NULL };
	int s_linesize[4]= { stride, 0, 0, 0 };
	uint8_t* d_data[4]= { (uint8_t*)window->pixels, NULL, NULL, NULL };
	int d_linesize[4]= { window->pitch, 0, 0, 0 };
	sws_scale(convparam, s_data, s_linesize, 0, h, d_data, d_linesize);
	
	SDL_UnlockSurface(window);

	SDL_UpdateRect(window, 0, 0, w, h);

	bool done= false;
	while (SDL_PollEvent(&event)) {
		switch (event.type) {
		case SDL_KEYDOWN:
			if (event.key.keysym.sym == SDLK_ESCAPE) {
				done= true;
			}
			break;
		case SDL_QUIT:
			done= true;
			break;
		}
	}
	
	if (done) {
		SDL_Quit();
		window= NULL;
	}
	
	return true;
}

typedef struct {
	long long code;
	const char* name;
} const_entry_t;
/*const char * find_constant(struct const_entry_t *array, int array_len, int code) {
	for (int i=0; i<array_len; i++) {
		if (array[i].code == code)
			return array[i].name;
	}
	return "??";
}*/
#define ARRAY_LENGTH(array) ((int)(sizeof(array)/sizeof(*array)))
#define F(x) { x, #x }
static const_entry_t
v4l_cap_decode[]= {
	F(V4L2_CAP_VIDEO_CAPTURE),
	F(V4L2_CAP_VIDEO_OUTPUT),
	F(V4L2_CAP_VIDEO_OVERLAY),
	F(V4L2_CAP_VBI_CAPTURE),
	F(V4L2_CAP_VBI_OUTPUT),
	F(V4L2_CAP_SLICED_VBI_CAPTURE),
	F(V4L2_CAP_SLICED_VBI_OUTPUT),
	F(V4L2_CAP_RDS_CAPTURE),
	F(V4L2_CAP_VIDEO_OUTPUT_OVERLAY),
	F(V4L2_CAP_TUNER),
	F(V4L2_CAP_AUDIO),
	F(V4L2_CAP_RADIO),
	F(V4L2_CAP_READWRITE),
	F(V4L2_CAP_ASYNCIO),
	F(V4L2_CAP_STREAMING)
},
v4l_input_status[]= {
	F(V4L2_IN_ST_NO_POWER),
	F(V4L2_IN_ST_NO_SIGNAL),
	F(V4L2_IN_ST_NO_COLOR),
	F(V4L2_IN_ST_NO_H_LOCK),
	F(V4L2_IN_ST_COLOR_KILL),
	F(V4L2_IN_ST_NO_SYNC),
	F(V4L2_IN_ST_NO_EQU),
	F(V4L2_IN_ST_NO_CARRIER),
	F(V4L2_IN_ST_MACROVISION),
	F(V4L2_IN_ST_NO_ACCESS),
	F(V4L2_IN_ST_VTR),
},
v4l_vid_std[]= {
	F(V4L2_STD_PAL_B),
	F(V4L2_STD_PAL_B1),
	F(V4L2_STD_PAL_G),
	F(V4L2_STD_PAL_H),
	F(V4L2_STD_PAL_I),
	F(V4L2_STD_PAL_D),
	F(V4L2_STD_PAL_D1),
	F(V4L2_STD_PAL_K),
	F(V4L2_STD_PAL_M),
	F(V4L2_STD_PAL_N),
	F(V4L2_STD_PAL_Nc),
	F(V4L2_STD_PAL_60),
	F(V4L2_STD_NTSC_M),
	F(V4L2_STD_NTSC_M_JP),
	F(V4L2_STD_NTSC_443),
	F(V4L2_STD_NTSC_M_KR),
	F(V4L2_STD_SECAM_B),
	F(V4L2_STD_SECAM_D),
	F(V4L2_STD_SECAM_G),
	F(V4L2_STD_SECAM_H),
	F(V4L2_STD_SECAM_K),
	F(V4L2_STD_SECAM_K1),
	F(V4L2_STD_SECAM_L),
	F(V4L2_STD_SECAM_LC),
	F(V4L2_STD_ATSC_8_VSB),
	F(V4L2_STD_ATSC_16_VSB)
};
#undef F

void showcaps(int fd) {
	struct v4l2_capability caps;
	int ret= ioctl(fd, VIDIOC_QUERYCAP, &caps);
	if (ret < 0) { perror("ioctl(QUERYCAP)"); exit(2); }
	
	printf("Driver : %s\nCard   : %s\nBus    : %s\nVersion: %u.%u.%u\nCaps   :",
		caps.driver,
		caps.card,
		caps.bus_info,
		(caps.version>>16)&0xFF, (caps.version>>8)&0xFF, caps.version&0xFF);
	for (int i=0; i < ARRAY_LENGTH(v4l_cap_decode); i++) {
		if (caps.capabilities & v4l_cap_decode[i].code)
			printf(" %s", v4l_cap_decode[i].name);
	}
	printf("\n");
}

void showcontrols(int fd) {
	struct v4l2_queryctrl ctrl;
	int cnt= 0;
	for (int i=V4L2_CID_BASE; i < V4L2_CID_LASTP1; i++) {
		memset(&ctrl, 0, sizeof(ctrl));
		ctrl.id= i;
		int ret= ioctl(fd, VIDIOC_QUERYCTRL, &ctrl);
		if (ret < 0) {
			if (errno == EINVAL) continue;
			perror("ioctl(QUERYCTRL)"); exit(2);
		}
		cnt++;
		
		printf("Control %3d: %-20s (%d-%d)%s%s%s%s%s%s\n", i, ctrl.name, ctrl.minimum, ctrl.maximum,
			(ctrl.flags&V4L2_CTRL_FLAG_DISABLED)? " disabled":"",
			(ctrl.flags&V4L2_CTRL_FLAG_GRABBED)? " grabbed":"",
			(ctrl.flags&V4L2_CTRL_FLAG_READ_ONLY)? " r/o":"",
			(ctrl.flags&V4L2_CTRL_FLAG_UPDATE)? " update":"",
			(ctrl.flags&V4L2_CTRL_FLAG_INACTIVE)? " inact":"",
			(ctrl.flags&V4L2_CTRL_FLAG_SLIDER)? " slider":""
		);
	}
	printf("%d controls\n", cnt);
}

void show_inputs(int fd) {
	struct v4l2_input inp;
	int i= 0;
	while (1) {
		memset(&inp, 0, sizeof(inp));
		inp.index= i;
		int ret= ioctl(fd, VIDIOC_ENUMINPUT, &inp);
		if (ret < 0) {
			if (errno == EINVAL) break;
			perror("ioctl(ENUMINPUT)");
			exit(2);
		}
		i++;
		
		if (inp.type == V4L2_INPUT_TYPE_TUNER) {
			printf("Input %s: tuner %d  status:", inp.name, inp.tuner);
		} else if (inp.type == V4L2_INPUT_TYPE_CAMERA) {
			printf("Input %s: camera status:", inp.name);
		} else {
			printf("Input %s: (unknown) status:", inp.name);
		}
		for (int f= 0; f < ARRAY_LENGTH(v4l_input_status); f++) {
			if (inp.status & v4l_input_status[f].code)
				printf(" %s", v4l_input_status[i].name);
		}
		printf("\n");
	}
	printf("%d inputs.\n", i);
}

void set_input(int fd, int idx) {
	if (-1 == ioctl(fd, VIDIOC_S_INPUT, &idx)) {
		perror("VIDEOC_S_INPUT");
		exit(2);
	}
	int newVal= -1;
	if (-1 == ioctl(fd, VIDIOC_G_INPUT, &newVal)) {
		perror("VIDEOC_G_INPUT");
		exit(2);
	}
	printf("Set input to %d (new val = %d)\n", idx, newVal);
}

void query_standard(int fd) {
	v4l2_std_id std_id;
	struct v4l2_standard standard;

	if (-1 == ioctl (fd, VIDIOC_G_STD, &std_id)) {
			/* Note when VIDIOC_ENUMSTD always returns EINVAL this
			   is no video device or it falls under the USB exception,
			   and VIDIOC_G_STD returning EINVAL is no error. */

			perror ("VIDIOC_G_STD");
			exit (2);
	}

	memset (&standard, 0, sizeof (standard));
	standard.index = 0;

	while (0 == ioctl (fd, VIDIOC_ENUMSTD, &standard)) {
			if (standard.id & std_id) {
				   printf ("Current video standard: %s\n", standard.name);
				   return;
			}

			standard.index++;
	}
	printf("Unknown standard\n");
}

void query_formats(int fd) {
	v4l2_fmtdesc fmtd;
	int i=0;
	do {
		memset(&fmtd, 0, sizeof(fmtd));
		fmtd.index= i++;
		int ret= ioctl(fd, VIDIOC_ENUM_FMT, &fmtd);
		if (ret < 0) {
			if (errno == EINVAL) return;
			perror("ioctl(ENUM_FMT)");
		}
		
		printf("Pixel Format %s: type=%d flags=%d pixelformat=%d (%4.4s)\n", fmtd.description, fmtd.type, fmtd.flags, fmtd.pixelformat, (char*)&fmtd.pixelformat);
	} while (1);
}

void query_imgfmt(int fd, v4l2_pix_format *fmt) {
	v4l2_format vfmt;
	v4l2_pix_format *pix= &vfmt.fmt.pix;
	memset(&vfmt, 0, sizeof(vfmt));
	vfmt.type= V4L2_BUF_TYPE_VIDEO_CAPTURE;
	if (ioctl(fd, VIDIOC_G_FMT, &vfmt) < 0) {
		perror("ioctl(G_FMT)");
		exit(1);
	}
	printf("Image Format: %d x %d %4.4s field=%d pitch=%d size=%d colorspace=%d\n",
		pix->width, pix->height, (char*)&pix->pixelformat, pix->field, pix->bytesperline, pix->sizeimage, pix->colorspace);
	
	if (fmt) memcpy(fmt, pix, sizeof(*fmt));
}

void init_mmap(int fd) {
	struct v4l2_requestbuffers reqbuf;
	struct v4l2_buffer buffer;
	unsigned int i;
	
	memset(&reqbuf, 0, sizeof(reqbuf));
	reqbuf.type= V4L2_BUF_TYPE_VIDEO_CAPTURE;
	reqbuf.memory= V4L2_MEMORY_MMAP;
	reqbuf.count= 8;
	
	if (-1 == ioctl(fd, VIDIOC_REQBUFS, &reqbuf)) {
		if (errno == EINVAL)
			printf("Video capturing or mmap-streaming is not supported\n");
		else
			perror("VIDIOC_REQBUFS");

		exit(2);
	}
	
	/* We want at least five buffers. */
	mmap_buffer_count= reqbuf.count;
	if (mmap_buffer_count < 5) {
		/* You may need to free the buffers here. */
		printf("Not enough buffer memory\n");
		exit(2);
	}

	mmap_buffers= (struct mmap_buffer_item*) realloc(mmap_buffers, mmap_buffer_count * sizeof(*mmap_buffers));
	assert(mmap_buffers != NULL);
	memset(mmap_buffers, 0, mmap_buffer_count * sizeof(*mmap_buffers));

	for (i = 0; i < mmap_buffer_count; i++) {
		memset(&buffer, 0, sizeof(buffer));
		buffer.type= reqbuf.type;
		buffer.memory= V4L2_MEMORY_MMAP;
		buffer.index= i;

		if (-1 == ioctl(fd, VIDIOC_QUERYBUF, &buffer)) {
			perror("VIDIOC_QUERYBUF");
			exit(2);
		}
		
		mmap_buffers[i].length= buffer.length; /* remember for munmap() */

		mmap_buffers[i].start= mmap(NULL, buffer.length,
			PROT_READ | PROT_WRITE, /* recommended */
			MAP_SHARED,             /* recommended */
			fd, buffer.m.offset);

		if (MAP_FAILED == mmap_buffers[i].start) {
			/* If you do not exit here you should unmap() and free()
			   the buffers mapped so far. */
			perror("mmap");
			exit(2);
		}
		
		if (-1 == ioctl(fd, VIDIOC_QBUF, &buffer)) {
			perror("VIDIOC_QBUF");
			exit(2);
		}
	}
}

void* acquire_mmap_frame(int fd, struct v4l2_buffer *buffer) {
	int i;
	
	memset(buffer, 0, sizeof(*buffer));
	buffer->type= V4L2_BUF_TYPE_VIDEO_CAPTURE;
	buffer->memory= V4L2_MEMORY_MMAP;
	buffer->index= i;
	if (-1 == ioctl(fd, VIDIOC_DQBUF, buffer)) {
		perror("VIDIOC_DQBUF");
		exit(2);
	}
	if (buffer->index < 0 || buffer->index >= mmap_buffer_count) {
		printf("Buffer index out of bounds! %d\n", buffer->index);
		exit(2);
	}
	return mmap_buffers[buffer->index].start;
}

void release_mmap_frame(int fd, struct v4l2_buffer *buffer) {
	if (-1 == ioctl(fd, VIDIOC_QBUF, buffer)) {
		perror("VIDIOC_QBUF");
		exit(2);
	}
}

void finalize_mmap(int fd) {
	int i;
	for (i = 0; i < mmap_buffer_count; i++)
		munmap(mmap_buffers[i].start, mmap_buffers[i].length);
	free(mmap_buffers);
	mmap_buffers= NULL;
}