#include <libmegapixels.h>
#include <stdio.h>
#include <limits.h>
#include <string.h>
#include <linux/videodev2.h>
#include <sys/ioctl.h>
#include <stdlib.h>
#include <sys/types.h>
#include <unistd.h>
#include <errno.h>
#include <sys/mman.h>

struct buffer {
		void *start;
		size_t length;
};
struct buffer *buffers;

int
xioctl(int fd, int request, void *arg)
{
	int r;
	do {
		r = ioctl(fd, request, arg);
	} while (r == -1 && errno == EINTR);
	return r;
}

int
main()
{
	char configpath[PATH_MAX];
	int ret = libmegapixels_find_config(configpath);
	libmegapixels_devconfig *config = {0};
	if (ret) {
		printf("Using config: %s\n", configpath);
		libmegapixels_load_file(&config, configpath);
	} else {
		fprintf(stderr, "No config found for this device\n");
		return 1;
	}

	if (config->count == 0) {
		fprintf(stderr, "No valid camera configuration\n");
		return 1;
	}

	libmegapixels_camera *camera = config->cameras[0];
	if (libmegapixels_open(camera) != 0) {
		fprintf(stderr, "Could not open default camera\n");
		return 1;
	}

	libmegapixels_mode *mode = camera->modes[0];
	unsigned int frame_size = libmegapixels_select_mode(camera, mode);
	if (frame_size == 0) {
		fprintf(stderr, "Could not select mode\n");
		return 1;
	}

	// Do the reqular V4L2 stuff to get a frame
	struct v4l2_capability cap;
	if (ioctl(camera->video_fd, VIDIOC_QUERYCAP, &cap) != 0) {
		fprintf(stderr, "VIDIOC_QUERYCAP failed\n");
		return 1;
	}
	if (!(cap.capabilities & V4L2_CAP_STREAMING)) {
		fprintf(stderr, "Device does not support streaming i/o\n");
		return 1;
	}

	struct v4l2_requestbuffers req = {0};
	req.count = 4;
	req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	req.memory = V4L2_MEMORY_MMAP;
	if (xioctl(camera->video_fd, VIDIOC_REQBUFS, &req) == -1) {
		fprintf(stderr, "VIDIOC_REQBUFS failed\n");
		return 1;
	}
	buffers = calloc(req.count, sizeof(*buffers));
	for (int i = 0; i < req.count; i++) {
		struct v4l2_buffer buf = {0};
		buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
		buf.memory = V4L2_MEMORY_MMAP;
		buf.index = i;

		if (xioctl(camera->video_fd, VIDIOC_QUERYBUF, &buf) == -1) {
			fprintf(stderr, "VIDIOC_QUERYBUF failed\n");
			return 1;
		}

		buffers[i].length = buf.length;
		buffers[i].start = mmap(NULL, buf.length, PROT_READ | PROT_WRITE, MAP_SHARED, camera->video_fd, buf.m.offset);
		if (buffers[i].start == MAP_FAILED) {
			fprintf(stderr, "mmap() failed\n");
			return 1;
		}
	}

	for (int i = 0; i < req.count; i++) {
		struct v4l2_buffer qbuf = {0};
		qbuf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
		qbuf.memory = V4L2_MEMORY_MMAP;
		qbuf.index = i;

		if (xioctl(camera->video_fd, VIDIOC_QBUF, &qbuf) == -1) {
			fprintf(stderr, "VIDIOC_QBUF failed\n");
			return 1;
		}
	}
	enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	if (xioctl(camera->video_fd, VIDIOC_STREAMON, &type) == -1) {
		fprintf(stderr, "VIDIOC_STREAMON failed\n");
		return 1;
	}


	int count = 5;
	while (count-- > 0) {
		while (1) {
			fd_set(fds);
			FD_ZERO(&fds);
			FD_SET(camera->video_fd, &fds);

			int sr = select(FD_SETSIZE, &fds, NULL, NULL, NULL);
			if (sr == -1) {
				if (errno == EINTR) {
					count++;
					continue;
				}
				fprintf(stderr, "select() failed: %s\n", strerror(errno));
				return 1;
			}


			struct v4l2_buffer buf = {0};
			buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
			buf.memory = V4L2_MEMORY_MMAP;
			if (xioctl(camera->video_fd, VIDIOC_DQBUF, &buf) == -1) {
				fprintf(stderr, "VIDIOC_DQBUF failed\n");
				return 1;
			}
			fprintf(stderr, "GOT FRAME!\n");
			if (xioctl(camera->video_fd, VIDIOC_QBUF, &buf) == -1) {
				fprintf(stderr, "VIDIOC_DQBUF failed\n");
				return 1;
			}
			break;
		}
	}
	return 0;
}