#pragma once
#include <iostream>
#include <memory>
#include <vector>
#include <future>
#include <complex>
#include "gdalwarper.h"

#define PI_180 180/3.141592653589793238462643383279;
#define T180_PI 3.141592653589793238462643383279/180;

#define Radians2Degrees(Radians) Radians*PI_180
#define Degrees2Radians(Degrees) Degrees*T180_PI

const long double PI=3.141592653589793238462643383279;
const long double epsilon = 0.000000000000001;
const long double pi = 3.14159265358979323846;
const long double d2r = pi / 180;
const long double r2d = 180 / pi;

const long double a = 6378137.0;		//椭球长半轴
const long double f_inverse = 298.257223563;			//扁率倒数
const long double b = a - a / f_inverse;
const long double e = sqrt(a * a - b * b) / a;
const long double eSquare = e*e;



int testPP();

using namespace std;
///
/// 内敛函数
/// 

struct point //  点   SAR影像的像素坐标；
{
	long double x; // 纬度 lat pixel_row
	long double y; // 经度 lon pixel_col
	long double z; // 高程 ati pixel_time
};

struct VectorPoint {
	long double x;
	long double y;
	long double z;
};
/// <summary>
/// 将经纬度转换为地固参心坐标系
/// </summary>
/// <param name="XYZP">经纬度点--degree</param>
/// <returns>投影坐标系点</returns>
inline point LLA2XYZ(point& LLA) {
	long double L = LLA.x * d2r;
	long double B = LLA.y * d2r;
	long double H = LLA.z;

	long double sinB = sin(B);
	long double cosB = cos(B);
	 
	//long double N = a / sqrt(1 - e * e * sin(B) * sin(B));
	long double N = a / sqrt(1 - eSquare * sinB * sinB);
	point result = { 0,0,0 };
	result.x = (N + H) * cosB * cos(L);
	result.y = (N + H) * cosB * sin(L);
	//result.z = (N * (1 - e * e) + H) * sin(B);
	result.z = (N * (1 - eSquare) + H) * sinB;
	return result;
}

/// <summary>
/// 将地固参心坐标系转换为经纬度
/// </summary>
/// <param name="XYZ">固参心坐标系</param>
/// <returns>经纬度--degree</returns>
point XYZ2LLA(point& XYZ);

/// <summary>
/// 计算两个点之间的XY平面欧式距离的平方
/// </summary>
/// <param name="p1"></param>
/// <param name="p2"></param>
/// <returns></returns>
inline long double caldistanceXY(point& p1, point& p2) {
	//return pow(p1.x - p2.x, 2) + pow(p1.y - p2.y, 2);
	return (p1.x - p2.x)* (p1.x - p2.x) + (p1.y - p2.y)* (p1.y - p2.y);
}

/// <summary>
/// 使用两个点，生成向量 p1-->p2
/// </summary>
/// <param name="p1">p1</param>
/// <param name="p2">p2</param>
/// <returns>向量</returns>
inline VectorPoint getVector(point& p1, point& p2) {
	VectorPoint result = { 0,0,0 };
	result.x = p2.x - p1.x;
	result.y = p2.y - p1.y;
	result.z = p2.z - p1.z;
	return result;
}

/// <summary>
/// 获取向量的模
/// </summary>
/// <param name="vector1">向量</param>
/// <returns>向量模</returns>
inline long double getModule(VectorPoint& vector1) {
	//return sqrt(pow(vector1.x, 2) + pow(vector1.y, 2) + pow(vector1.z, 2));
	return sqrt(vector1.x* vector1.x + vector1.y* vector1.y + vector1.z* vector1.z);
}

inline long double getModuleV1V2(VectorPoint& v1, VectorPoint& v2)
{
	return sqrt((v1.x * v1.x + v1.y * v1.y + v1.z * v1.z) * (v2.x * v2.x + v2.y * v2.y + v2.z * v2.z));
}

/// <summary>
/// 向量夹角的角度( 角度）
/// </summary>
/// <param name="vector1">向量</param>
/// <returns>向量夹角的角度</returns>
inline long double getVectorAngle(VectorPoint& vector1,VectorPoint& vector2) {
	//return Radians2Degrees( acos((vector1.x * vector2.x + vector1.y * vector2.y + vector1.z * vector2.z) / (getModule(vector1) * getModule(vector2))));
	return Radians2Degrees(acos((vector1.x * vector2.x + vector1.y * vector2.y + vector1.z * vector2.z) / (getModuleV1V2(vector1, vector2))));

}


/// <summary>
/// 向量的夹角值
/// </summary>
/// <param name="vector1">向量</param>
/// <returns>向量夹角的角度</returns>
inline long double getVectorAngleValue(VectorPoint& vector1, VectorPoint& vector2) {
	//return (vector1.x * vector2.x + vector1.y * vector2.y + vector1.z * vector2.z) / (getModule(vector1) * getModule(vector2));
	return (vector1.x * vector2.x + vector1.y * vector2.y + vector1.z * vector2.z) / (getModuleV1V2(vector1, vector2));
}
/// <summary>
/// 向量点乘
/// </summary>
/// <param name="V1">向量1</param>
/// <param name="v2">向量2</param>
/// <returns>点乘值</returns>
inline long double Vectordot(VectorPoint& V1, VectorPoint& v2) {
	return V1.x * v2.x + V1.y * v2.y + V1.z * v2.z;
}

/// <summary>
/// 向量点乘
/// </summary>
/// <param name="V1">向量1</param>
/// <param name="lamda">系数值</param>
/// <returns>向量与数之间的插值</returns>
inline VectorPoint VectordotNumber(VectorPoint& V1, long double lamda) {
	V1.x = V1.x * lamda;
	V1.y = V1.y * lamda; 
	V1.z = V1.z * lamda;
	return V1;
}

/// <summary>
/// 向量叉乘
/// 旋转方向：v1->v2
/// </summary>
/// <param name="v1">v1</param>
/// <param name="v2">v2</param>
/// <returns>叉乘的结果向量</returns>
inline VectorPoint VectorFork(VectorPoint &v1, VectorPoint& v2) {
	VectorPoint result{ 0,0,0 };
	result.x = v1.y * v2.z - v1.z * v2.y;
	result.y =v1.z*v2.x -v1.x * v2.z;
	result.z = v1.x * v2.y - v1.y * v2.x;
	return result;
}


//
// 参数文件解析
//

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

class ParameterInFile
{
public:
	ParameterInFile(std::string infile_path);
	ParameterInFile(const ParameterInFile& paras);
	~ParameterInFile();
public:
	//参数组
	std::string dem_path; //dem 路径
	std::string out_sar_sim_path; // 输出模拟sar
	std::string out_sar_sim_dem_path; // 输出模拟sar
	std::string out_sar_sim_resampling_path; // 输出模拟sar
	std::string out_sar_sim_resampling_rc;
	int sim_height; // 模拟影像的高
	int sim_width;
	long double widthspace;// 距离向分辨率
	std::string sar_sim_match_point_path; //输出模拟影像的地点x
	std::string sar_sim_match_point_xyz_path; //输出模拟影像的地点x
	int sample_f; //采样率
	long double R0; //近斜距
	long double LightSpeed;//光速
	long double lamda;//波长
	long double refrange;// 参考斜距
	
	long double delta_R; // 斜距间隔
	long double imgStartTime; //成像起始时间
	long double PRF;// 脉冲重复率
	long double delta_t;// 时间间隔
	// 多普勒
	int doppler_paramenter_number;// 多普勒系数个数
	long double* doppler_para;//多普勒系数
	//卫星轨道模型
	int polySatelliteModel;// 是否为卫星多轨道模型
	int polynum;// 多项数
	long double SatelliteModelStartTime;
	long double* polySatellitePara;
};

// 根据轨道模型计算卫星空间位置
struct SatelliteSpacePoint {
	long double x=0;
	long double y=0;
	long double z=0;
	long double vx=0;
	long double vy=0;
	long double vz=0;

};


/// <summary>
/// 根据卫星轨道模型计算卫星
/// </summary>
/// <param name="satelliteTime">卫星轨道点时间</param>
/// <param name="SatelliteModelStartTime">卫星轨道模型起始时间</param>
/// <param name="polySatellitePara">卫星轨道坐标模型参数</param>
/// <param name="polynum">多项式项数</param>
/// <returns></returns>
inline SatelliteSpacePoint getSatellitePostion(long double satelliteTime,long double SatelliteModelStartTime,long double* polySatellitePara,int polynum);

/// <summary>
///  数值模拟法计算多普勒频移值
/// </summary>
/// <param name="R">斜距</param>
/// <param name="LightSpeed">光速</param>
/// <param name="T0">多普勒参考时间</param>
/// <param name="doppler_para">多普勒参数</param>
/// <returns>多普勒频移值</returns>
inline long double calNumericalDopplerValue(long double R,long double LightSpeed,long double T0, long double* doppler_para,int doppler_paramenter_number);

/// <summary>
/// 根据理论模型计算多普勒频移值
/// </summary>
/// <param name="R">斜距</param>
/// <param name="lamda">波长</param>
/// <param name="R_sl">地面->卫星的空间向量</param>
/// <param name="V_sl">地面->卫星之间的速度向量</param>
/// <returns>多普勒频移值</returns>
inline long double calTheoryDopplerValue(long double R, long double lamda, VectorPoint R_sl, VectorPoint V_sl);

/// <summary>
/// 根据地面点求解对应的sar影像坐标
/// </summary>
/// <param name="landpoint">地面点的坐标--地固坐标系</param>
/// <param name="Starttime">影片开始成像时间</param>
/// <param name="lamda">波长</param>
/// <param name="T0">多普勒参考时间</param>
/// <param name="LightSpeed">光速</param>
/// <param name="delta_t">时间间隔</param>
/// <param name="R0">近斜距</param>
/// <param name="delta_R">斜距间隔</param>
/// <param name="SatelliteModelStartTime">卫星轨道模型时间</param>
/// <param name="polySatellitePara">卫星轨道坐标模型参数</param>
/// <param name="polynum">卫星轨道模型项数</param>
/// <param name="doppler_paramenter_number">多普勒模型数</param>
/// <returns>影像坐标（x:行号，y:列号，z：成像时刻）</returns>
inline point PSTN(point& landpoint, long double Starttime, long double lamda, long double T0, long double* doppler_para, long double LightSpeed, long double delta_t, long double R0, long double delta_R, long double SatelliteModelStartTime, long double* polySatellitePara, int polynum = 4, int doppler_paramenter_number = 5);



struct translateArray {
	long double a0, a1, a2;
	long double b0, b1, b2;
};
/// <summary>
///  转换影像
/// </summary>
/// <param name="row_ids"></param>
/// <param name="col_ids"></param>
/// <param name="value"></param>
/// <param name="gt"></param>
/// <returns></returns>
inline point Translation(long double row_ids,long double col_ids,long double value,translateArray& gt) {

	point result{ 0,0,0 };
	result.x = gt.a0 + gt.a1 * col_ids + gt.a2 * row_ids;
	result.y = gt.b0 + gt.b1 * col_ids + gt.b2 * row_ids;
	result.z = value;
	return result;
}

inline int Translation(long double& x, long double& y, long double& r, long double& c, translateArray& gt) {

	c = gt.a0 + gt.a1 * x + gt.a2 * y;
	r = gt.b0 + gt.b1 * x + gt.b2 * y;
	return 0;
}

/// <summary>
/// dem块
/// </summary>
class dem_block {

public:
	dem_block(int all_start_row,int all_start_col,int start_row, int end_row, int start_col, int end_col, int height, int width,int sample_f);
	dem_block(const dem_block& demblocks);
	~dem_block();
	dem_block resample_dem();
	//dem_block resample_dem_cudic();
	int rowcol2blockids(int row_ids, int col_ids);
	point getpointblock(int row_ids, int col_ids);
	int setpointblock(int row_ids, int col_ids, point& value);
	point getpointblock(int ids);
	int setpointblock(int ids, point& value);
	int UpdatePointCoodinarary();
	VectorPoint getslopeVector(int row_ids, int col_ids);
public:
	int all_start_row;
	int all_start_col;
	int start_row; // 目标区域的起始行号 
	int end_row; //
	int start_col; // 目标区域的起始列号
	int end_col;
	int height; 
	int width;
	int size;
	int sample_f;
	point* pointblock; // 原始块
};



inline point bilineadInterpolation(point& p,point& p11,point& p12,point& p21,point& p22);

inline point cubicInterpolation(point& p, point& p11, point& p12,point& p13,point& p14, point& p21, point& p22,point& p23,point& p24,point& p31,point& p32,point& p33,point& p34,point& p41,point& p42,point& p43,point& p44);

/// <summary>
/// 双线性插值方法
/// </summary>
/// <param name="p"></param>
/// <param name="p11"></param>
/// <param name="p12"></param>
/// <param name="p21"></param>
/// <param name="p22"></param>
/// <returns></returns>
inline point SARbilineadInterpolation(point& p, point& p11, point& p12, point& p21, point& p22) {




}
/// <summary>
/// 入射角 -- 弧度制
/// </summary>
struct IncidenceAngle
{
	long double incidenceAngle; // 雷达入射角
	long double localincidenceAngle; // 局地入射角
};

struct matchPoint {
	long double r, c, ti;
	long double land_x, land_y, land_z;
	long double distance;
	long double incidenceAngle, localincidenceAngle;
};
/// <summary>
///  模拟sar 的矩阵块，累加模块默认使用short 类型(累加上限：
/// </summary>
class sim_block {
public:
	sim_block(int start_row,int end_row,int start_col,int end_col,int height,int width);
	sim_block(const sim_block& sim_blocks);
	~sim_block();

	int rowcol2blockids(int row_ids, int col_ids);
	short getsimblock(int row_ids, int col_ids);
	int setsimblock(int row_ids,int col_ids, short value);
	int addsimblock(int row_ids, int col_ids,short value);
	matchPoint getpointblock(int row_ids,int col_ids);
	int setpointblock(int row_ids, int col_ids, matchPoint value);
	// 
	long double getdistanceblock(int row_ids, int col_ids);
	int setdistanceblock(int row_ids, int col_ids, long double value);

public:
	int start_row;
	int end_row;
	int start_col;
	int end_col;
	int height;
	int width;
	int size;
	short* block;
	long double* distanceblock;
	matchPoint* pointblock; 
};


/// <summary>
/// 根据卫星坐标，地面坐标，地面法向量，求解对应的雷达入射角和局地入射角
/// </summary>
/// <param name="satellitepoint">卫星空间坐标</param>
/// <param name="landpoint">地面坐标</param>
/// <param name="slopvector">地面坡度</param>
/// <returns>入射角文件</returns>
inline IncidenceAngle calIncidence(point satellitepoint,point landpoint,VectorPoint slopvector) {
	
	IncidenceAngle result;
	VectorPoint R_ls = getVector(landpoint, satellitepoint);
	result.localincidenceAngle = getVectorAngleValue(R_ls, slopvector);
	VectorPoint R_s{ satellitepoint.x,satellitepoint.y,satellitepoint.z };
	result.incidenceAngle = getVectorAngleValue(R_s, R_ls);
	return result;
}


int SimProcessBlock(dem_block demblock, ParameterInFile paras, matchPoint* result_shared, int* Pcount);

int ResamplingSim(ParameterInFile paras);
int ResampleGDAL(const char* pszSrcFile, const char* pszOutFile, float fResX, float fResY, GDALResampleAlg eResample);

/// <summary>
/// 模拟sar的处理算法模块。
/// 为了控制内存的取用
/// 线程数：8 --以进程类的方式进行管理--使用队列的方法
/// 线程内存：dem_block_size*sample_f*sample_f< 1 G
/// </summary>
/// <param name="paras">参数文件项</param>
/// <param name="thread_num">线程数默认为8</param>
/// <returns>执行情况</returns>
int SimProcess(ParameterInFile paras, int thread_num);
int SimProcess_LVY(ParameterInFile paras, int thread_num);
int WriteMatchPoint(string path, std::vector<matchPoint>* matchps);
// 测试函数接口

int testPTSN(ParameterInFile paras);


///
///
///  考虑影像的插值方案
/// 
/// 

class ConvertResampleParameter {
public:
	ConvertResampleParameter(string path);
	ConvertResampleParameter(const ConvertResampleParameter& para);
	~ConvertResampleParameter();
public:
	string in_ori_dem_path;
	string ori_sim;
	string out_sar_xyz_path;
	string out_sar_xyz_incidence_path;
	string out_orth_sar_incidence_path; 
	string out_orth_sar_local_incidence_path;
	string outFolder_path;
	int file_count;
	std::vector<string> inputFile_paths;
	std::vector<string> outFile_paths;
	std::vector<string> outFile_pow_paths;
	int ori2sim_num;
	long double* ori2sim_paras;
	int sim2ori_num;
	long double* sim2ori_paras;
};

inline int ori2sim(long double ori_x,long double ori_y,long double& sim_x,long double& sim_y,long double* conver_paras) {
	long double xy = ori_x * ori_y;
	long double x2 = ori_x * ori_x;
	long double y2 = ori_y * ori_y;

	sim_x = conver_paras[0] + ori_x * conver_paras[1] + ori_y * conver_paras[2] + x2 * conver_paras[3] + y2 * conver_paras[4] + xy * conver_paras[5];
	sim_y = conver_paras[6] + ori_x * conver_paras[7] + ori_y * conver_paras[8] + x2 * conver_paras[9] + y2 * conver_paras[10] + xy * conver_paras[11];
	return 1;
}

/// <summary>
/// 将模拟的行列号转换为目标行列号
/// </summary>
/// <param name="sim_r">模拟的行号</param>
/// <param name="sim_c">模拟的列号</param>
/// <param name="ori_r">待计算的目标行号</param>
/// <param name="ori_c">待计算的目标列号</param>
/// <param name="conver_paras">变换矩阵</param>
/// <returns>默认：0 表示计算结束</returns>
inline int  sim2ori(long double sim_r,long double sim_c,long double& ori_r,long double& ori_c, long double* conver_paras) {
	long double xy = sim_r * sim_c;
	long double x2 = sim_r * sim_r;
	long double y2 = sim_c * sim_c;
	ori_r = conver_paras[0] + sim_r * conver_paras[1] + sim_c * conver_paras[2] + x2 * conver_paras[3] + y2 * conver_paras[4] + xy * conver_paras[5];
	ori_c = conver_paras[6] + sim_r * conver_paras[7] + sim_c * conver_paras[8] + x2 * conver_paras[9] + y2 * conver_paras[10] + xy * conver_paras[11];
	return 0;
}

// 查询精确坐标

point GetOriRC(point& landp, ParameterInFile& paras, ConvertResampleParameter& convparas);

int SimProcessBlock_CalXYZ(dem_block demblock, ParameterInFile paras, ConvertResampleParameter converParas, matchPoint* result_shared, int* Pcount);
int SimProcess_CalXYZ(ParameterInFile paras, ConvertResampleParameter conveparas, int thread_num);


/*
 正射模块思路
 dem->sim->ori(r,c) <-重采样-> ori 
 step1: 计算dem 对应的 ori 坐标，并保存-> ori_sim.tif  (r,c,incidence,localincidence)  
 step2: 根据结果插值计算对应的a,b

*/
int SimProcess_Calsim2ori(ParameterInFile paras, ConvertResampleParameter conveparas, int thread_num); // 正射模块


/*
重采样：
step1：读取目标栅格，并根据目标栅格创建对象
step2: 
*/
int SimProcess_ResamplingOri2Orth(ParameterInFile paras, ConvertResampleParameter conveparas, int thread_num);

/*
生成强度图
*/
int SimProcess_Calspow(ParameterInFile paras, ConvertResampleParameter conveparas, int thread_num); // 正射模块