#include "ParameterInFile.h"
#include <iostream>
#include <fstream>
#include<string>
#include <memory>
#include <vector>
#include <malloc.h>
#include <exception>
#include <future>
#include <thread>

#include <windows.h>
////gdal 
#include "gdal_priv.h"
#include "ogr_geometry.h"
#include "gdalwarper.h"
#include "common.h"


using namespace std;
//
// 参数文件解析
//

//参数文件标准格式
// 文件值        类型   对应的代号
// DEM文件的路径 str   dem_path
// 模拟影像输出路径 str sar_sim_path
// 模拟影像的匹配坐标X输出路径 str sar_sim_match_point_x_path
// 模拟影像的匹配坐标Y输出路径 str sar_sim_match_point_y_path
// 模拟影像的匹配坐标Z输出路径 str sar_sim_match_point_z_path
// 采样率 long double sample_f
// 近斜距 long double R0
// 成像起始时间 long double  starttime ---UTC 时间
// 光速 long double
// 波长 long double
// 多普勒参考时间 long double TO  ---UTC 时间  
// 脉冲重复频率 long double  PRF
// 斜距采样间隔 long double delta_R
// 多普勒系数个数 int 
// 多普勒系数1 long double
// 多普勒系数2 long double
// ....
// 卫星轨道模型是否为多项式模型
// 卫星轨道模型多项式次数  int  4 5
// 卫星轨道模型X值系数1 long double
// ....
// 卫星轨道模型Y值系数1 long double
// ...
// 卫星轨道模型Z值系数1 long double
// ...
// 卫星轨道模型Vx值系数1 long double
// ...
// 卫星轨道模型Vy值系数1 long double
// ...
// 卫星轨道模型Vz值系数1 long double
// ...
// 
//

/// <summary>
/// 将地固参心坐标系转换为经纬度
/// </summary>
/// <param name="XYZ">固参心坐标系</param>
/// <returns>经纬度--degree</returns>
point XYZ2LLA(point& XYZ) {
	long double tmpX = XYZ.x;
	long double temY = XYZ.y;
	long double temZ = XYZ.z;

	long double curB = 0;
	long double N = 0;
	long double sqrtTempXY = sqrt(tmpX * tmpX + temY * temY);
	long double calB = atan2(temZ, sqrtTempXY);
	int counter = 0;
	long double sinCurB = 0;
	while (abs(curB - calB) * r2d > epsilon && counter < 25)
	{
		curB = calB;
		sinCurB = sin(curB);
		N = a / sqrt(1 - eSquare * sinCurB * sinCurB);
		calB = atan2(temZ + N * eSquare * sinCurB, sqrtTempXY);
		counter++;
	}

	point result = { 0,0,0 };
	result.x = atan2(temY, tmpX) * r2d;
	result.y = curB * r2d;
	result.z = temZ / sinCurB - N * (1 - eSquare);
	return result;
}


ParameterInFile::ParameterInFile(std::string infile_path)
{
	// 解析文件
	ifstream infile(infile_path, ios::in);
	if (!infile.is_open()) {
		throw "参数文件未打开";
	}
	try {
		int line_ids = 0;
		string buf;
		getline(infile, buf); this->dem_path = buf;
		getline(infile, buf); this->out_sar_sim_path = buf;
		getline(infile, buf); this->out_sar_sim_dem_path = buf;
		getline(infile, buf); this->out_sar_sim_resampling_path = buf;
		getline(infile, buf); this->out_sar_sim_resampling_rc = buf;
		getline(infile, buf); this->sim_height = stoi(buf);
		getline(infile, buf); this->sim_width = stoi(buf);
		getline(infile, buf); this->sar_sim_match_point_path = buf;
		getline(infile, buf); this->sar_sim_match_point_xyz_path = buf;

		getline(infile, buf); this->sample_f = stoi(buf);
		getline(infile, buf); this->R0 = stod(buf);
		getline(infile, buf); this->LightSpeed = stod(buf);
		getline(infile, buf); this->lamda = stod(buf);
		getline(infile, buf); this->delta_R = stod(buf);
		getline(infile, buf); this->imgStartTime = stod(buf);
		getline(infile, buf); this->PRF = stod(buf);
		getline(infile, buf); this->delta_t = stod(buf);
		getline(infile, buf); this->refrange = stod(buf);
		getline(infile, buf); this->widthspace = stod(buf);
		getline(infile, buf); cout << buf << endl;

		getline(infile, buf); this->doppler_paramenter_number = stoi(buf);

		// 读取多普勒系数
		this->doppler_para = (long double*)calloc(this->doppler_paramenter_number, sizeof(long double));
		if (this->doppler_para) {
			for (int i = 0; i < this->doppler_paramenter_number; i++) {
				getline(infile, buf);
				this->doppler_para[i] = stod(buf);
			}
		}
		else {
			throw "内存不足";
		}
		// 读取卫星轨道
		getline(infile, buf); this->polySatelliteModel = stoi(buf);
		if (this->polySatelliteModel != 1) {
			throw "不是多项式轨道模型";
		}
		getline(infile, buf); this->polynum = stoi(buf) + 1; // 多项式项数
		getline(infile, buf); this->SatelliteModelStartTime = stod(buf); // 轨道模型起始时间

		this->polySatellitePara = (long double*)calloc(this->polynum * 6, sizeof(long double));
		if (this->polySatellitePara) {
			for (int i = 0; i < this->polynum * 6; i++) {
				getline(infile, buf);
				this->polySatellitePara[i] = stod(buf);
			}
		}
		else {
			throw "内存不足";
		}
	}
	catch (exception e) {
		infile.close();
		throw "文件读取错误";
	}
	infile.close();
}

ParameterInFile::ParameterInFile(const ParameterInFile& paras)
{
	//参数组
	this->dem_path = paras.dem_path;
	this->out_sar_sim_path = paras.out_sar_sim_path;
	this->out_sar_sim_dem_path = paras.out_sar_sim_dem_path;
	this->out_sar_sim_resampling_path = paras.out_sar_sim_resampling_path;
	this->out_sar_sim_resampling_rc = paras.out_sar_sim_resampling_rc;
	this->sim_height = paras.sim_height;
	this->sim_width = paras.sim_width;
	this->sar_sim_match_point_path = paras.sar_sim_match_point_path;
	this->sar_sim_match_point_xyz_path = paras.sar_sim_match_point_xyz_path;
	this->sample_f = paras.sample_f;
	this->R0 = paras.R0;
	this->LightSpeed = paras.LightSpeed;
	this->lamda = paras.lamda;

	this->delta_R = paras.delta_R;
	this->imgStartTime = paras.imgStartTime;
	this->PRF = paras.PRF;
	this->delta_t = paras.delta_t;
	this->refrange = paras.refrange;
	// 多普勒
	this->widthspace = paras.widthspace;
	this->doppler_paramenter_number = paras.doppler_paramenter_number;
	//卫星轨道模型
	this->polySatelliteModel = paras.polySatelliteModel;
	this->polynum = paras.polynum;
	this->SatelliteModelStartTime = paras.SatelliteModelStartTime;
	this->doppler_para = (long double*)calloc(this->doppler_paramenter_number, sizeof(long double));
	memcpy(this->doppler_para, paras.doppler_para, paras.doppler_paramenter_number * sizeof(long double));
	this->polySatellitePara = (long double*)calloc(paras.polynum * 6, sizeof(long double));
	memcpy(this->polySatellitePara, paras.polySatellitePara, paras.polynum * 6 * sizeof(long double));
}

ParameterInFile::~ParameterInFile()
{
	// 析构函数
	free(this->doppler_para);
	free(this->polySatellitePara);
}


/// <summary>
/// 根据卫星轨道模型计算卫星
/// </summary>
/// <param name="satelliteTime">卫星轨道点时间</param>
/// <param name="SatelliteModelStartTime">卫星轨道模型起始时间</param>
/// <param name="polySatelliteParaX">卫星轨道X坐标模型参数</param>
/// <param name="polySatelliteParaY">卫星轨道Y坐标模型参数</param>
/// <param name="polySatelliteParaZ">卫星轨道Z坐标模型参数</param>
/// <param name="polySatelliteParaVx">卫星轨道Vx坐标模型参数</param>
/// <param name="polySatelliteParaVy">卫星轨道Vy坐标模型参数</param>
/// <param name="polySatelliteParaVz">卫星轨道Vz坐标模型参数</param>
/// <param name="polynum">多项式项数</param>
/// <returns></returns>
inline SatelliteSpacePoint getSatellitePostion(long double satelliteTime, long double SatelliteModelStartTime, long double* polySatellitePara, int polynum = 4)
{
	satelliteTime = satelliteTime - SatelliteModelStartTime;
	SatelliteSpacePoint result_point = { 0,0,0,0,0,0 };

	if (polynum <= 0)return result_point;
	int ptr_indx = 0;
	int yStep = polynum;
	int zStep = polynum*2;
	int vxStep = polynum*3;
	int vyStep = polynum*4;
	int vzStep = polynum*5;
	//x
	result_point.x +=  polySatellitePara[ptr_indx];
	result_point.y +=  polySatellitePara[ptr_indx + yStep];
	result_point.z +=  polySatellitePara[ptr_indx + zStep];
	result_point.vx +=  polySatellitePara[ptr_indx + vxStep];
	result_point.vy +=  polySatellitePara[ptr_indx + vyStep];
	result_point.vz +=  polySatellitePara[ptr_indx + vzStep];
	if (polynum == 1)return result_point;
	ptr_indx++;
	long double	powTime = satelliteTime;
	result_point.x += powTime * polySatellitePara[ptr_indx];
	result_point.y += powTime * polySatellitePara[ptr_indx + yStep];
	result_point.z += powTime * polySatellitePara[ptr_indx + zStep];
	result_point.vx += powTime * polySatellitePara[ptr_indx + vxStep];
	result_point.vy += powTime * polySatellitePara[ptr_indx + vyStep];
	result_point.vz += powTime * polySatellitePara[ptr_indx + vzStep];
	if (polynum == 2)return result_point;
	ptr_indx++;
	powTime *= satelliteTime;
	result_point.x += powTime * polySatellitePara[ptr_indx];
	result_point.y += powTime * polySatellitePara[ptr_indx + yStep];
	result_point.z += powTime * polySatellitePara[ptr_indx + zStep];
	result_point.vx += powTime * polySatellitePara[ptr_indx + vxStep];
	result_point.vy += powTime * polySatellitePara[ptr_indx + vyStep];
	result_point.vz += powTime * polySatellitePara[ptr_indx + vzStep];
	if (polynum == 3)return result_point;
	ptr_indx++;
	powTime *= satelliteTime;
	result_point.x += powTime * polySatellitePara[ptr_indx];
	result_point.y += powTime * polySatellitePara[ptr_indx + yStep];
	result_point.z += powTime * polySatellitePara[ptr_indx + zStep];
	result_point.vx += powTime * polySatellitePara[ptr_indx + vxStep];
	result_point.vy += powTime * polySatellitePara[ptr_indx + vyStep];
	result_point.vz += powTime * polySatellitePara[ptr_indx + vzStep];
	if (polynum == 4)return result_point;
	ptr_indx++;
	powTime *= satelliteTime;
	result_point.x += powTime * polySatellitePara[ptr_indx];
	result_point.y += powTime * polySatellitePara[ptr_indx + yStep];
	result_point.z += powTime * polySatellitePara[ptr_indx + zStep];
	result_point.vx += powTime * polySatellitePara[ptr_indx + vxStep];
	result_point.vy += powTime * polySatellitePara[ptr_indx + vyStep];
	result_point.vz += powTime * polySatellitePara[ptr_indx + vzStep];
	ptr_indx++;
	if (polynum == 5)return result_point;
	for (int i = 5; i < polynum; i++) {
		powTime *= satelliteTime;
		result_point.x += powTime * polySatellitePara[ptr_indx];
		result_point.y += powTime * polySatellitePara[ptr_indx+ yStep];
		result_point.z += powTime * polySatellitePara[ptr_indx + zStep];
		result_point.vx += powTime * polySatellitePara[ptr_indx + vxStep];
		result_point.vy += powTime * polySatellitePara[ptr_indx + vyStep];
		result_point.vz += powTime * polySatellitePara[ptr_indx + vzStep];
		ptr_indx++;
	}
	return result_point;
}

inline SatelliteSpacePoint getSatellitePostion_orange(long double satelliteTime, long double SatelliteModelStartTime, long double* polySatellitePara, int polynum = 4)
{
	satelliteTime = satelliteTime - SatelliteModelStartTime;
	SatelliteSpacePoint result_point = { 0,0,0,0,0,0 };
	int ptr_indx = 0;
	//x
	for (int i = 0; i < polynum; i++) {
		result_point.x += pow(satelliteTime, i) * polySatellitePara[ptr_indx];
		ptr_indx++;
	}

	//y
	for (int i = 0; i < polynum; i++) {
		result_point.y += pow(satelliteTime, i) * polySatellitePara[ptr_indx];
		ptr_indx++;
	}
	//z
	for (int i = 0; i < polynum; i++) {
		result_point.z += pow(satelliteTime, i) * polySatellitePara[ptr_indx];
		ptr_indx++;
	}
	//vx
	for (int i = 0; i < polynum; i++) {
		result_point.vx += pow(satelliteTime, i) * polySatellitePara[ptr_indx];
		ptr_indx++;
	}
	//vy
	for (int i = 0; i < polynum; i++) {
		result_point.vy += pow(satelliteTime, i) * polySatellitePara[ptr_indx];
		ptr_indx++;
	}
	//vz
	for (int i = 0; i < polynum; i++) {
		result_point.vz += pow(satelliteTime, i) * polySatellitePara[ptr_indx];
		ptr_indx++;
	}

	return result_point;
}

/// <summary>
///  数值模拟法计算多普勒频移值
///  修改时间：2022.07.03
/// </summary>
/// <param name="R">斜距</param>
/// <param name="LightSpeed">光速</param>
/// <param name="T0">多普勒参考时间</param>
/// <param name="doppler_para">多普勒参数</param>
/// <returns>多普勒频移值</returns>
inline long double calNumericalDopplerValue(long double R, long double LightSpeed, long double refrange, long double* doppler_para, int doppler_paramenter_number)
{
	//R = R * 2 / LightSpeed - T0;
	long double result = 0;
	result = doppler_para[0];
	R = (R - refrange) * (1000000 / LightSpeed);
	for (int i = 1; i < doppler_paramenter_number; i++) {
		result =result+ doppler_para[i]*pow(R,i);
	}
	return result;
}
/// <summary>
/// 根据理论模型计算多普勒频移值
/// </summary>
/// <param name="R">斜距</param>
/// <param name="lamda">波长</param>
/// <param name="R_sl">地面->卫星的空间向量</param>
/// <param name="V_sl">地面->卫星之间的速度向量</param>
/// <returns>多普勒频移值</returns>
inline long double calTheoryDopplerValue(long double R, long double lamda, VectorPoint R_sl, VectorPoint V_sl)
{
	long double result = -2 * (R_sl.x * V_sl.x + R_sl.y * V_sl.y + R_sl.z * V_sl.z) / (lamda * R);
	return result;
}

/// <summary>
/// 根据地面点求解对应的sar影像坐标
/// 修改2022.07.03
/// </summary>
/// <param name="landpoint">地面点的坐标--地固坐标系</param>
/// <param name="Starttime">影片开始成像时间</param>
/// <param name="lamda">波长</param>
/// <param name="refrange">多普勒参考斜距</param>
/// <param name="LightSpeed">光速</param>
/// <param name="delta_t">时间间隔</param>
/// <param name="R0">近斜距</param>
/// <param name="delta_R">斜距间隔</param>
/// <param name="SatelliteModelStartTime">卫星轨道模型时间</param>
/// <param name="polySatellitePara">卫星轨道坐标模型参数</param>
/// <param name="polynum">卫星轨道模型项数</param>
/// <param name="doppler_paramenter_number">多普勒模型数</param>
/// <returns>影像坐标（x:行号，y:列号，z：成像时刻）</returns>
inline point PSTN(point& landpoint, long double Starttime, long double lamda, long double refrange, long double* doppler_para, long double LightSpeed, long double prf, long double R0, long double widthspace, long double SatelliteModelStartTime, long double* polySatellitePara, int polynum, int doppler_paramenter_number)
{
	long double ti = Starttime;
	long double dt = 0.01;
	long double R = 0;
	long double FdThory1 = 0;
	long double FdThory2 = 0;
	long double FdNumber = 0;
	long double FdTheory_grad = 0;
	SatelliteSpacePoint spacepoint{ 0,0,0,0,0,0 };
	VectorPoint R_sl{ 0,0,0 };
	VectorPoint V_sl{ 0,0,0 };
	long double int_time = 0;
	long double endfactor = 1e-4;//exp((floor(log10(1/prf))-1)* 2.302585092994046);
	long double error = 0;
	for (int i = 0; i < 100; i++) {
		spacepoint = getSatellitePostion(ti + dt, SatelliteModelStartTime, polySatellitePara, polynum);
		R_sl.x = spacepoint.x - landpoint.x; // 卫星->地面坐标
		R_sl.y = spacepoint.y - landpoint.y; // 
		R_sl.z = spacepoint.z - landpoint.z;
		V_sl.x = spacepoint.vx;
		V_sl.y = spacepoint.vy;
		V_sl.z = spacepoint.vz;
		R = getModule(R_sl);
		FdThory1 = calTheoryDopplerValue(R, lamda, R_sl, V_sl);
		spacepoint = getSatellitePostion(ti, SatelliteModelStartTime, polySatellitePara, polynum);
		R_sl.x = spacepoint.x - landpoint.x;
		R_sl.y = spacepoint.y - landpoint.y;
		R_sl.z = spacepoint.z - landpoint.z;
		V_sl.x = spacepoint.vx;
		V_sl.y = spacepoint.vy;
		V_sl.z = spacepoint.vz;
		R = getModule(R_sl);
		FdThory2 = calTheoryDopplerValue(R, lamda, R_sl, V_sl);
		FdNumber = calNumericalDopplerValue(R, LightSpeed, refrange, doppler_para, doppler_paramenter_number);
		FdTheory_grad = (FdThory1 - FdThory2);
		error = FdNumber - FdThory2;
		int_time = error * dt / FdTheory_grad;
		if (abs(error) < endfactor) {
			ti = ti + int_time;
			break;
		}
		ti = ti + int_time;
	}
	point result{ 0,0,0 };
	result.x = (ti - Starttime) * prf;
	result.y = (R - R0) / widthspace;
	result.z = ti;
	return result;
}


/// <summary>
/// 双线性插值
/// 11  12
///    c
/// 21  22
/// </summary>
/// <param name="p">c的x,y坐标以左上角的行列号为起始点</param>
/// <param name="p11"> </param>
/// <param name="p12"></param>
/// <param name="p21"></param>
/// <param name="p22"></param>
/// <returns></returns>  
inline point bilineadInterpolation(point& p, point& p11, point& p12, point& p21, point& p22)
{
	long double r = 1 - p.x;
	long double c = 1 - p.y;
 
	long double rc = r * c;
	long double r_c_1 = r * p.y;// r* (1 - c);
	long double r_1_c = p.x * c; //(1 - r) * c;
	long double r_1_c_1 = p.x * p.y;// (1 - r)* (1 - c);
	// 计算插值结果
	p.x = rc * p11.x + r_c_1 * p12.x + r_1_c * p21.x + r_1_c_1 * p22.x;
	p.y = rc * p11.y + r_c_1 * p12.y + r_1_c * p21.y + r_1_c_1 * p22.y;
	p.z = rc * p11.z + r_c_1 * p12.z + r_1_c * p21.z + r_1_c_1 * p22.z;
	return p;
}

/// <summary>
/// 三次卷积插值
/// 11 12   13 14
/// 21 22   23 24 
///       c
/// 31 32  33  34 
/// 41 42  43  44
/// </summary>
/// <param name="p"></param>
/// <param name="p11"></param>
/// <param name="p12"></param>
/// <param name="p13"></param>
/// <param name="p14"></param>
/// <param name="p21"></param>
/// <param name="p22"></param>
/// <param name="p23"></param>
/// <param name="p24"></param>
/// <param name="p31"></param>
/// <param name="p32"></param>
/// <param name="p33"></param>
/// <param name="p34"></param>
/// <param name="p41"></param>
/// <param name="p42"></param>
/// <param name="p43"></param>
/// <param name="p44"></param>
/// <returns></returns>
inline point cubicInterpolation(point& p, point& p11, point& p12, point& p13, point& p14, point& p21, point& p22, point& p23, point& p24, point& p31, point& p32, point& p33, point& p34, point& p41, point& p42, point& p43, point& p44)
{
	long double r = p.x;
	long double c = p.y;
	long double r1 = r, r2 = r - 1, r3 = 2 - r, r4 = 3 - r;
	long double c1 = c, c2 = c - 1, c3 = 2 - c, c4 = 3 - c;
	long double wr1 = 4 - 8 * r1 + 5 * r1 * r1 - r1 * r1 * r1;
	long double wr2 = 1 - 2 * r2 * r2 + r2 * r2 * r2;
	long double wr3 = 1 - 2 * r3 * r3 + r3 * r3 * r3;
	long double wr4 = 4 - 8 * r4 + 5 * r4 * r4 - r4 * r4 * r4;

	long double wc1 = 4 - 8 * c1 + 5 * c1 * c1 - c1 * c1 * c1;
	long double wc2 = 1 - 2 * c2 * c2 + c2 * c2 * c2;
	long double wc3 = 1 - 2 * c3 * c3 + c3 * c3 * c3;
	long double wc4 = 4 - 8 * c4 + 5 * c4 * c4 - c4 * c4 * c4;

	long double wr1_wc1 = wr1 * wc1;
	long double wr2_wc1 = wr2 * wc1;
	long double wr3_wc1 = wr3 * wc1;
	long double wr4_wc1 = wr4 * wc1;

	long double wr1_wc2 = wr1 * wc2;
	long double wr2_wc2 = wr2 * wc2;
	long double wr3_wc2 = wr3 * wc2;
	long double wr4_wc2 = wr4 * wc2;

	long double wr1_wc3 = wr1 * wc3;
	long double wr2_wc3 = wr2 * wc3;
	long double wr3_wc3 = wr3 * wc3;
	long double wr4_wc3 = wr4 * wc3;

	long double wr1_wc4 = wr1 * wc4;
	long double wr2_wc4 = wr2 * wc4;
	long double wr3_wc4 = wr3 * wc4;
	long double wr4_wc4 = wr4 * wc4;

	p.x = 0;
	p.x = p.x + wr1_wc1 * p11.x + wr1_wc2 * p12.x + wr1_wc3 * p13.x + wr1_wc4 * p14.x;
	p.x = p.x + wr2_wc1 * p11.x + wr2_wc2 * p12.x + wr2_wc3 * p13.x + wr2_wc4 * p14.x;
	p.x = p.x + wr3_wc1 * p11.x + wr3_wc2 * p12.x + wr3_wc3 * p13.x + wr3_wc4 * p14.x;
	p.x = p.x + wr4_wc1 * p11.x + wr4_wc2 * p12.x + wr4_wc3 * p13.x + wr4_wc4 * p14.x;

	p.y = 0;
	p.y = p.y + wr1_wc1 * p11.y + wr1_wc2 * p12.y + wr1_wc3 * p13.y + wr1_wc4 * p14.y;
	p.y = p.y + wr2_wc1 * p11.y + wr2_wc2 * p12.y + wr2_wc3 * p13.y + wr2_wc4 * p14.y;
	p.y = p.y + wr3_wc1 * p11.y + wr3_wc2 * p12.y + wr3_wc3 * p13.y + wr3_wc4 * p14.y;
	p.y = p.y + wr4_wc1 * p11.y + wr4_wc2 * p12.y + wr4_wc3 * p13.y + wr4_wc4 * p14.y;


	p.z = 0;
	p.z = p.z + wr1_wc1 * p11.z + wr1_wc2 * p12.z + wr1_wc3 * p13.z + wr1_wc4 * p14.z;
	p.z = p.z + wr2_wc1 * p11.z + wr2_wc2 * p12.z + wr2_wc3 * p13.z + wr2_wc4 * p14.z;
	p.z = p.z + wr3_wc1 * p11.z + wr3_wc2 * p12.z + wr3_wc3 * p13.z + wr3_wc4 * p14.z;
	p.z = p.z + wr4_wc1 * p11.z + wr4_wc2 * p12.z + wr4_wc3 * p13.z + wr4_wc4 * p14.z;

	return p;
}

//
// GDAL 数组操作---内置函数不进入头文件声明
//

float* ReadRasterArray(GDALRasterBand* demBand, int arrayWidth, int arrayHeight, GDALDataType gdal_datatype) {


	float* result = new float[arrayWidth * arrayHeight]; // 别忘了释放内存
	if (gdal_datatype == GDALDataType::GDT_UInt16) {
		unsigned short* temp = (unsigned short*)calloc(arrayHeight * arrayWidth, sizeof(unsigned short));
		demBand->RasterIO(GF_Read, 0, 0, arrayWidth, arrayHeight, temp, arrayWidth, arrayHeight, gdal_datatype, 0, 0);
		for (int i = 0; i < arrayHeight * arrayWidth; i++) {
			result[i] = temp[i] * 1.0;
		}
		free(temp);
	}
	else if (gdal_datatype == GDALDataType::GDT_Int16) {
		short* temp = (short*)calloc(arrayHeight * arrayWidth, sizeof(short));
		demBand->RasterIO(GF_Read, 0, 0, arrayWidth, arrayHeight, temp, arrayWidth, arrayHeight, gdal_datatype, 0, 0);
		for (int i = 0; i < arrayHeight * arrayWidth; i++) {
			result[i] = temp[i] * 1.0;
		}
		free(temp);
	}
	else if (gdal_datatype == GDALDataType::GDT_Float32) {
		float* temp = (float*)calloc(arrayHeight * arrayWidth, sizeof(float));
		demBand->RasterIO(GF_Read, 0, 0, arrayWidth, arrayHeight, temp, arrayWidth, arrayHeight, gdal_datatype, 0, 0);
		for (int i = 0; i < arrayHeight * arrayWidth; i++) {
			result[i] = temp[i] * 1.0;
		}
		free(temp);
	}

	return result;
}



int WriteMatchPoint(string path, std::vector<matchPoint>* matchps) {
	std::cout << "输出匹配文件中：" << path;
	fstream f;
	//追加写入,在原来基础上加了ios::app 
	f.open(path, ios::out | ios::app);
	for (int i = 0; i < matchps->size(); i++) {
		matchPoint matchP = (*matchps)[i];
		//f << matchP.r << "," << matchP.c << "," << matchP.ti << "," << matchP.x << "," << matchP.y << "," << matchP.z << "\n";
	}
	f.close();
	std::cout << "输出匹配文件over:" << path;
	return 0;
}


sim_block::sim_block(int start_row, int end_row, int start_col, int end_col, int height, int width)
{
	this->start_row = start_row;
	this->end_row = end_row;
	this->start_col = start_col;
	this->end_col = end_col;
	this->height = height;
	this->width = width;
	this->size = height * width;
	this->block = (short*)calloc(this->size, sizeof(short));
	this->distanceblock = (long double*)calloc(this->size, sizeof(long double));
	this->pointblock = (matchPoint*)calloc(this->size, sizeof(matchPoint));
	for (int i = 0; i < this->size; i++) {
		this->distanceblock[i] = -1;
	}
}

/// <summary>
/// 深度拷贝
/// </summary>
/// <param name="sim_blocks"></param>
sim_block::sim_block(const sim_block& sim_blocks)
{
	this->height = sim_blocks.width;
	this->width = sim_blocks.height;
	this->start_row = sim_blocks.start_row;
	this->start_col = sim_blocks.start_col;
	this->end_col = sim_blocks.end_col;
	this->end_row = sim_blocks.end_row;
	this->size = sim_blocks.size;
	// 声明块
	this->block = (short*)calloc(this->size, sizeof(short));
	this->distanceblock = (long double*)calloc(this->size, sizeof(long double));
	this->pointblock = (matchPoint*)calloc(this->size, sizeof(matchPoint));
	// 移动并复制块
	memcpy(this->block, sim_blocks.block, this->size * sizeof(short));
	memcpy(this->distanceblock, sim_blocks.distanceblock, this->size * sizeof(long double));
	memcpy(this->pointblock, sim_blocks.pointblock, this->size * sizeof(matchPoint));

}

sim_block::~sim_block()
{
	if (this->block) {
		free(this->block);
		this->block = NULL;
	}
	if (this->distanceblock) {
		free(this->distanceblock);
		this->distanceblock = NULL;
	}
	if (this->pointblock) {
		free(this->pointblock);
		this->pointblock = NULL;
	}
}

int sim_block::rowcol2blockids(int row_ids, int col_ids)
{
	if (this->start_row > row_ids && this->end_row <= row_ids && this->start_col > col_ids && this->end_col <= col_ids) {
		return -1;
	}
	int ids = (row_ids - this->start_row) * this->width + col_ids - this->start_col;
	return ids;
}

/// <summary>
/// 根据行列号设置指定块的数值
/// </summary>
/// <param name="row_ids">行号</param>
/// <param name="col_ids">列号</param>
/// <param name="value">设置的值</param>
/// <returns></returns>
int sim_block::setsimblock(int row_ids, int col_ids, short value)
{
	int ids = this->rowcol2blockids(row_ids, col_ids);
	if (ids < 0) {
		return -1;
	}
	this->block[ids] = value;
	/*
	try {
		this->block[ids] = value;
	}
	catch (exception e) {
		throw "Error";
		return -1;
	}
	*/
	return 0;
}

int sim_block::addsimblock(int row_ids, int col_ids, short value)
{
	int ids = this->rowcol2blockids(row_ids, col_ids);
	if (ids < 0) {
		return -1;
	}
	this->block[ids] += value;
	/*
	try {
		this->block[ids] += value;
	}
	catch (exception e) {
		throw "Error";
		return -1;
	}
	*/
	return 0;
}


/// <summary>
/// 根据行列号，获取指定行列号块的数值
/// </summary>
/// <param name="row_ids">行号</param>
/// <param name="col_ids">列号</param>
/// <returns></returns>
short sim_block::getsimblock(int row_ids, int col_ids)
{
	int ids = this->rowcol2blockids(row_ids, col_ids);
	if (ids < 0) {
		return -1;
	}
	short result;
	result = this->block[ids];
	/*
	try {
		result = this->block[ids];
	}
	catch (exception e) {
		throw "Error";
		return -1;
	}
	*/
	return result;
}

/// <summary>
/// 获取模拟坐标对应的点坐标
/// </summary>
/// <param name="row_ids">行号</param>
/// <param name="col_ids">列号</param>
/// <returns></returns>
matchPoint sim_block::getpointblock(int row_ids, int col_ids)
{
	int ids = this->rowcol2blockids(row_ids, col_ids);
	if (ids < 0) {
		throw "坐标错误";
		return matchPoint{ 0,0,0,0,0,0 };
	}
	matchPoint result = this->pointblock[ids];
	return result;
}

/// <summary>
/// 设置模拟块对应的点坐标
/// </summary>
/// <param name="row_ids">行号</param>
/// <param name="col_ids">列号</param>
/// <param name="value">坐标点</param>
/// <returns>更新结果</returns>
int sim_block::setpointblock(int row_ids, int col_ids, matchPoint value)
{
	int ids = this->rowcol2blockids(row_ids, col_ids);
	if (ids < 0) { return -1; }
	this->pointblock[ids] = value;
	return 0;
}

long double sim_block::getdistanceblock(int row_ids, int col_ids)
{
	int ids = this->rowcol2blockids(row_ids, col_ids);
	if (ids < 0) {
		return -1;
	}
	long double result;
	result = this->distanceblock[ids];
	/*
	try {
		result = this->distanceblock[ids];
	}
	catch (exception e) {
		throw "Error";
		return -1;
	}
	*/
	return result;
}

int sim_block::setdistanceblock(int row_ids, int col_ids, long double value)
{
	int ids = this->rowcol2blockids(row_ids, col_ids);
	if (ids < 0) {
		return -1;
	}
	this->distanceblock[ids] = value;
	/*
	try {
		this->distanceblock[ids] = value;
	}
	catch (exception e) {
		throw "Error";
		return -1;
	}
	*/
	return 0;
}


/// <summary>
/// 初始化dem_block
/// </summary>
/// <param name="start_row"></param>
/// <param name="end_row"></param>
/// <param name="start_col"></param>
/// <param name="end_col"></param>
/// <param name="height"></param>
/// <param name="width"></param>
/// <param name="sample_f"></param>
dem_block::dem_block(int all_start_row, int all_start_col, int start_row, int end_row, int start_col, int end_col, int height, int width, int sample_f)
{
	this->all_start_row = all_start_row;
	this->all_start_col = all_start_col;
	this->start_row = start_row;
	this->end_row = end_row;
	this->start_col = start_col;
	this->end_col = end_col;
	this->height = height;
	this->width = width;
	this->size = height * width;
	this->pointblock = (point*)calloc(this->size, sizeof(point));
	this->sample_f = sample_f;
}

dem_block::dem_block(const dem_block& demblocks)
{
	this->all_start_row = demblocks.all_start_row;
	this->all_start_col = demblocks.all_start_col;
	this->start_row = demblocks.start_row;
	this->end_row = demblocks.end_row;
	this->start_col = demblocks.start_col;
	this->end_col = demblocks.end_col;
	this->height = demblocks.height;
	this->width = demblocks.width;
	this->size = this->height * this->width;
	this->pointblock = (point*)calloc(this->size, sizeof(point));
	this->sample_f = sample_f;
	memcpy(this->pointblock, demblocks.pointblock, this->size * sizeof(point));
}

dem_block::~dem_block()
{
	if (this->pointblock) {
		free(this->pointblock);
		this->pointblock = NULL;
		//delete this;
	}

}

/// <summary>
/// 重采样dem--三次卷积插值-- 必须在地理坐标系下进行（经纬度）
/// 为了保证采样左右各舍弃了1格
/// </summary>
/// <returns></returns>
//dem_block dem_block::resample_dem()
//{
//	int height = this->height -2;//[1,2 ....., n-1,n]
//	int width = this->width -2;//[1,2 ....., n-1,n]
//	height = height * this->sample_f; //重采样之后的高度
//	width = width * this->sample_f; // 重采样之后的宽度
//
//	int start_row = (this->start_row-1) * this ->sample_f;
//	int start_col = (this->start_col-1) * this->sample_f;
//	int end_row = (this->end_row-1) * this->sample_f;
//	int end_col = (this->end_col-1) * this->sample_f;
//	dem_block resample_dem = dem_block(start_row, end_row, start_col, end_col, height, width, this->sample_f);
//
//	// 执行dem重采样，采用双曲线插值
//	for (int i = sample_f; i < height; i++) { // 行 从第一行
//		int ori_i_2 = i / sample_f;
//		int ori_i_1, ori_i_3, ori_i_4;
//		if (ori_i_2 - 1 < 0) { continue; }
//		ori_i_1 = ori_i_2 - 1;
//		if (ori_i_2 + 1 > this->height || ori_i_2 + 2 > this->height) { break; }
//		ori_i_3 = ori_i_2 + 1;
//		ori_i_4 = ori_i_2 + 2;
//		for (int j = sample_f; j < width; j++) { //列 从第一列
//			point p{0,0,0};
//			int ori_j_2 = j / sample_f;
//			int ori_j_1, ori_j_3, ori_j_4;
//			if (ori_i_2 - 1 < 0) { continue; }
//			ori_j_1 = ori_j_2 - 1;
//			if (ori_j_2 + 1 > this->height || ori_j_2 + 2 > this->height) { break; }
//			ori_j_3 = ori_j_2 + 1;
//			ori_j_4 = ori_j_2 + 2;
//			// 获取插值需要的数据--三次重采样差值
//			p.x = (i % sample_f) * 1.0 / sample_f; p.x = p.x + 1;
//			p.y = (j % sample_f) * 1.0 / sample_f; p.y = p.y + 1;
//			point p11, p12, p13, p14, p21, p22, p23, p24, p31, p32, p33, p34, p41, p42, p43, p44;
//			// 获取对应节点
//			p11 = this->getpointblock(ori_i_1, ori_j_1);
//			p12 = this->getpointblock(ori_i_1, ori_j_2);
//			p13 = this->getpointblock(ori_i_1, ori_j_3);
//			p14 = this->getpointblock(ori_i_1, ori_j_4);
//			p21 = this->getpointblock(ori_i_2, ori_j_1);
//			p22 = this->getpointblock(ori_i_2, ori_j_2);
//			p23 = this->getpointblock(ori_i_2, ori_j_3);
//			p24 = this->getpointblock(ori_i_2, ori_j_4);
//			p31 = this->getpointblock(ori_i_3, ori_j_1);
//			p32 = this->getpointblock(ori_i_3, ori_j_2);
//			p33 = this->getpointblock(ori_i_3, ori_j_3);
//			p34 = this->getpointblock(ori_i_3, ori_j_4);
//			p41 = this->getpointblock(ori_i_4, ori_j_1);
//			p42 = this->getpointblock(ori_i_4, ori_j_2);
//			p43 = this->getpointblock(ori_i_4, ori_j_3);
//			p44 = this->getpointblock(ori_i_4, ori_j_4);
//			p = cubicInterpolation(p, p11, p12, p13, p14, p21, p22, p23, p24, p31, p32, p33, p34, p41, p42, p43, p44);
//			// 计算p 对应的行列号
//			resample_dem.setpointblock(i - sample_f, j - sample_f, p); 
//		}
//	}
//
//
//	return resample_dem;
//}
/* -------需要优化的方法--------*/
dem_block dem_block::resample_dem() {
	int height = this->height - 2;//[1,2 ....., n-1,n]
	int width = this->width - 2;//[1,2 ....., n-1,n]
	height = height * this->sample_f; //重采样之后的高度
	width = width * this->sample_f; // 重采样之后的宽度

	int start_row = (this->start_row - 1) * this->sample_f;
	int start_col = (this->start_col - 1) * this->sample_f;
	int end_row = (this->end_row - 1) * this->sample_f;
	int end_col = (this->end_col - 1) * this->sample_f;
	dem_block resample_dem = dem_block(all_start_row,all_start_col,start_row, end_row, start_col, end_col, height, width, this->sample_f);
	long double sample_f_1 = 1.0 / sample_f;
	// 执行dem重采样，采用双曲线插值

	int ori_i_2 =0;
	int ori_i_1, ori_i_3, ori_i_4;
	point p{ 0,0,0 };
	point p11, p12, p13, p14;

	for (int i = sample_f; i < height; i++) { // 行 从第一行
		 ori_i_2 = i * sample_f_1;
		if (ori_i_2 - 1 < 0) { continue; }
		ori_i_1 = ori_i_2 - 1;
		if (ori_i_2 + 1 > this->height || ori_i_2 + 2 > this->height) { break; }
		ori_i_3 = ori_i_2 + 1;
		ori_i_4 = ori_i_2 + 2;
		for (int j = sample_f; j < width; j++) { //列 从第一列
			p={ 0,0,0 };
			int ori_j_2 = j * sample_f_1;
			int ori_j_1, ori_j_3, ori_j_4;
			if (ori_i_2 - 1 < 0) { continue; }
			ori_j_1 = ori_j_2 - 1;
			if (ori_j_2 + 1 > this->height || ori_j_2 + 2 > this->height) { break; }
			ori_j_3 = ori_j_2 + 1;
			ori_j_4 = ori_j_2 + 2;
			// 获取插值需要的数据--三次重采样差值
			p.x = (i % sample_f) * sample_f_1 + 1;
			//p.x = p.x + 1;
			p.y = (j % sample_f) * sample_f_1 + 1;
			//p.y = p.y + 1;
		
			// 获取对应节点
			p11 = this->getpointblock(ori_i_2, ori_j_2);
			p12 = this->getpointblock(ori_i_2, ori_j_3);
			p13 = this->getpointblock(ori_i_3, ori_j_2);
			p14 = this->getpointblock(ori_i_3, ori_j_3);
			p = bilineadInterpolation(p, p11, p12, p13, p14); // ----- 优化 
			// 计算p 对应的行列号
			resample_dem.setpointblock(i - sample_f, j - sample_f, p);
		}
	}
	return resample_dem;
}


/// <summary>
/// 根据重采样结果获取对应行列号的坡面法式向量(
/// 必须提前调用UpdatePointCoodinarary()方法，保证坐标系为地固坐标系
/// </summary>
/// <param name="row_ids">行号</param>
/// <param name="col_ids">列号</param>
/// <returns></returns>
VectorPoint dem_block::getslopeVector(int row_ids, int col_ids)
{
	if (row_ids == 0 || col_ids == 0 || row_ids > this->height - 1 || col_ids > this->width - 1) { return VectorPoint{ 0,0,0 }; } // 无效值
	VectorPoint result{ 0,0,0 };
	// 计算向量值，注意向量值的计算初值结果
	point p1 = this->getpointblock(row_ids - 1, col_ids);
	point p2 = this->getpointblock(row_ids, col_ids - 1);
	point p3 = this->getpointblock(row_ids + 1, col_ids);
	point p4 = this->getpointblock(row_ids, col_ids + 1);
	VectorPoint n24 = getVector(p2, p4);
	VectorPoint n31 = getVector(p3, p1);
	result = VectorFork(n24, n31); // 目标向量
	return result;
}

int dem_block::rowcol2blockids(int row_ids, int col_ids)
{
	return row_ids * this->width + col_ids;
}


/// <summary>
/// 根据行列号获取对应的点
/// </summary>
/// <param name="row_ids"></param>
/// <param name="col_ids"></param>
/// <returns></returns>
point dem_block::getpointblock(int row_ids, int col_ids)
{
	int ids = this->rowcol2blockids(row_ids, col_ids);
	//try {
	return this->pointblock[ids];
	//}
	//catch (exception e) {
		//throw "error";
		//return point{ -1,-1,-1 };
	//}
}

/// <summary>
/// 根据行列号更新值
/// </summary>
/// <param name="row_ids"></param>
/// <param name="col_ids"></param>
/// <param name="value"></param>
/// <returns></returns>
int dem_block::setpointblock(int row_ids, int col_ids, point& value)
{
	this->pointblock[row_ids * this->width + col_ids]= value;
	/*
	int ids = this->rowcol2blockids(row_ids, col_ids);
	try {
		this->pointblock[ids] = value;
	}
	catch (exception ex) {
		throw "error";
		return -1;
	}
	
	*/
	return 0;
}

point dem_block::getpointblock(int ids)
{
	return this->pointblock[ids];
	/*try {
		point result = this->pointblock[ids];
		return result;
	}
	catch (exception e) {
		throw "error";
		return point{ -1,-1,-1 };
	}*/
}

int dem_block::setpointblock(int ids, point& value)
{
	this->pointblock[ids] = value;
	/*
	try {
		this->pointblock[ids] = value;
	}
	catch (exception ex) {
		throw "error";
		return -1;
	}
	*/
	return 0;
}

/// <summary>
///  将点坐标由经纬度->地固坐标系
/// </summary>
/// <param name="gt"></param>
/// <returns></returns>
int dem_block::UpdatePointCoodinarary()
{
	for (int i = 0; i < this->size; i++) {
		this->pointblock[i] = LLA2XYZ(this->pointblock[i]);
	}
	return 0;
}




ConvertResampleParameter::ConvertResampleParameter(string path)
{

	// 解析文件
	ifstream infile(path, ios::in);
	if (!infile.is_open()) {
		throw "参数文件未打开";
	}
	try {
		int line_ids = 0;
		string buf;
		getline(infile, buf); this->in_ori_dem_path = buf;
		getline(infile, buf); this->ori_sim = buf;
		getline(infile, buf); this->out_sar_xyz_path = buf;
		getline(infile, buf); this->out_sar_xyz_incidence_path = buf;
		getline(infile, buf); this->out_orth_sar_incidence_path = buf;
		getline(infile, buf); this->out_orth_sar_local_incidence_path = buf;
		getline(infile, buf); this->outFolder_path = buf;
		getline(infile, buf); this->file_count = stoi(buf);
		this->inputFile_paths = std::vector<string>(this->file_count);
		this->outFile_paths = std::vector<string>(this->file_count);
		this->outFile_pow_paths = std::vector<string>(this->file_count);
		for (int i = 0; i < this->file_count; i++) {
			getline(infile, buf); this->inputFile_paths[i] = buf;
			getline(infile, buf); this->outFile_paths[i] = buf;
			getline(infile, buf); this->outFile_pow_paths[i] = buf;
		}

		getline(infile, buf); this->ori2sim_num = stoi(buf);

		this->ori2sim_paras = (long double*)calloc(this->ori2sim_num, sizeof(long double));
		for (int i = 0; i < this->ori2sim_num; i++) {
			getline(infile, buf);
			this->ori2sim_paras[i] = stod(buf);
		}
		getline(infile, buf); this->sim2ori_num = stoi(buf);
		this->sim2ori_paras = (long double*)calloc(this->sim2ori_num, sizeof(long double));
		for (int i = 0; i < this->sim2ori_num; i++) {
			getline(infile, buf);
			this->sim2ori_paras[i] = stod(buf);
		}
	}
	catch (exception e) {
		infile.close();
		throw "文件读取错误";
	}
	infile.close();


}

ConvertResampleParameter::ConvertResampleParameter(const ConvertResampleParameter& para)
{
	this->in_ori_dem_path = para.in_ori_dem_path;
	this->ori_sim = para.ori_sim;
	this->out_sar_xyz_path = para.out_sar_xyz_path;
	this->out_sar_xyz_incidence_path = para.out_sar_xyz_incidence_path;
	this->out_orth_sar_incidence_path = para.out_orth_sar_incidence_path;
	this->out_orth_sar_local_incidence_path = para.out_orth_sar_local_incidence_path;
	this->outFolder_path = para.outFolder_path;
	this->file_count = para.file_count;
	this->inputFile_paths = std::vector<string>(this->file_count);
	this->outFile_paths = std::vector<string>(this->file_count);
	this->outFile_pow_paths = std::vector<string>(this->file_count);
	for (int i = 0; i < this->file_count; i++) {
		this->inputFile_paths[i] = para.inputFile_paths[i];
		this->outFile_paths[i] = para.outFile_paths[i];
		this->outFile_pow_paths[i] = para.outFile_pow_paths[i];
	}
	this->ori2sim_num = para.ori2sim_num;
	this->ori2sim_paras = (long double*)calloc(this->ori2sim_num, sizeof(long double));
	memcpy(this->ori2sim_paras, para.ori2sim_paras, this->ori2sim_num * sizeof(long double));
	this->sim2ori_num = para.sim2ori_num;
	this->sim2ori_paras = (long double*)calloc(this->sim2ori_num, sizeof(long double));
	memcpy(this->sim2ori_paras, para.sim2ori_paras, this->sim2ori_num * sizeof(long double));
}

ConvertResampleParameter::~ConvertResampleParameter()
{
	free(this->sim2ori_paras);
	free(this->ori2sim_paras);
	this->outFile_paths.clear();
	this->outFile_paths.swap(this->outFile_paths);
	this->inputFile_paths.clear();
	this->inputFile_paths.swap(this->inputFile_paths);
	this->outFile_pow_paths.clear();
	this->outFile_pow_paths.swap(this->outFile_pow_paths);
}

#include "threadpool.hpp"
//int SimProcessBlock(dem_block demblock, ParameterInFile paras, matchPoint* result_shared, int* Pcount)

// 插值算法



/// <summary>
/// 分块模拟求解
/// T0 变更为 refrange, delta_R: widthspace
/// </summary>
/// <param name="paras"></param>
/// <param name="demblock"></param>
/// <returns></returns>
int SimProcessBlock(dem_block demblock, ParameterInFile paras, matchPoint* result_shared, int* Pcount)
{
	long double Starttime = paras.imgStartTime;
	long double lamda = paras.lamda;
	long double refrange = paras.refrange;
	long double* doppler_para = paras.doppler_para;
	long double LightSpeed = paras.LightSpeed;
	long double delta_t = paras.delta_t;
	long double prf = paras.PRF;
	long double R0 = paras.R0;
	long double widthspace = paras.widthspace;
	long double SatelliteModelStartTime = paras.SatelliteModelStartTime;
	long double* polySatellitePara = paras.polySatellitePara;
	int sim_sar_width = paras.sim_width;
	int sim_sar_height = paras.sim_height;
	int polynum = paras.polynum;
	int doppler_paramenter_number = paras.doppler_paramenter_number;


	bool haspoint = true;
	for (int i = 0; i < demblock.size; i++) {
		point tempPoint = demblock.getpointblock(i);
		tempPoint = LLA2XYZ(tempPoint);
		// T0 变更为 refrange, delta_R: widthspace
		point pixelpoint = PSTN(tempPoint, Starttime, lamda, refrange, doppler_para, LightSpeed, prf, R0, widthspace, SatelliteModelStartTime, polySatellitePara, polynum, doppler_paramenter_number);
		if (pixelpoint.x >= -3000-paras.sample_f*5 && pixelpoint.x < paras.sim_height + paras.sample_f * 5 && pixelpoint.y >= -paras.sample_f * 5 && pixelpoint.y < paras.sim_width + paras.sample_f * 5) {
			haspoint = false;
			break;
		}
	}

	if (haspoint) {
		*Pcount = -1;
		return -1;
	}

	// dem重采样
	demblock.sample_f = paras.sample_f;
	dem_block resample = demblock.resample_dem(); // 重采样的时候经纬度，
	resample.UpdatePointCoodinarary(); // 可以优化

	//声明
	int sim_height = resample.end_row - resample.start_row;
	int sim_width = resample.end_col - resample.start_col;
	int sim_pixel_count = sim_width * sim_height;

	// 计算对应的行列号
	int ids = 0;
	SatelliteSpacePoint satepoint_;
	*Pcount = 0;
	try {
		// 计算间接定位法结果
		for (int i = resample.start_row; i < resample.end_row; i++) {
			for (int j = resample.start_col; j < resample.end_col; j++) {
				point landPoint = resample.getpointblock(i, j);
				point pixelpoint = PSTN(landPoint, Starttime, lamda, refrange, doppler_para, LightSpeed, prf, R0, widthspace, SatelliteModelStartTime, polySatellitePara, polynum, doppler_paramenter_number);
				point temppoint{ round(pixelpoint.x),round(pixelpoint.y),pixelpoint.z };
				if (temppoint.x < -3000 || temppoint.x >= paras.sim_height || temppoint.y < 0 || temppoint.y >= paras.sim_width) {
					continue;
				}
				VectorPoint n = resample.getslopeVector(i, j);
				satepoint_ = getSatellitePostion(pixelpoint.z, SatelliteModelStartTime, polySatellitePara, polynum);
				point satepoint{ satepoint_.x,satepoint_.y,satepoint_.z };//卫星坐标
				IncidenceAngle pixelIncidence = calIncidence(satepoint, landPoint, n);
				if (pixelIncidence.localincidenceAngle >= 0 && pixelIncidence.localincidenceAngle <= 1)
				{
					matchPoint matchp{ temppoint.x, temppoint.y, pixelpoint.z,0,0,0,0,pixelIncidence.incidenceAngle,pixelIncidence.localincidenceAngle};
					result_shared[ids] = matchp;
					*Pcount = *Pcount + 1;
					ids = ids + 1;
				}
			}
		}
		return 0;
	}
	catch (exception e) {
		//free(pixelpoints);
		throw "计算错误";
		return -1;
	}
}

static std::mutex s_sim_dem_Mutex; //数据锁
void taskFunc(ParameterInFile& paras, int dem_block_size, int width, short* sim_dem, int count_cure, int count_sum,
	int i, int j, int block_height, int block_width, int end_row_ex, int end_col_ex, float* demarray, translateArray& gtt, translateArray& inv_gtt)
{
	//dem_block *demblock, 
	int Pcount = 0;
	dem_block* demblock = new dem_block(i - 3, j - 3, 3, block_height - 3, 3, block_width - 3, block_height, block_width, paras.sample_f);// 构建块结构
	for (int ii = i - 3; ii < end_row_ex; ii++) {
		for (int jj = j - 3; jj < end_col_ex; jj++) {
			// 构建点集
			int dem_ids = ii * width + jj;
			point tempPoint = Translation(ii, jj, demarray[dem_ids], gtt);
			demblock->setpointblock(ii - i + 3, jj - j + 3, tempPoint);
		}
	}

	matchPoint*  result_shared = (matchPoint*)calloc((dem_block_size + 7) * (dem_block_size + 7) * paras.sample_f * paras.sample_f, sizeof(matchPoint));
	int nRet= SimProcessBlock(*demblock, paras,result_shared, &Pcount);

	int tempstate = Pcount;

	int row_ids = 0;
	int col_ids = 0;
	int ids = 0;
	matchPoint* pixelpoint = NULL;
	{
		//std::lock_guard<std::mutex> guard(s_sim_dem_Mutex);
		s_sim_dem_Mutex.lock();
		for (int ii = 0; ii < tempstate; ii++) {
			pixelpoint = &result_shared[ii];
			row_ids = int(pixelpoint->r)+3000;
			col_ids = int(pixelpoint->c);
			ids = row_ids *paras.sim_width + col_ids;
			sim_dem[ids] ++;
		}
		s_sim_dem_Mutex.unlock();
	}
	//根据匹配结果，尝试计算插值结果
	delete demblock;
	free(result_shared);

}


void taskFunc_Calsim2ori(ParameterInFile& paras, ConvertResampleParameter& conveparas,
	int start_row, int end_row, int start_col, int end_col,
	int dem_width,int dem_height,
	float* sar_local_angle, float* sar_angle, float* sar_dem_x, float* sar_dem_y,
	float* demarray, translateArray& gtt, translateArray& inv_gtt)
{
	/*
		// 计算向量值，注意向量值的计算初值结果
	point p1 = this->getpointblock(row_ids - 1, col_ids);
	point p2 = this->getpointblock(row_ids, col_ids - 1);
	point p3 = this->getpointblock(row_ids + 1, col_ids);
	point p4 = this->getpointblock(row_ids, col_ids + 1);
	VectorPoint n24 = getVector(p2, p4);
	VectorPoint n31 = getVector(p3, p1);
	result = // 目标向量
	*/
	double templocalincidenceAngle = 0;
	double tempincidenceAngle = 0;
	double templocalincidenceAngle1 = 0;
	double tempincidenceAngle1 = 0;
	for (int i = start_row; i <= end_row; i++) {
		if (i - 1 < 0 || i + 1 >= dem_height) {
			continue;
		}
		for (int j = start_col; j <= end_col; j++) {
			if (j - 1 < 0 || j + 1 >= dem_width) {
				continue;
			}
			int dem_ids = i * dem_width + j;
			point landPoint = Translation(i, j, demarray[dem_ids], gtt);
			landPoint =LLA2XYZ(landPoint);
			point pixelpoint = PSTN(landPoint, paras.imgStartTime, paras.lamda, paras.refrange, paras.doppler_para, paras.LightSpeed, paras.PRF, paras.R0, paras.delta_R, paras.SatelliteModelStartTime, paras.polySatellitePara, paras.polynum, paras.doppler_paramenter_number);
			long double r, c; // 结果的行列号
			sim2ori(pixelpoint.x, pixelpoint.y, r, c, conveparas.sim2ori_paras); // 计算得到的行列号 --> 
			
			// 计算向量1
			dem_ids = (i - 1) * dem_width + j; point p1 = Translation(i - 1, j, demarray[dem_ids], gtt); p1 = LLA2XYZ(p1);
			dem_ids = i * dem_width + (j - 1); point p2 = Translation(i, j - 1, demarray[dem_ids], gtt); p2 = LLA2XYZ(p2);
			dem_ids = (i + 1) * dem_width + j; point p3 = Translation(i + 1, j, demarray[dem_ids], gtt); p3 = LLA2XYZ(p3);
			dem_ids = i * dem_width + (j + 1); point p4 = Translation(i, j + 1, demarray[dem_ids], gtt); p4 = LLA2XYZ(p4);
			VectorPoint n24 = getVector(p2, p4);
			VectorPoint n31 = getVector(p3, p1);
			VectorPoint n = VectorFork(n24, n31);

			long double satelliteTime =r / paras.PRF + paras.imgStartTime;// pixelpoint.z;// 
			SatelliteSpacePoint satepoint_ = getSatellitePostion(satelliteTime, paras.SatelliteModelStartTime, paras.polySatellitePara, paras.polynum);
			point satepoint{ satepoint_.x,satepoint_.y,satepoint_.z };//卫星坐标
			IncidenceAngle pixelIncidence = calIncidence(satepoint, landPoint, n);

			dem_ids = i * dem_width + j;
			sar_local_angle[dem_ids] = acos(pixelIncidence.localincidenceAngle) * r2d;
			sar_angle[dem_ids] =90- acos(pixelIncidence.incidenceAngle) * r2d;

			sar_dem_x[dem_ids] = r;
			sar_dem_y[dem_ids] = c;
		}
	}
	//std::cout << "\r" << end_row << "/" << dem_height << "," << end_col << "/" << dem_width << "," << end_row*100.0 * end_col / (dem_width * dem_height) << "       ------------------";
}

int SimProcess_LVY(ParameterInFile paras, int thread_num)
{
	std::cout << "begin time:" << getCurrentTimeString() << std::endl;
	GDALAllRegister();// 注册格式的驱动
	// 打开DEM影像
	GDALDataset* demDataset = (GDALDataset*)(GDALOpen(paras.dem_path.c_str(), GA_ReadOnly));//以只读方式读取地形影像
	int width = demDataset->GetRasterXSize();
	int height = demDataset->GetRasterYSize();
	int nCount = demDataset->GetRasterCount();

	double* gt = (double*)calloc(6, sizeof(double)); // 仿射矩阵

	demDataset->GetGeoTransform(gt); // 获得仿射矩阵
	translateArray gtt{ gt[0],gt[1],gt[2],gt[3],gt[4],gt[5] };
	double* inv_gt = (double*)calloc(6, sizeof(double)); // 仿射矩阵
	GDALInvGeoTransform(gt, inv_gt); // 逆矩阵
	translateArray inv_gtt{ inv_gt[0],inv_gt[1],inv_gt[2],inv_gt[3],inv_gt[4],inv_gt[5] };
	GDALDataType gdal_datatype = demDataset->GetRasterBand(1)->GetRasterDataType();
	const char* Projection = demDataset->GetProjectionRef();
	GDALRasterBand* demBand = demDataset->GetRasterBand(1);
	float* demarray = ReadRasterArray(demBand, width, height, gdal_datatype);
	int dem_pixel_count = width * height;
	int psize = (paras.sim_height+3000) * paras.sim_width;
	short* sim_dem = (short*)calloc(psize, sizeof(short));


	//// 计算每个块的尺寸
	int dem_block_size = int(ceil(sqrt(10*1024* 1024 / (paras.sample_f* paras.sample_f *sizeof(gdal_datatype))))); // 获取每块dem的尺寸

	//构建内存储空间对象
//	try {
	int a = 2;

	int i = 3;
	int j = 3;
	int thread_ids = 0;
	bool hasfullpool = false;
	int count_sum = height * width / (dem_block_size * dem_block_size);
	int count_cure = 0;

	// 标志文件是否正在写出
	//bool outmatchpointing = false;
	//std::vector<thread> outmatchpointthread(0);
	int tempstate = 0;
	ThreadPool* subthreadPool = new ThreadPool(thread_num, true);
	ThreadPoolPtr poolPtr(subthreadPool);
	poolPtr->start();

	int start_row =0;
	int start_col =0;

	int end_row = 0;
	int end_col = 0;
	// 扩充
	int end_row_ex = 0;
	int end_col_ex = 0;

	int block_height = 0;
	int block_width = 0;
	while (i < height - 1) {
		j = 3;
		while (j < width - 1)
		{
			count_cure++;
			// 计算起始行列数
			start_row = i;
			start_col = j;

			end_row = start_row + dem_block_size;
			end_col = start_col + dem_block_size;
			// 扩充
			end_row_ex = end_row + 3;
			end_col_ex = end_col + 3;

			if (end_row_ex > height) {
				end_row = height - 3;
				end_row_ex = height;
			}
			if (end_col_ex > width) {
				end_col = width - 3;
				end_col_ex = width;
			}
			
			block_height = end_row - start_row + 6;
			block_width = end_col - start_col + 6;
			poolPtr->append(std::bind(taskFunc, paras, dem_block_size, width, sim_dem, count_cure, count_sum,
				i, j, block_height, block_width, end_row_ex, end_col_ex, demarray, gtt, inv_gtt));
			thread_ids++;
			j = j + dem_block_size;
		}
		i = i + dem_block_size;
	}

	std::cout << "SimProcess  doing"<<std::endl;
	poolPtr->waiteFinish();
	// 输出文件
	std::cout << "输出成果文件:" << getCurrentTimeString() << std::endl;
 
	GDALDriver* poDriver = GetGDALDriverManager()->GetDriverByName("GTiff");
	const char* out_sar_sim = paras.out_sar_sim_path.c_str();

	GDALDataset* poDstDS = poDriver->Create(out_sar_sim,  paras.sim_width, paras.sim_height + 3000, 1, GDT_Int16, NULL);
	poDstDS->GetRasterBand(1)->RasterIO(GF_Write, 0, 0,  paras.sim_width, paras.sim_height + 3000, sim_dem, paras.sim_width, paras.sim_height + 3000, GDT_Int16, 0, 0);
	GDALFlushCache(poDstDS);
	GDALClose(poDstDS);
	free(sim_dem);
	GDALClose(demDataset);
	delete  demarray;
	std::cout << "输出成果文件-over" << std::endl;
 
	std::cout << "end time:" << getCurrentTimeString() << std::endl;
	return 0;
}

int SimProcess_Calsim2ori(ParameterInFile paras, ConvertResampleParameter conveparas, int thread_num)
{
	std::cout << "begin time:" << getCurrentTimeString() << std::endl;
	GDALAllRegister();// 注册格式的驱动
	// 打开DEM影像
	GDALDataset* demDataset = (GDALDataset*)(GDALOpen(conveparas.in_ori_dem_path.c_str(), GA_ReadOnly));//以只读方式读取地形影像
	int width = demDataset->GetRasterXSize();
	int height = demDataset->GetRasterYSize();
	int nCount = demDataset->GetRasterCount();
	double* gt = (double*)calloc(6, sizeof(double)); // 仿射矩阵
	demDataset->GetGeoTransform(gt); // 获得仿射矩阵
	translateArray gtt{ gt[0],gt[1],gt[2],gt[3],gt[4],gt[5] };
	double*inv_gt = (double*)calloc(6, sizeof(double)); // 仿射矩阵
	GDALInvGeoTransform(gt, inv_gt); // 逆矩阵
	translateArray inv_gtt{ inv_gt[0],inv_gt[1],inv_gt[2],inv_gt[3],inv_gt[4],inv_gt[5] };
	GDALDataType gdal_datatype = demDataset->GetRasterBand(1)->GetRasterDataType();
	const char* Projection = demDataset->GetProjectionRef();
	GDALRasterBand* demBand = demDataset->GetRasterBand(1);
	float* demarray = ReadRasterArray(demBand, width, height, gdal_datatype);
	int dem_pixel_count = width * height;
	int psize = width * height;
	float* sar_local_angle = (float*)calloc(psize, sizeof(float));
	float* sar_angle = (float*)calloc(psize, sizeof(float));
	float* sar_dem_x = (float*)calloc(psize, sizeof(float));
	float* sar_dem_y = (float*)calloc(psize, sizeof(float));

	for (int i = 0; i < psize; i++) {
		sar_dem_x[i] = -100000;
		sar_dem_y[i] = -100000;
		sar_local_angle[i] = -9999;
		sar_angle[i] = -9999;
	}

	int blocksize =1000 ; // 获取每块dem的尺寸
	int tempstate = 0;
	ThreadPool* subthreadPool = new ThreadPool(thread_num, true);
	ThreadPoolPtr poolPtr(subthreadPool);
	poolPtr->start();
	for (int start_row = 1; start_row <height-2; ) {
		int end_row = (start_row + blocksize >= height-2 )? height-2 : (start_row + blocksize);

		for(int start_col =1; start_col < width -2;)
		{
			int end_col = (start_col + blocksize >= width-2) ? width-2 : (start_col + blocksize);

			poolPtr->append(std::bind(taskFunc_Calsim2ori, paras, conveparas, 
				start_row, end_row, start_col, end_col,width, height,
				sar_local_angle,sar_angle, sar_dem_x,sar_dem_y,
				demarray, gtt, inv_gtt));
			start_col = end_col;
		}
		start_row = end_row;
	}

	std::cout << "doing...." << std::endl;
	poolPtr->waiteFinish();
	// 输出文件;

	std::cout << "输出成果文件:" << getCurrentTimeString() << std::endl;

	GDALDriver* poDriver = GetGDALDriverManager()->GetDriverByName("GTiff");
	const char* out_sar_sim = paras.out_sar_sim_path.c_str();
	
	poolPtr->stop();
	GDALClose(demDataset);

	delete  demarray, demBand, subthreadPool;

	out_sar_sim = conveparas.ori_sim.c_str();
	GDALDataset* poDstDS = poDriver->Create(out_sar_sim, width, height,  2, GDT_Float32, NULL);
	poDstDS->SetGeoTransform(gt);
	poDstDS->SetProjection(Projection);
	poDstDS->GetRasterBand(1)->RasterIO(GF_Write, 0, 0, width, height, sar_dem_x, width, height, GDT_Float32, 0, 0);
	poDstDS->GetRasterBand(1)->SetNoDataValue(-100000);
	poDstDS->GetRasterBand(2)->RasterIO(GF_Write, 0, 0, width, height, sar_dem_y, width, height, GDT_Float32, 0, 0);
	poDstDS->GetRasterBand(2)->SetNoDataValue(-100000);
	GDALFlushCache(poDstDS);
	GDALClose(poDstDS);

	poDstDS = poDriver->Create(conveparas.out_orth_sar_local_incidence_path.c_str(), width, height, 1, GDT_Float32, NULL);
	poDstDS->GetRasterBand(1)->RasterIO(GF_Write, 0, 0, width, height, sar_local_angle, width, height, GDT_Float32, 0, 0);
	poDstDS->GetRasterBand(1)->SetNoDataValue(-9999);
	poDstDS->SetGeoTransform(gt);
	poDstDS->SetProjection(Projection);
	GDALFlushCache(poDstDS);
	GDALClose(poDstDS);

	poDstDS = poDriver->Create(conveparas.out_orth_sar_incidence_path.c_str(), width, height, 1, GDT_Float32, NULL);
	poDstDS->GetRasterBand(1)->RasterIO(GF_Write, 0, 0, width, height, sar_angle, width, height, GDT_Float32, 0, 0);
	poDstDS->GetRasterBand(1)->SetNoDataValue(-9999);
	poDstDS->SetGeoTransform(gt);
	poDstDS->SetProjection(Projection);
	GDALFlushCache(poDstDS);
	GDALClose(poDstDS);

	std::cout << "输出成果文件-over" << std::endl;

	std::cout << "end time:" << getCurrentTimeString() << std::endl;
	free(sar_dem_x);
	free(sar_dem_y);
	free(sar_local_angle);
	free(sar_angle);
	free(gt);
	free(inv_gt);
	return 0;
}

/*
11 12 13 14
21 22 23 24
31 32 33 34
41 42 43 44
*/
//分块重采样
int orth_ReSampling(int start_row,int end_row,int start_col,int end_col,
	int width,int height,int ori_width,int ori_height ,
	float* ori_array, float* result_arr, float* ori_r_array, float* ori_c_array) {
	for (int i = start_row; i < end_row; i++) {
		for (int j = start_col; j < end_col; j++) {
			int ori_ids = i * width + j;
			int p_ids = 0;
			point p{ ori_r_array[ori_ids] ,ori_c_array[ori_ids],0 };
			point p22 { floor(ori_r_array[ori_ids]),floor(ori_c_array[ori_ids]),0 };
			if (p22.x < 0 || p22.y < 0 || p22.x >= ori_height - 1 || p22.y >= ori_width - 1) {
				continue;
			}
			else if (p22.x < 1 || p22.y < 1 || p22.x >= ori_height - 2 || p22.y >= ori_width - 2) {
				//双线性
				point p23{ p22.x	,p22.y + 1,0 }; p_ids = p23.x * ori_width + p23.y; p23.z = ori_array[p_ids];
				point p32{ p22.x + 1,p22.y	  ,0 }; p_ids = p32.x * ori_width + p32.y; p32.z = ori_array[p_ids];
				point p33{ p22.x + 1,p22.y + 1,0 }; p_ids = p33.x * ori_width + p33.y; p33.z = ori_array[p_ids];
				p.x = p.x - p22.x;
				p.y = p.y - p22.y;
				p=bilineadInterpolation(p, p22, p23, p32, p33);
				result_arr[ori_ids] = p.z;
			}
			else { // p22.x >= 1 && p22.y >= 1 && p22.x < ori_height - 2&& p22.y < ori_width - 2
				// 三次
				point p11{ p22.x - 1,p22.y - 1,0 }; p_ids = p11.x * ori_width + p11.y; p11.z = ori_array[p_ids];
				point p12{ p22.x - 1,p22.y    ,0 }; p_ids = p12.x * ori_width + p12.y; p12.z = ori_array[p_ids];
				point p13{ p22.x - 1,p22.y + 1,0 }; p_ids = p13.x * ori_width + p13.y; p13.z = ori_array[p_ids];
				point p14{ p22.x - 1,p22.y + 2,0 }; p_ids = p14.x * ori_width + p14.y; p14.z = ori_array[p_ids];

				point p21{ p22.x	,p22.y - 1,0 }; p_ids = p21.x * ori_width + p21.y; p21.z = ori_array[p_ids];
				point p23{ p22.x	,p22.y + 1,0 }; p_ids = p23.x * ori_width + p23.y; p23.z = ori_array[p_ids];
				point p24{ p22.x	,p22.y + 2,0 }; p_ids = p24.x * ori_width + p24.y; p24.z = ori_array[p_ids];

				point p31{ p22.x + 1,p22.y - 1,0 }; p_ids = p31.x * ori_width + p31.y; p31.z = ori_array[p_ids];
				point p32{ p22.x + 1,p22.y	  ,0 }; p_ids = p32.x * ori_width + p32.y; p32.z = ori_array[p_ids];
				point p33{ p22.x + 1,p22.y + 1,0 }; p_ids = p33.x * ori_width + p33.y; p33.z = ori_array[p_ids];
				point p34{ p22.x + 1,p22.y + 2,0 }; p_ids = p34.x * ori_width + p34.y; p34.z = ori_array[p_ids];

				point p41{ p22.x + 2,p22.y - 1,0 }; p_ids = p41.x * ori_width + p41.y; p41.z = ori_array[p_ids];
				point p42{ p22.x + 2,p22.y	  ,0 }; p_ids = p42.x * ori_width + p42.y; p42.z = ori_array[p_ids];
				point p43{ p22.x + 2,p22.y + 1,0 }; p_ids = p43.x * ori_width + p43.y; p43.z = ori_array[p_ids];
				point p44{ p22.x + 2,p22.y + 2,0 }; p_ids = p44.x * ori_width + p44.y; p44.z = ori_array[p_ids];
				p.x = p.x - p11.x;
				p.y = p.y - p11.y;
				p = cubicInterpolation(p, p11, p12, p13, p14, p21, p22, p23, p24, p31, p32, p33, p34, p41, p42, p43, p44);
				result_arr[ori_ids] = p.z;
			}
		}
	}

	//std::cout << "\r" << end_row << "/" << height << "," << end_col << "/" << width << ","<<end_row*end_col*100.0/(width*height) << "------------------";
	return 0;
}

int orth_pow(int start_row, int end_row, int start_col, int end_col,
	int width, int height, 
	float* result_arr, float* ori_a_array, float* ori_b_array) {
	for (int i = start_row; i < end_row; i++) {
		for (int j = start_col; j < end_col; j++) {
			int ori_ids = i * width + j;
			result_arr[ori_ids] = log10(ori_a_array[ori_ids] * ori_a_array[ori_ids] + ori_b_array[ori_ids] * ori_b_array[ori_ids]+1)*10;
		}
	}

	//std::cout << "\r" << end_row << "/" << height << "," << end_col << "/" << width << ","<<end_row*end_col*100.0/(width*height) << "------------------";
	return 0;
}
/*
重采样对象
*/
int SimProcess_ResamplingOri2Orth(ParameterInFile paras, ConvertResampleParameter conveparas, int thread_num)
{
	std::cout << "begin time:" << getCurrentTimeString() << std::endl;
	GDALAllRegister();// 注册格式的驱动
	// 打开DEM影像
	GDALDataset* sim2oriDataset = (GDALDataset*)(GDALOpen(conveparas.ori_sim.c_str(), GA_ReadOnly));//以只读方式读取地形影像
	int width = sim2oriDataset->GetRasterXSize();
	int height = sim2oriDataset->GetRasterYSize();
	int nCount = sim2oriDataset->GetRasterCount();
	double*gt = (double*)calloc(6, sizeof(double)); // 仿射矩阵
	sim2oriDataset->GetGeoTransform(gt); // 获得仿射矩阵
	translateArray gtt{ gt[0],gt[1],gt[2],gt[3],gt[4],gt[5] };
	double*inv_gt = (double*)calloc(6, sizeof(double)); // 仿射矩阵
	GDALInvGeoTransform(gt, inv_gt); // 逆矩阵
	translateArray inv_gtt{ inv_gt[0],inv_gt[1],inv_gt[2],inv_gt[3],inv_gt[4],inv_gt[5] };
	GDALDataType gdal_datatype = sim2oriDataset->GetRasterBand(1)->GetRasterDataType();
	const char* Projection = sim2oriDataset->GetProjectionRef();
	GDALRasterBand* ori_r_Band = sim2oriDataset->GetRasterBand(1);
	float* ori_r_array = ReadRasterArray(ori_r_Band, width, height, gdal_datatype);
	GDALRasterBand* ori_c_Band = sim2oriDataset->GetRasterBand(2);
	float* ori_c_array = ReadRasterArray(ori_c_Band, width, height, gdal_datatype);
	// 读取行列号
	delete ori_r_Band, ori_c_Band;
	

	for (int t = 0; t < conveparas.file_count; t++) {
		// 读
		// 创建
		GDALAllRegister();// 注册格式的驱动
		GDALDriver* poDriver = GetGDALDriverManager()->GetDriverByName("GTiff");
		string in_file_path = conveparas.inputFile_paths[t];
		string out_file_path = conveparas.outFile_paths[t];

		GDALDataset* inDataset = (GDALDataset*)(GDALOpen(in_file_path.c_str(), GA_ReadOnly));
		int orth_width = inDataset->GetRasterXSize();
		int orth_height = inDataset->GetRasterYSize();
		int orth_num = inDataset->GetRasterCount();
		// 写

		GDALDataset* outDstDS = poDriver->Create(out_file_path.c_str(), width, height, orth_num, GDT_Float32, NULL);
		outDstDS->SetGeoTransform(gt);
		outDstDS->SetProjection(Projection);


		float* result_arr = (float*)calloc(width * height, sizeof(float));
		int psize = width * height;
		int blocksize = 1000;
		for (int b = 1; b <= orth_num; b++) {
			for (int i = 0; i < psize; i++) {
				result_arr[i] = -9999;
			}
			GDALRasterBand* ori_Band = inDataset->GetRasterBand(b);
			float* ori_arr= ReadRasterArray(ori_Band, orth_width, orth_height, gdal_datatype);

			// 线程池
			ThreadPool* subthreadPool = new ThreadPool(thread_num, true);
			ThreadPoolPtr poolPtr(subthreadPool);
			poolPtr->start();


			// 三次样条重采样
			for (int start_row = 0; start_row < height; start_row = start_row + blocksize) {
				int end_row = start_row + blocksize >= height ? height : start_row + blocksize;
				for (int start_col = 0; start_col < width; start_col = start_col + blocksize) {
					int end_col = start_col + blocksize >= width ? width : start_col + blocksize;
					// 分块计算结果
					poolPtr->append(std::bind(orth_ReSampling,start_row,end_row,start_col,end_col,width,height, orth_width, orth_height, 
						ori_arr, result_arr, ori_r_array, ori_c_array));
				}
			}

			std::cout << "resampling...." << std::endl;
			poolPtr->waiteFinish();
			// 输出文件
			std::cout<<t<<"  ， " << b << ",输出成果文件:" << getCurrentTimeString() << std::endl;
			outDstDS->GetRasterBand(b)->RasterIO(GF_Write, 0, 0, width, height, result_arr, width, height, GDT_Float32, 0, 0);
			outDstDS->GetRasterBand(b)->SetNoDataValue(-9999);
			poolPtr->stop();
			delete ori_arr, ori_Band, subthreadPool;
		}
		free(result_arr);
		GDALFlushCache(outDstDS);
		GDALClose(outDstDS);
		GDALClose(inDataset);
		
	}
	free(gt);
	free(inv_gt);
	delete ori_r_array, ori_c_array;// , sim2oriDataset;
	//GDALClose(sim2oriDataset);
	return 0;
}

/// <summary>
/// 生成影像的强度图
/// </summary>
/// <param name="paras"></param>
/// <param name="conveparas"></param>
/// <param name="thread_num"></param>
/// <returns></returns>
int SimProcess_Calspow(ParameterInFile paras, ConvertResampleParameter conveparas, int thread_num)
{
	for (int t = 0; t < conveparas.file_count; t++) {
		// 读
		// 创建
		GDALAllRegister();// 注册格式的驱动
		GDALDriver* poDriver = GetGDALDriverManager()->GetDriverByName("GTiff");
		string in_file_path = conveparas.outFile_paths[t];
		string out_file_path = conveparas.outFile_pow_paths[t];

		GDALDataset* inDataset = (GDALDataset*)(GDALOpen(in_file_path.c_str(), GA_ReadOnly));
		int width = inDataset->GetRasterXSize();
		int height = inDataset->GetRasterYSize();
		int num = inDataset->GetRasterCount();
		double*gt = (double*)calloc(6, sizeof(double)); // 仿射矩阵
		inDataset->GetGeoTransform(gt); // 获得仿射矩阵
		translateArray gtt{ gt[0],gt[1],gt[2],gt[3],gt[4],gt[5] };
		double*inv_gt = (double*)calloc(6, sizeof(double)); // 仿射矩阵
		GDALInvGeoTransform(gt, inv_gt); // 逆矩阵
		translateArray inv_gtt{ inv_gt[0],inv_gt[1],inv_gt[2],inv_gt[3],inv_gt[4],inv_gt[5] };
		const char* Projection = inDataset->GetProjectionRef();

		GDALDataset* outDstDS = poDriver->Create(out_file_path.c_str(), width, height, 1, GDT_Float32, NULL);
		outDstDS->SetGeoTransform(gt);
		outDstDS->SetProjection(Projection);
		GDALDataType gdal_datatype = inDataset->GetRasterBand(1)->GetRasterDataType();
		
		GDALRasterBand* ori_Band = inDataset->GetRasterBand(1);
		float* ori_a_array = ReadRasterArray(ori_Band, width, height, gdal_datatype);
		ori_Band = inDataset->GetRasterBand(2);
		float* ori_b_array = ReadRasterArray(ori_Band, width, height, gdal_datatype);



		float* result_arr = (float*)calloc(width * height, sizeof(float));
		int psize = width * height;
		int blocksize = 1000;
			for (int i = 0; i < psize; i++) {
				result_arr[i] = -9999;
			}

			// 线程池
			ThreadPool* subthreadPool = new ThreadPool(thread_num, true);
			ThreadPoolPtr poolPtr(subthreadPool);
			poolPtr->start();


			// 三次样条重采样
			for (int start_row = 0; start_row < height; start_row = start_row + blocksize) {
				int end_row = start_row + blocksize >= height ? height : start_row + blocksize;
				for (int start_col = 0; start_col < width; start_col = start_col + blocksize) {
					int end_col = start_col + blocksize >= width ? width : start_col + blocksize;
					// 分块计算结果
					poolPtr->append(std::bind(orth_pow, start_row, end_row, start_col, end_col, width, height,
						 result_arr, ori_a_array, ori_b_array));
				}
			}

			std::cout << "pow resampling ..." << std::endl;
			poolPtr->waiteFinish();
			// 输出文件
			std::cout << t << "  ， " << 1 << ",输出成果文件:" << getCurrentTimeString() << std::endl;
			outDstDS->GetRasterBand(1)->RasterIO(GF_Write, 0, 0, width, height, result_arr, width, height, GDT_Float32, 0, 0);
			outDstDS->GetRasterBand(1)->SetNoDataValue(-9999);
			poolPtr->stop();
			delete ori_a_array, ori_b_array, ori_Band, subthreadPool;
			free(gt);
			free(inv_gt);
			free(result_arr);
			//GDALFlushCache(outDstDS);
			//GDALClose(outDstDS);
			//GDALClose(inDataset);
			//delete poDriver;

	}

	//GDALClose(sim2oriDataset);
	return 0;
}

int ResamplingSim(ParameterInFile paras)
{
	GDALAllRegister();
	// 打开DEM影像
	GDALDataset* simDataset = (GDALDataset*)(GDALOpen(paras.out_sar_sim_path.c_str(), GA_ReadOnly));//以只读方式读取地形影像
	int ori_sim_width = simDataset->GetRasterXSize();
	int ori_sim_height = simDataset->GetRasterYSize();
	int width = paras.sim_width;
	int height = paras.sim_height;
	float width_scales = width * 1.0 / ori_sim_width;
	float height_scales = height * 1.0 / ori_sim_height;

	GDALClose(simDataset);
	ResampleGDAL(paras.out_sar_sim_path.c_str(), paras.out_sar_sim_resampling_path.c_str(), width_scales, height_scales, GDALResampleAlg::GRA_CubicSpline);
	return 0;
}

int ResampleGDAL(const char* pszSrcFile, const char* pszOutFile, float fResX = 1.0, float fResY = 1.0, GDALResampleAlg eResample = GRA_Bilinear)
{
	GDALAllRegister();
	CPLSetConfigOption("GDAL_FILENAME_IS_UTF8", "NO");
	GDALDataset* pDSrc = (GDALDataset*)GDALOpen(pszSrcFile, GA_ReadOnly);
	if (pDSrc == NULL)
	{
		return -1;
	}


	GDALDriver* pDriver = GetGDALDriverManager()->GetDriverByName("GTiff");
	if (pDriver == NULL)
	{
		GDALClose((GDALDatasetH)pDSrc);
		return -2;
	}
	int width = pDSrc->GetRasterXSize();
	int height = pDSrc->GetRasterYSize();
	int nBandCount = pDSrc->GetRasterCount();
	GDALDataType dataType = GDALDataType::GDT_Float32;// pDSrc->GetRasterBand(1)->GetRasterDataType();


	char* pszSrcWKT = NULL;
	pszSrcWKT = const_cast<char*>(pDSrc->GetProjectionRef());


	double dGeoTrans[6] = { 0 };
	int nNewWidth = width, nNewHeight = height;
	pDSrc->GetGeoTransform(dGeoTrans);


	bool bNoGeoRef = false;
	long double dOldGeoTrans0 = dGeoTrans[0];
	//如果没有投影，人为设置一个  
	if (strlen(pszSrcWKT) <= 0)
	{
		//OGRSpatialReference oSRS;
		//oSRS.SetUTM(50,true);	//北半球  东经120度
		//oSRS.SetWellKnownGeogCS("WGS84");
		//oSRS.exportToWkt(&pszSrcWKT);
		//pDSrc->SetProjection(pszSrcWKT);


		//////////////////////////////////////////////////////////////////////////
		dGeoTrans[0] = 1.0;
		pDSrc->SetGeoTransform(dGeoTrans);
		//////////////////////////////////////////////////////////////////////////


		bNoGeoRef = true;
	}


	//adfGeoTransform[0] /* top left x */
	//adfGeoTransform[1] /* w-e pixel resolution */
	//adfGeoTransform[2] /* rotation, 0 if image is "north up" */
	//adfGeoTransform[3] /* top left y */
	//adfGeoTransform[4] /* rotation, 0 if image is "north up" */
	//adfGeoTransform[5] /* n-s pixel resolution */


	dGeoTrans[1] = dGeoTrans[1] / fResX;
	dGeoTrans[5] = dGeoTrans[5] / fResY;
	nNewWidth = static_cast<int>(nNewWidth * fResX + 0.5);
	nNewHeight = static_cast<int>(nNewHeight * fResY + 0.5);


	//创建结果数据集
	GDALDataset* pDDst = pDriver->Create(pszOutFile, nNewWidth, nNewHeight, nBandCount, dataType, NULL);
	if (pDDst == NULL)
	{
		GDALClose((GDALDatasetH)pDSrc);
		return -2;
	}

	pDDst->SetProjection(pszSrcWKT);
	pDDst->SetGeoTransform(dGeoTrans);


	void* hTransformArg = NULL;
	hTransformArg = GDALCreateGenImgProjTransformer2((GDALDatasetH)pDSrc, (GDALDatasetH)pDDst, NULL); //GDALCreateGenImgProjTransformer((GDALDatasetH) pDSrc,pszSrcWKT,(GDALDatasetH) pDDst,pszSrcWKT,FALSE,0.0,1);


	if (hTransformArg == NULL)
	{
		GDALClose((GDALDatasetH)pDSrc);
		GDALClose((GDALDatasetH)pDDst);
		return -3;
	}

	GDALWarpOptions* psWo = GDALCreateWarpOptions();


	psWo->papszWarpOptions = CSLDuplicate(NULL);
	psWo->eWorkingDataType = dataType;
	psWo->eResampleAlg = eResample;


	psWo->hSrcDS = (GDALDatasetH)pDSrc;
	psWo->hDstDS = (GDALDatasetH)pDDst;


	psWo->pfnTransformer = GDALGenImgProjTransform;
	psWo->pTransformerArg = hTransformArg;


	psWo->nBandCount = nBandCount;
	psWo->panSrcBands = (int*)CPLMalloc(nBandCount * sizeof(int));
	psWo->panDstBands = (int*)CPLMalloc(nBandCount * sizeof(int));
	for (int i = 0; i < nBandCount; i++)
	{
		psWo->panSrcBands[i] = i + 1;
		psWo->panDstBands[i] = i + 1;
	}


	GDALWarpOperation oWo;
	if (oWo.Initialize(psWo) != CE_None)
	{
		GDALClose((GDALDatasetH)pDSrc);
		GDALClose((GDALDatasetH)pDDst);
		return -3;
	}


	oWo.ChunkAndWarpImage(0, 0, nNewWidth, nNewHeight);


	GDALDestroyGenImgProjTransformer(hTransformArg);
	GDALDestroyWarpOptions(psWo);
	if (bNoGeoRef)
	{
		dGeoTrans[0] = dOldGeoTrans0;
		pDDst->SetGeoTransform(dGeoTrans);
		//pDDst->SetProjection("");
	}
	GDALFlushCache(pDDst);
	GDALClose((GDALDatasetH)pDSrc);
	GDALClose((GDALDatasetH)pDDst);
	return 0;
}

/// <summary>
///  根据地面坐标获取计算结果
/// </summary>
/// <param name="landp"></param>
/// <param name="paras"></param>
/// <param name="convparas"></param>
/// <returns></returns>
point GetOriRC(point& landp, ParameterInFile& paras, ConvertResampleParameter& convparas)
{
	long double Starttime = paras.imgStartTime;
	long double lamda = paras.lamda;
	long double refrange = paras.refrange;
	long double* doppler_para = paras.doppler_para;
	long double LightSpeed = paras.LightSpeed;
	long double delta_t = paras.delta_t;
	long double R0 = paras.R0;
	long double delta_R = paras.delta_R;
	long double SatelliteModelStartTime = paras.SatelliteModelStartTime;
	long double* polySatellitePara = paras.polySatellitePara;
	int sim_sar_width = paras.sim_width;
	int sim_sar_height = paras.sim_height;
	int polynum = paras.polynum;
	int doppler_paramenter_number = paras.doppler_paramenter_number;

	point pixelpoint = PSTN(landp, paras.imgStartTime, paras.lamda, paras.refrange, paras.doppler_para, paras.LightSpeed, paras.PRF, paras.R0, paras.delta_R, paras.SatelliteModelStartTime, paras.polySatellitePara, paras.polynum, paras.doppler_paramenter_number);
	long double r, c;
	sim2ori(pixelpoint.x, pixelpoint.y, r, c, convparas.sim2ori_paras);
	return point{ r,c,0 };
}

// 根据坐标变换求解行列号
int calSARPosition(ParameterInFile paras, ConvertResampleParameter converPara) {
	//

	return 0;

}


///
/// 测试代码
/// 

int testPP() {

	point landpoint;
	landpoint.x = -1945072.5;
	landpoint.y = 5344083.0;
	landpoint.z = 2878316.0;
	point targetpoint;
	targetpoint.x = 31;
	targetpoint.y = 118;
	targetpoint.z = 33;

	landpoint = LLA2XYZ(targetpoint);
	std::cout << "PTSN\nX：" << landpoint.x<< "\nY:" << landpoint.y  << "\nZ：" << landpoint.z << "\n"; // <0.1 
	return 0;
}

int testPTSN(ParameterInFile paras) {
	point landpoint;
	landpoint.x = -1945072.5;
	landpoint.y = 5344083.0;
	landpoint.z = 2878316.0;
	point targetpoint;
	targetpoint.x = 31;
	targetpoint.y = 118;
	targetpoint.z = 1592144812.73686337471008300781;

	landpoint=LLA2XYZ(targetpoint);
	point pixelpoint = PSTN(landpoint, paras.imgStartTime, paras.lamda, paras.refrange, paras.doppler_para, paras.LightSpeed, paras.PRF, paras.R0, paras.delta_R, paras.SatelliteModelStartTime, paras.polySatellitePara, paras.polynum, paras.doppler_paramenter_number);
	std::cout << "PTSN\nX：" << targetpoint.x - pixelpoint.x << "\nY:" << targetpoint.y - pixelpoint.y << "\nZ：" << targetpoint.z - pixelpoint.z << "\n"; // <0.1 

	SatelliteSpacePoint sateP = getSatellitePostion(1592144812.7368634, paras.SatelliteModelStartTime, paras.polySatellitePara, paras.polynum);
	SatelliteSpacePoint sateT{ -3044863.2137617283, 5669112.1303918585, 3066571.1073331647, -26.15659591374557, 3600.8531673370803, -6658.59211430739 };
	std::cout << "Sate\nX：" << sateP.x - sateT.x << "\nY:" << sateP.y - sateT.y << "\nZ：" << sateP.z - sateT.z << "\nVx" << sateP.vx - sateT.vx << "\nVy:" << sateP.vy - sateT.vy << "\nVz:" << sateP.vz - sateT.vz << "\n";
	return 0;
}