#include <opencv2/opencv.hpp>
#include <opencv2/opencv_lib.hpp>

#define MY_LOGPOLAR_M_RATIO (46.2 / 256.0)

using namespace cv;
using namespace std;

void myGrayscaleImage(InputArray _imgSrc, OutputArray _imgDst) {
	Mat imgSrc = _imgSrc.getMat();
	_imgDst.create(imgSrc.rows, imgSrc.cols, CV_64FC1);
	Mat imgDst = _imgDst.getMat();

	for(int sy = 0; sy < imgSrc.rows; sy++) {
		Vec3b* srcData = imgSrc.ptr<Vec3b>(sy);
		double* dstData = imgDst.ptr<double>(sy);
		for(int sx = 0; sx < imgSrc.cols; sx++) {
			double b = srcData[sx][0];
			double g = srcData[sx][1];
			double r = srcData[sx][2];
			dstData[sx] = (0.299 * r + 0.587 * g + 0.114 * b);
		}
	}

	return;
}

void myDftImage(InputArray _imgSrc, OutputArray _imgDst) {
	Mat imgSrc = _imgSrc.getMat();
	_imgDst.create(imgSrc.rows, imgSrc.cols, imgSrc.type());
	Mat imgDst = _imgDst.getMat();

	Mat matReIm[] = {Mat_<double>(imgSrc), Mat::zeros(imgSrc.size(), CV_64F)};
	Mat matComp;
	merge(matReIm, 2, matComp);

	dft(matComp, matComp);

	split(matComp, matReIm);

	Mat matMag;
	magnitude(matReIm[0], matReIm[1], matMag);

	matMag += Scalar::all(1.0);
	log(matMag, matMag);

	int cx = (matMag.cols / 2), cy = (matMag.rows / 2);
	Mat matQad0(matMag, Rect(0, 0, cx, cy));
	Mat matQad1(matMag, Rect(cx, 0, cx, cy));
	Mat matQad2(matMag, Rect(0, cy, cx, cy));
	Mat matQad3(matMag, Rect(cx, cy, cx, cy));

	Mat matTmp;
	matQad0.copyTo(matTmp); matQad3.copyTo(matQad0); matTmp.copyTo(matQad3);
	matQad1.copyTo(matTmp); matQad2.copyTo(matQad1); matTmp.copyTo(matQad2);

	normalize(matMag, imgDst, 0.0, 255.0, CV_MINMAX);

	return;
}

void myLogPolar(InputArray _imgSrc, OutputArray _imgDst, Point2d center, double M, int flags) {
	Mat imgSrc = _imgSrc.getMat();
	_imgDst.create(imgSrc.rows, imgSrc.cols, imgSrc.type());
	Mat imgDst = _imgDst.getMat();

	IplImage imgSrcTmp = imgSrc;
	IplImage imgDstTmp = imgDst;

	cvLogPolar(&imgSrcTmp, &imgDstTmp, cvPoint2D32f(center.x, center.y), M, flags);

	imgDst = cvarrToMat(&imgDstTmp);

	return;
}

Point2d myRotationPhaseCorrelateImage(InputArray _imgSrcL, InputArray _imgSrcR) {
	Mat imgSrcL = _imgSrcL.getMat();
	Mat imgSrcR = _imgSrcR.getMat();

	Mat matHann;
	createHanningWindow(matHann, imgSrcL.size(), CV_64F);

	// Pass1
	Mat imgSrcHannL = imgSrcL.mul(matHann);
	Mat imgSrcHannR = imgSrcR.mul(matHann);

	Mat imgDftL, imgDftR;
	myDftImage(imgSrcHannL, imgDftL);
	myDftImage(imgSrcHannR, imgDftR);

	Mat imgDftPolL, imgDftPolR;
	Point2d center(imgSrcL.cols / 2.0, imgSrcL.rows / 2.0);
	myLogPolar(imgDftL, imgDftPolL, center, (MY_LOGPOLAR_M_RATIO * imgSrcL.rows), CV_INTER_LINEAR);
	myLogPolar(imgDftR, imgDftPolR, center, (MY_LOGPOLAR_M_RATIO * imgSrcL.rows), CV_INTER_LINEAR);

	double response;
	Point2d shift;
	shift = phaseCorrelateRes(imgDftPolL, imgDftPolR, matHann, &response);

	double angle = -(shift.y / imgSrcL.rows) * 360.0;
	double scale = 1.0 - shift.x / (MY_LOGPOLAR_M_RATIO * imgSrcL.rows);

	printf("[Pass1]\nAngle: %g Scale: %g\nResponse: %g\n", angle, scale, response);

	// Pass2
	Mat matAffi = getRotationMatrix2D(center, -angle, (1.0 / scale));
	Mat imgSrcCorrectR;
	warpAffine(imgSrcR, imgSrcCorrectR, matAffi, imgSrcCorrectR.size(), INTER_LINEAR);

	shift = phaseCorrelateRes(imgSrcL, imgSrcCorrectR, matHann, &response);

	printf("[Pass2]\nShift X: %g Shift Y: %g\nResponse: %g\n", shift.x, shift.y, response);

	Mat imgSrcCotR;
	warpAffine(imgSrcR, imgSrcCotR, matAffi, imgSrcCotR.size(), INTER_LINEAR);

	double aff[] = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0};
	Mat matAff(2, 3, CV_64FC1, aff);
	matAff.at<double>(0, 2) = -shift.x;
	matAff.at<double>(1, 2) = -shift.y;

	Mat imgSrcCotR0;
	warpAffine(imgSrcCotR, imgSrcCotR0, matAff, imgSrcCotR0.size(), INTER_LINEAR);

imwrite("test03L-sq-out.jpg", imgSrcCotR0);

	return(shift);
}

int main(int argc, char* argv[]) {
	Mat imgSrcL = imread("test03L-sq-r00.jpg", CV_LOAD_IMAGE_ANYCOLOR);
	Mat imgSrcR = imread("test03L-sq-x39y19r19.jpg", CV_LOAD_IMAGE_ANYCOLOR);

	if(imgSrcL.empty() || imgSrcR.empty()) {
		fprintf(stderr, "Error: Can not open ImageFile(s)\n");
		exit(-1);
	}

	Mat imgGrayL, imgGrayR;
	myGrayscaleImage(imgSrcL, imgGrayL);
	myGrayscaleImage(imgSrcR, imgGrayR);

	myRotationPhaseCorrelateImage(imgGrayL, imgGrayR);

	imshow("imgDstL", imgSrcL);
	imshow("imgDstR", imgSrcR);

	waitKey(0);

	return(0);
}