#include "FEKONearBPBasic.h"
#include "BaseToolLib/interpolation.h"
#include "BaseToollib/FileOperator.h"
#include "BaseToollib/BaseTool.h"
#include "BaseToollib/ImageOperatorBase.h"
#include "SARBaseToolLib/SARImageBase.h"
#include <unsupported/Eigen/FFT>
#include <stdlib.h>
#include <stdio.h>
#include <iostream>
#include <string>
#include <fstream>

#include "SARBaseToolLib/SARBaseTool.h"
#include <gsl/gsl_integration.h>
#include <gsl/gsl_spline.h>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <fftw3.h>
#include <omp.h>
#include <iostream>
#include <fstream>
#include <vector>




// 定义插值函数，以处理复数值
std::complex<double> InterpolateComplex(Eigen::MatrixXd& xi, Eigen::MatrixXd& yi, Eigen::MatrixXcd& data, double x, double y) {
	int m = xi.rows();
	int n = xi.cols();

	if (x < xi(0, 0) || x > xi(m - 1, n - 1) || y < yi(0, 0) || y > yi(m - 1, n - 1)) {
		std::complex<double> defaultMatrix = std::complex<double>(0,0);  // 超出范围时返回默认值
		return defaultMatrix;
	}

	int i, j;
	for (i = 0; i < m - 1; i++) {
		if (x >= xi(i, 0) && x <= xi(i + 1, 0)) {
			break;
		}
	}

	for (j = 0; j < n - 1; j++) {
		if (y >= yi(0, j) && y <= yi(0, j + 1)) {
			break;
		}
	}

	double x1 = xi(i, 0);
	double x2 = xi(i + 1, 0);
	double y1 = yi(0, j);
	double y2 = yi(0, j + 1);

	std::complex<double> Q11 = data(i, j);
	std::complex<double> Q12 = data(i, j + 1);
	std::complex<double> Q21 = data(i + 1, j);
	std::complex<double> Q22 = data(i + 1, j + 1);

	double dx1 = x2 - x;
	double dx2 = x - x1;
	double dy1 = y2 - y;
	double dy2 = y - y1;
	double denom = (x2 - x1) * (y2 - y1);

	std::complex<double> interpolatedValue = (Q11 * dx1 * dy1 + Q21 * dx2 * dy1 + Q12 * dx1 * dy2 + Q22 * dx2 * dy2) / denom;
	return interpolatedValue;
}


Eigen::MatrixXd Cartesian2Spherical(Eigen::MatrixXd CartesianPoint)
{
	Eigen::MatrixXd result = CartesianPoint;
	size_t rows = result.rows();
	size_t cols = result.cols();
	for (int i = 0; i < rows; i++) {
		double x = CartesianPoint(i, 0);
		double y = CartesianPoint(i, 1);
		double z = CartesianPoint(i, 2);
		result(i, 0) = std::sqrt(CartesianPoint(i, 0) * CartesianPoint(i, 0) +
			CartesianPoint(i, 1) * CartesianPoint(i, 1) +
			CartesianPoint(i, 2) * CartesianPoint(i, 2));
		result(i, 1) = std::acos(CartesianPoint(i, 2) / result(i, 0));// 行 theta 
		if (CartesianPoint(i, 0) == 0 && CartesianPoint(i, 1) == 0) {
			result(i, 2) = 0;
		}
		else {
			result(i, 2) = std::asin(y / (std::sin(result(i, 1)) * result(i, 0)));//一 三 -pi/2 pi/2
			if (CartesianPoint(i, 0) < 0 && CartesianPoint(i, 1) > 0) { // 二 
				result(i, 2) = PI - result(i, 2);// phi
			}
			else if (CartesianPoint(i, 0) < 0 && CartesianPoint(i, 1) < 0) {
				result(i, 2) = -1 * PI - result(i, 2);// phi
			}
			else {}
		}
	}

	return result;
}

int BP2DProcess(QString in_path, QString out_path, double Rref, double minX, double maxX, double minY, double maxY, double PlaneZ, int ImageHeight, int ImageWidth)
{
	BP2DProcessClass process;
	process.initProcess(in_path, out_path, Rref, minX, maxX, minY, maxY, PlaneZ, ImageHeight, ImageWidth);
	process.start();
	return -1;
}


int FBP2DProcess(QString in_path, QString out_path, double Rref, double minX, double maxX, double minY, double maxY, double PlaneZ, int ImageHeight, int ImageWidth)
{
	FBP2DProcessClass process;
	process.initProcess(in_path, out_path, Rref, minX, maxX, minY, maxY, PlaneZ, ImageHeight, ImageWidth);
	process.start();
	return -1;
}

int build2Bin(QString path, int width, int height, std::vector<double>& freqs, Eigen::MatrixXd AntPostion, Eigen::MatrixXcd echo)
{
	/*
* % 构建回波结果
fid=fopen("H_echo.bin","w+");
fwrite(fid,801,'int32');
fwrite(fid,801,'int32');
for i=1:length(freqs)
	fwrite(fid,freqs(i),'double');
end

for i=1:length(AntPostion)
	fwrite(fid,AntPostion(i,1),'double');
	fwrite(fid,AntPostion(i,2),'double');
	fwrite(fid,AntPostion(i,3),'double');
	fwrite(fid,real(echo_H_datas(i,:)),'double');
	fwrite(fid,imag(echo_H_datas(i,:)),'double');
end
fclose(fid);
**/
// 构建二进制文件

	std::ofstream file(path.toUtf8().constData(), std::ios::binary);
	double freq;

	file.write(reinterpret_cast<const char*>(&height), sizeof(height));
	file.write(reinterpret_cast<const char*>(&width), sizeof(width));

	for (size_t i = 0; i < freqs.size(); i++) {
		freq = freqs[i];
		file.write(reinterpret_cast<const char*>(&freq), sizeof(freq));
	}

	double x, y, z;
	double real_val, imag_val;

	for (size_t i = 0; i < AntPostion.rows(); i++) {
		x = AntPostion(i, 0);
		y = AntPostion(i, 1);
		z = AntPostion(i, 2);
		file.write(reinterpret_cast<const char*>(&x), sizeof(x));
		file.write(reinterpret_cast<const char*>(&y), sizeof(y));
		file.write(reinterpret_cast<const char*>(&z), sizeof(z));
		for (size_t j = 0; j < echo.cols(); j++) {
			real_val = std::real(echo(i, j));
			imag_val = std::imag(echo(i, j));
			file.write(reinterpret_cast<const char*>(&real_val), sizeof(real_val));
			file.write(reinterpret_cast<const char*>(&imag_val), sizeof(imag_val));
		}
	}

	file.close();
	return 0;
}

template<typename T>
Eigen::MatrixXcd BP2DImageByPluse(Eigen::MatrixXcd timeEcho, Eigen::MatrixXd AntPosition, double minX, double maxX, double minY, double maxY, double PlaneZ, double Rref, size_t ImageWidth, size_t ImageHeight, double startfreq, size_t timefreqnum, double timeFreqBandWidth, T* logclss)
{
	bool logfun = !(nullptr == logclss); // 空指针
	double delta_x = (maxX - minX) / (ImageWidth - 1);
	double delta_y = (maxY - minY) / (ImageHeight - 1);

	double* Rs = new double[timefreqnum]; // 时域 距离插值
	for (int i = 0; i < timefreqnum; i++) {
		Rs[i] = (LIGHTSPEED / timeFreqBandWidth) * i; // 参考 《雷达成像算法》-- 匹配滤波一章
	}
	double wave_len = LIGHTSPEED / startfreq;
	if (logfun) {
		logclss->logFUN(0, "process.....");
	}

	size_t process = 0;
	size_t PRFNUM = timeEcho.rows();
	Eigen::MatrixXcd img = Eigen::MatrixXcd::Zero(ImageHeight, ImageWidth);

	for (int i = 0; i < PRFNUM; i++) {
		gsl_interp_accel* accopm = gsl_interp_accel_alloc();
		const gsl_interp_type* tt = gsl_interp_linear; //gsl_interp_cspline_periodic;
		gsl_spline* spline_time_real = gsl_spline_alloc(tt, timefreqnum);
		gsl_spline* spline_time_imag = gsl_spline_alloc(tt, timefreqnum);

		double* real_time_ys = new double[timefreqnum];
		double* imag_time_ys = new double[timefreqnum];
		for (int tt = 0; tt < timefreqnum; tt++) {
			real_time_ys[tt] = timeEcho(i, tt).real();
			imag_time_ys[tt] = timeEcho(i, tt).imag();
		}
		gsl_spline_init(spline_time_real, Rs, real_time_ys, timefreqnum);
		gsl_spline_init(spline_time_imag, Rs, imag_time_ys, timefreqnum);
		for (int ii = 0; ii < ImageHeight; ii++) {
			for (int jj = 0; jj < ImageWidth; jj++) {
				double px = ii * delta_x + minX;
				double py = jj * delta_y + minY;
				double pz = PlaneZ;
				double dR = 0;
				double R = std::sqrt(
					std::pow(AntPosition(i, 0) - px, 2) +
					std::pow(AntPosition(i, 1) - py, 2) +
					std::pow(AntPosition(i, 2) - pz, 2)
				);
				dR = R - Rref;
				std::complex<double> echo_temp(
					gsl_spline_eval(spline_time_real, R, accopm),// 插值实部
					gsl_spline_eval(spline_time_imag, R, accopm)// 插值虚部
				);
				img(ii, jj) = img(ii, jj) + echo_temp * std::exp(std::complex<double>(0, 2 * pi * dR / wave_len));
			}
		}
		gsl_spline_free(spline_time_real);
		gsl_spline_free(spline_time_imag);
		gsl_interp_accel_free(accopm);
		// 保存文件
		process = process + 1;
		if (logfun) {
			logclss->logFUN((size_t)(process / PRFNUM * 100), "BP process over");
		}
	}
	if (logfun) {
		logclss->logFUN(100, "BP process over");
	}
	return img;
}


template<typename T>
Eigen::MatrixXcd BP2DImageByPixel(Eigen::MatrixXcd timeEcho, Eigen::MatrixXd AntPosition, double minX, double maxX, double minY, double maxY, double PlaneZ, double Rref, size_t ImageWidth, size_t ImageHeight, double startfreq, size_t timefreqnum, double timeFreqBandWidth, T* logclss)
{
	bool logfun = !(nullptr == logclss);
	double delta_x = (maxX - minX) / (ImageWidth - 1);
	double delta_y = (maxY - minY) / (ImageHeight - 1);
	double maxRange = delta_x * ImageWidth > delta_y * ImageHeight ? delta_x * ImageWidth : delta_y * ImageHeight;
	double halfR = Rref * 0.1;
	double maxRLs = std::sqrt(Rref * Rref + halfR * halfR) - Rref;
	double minR = Rref - maxRange * 2 - maxRLs; // 最小斜距
	double maxR = Rref + maxRange * 2 + maxRLs; // 最大斜距

	double dftime = timeFreqBandWidth / (timefreqnum - 1);

	// 后续需要增加距离窗的计算方法


	double dRs = (LIGHTSPEED / 2.0/timeFreqBandWidth); // 距离向分辨率
	double* Rs = new double[timefreqnum]; // 时域 距离插值
	for (int i = 0; i < timefreqnum; i++) {
		Rs[i] = 2*dRs * i; // 参考 《雷达成像算法》-- 匹配滤波一章
	}
	//double wave_len = LIGHTSPEED / startfreq;
	if (logfun) {
		logclss->logFUN(0, "process.....");
	}


	size_t process = 0;
	size_t PRFNUM = timeEcho.rows();

	Eigen::MatrixXcd img = Eigen::MatrixXcd::Zero(ImageHeight, ImageWidth);
	omp_lock_t lock;
	omp_init_lock(&lock); // 初始化互斥锁
	size_t parallel_step = 100;


#pragma omp parallel for // NEW ADD
	for (int ii = 0; ii < ImageHeight; ii = ii + parallel_step) {
		size_t start_i = ii;
		size_t max_i = ii + parallel_step;
		max_i = max_i < ImageHeight ? max_i : ImageHeight;


		double px;
		double py ;
		double pz;
		std::complex<double> echo_sum(0, 0);
		double R = 0;
		double dR = 0;


		// 估算坐标的夹角，估算双线性距离
		Eigen::MatrixXd Rline(PRFNUM, 2);
		Point_3d Vpluse_p;
		Point_3d Vs_e;
		double Rline_ref ;
		double offer_Rline;

		Eigen::MatrixX<std::complex<double>> echoline;
		double real, imag;
		std::complex<double> echo_temp;
		std::complex<double> echo_phase(0, 0);
		std::complex<double> last_value;
		std::complex<double> next_value;
		size_t last_ids;
		size_t next_ids;
		double index = 0;

		Vector3D p;
		Vector3D lineP;
		Vector3D lineDirection;
		double DopplerFreq;
		double wave_len;


		for (int i = start_i; i < max_i; i++)
		{
			for (int j = 0; j < ImageWidth; j++) {
				px = i * delta_x + minX;
				py = j * delta_y + minY;
				pz = PlaneZ;
				echo_sum.real(0);
				echo_sum.imag(0);
				R = 0;
				dR = 0;


				for (int mm = 0; mm < PRFNUM; mm++) { // 计算每个脉冲情况
					Rline(mm, 0) = std::sqrt(
						std::pow(AntPosition(mm, 0) - px, 2) +
						std::pow(AntPosition(mm, 1) - py, 2) +
						std::pow(AntPosition(mm, 2) - pz, 2)
					);

					// 计算夹角
					Vpluse_p.x = px - AntPosition(mm, 0);
					Vpluse_p.y = py - AntPosition(mm, 1);
					Vpluse_p.z = pz - AntPosition(mm, 2);
					if (mm == 0) {
						Vs_e.x = AntPosition(mm + 1, 0) - AntPosition(mm, 0);
						Vs_e.y = AntPosition(mm + 1, 1) - AntPosition(mm, 1);
						Vs_e.z = AntPosition(mm + 1, 2) - AntPosition(mm, 2);
					}
					else {
						Vs_e.x = AntPosition(mm, 0) - AntPosition(mm - 1, 0);
						Vs_e.y = AntPosition(mm, 1) - AntPosition(mm - 1, 1);
						Vs_e.z = AntPosition(mm, 2) - AntPosition(mm - 1, 2);
					}

					Rline(mm, 1) = Vpluse_p.x * Vs_e.x + Vpluse_p.y * Vs_e.y + Vpluse_p.z * Vs_e.z; // cos(90)=0
				}
				Rline_ref = Rref;
				// 查找最小值，获取 0多普勒条件下的 最短参考路径
				for (int mm = 0; mm < PRFNUM - 1; mm++) {
					if (Rline(mm, 0) == 0) { // 求解最小值
						Rline_ref = Rline(mm, 0);
						break;
					}
					else {
						double flag = Rline(mm, 1) * Rline(mm + 1, 1);
						if (flag < 0) { // 不同号
							// 求解目标点到 当前直线的最短距离
							p.x = px;
							p.y = py;
							p.z = pz; 
							lineP.x = AntPosition(mm, 0);
							lineP.y = AntPosition(mm, 1);
							lineP.z = AntPosition(mm, 2);
							lineDirection.x = AntPosition(mm + 1, 0) - AntPosition(mm, 0);
							lineDirection.y = AntPosition(mm + 1, 1) - AntPosition(mm, 1);
							lineDirection.z = AntPosition(mm + 1, 2) - AntPosition(mm, 2);

							Rline_ref = pointToLineDistance(p, lineP, lineDirection);
							break;
						}
					}
				}
				
				offer_Rline = Rline_ref - Rref; // 计算偏移线
				Rline = Rline.array() + offer_Rline;

				for (int mm = 0; mm < PRFNUM; mm++) {
					R= Rline(mm, 0); // 已经校正了距离徙动线
					
					//if (R<minR || R > maxR) { // 距离窗
					//	continue;
					//} 
					dR = R - Rref;
					// 插值计算
					index = R / 2 / dRs;
					last_ids = size_t(std::floor(index));
					next_ids = size_t(std::ceil(index));

					last_value = timeEcho(mm, last_ids);
					next_value = timeEcho(mm, next_ids);

					// 实部，虚部同时插值
					real = last_value.real() + ((next_value.real() - last_value.real()) / (next_ids - last_ids)) * (index - last_ids);
					imag = last_value.imag() + ((next_value.imag() - last_value.imag()) / (next_ids - last_ids)) * (index - last_ids);
					echo_temp.real(real);
					echo_temp.imag(imag);

					DopplerFreq =std::sqrt(R*R - Rline_ref* Rline_ref) / Rline_ref * startfreq;
					wave_len =  std::abs(LIGHTSPEED / DopplerFreq);  
					echo_phase.imag(-2 * pi * dR / wave_len);
					echo_sum = echo_sum + echo_temp;// *std::exp(echo_phase);
				}
				img(i, j) = echo_sum;
			}
			// 保存文件

		}
		omp_set_lock(&lock);
		process = process + parallel_step;
		process = process < ImageHeight ? process : ImageHeight;
		if (logfun) {
			logclss->logFUN(size_t(std::round(process * 1.0 / ImageHeight * 100)), "BP process over");
		}
		omp_unset_lock(&lock);
	}
	omp_destroy_lock(&lock); //销毁互斥器
	if (logfun) {
		logclss->logFUN(100, "BP process over");
	}
	return img;
}


int BP2DProcessClass::initProcess(QString in_path, QString out_path, double Rref, double minX, double maxX, double minY, double maxY, double PlaneZ, int ImageHeight, int ImageWidth)
{
	// 初始化参数
	this->readEchoFile(in_path);
	this->out_path = out_path;
	this->Rref = Rref; // 成像中心的参考距离
	this->minX = minX;
	this->maxX = maxX;
	this->minY = minY;
	this->maxY = maxY;
	this->Zplane = PlaneZ;
	this->ImageHeight = ImageHeight;
	this->ImageWidth = ImageWidth;
	// 计算坐标的到成像中心的坐标
	this->centerX = (this->maxX + this->minX) / 2;
	this->centerY = (this->minY + this->maxY) / 2;
	this->AntPosition.col(0) = this->AntPosition.col(0).array() - this->centerX;
	this->AntPosition.col(1) = this->AntPosition.col(1).array() - this->centerY;
	this->AntPosition.col(2) = this->AntPosition.col(2).array() - this->Zplane;
	// 计算频率相关信息
	this->f1 = this->Frequencylist(0);
	this->freqnum = this->Frequencylist.rows();
	this->fc = (this->f1 + this->Frequencylist(this->freqnum - 1));
	return -1;
}

int BP2DProcessClass::readEchoFile(QString in_path)
{
	std::ifstream fin(in_path.toUtf8().constData(), std::ios::in | std::ios::binary);
	if (!fin.is_open()) exit(2);
	// 读取参数
	int height = 0;
	int width = 0;
	fin.read((char*)&height, sizeof(int));
	fin.read((char*)&width, sizeof(int));
	this->height = height;
	this->width = width;
	//frequencylist[width*double] 
	double* freqs = new double[width];
	fin.read((char*)freqs, sizeof(double) * width);
	this->Frequencylist = Eigen::VectorXd::Zero(width, 1); // 列向量
	for (int i = 0; i < width; i++) {
		this->Frequencylist(i) = freqs[i];
	}
	delete[] freqs;
	freqs = nullptr;

	// 读取回波并解析脉冲天线位置
	this->AntPosition = Eigen::MatrixXd::Zero(height, 3);
	this->echo = Eigen::MatrixXcd::Zero(height, width);
	double tempvalue = 0;
	for (int i = 0; i < height; i++) {
		fin.read((char*)&tempvalue, sizeof(double)); this->AntPosition(i, 0) = tempvalue; //X
		fin.read((char*)&tempvalue, sizeof(double)); this->AntPosition(i, 1) = tempvalue;//Y      
		fin.read((char*)&tempvalue, sizeof(double)); this->AntPosition(i, 2) = tempvalue;//Z

		// echo
		for (int j = 0; j < width; j++) {
			fin.read((char*)&tempvalue, sizeof(double)); // real
			this->echo(i, j) = this->echo(i, j) + tempvalue;
		}
		//imag
		for (int j = 0; j < width; j++) {
			fin.read((char*)&tempvalue, sizeof(double)); // real
			this->echo(i, j) = this->echo(i, j) + std::complex<double>(0, tempvalue);
		}
	}
	fin.close();
	return -1;
}

int BP2DProcessClass::saveTiFF(Eigen::MatrixXcd m)
{
	return saveMatrixXcd2TiFF(m, this->out_path);
}

/// <summary>
/// 成像工作流，注意存在大量的内存浪费，后期可以根据情况进行优化
/// </summary>
/// <returns></returns>
int BP2DProcessClass::start()
{
	// step 0 生成文件夹路径，为中间临时文件输出,构建临时环境，正式版需要注释相关代码
	QString parantPath = getParantFolderNameFromPath(this->out_path);


	// step 1 插值频率域 -- FEKO 频率插值是均匀插值
	size_t Nf = this->freqnum;
	size_t Nxa = this->AntPosition.rows();
	double df = this->Frequencylist(1) - this->Frequencylist(0); // FEKO 的频率插值比较规整
	double startfreq = this->Frequencylist(0); // 起始频率

	// 输出频域 图像（未处理前） 并将复数数据转换为 振幅 与 相位 ，行：方位向，列：距离向-------------------
	QString before_path = JoinPath(parantPath, "freq_ampDB_angle_echo.tiff");
	WriteComplexData2AmpdB_Arg(before_path, this->echo);// 保存为频域回波
	this->logFUN(100, "Read Echo\n");
	// ----------------------------------------------------------------------------------------------

	// ----------------------------------------------------------------------------------------------

	this->logFUN(100, "hamming windows over");
	// ----------------- step2 傅里叶变换 （频域-->时域） -----------------------------------------
	Eigen::MatrixXcd echoTime = IFFTW1D(this->echo);
	size_t timefreqnum = echoTime.cols();
	double timeFreqBandWidth = df * (timefreqnum - 1);
	this->logFUN(100, "echo IFFT over");

	// 输出频域 图像（加窗） 并将复数数据转换为 振幅 与 相位 ，行：方位向，列：距离向-------------------
	QString time_echo_path = JoinPath(parantPath, "freq_ampDB_angle_echo_hanning_ifft.tiff");
	WriteComplexData2AmpdB_Arg(time_echo_path, echoTime);// 保存为频域回波
	this->logFUN(100, "after ifft ,in Time\n");
	// ----------------------------------------------------------------------------------------------
	// step 3 时域域加窗
	Nf = echoTime.cols();
	Eigen::MatrixXd wfxa = Hanning(Nf, Nxa, 0.54);					// wfxa = hanning(Nf,Nxa,alpha=0.54)
	double normw = wfxa.array().sum();								// normw = TOTAL(wfxa)

	Eigen::MatrixXcd dfrex = (echoTime.array())* (wfxa.array());		// dfrex = dfrex * wfxa
	// 输出频域 图像（加窗） 并将复数数据转换为 振幅 与 相位 ，行：方位向，列：距离向-------------------
	QString hanning_path = JoinPath(parantPath, "freq_ampDB_angle_echo_hanning.tiff");
	WriteComplexData2AmpdB_Arg(hanning_path, dfrex);// 保存为频域回波
	this->logFUN(100, "after hanning\n");
	echoTime = dfrex;
	// ----------------- step4 TBP成像 （时域） -----------------------------------------------------------
	Eigen::MatrixXcd img = BP2DImageByPixel(echoTime, this->AntPosition, this->minX, this->maxX, this->minY, this->maxY, this->Zplane, this->Rref, this->ImageWidth, this->ImageHeight, startfreq, timefreqnum, timeFreqBandWidth, this);
	this->logFUN(100, "echo bp over");



	// 输出 最终结果图像  并将复数数据转换为 振幅 与 相位 ，行：方位向，列：距离向-------------------
	QString FBP_resultDB_path = JoinPath(parantPath, "freq_ampDB_angle_echo_hanning_BP.tiff");
	WriteComplexData2AmpdB_Arg(FBP_resultDB_path, img);// 保存为频域回波
	// ----------------------------------------------------------------------------------------
	// 输出 最终结果图像  并将复数数据转换为 振幅 与 相位 ，行：方位向，列：距离向-------------------
	QString FBP_result_path = JoinPath(parantPath, "freq_amp_angle_echo_hanning_BP.tiff");
	WriteComplexData2Amp_Arg(FBP_result_path, img);// 保存为频域回波
	// ----------------------------------------------------------------------------------------

	this->saveTiFF(img);
	this->logFUN(100, "out bpimag over : " + this->out_path);
	return -1;
}

int BP2DProcessClass::logFUN(int percent, QString logtext) {
	qDebug() << "\rBPProcess [" << percent << "%]\t" << logtext;
	if (percent < 100) {
		qDebug()<<"\n";
	}
	return 0;
};






////////////////////////////////////////////////////////////////////////////////////////
/// <summary>
/// FBP成像工作流（频域），参考 bp_linear_2d
/// Juan M Lopez-Sanchez, University of Alicante
/// </summary>
/// <returns></returns>
////////////////////////////////////////////////////////////////////////////////////////
















int FBP2DProcessClass::initProcess(QString in_path, QString out_path, double Rref, double minX, double maxX, double minY, double maxY, double PlaneZ, int ImageHeight, int ImageWidth)
{
	// 初始化参数
	this->readEchoFile(in_path);
	this->out_path = out_path;
	this->Rref = Rref; // 成像中心的参考距离
	this->minX = minX;
	this->maxX = maxX;
	this->minY = minY;
	this->maxY = maxY;
	this->Zplane = PlaneZ;
	this->ImageHeight = ImageHeight;
	this->ImageWidth = ImageWidth;
	// 计算坐标的到成像中心的坐标
	this->centerX = (this->maxX + this->minX) / 2;
	this->centerY = (this->minY + this->maxY) / 2;
	this->AntPosition.col(0) = this->AntPosition.col(0).array() - this->centerX;
	this->AntPosition.col(1) = this->AntPosition.col(1).array() - this->centerY;
	this->AntPosition.col(2) = this->AntPosition.col(2).array() - this->Zplane;
	// 计算频率相关信息
	this->f1 = this->Frequencylist(0);
	this->freqnum = this->Frequencylist.rows();
	this->fc = (this->f1 + this->Frequencylist(this->freqnum - 1));
	return -1;
}

int FBP2DProcessClass::readEchoFile(QString in_path)
{
	std::ifstream fin(in_path.toUtf8().constData(), std::ios::in | std::ios::binary);
	if (!fin.is_open()) exit(2);
	// 读取参数
	int height = 0;
	int width = 0;
	fin.read((char*)&height, sizeof(int));
	fin.read((char*)&width, sizeof(int));
	this->height = height;
	this->width = width;
	//frequencylist[width*double] 
	double* freqs = new double[width];
	fin.read((char*)freqs, sizeof(double) * width);
	this->Frequencylist = Eigen::VectorXd::Zero(width, 1); // 列向量
	for (int i = 0; i < width; i++) {
		this->Frequencylist(i) = freqs[i];
	}
	delete[] freqs;
	freqs = nullptr;

	// 读取回波并解析脉冲天线位置
	this->AntPosition = Eigen::MatrixXd::Zero(height, 3);
	this->echo = Eigen::MatrixXcd::Zero(height, width);
	double tempvalue = 0;
	for (int i = 0; i < height; i++) {
		fin.read((char*)&tempvalue, sizeof(double)); this->AntPosition(i, 0) = tempvalue; //X
		fin.read((char*)&tempvalue, sizeof(double)); this->AntPosition(i, 1) = tempvalue;//Y      
		fin.read((char*)&tempvalue, sizeof(double)); this->AntPosition(i, 2) = tempvalue;//Z

		// echo
		for (int j = 0; j < width; j++) {
			fin.read((char*)&tempvalue, sizeof(double)); // real
			this->echo(i, j) = this->echo(i, j) + tempvalue;
		}
		//imag
		for (int j = 0; j < width; j++) {
			fin.read((char*)&tempvalue, sizeof(double)); // real
			this->echo(i, j) = this->echo(i, j) + std::complex<double>(0, tempvalue);
		}
	}
	fin.close();
	return -1;
}

int FBP2DProcessClass::saveTiFF(Eigen::MatrixXcd m)
{
	return saveMatrixXcd2TiFF(m, this->out_path);
}

















/// <summary>
/// FBP成像工作流（频域），参考 bp_linear_2d
/// Juan M Lopez-Sanchez, University of Alicante
/// </summary>
/// <returns></returns>
int FBP2DProcessClass::start()
{
	// step 0 生成文件夹路径，为中间临时文件输出,构建临时环境，正式版需要注释相关代码
	QString parantPath = getParantFolderNameFromPath(this->out_path);


	// step 1 插值频率域 -- FEKO 频率插值是均匀插值
	size_t Nf = this->freqnum;
	size_t Nxa = this->AntPosition.rows();
	// 输出频域 图像（未处理前） 并将复数数据转换为 振幅 与 相位 ，行：方位向，列：距离向-------------------
	QString before_path = JoinPath(parantPath, "freq_ampDB_angle_echo.tiff");
	WriteComplexData2AmpdB_Arg(before_path, this->echo);// 保存为频域回波
	this->logFUN(100, "Read Echo\n");
	// ----------------------------------------------------------------------------------------------
	// step 2 频率域加窗
	Eigen::MatrixXd wfxa = Hanning(Nf, Nxa, 0.54);					// wfxa = hanning(Nf,Nxa,alpha=0.54)
	double normw = wfxa.array().sum();								// normw = TOTAL(wfxa)

	Eigen::MatrixXcd dfrex = (this->echo.array()) * (wfxa.array());		// dfrex = dfrex * wfxa
	// 输出频域 图像（加窗） 并将复数数据转换为 振幅 与 相位 ，行：方位向，列：距离向-------------------
	QString hanning_path = JoinPath(parantPath, "freq_ampDB_angle_echo_hanning.tiff");
	WriteComplexData2AmpdB_Arg(hanning_path, dfrex);// 保存为频域回波
	this->logFUN(100, "after hanning\n");
	// ----------------------------------------------------------------------------------------------
	// step 3 Freq Backprojection algorithm
	Eigen::MatrixXcd result_image = Eigen::MatrixXcd::Zero(this->ImageHeight, this->ImageWidth); // 构建网格，每个网格坐标根据中心点，平面点计算
	double dx = (this->maxX - this->minX) / (this->ImageWidth - 1);
	double dy = (this->maxY - this->minY) / (this->ImageWidth - 1);
	double px = 0, py = 0, pz = 0; // 像素坐标
	Eigen::MatrixXd Xdis = Eigen::MatrixXd::Zero(Nxa, Nf);

	// ;  Nxa Nf
	// mfre = fre # replicate(1, Nxa) ---- Nxa * Nf 复制每行 f1,f2,f3 .......
	// c = 0.2997925 ; Speed of light
	// factorj = cj * (4.0*!PI/c) * mfre --- Nxa * Nf 复制每行 f1,f2,f3 .......
	// 

	Eigen::MatrixXcd factorj = Eigen::MatrixXcd::Zero(Nxa, Nf);
	for (size_t i = 0; i < Nxa; i++) {
		for (size_t j = 0; j < Nf; j++) {
			factorj(i, j) = std::complex<double>(0, 4.0 * PI / (LIGHTSPEED * 1e-9)) * (this->Frequencylist(j) * 1e-9); //  cj * (4.0*!PI/c) * mfre 
		}
	}

	double R0 = this->Rref;// -- 参考斜距
	Eigen::MatrixXd R = Eigen::MatrixXd::Zero(Nxa, Nf);
	Eigen::MatrixXd distR = Eigen::MatrixXd::Zero(Nxa, 1);
	Eigen::MatrixXcd term = Eigen::MatrixXcd::Zero(Nxa, Nf);
	Eigen::MatrixXcd data = Eigen::MatrixXcd::Zero(Nxa, Nf);

	for (size_t ix = 0; ix < this->ImageWidth; ix++) {
		if (ix % 100 == 0) {
			this->logFUN(size_t(ix * 1.0 / this->ImageWidth * 100), "FBP ....\n");
		}
		// theta = theta / !radeg  -- 根据入射角 计算 参考坐标
		// ya = Ro * sin(theta)   -- 雷达 y 坐标
		// za = Ro * cos(theta)   -- 雷达 z 坐标
		// 
		// xa_min = pos_start
		// xa_max = pos_stop
		// Nxa = Npos
		// 
		// dxa = ( xa_max - xa_min ) / float(Nxa-1)
		// xa = xa_min + findgen(Nxa) * dxa  -- 雷达 x 坐标
		//
		// dx = (x_max-x_min) / float(Nx-1)
		// x = x_min + findgen(Nx) * dx
		// 
		// dy = (y_max-y_min) / float(Ny-1)
		// y = y_min + findgen(Ny) * dy
		// mxa = replicate(1, Nf) # xa			;--- Nxa * Nf 每个列 ant_x1，   ant_x2,   ant_x3,......
		// xdis = (x[ix]-mxa)^2 + za^2			;--- Nxa * Nf 每个列 ant_x1_z1，ant_x2_z2,ant_x3_z3,......
		// R = sqrt( xdis + (ya - y[iy])^2 )	;--- Nxa * Nf 每个列 ant_x1，   ant_x2,   ant_x3,......
		// 
		px = ix * dx + this->minX;
		pz = this->Zplane;
		for (size_t iy = 0; iy < this->ImageHeight; iy++) {
			py = iy * dy + this->minY;

			// mxa = replicate(1, Nf) # xa			;--- Nxa * Nf 每个列 ant_x1，   ant_x2,   ant_x3,......
			// xdis = (x[ix]-mxa)^2 + za^2			;--- Nxa * Nf 每个列 ant_x1_z1，ant_x2_z2,ant_x3_z3,......
			// R = sqrt( xdis + (ya - y[iy])^2 )	;--- Nxa * Nf 每个列 ant_x1，   ant_x2,   ant_x3,......
			// 

			distR = ((this->AntPosition.col(0).array() - px).pow(2) + (this->AntPosition.col(1).array() - py).pow(2) + (this->AntPosition.col(2).array() - pz).pow(2)).array().sqrt().array(); // 复制

			for (size_t jj = 0; jj < Nf; jj++) { // 逐列复制
				R.col(jj) = distR.array();
			}
			// term = (R/Ro)^2 * exp( factorj * ( R - Ro ) ) ; Nxa x Nf

			term = (R.array() / R0).array().pow(2) * ((factorj.array() * (R.array() - R0)).array().exp());
			data = dfrex.array() * term.array();
			result_image(iy, ix) = data.array().sum();
			//  临时文件输出-----------------------
			if (iy == this->ImageHeight / 2 && ix == this->ImageWidth / 2) {
				// 输出 校正值  并将复数数据转换为 振幅 与 相位 ，行：方位向，列：距离向-------------------
				QString FBP_termDB_path = JoinPath(parantPath, "freq_ampDB_angle_echo_hanning_term_FBP.tiff");
				WriteComplexData2AmpdB_Arg(FBP_termDB_path, term);// 保存为频域回波

				// 输出 校正后图像  并将复数数据转换为 振幅 与 相位 ，行：方位向，列：距离向-------------------
				QString FBP_dataDB_path = JoinPath(parantPath, "freq_ampDB_angle_echo_hanning_data_FBP.tiff");
				WriteComplexData2AmpdB_Arg(FBP_dataDB_path, data);// 保存为频域回波

				// ----------------------------------------------------------------------------------------------
			}

			/***/
		}
	}
	result_image = result_image.array() / normw;
	// 输出 最终结果图像  并将复数数据转换为 振幅 与 相位 ，行：方位向，列：距离向-------------------
	QString FBP_resultDB_path = JoinPath(parantPath, "freq_ampDB_angle_echo_hanning_FBP.tiff");
	WriteComplexData2AmpdB_Arg(FBP_resultDB_path, result_image);// 保存为频域回波
	// ----------------------------------------------------------------------------------------------
	// 输出 最终结果图像  并将复数数据转换为 振幅 与 相位 ，行：方位向，列：距离向-------------------
	QString FBP_result_path = JoinPath(parantPath, "freq_amp_angle_echo_hanning_FBP.tiff");
	WriteComplexData2Amp_Arg(FBP_result_path, result_image);// 保存为频域回波
	// ----------------------------------------------------------------------------------------------
	this->saveTiFF(result_image);

	return 0;
}

int FBP2DProcessClass::logFUN(int percent, QString logtext)
{
	qDebug() << "\rFBPProcess [" << percent << "%]\t" << logtext;
	if (percent < 100) {
		qDebug() << "\n";
	}
	return 0;
}

int FEKOFarFieldProcessClass::initProcess(QString in_path, QString out_path, double Rref, double minX, double maxX, double minY, double maxY, double PlaneZ, int ImageHeight, int ImageWidth)
{
	// 初始化参数
	this->readEchoFile(in_path);
	this->out_path = out_path;
	this->Rref = Rref; // 成像中心的参考距离
	this->minX = minX;
	this->maxX = maxX;
	this->minY = minY;
	this->maxY = maxY;
	this->Zplane = PlaneZ;
	this->ImageHeight = ImageHeight;
	this->ImageWidth = ImageWidth;
	// 计算坐标的到成像中心的坐标
	this->centerX = (this->maxX + this->minX) / 2;
	this->centerY = (this->minY + this->maxY) / 2;
	this->AntPosition.col(0) = this->AntPosition.col(0).array() - this->centerX;
	this->AntPosition.col(1) = this->AntPosition.col(1).array() - this->centerY;
	this->AntPosition.col(2) = this->AntPosition.col(2).array() - this->Zplane;
	// 计算频率相关信息
	this->f1 = this->Frequencylist(0);
	this->freqnum = this->Frequencylist.rows();
	this->fc = (this->f1 + this->Frequencylist(this->freqnum - 1));
	return -1;
}

int FEKOFarFieldProcessClass::readEchoFile(QString in_path)
{
	std::ifstream fin(in_path.toUtf8().constData(), std::ios::in | std::ios::binary);
	if (!fin.is_open()) exit(2);
	// 读取参数
	int height = 0;
	int width = 0;
	fin.read((char*)&height, sizeof(int));
	fin.read((char*)&width, sizeof(int));
	this->height = height;
	this->width = width;
	//frequencylist[width*double] 
	double* freqs = new double[width];
	fin.read((char*)freqs, sizeof(double) * width);
	this->Frequencylist = Eigen::VectorXd::Zero(width, 1); // 列向量
	for (int i = 0; i < width; i++) {
		this->Frequencylist(i) = freqs[i];
	}
	delete[] freqs;
	freqs = nullptr;

	// 读取回波并解析脉冲天线位置
	this->AntPosition = Eigen::MatrixXd::Zero(height, 3);
	this->echo = Eigen::MatrixXcd::Zero(height, width);
	double tempvalue = 0;
	for (int i = 0; i < height; i++) {
		fin.read((char*)&tempvalue, sizeof(double)); this->AntPosition(i, 0) = tempvalue; //X
		fin.read((char*)&tempvalue, sizeof(double)); this->AntPosition(i, 1) = tempvalue;//Y      
		fin.read((char*)&tempvalue, sizeof(double)); this->AntPosition(i, 2) = tempvalue;//Z

		// echo
		for (int j = 0; j < width; j++) {
			fin.read((char*)&tempvalue, sizeof(double)); // real
			this->echo(i, j) = this->echo(i, j) + tempvalue;
		}
		//imag
		for (int j = 0; j < width; j++) {
			fin.read((char*)&tempvalue, sizeof(double)); // real
			this->echo(i, j) = this->echo(i, j) + std::complex<double>(0, tempvalue);
		}
	}
	fin.close();
	return -1;
}

int FEKOFarFieldProcessClass::saveTiFF(Eigen::MatrixXcd m)
{
	return saveMatrixXcd2TiFF(m, this->out_path);
}

int FEKOFarFieldProcessClass::start()
{

	//// step 0 生成文件夹路径，为中间临时文件输出,构建临时环境，正式版需要注释相关代码
	//QString parantPath = getParantFolderNameFromPath(this->out_path);


	//double freqbandWidth = this->Frequencylist(this->get_Nf() - 1) - this->Frequencylist(0);
	//double dx = LIGHTSPEED / 2.0 / std::abs(freqbandWidth); // 距离向分辨率
	//double startPhiAngle = this->AntPosition(0, 1);
	//double endPhiAngle = this->AntPosition(this->get_Nxa(), 1);
	//double AzAngleWidth = std::abs(endPhiAngle - startPhiAngle);
	//double halfAzAngleWidth = AzAngleWidth / 2.0;
	//double AzBandWidth = 2 * std::tan(Degrees2Radians(halfAzAngleWidth)) * this->get_minFreq();
	//double dy = LIGHTSPEED / 2.0 / std::abs(AzBandWidth); // 方位向分辨率



	//// step 1 插值频率域 -- FEKO 频率插值是均匀插值
	//size_t Nf = this->freqnum;
	//size_t Nxa = this->AntPosition.rows();   



	//double angStart = startPhiAngle;
	//double angEnd = endPhiAngle;
	//double freqEnd = this->Frequencylist(this->get_Nf() - 1);
	//double fxStart = this->Frequencylist(0);
	//double fyStart = std::tan(-1 * std::abs(halfAzAngleWidth)) * fxStart;
	//double fyEnd = std::tan(-1 * std::abs(halfAzAngleWidth)) * fxStart;
	//double fxEnd = std::sqrt(std::pow(freqEnd, 2) - std::pow(fyStart, 2));

	//double freqSamples = this->Frequencylist.rows();
	//double angSamples = this->AntPosition.rows();

	//double ysd = (fyEnd - fyStart) / (angSamples - 1); // 转换为 网格节点
	//double xsd = (fxEnd - fxStart) / (freqSamples - 1); // 转换为 网格节点

	//Eigen::MatrixXd image_points_freq(angSamples, freqSamples);
	//Eigen::MatrixXd image_points_ang(angSamples, freqSamples);
	//Eigen::MatrixXcd Echo_data(angSamples, freqSamples);

	//Eigen::MatrixXd xi(Nxa,Nf);
	//Eigen::MatrixXd yi(Nxa, Nf);


	//// 创建网格
	//for (int f = 0; f < freqSamples; f++) {
	//	for (int a = 0; a < angSamples; a++) {
	//		double x = fxStart + f * xsd;
	//		double y = fyStart + a * ysd;
	//		image_points_freq(a, f) = std::sqrt(x * x + y * y); // 获取每个点的 频率
	//		image_points_ang(a, f) = std::atan2(y, x); // 获取每个点的 角度坐标
	//		Echo_data(a, f) = InterpolateComplex(xi, yi, this->echo, image_points_freq(a, f), image_points_ang(a, f)); // 插值得到区域数据

	//	}
	//}
	//
	//// step 2 频率域加窗
	//Eigen::MatrixXd wfxa = Hanning(Nf, Nxa, 0.54);					// wfxa = hanning(Nf,Nxa,alpha=0.54)
	//double normw = wfxa.array().sum();								// normw = TOTAL(wfxa)

	//Eigen::MatrixXcd dfrex = (this->echo.array()) * (wfxa.array());		// dfrex = dfrex * wfxa
	//// 输出频域 图像（加窗） 并将复数数据转换为 振幅 与 相位 ，行：方位向，列：距离向-------------------
	//QString hanning_path = JoinPath(parantPath, "freq_ampDB_angle_echo_hanning.tiff");
	//WriteComplexData2AmpdB_Arg(hanning_path, dfrex);// 保存为频域回波
	//this->logFUN(100, "after hanning\n");

	//Echo_data = dfrex;

	//
	//// 执行二维傅立叶变换
	//Eigen::MatrixXcd H_echo_win_fft = FFTW2D(Echo_data);

	//// 获取矩阵的行数和列数
	//int n_freq = Echo_data.rows();
	//int n_angle = Echo_data.cols();

	//// 将傅立叶变换结果归一化
	//H_echo_win_fft = H_echo_win_fft.array()/(n_freq * n_angle);

	//// 执行逆傅立叶变换（如果需要）
	//// MatrixXcd H_echo_win_ifft = ifft2(H_echo_win_fft);
	//// H_echo_win_ifft /= (n_freq * n_angle);

	//// 执行平移
	//Eigen::MatrixXcd result_image = fftshift(H_echo_win_fft);


	//// 输出 最终结果图像  并将复数数据转换为 振幅 与 相位 ，行：方位向，列：距离向-------------------
	//QString FBP_resultDB_path = JoinPath(parantPath, "freq_ampDB_angle_echo_hanning_FBP.tiff");
	//WriteComplexData2AmpdB_Arg(FBP_resultDB_path, result_image);// 保存为频域回波
	//// ----------------------------------------------------------------------------------------------
	//// 输出 最终结果图像  并将复数数据转换为 振幅 与 相位 ，行：方位向，列：距离向-------------------
	//QString FBP_result_path = JoinPath(parantPath, "freq_amp_angle_echo_hanning_FBP.tiff");
	//WriteComplexData2Amp_Arg(FBP_result_path, result_image);// 保存为频域回波
	//// ----------------------------------------------------------------------------------------------
	//this->saveTiFF(result_image);


	return 0;
}

int FEKOFarFieldProcessClass::logFUN(int percent, QString logtext)
{
	qDebug() << "\rFBPProcess [" << percent << "%]\t" << logtext;
	if (percent < 100) {
		qDebug() << "\n";
	}
	return 0;
}
