#include <iostream>
#include <iomanip>
#include <vector>
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/features2d/features2d.hpp>

#include "CameraCalibrator.h"

cv::Mat applyLookUp(const cv::Mat &image, const cv::MatND &lookup)
{
	cv::Mat result(image.rows, image.cols, CV_8U);

	cv::Mat_<uchar>::iterator itr = result.begin<uchar>();
	cv::Mat_<uchar>::const_iterator it = image.begin<uchar>();
	cv::Mat_<uchar>::const_iterator itend = image.end<uchar>();
	for (; it != itend; ++it, ++itr)
	{

		*itr = lookup.at<uchar>(*it);
	}
	return result;
}

int main()
{
	cv::namedWindow("Board Image");
	cv::Mat image;
	std::vector<std::string> filelist;

	for (int i = 1; i <= 9; i++)
	{

		std::stringstream str;
		str << "chessboards/right" << std::setw(2) << std::setfill('0') << i << ".jpg";
		std::cout << str.str() << std::endl;

		filelist.push_back(str.str());
		image = cv::imread(str.str(), 0);
		cv::imshow("Board Image", image);

		cv::waitKey(100);
	}

	CameraCalibrator cameraCalibrator;
	cv::Size boardSize(9, 6);
	cameraCalibrator.addChessboardPoints(
		filelist,
		boardSize, "Detected points");

	cameraCalibrator.calibrate(image.size());

	image = cv::imread(filelist[6], 0);
	cv::Mat uImage = cameraCalibrator.remap(image);

	cv::Mat cameraMatrix = cameraCalibrator.getCameraMatrix();
	std::cout << " Camera intrinsic: " << cameraMatrix.rows << "x" << cameraMatrix.cols << std::endl;
	std::cout << cameraMatrix.at<double>(0, 0) << " " << cameraMatrix.at<double>(0, 1) << " " << cameraMatrix.at<double>(0, 2) << std::endl;
	std::cout << cameraMatrix.at<double>(1, 0) << " " << cameraMatrix.at<double>(1, 1) << " " << cameraMatrix.at<double>(1, 2) << std::endl;
	std::cout << cameraMatrix.at<double>(2, 0) << " " << cameraMatrix.at<double>(2, 1) << " " << cameraMatrix.at<double>(2, 2) << std::endl;

	cv::namedWindow("Original Image");
	cv::imshow("Original Image", image);
	cv::namedWindow("Undistorted Image");
	cv::imshow("Undistorted Image", uImage);

	cv::waitKey();
	return 0;

	int dim(256);
	cv::Mat lut(1,
				&dim,
				CV_8U);

	for (int i = 0; i < 256; i++)
	{

		lut.at<uchar>(i) = 255 - i;
	}

	cv::namedWindow("Negative image");
	cv::imshow("Negative image", applyLookUp(image, lut));

	cv::waitKey();
	return 0;
}