#include <cstdlib>
#include <cstdio>

#include <opencv/cv.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/contrib/contrib.hpp>

#define err(fmt, args...)	fprintf(stderr, "Error: " fmt, ##args)

int main(int argc, char **argv)
{
	int ret = 0;

	CvCapture *cap = cvCaptureFromCAM(CV_CAP_ANY);
	if (!cap) {
		err("Failed to open camera\n");
		exit(EXIT_FAILURE);
	}

	/* get initial frame to determine width/height */
	IplImage *frame = cvQueryFrame(cap);
	if (!frame) {
		err("Failed to capture frame\n");
		exit(EXIT_FAILURE);
	}

	int w = frame->width;
	int h = frame->height;
	printf("Image size is %dx%d\n", w, h);
	int ro0 = 3;	/* radius of blind spot */
	int R = 240;	/* no. of rings */
	cv::Point2i c(w / 2.0, h / 2.0);
	cv::LogPolar_Adjacent lp_adj(w, h, c, R, ro0, 0.25);
	cv::LogPolar_Overlapping lp_overlap(w, h, c, R, ro0);
	cv::LogPolar_Interp lp_bilin(w, h, c, R, ro0);
	cv::LogPolar_Interp lp_nearest(w, h, c, R, ro0, cv::INTER_NEAREST);

	cvNamedWindow("retinal", CV_WINDOW_AUTOSIZE);

	cv::Mat cortical, retinal;

	int key = 'a', i = 0;
	printf("Available modes: [a]djacent, [o]verlap, [b]ilniear, [n]earest\n");
	printf("Starting to capture, press 'q' to exit...\n");

	time_t start, end;
	int steps = 0;
	time(&start);

	while (1) {
		frame = cvQueryFrame(cap);

		if (!frame) {
			err("Failed to capture frame\n");
			ret = -1;
			break;
		}

		cv::Mat img(frame);

		if (key == 'a') {
			cv::cvtColor(img, img, CV_BGR2GRAY);
			cortical = lp_adj.to_cortical(img);
			retinal = lp_adj.to_cartesian(cortical);
		} else if (key == 'o') {
			cortical = lp_overlap.to_cortical(img);
			retinal = lp_overlap.to_cartesian(cortical);
		} else if (key == 'b') {
			cortical = lp_bilin.to_cortical(img);
			retinal = lp_bilin.to_cartesian(cortical);
		} else if (key == 'n') {
			cortical = lp_nearest.to_cortical(img);
			retinal = lp_nearest.to_cartesian(cortical);
		}

		time(&end);
		steps++;

		double s = difftime(end, start);
		double fps = steps / s;
		char buf[32];

		snprintf(buf, sizeof(buf), "FPS: %lf", fps);

		putText(img, buf, cv::Point(30, 30),
				cv::FONT_HERSHEY_COMPLEX_SMALL, 0.8,
				cv::Scalar(200,200,250), 1, CV_AA);
		imshow("retinal", retinal);

		int c = cvWaitKey(10);

		if ((c & 255) == 'p') {
			char name[128];

			snprintf(name, sizeof(name), "retinal-%d.jpg", i);
			imwrite(name, retinal);
			printf("Retinal image written to %s...\n", name);
			snprintf(name, sizeof(name), "cortical-%d.jpg", i);
			imwrite(name, cortical);
			printf("Cortical image written to %s...\n", name);
			snprintf(name, sizeof(name), "diff-%d.jpg", i);
			imwrite(name, img - cortical);
			printf("Difference image written to %s...\n", name);

			i++;
		} else if (c > 0)
			key = c;

		if (key == 'q' || (key & 255) == 27)
			break;
	}

	return ret;
}