#pragma once
///
/// 基本类、基本函数
/// 
//#define EIGEN_USE_MKL_ALL
//#define EIGEN_VECTORIZE_SSE4_2
//#include <mkl.h>

//#include <mkl.h>
#include <iostream>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <time.h>
#include <string>
#include <omp.h>
#include <complex>
#include <gdal.h>
#include <gdal_priv.h>
#include <gdalwarper.h>
#include <ogrsf_frmts.h> 
#include <opencv2/opencv.hpp>
#include <opencv2/core/eigen.hpp>
#include <opencv2/features2d.hpp>
#include <fstream>


using namespace std;
using namespace Eigen;

#define PI_180 180/3.141592653589793238462643383279
#define T180_PI 3.141592653589793238462643383279/180
#define LIGHTSPEED 299792458


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








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

const double a = 6378137.0;		//椭球长半轴
const double ae = 6378137.0;		//椭球长半轴
const double ee= 0.0818191910428;// 第一偏心率
const double f_inverse = 298.257223563;			//扁率倒数
const double b = a - a / f_inverse;
const double eSquare = (a * a - b * b) / (a * a);
const double e = sqrt(eSquare);
const double earth_Re = 6378136.49;
const double earth_Rp = (1 - 1 / f_inverse) * earth_Re;
const double earth_We = 0.000072292115;
///////////////////////////////////// 运行时间打印  //////////////////////////////////////////////////////////


std::string getCurrentTimeString();
std::string getCurrentShortTimeString();


///////////////////////////////  基本图像类     //////////////////////////////////////////////////////////

/// <summary>
/// 三维向量，坐标表达
/// </summary>
struct Landpoint //  点   SAR影像的像素坐标；
{
	/// <summary>
	/// 经度x
	/// </summary>
	double lon; // 经度x lon pixel_col
	/// <summary>
	/// 纬度y
	/// </summary>
	double lat; // 纬度y lat pixel_row
	/// <summary>
	/// 高度z
	/// </summary>
	double ati; // 高程z ati pixel_time
};
struct Point_3d {
	double x;
	double y;
	double z;
};

Landpoint operator +(const Landpoint& p1, const Landpoint& p2);

Landpoint operator -(const Landpoint& p1, const Landpoint& p2);

bool operator ==(const Landpoint& p1, const Landpoint& p2);

Landpoint operator *(const Landpoint& p, double scale);

/// <summary>
/// 向量A,B的夹角,角度
/// </summary>
/// <param name="a"></param>
/// <param name="b"></param>
/// <returns>角度制  0-360度，逆时针</returns>
double getAngle(const Landpoint& a, const Landpoint& b);

/// <summary>
/// 点乘
/// </summary>
/// <param name="p1"></param>
/// <param name="p2"></param>
/// <returns></returns>
double dot(const Landpoint& p1, const Landpoint& p2);

double getlength(const Landpoint& p1);

Landpoint crossProduct(const Landpoint& a, const Landpoint& b);



struct DemBox {
	double min_lat; //纬度
	double min_lon;//经度
	double max_lat;//纬度
	double max_lon;//经度
};


/// <summary>
/// gdalImage图像操作类
/// </summary>
class gdalImage
{

public: // 方法
	gdalImage(string raster_path);
	~gdalImage();
	void setHeight(int);
	void setWidth(int);
	void setTranslationMatrix(Eigen::MatrixXd gt);
	void setData(Eigen::MatrixXd);
	Eigen::MatrixXd getData(int start_row, int start_col, int rows_count, int cols_count, int band_ids);
	void saveImage(Eigen::MatrixXd,int start_row,int start_col, int band_ids);
	void saveImage();
	void setNoDataValue(double nodatavalue,int band_ids);
	int InitInv_gt();
	Landpoint getRow_Col(double lon, double lat);
	Landpoint getLandPoint(double i, double j, double ati);
	double mean(int bandids=1);
	double max(int bandids=1);
	double min(int bandids=1);
	GDALRPCInfo getRPC();
	Eigen::MatrixXd getLandPoint(Eigen::MatrixXd points);

	Eigen::MatrixXd getHist(int bandids);
public:
	string img_path; // 图像文件
	int height; // 高
	int width; // 宽
	int band_num;// 波段数
	int start_row;// 
	int start_col;//
	int data_band_ids;
	Eigen::MatrixXd gt; // 变换矩阵
	Eigen::MatrixXd inv_gt; // 逆变换矩阵
	Eigen::MatrixXd data;
	string projection;
};

gdalImage CreategdalImage(string img_path, int height, int width, int band_num, Eigen::MatrixXd gt, std::string projection, bool need_gt=true);

void clipGdalImage(string in_path, string out_path, DemBox box, double pixelinterval);

int ResampleGDAL(const char* pszSrcFile, const char* pszOutFile, double* gt, int new_width, int new_height, GDALResampleAlg eResample);

int ResampleGDALs(const char* pszSrcFile, int band_ids, GDALRIOResampleAlg eResample=GRIORA_Bilinear);



///////////////////////////////  基本图像类 结束    //////////////////////////////////////////////////////////

string Convert(float Num);
std::string JoinPath(const std::string& path, const std::string& filename);

////////////////////////////// 坐标部分基本方法 //////////////////////////////////////////


/// <summary>
/// 将经纬度转换为地固参心坐标系
/// </summary>
/// <param name="XYZP">经纬度点--degree</param>
/// <returns>投影坐标系点</returns>
Landpoint LLA2XYZ(const Landpoint& LLA);
Eigen::MatrixXd LLA2XYZ(Eigen::MatrixXd landpoint);

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


////////////////////////////// 坐标部分基本方法 //////////////////////////////////////////

/// <summary>
/// 计算地表坡度向量
/// </summary>
/// <param name="p0">固参心坐标系</param>
/// <param name="p1">固参心坐标系</param>
/// <param name="p2">固参心坐标系</param>
/// <param name="p3">固参心坐标系</param>
/// <param name="p4">固参心坐标系</param>
/// <returns>向量角度</returns>
//Landpoint getSlopeVector(Landpoint& p0, Landpoint& p1, Landpoint& p2, Landpoint& p3, Landpoint& p4, Eigen::MatrixXd UTC, double xp, double yp, double dut1, double dat);
Landpoint getSlopeVector(const Landpoint& p0, const Landpoint& p1, const Landpoint& p2, const Landpoint& p3, const Landpoint& p4);

//////////////////////////////  插值   ////////////////////////////////////////////

complex<double> Cubic_Convolution_interpolation(double u,double v,Eigen::MatrixX<complex<double>> img);

complex<double> Cubic_kernel_weight(double s);

double Bilinear_interpolation(Landpoint p0, Landpoint p11, Landpoint p21, Landpoint p12, Landpoint p22);



inline float cross2d(Point_3d a, Point_3d b) { return a.x * b.y - a.y * b.x; }

inline Point_3d operator-(Point_3d a, Point_3d b) {
	return Point_3d{ a.x - b.x, a.y - b.y, a.z - b.z };
};

inline Point_3d operator+(Point_3d a, Point_3d b) {
	return Point_3d{ a.x + b.x, a.y +b.y, a.z + b.z };
};

inline double operator/(Point_3d a, Point_3d b) {
	return sqrt(pow(a.x,2)+ pow(a.y, 2))/sqrt(pow(b.x, 2)+ pow(b.y, 2));
};
inline bool onSegment(Point_3d Pi, Point_3d Pj, Point_3d Q)
{
	if ((Q.x - Pi.x) * (Pj.y - Pi.y) == (Pj.x - Pi.x) * (Q.y - Pi.y)  //叉乘 
		//保证Q点坐标在pi,pj之间 
		&& min(Pi.x, Pj.x) <= Q.x && Q.x <= max(Pi.x, Pj.x)
		&& min(Pi.y, Pj.y) <= Q.y && Q.y <= max(Pi.y, Pj.y))
		return true;
	else
		return false;
}

inline Point_3d invBilinear(Point_3d p, Point_3d a, Point_3d b, Point_3d c, Point_3d d)
{
	Point_3d res ;

	Point_3d e = b - a;
	Point_3d f = d - a;
	Point_3d g = a - b + c - d;
	Point_3d h = p - a;

	double k2 = cross2d(g, f);
	double k1 = cross2d(e, f) + cross2d(h, g);
	double k0 = cross2d(h, e);
	double u, v;
	// if edges are parallel, this is a linear equation
	if (abs(k2) < 0.001)
	{
		v = -k0 / k1;
		u = (h.x  - f.x *v) / (e.x + g.x *v);
		p.z = a.z + (b.z - a.z) * u + (d.z - a.z) * v + (a.z - b.z + c.z - d.z) * u * v;
		return p;
	}
	// otherwise, it's a quadratic
	else
	{
		float w = k1 * k1 - 4.0 * k0 * k2;
		if (w < 0.0){
			// 可能在边界上
			if (onSegment(a, b, p)) {
				Point_3d tt = b - a;
				Point_3d ttpa = p - a;
				double scater=ttpa / tt;
				if(scater<0||scater>1){ return { -9999,-9999,-9999 }; }
				p.z = a.z + scater * tt.z;
				return p;
			}
			else if (onSegment(b, c, p)) {
				Point_3d tt = c-b;
				Point_3d ttpa = p - b;
				double scater = ttpa / tt;
				if (scater < 0 || scater>1) { return { -9999,-9999,-9999 }; }
				p.z = b.z + scater * tt.z;
				return p;
			}
			else if (onSegment(c, d, p)) {
				Point_3d tt = d-c;
				Point_3d ttpa = p - c;
				double scater = ttpa / tt;
				if (scater < 0 || scater>1) { return { -9999,-9999,-9999 }; }
				p.z = c.z + scater * tt.z;
				return p;
			}
			else if (onSegment(d, a, p)) {
				Point_3d tt = a-d;
				Point_3d ttpa = p - d;
				double scater = ttpa / tt;
				if (scater < 0 || scater>1) { return { -9999,-9999,-9999 }; }
				p.z = d.z + scater * tt.z;
				return p;
			}

			return { -9999,-9999,-9999 };
		}
		else {
			w = sqrt(w);

			float ik2 = 0.5 / k2;
			float v = (-k1 - w) * ik2;
			float u = (h.x - f.x * v) / (e.x + g.x * v);

			if (u < 0.0 || u>1.0 || v < 0.0 || v>1.0)
			{
				v = (-k1 + w) * ik2;
				u = (h.x - f.x * v) / (e.x + g.x * v);
			}
			p.z = a.z + (b.z - a.z) * u + (d.z - a.z) * v + (a.z - b.z + c.z - d.z) * u * v;
			return p;
		}
	}
	p.z = a.z + (b.z - a.z) * u + (d.z - a.z) * v + (a.z - b.z + c.z - d.z) * u * v;
	return p;
}



//
// WGS84 到J2000 坐标系的变换
// 参考网址：https://blog.csdn.net/hit5067/article/details/116894616
// 资料网址：http://celestrak.org/spacedata/ 
// 参数文件：
//		 a. Earth Orientation Parameter 文件： http://celestrak.org/spacedata/EOP-Last5Years.csv
//       b. Space Weather Data  文件： http://celestrak.org/spacedata/SW-Last5Years.csv
// 备注：上述文件是自2017年-五年内
/**
在wgs84 坐标系转到J2000 坐标系 主要 涉及到坐标的相互转换。一般给定的WGS坐标为 给定时刻的 t ,BLH
转换步骤：
step 1: WGS 84 转换到协议地球坐标系
step 2: 协议地球坐标系 转换为瞬时地球坐标系
step 3: 瞬时地球坐标系 转换为 瞬时真天球坐标系
step 4: 瞬时真天球坐标系 转到瞬时平天球 坐标系
step 5: 瞬时平天球坐标系转换为协议天球坐标系（J2000）
**/


inline double sind(double degree) {
	return sin(degree * d2r);
}

inline double cosd(double d) {
	return cos(d * d2r);
}

/*
class WGS84_J2000
{
public:
	WGS84_J2000();
	~WGS84_J2000();

public:
	// step1  WGS 84 转换到协议地球坐标系。
	static Eigen::MatrixXd WGS84TECEF(Eigen::MatrixXd WGS84_Lon_lat_ait);
	//step 2  协议地球坐标系 转换为瞬时地球坐标系
	static Eigen::MatrixXd ordinateSingleRotate(int axis, double angle_deg);
	// step 3  瞬时地球坐标系 转换为 瞬时真天球坐标系 
	// xyz= ordinateSingleRotate('z',-gst_deg)*earthFixedXYZ;
	static int utc2gst(Eigen::MatrixXd UTC, double dUT1, double dAT, double& gst_deg, double& JDTDB);
	// step 4 瞬时真天球坐标系 转到瞬时平天球 坐标系
	static int nutationInLongitudeCaculate(double JD, double& epthilongA_deg, double& dertaPthi_deg, double& dertaEpthilong_deg, double& epthilong_deg);
	// step5  瞬时平天球坐标系转换为协议天球坐标系（J2000）函数中 JDTDB 为给定时刻 的地球动力学时对应的儒略日，其计算方法由步骤三中的函数给出。
	// xyz=ordinateSingleRotate('Z',zetaA)*ordinateSingleRotate('y',-thitaA)*ordinateSingleRotate('z',zA)*xyz;
	static int precessionAngle(double JDTDB, double& zetaA, double& thitaA, double& zA);
	// YMD2JD 同时 YMD2JD函数为 年月日转换为儒略日，具体说明 见公元纪年法（儒略历-格里高历）转儒略日_${王小贱}的博客-CSDN博客_年积日计算公式
	static double YMD2JD(double y, double m, double d);
	static Eigen::MatrixXd WGS842J2000(Eigen::MatrixXd BLH_deg_m, Eigen::MatrixXd UTC, double xp, double yp, double dut1, double dat);
	static Landpoint WGS842J2000(Landpoint LBH_deg_m, Eigen::MatrixXd UTC, double xp, double yp, double dut1, double dat);
public:
	static std::string EOP_File_Path;
	static std::string Space_Weather_Data;
	// IAU2000模型有77项11列
	static Eigen::Matrix<double, 77, 11> IAU2000ModelParams;
};

*/


/*
计算卫星高度角、方位角（XYZ转ENU，ENU转RAH） https://blog.csdn.net/why1472587/article/details/128178417
*/
//经纬度（BLH）
typedef struct BLH {
	double B;
	double L;
	double H;
};
//站心坐标系也称NEU坐标系或东北高坐标系
typedef struct ENU {
	double E;
	double N;
	double U;
};

//r为卫星向径，A为卫星方位角，h为卫星的高度角。
typedef struct RAH {
	double R;
	double A;
	double H;
};

//xyz转blh：
BLH xyz2blh(double X, double Y, double Z);

//xyz转enu Xr 已知测站坐标
ENU xyz2enu(double Xr, double Yr, double Zr, double Xs, double Ys, double Zs);

//求卫星方位角、高度角
RAH Satrah(double Xr, double Yr, double Zr, double Xs, double Ys, double Zs);

//将值写入到txt中
void creatTxt(const std::string& txtPath, const std::string& data);