#pragma once
///
/// 仿真成像算法
/// 

//#define EIGEN_USE_MKL_ALL
//#define EIGEN_VECTORIZE_SSE4_2
//#include <mkl.h>
// 本地方法

#include <iostream>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <time.h>
#include <boost/filesystem.hpp>
#include <omp.h>
#include "baseTool.h"
#include "simptsn.h"
#include "SateOrbit.h"
#include "ImageMatch.h"
#include <gdal_utils.h>
#include <proj.h>
#include "gdal_priv.h"
#include "gdal_alg.h"
#include <gdal_priv.h>
#include <iostream>
#include <fstream>
#include <string>
#include <vector>
#include <gdalgrid.h>

using namespace std;
using namespace Eigen;



PSTNAlgorithm::PSTNAlgorithm()
{
}

PSTNAlgorithm::PSTNAlgorithm(string infile_path)
{
	std::cout << "=========================================================================" << endl;
	std::cout << "read  parameters :\t" << infile_path << endl;
	std::cout << "parameters files :\t" << infile_path << endl;
	std::cout << "params file Format:" << endl;
	std::cout << "height --- image height" << endl;
	std::cout << "width --- image width" << endl;
	std::cout << "R0  --- image nearRange" << endl;
	std::cout << "near_in_angle " << endl;
	std::cout << "far incident angle " << endl;
	std::cout << "LightSpeed " << endl;
	std::cout << "lamda" << endl;
	std::cout << "imgStartTime" << endl;
	std::cout << "PRF" << endl;
	std::cout << "refrange" << endl;
	std::cout << "fs" << endl;
	std::cout << "doppler_reference_time" << endl;
	std::cout << "doppler_paramenter_number " << endl;
	std::cout << "doppler_paramenter 1 " << endl;
	std::cout << "doppler_paramenter 2" << endl;
	std::cout << "doppler_paramenter 3 " << endl;
	std::cout << "doppler_paramenter 4 " << endl;
	std::cout << "doppler_paramenter 5 " << endl;
	std::cout << "ispolySatelliteModel" << endl;
	std::cout << "sate polynum " << endl;
	std::cout << "SatelliteModelStartTime" << endl;
	std::cout << "orbit params list " << endl;
	std::cout << "============================================================================" << endl;

	// 解析文件
	ifstream infile(infile_path, ios::in);
	if (!infile.is_open()) {
		throw "参数文件未打开";
	}
	try {
		int line_ids = 0;
		string buf;

		// 参数域
		getline(infile, buf); this->height = stoi(buf);
		getline(infile, buf); this->width = stoi(buf);			std::cout << "height,width\t" << this->height << "," << this->width << endl;
		getline(infile, buf); this->R0 = stod(buf);				std::cout <<""<< "R0\t" << this->R0 << endl; //近斜距
		getline(infile, buf); this->near_in_angle = stod(buf);	std::cout << "near_in_angle\t" << this->near_in_angle << endl; //近入射角
		getline(infile, buf); this->far_in_angle = stod(buf);	std::cout << "far_in_angle\t" << this->far_in_angle << endl; //远入射角

		getline(infile, buf); this->LightSpeed = stod(buf);		std::cout << "LightSpeed\t" << this->LightSpeed << endl;
		getline(infile, buf); this->lamda = stod(buf);			std::cout << "lamda\t" << this->lamda << endl;
		getline(infile, buf); this->imgStartTime = stod(buf);	std::cout << "imgStartTime\t" << this->imgStartTime << endl;
		getline(infile, buf); this->PRF = stod(buf);			std::cout << "PRF\t" << this->PRF << endl;
		getline(infile, buf); this->refrange = stod(buf);		std::cout << "refrange\t" << this->refrange << endl;
		getline(infile, buf); this->fs = stod(buf);				std::cout << "fs\t" << this->fs << endl;
		getline(infile, buf); this->doppler_reference_time = stod(buf);				std::cout << "doppler_reference_time\t" << this->doppler_reference_time << endl;
		getline(infile, buf); this->doppler_paramenter_number = stoi(buf); std::cout << "doppler_paramenter_number\t" << this->doppler_paramenter_number << endl;

		// 读取多普勒系数
		this->doppler_paras = Eigen::MatrixX<double>(this->doppler_paramenter_number, 1);
		if (this->doppler_paramenter_number > 0) {
			for (int i = 0; i < this->doppler_paramenter_number; i++) {
				getline(infile, buf);
				this->doppler_paras(i, 0) = stod(buf);
			}
			std::cout << "doppler_paramenter:\n" << this->doppler_paras << endl;
		}
		else {
			throw "内存不足";
		}
		// 读取卫星轨道
		getline(infile, buf);  std::cout << "ispolySatelliteModel\t" << buf << endl;
		if (stoi(buf) != 1) {
			throw "不是多项式轨道模型";
		}
		getline(infile, buf); int polynum = stoi(buf) + 1; // 多项式项数
		getline(infile, buf); double SatelliteModelStartTime = stod(buf); // 轨道模型起始时间

		Eigen::MatrixX<double> polySatellitePara(polynum, 6);
		if (polynum >= 4) {
			for (int i = 0; i < 6; i++) {
				for (int j = 0; j < polynum; j++) {
					getline(infile, buf);
					polySatellitePara(j, i) = stod(buf);
					std::cout << "orbit params " << i << " , " << j << buf << endl;
				}
			}
		}
		else {
			throw "内存不足";
		}
		OrbitPoly orbit(polynum, polySatellitePara, SatelliteModelStartTime);
		this->orbit = orbit;
		std::cout << "sate polynum\t" << polynum << endl;
		std::cout << "sate polySatellitePara\n" << polySatellitePara << endl;
		std::cout << "sate SatelliteModelStartTime\t" << SatelliteModelStartTime << endl;

		if (!infile.eof()) // 获取变换的函数
		{
			double yy, mm, dd;
			getline(infile, buf);
			if (buf != "" || buf.length() > 3) {
				yy = stod(buf);
				getline(infile, buf); mm = stod(buf);
				getline(infile, buf); dd = stod(buf);
				this->UTC = Eigen::Matrix<double, 1, 3>{ yy,mm,dd };
				std::cout << "UTC\t" << this->UTC << endl;
				std::cout << "\nWGS84 to J2000 Params:\t" << endl;
				getline(infile, buf); this->Xp = stod(buf);		std::cout << "Xp\t" << this->Xp << endl;
				getline(infile, buf); this->Yp = stod(buf);		std::cout << "Yp\t" << this->Yp << endl;
				getline(infile, buf); this->Dut1 = stod(buf);	std::cout << "dut1\t" << this->Dut1 << endl;
				getline(infile, buf); this->Dat = stod(buf);	std::cout << "dat\t" << this->Dat << endl;
			}
		}
	}
	catch (exception e) {
		infile.close();
		throw "文件读取错误";
	}
	infile.close();
	std::cout << "============================================================================" << endl;
}

PSTNAlgorithm::~PSTNAlgorithm()
{
}

/// <summary>
/// 将dem坐标转换为经纬文件
/// </summary>
/// <param name="dem_path">dem</param>
/// <param name="lon_path">经度</param>
/// <param name="lat_path">纬度</param>
/// <returns></returns>
int PSTNAlgorithm::dem2lat_lon(std::string dem_path, std::string lon_path, std::string lat_path)
{
	gdalImage dem_img(dem_path);
	//gdalImage lon_img = CreategdalImage(lon_path, dem_img.height, dem_img.width);
	//gdalImage lat_img = CreategdalImage(lat_path, dem_img.height, dem_img.width);


	return 0;
}

int PSTNAlgorithm::dem2SAR_RC(std::string dem_path, std::string sim_rc_path)
{
	return 0;
}


Eigen::MatrixX<double> PSTNAlgorithm::calNumericalDopplerValue(Eigen::MatrixX<double> R)
{
	Eigen::MatrixX<double> t = (R.array() - this->refrange) * (1000000 / LIGHTSPEED);
	Eigen::MatrixX<double> tlist(t.rows(), this->doppler_paramenter_number);// nx5
	tlist.col(0) = Eigen::MatrixX<double>::Ones(t.rows(), 1).col(0);
	for (int i = 1; i < this->doppler_paramenter_number; i++)
	{
		tlist.col(i) = t.array().pow(i);
	}
	//return tlist.array()*this
	return  tlist * this->doppler_paras; //nx5,5x1
	//return Eigen::MatrixX<double>(1, 1);
}


double PSTNAlgorithm::calNumericalDopplerValue(double R)
{
	//double t = (R - this->refrange) * (1e6 / LIGHTSPEED);   GF3
	double t = R / LIGHTSPEED - this->doppler_reference_time;    //S_SAR公式
	//double temp = this->doppler_paras(0) + this->doppler_paras(1) * t + this->doppler_paras(2) * t * t + this->doppler_paras(3) * t * t * t + this->doppler_paras(4) * t * t * t * t; // GF3 HJ
	double temp = this->doppler_paras(0) + this->doppler_paras(1) * t + this->doppler_paras(2) * t * t + this->doppler_paras(3) * t * t * t; // LT 
	return temp;
}


Eigen::MatrixX<double> PSTNAlgorithm::PSTN(Eigen::MatrixX<double> landpoints, double ave_dem = 0, double dt = 0.001, bool isCol = false)
{
	//Eigen::MatrixX<double> a(1, 1);
	//a(0, 0) = 1554386455.999999;
	//Eigen::MatrixX<double> satestatetest = this->orbit.SatelliteSpacePoint(a);
	//std::cout << satestatetest << endl;

	int point_num = landpoints.rows();
	Eigen::MatrixX<double> SARPoint = Eigen::MatrixX<double>::Ones(point_num, 3);
	double L_T = 0;
	double S_T = 0;
	double R_T = 0;
	double cos_theta = 0;

	Eigen::MatrixX<double> landpoint = Eigen::MatrixX<double>::Ones(1, 6);// 卫星状态
	Eigen::MatrixX<double> satestate = Eigen::MatrixX<double>::Ones(1, 6);// 卫星状态
	Eigen::MatrixX<double> satepoint = Eigen::MatrixX<double>::Ones(1, 3);// 卫星坐标
	Eigen::MatrixX<double> satevector = Eigen::MatrixX<double>::Ones(1, 3);// 卫星速度
	Eigen::MatrixX<double> sate_land = Eigen::MatrixX<double>::Ones(1, 3); // 卫星-地面矢量
	double ti = this->orbit.SatelliteModelStartTime;
	double R1 = 0;// 斜距
	double R2 = 0;// 斜距
	double FdTheory1 = 0;
	double FdTheory2 = 0;
	double FdNumberical = 0;
	double inc_t = 0;
	//double delta_angle = (this->far_in_angle - this->near_in_angle) / this->width;
	//std::cout << landpoint(0, 0) << "\t" << landpoint(0, 1) << "\t" << landpoint(0, 2) << endl;
	dt = dt / this->PRF;
	Landpoint satePoints = { 0, 0, 0 };
	Landpoint landPoint = { 0, 0, 0 };
	// 开始迭代
	for (int j = 0; j < point_num; j++) {
		landpoint = landpoints.row(j);
		for (int i = 0; i < 50; i++) {
			satestate = this->orbit.SatelliteSpacePoint(ti + dt);
			satepoint = satestate(Eigen::all, { 0,1,2 });
			satevector = satestate(Eigen::all, { 3,4,5 });
			sate_land = satepoint - landpoint;
			//std::cout << sate_land << endl;
			R1 = (sate_land.array().pow(2).array().rowwise().sum().array().sqrt())[0];;
			FdTheory1 = this->calTheoryDopplerValue(R1, sate_land, satevector);

			satestate = this->orbit.SatelliteSpacePoint(ti);
			satepoint = satestate(Eigen::all, { 0,1,2 });
			satevector = satestate(Eigen::all, { 3,4,5 });
			sate_land = satepoint - landpoint;
			R2 = (sate_land.array().pow(2).array().rowwise().sum().array().sqrt())[0];
			FdNumberical = this->calNumericalDopplerValue(R2);
			FdTheory2 = this->calTheoryDopplerValue(R2, sate_land, satevector);

			inc_t = dt * (FdTheory2 - FdNumberical) / (FdTheory1 - FdTheory2);
			if (abs(FdTheory1 - FdTheory2) < 1e-9) {
				break;
			}
			ti = ti - inc_t;
			if (abs(inc_t) <= dt) {
				break;
			}
		}
		SARPoint(j, 0) = ti;
		SARPoint(j, 1) = (ti - this->imgStartTime) * this->PRF;

		// 尝试使用入射角切分列号
		satestate = this->orbit.SatelliteSpacePoint(ti);
		satepoint = satestate(Eigen::all, { 0,1,2 });
		sate_land = satepoint - landpoint;
		R1 = (sate_land.array().pow(2).array().rowwise().sum().array().sqrt())[0];;
		//SARPoint(j, 2) = getIncAngle(satePoints, landPoint);
		//SARPoint(j, 2) = (R1 - this->R0 - (this->refrange - this->R0) / 3) / this->widthspace;
		
		SARPoint(j, 2) = getRangeColumn(R1, this->R0, this->fs, this->lamda);//    (R1 - this->R0) / this->widthspace; // 
	}
	//std::cout <<"SARPoints(200, 0):\t" << SARPoint(200, 0) << "\t" << SARPoint(200, 1) << "\t" << SARPoint(200, 2) << "\t" << endl;
	return SARPoint;
}

/*
Eigen::MatrixXd PSTNAlgorithm::WGS842J2000(Eigen::MatrixXd blh)
{
	return WGS84_J2000::WGS842J2000(blh, this->UTC, this->Xp, this->Yp, this->Dut1, this->Dat);
	//return Eigen::MatrixXd();
}

Landpoint PSTNAlgorithm::WGS842J2000(Landpoint p)
{
	Eigen::MatrixXd blh = Eigen::MatrixXd(1, 3);
	blh<<p.lon, p.lat, p.ati;
	blh=this->WGS842J2000(blh);
	return Landpoint{ blh(0,0),blh(0,1) ,blh(0,0) };
}
*/

/// <summary>
/// 计算理论多普勒距离
/// </summary>
/// <param name="R">斜距</param>
/// <param name="R_s1">nx3</param>
/// <param name="V_s1">nx3</param>
/// <returns></returns>
Eigen::MatrixX<double> PSTNAlgorithm::calTheoryDopplerValue(Eigen::MatrixX<double> R, Eigen::MatrixX<double> R_s1, Eigen::MatrixX<double> V_s1)
{
	return (R_s1.array() * V_s1.array()).rowwise().sum().array() * (-2) / (R.array() * this->lamda);
	//return Eigen::MatrixX<double>(1, 1);
}

double PSTNAlgorithm::calTheoryDopplerValue(double R, Eigen::MatrixX<double> R_s1, Eigen::MatrixX<double> V_s1) {
	return ((R_s1.array() * V_s1.array()).rowwise().sum().array() * (-2) / (R * this->lamda))[0];
}


simProcess::simProcess()
{

}
simProcess::~simProcess()
{
}
/// <summary>
/// 参数解析
/// </summary>
/// <param name="argc"></param>
/// <param name="argv"></param>
/// <returns></returns>
int simProcess::parameters(int argc, char* argv[])
{
	for (int i = 0; i < argc; i++) {
		std::cout << argv[i] << endl;
	}
	return 0;
}
/// <summary>
/// 间接定位法,初始化参数
/// </summary>
/// <param name="paras_path">参数文件路径</param>
/// <param name="workspace_path">工作空间路径</param>
/// <param name="out_dir_path">输出空间路径</param>
/// <param name="in_dem_path">输入DEM文件路径</param>
/// <param name="in_sar_path">输入SAR影像文件路径</param>
/// <returns>初始化姐u共</returns>
int simProcess::InitSimulationSAR(std::string paras_path, std::string workspace_path, std::string out_dir_path, std::string in_dem_path, std::string in_sar_path)
{

	this->pstn = PSTNAlgorithm(paras_path); // 初始化参数文件
	this->in_dem_path = in_dem_path;
	this->dem_path = JoinPath(workspace_path, "SAR_dem.tiff");// 获取输入DEM 
	this->workSpace_path = workspace_path;
	this->dem_r_path = JoinPath(workspace_path, "dem_r.tiff");
	
	string dem_rcs_path = JoinPath(workspace_path, "dem_rcs.tiff");
	this->sar_sim_wgs_path = JoinPath(workspace_path, "sar_sim_wgs.tiff");
	this->sar_sim_path = JoinPath(workspace_path, "sar_sim.tiff");
	this->ori_sim_count_tiff = JoinPath(workspace_path, "RD_ori_sim_count.tiff");
	this->in_sar_path = in_sar_path;
	this->sar_power_path = JoinPath(workspace_path, "in_sar_power.tiff");
	this->outSpace_path = out_dir_path;
	this->out_dem_slantRange_path = out_dir_path + "\\" + "dem_slantRange.tiff";// 地形斜距
	this->out_plant_slantRange_path = out_dir_path + "\\" + "flat_slantRange.tiff";// 平地斜距
	this->out_dem_rc_path = out_dir_path + "\\" + "WGS_SAR_map.tiff";// 经纬度与行列号映射


	this->out_incidence_path = out_dir_path + "\\" + "incidentAngle.tiff";// 入射角
	this->out_localIncidenct_path = out_dir_path + "\\" + "localincidentAngle.tiff";// 局地入射角
	this->out_ori_sim_tiff = out_dir_path + "\\" + "RD_ori_sim.tif";// 坐标变换
	this->out_sim_ori_tiff = out_dir_path + "\\" + "RD_sim_ori.tif";// 坐标变换
	this->dem_rc_path = this->out_sim_ori_tiff;// JoinPath(out_dir_path, "RD_sim_ori.tif");
	this->sar_sim_wgs_path = JoinPath(out_dir_path, "sar_sim_wgs.tiff");
	this->sar_sim_path = JoinPath(out_dir_path, "sar_sim.tiff");


	this->in_rpc_lon_lat_path = this->out_ori_sim_tiff;
	this->out_inc_angle_rpc_path = out_dir_path + "\\" + "RD_incidentAngle.tiff";// 局地入射角
	this->out_local_inc_angle_rpc_path = out_dir_path + "\\" + "RD_localincidentAngle.tiff";// 局地入射角
	std::cout << "==========================================================================" << endl;
	std::cout << "in_dem_path" << ":\t" << this->in_dem_path << endl;
	std::cout << "in_sar_path" << ":\t" << this->in_sar_path << endl;
	std::cout << "workSpace_path" << ":\t" << this->workSpace_path << endl;
	std::cout << "dem_path" << ":\t" << this->dem_path << endl;
	std::cout << "sar_power_path" << ":\t" << this->sar_power_path << endl;
	std::cout << "dem_r_path" << ":\t" << this->dem_r_path << endl;
	std::cout << "dem_rc_path" << ":\t" << this->dem_rc_path << endl;
	std::cout << "sar_sim_wgs_path" << ":\t" << this->sar_sim_wgs_path << endl;
	std::cout << "sar_sim_path" << ":\t" << this->sar_sim_path << endl;
	std::cout << "outSpace_path" << ":\t" << this->outSpace_path << endl;
	std::cout << "out_dem_slantRange_path" << ":\t" << this->out_dem_slantRange_path << endl;
	std::cout << "out_plant_slantRange_path" << ":\t" << this->out_plant_slantRange_path << endl;
	std::cout << "out_dem_rc_path" << ":\t" << this->out_dem_rc_path << endl;
	std::cout << "out_incidence_path" << ":\t" << this->out_incidence_path << endl;
	std::cout << "out_localIncidenct_path" << ":\t" << this->out_localIncidenct_path << endl;
	std::cout << "==========================================================================" << endl;
	this->CreateSARDEM();
	this->dem2SAR_row(); // 获取行号
	this->SARIncidentAngle();
	//this->SARSimulationWGS();
	//this->SARSimulation();
	this->in_sar_power();
	if (boost::filesystem::exists(boost::filesystem::path(this->out_dem_rc_path))) {
		boost::filesystem::remove(boost::filesystem::path(this->out_dem_rc_path));
	}
	boost::filesystem::copy_file(boost::filesystem::path(this->dem_rc_path), boost::filesystem::path(this->out_dem_rc_path));
	string sim_rcs_jpg_path = JoinPath(workspace_path, "dem_rcs.jpg");
	string sar_rcs_jpg_path = JoinPath(workspace_path, "sar_rcs.jpg");
	//this->createimagematchmodel(this->sar_power_path, sar_rcs_jpg_path, this->sar_sim_path, sim_rcs_jpg_path, this->workspace_path);
	//if (this->ismatchmodel) {
	//	std::cout << "校正" << endl;
	//	this->correctorth_rc(this->dem_rc_path, this->matchmodel);
	//}
	////插值计算行列号
	//this->ReflectTable_WGS2Range();
	//this->SARIncidentAngle_RPC();
	return 0;
}
int simProcess::InitASFSAR(std::string paras_path, std::string workspace_path, std::string out_dir_path, std::string in_dem_path)
{
	this->pstn = PSTNAlgorithm(paras_path); // 初始化参数文件
	this->in_dem_path = in_dem_path;
	this->dem_path = JoinPath(workspace_path, "SAR_dem.tiff");// 获取输入DEM 
	this->workSpace_path = workspace_path;
	this->dem_r_path = JoinPath(workspace_path, "dem_r.tiff");
	this->dem_rc_path = JoinPath(workspace_path, "dem_rc.tiff");
	string dem_rcs_path = JoinPath(workspace_path, "dem_rcs.tiff");
	this->sar_sim_wgs_path = JoinPath(workspace_path, "sar_sim_wgs.tiff");
	this->sar_sim_path = JoinPath(workspace_path, "sar_sim.tiff");
	this->ori_sim_count_tiff = JoinPath(workspace_path, "RD_ori_sim_count.tiff");
	this->in_sar_path = in_sar_path;
	this->sar_power_path = JoinPath(workspace_path, "in_sar_power.tiff");
	this->outSpace_path = out_dir_path;
	this->out_dem_slantRange_path = out_dir_path + "\\" + "dem_slantRange.tiff";// 地形斜距
	this->out_plant_slantRange_path = out_dir_path + "\\" + "flat_slantRange.tiff";// 平地斜距
	this->out_dem_rc_path = out_dir_path + "\\" + "WGS_SAR_map.tiff";// 经纬度与行列号映射
	this->out_incidence_path = out_dir_path + "\\" + "incidentAngle.tiff";// 入射角
	this->out_localIncidenct_path = out_dir_path + "\\" + "localincidentAngle.tiff";// 局地入射角
	this->out_ori_sim_tiff = out_dir_path + "\\" + "RD_ori_sim.tif";// 局地入射角
	this->in_rpc_lon_lat_path = this->out_ori_sim_tiff;
	this->out_inc_angle_rpc_path = out_dir_path + "\\" + "RD_incidentAngle.tiff";// 局地入射角
	this->out_local_inc_angle_rpc_path = out_dir_path + "\\" + "RD_localincidentAngle.tiff";// 局地入射角
	std::cout << "==========================================================================" << endl;
	std::cout << "in_dem_path" << ":\t" << this->in_dem_path << endl;
	std::cout << "in_sar_path" << ":\t" << this->in_sar_path << endl;
	std::cout << "workSpace_path" << ":\t" << this->workSpace_path << endl;
	std::cout << "dem_path" << ":\t" << this->dem_path << endl;
	std::cout << "sar_power_path" << ":\t" << this->sar_power_path << endl;
	std::cout << "dem_rc_path" << ":\t" << this->dem_rc_path << endl;
	std::cout << "sar_sim_wgs_path" << ":\t" << this->sar_sim_wgs_path << endl;
	std::cout << "sar_sim_path" << ":\t" << this->sar_sim_path << endl;
	std::cout << "outSpace_path" << ":\t" << this->outSpace_path << endl;
	std::cout << "out_dem_slantRange_path" << ":\t" << this->out_dem_slantRange_path << endl;
	std::cout << "out_plant_slantRange_path" << ":\t" << this->out_plant_slantRange_path << endl;
	std::cout << "out_dem_rc_path" << ":\t" << this->out_dem_rc_path << endl;
	std::cout << "out_incidence_path" << ":\t" << this->out_incidence_path << endl;
	std::cout << "out_localIncidenct_path" << ":\t" << this->out_localIncidenct_path << endl;
	std::cout << "==========================================================================" << endl;
	this->CreateSARDEM();
	this->dem2SAR_row(); // 获取行号
	this->SARIncidentAngle();
	this->SARSimulationWGS();
	ASFOrthClass asfclass;

	this->SARSimulation();
	this->in_sar_power();
	if (boost::filesystem::exists(boost::filesystem::path(this->out_dem_rc_path))) {
		boost::filesystem::remove(boost::filesystem::path(this->out_dem_rc_path));
	}
	boost::filesystem::copy_file(boost::filesystem::path(this->dem_rc_path), boost::filesystem::path(this->out_dem_rc_path));
	string sim_rcs_jpg_path = JoinPath(workspace_path, "dem_rcs.jpg");
	string sar_rcs_jpg_path = JoinPath(workspace_path, "sar_rcs.jpg");
	//this->createImageMatchModel(this->sar_power_path, sar_rcs_jpg_path, this->sar_sim_path, sim_rcs_jpg_path, this->workSpace_path);
	if (this->isMatchModel) {
		std::cout << "校正" << endl;
		//	this->correctOrth_rc(this->dem_rc_path, this->matchmodel);
	}

	// 插值计算行列号
	this->ReflectTable_WGS2Range();
	this->SARIncidentAngle_RPC();
	return 0;
	return 0;
}
/// <summary>
/// 创建SAR对应的DEM
/// </summary>
/// <returns>返回结果</returns>
int simProcess::CreateSARDEM()
{
	std::cout << "dem2SAR_Row_col begin time:" << getCurrentTimeString() << std::endl;
	{
		gdalImage dem(this->in_dem_path);
		double dem_mean = dem.mean();
		gdalImage sim_rc = CreategdalImage(this->dem_rc_path, dem.height, dem.width, 2, dem.gt, dem.projection);
		DemBox box{ 9999,9999,-9999,-9999 };
		{
			Eigen::MatrixXd sim_r;
			Eigen::MatrixXd sim_c;
			int line_invert = int(2000000 / dem.width);
			line_invert = line_invert > 10 ? line_invert : 10;
			int max_rows_ids = 0;
			int all_count = 0;
			bool state = true;
			omp_lock_t lock;
			omp_init_lock(&lock); // 初始化互斥锁
#pragma omp parallel for num_threads(8) // NEW ADD
			for (int max_rows_ids = 0; max_rows_ids < dem.height; max_rows_ids = max_rows_ids + line_invert) {
				//line_invert = dem_clip.height - max_rows_ids > line_invert ? line_invert : dem_clip.height - max_rows_ids;

				Eigen::MatrixXd demdata = dem.getData(max_rows_ids, 0, line_invert, dem.width, 1);
				Eigen::MatrixXd sim_r = demdata.array() * 0;
				Eigen::MatrixXd sim_c = demdata.array() * 0;

				int datarows = demdata.rows();
				int datacols = demdata.cols();
				Eigen::MatrixX<double> landpoint(datarows * datacols, 3);
				for (int i = 0; i < datarows; i++) {
					for (int j = 0; j < datacols; j++) {
						landpoint(i * datacols + j, 0) = i + max_rows_ids;
						landpoint(i * datacols + j, 1) = j;
						landpoint(i * datacols + j, 2) = demdata(i, j); // 因为不考虑斜距，所以平面置为0
					}
				}
				//demdata = Eigen::MatrixXd(1, 1);
				landpoint = dem.getLandPoint(landpoint);
				//landpoint.col(2) = landpoint.col(2).array() * 0;

				landpoint = LLA2XYZ(landpoint);
				landpoint = this->pstn.PSTN(landpoint, dem_mean, 0.001, true); // time,r,c
				//std::cout << "for time " << getCurrentTimeString() << std::endl;
				double min_lat = 9999;
				double max_lat = -9999;
				double min_lon = 9999;
				double max_lon = -9999;
				bool has_min_max = false;
				Landpoint p1{ 0,0,0 };
				for (int i = 0; i < datarows; i++) {
					for (int j = 0; j < datacols; j++) {
						sim_r(i, j) = landpoint(i * datacols + j, 1);
						sim_c(i, j) = landpoint(i * datacols + j, 2);
						p1 = dem.getLandPoint(i + max_rows_ids, j, demdata(i, j));
						if (sim_r(i, j) >= 0 && sim_c(i, j) >= 0 && sim_r(i, j) <= this->pstn.height && sim_c(i, j) <= this->pstn.width) {
							min_lat = min_lat < p1.lat ? min_lat : p1.lat;
							max_lat = max_lat > p1.lat ? max_lat : p1.lat;
							min_lon = min_lon < p1.lon ? min_lon : p1.lon;
							max_lon = max_lon > p1.lon ? max_lon : p1.lon;
							has_min_max = true;
						}
					}
				}
				//std::cout << "for time " << getCurrentTimeString() << std::endl;
				// 写入到文件中
				omp_set_lock(&lock); //获得互斥器
				//std::cout << "lock" << endl;
				if (has_min_max) {
					box.min_lat = min_lat < box.min_lat ? min_lat : box.min_lat;
					box.max_lat = max_lat > box.max_lat ? max_lat : box.max_lat;;
					box.min_lon = min_lon < box.min_lon ? min_lon : box.min_lon;;
					box.max_lon = max_lon > box.max_lon ? max_lon : box.max_lon;;
				}
				sim_rc.saveImage(sim_r, max_rows_ids, 0, 1);
				sim_rc.saveImage(sim_c, max_rows_ids, 0, 2);
				all_count = all_count + line_invert;
				std::cout << "rows:\t" << all_count << "/" << dem.height << "\t computing.....\t" << getCurrentTimeString() << endl;
				omp_unset_lock(&lock); //释放互斥器
			}
			omp_destroy_lock(&lock); //销毁互斥器
		}

		std::cout << "prepare over" << getCurrentTimeString() << std::endl;
		double dist_lat = box.max_lat - box.min_lat;
		double dist_lon = box.max_lon - box.min_lon;
		dist_lat = 0.2 * dist_lat;
		dist_lon = 0.2 * dist_lon;
		box.min_lat = box.min_lat - dist_lat;
		box.max_lat = box.max_lat + dist_lat;
		box.min_lon = box.min_lon - dist_lon;
		box.max_lon = box.max_lon + dist_lon;

		std::cout << "box:" << endl;
		std::cout << "min_lat-max_lat:\t" << box.min_lat << "\t" << box.max_lat << endl;
		std::cout << "min_lon-max_lon:\t" << box.min_lon << "\t" << box.max_lon << endl;
		std::cout << "cal box of sar over" << endl;

		// 计算采样之后的影像大小
		int width = 1.8 * this->pstn.width;
		int height = 1.8 * this->pstn.height;

		double heightspace = (box.max_lat - box.min_lat) / height;
		double widthspace = (box.max_lon - box.min_lon) / width;
		std::cout << "resample dem:\n" << getCurrentTimeString() << std::endl;
		std::cout << "in_dem:\t" << this->in_dem_path << endl;
		std::cout << "out_dem:\t" << this->dem_path << endl;
		std::cout << "width-height:\t" << width << "-" << height << endl;
		std::cout << "widthspace\t-\theight\tspace:\t" << widthspace << "\t-\t" << heightspace << endl;

		int pBandIndex[1] = { 1 };
		int pBandCount[1] = { 1 };

		double gt[6] = {
			box.min_lon,widthspace,0,	 //x
			box.max_lat,0,-heightspace//y
		};

		//boost::filesystem::copy(dem_path, this->dem_path);
		ResampleGDAL(this->in_dem_path.c_str(), this->dem_path.c_str(), gt, width, height, GRA_Bilinear);

		std::cout << "resample dem over:\n" << getCurrentTimeString() << std::endl;
		std::cout << "remove sim_rc_path:\t" << this->dem_rc_path << std::endl;
	}

	return 0;
}

/// <summary>
/// 获取行号
/// </summary>
/// <returns></returns>
int simProcess::dem2SAR()
{
	std::cout << "the row  of the sar in dem:" << getCurrentTimeString() << std::endl;
	{
		this->dem2SAR_row();
	}

	std::cout << "the slant range  of the sar in dem:" << getCurrentTimeString() << std::endl;
	{
		gdalImage dem_clip(this->dem_path);
		gdalImage dem_r(this->dem_r_path);
		double dem_mean = dem_clip.mean();
		gdalImage dem_slant_range = CreategdalImage(this->out_dem_slantRange_path, dem_clip.height, dem_clip.width, 1, dem_clip.gt, dem_clip.projection);
		gdalImage plant_slant_range = CreategdalImage(this->out_plant_slantRange_path, dem_clip.height, dem_clip.width, 1, dem_clip.gt, dem_clip.projection);


		int line_invert = int(10000000 / dem_clip.width);
		line_invert = line_invert > 10 ? line_invert : 10;
		int count_lines = 0;
		omp_lock_t lock;
		omp_init_lock(&lock); // 初始化互斥锁
		std::cout << "the slant range of the sar:" << getCurrentTimeString() << std::endl;
#pragma omp parallel for num_threads(6) // NEW ADD
		for (int max_rows_ids = 0; max_rows_ids < dem_clip.height; max_rows_ids = max_rows_ids + line_invert) {
			Eigen::MatrixXd dem_r_block = dem_r.getData(max_rows_ids, 0, line_invert, dem_clip.width, 1);  // 行号
			Eigen::MatrixXd demdata = dem_clip.getData(max_rows_ids, 0, line_invert, dem_clip.width, 1); // 地形
			Eigen::MatrixXd demslant_range = demdata.array() * 0; // 地形斜距
			int datarows = demdata.rows();
			int datacols = demdata.cols();
			Eigen::MatrixX<double> landpoint(datarows * datacols, 3);
			for (int i = 0; i < datarows; i++) {
				for (int j = 0; j < datacols; j++) {
					landpoint(i * datacols + j, 0) = i + max_rows_ids;
					landpoint(i * datacols + j, 1) = j;
					landpoint(i * datacols + j, 2) = demdata(i, j); // 设为0 ，则为初始0值
				}
			}
			landpoint = dem_clip.getLandPoint(landpoint);
			landpoint = LLA2XYZ(landpoint);// this->pstn.WGS842J2000(landpoint);
			Eigen::MatrixX<double> satestate = Eigen::MatrixX<double>::Ones(1, 6);// 卫星状态
			Eigen::MatrixX<double> satepoint = Eigen::MatrixX<double>::Ones(1, 3);// 卫星坐标
			long double ti = 0;
			for (int i = 0; i < datarows; i++) {
				for (int j = 0; j < datacols; j++) {
					ti = dem_r_block(i, j) / this->pstn.PRF + this->pstn.imgStartTime;
					satestate = this->pstn.orbit.SatelliteSpacePoint(ti);
					satepoint = satestate(Eigen::all, { 0,1,2 });
					satepoint(0, 0) = satepoint(0, 0) - landpoint(i * datacols + j, 0);
					satepoint(0, 1) = satepoint(0, 1) - landpoint(i * datacols + j, 1);
					satepoint(0, 2) = satepoint(0, 2) - landpoint(i * datacols + j, 2);
					demslant_range(i, j) = (satepoint.array().pow(2).array().rowwise().sum().array().sqrt())[0];
				}
			}
			//std::cout << "for time " << getCurrentTimeString() << std::endl;
			// 写入到文件中
			omp_set_lock(&lock); //获得互斥器
			dem_slant_range.saveImage(demslant_range, max_rows_ids, 0, 1);
			//sim_rc_reprocess.saveImage(sim_c, max_rows_ids, 0, 2);
			count_lines = count_lines + line_invert;
			std::cout << "rows:\t" << max_rows_ids << "\t-\t" << max_rows_ids + line_invert << "\t" << count_lines << "/" << dem_clip.height << "\t computing.....\t" << getCurrentTimeString() << endl;
			omp_unset_lock(&lock); //释放互斥器
		}
		count_lines = 0;
		std::cout << "the plant slant range of the sar:" << getCurrentTimeString() << std::endl;
#pragma omp parallel for num_threads(6) // NEW ADD
		for (int max_rows_ids = 0; max_rows_ids < dem_clip.height; max_rows_ids = max_rows_ids + line_invert) {
			Eigen::MatrixXd dem_r_block = dem_r.getData(max_rows_ids, 0, line_invert, dem_clip.width, 1);  // 行号
			Eigen::MatrixXd demdata = dem_clip.getData(max_rows_ids, 0, line_invert, dem_clip.width, 1); // 地形
			Eigen::MatrixXd demslant_range = demdata.array() * 0; // 地形斜距
			int datarows = demdata.rows();
			int datacols = demdata.cols();
			Eigen::MatrixX<double> landpoint(datarows * datacols, 3);
			for (int i = 0; i < datarows; i++) {
				for (int j = 0; j < datacols; j++) {
					landpoint(i * datacols + j, 0) = i + max_rows_ids;
					landpoint(i * datacols + j, 1) = j;
					landpoint(i * datacols + j, 2) = 0; // 设为0 ，则为初始0值
				}
			}
			landpoint = dem_clip.getLandPoint(landpoint);
			landpoint = LLA2XYZ(landpoint);
			Eigen::MatrixX<double> satestate = Eigen::MatrixX<double>::Ones(1, 6);// 卫星状态
			Eigen::MatrixX<double> satepoint = Eigen::MatrixX<double>::Ones(1, 3);// 卫星坐标
			long double ti = 0;
			for (int i = 0; i < datarows; i++) {
				for (int j = 0; j < datacols; j++) {
					ti = dem_r_block(i, j) / this->pstn.PRF + this->pstn.imgStartTime;
					satestate = this->pstn.orbit.SatelliteSpacePoint(ti);
					satepoint = satestate(Eigen::all, { 0,1,2 });
					satepoint(0, 0) = satepoint(0, 0) - landpoint(i * datacols + j, 0);
					satepoint(0, 1) = satepoint(0, 1) - landpoint(i * datacols + j, 1);
					satepoint(0, 2) = satepoint(0, 2) - landpoint(i * datacols + j, 2);
					demslant_range(i, j) = (satepoint.array().pow(2).array().rowwise().sum().array().sqrt())[0];
				}
			}
			//std::cout << "for time " << getCurrentTimeString() << std::endl;
			// 写入到文件中
			omp_set_lock(&lock); //获得互斥器
			plant_slant_range.saveImage(demslant_range, max_rows_ids, 0, 1);
			count_lines = count_lines + line_invert;
			std::cout << "rows:\t" << max_rows_ids << "\t-\t" << max_rows_ids + line_invert << "\t" << count_lines << "/" << dem_clip.height << "\t computing.....\t" << getCurrentTimeString() << endl;
			omp_unset_lock(&lock); //释放互斥器
		}
		omp_destroy_lock(&lock); //销毁互斥器
	}
	std::cout << "the row and col  of the sar in dem:" << getCurrentTimeString() << std::endl;
	{
		gdalImage dem_r(this->dem_r_path);
		gdalImage plant_slant_range(this->out_plant_slantRange_path);
		gdalImage dem_rc = CreategdalImage(this->dem_rc_path, dem_r.height, dem_r.width, 2, dem_r.gt, dem_r.projection);

		int line_invert = int(10000000 / dem_r.width);
		line_invert = line_invert > 10 ? line_invert : 10;
		int count_lines = 0;
		omp_lock_t lock;
		omp_init_lock(&lock); // 初始化互斥锁
		std::cout << "the row and col  of the sar:" << getCurrentTimeString() << std::endl;
#pragma omp parallel for num_threads(6) // NEW ADD
		for (int max_rows_ids = 0; max_rows_ids < dem_r.height; max_rows_ids = max_rows_ids + line_invert) {
			Eigen::MatrixXd dem_r_block = dem_r.getData(max_rows_ids, 0, line_invert, dem_r.width, 1);  // 行号
			Eigen::MatrixXd slant_range = plant_slant_range.getData(max_rows_ids, 0, line_invert, dem_r.width, 1); // 地形
			Eigen::MatrixXd dem_c = getRangeColumn(slant_range, this->pstn.R0, this->pstn.fs, this->pstn.lamda);//  (slant_range.array() - this->pstn.R0) / this->pstn.widthspace;
			omp_set_lock(&lock); //获得互斥器
			dem_rc.saveImage(dem_r_block, max_rows_ids, 0, 1);
			dem_rc.saveImage(dem_c, max_rows_ids, 0, 2);
			count_lines = count_lines + line_invert;
			std::cout << "rows:\t" << max_rows_ids << "\t-\t" << max_rows_ids + line_invert << "\t" << count_lines << "/" << dem_r.height << "\t computing.....\t" << getCurrentTimeString() << endl;
			omp_unset_lock(&lock); //释放互斥器
		}
		omp_destroy_lock(&lock); //销毁互斥器
	}


	return 0;
}
/// <summary>
/// 计算相关的倾斜角 -- 弧度值
/// </summary>
/// <returns></returns>
int simProcess::SARIncidentAngle()
{
	std::cout << "the incidence angle  and local incidence :" << getCurrentTimeString() << std::endl;
	{
		gdalImage dem_img(this->dem_path);
		gdalImage sim_r_img(this->dem_r_path);
		gdalImage localincidence_angle_img = CreategdalImage(this->out_localIncidenct_path, dem_img.height, dem_img.width, 1, dem_img.gt, dem_img.projection);// 注意这里保留仿真结果
		gdalImage incidence_angle_img = CreategdalImage(this->out_incidence_path, dem_img.height, dem_img.width, 1, dem_img.gt, dem_img.projection);// 注意这里保留仿真结果
		incidence_angle_img.setNoDataValue(-10, 1);
		localincidence_angle_img.setNoDataValue(-10, 1);
		double PRF = this->pstn.PRF;
		double imgstarttime = this->pstn.imgStartTime;
		int line_invert = int(5000000 / dem_img.width);// 基本大小
		line_invert = line_invert > 100 ? line_invert : 100;
		int start_ids = 0; // 表示1
		int line_width = dem_img.width;
		int count_lines = 0;
		for (int max_rows_ids = 0; max_rows_ids < dem_img.height; max_rows_ids = max_rows_ids + line_invert) {
			Eigen::MatrixXd sim_inclocal = incidence_angle_img.getData(max_rows_ids, 0, line_invert + 2, line_width, 1);
			Eigen::MatrixXd sim_localinclocal = localincidence_angle_img.getData(max_rows_ids, 0, line_invert + 2, line_width, 1);
			sim_inclocal = sim_inclocal.array() * 0 - 10;
			sim_localinclocal = sim_localinclocal.array() * 0 - 10;
			localincidence_angle_img.saveImage(sim_localinclocal, max_rows_ids, 0, 1);
			incidence_angle_img.saveImage(sim_inclocal, max_rows_ids, 0, 1);
		}

		omp_lock_t lock;
		omp_init_lock(&lock); // 初始化互斥锁
		line_invert = int(200000000 / dem_img.width);// 基本大小
		for (int max_rows_ids = 1; max_rows_ids < dem_img.height; max_rows_ids = max_rows_ids + line_invert) {
			start_ids = max_rows_ids - 1;
			Eigen::MatrixXd demdata = dem_img.getData(max_rows_ids - 1, 0, line_invert + 2, line_width, 1);
			Eigen::MatrixXd sim_r = sim_r_img.getData(max_rows_ids - 1, 0, line_invert + 2, line_width, 1).cast<double>(); //
			Eigen::MatrixXd sim_inclocal = incidence_angle_img.getData(max_rows_ids - 1, 0, line_invert + 2, line_width, 1);
			Eigen::MatrixXd sim_localinclocal = localincidence_angle_img.getData(max_rows_ids - 1, 0, line_invert + 2, line_width, 1);
			//sim_r = sim_r.array() * (1 / PRF) + imgstarttime;
			int rowcount = demdata.rows();
			int colcount = demdata.cols();
#pragma omp parallel for num_threads(8) // NEW ADD
			for (int i = 1; i < rowcount - 1; i++) {
				// sataState = 
				Landpoint p0, p1, p2, p3, p4, slopeVector, landpoint, satepoint;
				long double r;
				for (int j = 1; j < colcount - 1; j++) {
					r = sim_r(i, j);
					Eigen::MatrixX<double> sataState = this->pstn.orbit.SatelliteSpacePoint(r / PRF + imgstarttime);
					p0 = dem_img.getLandPoint(i + start_ids, j, demdata(i, j)); // 中心
					p1 = dem_img.getLandPoint(i + start_ids - 1, j, demdata(i - 1, j)); // 1 
					p2 = dem_img.getLandPoint(i + start_ids, j + 1, demdata(i, j + 1)); // 2
					p3 = dem_img.getLandPoint(i + start_ids + 1, j, demdata(i + 1, j)); // 3
					p4 = dem_img.getLandPoint(i + start_ids, j - 1, demdata(i, j - 1)); // 4
					slopeVector = getSlopeVector(p0, p1, p2, p3, p4);
					landpoint = LLA2XYZ(p0);
					satepoint = Landpoint{ sataState(0,0),sataState(0,1) ,sataState(0,2) };
					sim_localinclocal(i, j) = getlocalIncAngle(satepoint, landpoint, slopeVector);
					//p0.ati = 0;
					p0 = dem_img.getLandPoint(i + start_ids, j, demdata(i, j)); // 中心
					landpoint = LLA2XYZ(p0);
					sim_inclocal(i, j) = getIncAngle(satepoint, landpoint);
				}
			}
			int rows_save = sim_inclocal.rows() - 2;
			//sim_inclocal =.array();
			//sim_localinclocal = .array();
			omp_set_lock(&lock); //获得互斥器
			count_lines = count_lines + line_invert;
			localincidence_angle_img.saveImage(sim_localinclocal.block(1, 0, rows_save, line_width), max_rows_ids, 0, 1);
			incidence_angle_img.saveImage(sim_inclocal.block(1, 0, rows_save, line_width), max_rows_ids, 0, 1);
			std::cout << "rows:\t" << max_rows_ids << "\t-\t" << max_rows_ids + line_invert << "\t" << count_lines << "/" << dem_img.height << "\t computing.....\t" << getCurrentTimeString() << endl;
			omp_unset_lock(&lock); //释放互斥器
		}
		omp_destroy_lock(&lock); //销毁互斥器
	}
	std::cout << "the incidence angle  and local incidence over:\t" << getCurrentTimeString() << std::endl;
	return 0;
}
/// <summary>
/// 模拟WGS坐标下 SAR值
/// </summary>
/// <returns></returns>
int simProcess::SARSimulationWGS()
{
	std::cout << "computer the simulation value of evering dem meta,      begining:\t" << getCurrentTimeString() << endl;
	gdalImage localincidence_angle_img(this->out_localIncidenct_path);
	gdalImage sim_sar_img = CreategdalImage(this->sar_sim_wgs_path, localincidence_angle_img.height, localincidence_angle_img.width, 1, localincidence_angle_img.gt, localincidence_angle_img.projection);// 注意这里保留仿真结果
	int line_invert = int(5000000 / localincidence_angle_img.width);// 基本大小
	line_invert = line_invert > 100 ? line_invert : 100;
	int start_ids = 0; // 表示1
	int line_width = localincidence_angle_img.width;
	int count_lines = 0;
	omp_lock_t lock;
	omp_init_lock(&lock); // 初始化互斥锁
#pragma omp parallel for num_threads(6) // NEW ADD
	for (int max_rows_ids = 0; max_rows_ids < sim_sar_img.height; max_rows_ids = max_rows_ids + line_invert) {
		Eigen::MatrixXd sim_localinclocal = localincidence_angle_img.getData(max_rows_ids, 0, line_invert, line_width, 1);
		Eigen::MatrixXd sim_sar(line_invert, line_width);
		sim_localinclocal = sim_localinclocal.array() * d2r;
		// double((0.0133 * cos(localincangle) / pow(sin(localincangle + 0.1 * localincangle), 3)));
		Eigen::MatrixXd cos_angle = sim_localinclocal.array().cos();
		Eigen::MatrixXd sin_angle = sim_localinclocal.array().sin();
		sim_sar = (0.0133 * cos_angle.array()) / ((sin_angle.array() + 0.1 * cos_angle.array()).pow(3));
		omp_set_lock(&lock); //获得互斥器
		count_lines = count_lines + line_invert;
		sim_sar_img.saveImage(sim_sar, max_rows_ids, 0, 1);
		std::cout << "rows:\t" << max_rows_ids << "\t-\t" << max_rows_ids + line_invert << "\t" << count_lines << "/" << sim_sar_img.height << "\t computing.....\t" << getCurrentTimeString() << endl;
		omp_unset_lock(&lock); //释放互斥器
	}
	omp_destroy_lock(&lock); //销毁互斥器
	std::cout << "computer the simulation value of evering dem meta,      begining:\t" << getCurrentTimeString() << endl;

	return 0;
}

int simProcess::SARSimulation()
{
	std::cout << "computer the simulation value of sim_sar,      begining:\t" << getCurrentTimeString() << endl;
	gdalImage sim_sar_wgs_img(this->sar_sim_wgs_path);
	gdalImage sim_rc(this->dem_rc_path);
	gdalImage localIncident_img(this->out_localIncidenct_path);
	gdalImage sim_sar_img = CreategdalImage(this->sar_sim_path, this->pstn.height, this->pstn.width, 1, sim_sar_wgs_img.gt, sim_sar_wgs_img.projection, false);// 注意这里保留仿真结果
	{
		sim_sar_img.setNoDataValue(-9999, 1);
		//double PRF = this->pstn.PRF;
		//double imgstarttime = this->pstn.imgStartTime;
		int line_invert = int(30000000 / sim_sar_wgs_img.width);// 基本大小
		line_invert = line_invert > 100 ? line_invert : 100;
		int start_ids = 0; // 表示1
		int line_width = sim_sar_wgs_img.width;
		Eigen::MatrixXd sim_r(line_invert, line_width);
		Eigen::MatrixXd sim_c(line_invert, line_width);
		Eigen::MatrixXd sim_sum_h(line_invert, line_width);
		Eigen::MatrixXd sim_rsc_orth(line_invert, line_width);
		Eigen::MatrixXd sim_sum_sar(line_invert, line_width);
		Eigen::MatrixXd demdata(line_invert, line_width);
		Eigen::MatrixXd angle(line_invert, line_width);

		// 初始化
		do {
			sim_sum_sar = sim_sar_img.getData(start_ids, 0, line_invert, this->pstn.width, 1).cast<double>();
			sim_sum_sar = sim_sum_sar.array() * 0;
			sim_sar_img.saveImage(sim_sum_sar, start_ids, 0, 1);
			start_ids = start_ids + line_invert;
		} while (start_ids < this->pstn.height);
		{
			// 累加计算模拟值
			int rowcount = 0, colcount = 0;
			int row_min = 0, row_max = 0;
			start_ids = 0;
			std::cout << "time:\tsim_arr_row_min\tsim_arr_row_max\tsim_arr_all_lines\tsim_row_min\tsim_row_max\tsim_row_min0\tsim_row_max0" << std::endl;;
			do {
				std::cout << "\n" << getCurrentTimeString() << "\t" << start_ids << "\t" << start_ids + line_invert << "\t" << sim_rc.height << "\t";
				sim_r = sim_rc.getData(start_ids, 0, line_invert, line_width, 1).cast<double>(); // 行
				sim_c = sim_rc.getData(start_ids, 0, line_invert, line_width, 2).cast<double>(); // 列
				angle = localIncident_img.getData(start_ids, 0, line_invert, line_width, 1).cast<double>();
				sim_rsc_orth = sim_sar_wgs_img.getData(start_ids, 0, line_invert, line_width, 1).cast<double>(); // 计算值
				// 范围
				row_min = (floor(sim_r.minCoeff()));
				row_max = (ceil(sim_r.maxCoeff()));
				std::cout << row_min << "\t" << row_max << "\t";

				//std::cout << "line range:\t" << row_min << " - " << row_max  << "\t computing.....\t" << getCurrentTimeString() << endl;

				if (row_max < 0 || row_min>this->pstn.height) {
					start_ids = start_ids + line_invert;
					continue;
				}
				row_min = row_min <= 0 ? 0 : row_min;
				row_max = row_max >= this->pstn.height ? this->pstn.height : row_max;
				std::cout << row_min << "\t" << row_max << std::endl;
				//sim_sum_sar =sim_sar.getData(row_min, 0, row_max-row_min+1, this->pstn.width, 1).cast<double>();
				//sim_sum_count = sim_sar.getData(row_min, 0, row_max - row_min + 1, this->pstn.width, 2).cast<double>();
				if (row_min >= row_max) {
					break;
				}
				sim_sum_sar = sim_sar_img.getData(row_min, 0, row_max - row_min + 1, this->pstn.width, 1).cast<double>();
				rowcount = sim_r.rows();
				colcount = sim_r.cols();

#pragma omp parallel for num_threads(4) // NEW ADD
				for (int i = 0; i < rowcount; i++) {
					int row_ids = 0, col_ids = 0;
					for (int j = 0; j < colcount; j++) {
						row_ids = int(round(sim_r(i, j)));
						col_ids = int(round(sim_c(i, j)));
						if (row_ids >= 0 && row_ids < this->pstn.height && col_ids >= 0 && col_ids < this->pstn.width && (angle(i, j) < 90 || angle(i, j) > 270)) {
							sim_sum_sar(row_ids - row_min, col_ids) += sim_rsc_orth(i, j);
						}
					}
				}
				sim_sar_img.saveImage(sim_sum_sar, row_min, 0, 1);

				start_ids = start_ids + line_invert;
			} while (start_ids < sim_rc.height);
			std::cout << "\ncomputer the simulation value of sim_sar,    overing  :\t" << getCurrentTimeString() << endl;
		}
	}

	{
		std::cout << "Resample simulation value of sim_sar :\t" << getCurrentTimeString() << endl;
		ResampleGDALs(this->sar_sim_path.c_str(), 1, GRIORA_Bilinear);
	}

	return 0;
}

int simProcess::in_sar_power()
{
	std::cout << "compute the power value of ori sar, beiging:\t" << getCurrentTimeString() << endl;
	gdalImage ori_sar(this->in_sar_path);
	gdalImage sim_power = CreategdalImage(this->sar_power_path, ori_sar.height, ori_sar.width, 1, ori_sar.gt, ori_sar.projection);
	sim_power.setNoDataValue(-9999, 1);
	{
		Eigen::MatrixXd band1(1, 1);
		Eigen::MatrixXd band2(1, 1);
		Eigen::MatrixXd power_value(1, 1);
		int start_ids = 0;
		int line_invert = int(8000000 / ori_sar.width);
		line_invert = line_invert > 10 ? line_invert : 10;
		int row_count = 0, col_count = 0;
		do {
			std::cout << "rows:\t" << start_ids << "/" << ori_sar.height << "\t computing.....\t" << getCurrentTimeString() << endl;
			band1 = ori_sar.getData(start_ids, 0, line_invert, ori_sar.width, 1).cast<double>(); // a
			band2 = ori_sar.getData(start_ids, 0, line_invert, ori_sar.width, 2).cast<double>(); // b
			power_value = (band1.array().pow(2) + band2.array().pow(2) + 1).log10() * 10;// 10 * Eigen::log10(Eigen::pow(0.5, Eigen::pow(2, band1.array()) + Eigen::pow(2, band2.array()))).array();
			sim_power.saveImage(power_value, start_ids, 0, 1);
			start_ids = start_ids + line_invert;
		} while (start_ids < ori_sar.height);
	}
	{
		std::cout << "Resample the power value of ori sar :\t" << getCurrentTimeString() << endl;
		ResampleGDALs(this->sar_power_path.c_str(), 1, GRIORA_Bilinear);
	}
	std::cout << "compute the power value of ori sar, ending:\t" << getCurrentTimeString() << endl;
	{
		std::cout << "=========================================\n";
		std::cout << " the power image of  sar  :\n";
		std::cout << this->sar_power_path.c_str() << "\n";
		std::cout << "band 1 : power value   \t  10*log10(b1^2+b2^2)   \n";
		std::cout << "=========================================\n";

	}
	return 0;
	return 0;
}




int simProcess::Scatter2Grid(std::string lon_lat_path, std::string data_tiff, std::string grid_path, double space) {

	gdalImage lon_lat_img(lon_lat_path);
	double min_lon = 400, max_lon = -400, min_lat = 300, max_lat = -300;
	{
		int conver_lines = 2000;
		for (int max_rows_ids = 0; max_rows_ids < lon_lat_img.height; max_rows_ids = max_rows_ids + conver_lines) {
			Eigen::MatrixXd lon = lon_lat_img.getData(max_rows_ids, 0, conver_lines, lon_lat_img.width, 1);
			Eigen::MatrixXd lat = lon_lat_img.getData(max_rows_ids, 0, conver_lines, lon_lat_img.width, 2);
			int rows = lon.rows();
			int cols = lon.cols();
			for (int i = 0; i < rows; i++) {
				for (int j = 0; j < cols; j++) {
					if (min_lon > lon(i, j) && !isnan(lon(i, j)) && lon(i, j) > -200) {
						min_lon = lon(i, j);
					}
					if (min_lat > lat(i, j) && !isnan(lat(i, j)) && lat(i, j) > -200) {
						min_lat = lat(i, j);
					}
					if (max_lon < lon(i, j) && !isnan(lon(i, j)) && lon(i, j) > -200) {
						max_lon = lon(i, j);
					}

					if (max_lat < lat(i, j) && !isnan(lat(i, j)) && lat(i, j) > -200) {
						max_lat = lat(i, j);
					}
				}
			}
		}
	}
	std::cout << "lon:\t" << min_lon << " , " << max_lon << endl;
	std::cout << "lat:\t" << min_lat << " , " << max_lat << endl;

	double delta = space /( 110 * 1000);
	int width = int((max_lon - min_lon) / delta) + 1;
	int height = int((max_lat - min_lat) / delta) + 1;
	Eigen::MatrixXd gt(2, 3);
	gt(0, 0) = min_lon; gt(0, 1) = delta; gt(0, 2) = 0;//x
	gt(1, 0) = max_lat; gt(1, 1) = 0; gt(1, 2) = -1*delta;//y

	// # 
	char* pszSrcWKT = NULL;
	OGRSpatialReference oSRS;
	oSRS.importFromEPSG(4326);
	//oSRS.SetUTM(50, true);   //北半球  东经120度  
	//oSRS.SetWellKnownGeogCS("WGS84");
	oSRS.exportToWkt(&pszSrcWKT);
	gdalImage grid_img = CreategdalImage(grid_path, height, width, 1, gt, pszSrcWKT, true);
	int conver_lines = 2000;
	for (int max_rows_ids = 0; max_rows_ids < grid_img.height; max_rows_ids = max_rows_ids + conver_lines) {
		Eigen::MatrixXd grid_data = grid_img.getData(max_rows_ids, 0, conver_lines, width, 1);
		grid_data = grid_data.array() * 0 - 9999;
		grid_img.saveImage(grid_data, max_rows_ids, 0, 1);
	}
	grid_img.setNoDataValue(-9999, 1);
	Eigen::MatrixXd grid_data = grid_img.getData(0, 0, grid_img.height, grid_img.width, 1);
	gdalImage Range_data(data_tiff);
	grid_img.InitInv_gt();
	{
		int conver_lines = 500;
		int line_invert = 400;// 计算重叠率
		int line_offset = 60;
		// 逐区域迭代计算
		omp_lock_t lock;
		omp_init_lock(&lock); // 初始化互斥锁
		int allCount = 0;
	
		for (int max_rows_ids = 0; max_rows_ids < lon_lat_img.height; max_rows_ids = max_rows_ids + line_invert) {
			Eigen::MatrixXd lon_data = lon_lat_img.getData(max_rows_ids, 0, conver_lines, lon_lat_img.width, 1);
			Eigen::MatrixXd lat_data = lon_lat_img.getData(max_rows_ids, 0, conver_lines, lon_lat_img.width, 2);
			Eigen::MatrixXd data = Range_data.getData(max_rows_ids, 0, conver_lines, lon_lat_img.width, 1);
			int lon_row_count = lon_data.rows();
			for (int i = 0; i < lon_row_count; i++) {
				for (int j = 0; j < lon_lat_img.width; j++) {
					Landpoint p = grid_img.getRow_Col(lon_data(i, j), lat_data(i, j));
					lon_data(i, j) = p.lon;
					lat_data(i, j) = p.lat;
				}
			}
			int dem_rows_num = lon_data.rows();
			int dem_cols_num = lon_data.cols();

			#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++) {
					Point_3d p, p1, p2, p3, p4;

					p1 = { lat_data(i,j),lon_data(i,j) };
					p2 = { lat_data(i,j + 1),lon_data(i,j + 1) };
					p3 = { lat_data(i + 1, j + 1),lon_data(i + 1, j + 1) };
					p4 = { lat_data(i + 1, j),lon_data(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 = lat_data.block(i, j, 2, 2).minCoeff();
					double temp_max_r = lat_data.block(i, j, 2, 2).maxCoeff();
					double temp_min_c = lon_data.block(i, j, 2, 2).minCoeff();
					double temp_max_c = lon_data.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 < 0 || ii >= height || jj < 0 || jj >= width) {
									continue;
								}
								p = { double(ii),double(jj),0 };
								//if (PtInRect(p, p1, p2, p3, p4)) {
								p1.z = data(i, j);
								p2.z = data(i, j + 1);
								p3.z = data(i + 1, j + 1);
								p4.z = data(i + 1, j);

								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;
								}
								grid_data(ii , jj) = p.z;
								//}
							}
						}
					}
				}
			}
			allCount = allCount + line_invert;
			std::cout << "rows:\t" << allCount << "/" << lon_lat_img.height << "\t  computing.....\t" << getCurrentTimeString() << endl;
		}
	}
	grid_img.saveImage(grid_data, 0, 0, 1);
	return 0;
}



/*
描述：判断点是否在四边形内，该函数也适用于多边形，将点数改成你想要的边数就行
参数：
	pCur：当前点
	pLeftTop：左上角
	pRightTop：右上角
	pRightBelow：右下
	pLeftBelow：左下
返回值：如果在四边形内返回 true ，否则返回 false
*/

bool PtInRect(Point_3d pCur, Point_3d pLeftTop, Point_3d pRightTop, Point_3d pRightBelow, Point_3d pLeftBelow)
{
	int nCount = 4;//任意四边形有4个顶点
	Point_3d RectPoints[4] = { pLeftTop, pLeftBelow, pRightBelow, pRightTop };
	int nCross = 0;
	double lastPointX = -999.999;
	for (int i = 0; i < nCount; i++)
	{
		//依次取相邻的两个点
		Point_3d pBegin = RectPoints[i];
		Point_3d pEnd = RectPoints[(i + 1) % nCount];
		//相邻的两个点是平行于x轴的,当前点和x轴的平行线要么重合,要么不相交,不算
		if (pBegin.y == pEnd.y)continue;
		//交点在pBegin,pEnd的延长线上,不算
		if (pCur.y < min(pBegin.y, pEnd.y) || pCur.y > max(pBegin.y, pEnd.y))continue;
		//当前点和x轴的平行线与pBegin,pEnd直线的交点的x坐标
		double x = (double)(pCur.y - pBegin.y) * (double)(pEnd.x - pBegin.x) / (double)(pEnd.y - pBegin.y) + pBegin.x;
		if (x > pCur.x)//只看pCur右边交点
		{
			if (x != lastPointX)//防止角点算两次
			{
				nCross++;
				lastPointX = x;
			}
		}
	}
	// 单方向交点为奇数，点在多边形之内
	// 单方向交点为偶数，点在多边形之外
	return (nCross % 2 == 1);
}

int simProcess::ReflectTable_WGS2Range()
{//
	gdalImage sim_rc(this->dem_rc_path);
	gdalImage sim_sar_img = CreategdalImage(this->out_ori_sim_tiff, this->pstn.height, this->pstn.width, 2, sim_rc.gt, sim_rc.projection, false);// 注意这里保留仿真结果
	gdalImage sim_sar_count_img = CreategdalImage(this->ori_sim_count_tiff, this->pstn.height, this->pstn.width, 1, sim_rc.gt, sim_rc.projection, false);// 注意这里保留仿真结果
	gdalImage localIncident_img(this->out_localIncidenct_path);
	for (int max_rows_ids = 0; max_rows_ids < this->pstn.height; max_rows_ids = max_rows_ids + 1000) {
		Eigen::MatrixXd sim_sar = sim_sar_img.getData(max_rows_ids, 0, 1000, this->pstn.width, 1);
		Eigen::MatrixXd sim_sarc = sim_sar_img.getData(max_rows_ids, 0, 1000, this->pstn.width, 2);
		Eigen::MatrixXd sim_sar_count = sim_sar_count_img.getData(max_rows_ids, 0, 1000, this->pstn.width, 1);
		sim_sar = sim_sar.array() * 0 - 9999;
		sim_sarc= sim_sar.array() * 0 - 9999;
		sim_sar_count = sim_sar_count.array() * 0;
		sim_sar_img.saveImage(sim_sar, max_rows_ids, 0, 1);
		sim_sar_img.saveImage(sim_sarc, max_rows_ids, 0, 2);
		sim_sar_count_img.saveImage(sim_sar_count, max_rows_ids, 0, 1);
	}
	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 angle = localIncident_img.getData(max_rows_ids, 0, conver_lines, sim_rc.width, 1).cast<double>();
		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 min_row = dem_r.minCoeff() + 1;
		int max_row = dem_r.maxCoeff() + 1;
		int min_col = dem_c.minCoeff() + 1;
		int max_col = dem_c.maxCoeff() + 1;
		if (max_row < 0 || min_row >= this->pstn.height || max_col < 0 || min_col >= this->pstn.width) { // 增加边界判断条件
			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, this->pstn.width, 1);
		Eigen::MatrixXd sar_c = sim_sar_img.getData(min_row, 0, len_rows, this->pstn.width, 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++) {
				Point_3d 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 >= this->pstn.width) {
								continue;
							}
							p = { double(ii),double(jj),-9999 };
							//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;
		std::cout << "rows:\t" << allCount << "/" << sim_rc.height << "\t computing.....\t" << getCurrentTimeString() << endl;
		omp_unset_lock(&lock); //释放互斥器
	}

	std::cout << "\t resample computing.....\t" << getCurrentTimeString() << endl;
	{	int conver = 5000;
	int line_invert = 4000;// 计算重叠率
	ResampleGDALs(this->out_ori_sim_tiff.c_str(), 1, GRIORA_Bilinear);
	ResampleGDALs(this->out_ori_sim_tiff.c_str(), 2, GRIORA_Bilinear);
	}

	return 0;
}

int simProcess::InitRPCIncAngle(std::string paras_path, std::string workspace_path, std::string out_dir_path, std::string in_dem_path, std::string in_rpc_lon_lat_paths, std::string out_inc_angle_path, std::string out_local_inc_angle_path, std::string out_inc_angle_geo_path, std::string out_local_inc_angle_geo_path)
{
	this->pstn = PSTNAlgorithm(paras_path); // 初始化参数文件

	this->in_rpc_lon_lat_path = in_rpc_lon_lat_paths;
	this->in_dem_path = in_dem_path;
	this->outSpace_path = out_dir_path;
	this->workSpace_path = workspace_path;

	// 临时文件
	this->dem_path = JoinPath(this->workSpace_path, "SAR_dem.tiff");
	this->dem_rc_path = JoinPath(this->workSpace_path, "dem_rc.tiff");
	this->dem_r_path = JoinPath(this->workSpace_path, "dem_r.tiff");
	this->out_incidence_path = out_inc_angle_geo_path;
	this->out_localIncidenct_path = out_local_inc_angle_geo_path;
	this->rpc_wgs_path = JoinPath(this->workSpace_path, "rcp2dem_wgs.tiff");
	// 输出文件
	this->out_inc_angle_rpc_path = out_inc_angle_path;
	this->out_local_inc_angle_rpc_path = out_local_inc_angle_path;

	std::cout << "==========================================================================" << endl;
	std::cout << "in_dem_path" << ":\t" << this->in_dem_path << endl;
	std::cout << "in_rpc_lon_lat_path" << ":\t" << this->in_rpc_lon_lat_path << endl;
	std::cout << "workSpace_path" << ":\t" << this->workSpace_path << endl;
	std::cout << "out_dir_path" << ":\t" << this->out_dir_path << endl;
	std::cout << "out_inc_angle_path" << ":\t" << this->out_incidence_path << endl;
	std::cout << "out_local_inc_angle_path" << ":\t" << this->out_localIncidenct_path << endl;
	std::cout << "==========================================================================" << endl;
	this->createRPCDEM();
	this->dem2SAR_row(); // 获取行号
	this->SARIncidentAngle();
	this->SARIncidentAngle_RPC();
	this->in_sar_power();
	return 0;
	return 0;
}

int simProcess::dem2SAR_row()
{

	std::cout << "the row  of the sar in dem:" << getCurrentTimeString() << std::endl;
	{
		gdalImage dem_clip(this->dem_path);
		double dem_mean = dem_clip.mean();
		gdalImage sim_r_reprocess = CreategdalImage(this->dem_r_path, dem_clip.height, dem_clip.width, 1, dem_clip.gt, dem_clip.projection);
		gdalImage sim_rc_reprocess = CreategdalImage(this->dem_rc_path, dem_clip.height, dem_clip.width, 2, dem_clip.gt, dem_clip.projection);
		int line_invert = int(15000000 / dem_clip.width);
		line_invert = line_invert > 10 ? line_invert : 10;
		int count_lines = 0;
		omp_lock_t lock;
		omp_init_lock(&lock); // 初始化互斥锁
#pragma omp parallel for num_threads(8) // NEW ADD
		for (int max_rows_ids = 0; max_rows_ids < dem_clip.height; max_rows_ids = max_rows_ids + line_invert) {
			//line_invert = dem_clip.height - max_rows_ids > line_invert ? line_invert : dem_clip.height - max_rows_ids;

			Eigen::MatrixXd demdata = dem_clip.getData(max_rows_ids, 0, line_invert, dem_clip.width, 1);

			Eigen::MatrixXd sim_r = demdata.array() * 0;
			Eigen::MatrixXd sim_c = demdata.array() * 0;

			int datarows = demdata.rows();
			int datacols = demdata.cols();
			Eigen::MatrixX<double> landpoint(datarows * datacols, 3);
			for (int i = 0; i < datarows; i++) {
				for (int j = 0; j < datacols; j++) {
					landpoint(i * datacols + j, 0) = i + max_rows_ids;
					landpoint(i * datacols + j, 1) = j;
					landpoint(i * datacols + j, 2) = demdata(i, j); // 设为0 ，则为初始0值
				}
			}
			landpoint = dem_clip.getLandPoint(landpoint);
			//landpoint.col(2) = landpoint.col(2).array() * 0;
			landpoint = LLA2XYZ(landpoint);
			landpoint = this->pstn.PSTN(landpoint, dem_mean); // time,r,c
			//std::cout << "for time " << getCurrentTimeString() << std::endl;
			for (int i = 0; i < datarows; i++) {
				for (int j = 0; j < datacols; j++) {
					sim_r(i, j) = landpoint(i * datacols + j, 1);
					sim_c(i, j) = landpoint(i * datacols + j, 2);
				}
			}
			//std::cout << "for time " << getCurrentTimeString() << std::endl;
			// 写入到文件中
			omp_set_lock(&lock); //获得互斥器
			sim_r_reprocess.saveImage(sim_r, max_rows_ids, 0, 1);
			sim_rc_reprocess.saveImage(sim_r, max_rows_ids, 0, 1);
			sim_rc_reprocess.saveImage(sim_c, max_rows_ids, 0, 2);
			count_lines = count_lines + line_invert;
			std::cout << "rows:\t" << max_rows_ids << "\t-\t" << max_rows_ids + line_invert << "\t" << count_lines << "/" << dem_clip.height << "\t computing.....\t" << getCurrentTimeString() << endl;
			omp_unset_lock(&lock); //释放互斥器
		}
		omp_destroy_lock(&lock); //销毁互斥器
		std::cout << "dem2SAR_Row_col  dem_2_sarRC  over " << getCurrentTimeString() << std::endl;

		std::cout << "=========================================\n";
		std::cout << "the row and col of the sar in dem:\n";
		std::cout << this->dem_r_path << "\n";
		std::cout << "band 1 : the row of the  sar  \n";
		std::cout << "=========================================\n";
	}
	return 0;
}

/// <summary>
/// 计算RPC的入射角
/// </summary>
/// <returns></returns>
int simProcess::SARIncidentAngle_RPC() {
	this->calGEC_Incident_localIncident_angle(this->dem_path, this->in_rpc_lon_lat_path, this->out_inc_angle_rpc_path, this->out_local_inc_angle_rpc_path, this->pstn);
	return 0;
}

int simProcess::createRPCDEM()
{
	std::cout << "dem2SAR_Row_col begin time:" << getCurrentTimeString() << std::endl;
	{
		gdalImage dem(this->in_dem_path);
		gdalImage lonlat(this->in_rpc_lon_lat_path);
		DemBox box{ 9999,9999,-9999,-9999 };
		{
			int start_ids = 0;
			int line_invert = int(4000000 / dem.width);
			line_invert = line_invert > 10 ? line_invert : 10;
			for (start_ids = 0; start_ids < this->pstn.height; start_ids = start_ids + line_invert) {
				std::cout << "rows:\t" << start_ids << "/" << this->pstn.height << "\t computing.....\t" << getCurrentTimeString() << endl;
				Eigen::MatrixXd	lon = lonlat.getData(start_ids, 0, line_invert, this->pstn.width, 1);
				Eigen::MatrixXd	lat = lonlat.getData(start_ids, 0, line_invert, this->pstn.width, 2);
				double min_lat = lat.array().minCoeff();
				double max_lat = lat.array().maxCoeff();
				double min_lon = lon.array().minCoeff();
				double max_lon = lon.array().maxCoeff();
				box.min_lat = min_lat < box.min_lat ? min_lat : box.min_lat;
				box.max_lat = max_lat > box.max_lat ? max_lat : box.max_lat;;
				box.min_lon = min_lon < box.min_lon ? min_lon : box.min_lon;;
				box.max_lon = max_lon > box.max_lon ? max_lon : box.max_lon;;

			}
		}
		std::cout << "prepare over" << getCurrentTimeString() << std::endl;
		double dist_lat = box.max_lat - box.min_lat;
		double dist_lon = box.max_lon - box.min_lon;
		dist_lat = 0.1 * dist_lat;
		dist_lon = 0.1 * dist_lon;
		box.min_lat = box.min_lat - dist_lat;
		box.max_lat = box.max_lat + dist_lat;
		box.min_lon = box.min_lon - dist_lon;
		box.max_lon = box.max_lon + dist_lon;

		std::cout << "box:" << endl;
		std::cout << "min_lat-max_lat:\t" << box.min_lat << "\t" << box.max_lat << endl;
		std::cout << "min_lon-max_lon:\t" << box.min_lon << "\t" << box.max_lon << endl;
		std::cout << "cal box of sar over" << endl;

		// 计算采样之后的影像大小
		int width = 1.5 * this->pstn.width;
		int height = 1.5 * this->pstn.height;

		double heightspace = (box.max_lat - box.min_lat) / height;
		double widthspace = (box.max_lon - box.min_lon) / width;
		std::cout << "resample dem:\n" << getCurrentTimeString() << std::endl;
		std::cout << "in_dem:\t" << this->in_dem_path << endl;
		std::cout << "out_dem:\t" << this->dem_path << endl;
		std::cout << "width-height:\t" << width << "-" << height << endl;
		std::cout << "widthspace\t-\theight\tspace:\t" << widthspace << "\t-\t" << heightspace << endl;

		int pBandIndex[1] = { 1 };
		int pBandCount[1] = { 1 };

		double gt[6] = {
			box.min_lon,widthspace,0,	 //x
			box.max_lat,0,-heightspace//y
		};

		//boost::filesystem::copy(dem_path, this->dem_path);
		ResampleGDAL(this->in_dem_path.c_str(), this->dem_path.c_str(), gt, width, height, GRA_Bilinear);

		std::cout << "resample dem over:\n" << getCurrentTimeString() << std::endl;
		std::cout << "remove sim_rc_path:\t" << this->dem_rc_path << std::endl;
	}

	return 0;
}


/// <summary>
/// 模拟SAR影像
/// </summary>
/// <param name="dem_path">dem影像</param>
/// <param name="sim_rc_path">rc 影像</param>
/// <param name="sim_sar_path">结果：模拟sar影像</param>
/// <param name="PSTN">参数类</param>
/// <returns>执行状态</returns>
int simProcess::sim_SAR(std::string dem_path, std::string sim_rc_path, std::string sim_sar_path, PSTNAlgorithm PSTN)
{
	std::cout << "computer the simulation value of evering dem meta,      begining:\t" << getCurrentTimeString() << endl;
	gdalImage dem_img(dem_path);
	gdalImage sim_rc(sim_rc_path);
	gdalImage sim_sar = CreategdalImage(sim_sar_path, dem_img.height, dem_img.width, 2, dem_img.gt, dem_img.projection);// 注意这里保留仿真结果
	sim_sar.setNoDataValue(-9999, 1);
	double PRF = this->pstn.PRF;
	double imgstarttime = this->pstn.imgStartTime;
	int line_invert = int(50000000 / dem_img.width);// 基本大小
	line_invert = line_invert > 100 ? line_invert : 100;
	int start_ids = 0; // 表示1
	int line_width = dem_img.width;
	Eigen::MatrixX<double> sim_r(line_invert, line_width);
	Eigen::MatrixXd sim_c(line_invert, line_width);
	Eigen::MatrixXd dem(line_invert, line_width);
	Eigen::MatrixXd sim_rsc(line_invert, line_width);
	Eigen::MatrixXd sim_inclocal(line_invert, line_width);

	int rowcount = 0, colcount = 0;
	//omp_lock_t lock;
	//omp_init_lock(&lock); // 初始化互斥锁
	do {
		std::cout << "rows:\t" << start_ids << "-" << start_ids + line_invert + 2 << "/" << dem_img.height << "\t computing.....\t" << getCurrentTimeString() << endl;
		dem = dem_img.getData(start_ids, 0, line_invert + 2, line_width, 1);
		sim_r = sim_rc.getData(start_ids, 0, line_invert + 2, line_width, 1).cast<double>();
		sim_r = sim_r.array() * (1 / PRF) + imgstarttime;
		//sim_c = sim_rc.getData(start_ids, 0, line_invert + 1, line_width, 2);
		sim_rsc = dem.array() * 0;
		sim_inclocal = dem.array() * 0 - 9999;
		rowcount = dem.rows();
		colcount = dem.cols();


#pragma omp parallel for num_threads(4) // NEW ADD
		for (int i = 1; i < rowcount - 1; i++) {
			Eigen::MatrixX<double> sataState = sim_r.row(i).reshaped(line_width, 1);
			sataState = this->pstn.orbit.SatelliteSpacePoint(sataState);
			Landpoint p0, p1, p2, p3, p4, slopeVector, landpoint, satepoint;
			double localincangle;
			for (int j = 1; j < colcount - 1; j++) {
				p0 = dem_img.getLandPoint(i + start_ids, j, dem(i, j)); // 中心
				p1 = dem_img.getLandPoint(i + start_ids - 1, j, dem(i - 1, j)); // 1 
				p2 = dem_img.getLandPoint(i + start_ids, j + 1, dem(i, j + 1)); // 2
				p3 = dem_img.getLandPoint(i + start_ids + 1, j, dem(i + 1, j)); // 3
				p4 = dem_img.getLandPoint(i + start_ids, j - 1, dem(i, j - 1)); // 4
				slopeVector = getSlopeVector(p0, p1, p2, p3, p4);
				landpoint = LLA2XYZ(p0);
				satepoint = Landpoint{ sataState(j,0),sataState(j,1) ,sataState(j,2) };
				localincangle = getlocalIncAngle(satepoint, landpoint, slopeVector);
				sim_inclocal(i, j) = localincangle;
				if (localincangle < 90)
				{
					localincangle = localincangle * d2r;
					sim_rsc(i, j) = 100 * double((0.0133 * cos(localincangle) / pow(sin(localincangle + 0.1 * localincangle), 3)));
					//sim_rsc(i, j) = sim_rsc(i, j) > 100 ? -9999 : sim_rsc(i, j);
				}
			}
		}
		//omp_set_lock(&lock); //获得互斥器
		//std::cout << sim_rsc(1, 1961) << endl;
		sim_sar.saveImage(sim_rsc.block(1, 0, rowcount - 2, colcount), start_ids + 1, 0, 1);
		sim_sar.saveImage(sim_inclocal.block(1, 0, rowcount - 2, colcount), start_ids + 1, 0, 2);
		//omp_unset_lock(&lock); //释放互斥器
		start_ids = start_ids + line_invert - 4;
	} while (start_ids < dem_img.height);
	//omp_destroy_lock(&lock); //销毁互斥器
	std::cout << "computer the simulation value of evering dem meta,      over:\t" << getCurrentTimeString() << endl;
	std::cout << "==========================================================" << endl;
	std::cout << "the simulation sar value Raster has the projection that is same of dem" << endl;
	std::cout << sim_sar_path << endl;
	std::cout << "band 1: the simulation sar value" << endl;
	std::cout << "band 2: the local incident angle " << endl;
	std::cout << "==========================================================" << endl;
	return 0;
}


int simProcess::dem2aspect_slope(std::string dem_path, std::string aspect_path, std::string slope_path)
{
	std::cout << "computer aspect and slop begining:\t" << getCurrentTimeString() << endl;

	GDALAllRegister();// 注册格式的驱动
	GDALDriver* poDriver = GetGDALDriverManager()->GetDriverByName("GTiff");
	GDALDataset* dem = (GDALDataset*)(GDALOpen(dem_path.c_str(), GA_ReadOnly));
	GDALDEMProcessing(aspect_path.c_str(), dem, "aspect", NULL, NULL, NULL);
	GDALDEMProcessing(slope_path.c_str(), dem, "slope", NULL, NULL, NULL);
	GDALClose(dem);
	std::cout << "computer aspect and slop overing:\t" << getCurrentTimeString() << endl;
	return 0;
}

/// <summary>
/// 根据前面模拟结果与行列号，生成模拟仿真图，分批生成并计算目标对象
/// </summary>
/// <param name="sim_rc_path">仿真行列号</param>
/// <param name="sim_orth_path">正射单元格结果</param>
/// <param name="sim_sum_path">模拟影像路径</param>
/// <returns></returns>
int simProcess::sim_sum_SAR(std::string sim_dem_path, std::string sim_rc_path, std::string sim_orth_path, std::string sim_sum_path, PSTNAlgorithm PSTN)
{
	std::cout << "computer the simulation value of sim_sar,      begining:\t" << getCurrentTimeString() << endl;
	//gdalImage dem_img(dem_path);
	gdalImage sim_rc(sim_rc_path);
	gdalImage sim_rsc(sim_orth_path);
	gdalImage dem(sim_dem_path);
	gdalImage sim_sar = CreategdalImage(sim_sum_path, this->pstn.height, this->pstn.width, 1, sim_rc.gt, sim_rc.projection, false);// 注意这里保留仿真结果
	{
		sim_sar.setNoDataValue(-9999, 1);
		//double PRF = this->pstn.PRF;
		//double imgstarttime = this->pstn.imgStartTime;
		int line_invert = int(16000000 / sim_rc.width);// 基本大小
		line_invert = line_invert > 100 ? line_invert : 100;
		int start_ids = 0; // 表示1
		int line_width = sim_rc.width;
		Eigen::MatrixX<double> sim_r(line_invert, line_width);
		Eigen::MatrixXd sim_c(line_invert, line_width);
		Eigen::MatrixXd sim_sum_h(line_invert, line_width);
		Eigen::MatrixXd sim_rsc_orth(line_invert, line_width);
		Eigen::MatrixXd sim_sum_sar(line_invert, line_width);
		Eigen::MatrixXd sim_sum_count(line_invert, line_width);
		Eigen::MatrixXd demdata(line_invert, line_width);

		// 初始化
		do {
			sim_sum_sar = sim_sar.getData(start_ids, 0, line_invert, this->pstn.width, 1).cast<double>();
			sim_sum_sar = sim_sum_sar.array() * 0;
			sim_sar.saveImage(sim_sum_sar, start_ids, 0, 1);
			sim_sar.saveImage(sim_sum_sar, start_ids, 0, 2);
			sim_sar.saveImage(sim_sum_sar, start_ids, 0, 3);
			start_ids = start_ids + line_invert;
		} while (start_ids < this->pstn.height);
		{
			// 累加计算模拟值
			int rowcount = 0, colcount = 0;
			int row_min = 0, row_max = 0;
			start_ids = 0;
			std::cout << "time:\tsim_arr_row_min\tsim_arr_row_max\tsim_arr_all_lines\tsim_row_min\tsim_row_max\tsim_row_min0\tsim_row_max0" << std::endl;;


			do {
				std::cout << "\n" << getCurrentTimeString() << "\t" << start_ids << "\t" << start_ids + line_invert << "\t" << sim_rc.height << "\t";
				sim_r = sim_rc.getData(start_ids, 0, line_invert, line_width, 1).cast<double>(); // 行
				sim_c = sim_rc.getData(start_ids, 0, line_invert, line_width, 2).cast<double>(); // 列

				sim_rsc_orth = sim_rsc.getData(start_ids, 0, line_invert, line_width, 1).cast<double>(); // 计算值
				demdata = dem.getData(start_ids, 0, line_invert, line_width, 1).cast<double>(); // 计算值
				// 范围
				row_min = (floor(sim_r.minCoeff()));
				row_max = (ceil(sim_r.maxCoeff()));
				std::cout << row_min << "\t" << row_max << "\t";

				//std::cout << "line range:\t" << row_min << " - " << row_max  << "\t computing.....\t" << getCurrentTimeString() << endl;

				if (row_max < 0 || row_min>this->pstn.height) {
					start_ids = start_ids + line_invert;
					continue;
				}
				row_min = row_min <= 0 ? 0 : row_min;
				row_max = row_max >= this->pstn.height ? this->pstn.height : row_max;
				std::cout << row_min << "\t" << row_max << std::endl;
				//sim_sum_sar =sim_sar.getData(row_min, 0, row_max-row_min+1, this->pstn.width, 1).cast<double>();
				//sim_sum_count = sim_sar.getData(row_min, 0, row_max - row_min + 1, this->pstn.width, 2).cast<double>();
				if (row_min >= row_max) {
					break;
				}
				sim_sum_sar = sim_sar.getData(row_min, 0, row_max - row_min + 1, this->pstn.width, 1).cast<double>();
				sim_sum_count = sim_sar.getData(row_min, 0, row_max - row_min + 1, this->pstn.width, 2).cast<double>();
				sim_sum_h = sim_sar.getData(row_min, 0, row_max - row_min + 1, this->pstn.width, 3).cast<double>(); // 列
				rowcount = sim_r.rows();
				colcount = sim_r.cols();

#pragma omp parallel for num_threads(4) // NEW ADD
				for (int i = 0; i < rowcount; i++) {
					int row_ids = 0, col_ids = 0;
					for (int j = 0; j < colcount; j++) {
						row_ids = int(round(sim_r(i, j)));
						col_ids = int(round(sim_c(i, j)));
						if (row_ids >= 0 && row_ids < this->pstn.height && col_ids >= 0 && col_ids < this->pstn.width) {
							sim_sum_sar(row_ids - row_min, col_ids) += sim_rsc_orth(i, j);
							sim_sum_count(row_ids - row_min, col_ids) += 1;
							sim_sum_h(row_ids - row_min, col_ids) += demdata(i, j);
						}
					}
				}
				sim_sar.saveImage(sim_sum_sar, row_min, 0, 1);
				sim_sar.saveImage(sim_sum_count, row_min, 0, 2);
				sim_sar.saveImage(sim_sum_h, row_min, 0, 3);

				start_ids = start_ids + line_invert;
			} while (start_ids < sim_rc.height);
			std::cout << "\ncomputer the simulation value of sim_sar,    overing  :\t" << getCurrentTimeString() << endl;
		}
	}
	{
		std::cout << "computer the height of sim_sar,    begining  :\t" << getCurrentTimeString() << endl;
		Eigen::MatrixXd sim_sum_h(1, 1);
		Eigen::MatrixXd sim_sum_count(1, 1);

		int start_ids = 0;
		int line_invert = int(8000000 / this->pstn.width);
		line_invert = line_invert > 10 ? line_invert : 10;
		int row_count = 0, col_count = 0;
		do {
			sim_sum_count = sim_sar.getData(start_ids, 0, line_invert, this->pstn.width, 2).cast<double>(); // 行
			sim_sum_h = sim_sar.getData(start_ids, 0, line_invert, this->pstn.width, 3).cast<double>(); // 列
			row_count = sim_sum_h.rows();
			col_count = sim_sum_h.cols();
			std::cout << "min~max:" << endl;
			std::cout << sim_sum_count.minCoeff() << "\t" << sim_sum_count.maxCoeff() << endl;
			std::cout << sim_sum_h.minCoeff() << "\t" << sim_sum_h.maxCoeff() << endl;
			for (int i = 0; i < row_count; i++) {
				for (int j = 0; j < col_count; j++) {
					if (sim_sum_count(i, j) == 0) {
						sim_sum_h(i, j) = 0;
					}
					else {
						sim_sum_h(i, j) = sim_sum_h(i, j) / sim_sum_count(i, j);
					}
				}
			}
			sim_sar.saveImage(sim_sum_h, start_ids, 0, 3);
			start_ids = start_ids + line_invert;
		} while (start_ids < this->pstn.height);

		std::cout << "computer the height of sim_sar,    overing  :\t" << getCurrentTimeString() << endl;
	}

	{
		std::cout << "compute the average value of sim_sar, beiging:\t" << getCurrentTimeString() << endl;

		//ResampleGDALs(sim_sum_path.c_str(), 1, GRIORA_Bilinear);
		//ResampleGDALs(sim_sum_path.c_str(), 3, GRIORA_Bilinear);
		std::cout << "compute the average value of sim_sar,  over :\t" << getCurrentTimeString() << endl;
	}
	{
		std::cout << "=========================================\n";
		std::cout << "sim_sar_tif infomation  :\n";
		std::cout << sim_sum_path << "\n";
		std::cout << "band 1 : simulation sar value  \n";
		std::cout << "band 2 : the sum of the  effective value from the dem \n";
		std::cout << "band 3 : the dem altitude of the effective value from the dem  \n";
		std::cout << "=========================================\n";

	}
	return 0;
}


/// <summary>
/// 计算
/// </summary>
/// <param name="ori_sar_path"></param>
/// <param name="out_sar_power_path"></param>
/// <returns></returns>
int simProcess::ori_sar_power(std::string ori_sar_path, std::string out_sar_power_path)
{
	std::cout << "compute the power value of ori sar, beiging:\t" << getCurrentTimeString() << endl;
	gdalImage ori_sar(ori_sar_path);
	gdalImage sim_power = CreategdalImage(out_sar_power_path, ori_sar.height, ori_sar.width, 1, ori_sar.gt, ori_sar.projection);
	sim_power.setNoDataValue(-9999, 1);
	{
		Eigen::MatrixXd band1(1, 1);
		Eigen::MatrixXd band2(1, 1);
		Eigen::MatrixXd power_value(1, 1);
		int start_ids = 0;
		int line_invert = int(8000000 / ori_sar.width);
		line_invert = line_invert > 10 ? line_invert : 10;
		int row_count = 0, col_count = 0;
		do {
			std::cout << "rows:\t" << start_ids << "/" << ori_sar.height << "\t computing.....\t" << getCurrentTimeString() << endl;
			band1 = ori_sar.getData(start_ids, 0, line_invert, ori_sar.width, 1).cast<double>(); // a
			band2 = ori_sar.getData(start_ids, 0, line_invert, ori_sar.width, 2).cast<double>(); // b
			//power_value = band2.array() * 0-9999;
			power_value = (band1.array().pow(2) + band2.array().pow(2) + 1).log10() * 10;// 10 * Eigen::log10(Eigen::pow(0.5, Eigen::pow(2, band1.array()) + Eigen::pow(2, band2.array()))).array();
			//if () {
			//
			//}//			power_value.isNaN())
																								 //std::cout << power_value.minCoeff() << "\t" << power_value.maxCoeff() << "\t" << endl;
			//std::cout << "band 1\t" << band1.minCoeff() << "\t" << band1.maxCoeff() << "\t" << endl;
			//std::cout << "band 2\t" << band2.minCoeff() << "\t" << band2.maxCoeff() << "\t" << endl;
			sim_power.saveImage(power_value, start_ids, 0, 1);
			start_ids = start_ids + line_invert;
		} while (start_ids < ori_sar.height);
	}
	{
		std::cout << "Resample the power value of ori sar :\t" << getCurrentTimeString() << endl;
		ResampleGDALs(out_sar_power_path.c_str(), 1, GRIORA_Cubic);
	}
	std::cout << "compute the power value of ori sar, ending:\t" << getCurrentTimeString() << endl;
	{
		std::cout << "=========================================\n";
		std::cout << " the power image of  sar  :\n";
		std::cout << out_sar_power_path << "\n";
		std::cout << "band 1 : power value   \t  10*log10(b1^2+b2^2)   \n";
		std::cout << "=========================================\n";

	}
	return 0;
}

int simProcess::createImageMatchModel(std::string ori_sar_rsc_path, std::string ori_sar_rsc_jpg_path, std::string sim_sar_path, std::string sim_sar_jpg_path, std::string matchmodel_path)
{
	// 转换影像
	{
		std::cout << "compute the power value of  sar, begining:\t" << getCurrentTimeString() << endl;
		this->matchmodel.gdal2JPG(ori_sar_rsc_path, ori_sar_rsc_jpg_path, 1);
		this->matchmodel.gdal2JPG(sim_sar_path, sim_sar_jpg_path, 1);
		std::cout << "compute the power value of  sar, ending:\t" << getCurrentTimeString() << endl;
	}
	// 构建匹配模型
	{
		this->matchmodel.offsetXY_matrix = this->matchmodel.ImageMatch_ori_sim(ori_sar_rsc_jpg_path, sim_sar_jpg_path);
		if (this->matchmodel.offsetXY_matrix.rows() < 7) { // 无法进行精校准
			this->isMatchModel = false;
		}
		else {
			//this->matchmodel.match_model = this->matchmodel.CreateMatchModel(this->matchmodel.offsetXY_matrix);
			//this->matchmodel.outMatchModel(matchmodel_path);
		}
	}
	return 0;
}


/// <summary>
/// 根据精确匹配得到的纠正模型，纠正坐标偏移
/// </summary>
/// <param name="sim_rc_path"></param>
/// <param name="matchmodel"></param>
/// <returns></returns>
int simProcess::correctOrth_rc(std::string sim_rc_path, ImageMatch matchmodel)
{
	gdalImage sim_rc(sim_rc_path);
	{
		int lineVert = (1000000 / sim_rc.width);
		int start_ids = 0;
		do {
			Eigen::MatrixXd sim_r = sim_rc.getData(start_ids, 0, lineVert, sim_rc.width, 1);
			Eigen::MatrixXd sim_c = sim_rc.getData(start_ids, 0, lineVert, sim_rc.width, 2);
			// 计算偏移结果
			int rowcount = sim_r.rows();
			int colcount = sim_r.cols();
			for (int i = 0; i < rowcount; i++) {
				Eigen::MatrixXd temp = matchmodel.correctMatchModel(sim_r.row(i), sim_c.col(i));
				sim_r.row(i) = temp.row(0).array();
				sim_c.row(i) = temp.row(1).array();
			}

			// 写入影像
			sim_rc.saveImage(sim_r, start_ids, 0, 1);
			sim_rc.saveImage(sim_c, start_ids, 0, 2);
			start_ids = start_ids + lineVert;
		} while (start_ids < sim_rc.height);
	}
	return 0;
}

/// <summary>
/// 
/// </summary>
/// <param name="orth_rc_path"></param>
/// <param name="dem_path"></param>
/// <param name="out_inclocal_path">输出内入射角</param>
/// <param name="out_dem_path">输出dem</param>
/// <param name="out_count_path">count的tif</param>
/// <returns></returns>
int simProcess::correct_ati(std::string orth_rc_path, std::string dem_path, std::string out_inclocal_path, std::string out_dem_path, std::string out_count_path, PSTNAlgorithm PSTN)
{
	std::cout << "computer the simulation value of evering dem meta,      begining:\t" << getCurrentTimeString() << endl;
	gdalImage dem_img(dem_path);
	gdalImage orth_rc(orth_rc_path);

	//gdalImage ori_inclocal = CreategdalImage(out_inclocal_path, this->pstn.height, this->pstn.width, 1, orth_rc.gt, orth_rc.projection, false);// 注意这里保留仿真结果
	gdalImage ori_dem = CreategdalImage(out_dem_path, this->pstn.height, this->pstn.width, 1, orth_rc.gt, orth_rc.projection, false);// 注意这里保留仿真结果
	gdalImage ori_count = CreategdalImage(out_count_path, this->pstn.height, this->pstn.width, 1, orth_rc.gt, orth_rc.projection, false);// 注意这里保留仿真


	// 初始化
	{
		int line_invert = int(500000 / this->pstn.width);
		line_invert = line_invert > 10 ? line_invert : 10;
		int start_ids = 0;
		do {
			Eigen::MatrixXd	sar_dem = ori_dem.getData(start_ids, 0, line_invert, this->pstn.width, 1);
			//Eigen::MatrixXd	sar_inclocal = ori_inclocal.getData(start_ids, 0, line_invert, this->pstn.width, 1);
			Eigen::MatrixXd	sar_count = ori_count.getData(start_ids, 0, line_invert, this->pstn.width, 1);

			sar_dem = sar_dem.array() * 0;
			//sar_inclocal = sar_inclocal.array() * 0;
			sar_count = sar_count.array() * 0;

			ori_dem.saveImage(sar_dem, start_ids, 0, 1);
			//ori_inclocal.saveImage(sar_inclocal, start_ids, 0, 1);
			ori_count.saveImage(sar_count, start_ids, 0, 1);
			start_ids = start_ids + line_invert;
		} while (start_ids < this->pstn.height);
	}

	{
		double PRF = this->pstn.PRF;
		double imgstarttime = this->pstn.imgStartTime;
		int line_invert = int(50000000 / dem_img.width);// 基本大小
		line_invert = line_invert > 10 ? line_invert : 10;
		int start_ids = 0; // 表示1
		int line_width = dem_img.width;
		Eigen::MatrixX<double> sar_r(line_invert, line_width);
		Eigen::MatrixXd sar_c(line_invert, line_width);
		Eigen::MatrixXd dem(line_invert, line_width);
		Eigen::MatrixXd sar_dem;
		Eigen::MatrixXd sar_inclocal;
		Eigen::MatrixXd sar_count;

		int rowcount = 0, colcount = 0;
		int r_min, r_max, c_min, c_max;
		omp_lock_t lock;
		omp_init_lock(&lock); // 初始化互斥锁
		do {
			std::cout << "rows:\t" << start_ids << "-" << start_ids + line_invert + 2 << "/" << dem_img.height << "\t computing.....\t" << getCurrentTimeString() << endl;
			dem = dem_img.getData(start_ids, 0, line_invert + 2, line_width, 1);
			sar_r = orth_rc.getData(start_ids, 0, line_invert + 2, line_width, 1).cast<double>();
			sar_c = orth_rc.getData(start_ids, 0, line_invert + 2, line_width, 2).cast<double>();

			r_min = int(floor(sar_r.minCoeff()));
			r_max = int(ceil(sar_r.maxCoeff()));

			c_min = int(floor(sar_c.minCoeff()));
			c_max = int(ceil(sar_c.maxCoeff()));


			// 计算数据结果
			rowcount = r_max - r_min + 1;
			colcount = c_max - c_min + 1;

			if (r_max < 0 || r_min>this->pstn.height) {
				start_ids = start_ids + line_invert;
				continue;
			}
			r_min = r_min <= 0 ? 0 : r_min;
			r_max = r_max >= this->pstn.height ? this->pstn.height : r_max;

			sar_dem = ori_dem.getData(r_min, 0, r_max - r_min + 1, this->pstn.width, 1).cast<double>();
			sar_count = ori_count.getData(r_min, 0, r_max - r_min + 1, this->pstn.width, 1).cast<double>();
			//sar_inclocal = ori_inclocal.getData(r_min, 0, r_max - r_min + 1, this->pstn.width, 1).cast<double>(); // 列

			rowcount = dem.rows();
			colcount = dem.cols();
#pragma omp parallel for num_threads(8) // NEW ADD
			for (int i = 1; i < rowcount - 1; i++) {
				Eigen::MatrixX<double> sataState = sar_r.row(i).reshaped(line_width, 1).array() * (1 / PRF) + imgstarttime;
				sataState = this->pstn.orbit.SatelliteSpacePoint(sataState);
				Landpoint p0, p1, p2, p3, p4, slopeVector, landpoint, satepoint;
				double localincangle;
				int r, c;
				for (int j = 1; j < colcount - 1; j++) {
					p0 = dem_img.getLandPoint(i + start_ids, j, dem(i, j)); // 中心
					p1 = dem_img.getLandPoint(i + start_ids - 1, j, dem(i - 1, j)); // 1 
					p2 = dem_img.getLandPoint(i + start_ids, j + 1, dem(i, j + 1)); // 2
					p3 = dem_img.getLandPoint(i + start_ids + 1, j, dem(i + 1, j)); // 3
					p4 = dem_img.getLandPoint(i + start_ids, j - 1, dem(i, j - 1)); // 4
					slopeVector = getSlopeVector(p0, p1, p2, p3, p4);
					landpoint = LLA2XYZ(p0);
					satepoint = Landpoint{ sataState(j,0),sataState(j,1) ,sataState(j,2) };
					localincangle = getlocalIncAngle(satepoint, landpoint, slopeVector);

					if (localincangle < 90)
					{
						r = sar_r(i, j);
						c = sar_c(i, j);
						if (r >= 0 && r < this->pstn.height && c >= 0 && c < this->pstn.width) {
							//sar_inclocal(r - r_min, c) += localincangle;
							sar_count(r - r_min, c) += 1;
							sar_dem(r - r_min, c) += dem(i, j);
						}
					}
				}
			}
			//std::cout << sim_rsc(1, 1961) << endl;
			ori_dem.saveImage(sar_dem, r_min, 0, 1);
			//ori_inclocal.saveImage(sar_inclocal, r_min, 0, 1);
			ori_count.saveImage(sar_count, r_min, 0, 1);
			start_ids = start_ids + line_invert - 4;
		} while (start_ids < dem_img.height);
		omp_destroy_lock(&lock); //销毁互斥器
	}

	// 平均化
	{
		int line_invert = int(500000 / this->pstn.width);
		line_invert = line_invert > 10 ? line_invert : 10;
		int start_ids = 0;
		do {
			Eigen::MatrixXd	sar_dem = ori_dem.getData(start_ids, 0, line_invert, this->pstn.width, 1);
			//Eigen::MatrixXd	sar_inclocal = ori_inclocal.getData(start_ids, 0, line_invert, this->pstn.width, 1);
			Eigen::MatrixXd	sar_count = ori_count.getData(start_ids, 0, line_invert, this->pstn.width, 1);

			sar_dem = sar_dem.array() / sar_count.array();
			//sar_inclocal = sar_inclocal.array() / sar_count.array();

			ori_dem.saveImage(sar_dem, start_ids, 0, 1);
			//ori_inclocal.saveImage(sar_inclocal, start_ids, 0, 1);

			start_ids = start_ids + line_invert;
		} while (start_ids < this->pstn.height);
	}

	std::cout << "repair dem by resample,     begining:\t" << getCurrentTimeString() << endl;
	{


	}
	std::cout << "repair dem by resample,     overing:\t" << getCurrentTimeString() << endl;

	std::cout << "correct the evering dem meta,      over:\t" << getCurrentTimeString() << endl;
	std::cout << "==========================================================" << endl;
	std::cout << "the dem of sar:\t" << out_dem_path << endl;
	std::cout << "the local incident angle of sar:\t " << out_inclocal_path << endl;
	std::cout << "==========================================================" << endl;
	return 0;
}

int simProcess::ASF(std::string in_dem_path, std::string out_latlon_path, ASFOrthClass asfclass, PSTNAlgorithm PSTN)
{
	std::cout << "computer ASF,      begining:\t" << getCurrentTimeString() << endl;
	std::cout << "==========================================================" << endl;
	std::cout << "in parameters" << endl;
	std::cout << "the dem of sar:\t" << in_dem_path << endl;
	std::cout << "in sar rc path:\t" << in_dem_path << endl;
	std::cout << "out parameters" << endl;
	std::cout << "out latlon of sar:\t" << out_latlon_path << endl;
	std::cout << "==========================================================" << endl;


	gdalImage dem(in_dem_path);
	gdalImage latlon = CreategdalImage(out_latlon_path, this->pstn.height, this->pstn.width, 2, dem.gt, dem.projection, false);
	//gdalImage sar_rc(in_sar_rc_path);
	// 初始化坐标
	double aveageAti = 0;
	{
		int line_invert = int(500000 / dem.width);
		line_invert = line_invert > 10 ? line_invert : 10;
		int start_ids = 0;
		int count = 0;
		do {
			Eigen::MatrixXd	sar_dem = dem.getData(start_ids, 0, line_invert, dem.width, 1);
			int row_count = sar_dem.rows();
			int col_count = sar_dem.cols();
			for (int i = 0; i < row_count; i++) {
				for (int j = 0; j < col_count; j++) {
					if (!isnan(sar_dem(i, j)) && !isinf(sar_dem(i, j))) {
						aveageAti += sar_dem(i, j);
						count = count + 1;
					}
				}
			}
			start_ids = start_ids + line_invert;
		} while (start_ids < this->pstn.height);
		aveageAti = aveageAti / count;
	}
	cout << "the aveage ati of dem:\t" << aveageAti << endl;
	// 初始化坐标
	Landpoint initp = dem.getLandPoint(int(dem.height / 2), int(dem.width / 2), aveageAti);

	cout << "initlandpoint lon lat ati\t" << initp.lon << "\t" << initp.lat << "\t" << initp.ati << "\t" << endl;
	initp = LLA2XYZ(initp);
	cout << "initlandpoint lon lat ati\t" << initp.lon << "\t" << initp.lat << "\t" << initp.ati << "\t" << endl;

	double PRF = this->pstn.PRF;
	double imgstarttime = this->pstn.imgStartTime;
	double alpha0 = 0;
	// 迭代计算
	{
		int line_invert = int(1000000 / dem.width) > 10 ? int(1000000 / dem.width) : 10;
		line_invert = line_invert > 10 ? line_invert : 10;
		int start_ids = 0;
		omp_lock_t lock;
		omp_init_lock(&lock); // 初始化互斥锁
#pragma omp parallel for num_threads(8) // NEW ADD
		for (int start_ids = 0; start_ids < this->pstn.height; start_ids = start_ids + line_invert)
		{
			Eigen::MatrixXd	sar_dem = dem.getData(start_ids, 0, line_invert, this->pstn.width, 1);
			Eigen::MatrixXd	sar_lon = latlon.getData(start_ids, 0, line_invert, this->pstn.width, 1);
			Eigen::MatrixXd	sar_lat = latlon.getData(start_ids, 0, line_invert, this->pstn.width, 2);
			Eigen::Vector3d initTagetPoint(initp.lon, initp.lat, initp.ati);
			int row_min = start_ids;
			int row_max = sar_lon.rows() + row_min;
			std::cout << "rows:\t" << row_min << "-" << row_max << "/" << this->pstn.height << "\t computing.....\t" << getCurrentTimeString() << endl;

			for (int i = row_min; i < row_max; i++) {
				long double satetime = i * (1 / PRF) + imgstarttime;
				Eigen::MatrixXd sataspace = this->pstn.orbit.SatelliteSpacePoint(satetime); // 空间坐标
				Eigen::Vector3d SatellitePosition(sataspace(0, 0), sataspace(0, 1), sataspace(0, 2));
				Eigen::Vector3d SatelliteVelocity(sataspace(0, 3), sataspace(0, 4), sataspace(0, 5));
				Landpoint sata{ sataspace(0,0), sataspace(0, 1), sataspace(0, 2) };

				double beta0 = 50 * d2r;// (180-getAngle(sata, sata-initp))* d2r;

				for (int j = 0; j < this->pstn.width; j++) {
					double R = getRangebyColumn(j, this->pstn.R0, this->pstn.fs, this->pstn.lamda);
					double ati_H = sar_dem(i - row_min, j);
					if (!isnan(ati_H) && !isinf(ati_H)) {
						ati_H = aveageAti;
					}
					Eigen::Vector3d landp = asfclass.ASF(R, SatellitePosition, SatelliteVelocity, initTagetPoint, PSTN, this->pstn.R0, ati_H, alpha0, beta0);
					Landpoint landpoint{ landp(0),landp(1) ,landp(2) };
					landpoint = XYZ2LLA(landpoint);
					sar_lon(i - row_min, j) = landpoint.lon;
					sar_lat(i - row_min, j) = landpoint.lat;
				}
			}
			omp_set_lock(&lock);
			latlon.saveImage(sar_lon, row_min, 0, 1);
			latlon.saveImage(sar_lat, row_min, 0, 2);
			omp_unset_lock(&lock);
		}
		omp_destroy_lock(&lock); //销毁互斥器
	}
	std::cout << "out lon and lat of sar,   over:\t" << getCurrentTimeString() << endl;
	std::cout << "==========================================================" << endl;
	std::cout << "the lon lat of sar:\t" << out_latlon_path << endl;
	std::cout << "band 1: lon" << endl;
	std::cout << "band 2: lat " << endl;
	std::cout << "==========================================================" << endl;

	return 0;
}
/// <summary>
/// 计算入射角和具体入射角
/// </summary>
/// <param name="in_dem_path"></param>
/// <param name="in_rc_wgs84_path"></param>
/// <param name="out_incident_angle_path"></param>
/// <param name="out_local_incident_angle_path"></param>
/// <param name="PSTN"></param>
void simProcess::calcalIncident_localIncident_angle(std::string in_dem_path, std::string in_rc_wgs84_path, std::string out_incident_angle_path, std::string out_local_incident_angle_path, PSTNAlgorithm PSTN)
{
	this->calGEC_Incident_localIncident_angle(in_dem_path, in_rc_wgs84_path, out_incident_angle_path, out_local_incident_angle_path, PSTN);
}

/// <summary>
/// 获取GEC坐标系下的入射角与距地入射角
/// </summary>
/// <param name="in_dem_path"></param>
/// <param name="in_gec_lon_lat_path"></param>
/// <param name="out_incident_angle_path"></param>
/// <param name="out_local_incident_angle_path"></param>
/// <param name="PSTN"></param>
void simProcess::calGEC_Incident_localIncident_angle(std::string in_dem_path, std::string in_gec_lon_lat_path, std::string  out_incident_angle_path, std::string out_local_incident_angle_path, PSTNAlgorithm PSTN)
{
	std::cout << "computer Incident_localIncident_angle in sar,      begining:\t" << getCurrentTimeString() << endl;
	std::cout << "==========================================================" << endl;
	std::cout << "in parameters" << endl;
	std::cout << "the dem in wgs84 of sar:\t" << in_dem_path << endl;
	std::cout << "in gec lon lat path:\t" << in_gec_lon_lat_path << endl;
	std::cout << "out parameters" << endl;
	std::cout << "out incident_angle of sar:\t" << out_incident_angle_path << endl;
	std::cout << "out local incident_angle of sar:\t" << out_local_incident_angle_path << endl;
	std::cout << "==========================================================" << endl;

	gdalImage dem_img(in_dem_path);
	gdalImage lonlat(in_gec_lon_lat_path);
	gdalImage incident_angle = CreategdalImage(out_incident_angle_path, this->pstn.height, this->pstn.width, 1, dem_img.gt, dem_img.projection, false);
	gdalImage local_incident_angle = CreategdalImage(out_local_incident_angle_path, this->pstn.height, this->pstn.width, 1, dem_img.gt, dem_img.projection, false);
	// 初始化
	{
		dem_img.InitInv_gt();
		int line_invert = int(500000 / this->pstn.width);
		line_invert = line_invert > 10 ? line_invert : 10;
		int start_ids = 0;
		do {
			Eigen::MatrixXd	sar_inc = incident_angle.getData(start_ids, 0, line_invert, this->pstn.width, 1);
			Eigen::MatrixXd	sar_local_incident = local_incident_angle.getData(start_ids, 0, line_invert, this->pstn.width, 1);

			sar_inc = sar_inc.array() * 0;
			sar_local_incident = sar_local_incident.array() * 0;

			incident_angle.saveImage(sar_inc, start_ids, 0, 1);
			local_incident_angle.saveImage(sar_local_incident, start_ids, 0, 1);
			start_ids = start_ids + line_invert;
		} while (start_ids < this->pstn.height);
	}

	{
		double PRF = this->pstn.PRF;
		double imgstarttime = this->pstn.imgStartTime;

		int line_invert = 500;
		int start_ids = 0;
		int line_cound_sum = 0;
		omp_lock_t lock;
		omp_init_lock(&lock); // 初始化互斥锁

		for (start_ids = 0; start_ids < this->pstn.height; start_ids = start_ids + line_invert) {
			Eigen::MatrixXd	lon = lonlat.getData(start_ids, 0, line_invert, this->pstn.width, 1);
			Eigen::MatrixXd	lat = lonlat.getData(start_ids, 0, line_invert, this->pstn.width, 2);
			Eigen::MatrixXd dem_col = dem_img.inv_gt(0, 0) + dem_img.inv_gt(0, 1) * lon.array() + dem_img.inv_gt(0, 2) * lat.array();
			Eigen::MatrixXd dem_row = dem_img.inv_gt(1, 0) + dem_img.inv_gt(1, 1) * lon.array() + dem_img.inv_gt(1, 2) * lat.array();
			//lon = Eigen::MatrixXd::Ones(1,1);
			//lat= Eigen::MatrixXd::Ones(1,1);
			int min_row = floor(dem_row.array().minCoeff());
			int max_row = ceil(dem_row.array().maxCoeff());
			min_row = min_row - 1 >= 0 ? min_row - 1 : 0;
			max_row = max_row + 1 < dem_img.height ? max_row + 1 : dem_img.height;
			int row_len = max_row - min_row + 1;
			Eigen::MatrixXd dem = dem_img.getData(min_row, 0, row_len, dem_img.width, 1);

			Eigen::MatrixXd	sar_inc = incident_angle.getData(start_ids, 0, line_invert, this->pstn.width, 1);
			Eigen::MatrixXd	sar_local_incident = local_incident_angle.getData(start_ids, 0, line_invert, this->pstn.width, 1);
			int min_col = 0;
			int lon_row_count = dem_col.rows();
			int lon_col_count = dem_col.cols();
#pragma omp parallel for num_threads(8) // NEW ADD
			for (int i = 0; i < lon_row_count; i++)
			{
				Landpoint p0, p1, p2, p3, p4, slopeVector, landpoint, satepoint;
				double localincangle, inc_angle;
				// 同一行，卫星坐标一致
				long double satatime = (i + start_ids) * (1 / PRF) + imgstarttime;
				Eigen::MatrixXd sataState = this->pstn.orbit.SatelliteSpacePoint(satatime);
				satepoint = Landpoint{ sataState(0,0),sataState(0,1) ,sataState(0,2) };
				double dem_r, dem_c;
				int r0, r1;
				int c0, c1;
				for (int j = 0; j < lon_col_count; j++) {
					// 计算行列号
					dem_r = dem_row(i, j); //y
					dem_c = dem_col(i, j); // x
					r0 = floor(dem_r);
					r1 = ceil(dem_r);
					c0 = floor(dem_c);
					c1 = ceil(dem_c);

					p0 = Landpoint{ dem_c - c0,dem_r - r0,0 };
					p1 = Landpoint{ 0,0,dem(r0 - min_row, c0 - min_col) }; // p11(0,0)
					p2 = Landpoint{ 0,1,dem(r0 - min_row + 1, c0 - min_col) }; // p21(0,1)
					p3 = Landpoint{ 1,0,dem(r0 - min_row , c0 + 1 - min_col) }; // p12(1,0)
					p4 = Landpoint{ 1,1,dem(r0 - min_row + 1, c0 + 1 - min_col) }; // p22(1,1)
					p0.ati = Bilinear_interpolation(p0, p1, p2, p3, p4);

					p0 = dem_img.getLandPoint(dem_r, dem_c, p0.ati); // 中心
					p1 = dem_img.getLandPoint(r0, c0, dem(r0 - min_row, c0 - min_col)); // p11(0,0)
					p2 = dem_img.getLandPoint(r1, c0, dem(r1 - min_row, c0 - min_col)); // p21(0,1)
					p3 = dem_img.getLandPoint(r0, c1, dem(r0 - min_row, c1 - min_col)); // p12(1,0)
					p4 = dem_img.getLandPoint(r1, c1, dem(r1 - min_row, c1 - min_col)); // p22(1,1)

					slopeVector = getSlopeVector(p0, p1, p2, p3, p4); // 地面坡向矢量
					landpoint = LLA2XYZ(p0);
					localincangle = getlocalIncAngle(satepoint, landpoint, slopeVector);// 局地入射角
					inc_angle = getIncAngle(satepoint, landpoint); // 侧视角

					// 记录值
					sar_local_incident(i, j) = localincangle;
					sar_inc(i, j) = inc_angle;
				}
			}
			omp_set_lock(&lock);
			incident_angle.saveImage(sar_inc, start_ids, 0, 1);
			local_incident_angle.saveImage(sar_local_incident, start_ids, 0, 1);
			line_cound_sum = line_cound_sum + line_invert;
			std::cout << "rows:\t" << start_ids << "\t" << line_cound_sum << "/" << this->pstn.height << "\t computing.....\t" << getCurrentTimeString() << endl;
			omp_unset_lock(&lock);
		}
		omp_destroy_lock(&lock); //销毁互斥器
	}


	std::cout << "computer Incident_localIncident_angle in sar,      overing:\t" << getCurrentTimeString() << endl;
}

/// <summary>
/// 插值GTC的影像值
/// </summary>
/// <param name="in_rc_wgs84_path"></param>
/// <param name="in_ori_sar_path"></param>
/// <param name="out_orth_sar_path"></param>
/// <param name="PSTN"></param>
void simProcess::interpolation_GTC_sar(std::string in_rc_wgs84_path, std::string in_ori_sar_path, std::string out_orth_sar_path, PSTNAlgorithm pstn)
{
	std::cout << "interpolation(cubic convolution) orth sar value by rc_wgs84 and ori_sar image and model, begin time:" << getCurrentTimeString() << std::endl;


	gdalImage sar_rc(in_rc_wgs84_path);
	gdalImage ori_sar(in_ori_sar_path);
	gdalImage sar_img = CreategdalImage(out_orth_sar_path, sar_rc.height, sar_rc.width, 2, sar_rc.gt, sar_rc.projection);

	// 初始化
	{
		sar_rc.InitInv_gt();
		int line_invert = 100;
		int start_ids = 0;
		do {
			Eigen::MatrixXd	sar_a = sar_img.getData(start_ids, 0, line_invert, sar_rc.width, 1);
			Eigen::MatrixXd	sar_b = sar_img.getData(start_ids, 0, line_invert, sar_rc.width, 2);

			sar_a = sar_a.array() * 0 - 9999;
			sar_b = sar_b.array() * 0 - 9999;

			sar_img.saveImage(sar_a, start_ids, 0, 1);
			sar_img.saveImage(sar_a, start_ids, 0, 2);
			start_ids = start_ids + line_invert;
		} while (start_ids < sar_rc.height);
		sar_img.setNoDataValue(-9999, 1);
		sar_img.setNoDataValue(-9999, 2);
	}

	// 正式计算插值
	{
		int line_invert = 500;
		line_invert = line_invert > 10 ? line_invert : 10;
		int start_ids = 0;
		do {

			std::cout << "rows:\t" << start_ids << "/" << sar_rc.height << "\t computing.....\t" << getCurrentTimeString() << endl;
			Eigen::MatrixXd	sar_r = sar_rc.getData(start_ids, 0, line_invert, sar_rc.width, 1);
			Eigen::MatrixXd	sar_c = sar_rc.getData(start_ids, 0, line_invert, sar_rc.width, 2);

			// 处理行列号获取范围
			int min_r = floor(sar_r.array().minCoeff());
			int max_r = ceil(sar_r.array().maxCoeff());
			int min_c = floor(sar_c.array().minCoeff());
			int max_c = ceil(sar_c.array().maxCoeff());

			if (max_r < 0 || max_c < 0 || min_r >= this->pstn.height || min_c >= this->pstn.width) {
				start_ids = start_ids + line_invert;
				if (start_ids < sar_rc.height) {
					continue;
				}
				else {
					break;
				}
			}

			min_r = min_r >= 0 ? min_r : 0;
			max_r = max_r < this->pstn.height ? max_r : this->pstn.height;
			min_c = min_c >= 0 ? min_c : 0;
			max_c = max_c < this->pstn.width ? max_c : this->pstn.width;

			// 获取影像
			int len_row = max_r - min_r + 4;
			Eigen::MatrixXd	ori_a = ori_sar.getData(min_r, 0, len_row, this->pstn.width, 1);
			Eigen::MatrixXd	ori_b = ori_sar.getData(min_r, 0, len_row, this->pstn.width, 2);
			int row_count = ori_a.rows();
			int col_count = ori_a.cols();

			Eigen::MatrixX<complex<double>> ori(ori_a.rows(), ori_a.cols());
			for (int i = 0; i < row_count; i++) {
				for (int j = 0; j < col_count; j++) {
					ori(i, j) = complex<double>{ ori_a(i, j), ori_b(i, j) };
				}
			}
			ori_a = Eigen::MatrixXd(1, 1);
			ori_b = Eigen::MatrixXd(1, 1); // 释放内存
			Eigen::MatrixXd	sar_a = sar_img.getData(start_ids, 0, line_invert, sar_rc.width, 1);
			Eigen::MatrixXd	sar_b = sar_img.getData(start_ids, 0, line_invert, sar_rc.width, 2);
			sar_r = sar_rc.getData(start_ids, 0, line_invert, sar_rc.width, 1);
			sar_c = sar_rc.getData(start_ids, 0, line_invert, sar_rc.width, 2);
			row_count = sar_r.rows();
			col_count = sar_r.cols();
#pragma omp parallel for num_threads(8) // NEW ADD
			for (int i = 0; i < row_count; i++) {
				int r0, r1, r2, r3, c0, c1, c2, c3;
				double r, c;
				for (int j = 0; j < col_count; j++) {
					r = sar_r(i, j);
					c = sar_c(i, j);
					if (r < 0 || c < 0 || r >= this->pstn.height || c >= this->pstn.width) {
						continue;
					}
					else {
						int kkkkk = 0;

					}
					r1 = floor(r);
					r0 = r1 - 1; r2 = r1 + 1; r3 = r1 + 2; // 获取行
					c1 = floor(c);
					c0 = c1 - 1; c2 = c1 + 1; c3 = c1 + 2; // 获取列
					// 考虑边界情况
					if (r0<-1 || c0<-1 || c3>this->pstn.width || r3>this->pstn.height) {
						continue;
					}
					// 边界插值计算，插值模块权重
					Eigen::MatrixX<complex<double>> img_block(4, 4);
					if (r0 == -1 || c0 == -1) {
						if (r0 == -1) {
							if (c0 == -1) {
								img_block(3, 3) = ori(r3 - min_r, c3);
								img_block(3, 2) = ori(r3 - min_r, c2);
								img_block(3, 1) = ori(r3 - min_r, c1);
								img_block(3, 0) = ori(r3 - min_r, c1) *complex<double>{3, 0} - complex<double>{3, 0} *ori(r3 - min_r, c2) + ori(r3 - min_r, c3); // k,-1

								img_block(2, 3) = ori(r2 - min_r, c3);
								img_block(2, 2) = ori(r2 - min_r, c2);
								img_block(2, 1) = ori(r2 - min_r, c1);
								img_block(2, 0) = ori(r2 - min_r, c1) * complex<double>{3, 0} - complex<double>{3, 0} *ori(r2 - min_r, c2) + ori(r2 - min_r, c3); // k,-1

								img_block(1, 3) = ori(r1 - min_r, c3);
								img_block(1, 2) = ori(r1 - min_r, c2);
								img_block(1, 1) = ori(r1 - min_r, c1);
								img_block(1, 0) = ori(r1 - min_r, c1) * complex<double>{3, 0} - complex<double>{3, 0} *ori(r1 - min_r, c2) + ori(r1 - min_r, c3); // k,-1

								img_block(0, 3) = ori(r1 - min_r, c3) * complex<double>{3, 0} - complex<double>{3, 0} *ori(r2 - min_r, c3) + ori(r3 - min_r, c3);//-1,k
								img_block(0, 2) = ori(r1 - min_r, c2) * complex<double>{3, 0} - complex<double>{3, 0} *ori(r2 - min_r, c2) + ori(r3 - min_r, c2);//-1,
								img_block(0, 1) = ori(r1 - min_r, c1) * complex<double>{3, 0} - complex<double>{3, 0} *ori(r2 - min_r, c1) + ori(r3 - min_r, c1);//-1,
								img_block(0, 0) = img_block(1, 0) * complex<double>{3, 0} - complex<double>{3, 0} *img_block(2, 0) + img_block(3, 0);// -1,-1
							}
							else if (c3 == this->pstn.width) {
								img_block(3, 3) = ori(r3 - min_r, c2) * complex<double>{3, 0} - complex<double>{3, 0} *ori(r3 - min_r, c1) + ori(r3 - min_r, c0);//2,M+1
								img_block(3, 2) = ori(r3 - min_r, c2);
								img_block(3, 1) = ori(r3 - min_r, c1);
								img_block(3, 0) = ori(r3 - min_r, c0);//j,M-2

								img_block(2, 3) = ori(r2 - min_r, c2) * complex<double>{3, 0} - complex<double>{3, 0} *ori(r2 - min_r, c1) + ori(r2 - min_r, c0);//1,M+1
								img_block(2, 2) = ori(r2 - min_r, c2);
								img_block(2, 1) = ori(r2 - min_r, c1);
								img_block(2, 0) = ori(r2 - min_r, c0);//j,M-2

								img_block(1, 3) = ori(r1 - min_r, c2) * complex<double>{3, 0} - complex<double>{3, 0} *ori(r1 - min_r, c1) + ori(r1 - min_r, c0);//0,M+1
								img_block(1, 2) = ori(r1 - min_r, c2);
								img_block(1, 1) = ori(r1 - min_r, c1);
								img_block(1, 0) = ori(r1 - min_r, c0);//j,M-2

								img_block(0, 3) = img_block(2, 3) * complex<double>{3, 0} - complex<double>{3, 0} *img_block(2, 3) + img_block(3, 3);//-1,M+1
								img_block(0, 2) = ori(r1 - min_r, c2) * complex<double>{3, 0} - complex<double>{3, 0} *ori(r2 - min_r, c2) + ori(r3 - min_r, c2);//-1,
								img_block(0, 1) = ori(r1 - min_r, c1) * complex<double>{3, 0} - complex<double>{3, 0} *ori(r2 - min_r, c1) + ori(r3 - min_r, c1);//-1,
								img_block(0, 0) = ori(r1 - min_r, c0) * complex<double>{3, 0} - complex<double>{3, 0} *ori(r2 - min_r, c0) + ori(r3 - min_r, c0);//-1,k
							}
							else {
								img_block(3, 3) = ori(r3 - min_r, c3);
								img_block(3, 2) = ori(r3 - min_r, c2);
								img_block(3, 1) = ori(r3 - min_r, c1);
								img_block(3, 0) = ori(r3 - min_r, c0);

								img_block(2, 3) = ori(r2 - min_r, c3);
								img_block(2, 2) = ori(r2 - min_r, c2);
								img_block(2, 1) = ori(r2 - min_r, c1);
								img_block(2, 0) = ori(r2 - min_r, c0);

								img_block(1, 3) = ori(r1 - min_r, c3);
								img_block(1, 2) = ori(r1 - min_r, c2);
								img_block(1, 1) = ori(r1 - min_r, c1);
								img_block(1, 0) = ori(r1 - min_r, c0);

								img_block(0, 3) = ori(r1 - min_r, c3) * complex<double>{3, 0} - complex<double>{3, 0} *ori(r2 - min_r, c3) + ori(r3 - min_r, c3);//-1,k
								img_block(0, 2) = ori(r1 - min_r, c2) * complex<double>{3, 0} - complex<double>{3, 0} *ori(r2 - min_r, c2) + ori(r3 - min_r, c2);//-1
								img_block(0, 1) = ori(r1 - min_r, c1) * complex<double>{3, 0} - complex<double>{3, 0} *ori(r2 - min_r, c1) + ori(r3 - min_r, c1);//-1
								img_block(0, 0) = ori(r1 - min_r, c0) * complex<double>{3, 0} - complex<double>{3, 0} *ori(r2 - min_r, c0) + ori(r3 - min_r, c0);//-1
							}

						}

						else if (c0 == -1)//r0 != -1
						{
							if (r3 == this->pstn.height) {
								img_block(0, 0) = ori(r0 - min_r, c1) * complex<double>{3, 0} - complex<double>{3, 0} *ori(r0 - min_r, c2) + ori(r0 - min_r, c3);//j,-1
								img_block(0, 1) = ori(r0 - min_r, c1);
								img_block(0, 2) = ori(r0 - min_r, c2);
								img_block(0, 3) = ori(r0 - min_r, c3);

								img_block(1, 0) = ori(r1 - min_r, c1) * complex<double>{3, 0} - complex<double>{3, 0} *ori(r1 - min_r, c2) + ori(r1 - min_r, c3);//j,-1
								img_block(1, 1) = ori(r1 - min_r, c1);
								img_block(1, 2) = ori(r1 - min_r, c2);
								img_block(1, 3) = ori(r1 - min_r, c3);

								img_block(2, 0) = ori(r2 - min_r, c1) * complex<double>{3, 0} - complex<double>{3, 0} *ori(r2 - min_r, c2) + ori(r2 - min_r, c3);//j,-1
								img_block(2, 1) = ori(r2 - min_r, c1);
								img_block(2, 2) = ori(r2 - min_r, c2);
								img_block(2, 3) = ori(r2 - min_r, c3);

								img_block(3, 0) = img_block(2, 0) * complex<double>{3, 0} - complex<double>{3, 0} *img_block(1, 0) + img_block(0, 0);//N+1,-1
								img_block(3, 1) = ori(r2 - min_r, c1) * complex<double>{3, 0} - complex<double>{3, 0} *ori(r1 - min_r, c1) + ori(r0 - min_r, c1);//N+1,j
								img_block(3, 2) = ori(r2 - min_r, c2) * complex<double>{3, 0} - complex<double>{3, 0} *ori(r1 - min_r, c2) + ori(r0 - min_r, c2);//N+1,j
								img_block(3, 3) = ori(r2 - min_r, c3) * complex<double>{3, 0} - complex<double>{3, 0} *ori(r1 - min_r, c3) + ori(r0 - min_r, c3);//N+1,j
							}
							else { // 
								img_block(0, 0) = ori(r0 - min_r, c1) * complex<double>{3, 0} - complex<double>{3, 0} *ori(r0 - min_r, c2) + ori(r0 - min_r, c3);//j,-1
								img_block(0, 1) = ori(r0 - min_r, c1);
								img_block(0, 2) = ori(r0 - min_r, c2);
								img_block(0, 3) = ori(r0 - min_r, c3);

								img_block(1, 0) = ori(r1 - min_r, c1) * complex<double>{3, 0} - complex<double>{3, 0} *ori(r1 - min_r, c2) + ori(r1 - min_r, c3);//j,-1
								img_block(1, 1) = ori(r1 - min_r, c1);
								img_block(1, 2) = ori(r1 - min_r, c2);
								img_block(1, 3) = ori(r1 - min_r, c3);

								img_block(2, 0) = ori(r2 - min_r, c1) * complex<double>{3, 0} - complex<double>{3, 0} *ori(r2 - min_r, c2) + ori(r2 - min_r, c3);//j,-1
								img_block(2, 1) = ori(r2 - min_r, c1);
								img_block(2, 2) = ori(r2 - min_r, c2);
								img_block(2, 3) = ori(r2 - min_r, c3);

								img_block(3, 0) = img_block(2, 0) * complex<double>{3, 0} - complex<double>{3, 0} *img_block(1, 0) + img_block(0, 0);//N+1,-1
								img_block(3, 1) = ori(r3 - min_r, c1);
								img_block(3, 2) = ori(r3 - min_r, c2);
								img_block(3, 3) = ori(r3 - min_r, c3);
							}
						}

					}
					else if (r3 == this->pstn.height || c3 == this->pstn.width) {
						if (r3 == this->pstn.height) {
							if (c3 == this->pstn.width) {
								img_block(0, 0) = ori(r0 - min_r, c0);
								img_block(0, 1) = ori(r0 - min_r, c1);
								img_block(0, 2) = ori(r0 - min_r, c2);
								img_block(0, 3) = ori(r0 - min_r, c2) * complex<double>{3, 0} - complex<double>{3, 0} *ori(r0 - min_r, c1) + ori(r0 - min_r, c0);;//j,M+1

								img_block(1, 0) = ori(r1 - min_r, c0);
								img_block(1, 1) = ori(r1 - min_r, c1);
								img_block(1, 2) = ori(r1 - min_r, c2);
								img_block(1, 3) = ori(r1 - min_r, c2) * complex<double>{3, 0} - complex<double>{3, 0} *ori(r1 - min_r, c1) + ori(r1 - min_r, c0);;//j,M+1

								img_block(2, 0) = ori(r2 - min_r, c0);//j,-1
								img_block(2, 1) = ori(r2 - min_r, c1);
								img_block(2, 2) = ori(r2 - min_r, c2);
								img_block(2, 3) = ori(r2 - min_r, c2) * complex<double>{3, 0} - complex<double>{3, 0} *ori(r2 - min_r, c1) + ori(r2 - min_r, c0);;//j,M+1

								img_block(3, 0) = ori(r2 - min_r, c0) * complex<double>{3, 0} - complex<double>{3, 0} *ori(r1 - min_r, c0) + ori(r0 - min_r, c0);//N+1,-1
								img_block(3, 1) = ori(r2 - min_r, c1) * complex<double>{3, 0} - complex<double>{3, 0} *ori(r1 - min_r, c1) + ori(r0 - min_r, c1);//N+1,j
								img_block(3, 2) = ori(r2 - min_r, c2) * complex<double>{3, 0} - complex<double>{3, 0} *ori(r1 - min_r, c2) + ori(r0 - min_r, c2);//N+1,j
								img_block(3, 3) = img_block(2, 3) * complex<double>{3, 0} - complex<double>{3, 0} *img_block(1, 3) + img_block(0, 3);//N+1,M+1

							}
							else {
								img_block(0, 0) = ori(r0 - min_r, c0);
								img_block(0, 1) = ori(r0 - min_r, c1);
								img_block(0, 2) = ori(r0 - min_r, c2);
								img_block(0, 3) = ori(r0 - min_r, c3);//j,M+1

								img_block(1, 0) = ori(r1 - min_r, c0);
								img_block(1, 1) = ori(r1 - min_r, c1);
								img_block(1, 2) = ori(r1 - min_r, c2);
								img_block(1, 3) = ori(r1 - min_r, c3);//j,M+1

								img_block(2, 0) = ori(r2 - min_r, c0);//j,-1
								img_block(2, 1) = ori(r2 - min_r, c1);
								img_block(2, 2) = ori(r2 - min_r, c2);
								img_block(2, 3) = ori(r2 - min_r, c3);//j,M+1

								img_block(3, 0) = ori(r2 - min_r, c0) * complex<double>{3, 0} - complex<double>{3, 0} *ori(r1 - min_r, c0) + ori(r0 - min_r, c0);//N+1,-1
								img_block(3, 1) = ori(r2 - min_r, c1) * complex<double>{3, 0} - complex<double>{3, 0} *ori(r1 - min_r, c1) + ori(r0 - min_r, c1);//N+1,j
								img_block(3, 2) = ori(r2 - min_r, c2) * complex<double>{3, 0} - complex<double>{3, 0} *ori(r1 - min_r, c2) + ori(r0 - min_r, c2);//N+1,j
								img_block(3, 3) = ori(r2 - min_r, c3) * complex<double>{3, 0} - complex<double>{3, 0} *ori(r1 - min_r, c3) + ori(r0 - min_r, c3);//N+1,j
							}
						}
						else if (c3 == this->pstn.width) { // r3 != this->pstn.height
							img_block(3, 3) = ori(r3 - min_r, c2) * complex<double>{3, 0} - complex<double>{3, 0} *ori(r3 - min_r, c1) + ori(r3 - min_r, c0);//2,M+1
							img_block(3, 2) = ori(r3 - min_r, c2);
							img_block(3, 1) = ori(r3 - min_r, c1);
							img_block(3, 0) = ori(r3 - min_r, c0);//j,M-2

							img_block(2, 3) = ori(r2 - min_r, c2) * complex<double>{3, 0} - complex<double>{3, 0} *ori(r2 - min_r, c1) + ori(r2 - min_r, c0);//1,M+1
							img_block(2, 2) = ori(r2 - min_r, c2);
							img_block(2, 1) = ori(r2 - min_r, c1);
							img_block(2, 0) = ori(r2 - min_r, c0);//j,M-2

							img_block(1, 3) = ori(r1 - min_r, c2) * complex<double>{3, 0} - complex<double>{3, 0} *ori(r1 - min_r, c1) + ori(r1 - min_r, c0);//0,M+1
							img_block(1, 2) = ori(r1 - min_r, c2);
							img_block(1, 1) = ori(r1 - min_r, c1);
							img_block(1, 0) = ori(r1 - min_r, c0);//j,M-2

							img_block(0, 3) = ori(r0 - min_r, c2) * complex<double>{3, 0} - complex<double>{3, 0} *ori(r0 - min_r, c1) + ori(r0 - min_r, c0);//0,M+1
							img_block(0, 2) = ori(r0 - min_r, c2);
							img_block(0, 1) = ori(r0 - min_r, c1);
							img_block(0, 0) = ori(r0 - min_r, c0);
						}
					}
					else {
						img_block(3, 3) = ori(r3 - min_r, c3);
						img_block(3, 2) = ori(r3 - min_r, c2);
						img_block(3, 1) = ori(r3 - min_r, c1);
						img_block(3, 0) = ori(r3 - min_r, c0);

						img_block(2, 3) = ori(r2 - min_r, c3);
						img_block(2, 2) = ori(r2 - min_r, c2);
						img_block(2, 1) = ori(r2 - min_r, c1);
						img_block(2, 0) = ori(r2 - min_r, c0);

						img_block(1, 3) = ori(r1 - min_r, c3);
						img_block(1, 2) = ori(r1 - min_r, c2);
						img_block(1, 1) = ori(r1 - min_r, c1);
						img_block(1, 0) = ori(r1 - min_r, c0);

						img_block(0, 3) = ori(r0 - min_r, c3);
						img_block(0, 2) = ori(r0 - min_r, c2);
						img_block(0, 1) = ori(r0 - min_r, c1);
						img_block(0, 0) = ori(r0 - min_r, c0);
					}

					complex<double> interpolation_value = Cubic_Convolution_interpolation(r - r1, c - c1, img_block);
					// 
					sar_a(i, j) = interpolation_value.real();
					sar_b(i, j) = interpolation_value.imag();
				}
			}
			sar_img.saveImage(sar_a, start_ids, 0, 1);
			sar_img.saveImage(sar_b, start_ids, 0, 2);
			start_ids = start_ids + line_invert;
		} while (start_ids < sar_rc.height);
	}
	std::cout << "interpolation(cubic convolution) orth sar value by rc_wgs84 and ori_sar image and model,  over:\t" << getCurrentTimeString() << std::endl;
}


/// <summary>
/// 插值GTC的影像值
/// </summary>
/// <param name="in_rc_wgs84_path"></param>
/// <param name="in_ori_sar_path"></param>
/// <param name="out_orth_sar_path"></param>
/// <param name="PSTN"></param>
void simProcess::interpolation_GTC_sar_sigma(std::string in_rc_wgs84_path, std::string in_ori_sar_path, std::string out_orth_sar_path, PSTNAlgorithm pstn)
{
	std::cout << "interpolation(cubic convolution) orth sar value by rc_wgs84 and ori_sar image and model, begin time:" << getCurrentTimeString() << std::endl;
	gdalImage sar_rc(in_rc_wgs84_path);
	gdalImage ori_sar(in_ori_sar_path);
	gdalImage sar_img = CreategdalImage(out_orth_sar_path, sar_rc.height, sar_rc.width, 1, sar_rc.gt, sar_rc.projection);

	// 初始化
	{
		sar_rc.InitInv_gt();
		int line_invert = 100;
		int start_ids = 0;
		do {
			Eigen::MatrixXd	sar_a = sar_img.getData(start_ids, 0, line_invert, sar_rc.width, 1);
			//Eigen::MatrixXd	sar_b = sar_img.getData(start_ids, 0, line_invert, sar_rc.width, 2);

			sar_a = sar_a.array() * 0;
			//sar_b = sar_b.array() * 0 - 9999;

			sar_img.saveImage(sar_a, start_ids, 0, 1);
			//sar_img.saveImage(sar_a, start_ids, 0, 2);
			start_ids = start_ids + line_invert;
		} while (start_ids < sar_rc.height);
		sar_img.setNoDataValue(-9999, 1);
		//sar_img.setNoDataValue(-9999, 2);
	}

	// 正式计算插值
	{
		int line_invert = 600;
		line_invert = line_invert > 10 ? line_invert : 10;
		int start_ids = 0;
		do {

			std::cout << "rows:\t" << start_ids << "/" << sar_rc.height << "\t computing.....\t" << getCurrentTimeString() << endl;
			Eigen::MatrixXd	sar_r = sar_rc.getData(start_ids, 0, line_invert, sar_rc.width, 1);
			Eigen::MatrixXd	sar_c = sar_rc.getData(start_ids, 0, line_invert, sar_rc.width, 2);

			// 处理行列号获取范围
			int min_r = floor(sar_r.array().minCoeff());
			int max_r = ceil(sar_r.array().maxCoeff());
			int min_c = floor(sar_c.array().minCoeff());
			int max_c = ceil(sar_c.array().maxCoeff());

			if (max_r < 0 || max_c < 0 || min_r >= this->pstn.height || min_c >= this->pstn.width) {
				start_ids = start_ids + line_invert;
				if (start_ids < sar_rc.height) {
					continue;
				}
				else {
					break;
				}
			}

			min_r = min_r >= 0 ? min_r : 0;
			max_r = max_r < this->pstn.height ? max_r : this->pstn.height;
			min_c = min_c >= 0 ? min_c : 0;
			max_c = max_c < this->pstn.width ? max_c : this->pstn.width;

			// 获取影像
			int len_row = max_r - min_r + 4;
			Eigen::MatrixXd	ori_a = ori_sar.getData(min_r, 0, len_row, this->pstn.width, 1);
			//Eigen::MatrixXd	ori_b = ori_sar.getData(min_r, 0, len_row, this->pstn.width, 2);
			int row_count = ori_a.rows();
			int col_count = ori_a.cols();

			Eigen::MatrixX<complex<double>> ori(ori_a.rows(), ori_a.cols());
			for (int i = 0; i < row_count; i++) {
				for (int j = 0; j < col_count; j++) {
					ori(i, j) = complex<double>{ ori_a(i, j), 0 };
				}
			}
			ori_a = Eigen::MatrixXd(1, 1);
			//ori_b = Eigen::MatrixXd(1, 1); // 释放内存
			Eigen::MatrixXd	sar_a = sar_img.getData(start_ids, 0, line_invert, sar_rc.width, 1);
			//Eigen::MatrixXd	sar_b = sar_img.getData(start_ids, 0, line_invert, sar_rc.width, 2);
			sar_r = sar_rc.getData(start_ids, 0, line_invert, sar_rc.width, 1);
			sar_c = sar_rc.getData(start_ids, 0, line_invert, sar_rc.width, 2);
			row_count = sar_r.rows()-100;
			col_count = sar_r.cols();
#pragma omp parallel for num_threads(8) // NEW ADD
			for (int i = 0; i < row_count; i++) {
				int r0, r1, r2, r3, c0, c1, c2, c3;
				double r, c;
				for (int j = 0; j < col_count; j++) {
					r = sar_r(i, j);
					c = sar_c(i, j);
					if (r < 0 || c < 0 || r >= this->pstn.height || c >= this->pstn.width) {
						continue;
					}
					else {
						int kkkkk = 0;

					}
					r1 = floor(r);
					r0 = r1 - 1; r2 = r1 + 1; r3 = r1 + 2; // 获取行
					c1 = floor(c);
					c0 = c1 - 1; c2 = c1 + 1; c3 = c1 + 2; // 获取列
					// 考虑边界情况
					if (r0<-1 || c0<-1 || c3>this->pstn.width || r3>this->pstn.height) {
						continue;
					}
					// 边界插值计算，插值模块权重
					Eigen::MatrixX<complex<double>> img_block(4, 4);
					if (r0 == -1 || c0 == -1) {
						if (r0 == -1) {
							if (c0 == -1) {
								img_block(3, 3) = ori(r3 - min_r, c3);
								img_block(3, 2) = ori(r3 - min_r, c2);
								img_block(3, 1) = ori(r3 - min_r, c1);
								img_block(3, 0) = ori(r3 - min_r, c1) * complex<double>{3, 0} - complex<double>{3,0} *ori(r3 - min_r, c2) + ori(r3 - min_r, c3); // k,-1

								img_block(2, 3) = ori(r2 - min_r, c3);
								img_block(2, 2) = ori(r2 - min_r, c2);
								img_block(2, 1) = ori(r2 - min_r, c1);
								img_block(2, 0) = ori(r2 - min_r, c1) * complex<double>{3, 0} - complex<double>{3, 0} *ori(r2 - min_r, c2) + ori(r2 - min_r, c3); // k,-1

								img_block(1, 3) = ori(r1 - min_r, c3);
								img_block(1, 2) = ori(r1 - min_r, c2);
								img_block(1, 1) = ori(r1 - min_r, c1);
								img_block(1, 0) = ori(r1 - min_r, c1) * complex<double>{3, 0} - complex<double>{3, 0} *ori(r1 - min_r, c2) + ori(r1 - min_r, c3); // k,-1

								img_block(0, 3) = ori(r1 - min_r, c3) * complex<double>{3, 0} - complex<double>{3, 0} *ori(r2 - min_r, c3) + ori(r3 - min_r, c3);//-1,k
								img_block(0, 2) = ori(r1 - min_r, c2) * complex<double>{3, 0} - complex<double>{3, 0} *ori(r2 - min_r, c2) + ori(r3 - min_r, c2);//-1,
								img_block(0, 1) = ori(r1 - min_r, c1) * complex<double>{3, 0} - complex<double>{3, 0} *ori(r2 - min_r, c1) + ori(r3 - min_r, c1);//-1,
								img_block(0, 0) = img_block(1, 0) * complex<double>{3, 0} - complex<double>{3, 0} *img_block(2, 0) + img_block(3, 0);// -1,-1
							}
							else if (c3 == this->pstn.width) {
								img_block(3, 3) = ori(r3 - min_r, c2) * complex<double>{3, 0} - complex<double>{3, 0} *ori(r3 - min_r, c1) + ori(r3 - min_r, c0);//2,M+1
								img_block(3, 2) = ori(r3 - min_r, c2);
								img_block(3, 1) = ori(r3 - min_r, c1);
								img_block(3, 0) = ori(r3 - min_r, c0);//j,M-2

								img_block(2, 3) = ori(r2 - min_r, c2) * complex<double>{3, 0} - complex<double>{3, 0} *ori(r2 - min_r, c1) + ori(r2 - min_r, c0);//1,M+1
								img_block(2, 2) = ori(r2 - min_r, c2);
								img_block(2, 1) = ori(r2 - min_r, c1);
								img_block(2, 0) = ori(r2 - min_r, c0);//j,M-2

								img_block(1, 3) = ori(r1 - min_r, c2) * complex<double>{3, 0} - complex<double>{3, 0} *ori(r1 - min_r, c1) + ori(r1 - min_r, c0);//0,M+1
								img_block(1, 2) = ori(r1 - min_r, c2);
								img_block(1, 1) = ori(r1 - min_r, c1);
								img_block(1, 0) = ori(r1 - min_r, c0);//j,M-2

								img_block(0, 3) = img_block(2, 3) * complex<double>{3, 0} - complex<double>{3, 0} *img_block(2, 3) + img_block(3, 3);//-1,M+1
								img_block(0, 2) = ori(r1 - min_r, c2) * complex<double>{3, 0} - complex<double>{3, 0} *ori(r2 - min_r, c2) + ori(r3 - min_r, c2);//-1,
								img_block(0, 1) = ori(r1 - min_r, c1) * complex<double>{3, 0} - complex<double>{3, 0} *ori(r2 - min_r, c1) + ori(r3 - min_r, c1);//-1,
								img_block(0, 0) = ori(r1 - min_r, c0) * complex<double>{3, 0} - complex<double>{3, 0} *ori(r2 - min_r, c0) + ori(r3 - min_r, c0);//-1,k
							}
							else {
								img_block(3, 3) = ori(r3 - min_r, c3);
								img_block(3, 2) = ori(r3 - min_r, c2);
								img_block(3, 1) = ori(r3 - min_r, c1);
								img_block(3, 0) = ori(r3 - min_r, c0);

								img_block(2, 3) = ori(r2 - min_r, c3);
								img_block(2, 2) = ori(r2 - min_r, c2);
								img_block(2, 1) = ori(r2 - min_r, c1);
								img_block(2, 0) = ori(r2 - min_r, c0);

								img_block(1, 3) = ori(r1 - min_r, c3);
								img_block(1, 2) = ori(r1 - min_r, c2);
								img_block(1, 1) = ori(r1 - min_r, c1);
								img_block(1, 0) = ori(r1 - min_r, c0);

								img_block(0, 3) = ori(r1 - min_r, c3) * complex<double>{3, 0} - complex<double>{3, 0} *ori(r2 - min_r, c3) + ori(r3 - min_r, c3);//-1,k
								img_block(0, 2) = ori(r1 - min_r, c2) * complex<double>{3, 0} - complex<double>{3, 0} *ori(r2 - min_r, c2) + ori(r3 - min_r, c2);//-1
								img_block(0, 1) = ori(r1 - min_r, c1) * complex<double>{3, 0} - complex<double>{3, 0} *ori(r2 - min_r, c1) + ori(r3 - min_r, c1);//-1
								img_block(0, 0) = ori(r1 - min_r, c0) * complex<double>{3, 0} - complex<double>{3, 0} *ori(r2 - min_r, c0) + ori(r3 - min_r, c0);//-1
							}

						}

						else if (c0 == -1)//r0 != -1
						{
							if (r3 == this->pstn.height) {
								img_block(0, 0) = ori(r0 - min_r, c1) * complex<double>{3, 0} - complex<double>{3, 0} *ori(r0 - min_r, c2) + ori(r0 - min_r, c3);//j,-1
								img_block(0, 1) = ori(r0 - min_r, c1);
								img_block(0, 2) = ori(r0 - min_r, c2);
								img_block(0, 3) = ori(r0 - min_r, c3);

								img_block(1, 0) = ori(r1 - min_r, c1) * complex<double>{3, 0} - complex<double>{3, 0} *ori(r1 - min_r, c2) + ori(r1 - min_r, c3);//j,-1
								img_block(1, 1) = ori(r1 - min_r, c1);
								img_block(1, 2) = ori(r1 - min_r, c2);
								img_block(1, 3) = ori(r1 - min_r, c3);

								img_block(2, 0) = ori(r2 - min_r, c1) * complex<double>{3, 0} - complex<double>{3, 0} *ori(r2 - min_r, c2) + ori(r2 - min_r, c3);//j,-1
								img_block(2, 1) = ori(r2 - min_r, c1);
								img_block(2, 2) = ori(r2 - min_r, c2);
								img_block(2, 3) = ori(r2 - min_r, c3);

								img_block(3, 0) = img_block(2, 0) * complex<double>{3, 0} - complex<double>{3, 0} *img_block(1, 0) + img_block(0, 0);//N+1,-1
								img_block(3, 1) = ori(r2 - min_r, c1) * complex<double>{3, 0} - complex<double>{3, 0} *ori(r1 - min_r, c1) + ori(r0 - min_r, c1);//N+1,j
								img_block(3, 2) = ori(r2 - min_r, c2) * complex<double>{3, 0} - complex<double>{3, 0} *ori(r1 - min_r, c2) + ori(r0 - min_r, c2);//N+1,j
								img_block(3, 3) = ori(r2 - min_r, c3) * complex<double>{3, 0} - complex<double>{3, 0} *ori(r1 - min_r, c3) + ori(r0 - min_r, c3);//N+1,j
							}
							else { // 
								img_block(0, 0) = ori(r0 - min_r, c1) * complex<double>{3, 0} - complex<double>{3, 0} *ori(r0 - min_r, c2) + ori(r0 - min_r, c3);//j,-1
								img_block(0, 1) = ori(r0 - min_r, c1);
								img_block(0, 2) = ori(r0 - min_r, c2);
								img_block(0, 3) = ori(r0 - min_r, c3);

								img_block(1, 0) = ori(r1 - min_r, c1) * complex<double>{3, 0} - complex<double>{3, 0} *ori(r1 - min_r, c2) + ori(r1 - min_r, c3);//j,-1
								img_block(1, 1) = ori(r1 - min_r, c1);
								img_block(1, 2) = ori(r1 - min_r, c2);
								img_block(1, 3) = ori(r1 - min_r, c3);

								img_block(2, 0) = ori(r2 - min_r, c1) * complex<double>{3, 0} - complex<double>{3, 0} *ori(r2 - min_r, c2) + ori(r2 - min_r, c3);//j,-1
								img_block(2, 1) = ori(r2 - min_r, c1);
								img_block(2, 2) = ori(r2 - min_r, c2);
								img_block(2, 3) = ori(r2 - min_r, c3);

								img_block(3, 0) = img_block(2, 0) * complex<double>{3, 0} - complex<double>{3, 0} *img_block(1, 0) + img_block(0, 0);//N+1,-1
								img_block(3, 1) = ori(r3 - min_r, c1);
								img_block(3, 2) = ori(r3 - min_r, c2);
								img_block(3, 3) = ori(r3 - min_r, c3);
							}
						}

					}
					else if (r3 == this->pstn.height || c3 == this->pstn.width) {
						if (r3 == this->pstn.height) {
							if (c3 == this->pstn.width) {
								img_block(0, 0) = ori(r0 - min_r, c0);
								img_block(0, 1) = ori(r0 - min_r, c1);
								img_block(0, 2) = ori(r0 - min_r, c2);
								img_block(0, 3) = ori(r0 - min_r, c2) * complex<double>{3, 0} - complex<double>{3, 0} *ori(r0 - min_r, c1) + ori(r0 - min_r, c0);;//j,M+1

								img_block(1, 0) = ori(r1 - min_r, c0);
								img_block(1, 1) = ori(r1 - min_r, c1);
								img_block(1, 2) = ori(r1 - min_r, c2);
								img_block(1, 3) = ori(r1 - min_r, c2) * complex<double>{3, 0} - complex<double>{3, 0} *ori(r1 - min_r, c1) + ori(r1 - min_r, c0);;//j,M+1

								img_block(2, 0) = ori(r2 - min_r, c0);//j,-1
								img_block(2, 1) = ori(r2 - min_r, c1);
								img_block(2, 2) = ori(r2 - min_r, c2);
								img_block(2, 3) = ori(r2 - min_r, c2) * complex<double>{3, 0} - complex<double>{3, 0} *ori(r2 - min_r, c1) + ori(r2 - min_r, c0);;//j,M+1

								img_block(3, 0) = ori(r2 - min_r, c0) * complex<double>{3, 0} - complex<double>{3, 0} *ori(r1 - min_r, c0) + ori(r0 - min_r, c0);//N+1,-1
								img_block(3, 1) = ori(r2 - min_r, c1) * complex<double>{3, 0} - complex<double>{3, 0} *ori(r1 - min_r, c1) + ori(r0 - min_r, c1);//N+1,j
								img_block(3, 2) = ori(r2 - min_r, c2) * complex<double>{3, 0} - complex<double>{3, 0} *ori(r1 - min_r, c2) + ori(r0 - min_r, c2);//N+1,j
								img_block(3, 3) = img_block(2, 3) * complex<double>{3, 0} - complex<double>{3, 0} *img_block(1, 3) + img_block(0, 3);//N+1,M+1

							}
							else {
								img_block(0, 0) = ori(r0 - min_r, c0);
								img_block(0, 1) = ori(r0 - min_r, c1);
								img_block(0, 2) = ori(r0 - min_r, c2);
								img_block(0, 3) = ori(r0 - min_r, c3);//j,M+1

								img_block(1, 0) = ori(r1 - min_r, c0);
								img_block(1, 1) = ori(r1 - min_r, c1);
								img_block(1, 2) = ori(r1 - min_r, c2);
								img_block(1, 3) = ori(r1 - min_r, c3);//j,M+1

								img_block(2, 0) = ori(r2 - min_r, c0);//j,-1
								img_block(2, 1) = ori(r2 - min_r, c1);
								img_block(2, 2) = ori(r2 - min_r, c2);
								img_block(2, 3) = ori(r2 - min_r, c3);//j,M+1

								img_block(3, 0) = ori(r2 - min_r, c0) * complex<double>{3, 0} - complex<double>{3, 0} *ori(r1 - min_r, c0) + ori(r0 - min_r, c0);//N+1,-1
								img_block(3, 1) = ori(r2 - min_r, c1) * complex<double>{3, 0} - complex<double>{3, 0} *ori(r1 - min_r, c1) + ori(r0 - min_r, c1);//N+1,j
								img_block(3, 2) = ori(r2 - min_r, c2) * complex<double>{3, 0} - complex<double>{3, 0} *ori(r1 - min_r, c2) + ori(r0 - min_r, c2);//N+1,j
								img_block(3, 3) = ori(r2 - min_r, c3) * complex<double>{3, 0} - complex<double>{3, 0} *ori(r1 - min_r, c3) + ori(r0 - min_r, c3);//N+1,j
							}
						}
						else if (c3 == this->pstn.width) { // r3 != this->pstn.height
							img_block(3, 3) = ori(r3 - min_r, c2) * complex<double>{3, 0} - complex<double>{3, 0} *ori(r3 - min_r, c1) + ori(r3 - min_r, c0);//2,M+1
							img_block(3, 2) = ori(r3 - min_r, c2);
							img_block(3, 1) = ori(r3 - min_r, c1);
							img_block(3, 0) = ori(r3 - min_r, c0);//j,M-2

							img_block(2, 3) = ori(r2 - min_r, c2) * complex<double>{3, 0} - complex<double>{3, 0} *ori(r2 - min_r, c1) + ori(r2 - min_r, c0);//1,M+1
							img_block(2, 2) = ori(r2 - min_r, c2);
							img_block(2, 1) = ori(r2 - min_r, c1);
							img_block(2, 0) = ori(r2 - min_r, c0);//j,M-2

							img_block(1, 3) = ori(r1 - min_r, c2) * complex<double>{3, 0} - complex<double>{3, 0} *ori(r1 - min_r, c1) + ori(r1 - min_r, c0);//0,M+1
							img_block(1, 2) = ori(r1 - min_r, c2);
							img_block(1, 1) = ori(r1 - min_r, c1);
							img_block(1, 0) = ori(r1 - min_r, c0);//j,M-2

							img_block(0, 3) = ori(r0 - min_r, c2) * complex<double>{3, 0} - complex<double>{3, 0} *ori(r0 - min_r, c1) + ori(r0 - min_r, c0);//0,M+1
							img_block(0, 2) = ori(r0 - min_r, c2);
							img_block(0, 1) = ori(r0 - min_r, c1);
							img_block(0, 0) = ori(r0 - min_r, c0);
						}
					}
					else {
						img_block(3, 3) = ori(r3 - min_r, c3);
						img_block(3, 2) = ori(r3 - min_r, c2);
						img_block(3, 1) = ori(r3 - min_r, c1);
						img_block(3, 0) = ori(r3 - min_r, c0);

						img_block(2, 3) = ori(r2 - min_r, c3);
						img_block(2, 2) = ori(r2 - min_r, c2);
						img_block(2, 1) = ori(r2 - min_r, c1);
						img_block(2, 0) = ori(r2 - min_r, c0);

						img_block(1, 3) = ori(r1 - min_r, c3);
						img_block(1, 2) = ori(r1 - min_r, c2);
						img_block(1, 1) = ori(r1 - min_r, c1);
						img_block(1, 0) = ori(r1 - min_r, c0);

						img_block(0, 3) = ori(r0 - min_r, c3);
						img_block(0, 2) = ori(r0 - min_r, c2);
						img_block(0, 1) = ori(r0 - min_r, c1);
						img_block(0, 0) = ori(r0 - min_r, c0);
					}

					complex<double> interpolation_value = Cubic_Convolution_interpolation(r - r1, c - c1, img_block);
					// 
					//if (interpolation_value.real() < 0) {
					//	int a = 1;
					//}
					sar_a(i, j) = interpolation_value.real();
					//sar_b(i, j) = interpolation_value.imag();
				}
			}
			sar_img.saveImage(sar_a, start_ids, 0, 1);
			//sar_img.saveImage(sar_b, start_ids, 0, 2);
			start_ids = start_ids + line_invert-100;
		} while (start_ids < sar_rc.height);
	}
	std::cout << "interpolation(cubic convolution) orth sar value by rc_wgs84 and ori_sar image and model,  over:\t" << getCurrentTimeString() << std::endl;
}

void simProcess::interpolation_bil(std::string in_rc_wgs84_path, std::string in_ori_sar_path, std::string out_orth_sar_path, PSTNAlgorithm pstn)
{
	std::cout << "interpolation(cubic convolution) orth sar value by rc_wgs84 and ori_sar image and model, begin time:" << getCurrentTimeString() << std::endl;

	gdalImage sar_rc(in_rc_wgs84_path);
	gdalImage ori_sar(in_ori_sar_path);
	
	this->pstn.width = ori_sar.width;
	this->pstn.height = ori_sar.height;
	gdalImage sar_img = CreategdalImage(out_orth_sar_path, sar_rc.height, sar_rc.width, 1, sar_rc.gt, sar_rc.projection);

	// 初始化
	{
		sar_rc.InitInv_gt();
		int line_invert = 100;
		int start_ids = 0;
		do {
			Eigen::MatrixXd	sar_a = sar_img.getData(start_ids, 0, line_invert, sar_rc.width, 1);
			//Eigen::MatrixXd	sar_b = sar_img.getData(start_ids, 0, line_invert, sar_rc.width, 2);

			sar_a = sar_a.array() * 0 - 9999;
			//sar_b = sar_b.array() * 0 - 9999;

			sar_img.saveImage(sar_a, start_ids, 0, 1);
			//sar_img.saveImage(sar_a, start_ids, 0, 2);
			start_ids = start_ids + line_invert;
		} while (start_ids < sar_rc.height);
		sar_img.setNoDataValue(-9999, 1);
		//sar_img.setNoDataValue(-9999, 2);
	}

	// 正式计算插值
	{
		int line_invert = 500;
		line_invert = line_invert > 10 ? line_invert : 10;
		int start_ids = 0;
		do {

			std::cout << "rows:\t" << start_ids << "/" << sar_rc.height << "\t computing.....\t" << getCurrentTimeString() << endl;
			Eigen::MatrixXd	sar_r = sar_rc.getData(start_ids, 0, line_invert, sar_rc.width, 1);
			Eigen::MatrixXd	sar_c = sar_rc.getData(start_ids, 0, line_invert, sar_rc.width, 2);

			// 处理行列号获取范围
			int min_r = floor(sar_r.array().minCoeff());
			int max_r = ceil(sar_r.array().maxCoeff());
			int min_c = floor(sar_c.array().minCoeff());
			int max_c = ceil(sar_c.array().maxCoeff());

			if (max_r < 0 || max_c < 0 || min_r >= this->pstn.height || min_c >= this->pstn.width) {
				start_ids = start_ids + line_invert;
				if (start_ids < sar_rc.height) {
					continue;
				}
				else {
					break;
				}
			}

			if (start_ids == 13500) {
				int sdaf = 1;
			}

			min_r = min_r >= 0 ? min_r : 0;
			max_r = max_r < this->pstn.height ? max_r : this->pstn.height;
			min_c = min_c >= 0 ? min_c : 0;
			max_c = max_c < this->pstn.width ? max_c : this->pstn.width;

			// 获取影像
			int len_row = max_r - min_r + 4;
			Eigen::MatrixXd	ori_a = ori_sar.getData(min_r, 0, len_row, this->pstn.width, 1);
			//Eigen::MatrixXd	ori_b = ori_sar.getData(min_r, 0, len_row, this->pstn.width, 2);
			int row_count = ori_a.rows();
			int col_count = ori_a.cols();

			Eigen::MatrixX<double> ori(ori_a.rows(), ori_a.cols());
			for (int i = 0; i < row_count; i++) {
				for (int j = 0; j < col_count; j++) {
					//ori(i, j) = complex<double>{ ori_a(i, j), ori_b(i, j) };
					ori(i, j) = double{ ori_a(i, j)};
				}
			}
			ori_a = Eigen::MatrixXd(1, 1);
			//ori_b = Eigen::MatrixXd(1, 1); // 释放内存
			Eigen::MatrixXd	sar_a = sar_img.getData(start_ids, 0, line_invert, sar_rc.width, 1);
			//Eigen::MatrixXd	sar_b = sar_img.getData(start_ids, 0, line_invert, sar_rc.width, 2);
			sar_r = sar_rc.getData(start_ids, 0, line_invert, sar_rc.width, 1);
			sar_c = sar_rc.getData(start_ids, 0, line_invert, sar_rc.width, 2);
			row_count = sar_r.rows();
			col_count = sar_r.cols();
#pragma omp parallel for num_threads(8) // NEW ADD
			for (int i = 0; i < row_count; i++) {
				int r0, r1, c0, c1;
				double r, c;
				for (int j = 0; j < col_count; j++) {
					r = sar_r(i, j);
					c = sar_c(i, j);
					if (r < 0 || c < 0 || r >= this->pstn.height || c >= this->pstn.width) {
						continue;
					}
					else {
						int kkkkk = 0;

					}
					r1 = floor(r);
					r0 = r1 + 1; // 获取行
					c1 = floor(c);
					c0 = c1 + 1;  // 获取列
					// 考虑边界情况
					if (r1<=0 || c1<=0 || c0>= this->pstn.width || r0>=this->pstn.height) {
						continue;
					}
				/*	if (c1 <= 0 || c0 >= this->pstn.width || (r1 - min_r) <= 0 || (r0 - min_r) <= 0 || (r1 - min_r) >= ori_a.rows() || (r0 - min_r) >= ori_a.cols()) {
						continue;
					}*/
				/*	if ((r1 - min_r) <= 0 || (r0 - min_r) <= 0 || (r1 - min_r) >= ori_a.rows() || (r0 - min_r) >= ori_a.cols() || c1 >= ori_a.cols() || c0 >= ori_a.cols()) {
						continue;
					}*/
					Landpoint p0, p1, p2, p3, p4;
					//std::cout << "r= " << r << " r1= " << r1 << " c= " << c << " c1= " << c1 << " min_r = " << min_r << std::endl;
					p0 = Landpoint{ r - r1,c - c1,0 };
					//std::cout << "row= " << r1 - min_r << "col= " <<c1 << std::endl;
					//std::cout << "row1= " <<ori.rows() << "col1= " << ori.cols() << std::endl;
					p1 = Landpoint{ 0,0,ori(r1 - min_r, c1) }; // p11(0,0)
					p2 = Landpoint{ 0,1,ori(r1 - min_r, c0) }; // p21(0,1)
					p3 = Landpoint{ 1,0,ori(r0 - min_r , c1) }; // p12(1,0)
					p4 = Landpoint{ 1,1,ori(r0 - min_r, c0) }; // p22(1,1)
					p0.ati = Bilinear_interpolation(p0, p1, p2, p3, p4);
					//complex<double> interpolation_value = Bilinear_interpolation(p0, p1, p2, p3, p4);

					sar_a(i, j) = p0.ati;
				}
			}
			sar_img.saveImage(sar_a, start_ids, 0, 1);
			//sar_img.saveImage(sar_b, start_ids, 0, 2);
			start_ids = start_ids + line_invert;
		} while (start_ids < sar_rc.height);
	}

	std::cout << "interpolation(cubic convolution) orth sar value by rc_wgs84 and ori_sar image and model,  over:\t" << getCurrentTimeString() << std::endl;

}


void simProcess::lee_process(std::string in_ori_sar_path, std::string out_orth_sar_path, int w_size, double noise_var = 0.25)
{
	std::cout << "lee process, begin time:" << getCurrentTimeString() << std::endl;

	gdalImage sar_rc(in_ori_sar_path);

	gdalImage sar_img = CreategdalImage(out_orth_sar_path, sar_rc.height, sar_rc.width, 1, sar_rc.gt, sar_rc.projection);

	// 初始化
	{
		sar_rc.InitInv_gt();
		int line_invert = 100;
		int start_ids = 0;
		do {
			Eigen::MatrixXd	sar_a = sar_img.getData(start_ids, 0, line_invert, sar_rc.width, 1);
			sar_a = sar_a.array() * 0;
			sar_img.saveImage(sar_a, start_ids, 0, 1);
			start_ids = start_ids + line_invert;
		} while (start_ids < sar_rc.height);
		sar_img.setNoDataValue(0, 1);
	}

	// 滤波
	{
		int line_invert = 600;
		line_invert = line_invert > 10 ? line_invert : 10;
		int start_ids = 0;
		do {

			std::cout << "rows:\t" << start_ids << "/" << sar_rc.height << "\t computing.....\t" << getCurrentTimeString() << endl;
			Eigen::MatrixXd	sar_r = sar_rc.getData(start_ids, 0, line_invert, sar_rc.width, 1);
			Eigen::MatrixXd	sar_a = sar_img.getData(start_ids, 0, line_invert, sar_rc.width, 1);
			int row_count = sar_r.rows() - 100;
			int col_count = sar_r.cols();
			int win_s = floor(w_size / 2);
			int count = w_size * w_size;
#pragma omp parallel for num_threads(8) // NEW ADD
			for (int i = 0; i < row_count; i++) {
				int r0, r1, r2, r3, c0, c1, c2, c3;
				double r, c, snr;
				for (int j = 0; j < col_count; j++) {
					double sum = 0;
					double ave = 0;
					double sumSquared = 0;
					double variance = 0;
					//((i - win_s) < 0 || (j - win_s) < 0 || (i + win_s) > sar_rc.height || (j + win_s) > sar_rc.width)
					if (i == 0 || j == 0 || i == sar_rc.height || j == sar_rc.width || (i - win_s) <= 0 || (j - win_s) <= 0 || (i + win_s) >= sar_rc.height || (j + win_s) >= sar_rc.width)
					{
						sar_a(i, j) = sar_r(i, j);
					}
					else
					{
						r0 = i - win_s;
						r1 = i + win_s;
						c0 = j - win_s;
						c1 = j + win_s;
						// 计算窗口均值与方差
						for (int n = r0; n <= r1; n++)
						{
							for (int m = c0; m <= c1; m++)
							{
								sum += sar_r(n, m);
								sumSquared += sar_r(n, m) * sar_r(n, m);
							}
						}
						//均值
						ave = sum / count;
						//方差
						variance = sumSquared / count - ave * ave;
						if (variance == 0) {
							sar_a(i, j) = sar_r(i, j);
						}
						else {
							sar_a(i, j) = sar_r(i, j) + noise_var * (ave - sar_r(i, j));
							
						}
					}
					
				}
			}
			sar_img.saveImage(sar_a, start_ids, 0, 1);
			start_ids = start_ids + line_invert - 100;
		} while (start_ids < sar_rc.height);

	}

}



/// <summary>
/// 根据RPC的行列号，生成RPC对应的DEM文件
/// </summary>
/// <param name="in_rpc_rc_path"></param>
/// <param name="in_dem_path"></param>
/// <param name="out_rpc_dem_path"></param>
void simProcess::CreateRPC_DEM(std::string in_rpc_rc_path, std::string in_dem_path, std::string out_rpc_dem_path)
{
	gdalImage rpc_rc(in_rpc_rc_path);
	gdalImage dem_img(in_dem_path);
	gdalImage rpc_dem_img = CreategdalImage(out_rpc_dem_path, rpc_rc.height, rpc_rc.width, 1, rpc_rc.gt, rpc_rc.projection);

	// 初始化
	{
		dem_img.InitInv_gt();
		int line_invert = 100;
		int start_ids = 0;
		do {
			Eigen::MatrixXd	rpc_dem = rpc_dem_img.getData(start_ids, 0, line_invert, rpc_dem_img.width, 1);
			rpc_dem = rpc_dem.array() * 0 - 9999;
			rpc_dem_img.saveImage(rpc_dem, start_ids, 0, 1);
			start_ids = start_ids + line_invert;
		} while (start_ids < rpc_dem_img.height);
		rpc_dem_img.setNoDataValue(-9999, 1);
	}

	// 插值计算结果
	{
		dem_img.InitInv_gt();
		int line_invert = 100;
		int start_ids = 0;
		do {

			std::cout << "rows:\t" << start_ids << "/" << rpc_dem_img.height << "\t computing.....\t" << getCurrentTimeString() << endl;
			Eigen::MatrixXd	rpc_dem = rpc_dem_img.getData(start_ids, 0, line_invert, rpc_dem_img.width, 1);
			Eigen::MatrixXd rpc_row = rpc_dem.array() * 0 - 1;
			Eigen::MatrixXd rpc_col = rpc_dem.array() * 0 - 1;
			int row_count = rpc_dem.rows();
			int col_count = rpc_dem.cols();
			for (int i = 0; i < row_count; i++) {
				for (int j = 0; j < col_count; j++) {
					Landpoint landpoint = rpc_dem_img.getLandPoint(i + start_ids, j, 0);
					Landpoint land_row_col = dem_img.getRow_Col(landpoint.lon, landpoint.lat); // x,y,z
					rpc_row(i, j) = land_row_col.lat; // row
					rpc_col(i, j) = land_row_col.lon; // col
				}
			}

			double min_r = floor(rpc_row.minCoeff()) - 1;
			double max_r = ceil(rpc_row.maxCoeff()) + 1;
			min_r = min_r >= 0 ? min_r : 0;
			int len_r = max_r - min_r;
			Eigen::MatrixXd dem = dem_img.getData(min_r, 0, len_r + 2, dem_img.width, 1);

			for (int i = 0; i < row_count; i++) {
				for (int j = 0; j < col_count; j++) {
					Landpoint p0 = Landpoint{ rpc_row(i,j),rpc_col(i,j),0 };
					Landpoint p1 = Landpoint{ floor(p0.lon),floor(p0.lat),0 }; p1.ati = dem(int(p1.lon - min_r), int(p1.lat)); p1 = Landpoint{ 0,0,p1.ati };
					Landpoint p2 = Landpoint{ floor(p0.lon),ceil(p0.lat),0 }; p2.ati = dem(int(p2.lon - min_r), int(p2.lat)); p2 = Landpoint{ 0,1,p2.ati };
					Landpoint p3 = Landpoint{ ceil(p0.lon),floor(p0.lat),0 }; p3.ati = dem(int(p3.lon - min_r), int(p3.lat)); p3 = Landpoint{ 1,0,p3.ati };
					Landpoint p4 = Landpoint{ ceil(p0.lon),ceil(p0.lat),0 }; p4.ati = dem(int(p4.lon - min_r), int(p4.lat)); p4 = Landpoint{ 1,1,p4.ati };
					p0 = Landpoint{ p0.lon - floor(p0.lon), p0.lat - floor(p0.lat), 0 };
					p0.ati = Bilinear_interpolation(p0, p1, p2, p3, p4);
					rpc_dem(i, j) = p0.ati;
				}
			}
			rpc_dem_img.saveImage(rpc_dem, start_ids, 0, 1);
			start_ids = start_ids + line_invert;
		} while (start_ids < rpc_dem_img.height);
	}


}

void simProcess::CreateRPC_refrenceTable(string in_rpc_tiff_path,string in_merge_dem,string out_rpc_lon_lat_tiff_path)
{
	CPLSetConfigOption("GDAL_FILENAME_IS_UTF8", "NO");
	CPLSetConfigOption("GDAL_DATA", "./data");
	GDALAllRegister();//注册驱动

	const char* pszTif = in_rpc_tiff_path.c_str();

	gdalImage rpc_img(in_rpc_tiff_path);

	GDALRPCInfo oInfo = rpc_img.getRPC();

	//设置RPC模型中所需的DEM路径
	char** papszTransOption = NULL;
	//papszTransOption = CSLSetNameValue(papszTransOption, "RPC_DEM", "E:\\DEM.img");   //设置DEM

	//使用RPC信息，DEM等构造RPC转换参数
	void* pRPCTransform = GDALCreateRPCTransformer(&oInfo, FALSE, 0, papszTransOption);


	// 创建影像

	gdalImage lon_lat_img = CreategdalImage(out_rpc_lon_lat_tiff_path, rpc_img.height, rpc_img.width, 2, rpc_img.gt, rpc_img.projection, false);
	// 计算角点
	string temp_dem_path = in_merge_dem + ".bak";
	double merge_gt[6] = {
		rpc_img.gt(0,0), rpc_img.gt(0,1), rpc_img.gt(0,2),
		rpc_img.gt(0,3), rpc_img.gt(0,4), rpc_img.gt(0,5)
	}	;
	ResampleGDAL(in_merge_dem.c_str(), temp_dem_path.c_str(), merge_gt, rpc_img.width, rpc_img.height, GRA_Bilinear);

	gdalImage merger_dem_tiff(temp_dem_path);
	double X[4] = { 0,0,rpc_img.width - 1,rpc_img.width - 1 };
	double Y[4] = { 0,rpc_img.height - 1,rpc_img.height - 1,0 };
	double Z[4] = { 0,0,0,0 };
	int sucess_num[4] = { false,false,false,false };
	GDALRPCTransform(pRPCTransform, FALSE, 4, X, Y, Z, sucess_num);
	for (int i = 0; i < 4; i++) {
		std::cout << X[i] << "\t" << Y[i] << "\t" << Z[i] << "\t" << sucess_num[i] << std::endl;
	}
	int line_invert = int(1500000 / rpc_img.width);
	line_invert = line_invert > 10 ? line_invert : 10;
	int count_lines = 0;
	omp_lock_t lock;
	omp_init_lock(&lock); // 初始化互斥锁
#pragma omp parallel for num_threads(6) // NEW ADD
	for (int max_rows_ids = 0; max_rows_ids < lon_lat_img.height; max_rows_ids = max_rows_ids + line_invert) {
		//line_invert = dem_clip.height - max_rows_ids > line_invert ? line_invert : dem_clip.height - max_rows_ids;

		Eigen::MatrixXd lon = lon_lat_img.getData(max_rows_ids, 0, line_invert, lon_lat_img.width, 1);
		Eigen::MatrixXd lat = lon_lat_img.getData(max_rows_ids, 0, line_invert, lon_lat_img.width, 2);
		Eigen::MatrixXd dem = merger_dem_tiff.getData(max_rows_ids, 0, line_invert, lon_lat_img.width, 2);
		int row_count = lon.rows();
		int col_count = lon.cols();
		for (int i = 0; i < row_count; i++) {
			for (int j = 0; j < col_count; j++) {
				//定义图像的四个角点行列号坐标
				double dY[1] = { i + max_rows_ids };
				double dX[1] = { j };
				double dZ[1] = { dem(i,j)};
				int nSuccess[1] = { FALSE };
				GDALRPCTransform(pRPCTransform, FALSE, 1, dX, dY, dZ, nSuccess);
				lon(i, j) = dX[0];
				lat(i, j) = dY[0];
			}
		}

		//std::cout << "for time " << getCurrentTimeString() << std::endl;
		// 写入到文件中
		omp_set_lock(&lock); //获得互斥器
		std::cout << lon.array().minCoeff() << endl;
		lon_lat_img.saveImage(lon, max_rows_ids, 0, 1);
		lon_lat_img.saveImage(lat, max_rows_ids, 0, 2);
		count_lines = count_lines + line_invert;
		std::cout << "rows:\t" << max_rows_ids << "\t-\t" << max_rows_ids + line_invert << "\t" << count_lines << "/" << lon_lat_img.height << "\t computing.....\t" << getCurrentTimeString() << endl;
		omp_unset_lock(&lock); //释放互斥器
	}
	omp_destroy_lock(&lock); //销毁互斥器
	//释放资源
	GDALDestroyRPCTransformer(pRPCTransform);
	CSLDestroy(papszTransOption);
}

/// <summary>
/// 映射表插值计算
/// </summary>
/// <param name="in_lon_lat_path">映射表</param>
/// <param name="in_radar_path">输入数据</param>
/// <param name="out_radar_path">输出数据</param>
void simProcess::CorrectionFromSLC2Geo(string in_lon_lat_path, string in_radar_path, string out_radar_path,int in_band_ids)
{
	gdalImage in_lon_lat(in_lon_lat_path);
	gdalImage in_image(in_radar_path);
	gdalImage out_radar(out_radar_path);

	cv::Mat in_mat;
	cv::Mat out_mat;

	cv::eigen2cv(in_image.getData(0, 0, in_lon_lat.height, in_lon_lat.width, in_band_ids),in_mat);
	
	// 定义映射关系
	cv::Mat in_x,in_y;
	cv::eigen2cv(in_lon_lat.getData(0, 0, in_lon_lat.height, in_lon_lat.width, 1), in_x);
	cv::eigen2cv(in_lon_lat.getData(0, 0, in_lon_lat.height, in_lon_lat.width, 1), in_y);

	// 重新进行插值计算

}
/// <summary>
/// 根据DEM创建SAR_rc
/// </summary>
/// <param name="dem_path"></param>
/// <param name="sim_rc_path"></param>
/// <param name="PSTN"></param>
/// <returns></returns>


double getRangeColumn(long double R, long double NearRange, long double Fs, long double lamda)
{
		return (double)(2 * (R - NearRange) / LIGHTSPEED * Fs); // 计算采样率;
}

Eigen::MatrixXd getRangeColumn(Eigen::MatrixXd R, long double NearRange, long double Fs, long double lamda)
{
	return ((R.array() - NearRange).array() / lamda * Fs).array().cast<double>();
}

double getRangebyColumn(double j, long double NearRange, long double Fs, long double lamda)
{
	return (j *  lamda / 2) /  Fs + NearRange;
 
}

Eigen::MatrixXd getRangebyColumn(Eigen::MatrixXd j, long double NearRange, long double Fs, long double lamda)
{
	return ((j.array() * lamda / 2) / Fs + NearRange).array().cast<double>();
}

double getlocalIncAngle(Landpoint& satepoint, Landpoint& landpoint, Landpoint& slopeVector) {
	Landpoint Rsc = satepoint - landpoint; // AB=B-A
	//double R = getlength(Rsc);
	//std::cout << R << endl;
	double angle = getAngle(Rsc, slopeVector);
	if (angle >= 180) {
		return 360 - angle;
	}
	else {
		return angle;
	}
}

double getIncAngle(Landpoint& satepoint, Landpoint& landpoint) {

	Landpoint Rsc = satepoint - landpoint; // AB=B-A
	Landpoint Rs = landpoint;// landpoint;
	double angle = getAngle(Rsc, Rs);
	if (angle >= 180) {
		return 360 - angle;
	}
	else {
		return angle;
	}
}




////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////    ASF 计算                   ///////////////////////////////////////////////////
////// 《星载合成孔径雷达影像正射校正方法研究》  陈尔学  /////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////

/// <summary>
/// 计算 变换矩阵 卫星坐标系-> ECR
/// </summary>
/// <param name="sataspace">[x,y,z,vx,vy,vz]   nx6 </param>
/// <returns></returns>
Eigen::MatrixXd ASFOrthClass::Satellite2ECR(Eigen::Vector3d Rsc, Eigen::Vector3d Vsc)
{

	Eigen::Vector3d x, y, z;

	Eigen::MatrixXd M(3, 3);

	z = Rsc / Rsc.norm();
	y = Rsc.cross(Vsc) / (Rsc.norm() * Vsc.norm());
	x = y.cross(z);

	M.col(0) = x;
	M.col(1) = y;
	M.col(2) = z;
	return M;
}
/// <summary>
/// 获取从卫星指向地面目标的单位矢量
/// </summary>
/// <param name="alpha">弧度值</param>
/// <param name="beta">弧度值</param>
/// <param name="M">变换矩阵</param>
/// <returns></returns>
Eigen::Vector3d ASFOrthClass::UnitVectorOfSatelliteAndTarget(double alpha, double beta, Eigen::MatrixXd M)
{
	// 目标T的单位向量 ST = R * M * P
	Eigen::Vector3d P(sin(alpha), -1.0 * cos(alpha) * sin(beta), -1.0 * cos(alpha) * cos(beta));
	return  M * P;
}

/// <summary>
/// 
/// </summary>
/// <param name="R"></param>
/// <param name="alpha"></param>
/// <param name="beta"></param>
/// <param name="SatellitePosition"></param>
/// <param name="SatelliteVelocity"></param>
/// <param name="R_threshold"></param>
/// <param name="H"></param>
/// <returns></returns>
double ASFOrthClass::GetLookFromRangeYaw(double R, double alpha, double beta, Eigen::Vector3d SatellitePosition, Eigen::Vector3d SatelliteVelocity, double R_threshold, double H)
{
	/**
		根据R和alpha 计算出beta来。
		根据参考论文，式3 - 13，完成此方法。（注意这是近似法
		args :
		R：斜距
			alpha : 雷达侧视角
			beta0 : beta初始值
			SatellitePosition : 3x1 卫星空间位置
			TargetPosition：3x1 目标对象
	*/
	Eigen::MatrixXd M = this->Satellite2ECR(SatellitePosition, SatelliteVelocity);
	// 地球半径
	double Rp = earth_Rp + H;
	double Rs_norm = SatellitePosition.norm();
	// 初始化beta
	if (beta == 0) {
		double beta_cos = (Rs_norm * Rs_norm + R * R - Rp * Rp) / (2 * Rs_norm * R);
		beta = acos(beta_cos);
	}

	double  delta_R, sin_eta, delta_beta, tan_eta;
	// 迭代
	int i = 0;
	do {
		// 计算斜率的增加
		delta_R = R - this->FR(alpha, beta, SatellitePosition, M, R_threshold, H);
		// 计算入射角
		sin_eta = Rs_norm * sin(beta) / Rp;
		tan_eta = sin_eta / sqrt(1 - sin_eta * sin_eta);
		// 计算增量
		delta_beta = delta_R / (R * tan_eta);
		// 更新
		beta = beta + delta_beta;
		// 判断循环终止条件
		i = i + 1;
		if (i >= 10000) // 达到终止条件
		{
			return -9999;
		}
	} while (abs(delta_R) > 0.01);
	return beta;
}

/// <summary>
/// 从beta，alpha获取获取目标的空间位置
/// </summary>
/// <param name="alpha"></param>
/// <param name="beta"></param>
/// <param name="SatellitePosition"></param>
/// <param name="R"></param>
/// <param name="M"></param>
/// <returns></returns>
Eigen::Vector3d ASFOrthClass::GetXYZByBetaAlpha(double alpha, double beta, Eigen::Vector3d SatellitePosition, double R, Eigen::MatrixXd M)
{
	return SatellitePosition + R * this->UnitVectorOfSatelliteAndTarget(alpha, beta, M);
}

/// <summary>
/// FD
/// </summary>
/// <param name="alpha"></param>
/// <param name="beta"></param>
/// <param name="SatelliteVelocity"></param>
/// <param name="TargetVelocity"></param>
/// <param name="R"></param>
/// <param name="lamda"></param>
/// <param name="M"></param>
/// <returns></returns>
double ASFOrthClass::FD(double alpha, double beta, Eigen::Vector3d SatelliteVelocity, Eigen::Vector3d TargetVelocity, double R, double lamda, Eigen::MatrixXd M)
{
	/**
		根据beta, alpha, 卫星空间位置, 斜距，转换矩阵
	*/
	Eigen::Vector3d UnitVector = this->UnitVectorOfSatelliteAndTarget(alpha, beta, M);
	Eigen::Vector3d Rst = -1 * R * UnitVector;
	double	Rst_Vst = Rst.dot(SatelliteVelocity - TargetVelocity);
	return -2 / (R * lamda) * Rst_Vst;
}


/// <summary>
/// FR
/// </summary>
/// <param name="alpha"></param>
/// <param name="beta"></param>
/// <param name="SatellitePosition"></param>
/// <param name="M"></param>
/// <param name="R_threshold"></param>
/// <param name="H"></param>
/// <returns></returns>
double ASFOrthClass::FR(double alpha, double beta, Eigen::Vector3d SatellitePosition, Eigen::MatrixXd M, double R_threshold, double H)
{
	/*
	根据 雷达侧视角，雷达视角，卫星位置，坐标系变换矩阵，大地高等参数，根据参考论文中公式3 - 1 == 》3 - 10的计算过程，设计此方程
	args :
		alpha:雷达侧视角 卫星与地物间速度运动差导致的（即多普勒频移的结果）
		beta : 雷达视角
		Satellite_position : 卫星空间位置
		M：坐标系转换矩阵
		H：大地高
	return :
		R：斜距
	*/
	//std::cout << beta << endl;
	Eigen::Vector3d UnitVector = R_threshold * this->UnitVectorOfSatelliteAndTarget(alpha, beta, M);
	// 解方程 3 - 10 纠正错误，A公式有问题
	double a = (earth_Re + H) * (earth_Re + H);
	double b = earth_Rp * earth_Rp;
	long double Xs = SatellitePosition(0);
	long double Ys = SatellitePosition(1);
	long double Zs = SatellitePosition(2);
	long double Xp = UnitVector(0);
	long double Yp = UnitVector(1);
	long double Zp = UnitVector(2);
	//std::cout << Xp<<"\t"<< Yp<<"\t"<< Zp << endl;
	long double A = (Xp * Xp + Yp * Yp) / a + (Zp * Zp) / b;
	long double B = 2 * (Xs * Xp + Ys * Yp) / a + 2 * Zs * Zp / b;
	long double C = (Xs * Xs + Ys * Ys) / a + Zs * Zs / b - 1;
	//std::cout << A << "\t" << B << "\t" << C << endl;
	C = B * B - 4 * A * C;
	//std::cout << A << "\t" << B << "\t" << C << endl;
	double R1 = (-B - sqrt(C)) / (2 * A);
	//double R2 = (-B + sqrt(C)) / (2 * A);
	//std::cout << R1 << "\t" << R2 << endl;
	if (R1 > 1) {
		return R1 * R_threshold;
	}
	else {
		return -9999;
		//return (-B + math.sqrt(t)) / (2 * A) # 这里通过几何分析排除这个解
	}

}

/// <summary>
/// ASF方法
/// </summary>
/// <param name="R"></param>
/// <param name="SatellitePosition">卫星高度</param>
/// <param name="SatelliteVelocity">卫星速度</param>
/// <param name="TargetPosition">初始化地面坐标</param>
/// <param name="PSTN"></param>
/// <param name="R_threshold"></param>
/// <param name="H">初始高程</param>
/// <param name="alpha0"></param>
/// <param name="beta0"></param>
/// <returns></returns>
Eigen::Vector3d ASFOrthClass::ASF(double R, Eigen::Vector3d SatellitePosition, Eigen::Vector3d SatelliteVelocity, Eigen::Vector3d TargetPosition, PSTNAlgorithm PSTN, double R_threshold, double H, double alpha0, double beta0)
{
	// 部分参数初始化
	double alpha = alpha0;
	double beta = beta0;
	double delta_alpha = 0;
	// 这个公式是根据《中国海洋合成孔径雷达卫星工程、产品与处理》
	// P164 5.8.4 多普勒参数计算
	double fd = PSTN.calNumericalDopplerValue(R);
	Eigen::MatrixXd	M = this->Satellite2ECR(SatellitePosition, SatelliteVelocity);
	double FD_, delta_fd;
	Eigen::Vector3d XYZ;
	int i = 0;// 统计迭代次数
	while (true) {
		Eigen::Vector3d	TargetVelocity = Eigen::Vector3d(-1 * earth_We * TargetPosition(1), earth_We * TargetPosition(0), 0);// 计算地面速度, 粗糙计算时，默认为0
		beta = this->GetLookFromRangeYaw(R, alpha, beta, SatellitePosition, SatelliteVelocity, R_threshold, H);
		FD_ = this->FD(alpha, beta, SatelliteVelocity, TargetVelocity, R, PSTN.lamda, M);
		delta_fd = FD_ - fd;
		delta_alpha = -1 * delta_fd * PSTN.lamda / (2 * (SatelliteVelocity - TargetVelocity).norm());
		TargetPosition = this->GetXYZByBetaAlpha(alpha, beta, SatellitePosition, R, M);
		//cout << i << "\t" << delta_alpha * R << "\t" << delta_alpha<<"\t"<< (abs(delta_alpha * R) < 0.1)<<"\t"<< (abs(delta_alpha) < 0.001) << endl;
		if (abs(delta_alpha * R) < 0.1 || abs(delta_alpha) < 0.0003)
		{
			break;
		}
		alpha = alpha + delta_alpha;
		i = i + 1;
		if (i > 10000) {
			throw new exception("ASF 失败");
		}

	}
	return TargetPosition;
}