#pragma once

#include "opencv2\opencv.hpp"
#include <iostream>


namespace harris {

	using namespace std;
	using namespace cv;


	inline double det(const Mat1d& A)
	{
		return A(0, 0)*A(1, 1) - A(1, 0)*A(0, 1);
	}

	inline double trace(const Mat1d& A)
	{
		return A(0, 0) + A(1, 1);
	}

	inline double sign(double x) {
		return x > 0 ? 1.0 :
			x < 0 ? -1.0 : 0.0;
	}




	static void cornerEigenValsVecs(const Mat& src, Mat1f& eigenv,
		int block_size = 2, int aperture_size = 3,
		double kappa = 0., int borderType = cv::BORDER_DEFAULT) {

		int depth = src.depth();

		double scale = 1.0 / 255;

		CV_Assert(src.type() == CV_8UC1 || src.type() == CV_32FC1);

		Mat Dx, Dy;

		Sobel(src, Dx, CV_32F, 1, 0, aperture_size, scale, 0, borderType);
		Sobel(src, Dy, CV_32F, 0, 1, aperture_size, scale, 0, borderType);

		double dbgMax, dbgMin;
		cv::minMaxLoc(Dx, &dbgMin, &dbgMax);
		cout << "\tMaxDx : " << dbgMax << ", MinDx : " << dbgMin << endl;
		cv::minMaxLoc(Dy, &dbgMin, &dbgMax);
		cout << "\tMaxDy : " << dbgMax << ", MinDy : " << dbgMin << endl;

		Size size = src.size();
		Mat3f cov(size);
		int i, j;

		for (i = 0; i < size.height; i++)
		{
			float* cov_data = (float*)(cov.data + i*cov.step);
			const float* dxdata = (const float*)(Dx.data + i*Dx.step);
			const float* dydata = (const float*)(Dy.data + i*Dy.step);

			for (j = 0; j < size.width; j++)
			{
				float dx = dxdata[j];
				float dy = dydata[j];
#if 0
				dx = fabs(dx) > 1.0 ? sign(dx) : dx;
				dy = fabs(dy) > 1.0 ? sign(dy) : dy;
#endif

				cov_data[j * 3] = dx*dx;
				cov_data[j * 3 + 1] = dx*dy;
				cov_data[j * 3 + 2] = dy*dy;
			}
		}

		boxFilter(cov, cov, cov.depth(), Size(block_size, block_size),
			Point(-1, -1), false, borderType);

		eigenv = Mat1f::zeros(src.size());

		for (int j = 0; j < src.rows; ++j) {

			for (int i = 0; i < src.cols; ++i) {
				Mat1d A = Mat1d::zeros(2, 2);
				A(0, 0) = cov(j, i)[0]; // Ix**2
				A(1, 0) = A(0, 1) = cov(j, i)[1];
				A(1, 1) = cov(j, i)[2];
				eigenv(j, i) = det(A) - kappa * trace(A) * trace(A);
			}
		}

		cv::minMaxLoc(eigenv, &dbgMin, &dbgMax);
		cout << "\tMaxEigen : " << dbgMax << ", MinEigen : " << dbgMin << endl;
	}

	inline Mat3b get_vis(const Mat1d& src, double val_max, double val_min)
	{
		Mat3b rgb(src.rows, src.cols);
		for (int j = 0; j < src.rows; ++j) {
			for (int i = 0; i < src.cols; ++i)
			{
				rgb(j, i) = colorTrans::floatToColor<uchar>(src(j, i), val_min, val_max);
			}
		}
		return rgb;
	}

	vector<Mat1f> getHarrisMap(const Mat1b& src, int blockSize, int apertureSize, double k, int maxLevel) {
		vector<Mat1b> srcs;
		cv::buildPyramid(src, srcs, maxLevel);

		vector<Mat1f> dsts(srcs.size());

		for (int n = 0; n < srcs.end() - srcs.begin(); ++n) {
			cout << n << endl;

			// sobel掛けて固有値だす
			cornerEigenValsVecs(srcs[n], dsts[n], blockSize, apertureSize, k);
//#ifdef _DEBUG
#if 1
			double dbgMax, dbgMin;
			cv::minMaxLoc(dsts[n], &dbgMin, &dbgMax);
			//cout << "\tMax : " << dbgMax << ", Min : " << dbgMin << endl;



#if 0
			Mat3b rgb;
			Mat1f tmp = dsts[n].clone();
			tmp = tmp - (float)dbgMin;
			//tmp = tmp + 128;
			tmp = tmp * 255 / (dbgMax - dbgMin);
			tmp = tmp + (float)dbgMin * 255 / (dbgMax - dbgMin) + 128;
			cv::applyColorMap(Mat1b(tmp), rgb, cv::COLORMAP_JET);
#else
			Mat1d tmp = dsts[n];
			const double harris_max_thr = 0.05;
			const double harris_min_thr = -0.05;
			Mat3b rgb = get_vis(tmp, harris_max_thr, harris_min_thr); // 値をカラー表示
#endif
			imwrite(format("harrisMap_%03d.png", n), rgb);
			//imwrite(format("harrisMap_%03d.png", n), Mat1b(tmp));
			imwrite(format("img_layer_%03d.jpg", n), srcs[n]);
#endif

		}

		return dsts;
	}

	Mat1b genMask(const vector<Mat1f>& eigenValMaps, const vector<double>& thresholds) {
		if (eigenValMaps.empty()) {
			Mat1b();
		}

		Mat1b mask = Mat1b::zeros(eigenValMaps[0].size());
		//vector<Mat1b> masks(eigenValMaps.size());
		for (int n = 0; n < eigenValMaps.size(); ++n) {
			Mat1f tmpMask = Mat1f::zeros(eigenValMaps[n].size());
			cv::threshold(eigenValMaps[n], tmpMask, thresholds[n], 255, cv::THRESH_BINARY);
			Mat1b resizedMask;
			cv::resize(Mat1b(tmpMask), resizedMask, mask.size());
//#ifdef _DEBUG
#if 1
			imwrite(format("thresholded_%03d.png", n), resizedMask);
#endif
			for (int j = 0; j < mask.rows; ++j) {
				for (int i = 0; i < mask.cols; ++i) {
					if (resizedMask(j, i) > 0) {
						mask(j, i) = 255;
					}
				}
			}

		}

		return mask;
	}

	void main(int argc, char* argv[]) {
		Mat1b src_gray = imread(argv[1], 0);

		/// Detector parameters
		int blockSize = 2;
		int apertureSize = 3;
		double k = 0.04;
		int maxLevel = 4;

		vector<Mat1f> harrisMap = getHarrisMap(src_gray, blockSize, apertureSize, k, maxLevel);

		vector<double> thresholds(harrisMap.size());
		for (int n = 0; n < thresholds.size(); ++n) {
			thresholds[n] = 0.05;
		}
		Mat1b mask = genMask(harrisMap, thresholds);
		if (!mask.empty()) {
			imwrite("mask_merged.png", mask);
		}
	}
}