#include <stdio.h>
#include <iostream>
#include "fftw3.h"
#include "vector"

#include <opencv2/legacy/legacy.hpp>
#include <opencv2/nonfree/nonfree.hpp>//opencv_nonfree模組：包含一些擁有專利的演算法，如SIFT、SURF函式原始碼。 
#include "opencv2/core/core.hpp"
#include "opencv2/features2d/features2d.hpp"
#include "opencv2/highgui/highgui.hpp"
#include <opencv2/nonfree/features2d.hpp>

using namespace cv;
using namespace std;

void PhaseCorrelation2D(const Mat &signal,//原影象訊號
	const  Mat &pattern,//帶配準影象訊號
	int &height_offset,//用來獲取估計的高度偏移量
	int &width_offset);//獲取寬的偏移量


//本程式做了一個很簡單的測試：計算“配準樣圖”中那些樣圖的偏移量
//（這些圖都是通過matlab理想化的平移圖）	
int main()
{
	Mat srcImage11 = imread("image_gray.jpg");
	Mat srcImage21 = imread("img_result_-8_-25.jpg");
	Mat srcImage1, srcImage2;
	cvtColor(srcImage11, srcImage1, CV_BGR2GRAY);
	cvtColor(srcImage21, srcImage2, CV_BGR2GRAY);
	
	int height_offset = 0;
	int width_offset = 0;
	PhaseCorrelation2D(srcImage1,srcImage2,height_offset,width_offset);//寬的偏移量
	cout <<"Phase Correlation法 :  高度偏移量"<< -height_offset<<"   寬度偏移量" << -width_offset << endl;
	getchar();
	return 0;
}

//該函式返回影象的剛性平移量
void PhaseCorrelation2D(const Mat &signal,//原訊號
	const  Mat &pattern,//帶配準信號
	int &height_offset,//高的偏移量
	int &width_offset)//寬的偏移量
{
	int nRow = signal.rows;
	int nCol = signal.cols;
	fftw_complex *signal_img = (fftw_complex*)fftw_malloc(sizeof(fftw_complex)*nRow*nCol);
	fftw_complex *pattern_img = (fftw_complex*)fftw_malloc(sizeof(fftw_complex)*nRow*nCol);

	for (int i = 0; i < nRow; i++)
	{
		for (int j = 0; j < nCol; j++)
		{
			signal_img[i*nCol + j][0] = signal.at<uchar>(i, j);
			signal_img[i*nCol + j][1] = 0;
			pattern_img[i*nCol + j][0] = pattern.at<uchar>(i, j);
			pattern_img[i*nCol + j][1] = 0;
		}
	}

	// 對兩幅影象進行傅立葉變換
	fftw_plan signal_forward_plan = fftw_plan_dft_2d(nRow, nCol, signal_img, signal_img,
		FFTW_FORWARD, FFTW_ESTIMATE);
	fftw_plan pattern_forward_plan = fftw_plan_dft_2d(nRow, nCol, pattern_img, pattern_img,
		FFTW_FORWARD, FFTW_ESTIMATE);
	fftw_execute(signal_forward_plan);
	fftw_execute(pattern_forward_plan);

	// 求互功率譜
	fftw_complex *cross_img = (fftw_complex*)fftw_malloc(sizeof(fftw_complex)*nRow*nCol);
	double temp;
	for (int i = 0; i < nRow*nCol; i++)
	{
		cross_img[i][0] = (signal_img[i][0] * pattern_img[i][0]) -
			(signal_img[i][1] * (-1.0*pattern_img[i][1]));
		cross_img[i][1] = (signal_img[i][0] * (-1.0*pattern_img[i][1])) +
			(signal_img[i][1] * pattern_img[i][0]);
		temp = sqrt(cross_img[i][0] * cross_img[i][0] + cross_img[i][1] * cross_img[i][1]) + 0.001;
		cross_img[i][0] /= temp;
		cross_img[i][1] /= temp;
	}

	// backward fft，對互功率譜求反變換
	// FFTW computes an unnormalized transform,
	// in that there is no coefficient in front of
	// the summation in the DFT.
	// In other words, applying the forward and then
	// the backward transform will multiply the input by n.

	// BUT, we only care about the maximum of the inverse DFT,
	// so we don't need to normalize the inverse result.

	// the storage order in FFTW is row-order
	fftw_plan cross_backward_plan = fftw_plan_dft_2d(nRow, nCol, cross_img, cross_img,
		FFTW_BACKWARD, FFTW_ESTIMATE);
	fftw_execute(cross_backward_plan);

	// free memory
	fftw_destroy_plan(signal_forward_plan);
	fftw_destroy_plan(pattern_forward_plan);
	fftw_destroy_plan(cross_backward_plan);
	fftw_free(signal_img);
	fftw_free(pattern_img);

	double *cross_real = new double[nRow*nCol];
	for (int i = 0; i < nRow*nCol; i++)
		cross_real[i] = cross_img[i][0];
	//根據反變換求出平移量
	int max_loc = 0;//準備存放最大值的位置座標（注意，只有一個值）
	double max_vlaue = 0.0;
	for (int i = 0; i < nRow*nCol; i++)
	{
		if (max_vlaue<cross_real[i])
		{
			max_vlaue = cross_real[i];
			max_loc = i;
		}
	}

	height_offset = (int)floor(((int)max_loc) / nCol);
	width_offset = (int)max_loc - nCol*height_offset;

	if (height_offset > 0.5*nRow)
		height_offset = height_offset - nRow;
	if (width_offset > 0.5*nCol)
		width_offset = width_offset - nCol;

	delete[] cross_real;
	cross_real = NULL;
}