#include "sfr.h"
#include <opencv2/opencv.hpp>
#include <complex>
#include <cmath>
#include <iostream>
#include <fstream>
#include <vector>



void de_Gamma(cv::Mat &Src, double gamma)
{
	if (Src.channels() != 1) { return; }

	for (int i = 0; i < Src.rows; ++i)
	{
		uchar *SrcP = Src.ptr(i);
		for (int j = 0; j < Src.cols; ++j)
		{
			SrcP[j] = 255 * (pow((double)SrcP[j] / 255, 1 / gamma));
		}
	}
}

/// <summary>
/// 简单线性回归，从给定的质心相对中心行的偏移量(Cen_Shifts)和垂直偏移量(y_shifts)中计算出一条直线
/// </summary>
/// <param name="Cen_Shifts">质心相对中心行的偏移量</param>
/// <param name="y_shifts">垂直偏移量</param>
/// <param name="a">直线截距</param>
/// <param name="b">直线斜率</param>
void SLR(std::vector<double> &Cen_Shifts, std::vector<double> &y_shifts, double *a, double *b)
{
	//a -> intercept, b->slope of slanted edge

	int y_size = y_shifts.size();
	//using simple linear regression to solve equation and get slope and intercept
	double xsquare = 0, xavg = 0, yavg = 0;
	int i;
	for (i = 0; i < y_size; ++i)
	{
		yavg += y_shifts[i];
		xavg += Cen_Shifts[i];
	}
	xavg /= (double)y_size;
	yavg /= (double)y_size;

	//simple linear regession
	for (i = 0; i < y_size; ++i)
	{
		//对于 Cen_Shifts 中的每个元素，计算其与平均值 xavg 的差值的平方和，累加到 xsquare
		double temp = (Cen_Shifts[i] - xavg);
		*b += temp * (y_shifts[i] - yavg);
		xsquare += temp * temp;
	}

	*b /= xsquare;
	*a = yavg - (*b)*xavg;
}

/// <summary>
/// 计算质心
/// </summary>
/// <param name="Src"></param>
/// <param name="y_shifts">每一行与图像中心行的垂直偏移量</param>
/// <param name="CCoffset">中心行的质心位置</param>
/// <returns>将每一个质心位置，减去中心行的质心位置CC，得到对于中心行质心的偏移量</returns>
std::vector<double> CentroidFind(cv::Mat &Src,std::vector<double> &y_shifts, double *CCoffset)
{
	std::vector<double> Cen_Shifts(Src.rows);
	int i, j, height = Src.rows, width = Src.cols;

	cv::Mat tempSrc(Src.size(), CV_8UC1);

	//Do the bilaterFilter on template ROI image to make sure we can find the slanted edge more acurrate
	//使用双边滤波平滑图像，同时保留边缘信息，以便更准确的找到倾斜边缘
	cv::bilateralFilter(Src, tempSrc, 3, 200, 200);

	//通过<累加每一行像素灰度值的差分和对应位置的乘积（分子）> 以及 <灰度值的差分（分母）>，计算每一行的质心位置。
	// calculate the centroid of every row in ROI
	// centroid formuld: Total moments/Total amount
	//对tempSrc每一行累加灰度值差异和对应位置来计算该行的质心位置
	for (i = 0; i < height; ++i)
	{
		double molecule = 0, denominator = 0, temp = 0;
		uchar *tempSrcPtr = tempSrc.ptr<uchar>(i);//行指针
		for (j = 0; j < width - 1; ++j)
		{
			temp = (double)tempSrcPtr[j + 1] - (double)tempSrcPtr[j];
			molecule += temp * j;	//分子，灰度值差异和位置的乘积之和
			denominator += temp;	//分母，灰度值差异之和
		}
		Cen_Shifts[i] = molecule / denominator;//质心位置，分子/分母
	}

	//Eliminate the noise far from slant edge(+/- 10 pixels)
	//去除噪声
	//通过高斯模糊和将质心周围10个像素的灰度值替换回原始图像的值，来去除远离倾斜边缘的噪声。
	tempSrc = Src.clone();	//先将原图备份
	cv::GaussianBlur(Src, Src, cv::Size(3, 3), 0);	//将原图做高斯模糊处理
	for (i = 0; i < height; ++i)
	{
		uchar *SrcPtr = Src.ptr<uchar>(i);		//高斯模糊后的图
		uchar *tempPtr = tempSrc.ptr<uchar>(i);	//原图
		//在质心位置周围的正负10个像素范围内，将tempSrc中的像素值复制回Src。
		//这样，将质心左右10个像素内的灰度值换回原图的灰度值，其他地方都是模糊处理过的，边界更加清晰
		for (j = int(Cen_Shifts[i]) - 10; j < int(Cen_Shifts[i]) + 10; ++j)
		{
			SrcPtr[j] = tempPtr[j];
		}
	}

	// check whether the edge in image is too close to the image corners
	//检查边界，检查图像边缘是否太接近图像角落
	if (Cen_Shifts[0] < 2.0 || width - Cen_Shifts[0] < 2.0)
	{
		std::cerr << "The edge in ROI is too close to the image corners" << std::endl;
		return {};
	}
	if (Cen_Shifts[height - 1] < 2 || width - Cen_Shifts[height - 1] < 2)
	{
		std::cerr << "The edge in ROI is too close to the image corners" << std::endl;
		return {};
	}
	//图像中心行质心位置 CCoffset = Cen_Shifts[h/2]
	int half_ySize = height / 2;	//中间位置质心index
	int CC = Cen_Shifts[half_ySize]; //Centroid of image center图像质心坐标
	*CCoffset = CC;
	//计算偏移量
	for (i = 0; i < height; ++i)
	{
		//对于Cen_Shifts中的每一个质心位置，减去中心行的质心位置CC，得到相对于中心行的偏移量
		Cen_Shifts[i] -= CC; //Calculate the Shifts between Centroids and Centroid of image center		
		//同时，计算每一行与图像中心行的垂直偏移量，并存储在y_shifts中
		y_shifts[i] = i - half_ySize; //Calculate the shifts between height of image center and each row
	}

	return Cen_Shifts;
}

/// <summary>
/// 超采样，目的是将原始图像中的像素值映射到一个长度为原始图像宽度四倍的新数据集中
/// 此过程通常用于增加数据的密度或分辨率，以便更好地分析图像中的细节或变化。
/// </summary>
/// <param name="Src"></param>
/// <param name="slope"></param>
/// <param name="CCoffset"></param>
/// <param name="height"></param>
/// <param name="width"></param>
/// <param name="SamplingLen"></param>
/// <returns></returns>
std::vector<double> OverSampling(cv::Mat &Src, double slope, double CCoffset, int height, int width, int *SamplingLen)
{
	std::vector<double> RowShifts(height, 0);

	int i, j, k;
	int halfY = height >> 1;

	//calculate the pixel shift of each row to align the centroid of each row as close as possible
	//计算每行的像素位移，使每行的质心尽可能接近
	for (i = 0; i < height; ++i) { RowShifts[i] = (double)(i - halfY) / slope + CCoffset;  }

	//DataMap -> to record the index of pixel after shift
	//Datas -> to record the original pixel value 
	std::vector<double> DataMap(height*width, 0);
	std::vector<double> Datas(height*width, 0);

	for (i = 0, k = 0; i < height; ++i)
	{
		int baseindex = width * i;
		for (j = 0; j < width; ++j)
		{
			DataMap[baseindex+j] = j - RowShifts[i];
			Datas[baseindex + j] = int(Src.at<uchar>(i, j));
		}
	}

	std::vector<double> SamplingBar(*SamplingLen, 0);
	std::vector<int> MappingCount(*SamplingLen, 0);

	//Start to mapping the original data to 4x sampling data and record the count of each pixel in 4x sampling data
	for (i = 0; i < height*width; ++i)
	{
		int MappingIndex = static_cast<int>(4 * DataMap[i]);
		if (MappingIndex >= 0 && MappingIndex < *SamplingLen)
		{
			SamplingBar[MappingIndex] = SamplingBar[MappingIndex] + Datas[i];
			MappingCount[MappingIndex]++;
		}
	}

	//average the pixel value in 4x sampling data, if the pixel value in pixel is zero, copy the value of close pixel
	for (i = 0; i < *SamplingLen; ++i) {
		j = 0;
		k = 1;
		if (MappingCount[i] == 0) {

			if (i == 0) {
				while (!j)
				{
					if (MappingCount[i + k] != 0)
					{
						SamplingBar[i] = SamplingBar[i + k] / ((double)MappingCount[i + k]);
						j = 1;
					}
					else ++k;
				}
			}
			else {
				while (!j && ((i - k) >= 0))
				{
					if (MappingCount[i - k] != 0)
					{
						SamplingBar[i] = SamplingBar[i - k];   /* Don't divide by counts since it already happened in previous iteration */
						j = 1;
					}
					else ++k;
				}
				if ((i - k) < 0)
				{
					k = 1;
					while (!j)
					{
						if (MappingCount[i + k] != 0)
						{
							SamplingBar[i] = SamplingBar[i + k] / ((double)MappingCount[i + k]);
							j = 1;
						}
						else ++k;
					}
				}
			}
		}
		else
			SamplingBar[i] = (SamplingBar[i]) / ((double)MappingCount[i]);
	}

	// reduce the length of sampling data 
	// because the datas at the edge are only matters, we truncate the data close to the two side of sampling data
	// which has very small contribution to the result

	//truncating the data smaller than 10% and bigger than 90% of original length
	int originalSamplingLen = *SamplingLen;
	*SamplingLen = *SamplingLen * 0.8;

	std::vector<double> deSampling(*SamplingLen,0);
	//derivative sampling data(which is ESF) to get the line spread function
	for (i = originalSamplingLen * 0.1, j = 1; i < originalSamplingLen, j < *SamplingLen; ++i,++j)
	{
		deSampling[j] = SamplingBar[i + 1] - SamplingBar[i];
	}
	
	return deSampling;
}


std::vector<double> HammingWindows(std::vector<double> &deSampling, int SamplingLen)
{
	int i, j;
	std::vector<double> tempData(SamplingLen);

	//We want to shift the peak data to the center of line spread function data
	//Because we will do the hamming window later, this will keep the important data away from filtering
	//In case there are two peaks, we use two variable to record the peak data position
	int L_location = -1, R_location = -1;
	//寻找最大值
	double SamplingMax = 0;
	for (i = 0; i < SamplingLen; ++i)
	{
		if (fabs(deSampling[i]) > fabs(SamplingMax))
			SamplingMax = deSampling[i];
	}
	// 记录最大值的索引
	for (i = 0; i < SamplingLen; ++i)
	{
		if (deSampling[i] == SamplingMax)
		{
			if (L_location < 0) { L_location = i; }
			R_location = i;
		}
	}

	//the shift amount 
	//计算偏移量
	int PeakOffset = (R_location + L_location) / 2 - SamplingLen / 2;
	//应用偏移量
	if (PeakOffset)
	{
		for (i = 0; i < SamplingLen; ++i)
		{
			int newIndex = i - PeakOffset;
			if (newIndex >= 0 && newIndex < SamplingLen) 
			{ 
				tempData[newIndex] = deSampling[i]; 
			}
		}
	}
	else
	{
		for (i = 0; i < SamplingLen; ++i) { tempData[i] = deSampling[i]; }
	}

	//do the hamming window filtering
	for (int i = 0; i < SamplingLen; ++i)
	{
		//w(n) = 0.54 - 0.46 * cos(2 * π * n / (N - 1)), 0 <= n <= N - 1
		tempData[i] = tempData[i] * (0.54 - 0.46*cos(2 * M_PI*i / (SamplingLen - 1)));
	}

	return tempData;
}

void DFT(std::vector<double> &data, int size)
{
	int i, j;
	//动态分配一个复数数组arr，用于存储数据的复数表示（尽管输入是double类型，但DFT通常在复数域中进行计算）。
	std::complex<double> *arr = new std::complex<double>[size];
	//将data中的每个double值转换为复数形式（虚部为0），存储在arr中
	for (i = 0; i < size; ++i)
	{
		arr[i] = std::complex<double>(data[i], 0);
	}
	//DFT计算
	//只计算到size/2，因为对于实数输入，频谱是对称的
	for (i = 0; i < size / 2.0; ++i)
	{
		std::complex<double> temp = 0;
		for (j = 0; j < size; ++j)
		{
			double w = 2 * 3.1415*i*j / size;
			//计算对应的旋转因子deg（由cos(w)和-sin(w)构成）
			std::complex<double> deg(cos(w), -sin(w));
			//将其与当前样本相乘，然后累加到temp中
			temp += arr[j] * deg;
		}
		//计算temp的模值（即频谱的幅度）
		data[i] = sqrt(temp.real()*temp.real() + temp.imag()*temp.imag());
	}
}

/// <summary>
/// 确保图像的高度可以被斜率整除，从而在图像中形成整数个完整的周期
/// </summary>
/// <param name="slope"></param>
/// <param name="ImgHeight"></param>
void ReduceRows(double slope, int *ImgHeight)
{
	double tempSlope = fabs(slope);
	int cycs = (*ImgHeight) / tempSlope;
	if (tempSlope*cycs < *ImgHeight) { *ImgHeight = tempSlope * cycs; }
}

// 假设OverSamplingData已经被DFT处理并归一化  
double GetMTFAtFrequencyHalf(const std::vector<double>& OverSamplingData, int SamplingLen) 
{
	int width = SamplingLen / 4; // 假设DFT之后数据长度是原始的四分之一  
	int indexAtHalfFrequency = width / 2; // 接近0.5频率的索引  

	// 如果SamplingLen是某个偶数的4倍，indexAtHalfFrequency将直接对应到0.5频率点  
	// 否则，您可能需要插值来得到精确的值  

	// 直接访问索引，如果它对应到0.5频率点  
	double mtfAtHalfLosse = OverSamplingData[indexAtHalfFrequency];

	// 如果需要插值（这里只是一个简单的线性插值示例）  
	// 假设我们有indexAtHalfFrequency和indexAtHalfFrequency+1的值，并且它们分别对应稍微低于和稍微高于0.5的频率  
	double mtfBefore = OverSamplingData[indexAtHalfFrequency];  
	double mtfAfter = OverSamplingData[std::min(indexAtHalfFrequency + 1, SamplingLen - 1)]; // 确保不越界  
	double interpolationFactor = (0.5 - (indexAtHalfFrequency / (double)width)) * 2; // 计算插值因子  
	double mtfAtHalf = mtfBefore + interpolationFactor * (mtfAfter - mtfBefore); // 插值结果  

	// 返回MTF值  
	return mtfAtHalf;
}

double GetMTFAtFrequency(const std::vector<double>& OverSamplingData, int SamplingLen, double frequency)
{
	if (frequency < 0.0 || frequency > 1.0) {
		throw std::invalid_argument("Frequency must be between 0.0 and 1.0");
	}
	//因为是4倍超采样，所以DFT之后数据长度是原始的四分之一 
	int width = std::round(SamplingLen / 4);
	int indexAtFrequency = static_cast<int>(frequency * width);
	indexAtFrequency = std::min(indexAtFrequency, width - 1);//确保不越界
	// 直接访问索引，如果它对应到0.5频率点  
	double mtfLoose = OverSamplingData[indexAtFrequency];

	// 如果需要插值（这里只是一个简单的线性插值示例）  
	// 假设我们有indexAtHalfFrequency和indexAtHalfFrequency+1的值，并且它们分别对应稍微低于和稍微高于0.5的频率 
	if (indexAtFrequency < width - 1)//确保不是最后一个点
	{
		double mtfBefore = OverSamplingData[indexAtFrequency];
		double mtfAfter = OverSamplingData[indexAtFrequency+1];
		//假设在小范围内是线性的
		double y1 = mtfBefore;
		double y2 = mtfAfter;
		double x1 = (double)indexAtFrequency / width;
		double x2 = (double)(indexAtFrequency + 1) / width;
		double k = (y2 - y1) / (x2 - x1);
		double b = y1 - k * x1;
		double mtfS = k * frequency + b;	//y = k*x+b
		double myMtf = (y2 * (frequency - x1) - y1 * (frequency - x2)) / (x2 - x1);
		// 计算插值因子（假设频率在索引之间是均匀分布的）  
		double interpolationFactor = frequency * width - indexAtFrequency;
		double mtf = mtfBefore + interpolationFactor * (mtfAfter - mtfBefore); // 插值结果  
		return mtf;
	}

}

int SFRCalculation(cv::Mat &ROI, double gamma)
{
	if (ROI.empty())
	{
		std::cerr << "Open the ROI image error" << std::endl;
		return 0;
	}

	int height = ROI.rows, width = ROI.cols;
	//Do the gamma decoding to eliminate the gamma encoded by camera device 
	de_Gamma(ROI, gamma);
	int i, j;

	double slope = 0, intercept = 0;
	
	//Center centroid offset
	double CCoffset = 0;
	std::vector<double> y_shifts(height);
	//1.计算质心偏移
	////Calculate the shifts between Centroids and Centroid of image center	
	//y_shifts : 每一行与图像中心行的垂直偏移量
	//CCoffset ： 中心行的质心位置
	//Cen_Shifts : 将每一个质心位置，减去中心行的质心位置CC，得到对于中心行质心的偏移量
	std::vector<double> Cen_Shifts = CentroidFind(ROI, y_shifts, &CCoffset);
	if (Cen_Shifts.empty()) { return 0; }
	//2.线性回归
	//simple linear regression for slanted edge fitting
	SLR(Cen_Shifts, y_shifts, &intercept, &slope);
	//3.数据缩减，根据斜率调整数据行数（height），以确保数据中有整数个完整的相位旋转周期
	//Truncate the number of rows of data to largest slope cycle which will have an integer number of full phase rotations
	ReduceRows(slope, &height);

	//update the CCoffset to the offset between original mid point of image and reference mid point we calculated
	//将CCoffset(中心行的质心位置)更新为我们计算的原始图像中点与参考中点之间的偏移量
	//4.更新CCoffset，将CCoffset更新为原始图像中心与计算得到的参考中点之间的偏移量
	CCoffset = CCoffset + 0.5 + intercept - width / 2;
	//5.4倍超采样
	//Mapping the pixel value of original image into a sampling data which the length is 4 times of original image width
	//This step is for concentrating the amount of change of original pixel values
	int SamplingLen = width * 4;
	std::vector<double> OverSamplingData = OverSampling(ROI, slope, CCoffset, height, width, &SamplingLen);
	//6.应用汉明窗
	//Using hamming window to filter the ripple signal of two side of data
	//在时域上对信号进行加权的函数，通常用于减少频谱泄露,提高频谱分析的准确性
	OverSamplingData = HammingWindows(OverSamplingData, SamplingLen);
	//7.离散傅里叶变换
	//decrete four transform
	DFT(OverSamplingData, SamplingLen);
	width = int(SamplingLen / 4);
	double maxData = 0;
	for (i = 0; i < SamplingLen; ++i)
	{
		if (OverSamplingData[i] != 0)
		{
			//找到了 OverSamplingData 数组中的第一个非零值
			maxData = OverSamplingData[i];
			break;
		}
	}

	for (int i = 0; i < SamplingLen; ++i) 
	{ 
		OverSamplingData[i] /= maxData; 
	}

	//保存MTF
	std::fstream mtf_file("./ref/mtf.csv", std::ios::out);
	for (i = 0; i <= width; ++i)
	{
		double frequency = (double)i / width;
		mtf_file << frequency << "," << OverSamplingData[i] << "\n";

		cv::String str = cv::format("%.3f = %f", frequency, OverSamplingData[i]);
		std::cout << str << std::endl;
	}
	//MTF50
	double mtfAtHalf = GetMTFAtFrequencyHalf(OverSamplingData, SamplingLen);
	std::cout << "MTF at frequency 0.50: " << mtfAtHalf << std::endl;

	//MTF af frequency
	double frequency = 0.7; // 您想要查询的频率  
	double mtfAtFrequency = GetMTFAtFrequency(OverSamplingData, SamplingLen, frequency);
	std::cout << "MTF at frequency " << frequency << ": " << mtfAtFrequency << std::endl;


	mtf_file.close();
	
	return 1;
}