#include "ImageNetOperator.h"
#include "LogInfoCls.h"
#include "PrintMsgToQDebug.h"
#include <QDebug>
#include "ImageOperatorBase.h"
#include "GPUBaseTool.h"
#include "GPUBPImageNet.cuh"
#include "BaseTool.h"
#include "BaseConstVariable.h"


void InitCreateImageXYZProcess(QString& outImageLLPath, QString& outImageXYZPath, QString& InEchoGPSDataPath,
	double& NearRange, double& RangeResolution, int64_t& RangeNum)
{
	qDebug() << "---------------------------------------------------------------------------------";
	qDebug() << u8"创建粗成像平面斜距投影网格";
	gdalImage antimg(InEchoGPSDataPath);
	qDebug() << u8"1. 回波GPS坐标点文件参数：\t";
	qDebug() << u8"文件路径：\t" << InEchoGPSDataPath;
	qDebug() << u8"GPS 点数：\t" << antimg.height;
	qDebug() << u8"文件列数：\t" << antimg.width;
	qDebug() << u8"2.斜距网格参数：";
	qDebug() << u8"近距离：\t" << NearRange;
	qDebug() << u8"分辨率：\t" << RangeResolution;
	qDebug() << u8"网格点数：\t" << RangeNum;
	qDebug() << u8"3.输出文件参数：";
	gdalImage outimgll = CreategdalImageDouble(outImageLLPath, antimg.height,RangeNum, 3,true,true);
	gdalImage outimgxyz = CreategdalImageDouble(outImageXYZPath, antimg.height, RangeNum, 3, true, true);
	qDebug() << u8"成像平面文件（经纬度）网格路径：\t" << outImageLLPath;
	qDebug() << u8"成像平面文件（XYZ）网格路径：\t" << outImageXYZPath;
	qDebug() << u8"4.开始创建成像网格XYZ";
	long prfcount = antimg.height;
	long rangeNum = RangeNum;
	double Rnear = NearRange;
	double dx = RangeResolution;
	
	long blockRangeCount = Memory1GB / sizeof(double) / 4 / prfcount *6;
	blockRangeCount = blockRangeCount < 1 ? 1 : blockRangeCount;

	std::shared_ptr<double>  Pxs((double*)mallocCUDAHost(sizeof(double) * prfcount), FreeCUDAHost);
	std::shared_ptr<double>  Pys((double*)mallocCUDAHost(sizeof(double) * prfcount), FreeCUDAHost);
	std::shared_ptr<double>  Pzs((double*)mallocCUDAHost(sizeof(double) * prfcount), FreeCUDAHost);
	std::shared_ptr<double>  AntDirectX((double*)mallocCUDAHost(sizeof(double) * prfcount), FreeCUDAHost);
	std::shared_ptr<double>  AntDirectY((double*)mallocCUDAHost(sizeof(double) * prfcount), FreeCUDAHost);
	std::shared_ptr<double>  AntDirectZ((double*)mallocCUDAHost(sizeof(double) * prfcount), FreeCUDAHost);

	{
		long colnum = 19;
		std::shared_ptr<double> antpos =readDataArr<double>(antimg,0,0,prfcount, colnum,1,GDALREADARRCOPYMETHOD::VARIABLEMETHOD);
		double time = 0;
		double Px = 0;
		double Py = 0;
		double Pz = 0;
		for (long i = 0; i < prfcount; i++) {

			Pxs.get()[i] = antpos.get()[i * 19 + 1]; // 卫星坐标
			Pys.get()[i] = antpos.get()[i * 19 + 2];
			Pzs.get()[i] = antpos.get()[i * 19 + 3];
			AntDirectX.get()[i] = antpos.get()[i * 19 + 13];// zero doppler
			AntDirectY.get()[i] = antpos.get()[i * 19 + 14];
			AntDirectZ.get()[i] = antpos.get()[i * 19 + 15];

			double NormAnt = std::sqrt(AntDirectX.get()[i] * AntDirectX.get()[i] +
				AntDirectY.get()[i] * AntDirectY.get()[i] +
				AntDirectZ.get()[i] * AntDirectZ.get()[i]);
			AntDirectX.get()[i] = AntDirectX.get()[i] / NormAnt;
			AntDirectY.get()[i] = AntDirectY.get()[i] / NormAnt;
			AntDirectZ.get()[i] = AntDirectZ.get()[i] / NormAnt;// 归一化
		}
		antpos.reset();
	}

	std::shared_ptr<double>  d_Pxs((double*)mallocCUDADevice(sizeof(double) * prfcount), FreeCUDADevice);
	std::shared_ptr<double>  d_Pys((double*)mallocCUDADevice(sizeof(double) * prfcount), FreeCUDADevice);
	std::shared_ptr<double>  d_Pzs((double*)mallocCUDADevice(sizeof(double) * prfcount), FreeCUDADevice);
	std::shared_ptr<double>  d_AntDirectX((double*)mallocCUDADevice(sizeof(double) * prfcount), FreeCUDADevice);
	std::shared_ptr<double>  d_AntDirectY((double*)mallocCUDADevice(sizeof(double) * prfcount), FreeCUDADevice);
	std::shared_ptr<double>  d_AntDirectZ((double*)mallocCUDADevice(sizeof(double) * prfcount), FreeCUDADevice);

	HostToDevice(Pxs.get(), d_Pxs.get(), sizeof(double) * prfcount);
	HostToDevice(Pys.get(), d_Pys.get(), sizeof(double) * prfcount);
	HostToDevice(Pzs.get(), d_Pzs.get(), sizeof(double) * prfcount);
	HostToDevice(AntDirectX.get(), d_AntDirectX.get(), sizeof(double) * prfcount);
	HostToDevice(AntDirectY.get(), d_AntDirectY.get(), sizeof(double) * prfcount);
	HostToDevice(AntDirectZ.get(), d_AntDirectZ.get(), sizeof(double) * prfcount);


	for (long startcolidx = 0; startcolidx < RangeNum; startcolidx = startcolidx + blockRangeCount) {

		long tempechocol = blockRangeCount;
		if (startcolidx + tempechocol >= RangeNum) {
			tempechocol = RangeNum - startcolidx;
		}
		qDebug() << " imgxyz :\t" << startcolidx << "\t-\t" << startcolidx + tempechocol << " / " << RangeNum;

		std::shared_ptr<double> demx = readDataArr<double>(outimgxyz, 0, startcolidx, prfcount, tempechocol, 1, GDALREADARRCOPYMETHOD::VARIABLEMETHOD);
		std::shared_ptr<double> demy = readDataArr<double>(outimgxyz, 0, startcolidx, prfcount, tempechocol, 2, GDALREADARRCOPYMETHOD::VARIABLEMETHOD);
		std::shared_ptr<double> demz = readDataArr<double>(outimgxyz, 0, startcolidx, prfcount, tempechocol, 3, GDALREADARRCOPYMETHOD::VARIABLEMETHOD);

		std::shared_ptr<double> h_demx((double*)mallocCUDAHost(sizeof(double) * prfcount * tempechocol), FreeCUDAHost);
		std::shared_ptr<double> h_demy((double*)mallocCUDAHost(sizeof(double) * prfcount * tempechocol), FreeCUDAHost);
		std::shared_ptr<double> h_demz((double*)mallocCUDAHost(sizeof(double) * prfcount * tempechocol), FreeCUDAHost);

#pragma omp parallel for 
		for (long ii = 0; ii < prfcount; ii++) {
			for (long jj = 0; jj < tempechocol; jj++) {
				h_demx.get()[ii * tempechocol + jj] = demx.get()[ii * tempechocol + jj];
				h_demy.get()[ii * tempechocol + jj] = demy.get()[ii * tempechocol + jj];
				h_demz.get()[ii * tempechocol + jj] = demz.get()[ii * tempechocol + jj];
			}
		}

		std::shared_ptr<double> d_demx((double*)mallocCUDADevice(sizeof(double) * prfcount * tempechocol), FreeCUDADevice);
		std::shared_ptr<double> d_demy((double*)mallocCUDADevice(sizeof(double) * prfcount * tempechocol), FreeCUDADevice);
		std::shared_ptr<double> d_demz((double*)mallocCUDADevice(sizeof(double) * prfcount * tempechocol), FreeCUDADevice);

		HostToDevice(h_demx.get(), d_demx.get(), sizeof(double) * prfcount * tempechocol);
		HostToDevice(h_demy.get(), d_demy.get(), sizeof(double) * prfcount * tempechocol);
		HostToDevice(h_demz.get(), d_demz.get(), sizeof(double) * prfcount * tempechocol);


		TIMEBPCreateImageGrid(
			d_Pxs.get(), d_Pys.get(), d_Pzs.get(),
			d_AntDirectX.get(), d_AntDirectY.get(), d_AntDirectZ.get(),
			d_demx.get(), d_demy.get(), d_demz.get(),
			prfcount, tempechocol, 0,
			Rnear + dx * startcolidx, dx // 更新最近修读
		);

		DeviceToHost(h_demx.get(), d_demx.get(), sizeof(double) * prfcount * tempechocol);
		DeviceToHost(h_demy.get(), d_demy.get(), sizeof(double) * prfcount * tempechocol);
		DeviceToHost(h_demz.get(), d_demz.get(), sizeof(double) * prfcount * tempechocol);

#pragma omp parallel for 
		for (long ii = 0; ii < prfcount; ii++) {
			for (long jj = 0; jj < tempechocol; jj++) {
				demx.get()[ii * tempechocol + jj] = h_demx.get()[ii * tempechocol + jj];
				demy.get()[ii * tempechocol + jj] = h_demy.get()[ii * tempechocol + jj];
				demz.get()[ii * tempechocol + jj] = h_demz.get()[ii * tempechocol + jj];
			}
		}

		outimgxyz.saveImage(demx, 0, startcolidx, prfcount, tempechocol, 1);
		outimgxyz.saveImage(demy, 0, startcolidx, prfcount, tempechocol, 2);
		outimgxyz.saveImage(demz, 0, startcolidx, prfcount, tempechocol, 3);

		// 将XYZ转换为经纬度
		std::shared_ptr<double> demllx = readDataArr<double>(outimgll, 0, startcolidx, prfcount, tempechocol, 1, GDALREADARRCOPYMETHOD::VARIABLEMETHOD);
		std::shared_ptr<double> demlly = readDataArr<double>(outimgll, 0, startcolidx, prfcount, tempechocol, 2, GDALREADARRCOPYMETHOD::VARIABLEMETHOD);
		std::shared_ptr<double> demllz = readDataArr<double>(outimgll, 0, startcolidx, prfcount, tempechocol, 3, GDALREADARRCOPYMETHOD::VARIABLEMETHOD);

#pragma omp parallel for 
		for (long ii = 0; ii < prfcount; ii++) {
			for (long jj = 0; jj < tempechocol; jj++) {
				double x = demx.get()[ii * tempechocol + jj];
				double y = demy.get()[ii * tempechocol + jj];
				double z = demz.get()[ii * tempechocol + jj];
				Landpoint point;
				XYZ2BLH_FixedHeight(x, y, z, 0, point);
				demllx.get()[ii * tempechocol + jj] = point.lon;
				demlly.get()[ii * tempechocol + jj] = point.lat;
				demllz.get()[ii * tempechocol + jj] = point.ati;
			}
		}


		outimgll.saveImage(demllx, 0, startcolidx, prfcount, tempechocol, 1);
		outimgll.saveImage(demlly, 0, startcolidx, prfcount, tempechocol, 2);
		outimgll.saveImage(demllz, 0, startcolidx, prfcount, tempechocol, 3);

	}

	qDebug() << u8"6.保存成像网格结果";
	qDebug() << "---------------------------------------------------------------------------------";
}

bool OverlapCheck(QString& ImageLLPath, QString& ImageDEMPath)
{
	// 检查DEM是否是WGS84坐标系
	//long demEPSG = GetEPSGFromRasterFile(ImageDEMPath);
	//if (demEPSG != 4326) {
	//	qDebug() << u8"DEM坐标系不是WGS84坐标系,ESPG:"<< demEPSG;
	//	return false;
	//}

	gdalImage demimg(ImageDEMPath);
	gdalImage imgll(ImageLLPath);

	long imgheight = imgll.height;
	long imgwidth = imgll.width;
	Eigen::MatrixXd imglonArr = imgll.getData(0, 0, imgheight, imgwidth, 1);
	Eigen::MatrixXd imglatArr = imgll.getData(0, 0, imgheight, imgwidth, 2);

	// 打印范围
	qDebug() << u8"影像范围：";
	qDebug() << u8"最小经度：\t" << imglonArr.minCoeff();
	qDebug() << u8"最大经度：\t" << imglonArr.maxCoeff();
	qDebug() << u8"最小纬度：\t" << imglatArr.minCoeff();
	qDebug() << u8"最大纬度：\t" << imglatArr.maxCoeff();
	qDebug() << u8"DEM范围：";
	RasterExtend demextend = demimg.getExtend();
	qDebug() << u8"最小经度：\t" << demextend.min_x;
	qDebug() << u8"最大经度：\t" << demextend.max_x;
	qDebug() << u8"最小纬度：\t" << demextend.min_y;
	qDebug() << u8"最大纬度：\t" << demextend.max_y;
	qDebug() << u8"影像大小：\t" << demimg.height << " * " << demimg.width;



	for (long i = 0; i < imgheight; i++)
	{
		for (long j = 0; j < imgwidth; j++)
		{
			double lon = imglonArr(i, j); // X
			double lat = imglatArr(i, j); // Y
			Landpoint point = demimg.getRow_Col(lon, lat);
			imglonArr(i, j) = point.lon;
			imglatArr(i, j) = point.lat;
		}
	}

	double minX = imglonArr.minCoeff();
	double maxX = imglonArr.maxCoeff();
	double minY = imglatArr.minCoeff();
	double maxY = imglatArr.maxCoeff();

	//打印范围
	qDebug() << u8"dem 的范围：";
	qDebug() << u8"minX:"<<minX<<"\t"<<demimg.width;
	qDebug() << u8"maxX:"<<maxX << "\t" << demimg.width;
	qDebug() << u8"minY:"<<minY << "\t" << demimg.height;
	qDebug() << u8"maxY:"<<maxY << "\t" << demimg.height;
	qDebug() << u8"图像行列：\t" << demimg.height << " , " << demimg.width;

	if (minX<1 || maxX>demimg.width - 1 || minY<1 || maxY>demimg.height - 1) {
		return false;
	}
	else {
		return true;
	}



}

bool GPSPointsNumberEqualCheck(QString& ImageLLPath, QString& InEchoGPSDataPath)
{

	gdalImage antimg(InEchoGPSDataPath);
	gdalImage imgll(ImageLLPath);
	return antimg.height == imgll.height;
 
}



void InterploateAtiByRefDEM(QString& ImageLLPath, QString& ImageDEMPath, QString& outImageLLAPath, QString& InEchoGPSDataPath)
{
	gdalImage demimg(ImageDEMPath);
	gdalImage imgll(ImageLLPath);
	gdalImage outimgll = CreategdalImageDouble(outImageLLAPath, imgll.height, imgll.width, 4, true, true); // 经度、纬度、高程、斜距

	long imgheight = imgll.height;
	long imgwidth = imgll.width;

	Eigen::MatrixXd imglonArr = imgll.getData(0, 0, imgheight, imgwidth, 1);
	Eigen::MatrixXd imglatArr = imgll.getData(0, 0, imgheight, imgwidth, 2);
	Eigen::MatrixXd demArr = demimg.getData(0, 0, demimg.height, demimg.width, 1);
	Eigen::MatrixXd imgatiArr = Eigen::MatrixXd::Zero(imgheight, imgwidth);
	Eigen::MatrixXd imgRArr = Eigen::MatrixXd::Zero(imgheight, imgwidth);

	outimgll.saveImage(imglonArr, 0, 0, 1);
	outimgll.saveImage(imglatArr, 0, 0, 2);


	double minX = imglonArr.minCoeff();
	double maxX = imglonArr.maxCoeff();
	double minY = imglatArr.minCoeff();
	double maxY = imglatArr.maxCoeff();
	//打印范围
	qDebug() << u8"dem 的范围：";
	qDebug() << u8"minX:" << minX << "\t" << demimg.width;
	qDebug() << u8"maxX:" << maxX << "\t" << demimg.width;
	qDebug() << u8"minY:" << minY << "\t" << demimg.height;
	qDebug() << u8"maxY:" << maxY << "\t" << demimg.height;
	qDebug() << u8"图像行列：\t" << demimg.height << " , " << demimg.width;


	for (long i = 0; i < imgheight; i++) {
		//printf("\rprocess:%f precent\t\t\t",i*100.0/imgheight);
		for (long j = 0; j < imgwidth; j++) {
			double lon = imglonArr(i, j);
			double lat = imglatArr(i, j);
			Landpoint point = demimg.getRow_Col(lon, lat);

			if (point.lon<1 || point.lon>demimg.width - 2 || point.lat < 1 || point.lat - 2) {
				continue;
			}
			else {}

			Landpoint p0, p11, p21, p12, p22;

			p0.lon = point.lon;
			p0.lat = point.lat;

			p11.lon = floor(p0.lon);
			p11.lat = floor(p0.lat);
			p11.ati = demArr(long(p11.lat), long(p11.lon));

			p12.lon = ceil(p0.lon);
			p12.lat = floor(p0.lat);
			p12.ati = demArr(long(p12.lat), long(p12.lon));

			p21.lon = floor(p0.lon);
			p21.lat = ceil(p0.lat);
			p21.ati = demArr(long(p21.lat), long(p21.lon));

			p22.lon = ceil(p0.lon);
			p22.lat = ceil(p0.lat);
			p22.ati = demArr(long(p22.lat), long(p22.lon));

			p0.lon = p0.lon - p11.lon;
			p0.lat = p0.lat - p11.lat;

			p12.lon = p12.lon - p11.lon;
			p12.lat = p12.lat - p11.lat;

			p21.lon = p21.lon - p11.lon;
			p21.lat = p21.lat - p11.lat;

			p22.lon = p22.lon - p11.lon;
			p22.lat = p22.lat - p11.lat;

			p11.lon = p11.lon - p11.lon;
			p11.lat = p11.lat - p11.lat;

			p0.ati=Bilinear_interpolation(p0, p11, p21, p12, p22);
			imgatiArr(i, j) = p0.ati;

		}
	}
	outimgll.saveImage(imgatiArr, 0, 0, 3);
	qDebug() << u8"计算每个点的斜距值";


	gdalImage antimg(InEchoGPSDataPath);
	qDebug() << u8"1. 回波GPS坐标点文件参数：\t";
	qDebug() << u8"文件路径：\t" << InEchoGPSDataPath;
	qDebug() << u8"GPS 点数：\t" << antimg.height;
	qDebug() << u8"文件列数：\t" << antimg.width;
	
	long prfcount = antimg.height;


	std::shared_ptr<double>  Pxs((double*)mallocCUDAHost(sizeof(double) * prfcount), FreeCUDAHost);
	std::shared_ptr<double>  Pys((double*)mallocCUDAHost(sizeof(double) * prfcount), FreeCUDAHost);
	std::shared_ptr<double>  Pzs((double*)mallocCUDAHost(sizeof(double) * prfcount), FreeCUDAHost);
	std::shared_ptr<double>  AntDirectX((double*)mallocCUDAHost(sizeof(double) * prfcount), FreeCUDAHost);
	std::shared_ptr<double>  AntDirectY((double*)mallocCUDAHost(sizeof(double) * prfcount), FreeCUDAHost);
	std::shared_ptr<double>  AntDirectZ((double*)mallocCUDAHost(sizeof(double) * prfcount), FreeCUDAHost);

	{
		long colnum = 19;
		std::shared_ptr<double> antpos = readDataArr<double>(antimg, 0, 0, prfcount, colnum, 1, GDALREADARRCOPYMETHOD::VARIABLEMETHOD);
		double time = 0;
		double Px = 0;
		double Py = 0;
		double Pz = 0;
		for (long i = 0; i < prfcount; i++) {

			Pxs.get()[i] = antpos.get()[i * 19 + 1]; // 卫星坐标
			Pys.get()[i] = antpos.get()[i * 19 + 2];
			Pzs.get()[i] = antpos.get()[i * 19 + 3];
			AntDirectX.get()[i] = antpos.get()[i * 19 + 13];// zero doppler
			AntDirectY.get()[i] = antpos.get()[i * 19 + 14];
			AntDirectZ.get()[i] = antpos.get()[i * 19 + 15];

			double NormAnt = std::sqrt(AntDirectX.get()[i] * AntDirectX.get()[i] +
				AntDirectY.get()[i] * AntDirectY.get()[i] +
				AntDirectZ.get()[i] * AntDirectZ.get()[i]);
			AntDirectX.get()[i] = AntDirectX.get()[i] / NormAnt;
			AntDirectY.get()[i] = AntDirectY.get()[i] / NormAnt;
			AntDirectZ.get()[i] = AntDirectZ.get()[i] / NormAnt;// 归一化
		}
		antpos.reset();
	}



#pragma omp parallel for 
	for (long prfid = 0; prfid < prfcount; prfid++) {
		double Px = Pxs.get()[prfid];
		double Py = Pys.get()[prfid];
		double Pz = Pzs.get()[prfid];
		double R = 0;
		Landpoint LLA = {};
		Point3 XYZ = {};
		for (long j = 0; j < imgwidth; j++) {
			LLA.lon = imglonArr(prfid, j);
			LLA.lat = imglatArr(prfid, j);
			LLA.ati = imgatiArr(prfid, j);

			LLA2XYZ(LLA, XYZ);

			R = sqrt(pow(Px - XYZ.x, 2) +
				pow(Py - XYZ.y, 2) +
				pow(Pz - XYZ.z, 2));
			imgRArr(prfid, j) = R;
		}
	}

	outimgll.saveImage(imgRArr, 0, 0, 4);

	qDebug() << u8"插值完成";
}


void InterploateClipAtiByRefDEM(QString ImageLLPath, QString& ImageDEMPath, QString& outImageLLAPath, QString& InEchoGPSDataPath) {

	gdalImage demimg(ImageDEMPath);
	gdalImage imgll(ImageLLPath);

	// 裁剪
	long imgheight = imgll.height;
	long imgwidth = imgll.width;

	long minRow = -1;
	long maxRow = imgheight;
	long minCol = -1;
	long maxCol = imgwidth;


	Eigen::MatrixXd imglonArr = imgll.getData(0, 0, imgheight, imgwidth, 1);
	Eigen::MatrixXd imglatArr = imgll.getData(0, 0, imgheight, imgwidth, 2);

#pragma omp parallel for 
	for (long i = 0; i < imgheight; i++) {
		for (long j = 0; j < imgwidth; j++) {
			double lon = imglonArr(i, j);
			double lat = imglatArr(i, j);
			Landpoint point = demimg.getRow_Col(lon, lat);
			imglonArr(i, j) = point.lon; 
			imglatArr(i, j) = point.lat;
		}
	}

	// 开始逐行扫描

	bool minRowFlag=true, maxRowFlag= true, minColFlag = true, maxColFlag = true;

	for (long i = 0; i < imgheight; i++) {
		for (long j = 0; j < imgwidth; j++) {
			if (imglonArr(i, j) > 0 && minRowFlag) {
				minRowFlag = false;
				minRow = i;
				break;
			}
			if (imglonArr(i, j) < imgheight) {
				maxRow = i;
			}
		}
	}

	for (long j = 0; j < imgwidth; j++) {
		for (long i = 0; i < imgheight; i++) {
			if (imglatArr(i, j) > 0 && minColFlag) {
				minColFlag = false;
				minCol = j;
				break;
			}
			if (imglatArr(i, j) < imgheight) {
				maxCol = j;
			}
		}
	}


	long RowCount = maxRow - minRow;
	long ColCount = maxCol - minCol;

	gdalImage outimgll = CreategdalImageDouble(outImageLLAPath, RowCount, ColCount, 4, true, true); // 经度、纬度、高程、斜距

	imgheight = outimgll.height;
	imgwidth = outimgll.width;

	imglonArr = imgll.getData(minRow, minCol, RowCount, ColCount, 1);
	imglatArr = imgll.getData(minRow, minCol, RowCount, ColCount, 2);

	Eigen::MatrixXd demArr = demimg.getData(0, 0, demimg.height, demimg.width, 1);

	Eigen::MatrixXd imgatiArr = Eigen::MatrixXd::Zero(imgheight, imgwidth);
	Eigen::MatrixXd imgRArr = Eigen::MatrixXd::Zero(imgheight, imgwidth);

	outimgll.saveImage(imglonArr, 0, 0, 1);
	outimgll.saveImage(imglatArr, 0, 0, 2);


	double minX = imglonArr.minCoeff();
	double maxX = imglonArr.maxCoeff();
	double minY = imglatArr.minCoeff();
	double maxY = imglatArr.maxCoeff();

	//打印范围
	qDebug() << u8"dem 的范围：";
	qDebug() << u8"minX:" << minX << "\t" << demimg.width;
	qDebug() << u8"maxX:" << maxX << "\t" << demimg.width;
	qDebug() << u8"minY:" << minY << "\t" << demimg.height;
	qDebug() << u8"maxY:" << maxY << "\t" << demimg.height;
	qDebug() << u8"图像行列：\t" << demimg.height << " , " << demimg.width;


	for (long i = 0; i < imgheight; i++) {
		//printf("\rprocess:%f precent\t\t\t",i*100.0/imgheight);
		for (long j = 0; j < imgwidth; j++) {
			double lon = imglonArr(i, j);
			double lat = imglatArr(i, j);
			Landpoint point = demimg.getRow_Col(lon, lat);

			if (point.lon<1 || point.lon>demimg.width - 2 || point.lat < 1 || point.lat - 2) {
				continue;
			}
			else {}

			Landpoint p0, p11, p21, p12, p22;

			p0.lon = point.lon;
			p0.lat = point.lat;

			p11.lon = floor(p0.lon);
			p11.lat = floor(p0.lat);
			p11.ati = demArr(long(p11.lat), long(p11.lon));

			p12.lon = ceil(p0.lon);
			p12.lat = floor(p0.lat);
			p12.ati = demArr(long(p12.lat), long(p12.lon));

			p21.lon = floor(p0.lon);
			p21.lat = ceil(p0.lat);
			p21.ati = demArr(long(p21.lat), long(p21.lon));

			p22.lon = ceil(p0.lon);
			p22.lat = ceil(p0.lat);
			p22.ati = demArr(long(p22.lat), long(p22.lon));

			p0.lon = p0.lon - p11.lon;
			p0.lat = p0.lat - p11.lat;

			p12.lon = p12.lon - p11.lon;
			p12.lat = p12.lat - p11.lat;

			p21.lon = p21.lon - p11.lon;
			p21.lat = p21.lat - p11.lat;

			p22.lon = p22.lon - p11.lon;
			p22.lat = p22.lat - p11.lat;

			p11.lon = p11.lon - p11.lon;
			p11.lat = p11.lat - p11.lat;

			p0.ati = Bilinear_interpolation(p0, p11, p21, p12, p22);
			imgatiArr(i, j) = p0.ati;
		}
	}

	outimgll.saveImage(imgatiArr, 0, 0, 3);
	qDebug() << u8"计算每个点的斜距值";


	gdalImage antimg(InEchoGPSDataPath);
	qDebug() << u8"1. 回波GPS坐标点文件参数：\t";
	qDebug() << u8"文件路径：\t" << InEchoGPSDataPath;
	qDebug() << u8"GPS 点数：\t" << antimg.height;
	qDebug() << u8"文件列数：\t" << antimg.width;

	long prfcount = antimg.height;


	std::shared_ptr<double>  Pxs((double*)mallocCUDAHost(sizeof(double) * prfcount), FreeCUDAHost);
	std::shared_ptr<double>  Pys((double*)mallocCUDAHost(sizeof(double) * prfcount), FreeCUDAHost);
	std::shared_ptr<double>  Pzs((double*)mallocCUDAHost(sizeof(double) * prfcount), FreeCUDAHost);
	std::shared_ptr<double>  AntDirectX((double*)mallocCUDAHost(sizeof(double) * prfcount), FreeCUDAHost);
	std::shared_ptr<double>  AntDirectY((double*)mallocCUDAHost(sizeof(double) * prfcount), FreeCUDAHost);
	std::shared_ptr<double>  AntDirectZ((double*)mallocCUDAHost(sizeof(double) * prfcount), FreeCUDAHost);

	{
		long colnum = 19;
		std::shared_ptr<double> antpos = readDataArr<double>(antimg, 0, 0, prfcount, colnum, 1, GDALREADARRCOPYMETHOD::VARIABLEMETHOD);
		double time = 0;
		double Px = 0;
		double Py = 0;
		double Pz = 0;
		for (long i = 0; i < prfcount; i++) {

			Pxs.get()[i] = antpos.get()[i * 19 + 1]; // 卫星坐标
			Pys.get()[i] = antpos.get()[i * 19 + 2];
			Pzs.get()[i] = antpos.get()[i * 19 + 3];
			AntDirectX.get()[i] = antpos.get()[i * 19 + 13];// zero doppler
			AntDirectY.get()[i] = antpos.get()[i * 19 + 14];
			AntDirectZ.get()[i] = antpos.get()[i * 19 + 15];

			double NormAnt = std::sqrt(AntDirectX.get()[i] * AntDirectX.get()[i] +
				AntDirectY.get()[i] * AntDirectY.get()[i] +
				AntDirectZ.get()[i] * AntDirectZ.get()[i]);
			AntDirectX.get()[i] = AntDirectX.get()[i] / NormAnt;
			AntDirectY.get()[i] = AntDirectY.get()[i] / NormAnt;
			AntDirectZ.get()[i] = AntDirectZ.get()[i] / NormAnt;// 归一化
		}
		antpos.reset();
	}



#pragma omp parallel for 
	for (long prfid = minRow; prfid < maxRow; prfid++) {
		double Px = Pxs.get()[prfid];
		double Py = Pys.get()[prfid];
		double Pz = Pzs.get()[prfid];
		double R = 0;
		Landpoint LLA = {};
		Point3 XYZ = {};
		for (long j = 0; j < imgwidth; j++) {
			LLA.lon = imglonArr(prfid-minRow, j);
			LLA.lat = imglatArr(prfid - minRow, j);
			LLA.ati = imgatiArr(prfid - minRow, j);

			LLA2XYZ(LLA, XYZ);

			R = sqrt(pow(Px - XYZ.x, 2) +
				pow(Py - XYZ.y, 2) +
				pow(Pz - XYZ.z, 2));
			imgRArr(prfid - minRow, j) = R;
		}
	}

	outimgll.saveImage(imgRArr, 0, 0, 4);

	qDebug() << u8"插值完成";

}




int  ReflectTable_WGS2Range(QString dem_rc_path,QString outOriSimTiffPath,QString ori_sim_count_tiffPath,long OriHeight,long OriWidth)
{
	gdalImage sim_rc(dem_rc_path);
	gdalImage sim_sar_img = CreategdalImage(outOriSimTiffPath, OriHeight, OriWidth, 2, sim_rc.gt, sim_rc.projection, false);// 注意这里保留仿真结果
	for (int max_rows_ids = 0; max_rows_ids < OriHeight; max_rows_ids = max_rows_ids + 1000) {
		Eigen::MatrixXd sim_sar = sim_sar_img.getData(max_rows_ids, 0, 1000, OriWidth, 1);
		Eigen::MatrixXd sim_sarc = sim_sar_img.getData(max_rows_ids, 0, 1000, OriWidth, 2);
		sim_sar = sim_sar.array() * 0 - 9999;
		sim_sarc = sim_sar.array() * 0 - 9999;
		sim_sar_img.saveImage(sim_sar, max_rows_ids, 0, 1);
		sim_sar_img.saveImage(sim_sarc, max_rows_ids, 0, 2);
	}
	sim_sar_img.setNoDataValue(-9999, 1);
	sim_sar_img.setNoDataValue(-9999, 2);
	int conver_lines = 5000;
	int line_invert = 4000;// 计算重叠率
	int line_offset = 60;
	// 逐区域迭代计算
	omp_lock_t lock;
	omp_init_lock(&lock); // 初始化互斥锁
	int allCount = 0;

	for (int max_rows_ids = 0; max_rows_ids < sim_rc.height; max_rows_ids = max_rows_ids + line_invert) {
		Eigen::MatrixXd dem_r = sim_rc.getData(max_rows_ids, 0, conver_lines, sim_rc.width, 1);
		Eigen::MatrixXd dem_c = sim_rc.getData(max_rows_ids, 0, conver_lines, sim_rc.width, 2);
		int dem_rows_num = dem_r.rows();
		int dem_cols_num = dem_r.cols();
		// 更新插值经纬度
		//Eigen::MatrixXd dem_lon = dem_r;
		//Eigen::MatrixXd dem_lat = dem_c;
		// 构建索引 更新经纬度并更新链

		int temp_r, temp_c;

		int min_row = dem_r.minCoeff() + 1;
		int max_row = dem_r.maxCoeff() + 1;

		if (max_row < 0) {
			continue;
		}

		int len_rows = max_row - min_row;
		min_row = min_row < 0 ? 0 : min_row;
		Eigen::MatrixXd sar_r = sim_sar_img.getData(min_row, 0, len_rows, OriWidth, 1);
		Eigen::MatrixXd sar_c = sim_sar_img.getData(min_row, 0, len_rows, OriWidth, 2);
		len_rows = sar_r.rows();


#pragma omp parallel for num_threads(8) // NEW ADD
		for (int i = 0; i < dem_rows_num - 1; i++) {
			for (int j = 0; j < dem_cols_num - 1; j++) {
				Point3 p, p1, p2, p3, p4;
				Landpoint lp1, lp2, lp3, lp4;
				lp1 = sim_rc.getLandPoint(i + max_rows_ids, j, 0);
				lp2 = sim_rc.getLandPoint(i + max_rows_ids, j + 1, 0);
				lp3 = sim_rc.getLandPoint(i + 1 + max_rows_ids, j + 1, 0);
				lp4 = sim_rc.getLandPoint(i + 1 + max_rows_ids, j, 0);

				p1 = { dem_r(i,j),dem_c(i,j) };
				p2 = { dem_r(i,j + 1),dem_c(i,j + 1) };
				p3 = { dem_r(i + 1,j + 1),dem_c(i + 1,j + 1) };
				p4 = { dem_r(i + 1,j),dem_c(i + 1,j) };

				//if (angle(i, j) >= 90 && angle(i, j + 1) >= 90 && angle(i + 1, j) >= 90 && angle(i + 1, j + 1) >= 90) {
				//	continue;
				//}

				double temp_min_r = dem_r.block(i, j, 2, 2).minCoeff();
				double temp_max_r = dem_r.block(i, j, 2, 2).maxCoeff();
				double temp_min_c = dem_c.block(i, j, 2, 2).minCoeff();
				double temp_max_c = dem_c.block(i, j, 2, 2).maxCoeff();
				if ((int(temp_min_r) != int(temp_max_r)) && (int(temp_min_c) != int(temp_max_c))) {
					for (int ii = int(temp_min_r); ii <= temp_max_r + 1; ii++) {
						for (int jj = int(temp_min_c); jj < temp_max_c + 1; jj++) {
							if (ii < min_row || ii - min_row >= len_rows || jj < 0 || jj >= OriWidth) {
								continue;
							}
							p = { double(ii),double(jj),0 };
							//if (PtInRect(p, p1, p2, p3, p4)) {
							p1.z = lp1.lon;
							p2.z = lp2.lon;
							p3.z = lp3.lon;
							p4.z = lp4.lon;

							p = invBilinear(p, p1, p2, p3, p4);
							if (isnan(p.z)) {
								continue;
							}

							if (p.x < 0) {
								continue;
							}
							double mean = (p1.z + p2.z + p3.z + p4.z) / 4;
							if (p.z > p1.z && p.z > p2.z && p.z > p3.z && p.z > p4.z) {
								p.z = mean;
							}
							if (p.z < p1.z && p.z < p2.z && p.z < p3.z && p.z < p4.z) {
								p.z = mean;
							}
							sar_r(ii - min_row, jj) = p.z;
							p1.z = lp1.lat;
							p2.z = lp2.lat;
							p3.z = lp3.lat;
							p4.z = lp4.lat;
							p = invBilinear(p, p1, p2, p3, p4);
							if (isnan(p.z)) {
								continue;
							}

							if (p.x < 0) {
								continue;
							}
							mean = (p1.z + p2.z + p3.z + p4.z) / 4;
							if (p.z > p1.z && p.z > p2.z && p.z > p3.z && p.z > p4.z) {
								p.z = mean;
							}
							if (p.z < p1.z && p.z < p2.z && p.z < p3.z && p.z < p4.z) {
								p.z = mean;
							}
							sar_c(ii - min_row, jj) = p.z;
							//}
						}
					}

				}
			}
		}

		omp_set_lock(&lock); //获得互斥器
		sim_sar_img.saveImage(sar_r, min_row, 0, 1);
		sim_sar_img.saveImage(sar_c, min_row, 0, 2);
		allCount = allCount + conver_lines;
		qDebug() << "rows:\t" << allCount << "/" << sim_rc.height << "\t computing.....\t" ;
		omp_unset_lock(&lock); //释放互斥器


	}

	return 0;
}



int ResampleEChoDataFromGeoEcho(QString L2echodataPath, QString RangeLooktablePath, QString L1AEchoDataPath) {
	gdalImageComplex echodata(L2echodataPath);
	gdalImage looktable(RangeLooktablePath);
	gdalImageComplex l1adata = CreategdalImageComplexNoProj(L1AEchoDataPath, looktable.height, looktable.width, 1, true);
	Eigen::MatrixXcd echoArr = echodata.getDataComplex(0, 0, echodata.height, echodata.width, 1);

	long blockHeight = Memory1GB / looktable.width / 8 * 2;

	for (long startRow = 0; startRow < looktable.height; startRow = startRow + blockHeight) {
		printf("\rGEC: process:%f precent\t\t\t", startRow * 100.0 / looktable.height);
		blockHeight = blockHeight + startRow < looktable.height ? blockHeight : looktable.height - startRow;
		Eigen::MatrixXd imglonArr = looktable.getData(startRow, 0, blockHeight, looktable.width, 1);
		Eigen::MatrixXd imglatArr = looktable.getData(startRow, 0, blockHeight, looktable.width, 2);
		Eigen::MatrixXcd l1aArr = l1adata.getDataComplex(0, 0, blockHeight, l1adata.width, 1);
		l1aArr = l1aArr.array() * 0;

		long imgheight = blockHeight;
		long imgwidth = looktable.width;
		for (long i = 0; i < imgheight; i++) {
			
			for (long j = 0; j < imgwidth; j++) {
				double lon = imglonArr(i, j);
				double lat = imglatArr(i, j);
				Landpoint point = echodata.getRow_Col(lon, lat);

				if (point.lon<1 || point.lon>echodata.width - 2 || point.lat < 1 || point.lat >echodata.height - 2) {
					continue;
				}
				else {}
				// 实部插值
				{
					Landpoint p0, p11, p21, p12, p22;

					p0.lon = point.lon;
					p0.lat = point.lat;

					p11.lon = floor(p0.lon);
					p11.lat = floor(p0.lat);
					p11.ati = echoArr(long(p11.lat), long(p11.lon)).real();

					p12.lon = ceil(p0.lon);
					p12.lat = floor(p0.lat);
					p12.ati = echoArr(long(p12.lat), long(p12.lon)).real();

					p21.lon = floor(p0.lon);
					p21.lat = ceil(p0.lat);
					p21.ati = echoArr(long(p21.lat), long(p21.lon)).real();

					p22.lon = ceil(p0.lon);
					p22.lat = ceil(p0.lat);
					p22.ati = echoArr(long(p22.lat), long(p22.lon)).real();

					p0.lon = p0.lon - p11.lon;
					p0.lat = p0.lat - p11.lat;

					p12.lon = p12.lon - p11.lon;
					p12.lat = p12.lat - p11.lat;

					p21.lon = p21.lon - p11.lon;
					p21.lat = p21.lat - p11.lat;

					p22.lon = p22.lon - p11.lon;
					p22.lat = p22.lat - p11.lat;

					p11.lon = p11.lon - p11.lon;
					p11.lat = p11.lat - p11.lat;

					p0.ati = Bilinear_interpolation(p0, p11, p21, p12, p22);
					l1aArr(i, j).real(p0.ati);
				}
				//虚部插值
				{
					Landpoint p0, p11, p21, p12, p22;

					p0.lon = point.lon;
					p0.lat = point.lat;

					p11.lon = floor(p0.lon);
					p11.lat = floor(p0.lat);
					p11.ati = echoArr(long(p11.lat), long(p11.lon)).imag();

					p12.lon = ceil(p0.lon);
					p12.lat = floor(p0.lat);
					p12.ati = echoArr(long(p12.lat), long(p12.lon)).imag();

					p21.lon = floor(p0.lon);
					p21.lat = ceil(p0.lat);
					p21.ati = echoArr(long(p21.lat), long(p21.lon)).imag();

					p22.lon = ceil(p0.lon);
					p22.lat = ceil(p0.lat);
					p22.ati = echoArr(long(p22.lat), long(p22.lon)).imag();

					p0.lon = p0.lon - p11.lon;
					p0.lat = p0.lat - p11.lat;

					p12.lon = p12.lon - p11.lon;
					p12.lat = p12.lat - p11.lat;

					p21.lon = p21.lon - p11.lon;
					p21.lat = p21.lat - p11.lat;

					p22.lon = p22.lon - p11.lon;
					p22.lat = p22.lat - p11.lat;

					p11.lon = p11.lon - p11.lon;
					p11.lat = p11.lat - p11.lat;

					p0.ati = Bilinear_interpolation(p0, p11, p21, p12, p22);
					l1aArr(i, j).imag(p0.ati);
				}

			}
		}

		l1adata.saveImage(l1aArr, startRow, 0, 1);
	}
	
	
	
	return 0;

}