#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 <errno.h>
#include <sys/mman.h>
#include <getopt.h>
#include <ctype.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;
}

void
usage(char *name)
{
	fprintf(stderr, "Usage: %s [-h] [-n offset] [-c camera] [-o file]\n", name);
	fprintf(stderr, "Get a frame using libmegapixels\n\n");
	fprintf(stderr, "Arguments:\n");
	fprintf(stderr, "  -n offset   Get the Nth frame from the output (defaults to 5)\n");
	fprintf(stderr, "  -c camera   Use a specific camera number\n");
	fprintf(stderr, "  -m modenum  Use another camera mode than the first\n");
	fprintf(stderr, "  -o file     File to store the frame in\n");
	fprintf(stderr, "  -v          Show verbose debug info\n");
	fprintf(stderr, "  -h          Display this help text\n");
}

int
main(int argc, char *argv[])
{
	int c;
	int camera_id = 0;
	long res;
	char *end;
	char *outfile = NULL;
	int count = 5;
	int mode_idx = 0;
	int verbose = 0;

	while ((c = getopt(argc, argv, "hvc:n:o:m:")) != -1) {
		switch (c) {
			case 'c':
				res = strtol(optarg, &end, 10);
				if (end == optarg || end == NULL || *end != '\0') {
					fprintf(stderr, "Invalid number for -c\n");
					return 1;
				}
				camera_id = (int) res;
				break;
			case 'o':
				outfile = optarg;
				break;
			case 'n':
				res = strtol(optarg, &end, 10);
				if (end == optarg || end == NULL || *end != '\0') {
					fprintf(stderr, "Invalid number for -n\n");
					return 1;
				}
				count = (int) res;
				break;
			case 'm':
				res = strtol(optarg, &end, 10);
				if (end == optarg || end == NULL || *end != '\0') {
					fprintf(stderr, "Invalid number for -m\n");
					return 1;
				}
				mode_idx = (int) res;
				break;
			case 'h':
				usage(argv[0]);
				return 0;
				break;
			case 'v':
				verbose = 1;
				break;
			case '?':
				if (optopt == 'd' || optopt == 'l') {
					fprintf(stderr, "Option -%c requires an argument.\n", optopt);
				} else if (isprint(optopt)) {
					fprintf(stderr, "Unknown option '-%c'\n", optopt);
				} else {
					fprintf(stderr, "Unknown option character x%x\n", optopt);
				}
				return 1;
			default:
				usage(argv[0]);
				return 1;
		}
	}


	char configpath[PATH_MAX];
	int ret = libmegapixels_find_config(configpath);
	libmegapixels_devconfig *config = {0};
	libmegapixels_init(&config);
	if (verbose) {
		libmegapixels_loglevel(2);
	}

	if (ret) {
		printf("Using config: %s\n", configpath);
		libmegapixels_load_file(config, configpath);
	} else {
		fprintf(stderr, "No config found for this device\n");
	}
	libmegapixels_load_uvc(config);

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

	if (camera_id > config->count - 1) {
		fprintf(stderr, "Camera id %d does not exist\n", camera_id);
		return 1;
	}

	libmegapixels_camera *camera = config->cameras[camera_id];
	if (libmegapixels_open(camera) != 0) {
		fprintf(stderr, "Could not open default camera\n");
		return 1;
	}
	if (mode_idx > camera->num_modes) {
		fprintf(stderr, "Invalid mode index: %d\n", mode_idx);
	}

	libmegapixels_mode *mode = camera->modes[mode_idx];
	struct v4l2_format format = {0};
	unsigned int frame_size = libmegapixels_select_mode(camera, mode, &format);
	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;
	int mplanes = 0;
	enum v4l2_buf_type buftype = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	if (ioctl(camera->video_fd, VIDIOC_QUERYCAP, &cap) != 0) {
		fprintf(stderr, "VIDIOC_QUERYCAP failed: %s\n", strerror(errno));
		return 1;
	}
	if (!(cap.capabilities & V4L2_CAP_STREAMING)) {
		fprintf(stderr, "Device does not support streaming i/o\n");
		return 1;
	}
	if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE)) {
		if (cap.capabilities & V4L2_CAP_VIDEO_CAPTURE_MPLANE) {
			mplanes = 1;
			buftype = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
		} else {
			fprintf(stderr, "Device does not support V4L2_CAP_VIDEO_CAPTURE\n");
			return 1;
		}
	}

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

		struct v4l2_plane planes[1];
		if (mplanes) {
			buf.m.planes = planes;
			buf.length = 1;
		}

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

		unsigned int offset;
		if (mplanes) {
			buffers[i].length = planes[0].length;
			offset = planes[0].m.mem_offset;
		} else {
			buffers[i].length = buf.length;
			offset = buf.m.offset;
		}
		buffers[i].start = mmap(NULL, buffers[i].length, PROT_READ | PROT_WRITE, MAP_SHARED, camera->video_fd, 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 = buftype;
		qbuf.memory = V4L2_MEMORY_MMAP;
		qbuf.index = i;

		if (mplanes) {
			struct v4l2_plane qplanes[1];
			qbuf.m.planes = qplanes;
			qbuf.length = 1;
		}

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


	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 = buftype;
			buf.memory = V4L2_MEMORY_MMAP;
			if (mplanes) {
				struct v4l2_plane dqplanes[1];
				buf.m.planes = dqplanes;
				buf.length = 1;
			}
			if (xioctl(camera->video_fd, VIDIOC_DQBUF, &buf) == -1) {
				fprintf(stderr, "VIDIOC_DQBUF failed\n");
				return 1;
			}

			fprintf(stderr, "received frame\n");

			if (count == 0 && outfile != NULL) {
				FILE *fp = fopen(outfile, "w");
				fwrite(buffers[buf.index].start, buf.bytesused, 1, fp);
				fclose(fp);
				printf("Stored frame to: %s\n", outfile);
				printf("Format: %dx%d\n", mode->width, mode->height);

				char fourcc[5] = {0};
				fourcc[0] = (char) (mode->v4l_pixfmt & 0xff);
				fourcc[1] = (char) ((mode->v4l_pixfmt >> 8) & 0xff);
				fourcc[2] = (char) ((mode->v4l_pixfmt >> 16) & 0xff);
				fourcc[3] = (char) ((mode->v4l_pixfmt >> 24) & 0xff);
				printf("Pixfmt: %s\n", fourcc);
			}

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