#include "BaseToollib/ImageOperatorBase.h"
#include "SARBaseTool.h"
#include "SARImageBase.h"
#include <Eigen/Core>
#include <Eigen/Dense>
#include <omp.h>
#include <io.h>
#include <stdio.h>
#include <stdlib.h>
#include <iostream>
#include <gdal.h>
#include <gdal_priv.h>
#include <gdalwarper.h>
#include <proj.h>
#include <string.h>
#include <memory.h>
#include <memory>
#include <fftw3.h>
#include <gsl/gsl_integration.h>
#include <gsl/gsl_spline.h>
#include "referenceHeader.h"
#include "OCCTBase.h"
#include "BaseToollib/GeoOperator.h"
#include "SARBaseToolLib/SARImageBase.h"





double getRangeResolution(double startfreq, double endfreq)
{
	return LIGHTSPEED / 2 / std::abs(endfreq - startfreq);
}

double getAzimuthResolution(double AzAngleRange, double startFreq)
{
	return LIGHTSPEED / 2 / (AzAngleRange * startFreq);
}

Eigen::MatrixXd  hammingWindows(size_t num)
{
	double alpha = 25. / 46;
	double beta = 1 - alpha;
	double scale = 1;
	size_t m = num;
	Eigen::MatrixXd x = Eigen::MatrixXd::Zero(m, 1);
	for (int i = 0; i < m; i++) {
		x(i, 0) = alpha - beta * std::cos(2 * PI * i / (num - 1));
	}
	x = x.array() * scale;
	return  x;
}


Eigen::MatrixXd Hanning(size_t Nf, size_t Nxa, double alpha)
{
										// n1 = double ? DOUBLE(n1In[0]) : FLOAT(n1In[0])
	double a = 2 * PI / Nf;				// a = 2 * pi / N1           ;scale factor
	double n2 = Nxa;					// n2 = double ? DOUBLE(n2In[0]) : FLOAT(n2In[0])
	double b = 2 * pi / n2;				// dim 2 scale fact
	double one = 1.0;					// double ? 1d : 1.0
	double temp_row = 0;
	double temp_col = 0;
	Eigen::MatrixXd result_arr = Eigen::MatrixXd::Zero(Nxa, Nf); // 结果
	for (int i = 0; i < Nxa; i++) {
		for (int j = 0; j < Nf; j++) {
																   // index = double ? DINDGEN(n1) : FINDGEN(n1) ; Nf
			temp_row = (alpha - one) * cos(j * a) + alpha; // (alpha-one) * cos(index*a) + alpha ;One row
																   // index = double ? DINDGEN(n2) : FINDGEN(n2) ; Nxa
			temp_col = (alpha - one) * cos(i * b) + alpha; // col = (alpha-one) * cos(index*b) + alpha ;One column
			result_arr(i, j) = temp_row * temp_col;//  RETURN,(row # col) ;DINDGEN(n1)#DINDGEN(n2) 
		}
	}
	return result_arr;
}


Eigen::MatrixXcd HammingWindows(Eigen::MatrixXcd FreqEcho)
{
	size_t data_cols = FreqEcho.cols();
	size_t data_rows = FreqEcho.rows();

	// 进行进行hanmming windows 
	Eigen::MatrixXd hamming_Az = hammingWindows(data_cols);
	Eigen::MatrixXd hamming_Rz = hammingWindows(data_rows);
	for (int i = 0; i < data_cols; i++) {
		for (int j = 0; j < data_rows; j++) {
			FreqEcho(j, i) = FreqEcho(j, i) * hamming_Az(i, 0) * hamming_Rz(j, 0);
		}
	}
	return FreqEcho;
}

Eigen::MatrixXcd InterpFreqEcho(Eigen::MatrixXcd freqEcho, Eigen::VectorXd SourceFreqlist, Eigen::VectorXd interpFreqlist)
{
	assert(freqEcho.cols() == SourceFreqlist.rows());



	size_t data_rows = freqEcho.rows();
	size_t data_cols = freqEcho.cols();

	size_t out_cols = interpFreqlist.rows();
	Eigen::MatrixXcd resultECHO = Eigen::MatrixXcd::Zero(data_rows, out_cols);


#pragma omp parallel for  // NEW ADD
	for (int i = 0; i < data_rows; i++) { //  单脉冲
		double* xs = new double[data_rows];
		for (int i = 0; i < data_cols; i++) {
			xs[i] = SourceFreqlist(i); // 原始回波
		}
		double* real_ys = new double[data_cols]; 
		double* imag_ys = new double[data_cols];

		gsl_interp_accel* acc = gsl_interp_accel_alloc();
		const gsl_interp_type* t = gsl_interp_cspline_periodic;
		gsl_spline* spline_real = gsl_spline_alloc(t, data_rows);
		gsl_spline* spline_imag = gsl_spline_alloc(t, data_rows);

		for (int jj = 0; jj < data_cols; jj++) {
			real_ys[jj] = freqEcho(jj, i).real();
			imag_ys[jj] = freqEcho(jj, i).imag();
		}
		gsl_spline_init(spline_real, xs, real_ys, data_rows);
		gsl_spline_init(spline_imag, xs, imag_ys, data_rows);
		for (int j = 0; j < out_cols; j++) { // 受限于当前函数只能逐点插值，或者后期可修改为 gsl 直接插值
			double cx = interpFreqlist(j,0);
			std::complex<double> result(
				gsl_spline_eval(spline_real, cx, acc),// 插值实部
				gsl_spline_eval(spline_imag, cx, acc)// 插值虚部
			);
			resultECHO(i, j) = result;
		}

		gsl_spline_free(spline_real);
		gsl_spline_free(spline_imag);
		gsl_interp_accel_free(acc);

		delete[] xs;
		delete[] real_ys, imag_ys;
	}
	return resultECHO;
}

int WriteComplexData2Amp_Arg(QString out_tiff_path, Eigen::MatrixXcd data)
{
	Eigen::MatrixXd amp_data = Eigen::MatrixXd::Zero(data.rows(), data.cols());
	Eigen::MatrixXd arg_data = Eigen::MatrixXd::Zero(data.rows(), data.cols());
	size_t row_count = data.rows();
	size_t col_count = data.cols();
	for (int i = 0; i < row_count; i++) {
		for (int j = 0; j < col_count; j++) {
			amp_data(i, j) = std::abs(data(i, j));
			arg_data(i, j) = std::arg(data(i, j));
		}
	}


	Eigen::MatrixXd gt = Eigen::MatrixXd::Zero(2, 3);

	gdalImage image_tiff = CreategdalImage(out_tiff_path, row_count, col_count, 2, gt, "", false, true);// 注意这里保留仿真结果

	image_tiff.saveImage(amp_data, 0, 0, 1);
	image_tiff.saveImage(arg_data, 0, 0, 2);

	return 0;
}

int WriteComplexData2AmpdB_Arg(QString out_tiff_path, Eigen::MatrixXcd data)
{
	Eigen::MatrixXd amp_data = Eigen::MatrixXd::Zero(data.rows(), data.cols());
	Eigen::MatrixXd arg_data = Eigen::MatrixXd::Zero(data.rows(), data.cols());
	size_t row_count = data.rows();
	size_t col_count = data.cols();
	for (int i = 0; i < row_count; i++) {
		for (int j = 0; j < col_count; j++) {
			amp_data(i, j) = 10.0*std::log10(std::abs(data(i, j)));
			arg_data(i, j) = std::arg(data(i, j));
		}
	}


	Eigen::MatrixXd gt = Eigen::MatrixXd::Zero(2, 3);

	gdalImage image_tiff = CreategdalImage(out_tiff_path, row_count, col_count, 2, gt, "", false, true);// 注意这里保留仿真结果

	image_tiff.saveImage(amp_data, 0, 0, 1);
	image_tiff.saveImage(arg_data, 0, 0, 2);

	return 0;
}

size_t nextpow2(size_t num)
{
	double n = std::round(std::log10(num * 1.0) / std::log10(2.0));
	return  pow(2, (size_t)std::ceil(n) + 1);
}


Eigen::MatrixXcd IFFTW1D(Eigen::MatrixXcd ECHOdata)
{
	size_t data_rows = ECHOdata.rows();
	size_t data_cols = ECHOdata.cols();
	// 频域 转换到 时域
	size_t n2 = nextpow2(data_cols) ;
	//qDebug() << data_rows << "," << data_cols << "," << n2 << "\n";
	// 补零
	Eigen::MatrixXcd ExpandECHO = Eigen::MatrixXcd::Zero(data_rows, n2);
	for (int i = 0; i < data_rows; i++) {
		for (int j = 0; j < data_cols; j++) {
			ExpandECHO(i, j) = ECHOdata(i, j);
		}
	}
	// 结果
	Eigen::MatrixXcd echoTime = Eigen::MatrixXcd::Zero(data_rows, n2);

	fftw_complex* din = (fftw_complex*)fftw_malloc(sizeof(double) * n2 * 2);
	fftw_complex* dout = (fftw_complex*)fftw_malloc(sizeof(double) * n2 * 2);
	fftw_plan p;
	double n = 1.0 / n2; // 因为 fftw 的逆傅里叶变换，没有除于N,所以实际上是原来的N倍
	//逐行进行fftw
	p = fftw_plan_dft_1d(n2, din, dout, FFTW_BACKWARD, FFTW_MEASURE);//FFTW_BACKWARD 逆，
	fftw_execute(p); //FFTW_MEASURE 估计最优变换方法，后面复用变换方案
	// 傅里叶 计算进度
	for (int i = 0; i < data_rows; i++) {
		for (int j = 0; j < n2; j++) { // 初始化输入
			din[j][0] = std::real(ExpandECHO(i, j));
			din[j][1] = std::imag(ExpandECHO(i, j));
		}
		// 构建fftw 任务
		fftw_execute(p);
		// 保存结果
		for (int j = 0; j < n2; j++) { //读取结果
			echoTime(i, j) = std::complex<double>(dout[j][0], dout[j][1]) * n; // 每次变换后都除于n
		}
		printf("\rIFFT[%.2lf%%]...:", i * 100.0 / (data_rows - 1));
	}
	fftw_destroy_plan(p);
	fftw_free(din);
	fftw_free(dout);
	qDebug() << "\n";
	qDebug() << "IFFT over" << "\n";
	return echoTime;
}

Eigen::MatrixXcd FFTW1D(Eigen::MatrixXcd ECHO)
{
	size_t data_rows = ECHO.rows();
	size_t data_cols = ECHO.cols();
	// 频域 转换到 时域
	size_t n2 = data_cols;
	// 补零
	Eigen::MatrixXcd ExpandECHO = Eigen::MatrixXcd::Zero(data_rows, n2);
	for (int i = 0; i < data_rows; i++) {
		for (int j = 0; j < data_cols; j++) {
			ExpandECHO(i, j) = ECHO(i, j);
		}
	}
	// 结果
	Eigen::MatrixXcd echofreq = Eigen::MatrixXcd::Zero(data_rows, n2);
	fftw_complex* din = (fftw_complex*)fftw_malloc(sizeof(double) * n2 * 2);
	fftw_complex* dout = (fftw_complex*)fftw_malloc(sizeof(double) * n2 * 2);
	fftw_plan p;
	//逐行进行fftw
	p = fftw_plan_dft_1d(n2, din, dout, FFTW_FORWARD, FFTW_MEASURE);//FFTW_FORWARD 顺，
	fftw_execute(p); //FFTW_MEASURE 估计最优变换方法，后面复用变换方案
	for (int i = 0; i < data_rows; i++) {
		for (int j = 0; j < n2; j++) { // 初始化输入
			din[j][0] = std::real(ExpandECHO(i, j));
			din[j][1] = std::imag(ExpandECHO(i, j));
		}
		// 构建fftw 任务
		fftw_execute(p);
		// 保存结果
		for (int j = 0; j < n2; j++) { //读取结果
			echofreq(i, j) = std::complex<double>(dout[j][0], dout[j][1]);
		}
		printf("\rFFT[%.2lf%%]...:", i * 100.0 / (data_rows - 1));
	}
	fftw_destroy_plan(p);
	fftw_free(din);
	fftw_free(dout);
	qDebug() << "\n";
	qDebug() << "FFT over" << "\n";
	return echofreq;
}



Eigen::MatrixXcd FFTW2D(Eigen::MatrixXcd ECHO)
{
	// 获取数据的尺寸
	int rows = ECHO.rows();
	int cols = ECHO.cols();

	// 创建FFTW输入和输出数组
	fftw_complex* in = reinterpret_cast<fftw_complex*>(ECHO.data());
	fftw_complex* out = static_cast<fftw_complex*>(fftw_malloc(sizeof(fftw_complex) * rows * cols));

	// 创建FFTW计划，执行傅立叶变换
	fftw_plan plan = fftw_plan_dft_2d(rows, cols, in, out, FFTW_FORWARD, FFTW_ESTIMATE);
	fftw_execute(plan);

	// 打印傅立叶变换结果
	Eigen::MatrixXcd result(rows, cols);
	result = Map<Eigen::MatrixXcd>(reinterpret_cast<std::complex<double>*>(out), rows, cols);
	//qDebug() << "Fourier Transform Result: " << "\n" << result << "\n";

	// 销毁FFTW计划和内存
	fftw_destroy_plan(plan);
	fftw_free(out);
	qDebug() << "\n";
	qDebug() << "FFT over" << "\n";
	return result;
}


Eigen::MatrixXcd fftshift(Eigen::MatrixXcd X) {
	int rows = X.rows();
	int cols = X.cols();
	int rows_half = rows / 2;
	int cols_half = cols / 2;

	Eigen::MatrixXcd tmp = X.topLeftCorner(rows_half, cols_half);
	X.topLeftCorner(rows_half, cols_half) = X.bottomRightCorner(rows_half, cols_half);
	X.bottomRightCorner(rows_half, cols_half) = tmp;

	tmp = X.topRightCorner(rows_half, cols_half);
	X.topRightCorner(rows_half, cols_half) = X.bottomLeftCorner(rows_half, cols_half);
	X.bottomLeftCorner(rows_half, cols_half) = tmp;

	return X;
}