/*
 * OMAP3 live video display test application
 *
 * Copyright (C) 2010-2011 Ideas on board SPRL
 *
 * Contact: Laurent Pinchart <laurent.pinchart@ideasonboard.com>
 *
 * 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.,
 * 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 */

#define _BSD_SOURCE
#include <dirent.h>
#include <endian.h>
#include <errno.h>
#include <fcntl.h>
#include <getopt.h>
#include <signal.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <time.h>
#include <unistd.h>

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

#include <linux/fb.h>
#include <linux/spi/spidev.h>

#include "isp/list.h"
#include "isp/omap3isp.h"
#include "isp/tools.h"
#include "isp/v4l2.h"
#include "isp/v4l2-pool.h"

#include "videoout.h"

#define MEDIA_DEVICE		"/dev/media0"
#define VIDEOOUT_DEVICE		"/dev/video0"
#define SELECT_TIMEOUT		2000		/* in milliseconds */

static struct timespec fps_ts = { 0, 0 };
static unsigned int fps_last = 0;
static bool fps_show = true;

static unsigned int frame_count = 0;
static unsigned int frame_skip = 0;

static struct omap3_isp_device *isp = NULL;
static struct videoout *vo = NULL;

static bool done = false;

/* -----------------------------------------------------------------------------
 * Events
 */

struct event_fd {
	struct list_entry list;

	int fd;
	bool write;
	void (*callback)(void *priv);
	void *priv;
};

static struct {
	struct list_entry events;

	int maxfd;
	fd_set rfds;
	fd_set wfds;
} events;

static void events_watch_fd(int fd, bool write, void(*callback)(void *), void *priv)
{
	struct event_fd *event;

	event = malloc(sizeof *event);
	if (event == NULL)
		return;

	event->fd = fd;
	event->write = write;
	event->callback = callback;
	event->priv = priv;

	if (write)
		FD_SET(fd, &events.wfds);
	else
		FD_SET(fd, &events.rfds);

	events.maxfd = max(events.maxfd, fd);

	list_append(&event->list, &events.events);
}

static void events_unwatch_fd(int fd)
{
	struct event_fd *event = NULL;
	struct event_fd *entry;
	int maxfd = 0;

	list_for_each_entry(entry, &events.events, list) {
		if (entry->fd == fd)
			event = entry;
		else
			maxfd = max(maxfd, entry->fd);
	}

	if (event == NULL)
		return;

	if (event->write)
		FD_CLR(fd, &events.wfds);
	else
		FD_CLR(fd, &events.rfds);

	events.maxfd = maxfd;

	list_remove(&event->list);
	free(event);
}

static void events_init(void)
{
	memset(&events, 0, sizeof events);

	FD_ZERO(&events.rfds);
	FD_ZERO(&events.wfds);
	events.maxfd = 0;
	list_init(&events.events);
}

static void events_dispatch(const fd_set *rfds, const fd_set *wfds)
{
	struct event_fd *event;

	list_for_each_entry(event, &events.events, list) {
		if (!event->write && FD_ISSET(event->fd, rfds))
			event->callback(event->priv);
		if (event->write && FD_ISSET(event->fd, wfds))
			event->callback(event->priv);
	}
}

/* -----------------------------------------------------------------------------
 * Video input
 */

static void viewfinder_process(struct omap3_isp_device *isp __attribute__((__unused__)),
			       struct v4l2_video_buffer *buffer)
{
	struct timespec ts;
	float interval;
	int ret;

	frame_count++;

	if (fps_show){
		clock_gettime(CLOCK_MONOTONIC, &ts);

		interval = ts.tv_sec - fps_ts.tv_sec;
		interval += (ts.tv_nsec - fps_ts.tv_nsec) / 1000000000.0;

		if (interval >= 5) {
			if (frame_count > 1)
				printf("frame rate: %f fps\n",
				      (frame_count - fps_last) / interval);

			fps_last = frame_count;
			fps_ts = ts;
		}
	}

	if (buffer->error) {
		printf("warning: error in dequeued buffer, skipping\n");
		return;
	}

	/* Queue the buffer to the display, or requeue it to the viewfinder if
	 * it needs to be skipped.
	 */
	if ((frame_count % (frame_skip + 1)) == 0) {
		vo_queue_buffer(vo, buffer);
		return;
	}

	ret = omap3_isp_viewfinder_put_buffer(isp, buffer);
	if (ret < 0)
		printf("error: unable to requeue buffer: %s (%d)\n",
		       strerror(-ret), ret);
}


static void events_watch_fd_isp(int fd, enum omap3_isp_event_type type,
				void (*callback)(void *priv), void *priv)
{
	events_watch_fd(fd, type == OMAP3_ISP_EVENT_WRITE, callback, priv);
}

static struct omap3_isp_operations isp_ops = {
	.viewfinder_ready = viewfinder_process,
	.watch_fd = events_watch_fd_isp,
	.unwatch_fd = events_unwatch_fd,
};

/* -----------------------------------------------------------------------------
 * Video output
 */

static void video_out_process(void *priv __attribute__((__unused__)))
{
	struct v4l2_video_buffer buffer;
	int ret;

	ret = vo_dequeue_buffer(vo, &buffer);
	if (ret < 0)
		return;

	/* Requeue the buffer */
	ret = omap3_isp_viewfinder_put_buffer(isp, &buffer);
	if (ret < 0) {
		printf("error: unable to requeue buffer: %s (%d)\n",
		       strerror(-ret), ret);
		return;
	}
}

static void events_watch_fd_vo(int fd)
{
	events_watch_fd(fd, true, video_out_process, NULL);
}

static struct video_out_operations vo_ops = {
	.watch_fd = events_watch_fd_vo,
	.unwatch_fd = events_unwatch_fd,
};

static const char *video_out_find(void)
{
	unsigned int video_idx = 256;
	static char devname[14];
	struct stat devstat;
	struct dirent *ent;
	char *end;
	DIR *dir;
	int ret;

	dir = opendir("/sys/bus/platform/devices/omap_vout/video4linux");
	if (dir == NULL)
		return NULL;

	while ((ent = readdir(dir)) != NULL) {
		unsigned int idx;

		if (strncmp(ent->d_name, "video", 5))
			continue;

		idx = strtoul(ent->d_name + 5, &end, 10);
		if (*end != '\0')
			continue;

		if (idx < video_idx)
			video_idx = idx;
	}

	closedir(dir);

	if (video_idx == 256)
		return NULL;

	sprintf(devname, "/dev/video%u", video_idx);
	ret = stat(devname, &devstat);
	if (ret < 0)
		return NULL;

	/* Sanity check: udev might have reordered the device nodes.
	 * Make sure the major/minor match. We should really use libudev.
	 */
	if (major(devstat.st_rdev) != 81 || minor(devstat.st_rdev) != video_idx)
		return NULL;

	return devname;
}

/* -----------------------------------------------------------------------------
 * Frame buffer
 */

static int fb_init(struct v4l2_rect *rect)
{
	struct fb_fix_screeninfo fix;
	struct fb_var_screeninfo var;
	unsigned int i;
	void *mem = NULL;
	int ret;
	int fd;

	fd = open("/dev/fb0", O_RDWR);
	if (fd == -1)
		return -ENODEV;

	ret = ioctl(fd, FBIOGET_FSCREENINFO, &fix);
	if (ret < 0) {
		printf("error: failed to get fixed screen info\n");
		goto done;
	}

	ret = ioctl(fd, FBIOGET_VSCREENINFO, &var);
	if (ret < 0) {
		printf("error: failed to get variable screen info\n");
		goto done;
	}

	mem = mmap(NULL, fix.smem_len, PROT_READ | PROT_WRITE, MAP_SHARED,
		   fd, 0);
	if (mem == MAP_FAILED) {
		printf("error: unable to map frame buffer\n");
		ret = -ENOMEM;
		goto done;
	}

	/* Fill the frame buffer with the background color. */
	for (i = 0; i < fix.smem_len; i += 4)
		*(uint32_t *)(mem + i) = 0x00123456;

	/* Return the frame buffer size. */
	rect->left = var.xoffset;
	rect->top = var.yoffset;
	rect->width = var.xres;
	rect->height = var.yres;

done:
	if (mem != NULL)
		munmap(mem, fix.smem_len);
	close(fd);

	return ret;
}

/* -------------------------------------------------------------------------- */

static void sigint_handler(int signal __attribute__((__unused__)))
{
	/* Set the done flag to true when the user presses CTRL-C to interrupt
	 * the main loop.
	 */
	done = true;
}

static void usage(const char *argv0)
{
	printf("Usage: %s [options]\n", argv0);
	printf("Supported options:\n");
	printf("-b, --buffers n		Use n display buffers\n");
	printf("-h, --help		Show this help screen\n");
	printf("-s, --skip n		Skip display of n frames out of n+1\n");
}

static struct option opts[] = {
	{ "help", 0, 0, 'h' },
	{ "skip", 1, 0, 's' },
	{ 0, 0, 0, 0 }
};

int main(int argc __attribute__((__unused__)), char *argv[] __attribute__((__unused__)))
{
	struct v4l2_mbus_framefmt view_format;
	struct v4l2_buffers_pool *pool = NULL;
	struct timespec start, end;
	unsigned int buffers = 3;
	struct v4l2_rect rect;
	int exit_code = EXIT_FAILURE;
	const char *vo_devname;
	float fps;
	int ret;
	int c;

	while ((c = getopt_long(argc, argv, "b:hs:", opts, NULL)) != -1) {
		switch (c) {
		case 'b':
			buffers = atoi(optarg);
			break;
		case 'h':
			usage(argv[0]);
			return 0;
		case 's':
			frame_skip = atoi(optarg);
			break;
		default:
			printf("Invalid option -%c\n", c);
			printf("Run %s -h for help.\n", argv[0]);
			return 1;
		}
	}

	events_init();

	memset(&rect, 0, sizeof rect);
	ret = fb_init(&rect);
	if (ret < 0) {
		printf("error: unable to initialize frame buffer\n");
		goto cleanup;
	}

	/* Register a signal handler for SIGINT, received when the user presses
	 * CTRL-C. This will allow the main loop to be interrupted, and resources
	 * to be freed cleanly.
	 */
	signal(SIGINT, sigint_handler);

	/* Open the OMAP3 ISP device and setup the capture pipeline. */
	isp = omap3_isp_open(MEDIA_DEVICE, &isp_ops);
	if (isp == NULL) {
		printf("error: unable to open media device %s\n", MEDIA_DEVICE);
		goto cleanup;
	}

	memset(&view_format, 0, sizeof view_format);
	view_format.code = V4L2_MBUS_FMT_YUYV8_1X16;
	view_format.width = rect.width;
	view_format.height = rect.height;

	ret = omap3_isp_viewfinder_setup(isp, &view_format);
	if (ret < 0) {
		printf("error: unable to setup pipeline\n");
		goto cleanup;
	}

	printf("viewfinder configured for %04x %ux%u\n",
	       view_format.code, view_format.width, view_format.height);

	/* Initialize video output. */
	vo_devname = video_out_find();
	if (vo_devname == NULL) {
		printf("error: unable to find video output device\n");
		goto cleanup;
	}

	vo = vo_init(vo_devname, &vo_ops, buffers, rect.width, rect.height);
	if (vo == NULL) {
		printf("error: unable to initialize video output\n");
		goto cleanup;
	}

	vo_enable_colorkey(vo, 0x123456);

	/* Allocate a buffers pool and use it for the viewfinder. */
	pool = vo_get_pool(vo);
	if (pool == NULL) {
		printf("error: unable to get video output buffers pool\n");
		goto cleanup;
	}

	ret = omap3_isp_viewfinder_set_pool(isp, pool);
	if (ret < 0) {
		printf("error: unable to set buffers pool\n");
		goto cleanup;
	}

	/* Start the ISP. */
	ret = omap3_isp_viewfinder_start(isp);
	if (ret < 0)
		goto cleanup;

	clock_gettime(CLOCK_MONOTONIC, &start);

	/* Main capture loop. Wait for a video buffer using select() and process
	 * it.
	 */
	while (!done) {
		struct timeval timeout;
		fd_set rfds;
		fd_set wfds;

		timeout.tv_sec = SELECT_TIMEOUT / 1000;
		timeout.tv_usec = (SELECT_TIMEOUT % 1000) * 1000;
		rfds = events.rfds;
		wfds = events.wfds;

		ret = select(events.maxfd + 1, &rfds, &wfds, NULL, &timeout);
		if (ret < 0) {
			/* EINTR means that a signal has been received, continue
			 * to the next iteration in that case.
			 */
			if (errno == EINTR)
				continue;

			printf("error: select failed with %d\n", errno);
			goto cleanup;
		}
		if (ret == 0) {
			/* select() should never time out as the ISP is supposed
			 * to capture images continuously. A timeout is thus
			 * considered as a fatal error.
			 */
			printf("error: select timeout\n");
			goto cleanup;
		}

		events_dispatch(&rfds, &wfds);
	}

	clock_gettime(CLOCK_MONOTONIC, &end);

	/* Stop the ISP. */
	omap3_isp_viewfinder_stop(isp);

	/* Print some statistics. */
	end.tv_sec -= start.tv_sec;
	end.tv_nsec -= start.tv_nsec;
	if (end.tv_nsec < 0) {
		end.tv_sec--;
		end.tv_nsec += 1000000000;
	}

	fps = frame_count / (end.tv_sec + end.tv_nsec / 1000000000.0);

	printf("%u images processed in %lu.%06lu seconds (%f fps)\n",
	       frame_count, end.tv_sec, end.tv_nsec / 1000, fps);

	exit_code = EXIT_SUCCESS;

cleanup:
	/* Cleanup the ISP and video output resources. */
	if (isp)
		omap3_isp_close(isp);
	if (vo)
		vo_cleanup(vo);

	return exit_code;
}