#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 <mkl.h>
#include <string>
#include <omp.h>
#include <boost/filesystem.hpp>
#include <boost/filesystem/fstream.hpp>
#include < io.h >
#include < stdio.h >
#include < stdlib.h >
#include <gdal.h>
#include <gdal_priv.h>
#include <gdalwarper.h>
//#include <ogr_geos.h>
#include <ogrsf_frmts.h> //#include "ogrsf_frmts.h"
#include <opencv2/opencv.hpp>
#include <opencv2/core/eigen.hpp>
#include <opencv2/features2d.hpp>
#include <fstream>
#include <proj.h>

#include "baseTool.h"
using namespace std;
using namespace Eigen;
using namespace cv;



/**
 * @brief GetCurrentTime ��ȡ��ǰʱ��
 * @return
 */
std::string getCurrentTimeString() {
	struct tm ConversionTime;
	std::time_t t = std::time(NULL);
	char mbstr[100];
	_localtime64_s(&ConversionTime, &t);
	std::strftime(mbstr, sizeof(mbstr), "%Y-%m-%d %H:%M:%S", &ConversionTime);
	std::string strTime = mbstr;
	return strTime;
}

std::string getCurrentShortTimeString() {
	struct tm ConversionTime;
	std::time_t t = std::time(NULL);
	char mbstr[100];
	_localtime64_s(&ConversionTime, &t);
	std::strftime(mbstr, sizeof(mbstr), "%Y-%m-%d %H:%M:%S", &ConversionTime);
	std::string strTime = mbstr;
	return strTime;
}

Landpoint operator +(const Landpoint& p1, const Landpoint& p2)
{
	return Landpoint{ p1.lon + p2.lon,p1.lat + p2.lat,p1.ati + p2.ati };
}

Landpoint operator -(const Landpoint& p1, const Landpoint& p2)
{
	return Landpoint{ p1.lon - p2.lon,p1.lat - p2.lat,p1.ati - p2.ati };
}

bool operator ==(const Landpoint& p1, const Landpoint& p2)
{
	return p1.lat == p2.lat && p1.lon == p2.lon && p1.ati == p2.ati;
}



Landpoint operator *(const Landpoint& p, double scale)
{
	return Landpoint{
		p.lon * scale,
		p.lat * scale,
		p.ati * scale
	};
}

double getAngle(const Landpoint& a, const Landpoint& b)
{
	double c = dot(a, b) / (getlength(a) * getlength(b));
	if (a.lon * b.lat - a.lat * b.lon >= 0) {
		return acos(c > 1 ? 1 : c < -1 ? -1 : c) * r2d;
	}
	else {
		return 360 - acos(c > 1 ? 1 : c < -1 ? -1 : c) * r2d;
	}
}

double dot(const Landpoint& p1, const Landpoint& p2)
{
	return p1.lat * p2.lat + p1.lon * p2.lon + p1.ati * p2.ati;
}

double getlength(const Landpoint& p1) {

	return sqrt(dot(p1, p1));
}

Landpoint crossProduct(const Landpoint& a, const Landpoint& b) {
	return Landpoint{
		a.lat * b.ati - a.ati * b.lat,//x 
		a.ati * b.lon - a.lon * b.ati,//y
		a.lon * b.lat - a.lat * b.lon//z
	};
}

/// <summary>
/// ����ر��¶�����������㶼��WGS84����ϵ����p1Ϊ���㣬
///    �� N
/// W  p1
/// p4 p0 p2 E
///    p3 S
/// n21=n2xn1
/// n=(n21+n32+n43+n41)*0.25;
/// </summary>
/// <param name="p0">Ŀ���1�7</param>
/// <param name="p1">����1</param>
/// <param name="p2">����2</param>
/// <param name="p3">����3</param>
/// <param name="p4">����4</param>
/// <returns>�����Ƕ�</returns>
Landpoint getSlopeVector(const Landpoint& p0, const  Landpoint& p1, const  Landpoint& p2, const  Landpoint& p3, const Landpoint& p4) {

	Landpoint n0 = LLA2XYZ(p0), 
		n1 = LLA2XYZ(p1),
		n2 = LLA2XYZ(p2),
		n3 = LLA2XYZ(p3),
		n4 = LLA2XYZ(p4);
	Landpoint n01 = n1 - n0, n02 = n2 - n0, n03 = n3 - n0, n04 = n4 - n0;
	// ��n01��������
	Landpoint np01 = p1-p0, np02 = p2-p0, np03 = p3-p0, np04 = p4-p0;
	double a2 = getAngle(Landpoint{ np01.lon,np01.lat,0 }, Landpoint{ np02.lon,np02.lat,0 });// 01->02 ��ʱ��
	double a3 = getAngle(Landpoint{ np01.lon,np01.lat,0 }, Landpoint{ np03.lon,np03.lat,0 });// 01->03 ��ʱ��
	double a4 = getAngle(Landpoint{ np01.lon,np01.lat,0 }, Landpoint{ np04.lon,np04.lat,0 });// 01->04 ��ʱ��
	//std::cout << a2 << "\t" << a3 << "\t" << a4 << endl;
	a2 = 360 - a2;
	a3 = 360 - a3;
	a4 = 360 - a4;
	Landpoint N, W, S, E;
	N = n01;
	if (a2 >= a3 && a2 >= a4) {
		W = n02;
		if (a3 >= a4) {
			S = n03;
			E = n04;
		}
		else {
			S = n04;
			E = n03;
		}
	}
	else if (a3 >= a2 && a3 >= a4) {
		W = n03;
		if (a2 >= a4) {
			S = n02;
			E = n04;
		}
		else {
			S = n04;
			E = n02;
		}
	}
	else if (a4 >= a2 && a4 >= a3)
	{
		W = n04;
		if (a2 >= a3) {
			S = n02;
			E = n03;
		}
		else {
			S = n03;
			E = n02;
		}
	}
	return (crossProduct(N, W) + crossProduct(W, S) + crossProduct(S, E) + crossProduct(E, N)) * 0.25;
}

/// <summary>
/// ���ݶ�������С
/// p11 p12				p13 p14
/// p21 p22				p23 p24
///			p(2+u,2+v)
/// p31 p32				p33 p34
/// p41 p42				p43 p44
/// </summary>
/// <param name="u">��Ԫ�������С������1�7</param>
/// <param name="v">��Ԫ�������С������1�7</param>
/// <param name="img">4x4 ��ֵ����</param>
/// <returns></returns>
complex<double> Cubic_Convolution_interpolation(double u, double v, Eigen::MatrixX<complex<double>> img)
{
	if (img.rows() != 4 || img.cols() != 4) {
		throw exception("the size of img's block is not right");
	}
	// ������ģ��
	Eigen::MatrixX<complex<double>> wrc(1, 4);// ʹ�� complex<double> ������Ҫԭ����Ϊ�˻�ȡֵ
	Eigen::MatrixX<complex<double>> wcr(4, 1);//
	for (int i = 0; i < 4; i++) {
		wrc(0, i) = Cubic_kernel_weight(u+1-i); // u+1,u,u-1,u-2
		wcr(i, 0) = Cubic_kernel_weight(v + 1 - i);
	}

	Eigen::MatrixX<complex<double>> interValue = wrc * img * wcr;
	return interValue(0, 0);
}

complex<double> Cubic_kernel_weight(double s)
{
	s = abs(s);
	if (s <= 1) {
		return complex<double>(1.5 * s * s * s - 2.5 * s * s + 1,0);
	}
	else if (s <= 2) {
		return complex<double>(-0.5 * s * s * s + 2.5 * s * s - 4 * s + 2,0);
	}
	else {
		return complex<double>(0,0);
	}
}

/// <summary>
///  p11  p12 -- x 
///    p0(u,v)
///  p21  p22
///   |
///   y
/// p11(0,0)
/// p21(0,1)
/// P12(1,0)
/// p22(1,1)
/// </summary>
/// <param name="p0">x,y,z</param>
/// <param name="p1">x,y,z</param>
/// <param name="p2">x,y,z</param>
/// <param name="p3">x,y,z</param>
/// <param name="p4">x,y,z</param>
/// <returns></returns>
double Bilinear_interpolation(Landpoint p0, Landpoint p11, Landpoint p21, Landpoint p12, Landpoint p22)
{

	return	p11.ati * (1 - p0.lon) * (1 - p0.lat) + 
			p12.ati * p0.lon * (1 - p0.lat) + 
			p21.ati * (1 - p0.lon) * p0.lat + 
			p22.ati * p0.lon * p0.lat;
}





/// <summary>
/// ����γ��ת��Ϊ�ع̲�������ϵ
/// </summary>
/// <param name="XYZP">��γ�ȵ�--degree</param>
/// <returns>ͶӰ����ϵ��</returns>
Landpoint LLA2XYZ(const Landpoint& LLA) {
	double L = LLA.lon * d2r;
	double B = LLA.lat * d2r;
	double H = LLA.ati;

	double sinB = sin(B);
	double cosB = cos(B);

	//double N = a / sqrt(1 - e * e * sin(B) * sin(B));
	double N = a / sqrt(1 - eSquare * sinB * sinB);
	Landpoint result = { 0,0,0 };
	result.lon = (N + H) * cosB * cos(L);
	result.lat = (N + H) * cosB * sin(L);
	//result.z = (N * (1 - e * e) + H) * sin(B);
	result.ati = (N * (1 - 1/f_inverse)* (1 - 1 / f_inverse) + H) * sinB;
	return result;
}

/// <summary>
/// ����n,3)   lon,lat,ati
/// </summary>
/// <param name="landpoint"></param>
/// <returns></returns>
Eigen::MatrixXd LLA2XYZ(Eigen::MatrixXd landpoint)
{
	landpoint.col(0) = landpoint.col(0).array() * d2r; // lon
	landpoint.col(1) = landpoint.col(1).array() * d2r; // lat

	Eigen::MatrixXd sinB = (landpoint.col(1).array().sin());//lat
	Eigen::MatrixXd cosB = (landpoint.col(1).array().cos());//lat

	Eigen::MatrixXd N = a / ((1 - sinB.array().pow(2) * eSquare).array().sqrt());
	Eigen::MatrixXd result(landpoint.rows(), 3);

	result.col(0) = (N.array() + landpoint.col(2).array()) * cosB.array() * Eigen::cos(landpoint.col(0).array()).array(); //x
	result.col(1) = (N.array() + landpoint.col(2).array()) * cosB.array() * Eigen::sin(landpoint.col(0).array()).array(); //y

	result.col(2) = (N.array() * (1 - 1/f_inverse)* (1 - 1 / f_inverse) + landpoint.col(2).array()) * sinB.array(); //z

	return result;
}


/// <summary>
/// ���ع̲�������ϵת��Ϊ��γ��
/// </summary>
/// <param name="XYZ">�̲�������ϵ</param>
/// <returns>���--degree</returns>
Landpoint XYZ2LLA(const Landpoint& XYZ) {
	double tmpX = XYZ.lon;// ���� x
	double temY = XYZ.lat;// � y
	double temZ = XYZ.ati;

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

	Landpoint result = { 0,0,0 };
	result.lon = atan2(temY, tmpX) * r2d;
	result.lat = curB * r2d;
	result.ati = temZ / sinCurB - N * (1 - eSquare);
	return result;
}

/// <summary>
/// ����ͼ���ȡӰ��1�7
/// </summary>
/// <param name="dem_path"></param>
gdalImage::gdalImage(string raster_path)
{
	omp_lock_t lock;
	omp_init_lock(&lock); // ��ʼ��������
	omp_set_lock(&lock); //��û�����1�7
	this->img_path = raster_path;

	GDALAllRegister();// ע���ʽ������1�7
	// ��DEMӰ��
	GDALDataset* rasterDataset = (GDALDataset*)(GDALOpen(raster_path.c_str(), GA_ReadOnly));//��ֻ����ʽ��ȡ����Ӱ��
	this->width = rasterDataset->GetRasterXSize();
	this->height = rasterDataset->GetRasterYSize();
	this->band_num = rasterDataset->GetRasterCount();

	double* gt = new double[6];
	// ��÷������
	rasterDataset->GetGeoTransform(gt);
	this->gt = Eigen::MatrixXd(2, 3);
	this->gt << gt[0], gt[1], gt[2], gt[3], gt[4], gt[5];
	
	this->projection = rasterDataset->GetProjectionRef();
	// ��������
	//double* inv_gt = new double[6];;
	//GDALInvGeoTransform(gt, inv_gt); // ��������
	// ����ͶӰ
	GDALFlushCache((GDALDatasetH)rasterDataset);
	GDALClose((GDALDatasetH)rasterDataset);
	rasterDataset = NULL;// ָ���ÿ�
	this->InitInv_gt();
	delete[] gt;
	////GDALDestroy(); // or, DllMain at DLL_PROCESS_DETACH
	omp_unset_lock(&lock); //�ͷŻ�����
	omp_destroy_lock(&lock); //���ٻ�����
}

gdalImage::~gdalImage()
{
}

void gdalImage::setHeight(int height)
{
	this->height = height;
}

void gdalImage::setWidth(int width)
{
	this->width = width;
}

void gdalImage::setTranslationMatrix(Eigen::MatrixXd gt)
{
	this->gt = gt;
}

void gdalImage::setData(Eigen::MatrixXd data)
{
	this->data = data;
}

Eigen::MatrixXd gdalImage::getData(int start_row, int start_col, int rows_count, int cols_count, int band_ids = 1)
{
	omp_lock_t lock;
	omp_init_lock(&lock);  
	omp_set_lock(&lock);  
	GDALAllRegister(); 
 
	GDALDataset* rasterDataset = (GDALDataset*)(GDALOpen(this->img_path.c_str(), GA_ReadOnly));//��ֻ����ʽ��ȡ����Ӱ��

	GDALDataType gdal_datatype = rasterDataset->GetRasterBand(1)->GetRasterDataType();
	GDALRasterBand* demBand = rasterDataset->GetRasterBand(band_ids);

	rows_count = start_row + rows_count <= this->height ? rows_count : this->height - start_row;
	cols_count = start_col + cols_count <= this->width ? cols_count : this->width - start_col;

	MatrixXd datamatrix(rows_count, cols_count);


	if (gdal_datatype == GDALDataType::GDT_UInt16) {

		unsigned short* temp = new unsigned short[rows_count * cols_count];
		demBand->RasterIO(GF_Read, start_col, start_row, cols_count, rows_count, temp, cols_count, rows_count, gdal_datatype, 0, 0);
		for (int i = 0; i < rows_count; i++) {
			for (int j = 0; j < cols_count; j++) {
				datamatrix(i, j) = temp[i * cols_count + j];
			}
		}
		delete[] temp;

	}
	else if (gdal_datatype == GDALDataType::GDT_Int16) {

		short* temp = new short[rows_count * cols_count];
		demBand->RasterIO(GF_Read, start_col, start_row, cols_count, rows_count, temp, cols_count, rows_count, gdal_datatype, 0, 0);
		for (int i = 0; i < rows_count; i++) {
			for (int j = 0; j < cols_count; j++) {
				datamatrix(i, j) = temp[i * cols_count + j];
			}
		}
		delete[] temp;
	}
	else if (gdal_datatype == GDALDataType::GDT_Float32) {
		float* temp = new float[rows_count * cols_count];
		demBand->RasterIO(GF_Read, start_col, start_row, cols_count, rows_count, temp, cols_count, rows_count, gdal_datatype, 0, 0);

		for (int i = 0; i < rows_count; i++) {
			for (int j = 0; j < cols_count; j++) {
				datamatrix(i, j) = temp[i * cols_count + j];
			}
		}
		delete[] temp;
	}
	GDALClose((GDALDatasetH)rasterDataset);
	omp_unset_lock(&lock); //�ͷŻ�����
	omp_destroy_lock(&lock); //���ٻ�����
	//GDALDestroy(); // or, DllMain at DLL_PROCESS_DETACH
	return datamatrix;

}

/// <summary>
/// д��ң��Ӱ��
/// </summary>
/// <param name="data"></param>
/// <param name="start_row"></param>
/// <param name="start_col"></param>
/// <param name="band_ids"></param>
void gdalImage::saveImage(Eigen::MatrixXd data, int start_row = 0, int start_col = 0, int band_ids = 1)
{
	omp_lock_t lock;
	omp_init_lock(&lock);  
	omp_set_lock(&lock);  
	if (start_row + data.rows() > this->height || start_col + data.cols() > this->width) {
		string tip = "file path: " + this->img_path;
		throw exception(tip.c_str());
	}
	GDALAllRegister(); 
	GDALDriver* poDriver = GetGDALDriverManager()->GetDriverByName("GTiff");
	GDALDataset* poDstDS = nullptr;
	if (boost::filesystem::exists(this->img_path)) {
		poDstDS = (GDALDataset*)(GDALOpen(this->img_path.c_str(), GA_Update));
	}
	else {
		poDstDS = poDriver->Create(this->img_path.c_str(), this->width, this->height, this->band_num, GDT_Float32, NULL); // ������
		poDstDS->SetProjection(this->projection.c_str());

	 
		double gt_ptr[6];
		for (int i = 0; i < this->gt.rows(); i++) {
			for (int j = 0; j < this->gt.cols(); j++) {
				gt_ptr[i * 3 + j] = this->gt(i, j);
			}
		}
		poDstDS->SetGeoTransform(gt_ptr);
		delete[] gt_ptr;
	}

	int datarows = data.rows();
	int datacols = data.cols();

	float* databuffer = new float[datarows * datacols];// (float*)malloc(datarows * datacols * sizeof(float));

	for (int i = 0; i < datarows; i++) {
		for (int j = 0; j < datacols; j++) {
			float temp = float(data(i, j));
			databuffer[i * datacols + j] = temp;
		}
	}
	//poDstDS->RasterIO(GF_Write,start_col, start_row, datacols, datarows, databuffer, datacols, datarows, GDT_Float32,band_ids, num,0,0,0);
	poDstDS->GetRasterBand(band_ids)->RasterIO(GF_Write, start_col, start_row, datacols, datarows, databuffer, datacols, datarows, GDT_Float32, 0, 0);
	
	GDALFlushCache(poDstDS);
	GDALClose((GDALDatasetH)poDstDS);
	//GDALDestroy(); // or, DllMain at DLL_PROCESS_DETACH
	delete[] databuffer;
	omp_unset_lock(&lock); //�ͷŻ�����
	omp_destroy_lock(&lock); //���ٻ�����
}

void gdalImage::saveImage()
{
	this->saveImage(this->data, this->start_row, this->start_col, this->data_band_ids);
}

void gdalImage::setNoDataValue(double nodatavalue = -9999, int band_ids = 1)
{


	GDALAllRegister();// ע���ʽ������1�7
	//GDALDriver* poDriver = GetGDALDriverManager()->GetDriverByName("GTiff");
	GDALDataset* poDstDS = (GDALDataset*)(GDALOpen(img_path.c_str(), GA_Update));
	poDstDS->GetRasterBand(band_ids)->SetNoDataValue(nodatavalue);
	GDALFlushCache((GDALDatasetH)poDstDS);
	GDALClose((GDALDatasetH)poDstDS);
}

int gdalImage::InitInv_gt()
{
	//1 lon lat = x
	//1 lon lat = y
	Eigen::MatrixXd temp(2, 3);
	this->inv_gt = temp;
	double a = this->gt(0, 0);
	double b = this->gt(0, 1);
	double c = this->gt(0, 2);
	double d = this->gt(1, 0);
	double e = this->gt(1, 1);
	double f = this->gt(1, 2);
	double g = 1;
	double det_gt = b * f - c * e;
	if (det_gt == 0) {
		return 0;
	}
	this->inv_gt(0, 0) = (c * d - a * f) / det_gt; //2
	this->inv_gt(0, 1) = f / det_gt; // lon
	this->inv_gt(0, 2) = -c / det_gt; // lat
	this->inv_gt(1, 0) = (a*e-b*d) / det_gt; //1
	this->inv_gt(1, 1) = -e / det_gt; // lon
	this->inv_gt(1, 2) = b / det_gt; // lat
	return 1;
}

Landpoint gdalImage::getRow_Col(double lon, double lat)
{
	Landpoint p{ 0,0,0 };
	p.lon = this->inv_gt(0, 0) + lon * this->inv_gt(0, 1) + lat * this->inv_gt(0, 2); //x
	p.lat = this->inv_gt(1, 0) + lon * this->inv_gt(1, 1) + lat * this->inv_gt(1, 2); //y
	return p;
}

Landpoint gdalImage::getLandPoint(double row, double col, double ati = 0)
{
	Landpoint p{ 0,0,0 };
	p.lon = this->gt(0, 0) + col * this->gt(0, 1) + row * this->gt(0, 2); //x
	p.lat = this->gt(1, 0) + col * this->gt(1, 1) + row * this->gt(1, 2); //y
	p.ati = ati;
	return p;
}

double gdalImage::mean(int bandids )
{
	double mean_value = 0;
	double count = this->height* this->width;
	int line_invert = 100;
	int start_ids = 0;
	do {
		Eigen::MatrixXd	sar_a = this->getData(start_ids, 0, line_invert, this->width, bandids);
		mean_value = mean_value+ sar_a.sum() / count;
		start_ids = start_ids + line_invert;
	} while (start_ids < this->height);
	return mean_value;
}

double gdalImage::max(int bandids)
{
	double max_value = 0;
	bool state_max = true;
	int line_invert = 100;
	int start_ids = 0;
	double temp_max = 0;
	do {
		Eigen::MatrixXd	sar_a = this->getData(start_ids, 0, line_invert, this->width, bandids);
		if (state_max) {
			state_max = false;
			max_value = sar_a.maxCoeff();
		}
		else {
			temp_max= sar_a.maxCoeff();
			if (max_value < temp_max) {
				max_value = temp_max;
			}
		}
		start_ids = start_ids + line_invert;
	} while (start_ids < this->height);
	return max_value;
}

double gdalImage::min(int bandids)
{
	double min_value = 0;
	bool state_min = true;
	int line_invert = 100;
	int start_ids = 0;
	double temp_min = 0;
	do {
		Eigen::MatrixXd	sar_a = this->getData(start_ids, 0, line_invert, this->width, bandids);
		if (state_min) {
			state_min = false;
			min_value = sar_a.minCoeff();
		}
		else {
			temp_min = sar_a.minCoeff();
			if (min_value < temp_min) {
				min_value = temp_min;
			}
		}
		start_ids = start_ids + line_invert;
	} while (start_ids < this->height);
	return min_value;
}

GDALRPCInfo gdalImage::getRPC()
{
	CPLSetConfigOption("GDAL_FILENAME_IS_UTF8", "NO");
	CPLSetConfigOption("GDAL_DATA", "./data");
	GDALAllRegister();//ע������
	//������
	GDALDataset* pDS = (GDALDataset*)GDALOpen(this->img_path.c_str(), GA_ReadOnly);
	//��Ԫ�����л�ȡRPC��Ϣ
	char** papszRPC = pDS->GetMetadata("RPC");

	//����ȡ��RPC��Ϣ����ɽṹ��1�7
	GDALRPCInfo oInfo;
	GDALExtractRPCInfo(papszRPC, &oInfo);

	GDALClose((GDALDatasetH)pDS);

	return oInfo;
}

Eigen::MatrixXd gdalImage::getLandPoint(Eigen::MatrixXd points)
{
	if (points.cols() != 3) {
		throw new exception("the size of points is equit 3!!!");
	}

	Eigen::MatrixXd result(points.rows(), 3);
	result.col(2) = points.col(2);// �߳�
	points.col(2) = points.col(2).array() * 0 + 1;
	points.col(0).swap(points.col(2));// ����
	Eigen::MatrixXd gts(3, 2);
	gts.col(0) = this->gt.row(0);
	gts.col(1) = this->gt.row(1);
	//cout << this->gt(0, 0) << "\t" << this->gt(0, 1) << "\t" << this->gt(0, 2) << endl;;
	//cout << gts(0, 0) << "\t" << gts(1,0) << "\t" << gts(2, 0) << endl;;
	//cout << this->gt(1, 0) << "\t" << this->gt(1, 1) << "\t" << this->gt(1, 2) << endl;;
	//cout<< gts(0, 1) << "\t" << gts(1, 1) << "\t" << gts(2,1) << endl;;
	//cout << points(3, 0) << "\t" << points(3, 1) << "\t" << points(3, 2) << endl;;


	result.block(0, 0, points.rows(), 2) = points * gts;
	return result;
}

Eigen::MatrixXd gdalImage::getHist(int bandids)
{
	GDALAllRegister();// ע���ʽ������1�7
	// ��DEMӰ��
	GDALDataset* rasterDataset = (GDALDataset*)(GDALOpen(this->img_path.c_str(), GA_ReadOnly));//��ֻ����ʽ��ȡ����Ӱ��

	GDALDataType gdal_datatype = rasterDataset->GetRasterBand(1)->GetRasterDataType();
	GDALRasterBand* xBand = rasterDataset->GetRasterBand(bandids);

	//��ȡͼ��ֱ��ͼ

	double dfMin = this->min(bandids);
	double dfMax = this->max(bandids); 
	int count = int((dfMax - dfMin) / 0.01);
	count = count > 255 ? count : 255;
	GUIntBig* panHistogram =new GUIntBig[count];
	xBand->GetHistogram(dfMin, dfMax, count, panHistogram, TRUE, FALSE, NULL, NULL);
	Eigen::MatrixXd result(count, 2);
	double delta = (dfMax - dfMin) / count;
	for (int i = 0; i < count; i++) {
		result(i, 0) = dfMin + i * delta;
		result(i, 1) = double(panHistogram[i]);
	}
	delete[] panHistogram;
	GDALClose((GDALDatasetH)rasterDataset);
	return result;
}


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

	if (boost::filesystem::exists(img_path.c_str())) {
		boost::filesystem::remove(img_path.c_str());
	}
	GDALAllRegister();// ע���ʽ������1�7
	GDALDriver* poDriver = GetGDALDriverManager()->GetDriverByName("GTiff");
	GDALDataset* poDstDS = poDriver->Create(img_path.c_str(), width, height, band_num, GDT_Float32, NULL); // ������
	if (need_gt) {
		poDstDS->SetProjection(projection.c_str());

		// ����ת������
		double gt_ptr[6] = { 0 };
		for (int i = 0; i < gt.rows(); i++) {
			for (int j = 0; j < gt.cols(); j++) {
				gt_ptr[i * 3 + j] = gt(i, j);
			}
		}
		poDstDS->SetGeoTransform(gt_ptr);
	}
	for (int i = 1; i <= band_num; i++) {
		poDstDS->GetRasterBand(i)->SetNoDataValue(-9999);
	}
	GDALFlushCache((GDALDatasetH)poDstDS);
	GDALClose((GDALDatasetH)poDstDS);
	////GDALDestroy(); // or, DllMain at DLL_PROCESS_DETACH
	gdalImage result_img(img_path);
	return result_img;
}


/// <summary>
/// �ü�DEM
/// </summary>
/// <param name="in_path"></param>
/// <param name="out_path"></param>
/// <param name="box"></param>
/// <param name="pixelinterval"></param>
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)
{
	GDALAllRegister();
	CPLSetConfigOption("GDAL_FILENAME_IS_UTF8", "NO");
	GDALDataset* pDSrc = (GDALDataset*)GDALOpen(pszSrcFile, GA_ReadOnly);
	if (pDSrc == NULL)
	{
		return -1;
	}

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

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

	//���û��ͶӰ����Î?¿½ï¿½ï¿½ï¿½Ò»ï¿½ï¿?1ï¿?7    
	if (strlen(pszSrcWKT) <= 0)
	{
		OGRSpatialReference oSRS;
		oSRS.importFromEPSG(4326);
		//oSRS.SetUTM(50, true);   //������  ����120��  
		//oSRS.SetWellKnownGeogCS("WGS84");
		oSRS.exportToWkt(&pszSrcWKT);
	}
	std::cout << "GDALCreateGenImgProjTransformer " << endl;
	void* hTransformArg;
	hTransformArg = GDALCreateGenImgProjTransformer((GDALDatasetH)pDSrc, pszSrcWKT, NULL, pszSrcWKT, FALSE, 0.0, 1);
	std::cout << "no proj " << endl;
	//(û��ͶӰ��Ӱ��������߲�Í?¿½ï¿?1ï¿?7)  
	if (hTransformArg == NULL)
	{
		GDALClose((GDALDatasetH)(GDALDatasetH)pDSrc);
		return -3;
	}
	std::cout << "has proj " << endl;
	double dGeoTrans[6] = { 0 };
	int nNewWidth = 0, nNewHeight = 0;
	if (GDALSuggestedWarpOutput((GDALDatasetH)pDSrc, GDALGenImgProjTransform, hTransformArg, dGeoTrans, &nNewWidth, &nNewHeight) != CE_None)
	{
		GDALClose((GDALDatasetH)(GDALDatasetH)pDSrc);
		return -3;
	}

	//GDALDestroyGenImgProjTransformer(hTransformArg);

	//����������ݼ�1�7  
	GDALDataset* pDDst = pDriver->Create(pszOutFile, new_width, new_height, nBandCount, dataType, NULL);
	if (pDDst == NULL)
	{
		GDALClose((GDALDatasetH)(GDALDatasetH)pDSrc);
		return -2;
	}

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

	GDALWarpOptions* psWo = GDALCreateWarpOptions();

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

	psWo->hSrcDS = (GDALDatasetH)pDSrc;
	psWo->hDstDS = (GDALDatasetH)pDDst;
	std::cout << "GDALCreateGenImgProjTransformer" << endl;
	psWo->pfnTransformer = GDALGenImgProjTransform;
	psWo->pTransformerArg = GDALCreateGenImgProjTransformer((GDALDatasetH)pDSrc, pszSrcWKT, (GDALDatasetH)pDDst, pszSrcWKT, FALSE, 0.0, 1);;
	
	std::cout << "GDALCreateGenImgProjTransformer has created" << endl;
	psWo->nBandCount = nBandCount;
	psWo->panSrcBands = (int*)CPLMalloc(nBandCount * sizeof(int));
	psWo->panDstBands = (int*)CPLMalloc(nBandCount * sizeof(int));
	for (int i = 0; i < nBandCount; i++)
	{
		psWo->panSrcBands[i] = i + 1;
		psWo->panDstBands[i] = i + 1;
	}

	GDALWarpOperation oWo;
	if (oWo.Initialize(psWo) != CE_None)
	{
		GDALClose((GDALDatasetH)(GDALDatasetH)pDSrc);
		GDALClose((GDALDatasetH)(GDALDatasetH)pDDst);
		return -3;
	}
	std::cout << "ChunkAndWarpImage:"<< new_width<<","<< new_height << endl;
	oWo.ChunkAndWarpMulti(0, 0, new_width, new_height);
	GDALFlushCache(pDDst);
	std::cout << "ChunkAndWarpImage over" << endl;
	//GDALDestroyGenImgProjTransformer(psWo->pTransformerArg);
	//GDALDestroyWarpOptions(psWo);
	GDALClose((GDALDatasetH)(GDALDatasetH)pDSrc);
	GDALClose((GDALDatasetH)(GDALDatasetH)pDDst);
	////GDALDestroy(); // or, DllMain at DLL_PROCESS_DETACH
	return 0;
}

int ResampleGDALs(const char* pszSrcFile, int band_ids, GDALRIOResampleAlg eResample)
{
	GDALAllRegister();
	CPLSetConfigOption("GDAL_FILENAME_IS_UTF8", "NO");
	GDALDataset* pDSrc = (GDALDataset*)GDALOpen(pszSrcFile, GA_Update);
	if (pDSrc == NULL)
	{
		return -1;
	}

	GDALDataType gdal_datatype = pDSrc->GetRasterBand(1)->GetRasterDataType();

	GDALRasterBand* demBand = pDSrc->GetRasterBand(band_ids);

	int width = pDSrc->GetRasterXSize();
	int height = pDSrc->GetRasterYSize();
	int start_col = 0, start_row = 0, rows_count = 0, cols_count;

	int row_delta = int(120000000 / width);

	GDALRasterIOExtraArg psExtraArg;
	INIT_RASTERIO_EXTRA_ARG(psExtraArg);
	psExtraArg.eResampleAlg = eResample;

	do {

		rows_count = start_row + row_delta < height ? row_delta : height - start_row;
		cols_count = width;

		if (gdal_datatype == GDALDataType::GDT_UInt16) {

			unsigned short* temp = new unsigned short[rows_count * cols_count];
			demBand->RasterIO(GF_Read, start_col, start_row, cols_count, rows_count, temp, cols_count, rows_count, gdal_datatype, 0, 0);
			demBand->RasterIO(GF_Write, start_col, start_row, cols_count, rows_count, temp, cols_count, rows_count, gdal_datatype, 0, 0, &psExtraArg);
			delete[] temp;

		}
		else if (gdal_datatype == GDALDataType::GDT_Int16) {

			short* temp = new short[rows_count * cols_count];
			demBand->RasterIO(GF_Read, start_col, start_row, cols_count, rows_count, temp, cols_count, rows_count, gdal_datatype, 0, 0);
			demBand->RasterIO(GF_Write, start_col, start_row, cols_count, rows_count, temp, cols_count, rows_count, gdal_datatype, 0, 0, &psExtraArg);
			delete[] temp;
		}
		else if (gdal_datatype == GDALDataType::GDT_Float32) {
			float* temp = new float[rows_count * cols_count];
			demBand->RasterIO(GF_Read, start_col, start_row, cols_count, rows_count, temp, cols_count, rows_count, gdal_datatype, 0, 0);
			demBand->RasterIO(GF_Write, start_col, start_row, cols_count, rows_count, temp, cols_count, rows_count, gdal_datatype, 0, 0, &psExtraArg);
			delete[] temp;
		}
		start_row = start_row + rows_count;
	} while (start_row < height);
	GDALClose((GDALDatasetH)pDSrc);


	return 0;
}


string Convert(float Num)
{
	ostringstream oss;
	oss << Num;
	string str(oss.str());
	return str;
}



std::string JoinPath(const std::string& path, const std::string& filename)
{
	int dir_size = path.size();

	int last_sprt_index = path.find_last_of('\\');
	int last_is_sprt = (last_sprt_index == dir_size - 1);

	if (last_is_sprt)
	{
		return path + filename;
	}
	else
	{
		return path + "\\" + filename;
	}
}




/// <summary>
////////////////////////////////////////////////////////////////////// WGS84 to J2000    /////////////////////////////////////////////////////////////////////////////////////////
/// </summary>
/*
Eigen::Matrix<double, 77, 11>  WGS84_J2000::IAU2000ModelParams = Eigen::Matrix<double, 77, 11>();

WGS84_J2000::WGS84_J2000()
{
	WGS84_J2000::IAU2000ModelParams << 0.0, 0.0, 0.0, 0.0, 1.0, -172064161.0, -174666.0, 33386.0, 92052331.0, 9086.0, 15377.0,
		0.0, 0.0, 2.0, -2.0, 2.0, -13170906.0, -1675.0, -13696.0, 5730336.0, -3015.0, -4587.0,
		0.0, 0.0, 2.0, 0.0, 2.0, -2276413.0, -234.0, 2796.0, 978459.0, -485.0, 1374.0,
		0.0, 0.0, 0.0, 0.0, 2.0, 2074554.0, 207.0, -698.0, -897492.0, 470.0, -291.0,
		0.0, 1.0, 0.0, 0.0, 0.0, 1475877.0, -3633.0, 11817.0, 73871.0, -184.0, -1924.0,
		0.0, 1.0, 2.0, -2.0, 2.0, -516821.0, 1226.0, -524.0, 224386.0, -677.0, -174.0,
		1.0, 0.0, 0.0, 0.0, 0.0, 711159.0, 73.0, -872.0, -6750.0, 0.0, 358.0,
		0.0, 0.0, 2.0, 0.0, 1.0, -387298.0, -367.0, 380.0, 200728.0, 18.0, 318.0,
		1.0, 0.0, 2.0, 0.0, 2.0, -301461.0, -36.0, 816.0, 129025.0, -63.0, 367.0,
		0.0, -1.0, 2.0, -2.0, 2.0, 215829.0, -494.0, 111.0, -95929.0, 299.0, 132.0,
		0.0, 0.0, 2.0, -2.0, 1.0, 128227.0, 137.0, 181.0, -68982.0, -9.0, 39.0,
		-1.0, 0.0, 2.0, 0.0, 2.0, 123457.0, 11.0, 19.0, -53311.0, 32.0, -4.0,
		-1.0, 0.0, 0.0, 2.0, 0.0, 156994.0, 10.0, -168.0, -1235.0, 0.0, 82.0,
		1.0, 0.0, 0.0, 0.0, 1.0, 63110.0, 63.0, 27.0, -33228.0, 0.0, -9.0,
		-1.0, 0.0, 0.0, 0.0, 1.0, -57976.0, -63.0, -189.0, 31429.0, 0.0, -75.0,
		-1.0, 0.0, 2.0, 2.0, 2.0, -59641.0, -11.0, 149.0, 25543.0, -11.0, 66.0,
		1.0, 0.0, 2.0, 0.0, 1.0, -51613.0, -42.0, 129.0, 26366.0, 0.0, 78.0,
		-2.0, 0.0, 2.0, 0.0, 1.0, 45893.0, 50.0, 31.0, -24236.0, -10.0, 20.0,
		0.0, 0.0, 0.0, 2.0, 0.0, 63384.0, 11.0, -150.0, -1220.0, 0.0, 29.0,
		0.0, 0.0, 2.0, 2.0, 2.0, -38571.0, -1.0, 158.0, 16452.0, -11.0, 68.0,
		0.0, -2.0, 2.0, -2.0, 2.0, 32481.0, 0.0, 0.0, -13870.0, 0.0, 0.0,
		-2.0, 0.0, 0.0, 2.0, 0.0, -47722.0, 0.0, -18.0, 477.0, 0.0, -25.0,
		2.0, 0.0, 2.0, 0.0, 2.0, -31046.0, -1.0, 131.0, 13238.0, -11.0, 59.0,
		1.0, 0.0, 2.0, -2.0, 2.0, 28593.0, 0.0, -1.0, -12338.0, 10.0, -3.0,
		-1.0, 0.0, 2.0, 0.0, 1.0, 20441.0, 21.0, 10.0, -10758.0, 0.0, -3.0,
		2.0, 0.0, 0.0, 0.0, 0.0, 29243.0, 0.0, -74.0, -609.0, 0.0, 13.0,
		0.0, 0.0, 2.0, 0.0, 0.0, 25887.0, 0.0, -66.0, -550.0, 0.0, 11.0,
		0.0, 1.0, 0.0, 0.0, 1.0, -14053.0, -25.0, 79.0, 8551.0, -2.0, -45.0,
		-1.0, 0.0, 0.0, 2.0, 1.0, 15164.0, 10.0, 11.0, -8001.0, 0.0, -1.0,
		0.0, 2.0, 2.0, -2.0, 2.0, -15794.0, 72.0, -16.0, 6850.0, -42.0, -5.0,
		0.0, 0.0, -2.0, 2.0, 0.0, 21783.0, 0.0, 13.0, -167.0, 0.0, 13.0,
		1.0, 0.0, 0.0, -2.0, 1.0, -12873.0, -10.0, -37.0, 6953.0, 0.0, -14.0,
		0.0, -1.0, 0.0, 0.0, 1.0, -12654.0, 11.0, 63.0, 6415.0, 0.0, 26.0,
		-1.0, 0.0, 2.0, 2.0, 1.0, -10204.0, 0.0, 25.0, 5222.0, 0.0, 15.0,
		0.0, 2.0, 0.0, 0.0, 0.0, 16707.0, -85.0, -10.0, 168.0, -1.0, 10.0,
		1.0, 0.0, 2.0, 2.0, 2.0, -7691.0, 0.0, 44.0, 3268.0, 0.0, 19.0,
		-2.0, 0.0, 2.0, 0.0, 0.0, -11024.0, 0.0, -14.0, 104.0, 0.0, 2.0,
		0.0, 1.0, 2.0, 0.0, 2.0, 7566.0, -21.0, -11.0, -3250.0, 0.0, -5.0,
		0.0, 0.0, 2.0, 2.0, 1.0, -6637.0, -11.0, 25.0, 3353.0, 0.0, 14.0,
		0.0, -1.0, 2.0, 0.0, 2.0, -7141.0, 21.0, 8.0, 3070.0, 0.0, 4.0,
		0.0, 0.0, 0.0, 2.0, 1.0, -6302.0, -11.0, 2.0, 3272.0, 0.0, 4.0,
		1.0, 0.0, 2.0, -2.0, 1.0, 5800.0, 10.0, 2.0, -3045.0, 0.0, -1.0,
		2.0, 0.0, 2.0, -2.0, 2.0, 6443.0, 0.0, -7.0, -2768.0, 0.0, -4.0,
		-2.0, 0.0, 0.0, 2.0, 1.0, -5774.0, -11.0, -15.0, 3041.0, 0.0, -5.0,
		2.0, 0.0, 2.0, 0.0, 1.0, -5350.0, 0.0, 21.0, 2695.0, 0.0, 12.0,
		0.0, -1.0, 2.0, -2.0, 1.0, -4752.0, -11.0, -3.0, 2719.0, 0.0, -3.0,
		0.0, 0.0, 0.0, -2.0, 1.0, -4940.0, -11.0, -21.0, 2720.0, 0.0, -9.0,
		-1.0, -1.0, 0.0, 2.0, 0.0, 7350.0, 0.0, -8.0, -51.0, 0.0, 4.0,
		2.0, 0.0, 0.0, -2.0, 1.0, 4065.0, 0.0, 6.0, -2206.0, 0.0, 1.0,
		1.0, 0.0, 0.0, 2.0, 0.0, 6579.0, 0.0, -24.0, -199.0, 0.0, 2.0,
		0.0, 1.0, 2.0, -2.0, 1.0, 3579.0, 0.0, 5.0, -1900.0, 0.0, 1.0,
		1.0, -1.0, 0.0, 0.0, 0.0, 4725.0, 0.0, -6.0, -41.0, 0.0, 3.0,
		-2.0, 0.0, 2.0, 0.0, 2.0, -3075.0, 0.0, -2.0, 1313.0, 0.0, -1.0,
		3.0, 0.0, 2.0, 0.0, 2.0, -2904.0, 0.0, 15.0, 1233.0, 0.0, 7.0,
		0.0, -1.0, 0.0, 2.0, 0.0, 4348.0, 0.0, -10.0, -81.0, 0.0, 2.0,
		1.0, -1.0, 2.0, 0.0, 2.0, -2878.0, 0.0, 8.0, 1232.0, 0.0, 4.0,
		0.0, 0.0, 0.0, 1.0, 0.0, -4230.0, 0.0, 5.0, -20.0, 0.0, -2.0,
		-1.0, -1.0, 2.0, 2.0, 2.0, -2819.0, 0.0, 7.0, 1207.0, 0.0, 3.0,
		-1.0, 0.0, 2.0, 0.0, 0.0, -4056.0, 0.0, 5.0, 40.0, 0.0, -2.0,
		0.0, -1.0, 2.0, 2.0, 2.0, -2647.0, 0.0, 11.0, 1129.0, 0.0, 5.0,
		-2.0, 0.0, 0.0, 0.0, 1.0, -2294.0, 0.0, -10.0, 1266.0, 0.0, -4.0,
		1.0, 1.0, 2.0, 0.0, 2.0, 2481.0, 0.0, -7.0, -1062.0, 0.0, -3.0,
		2.0, 0.0, 0.0, 0.0, 1.0, 2179.0, 0.0, -2.0, -1129.0, 0.0, -2.0,
		-1.0, 1.0, 0.0, 1.0, 0.0, 3276.0, 0.0, 1.0, -9.0, 0.0, 0.0,
		1.0, 1.0, 0.0, 0.0, 0.0, -3389.0, 0.0, 5.0, 35.0, 0.0, -2.0,
		1.0, 0.0, 2.0, 0.0, 0.0, 3339.0, 0.0, -13.0, -107.0, 0.0, 1.0,
		-1.0, 0.0, 2.0, -2.0, 1.0, -1987.0, 0.0, -6.0, 1073.0, 0.0, -2.0,
		1.0, 0.0, 0.0, 0.0, 2.0, -1981.0, 0.0, 0.0, 854.0, 0.0, 0.0,
		-1.0, 0.0, 0.0, 1.0, 0.0, 4026.0, 0.0, -353.0, -553.0, 0.0, -139.0,
		0.0, 0.0, 2.0, 1.0, 2.0, 1660.0, 0.0, -5.0, -710.0, 0.0, -2.0,
		-1.0, 0.0, 2.0, 4.0, 2.0, -1521.0, 0.0, 9.0, 647.0, 0.0, 4.0,
		-1.0, 1.0, 0.0, 1.0, 1.0, 1314.0, 0.0, 0.0, -700.0, 0.0, 0.0,
		0.0, -2.0, 2.0, -2.0, 1.0, -1283.0, 0.0, 0.0, 672.0, 0.0, 0.0,
		1.0, 0.0, 2.0, 2.0, 1.0, -1331.0, 0.0, 8.0, 663.0, 0.0, 4.0,
		-2.0, 0.0, 2.0, 2.0, 2.0, 1383.0, 0.0, -2.0, -594.0, 0.0, -2.0,
		-1.0, 0.0, 0.0, 0.0, 2.0, 1405.0, 0.0, 4.0, -610.0, 0.0, 2.0,
		1.0, 1.0, 2.0, -2.0, 2.0, 1290.0, 0.0, 0.0, -556.0, 0.0, 0.0;
}
WGS84_J2000::~WGS84_J2000()
{

}
/// <summary>
/// step1  WGS 84 ת����Э���������ϵ��1�7
/// ��ؾ�γ�ȸ߳�BLH��ع�����ϵ��×?¿½ï¿?1ï¿?7 BLH-- > XG
/// ECEF, Earth Centered Earth Fixed; �ع�����ϵ �ο�ƽ�棺ƽ����æ£?¿½ï¿½ï¿½ï¿½ï¿½ï¿½ï¿½Ä£ï¿½ï¿½ï¿½ï¿½ï¿½ï¿½ï¿½ï¿½ï¿½ï¿½ï¿½ï¿½CIO�����ߴ�ֱ��ƽ�档
/// x��Ϊ�ο�ƽ�����������ƽ�潻�ߣ�z��Ϊ����ָ��CIO�㡣
/// </summary>
/// <param name="LBH_deg_m">B L H�ֱ�Ϊ���γ�ȡ���ؾ��Ⱥʹ�ظ߳�1�7 �������ΪN * 3����</param>
/// <returns>XYZ Ϊ�ع�����ϵ�� x y z���������ΪN * 3����</returns>
Eigen::MatrixXd WGS84_J2000::WGS84TECEF(Eigen::MatrixXd LBH_deg_m)
{
	return LLA2XYZ(LBH_deg_m);
}
/// <summary>
/// step 2  ���������Ï?1ï¿?7 ת��Ϊ˲ʱ��������ϵ 
/// ����Ҫ�漰��ѯ ����ʱ�̵�  xp,yp , ���Բ�ѯEOP�ļ���ã����ص�ַ����http://celestrak.com/spacedata/
/// earthFixedXYZ=ordinateSingleRotate('x',yp)*ordinateSingleRotate('y',xp)*earthFixedXYZ;
/// </summary>
/// <param name="axis">axis ��ʾΧ����ת��������  '1' ��ʾΧ�� x����ʱ����ת;'2' ��ʾΧ�� y����ʱ����ת;'3'��ʾΧ�� z����ʱ����ת</param>
/// <param name="angle_deg">angle_deg ��ʾ��ת�ĽǶ� Ĭ�� degree</param>
/// <returns></returns>
Eigen::MatrixXd WGS84_J2000::ordinateSingleRotate(int axis, double angle_deg)
{
	angle_deg = angle_deg * d2r;
	Eigen::MatrixXd R = Eigen::MatrixXd::Ones(3, 3);
	switch (axis)
	{
	case 1: // x
		R << 1, 0.0, 0.0,
			0.0, cos(angle_deg), sin(angle_deg),
			0.0, -1 * sin(angle_deg), cos(angle_deg);
		break;
	case 2:// y
		R << cos(angle_deg), 0.0, -1 * sin(angle_deg),
			0.0, 1, 0.0,
			sin(angle_deg), 0.0, cos(angle_deg);
		break;
	case 3:// z
		R << cos(angle_deg), sin(angle_deg), 0.0,
			-1 * sin(angle_deg), cos(angle_deg), 0.0,
			0.0, 0.0, 1;
		break;
	default:
		throw exception("Error Axis");
		exit(-1);
	}
	return R;
}

/// <summary>
/// step 3  ˲ʱ��������ϵ ת��Ϊ ˲ʱ����������ϵ
/// ��utcʱ��ת��Ϊ�������κ���ʱ
/// xyz= ordinateSingleRotate('z',-gst_deg)*earthFixedXYZ;
/// UNTITLED ���㵱�غ���ʱ,����ֵ��ʱ��Ϊ��λ
/// %dAT ������
/// % TAI����ԭ��ʱ�� ��ʱ����׼  TAI = UTC + dAT;% ����ԭ��ʱ��
/// % TT ����ʱ  TT = TAI + 32.184 ��2014���ʱ��1�7;
/// % TDT ������ѧʱ��
/// % ET ����ʱ��
/// % ����ʱ = ������ѧʱ�� = ����ʱ��
/// %����ʱ=������ѧʱ��=����ʱ��
/// </summary>
/// <param name="UTC">��ʽΪy m d ����d����ֵΪС������h m s ����sec/3600+min/60+h)/24ת����С�������ۼӵ�day ��</param>
/// <param name="dUT1">dUT1 Ϊut1-utc �IJ��ֵ����������1�룬��iers�ɻ����Ö?1ï¿?7 0</param>
/// <param name="dAT">������ 37</param>
/// <param name="gst_deg">����ԭ��ʱ�� ��ʱ����׼  TAI=UTC+dAT;  %����ԭ��ʱ��</param>
/// <param name="JDTDB">����ʱ  TT = TAI+32.184 ��2014���ʱ��1�7;</param>
/// <returns></returns>
int WGS84_J2000::utc2gst(Eigen::MatrixXd UTC, double dUT1, double dAT, double& gst_deg, double& JDTDB)
{

	//function[gst_deg, JDTDB] = utc2gst(UTC, dUT1, dAT)
	//% TDT ������ѧʱ��
	//	% ET ����ʱ��
	//	% ����ʱ = ������ѧʱ�� = ����ʱ��
	double J2000 = 2451545;

	double	JDutc = YMD2JD(UTC(0,0), UTC(0,1), UTC(0,2));
	double JDUT1 = JDutc + dUT1 / 86400;// % UT1 Ϊ����ʱ������ʱ������ת�IJ����ȣ������UTCʱ����dUT1�IJ��dUT1�ڸ���������ʱ�ź��л���0.1��ľ��ȸ���IERS���������󣬻���1e - 5�ľ��ȸ�����
	double 	dT = 32.184 + dAT - dUT1;
	double JDTT = JDUT1 + dT / 86400;


	//% JDTT = YMD2JD(UTC(1), UTC(2), UTC(3)) + dUT1 / 86400;
	//% ���ȼ���TDB, TDB�����Ķ���ѧʱ����̫���������ǵ������������е�ʱ��߶�1�7
	double T = (JDTT - J2000) / 36525;
	double temp = 0.001657 * sin(628.3076 * T + 6.2401)
		+ 0.000022 * sin(575.3385 * T + 4.2970)
		+ 0.000014 * sin(1256.6152 * T + 6.1969)
		+ 0.000005 * sin(606.9777 * T + 4.0212)
		+ 0.000005 * sin(52.9691 * T + 0.4444)
		+ 0.000002 * sin(21.3299 * T + 5.5431)
		+ 0.00001 * T * sin(628.3076 * T + 4.2490);
	JDTDB = JDTT + temp / 86400;

	//% �������UT2, UT2����UT1�Ļ������������������Լ��ڱ仯��õ�������ʱ��1�7
	//	%% ����UT1����Tb, TbΪ�Ա�������Ϊ��λ������ԪB2000.0����ff
	//	% B2000 = 2451544.033;
	//% Tb = (YMD2JD(UT1(1), UT1(2), UT1(3)) - B2000) / 365.2422;
	//% dTs = 0.022 * sin(2 * pi * Tb) - 0.012 * cos(2 * pi * Tb) - 0.006 * sin(4 * pi * Tb) + 0.007 * cos(4 * pi * Tb);
	//% dTs = dTs / 86400;
	//% UT2 = UT1 + [0 0 dTs];

	//% ��������������ƽ����ʱGMST; ��λΪ��
	T = (JDTDB - J2000) / 36525;
	double T2 = T * T;
	double T3 = T2 * T;
	double T4 = T3 * T;
	double T5 = T4 * T;
	double Du = JDUT1 - J2000;
	double thita = 0.7790572732640 + 1.00273781191135448 * Du;//% ��J2000�����×?¿½ï¿½ï¿½ï¿½È¦ï¿½ï¿½ï¿½ï¿?1ï¿?7
	double 	GMST_deg = (-0.0000000368 * T5 - 0.000029956 * T4 - 0.00000044 * T3 + 1.3915817 * T2 + 4612.156534 * T + 0.014506) / 3600 + (thita - floor(thita)) * 360;
	double epthilongA_deg, dertaPthi_deg, dertaEpthilong_deg, epthilong_deg;
	WGS84_J2000::nutationInLongitudeCaculate(JDTDB, epthilongA_deg, dertaPthi_deg, dertaEpthilong_deg, epthilong_deg);
	//[epthilongA_deg, dertaPthi_deg] = nutationInLongitudeCaculate(JDTDB);

	//% ����à¾?¿½Â¶ï¿½ï¿½ï¿½ï¿½ï¿½ï¿½ï¿½ï¿½ï¿½eclipticObliquitygama����λΪ����
	//% ��������ƽ�����1�7   ̫��ƽ�����1�7 ����γ�ȷ��� ����ƽ�Ǿ�(����ƽ�ƾ� - ̫��ƽ�ƾ��� ����������ƽ�ƾ� ��λΪ����
	double F_deg = (0.00000417 * T4 - 0.001037 * T3 - 12.7512 * T2 + 1739527262.8478 * T + 335779.526232) / 3600;
	F_deg = fmod(F_deg, 360);
	double D_deg = (-0.00003169 * T4 + 0.006593 * T3 - 6.3706 * T2 + 1602961601.2090 * T + 1072260.70369) / 3600;
	D_deg = fmod(D_deg, 360);
	double  Omiga_deg = (-0.00005939 * T4 + 0.007702 * T3 + 7.4722 * T2 - 6962890.5431 * T + 450160.398036) / 3600;
	Omiga_deg = fmod(Omiga_deg, 360);

	double epthilongGama_deg = dertaPthi_deg * cosd(epthilongA_deg)
		+ (0.00264096 * sind(Omiga_deg )
			+ 0.00006352 * sind(2 * Omiga_deg )
			+ 0.00001175 * sind(2 * F_deg - 2 * D_deg + 3 * Omiga_deg)
			+ 0.00001121 * sind(2 * F_deg - 2 * D_deg + Omiga_deg)
			- 0.00000455 * sind(2 * F_deg - 2 * D_deg + 2 * Omiga_deg)
			+ 0.00000202 * sind(2 * F_deg + 3 * Omiga_deg)
			+ 0.00000198 * sind(2 * F_deg + Omiga_deg)
			- 0.00000172 * sind(3 * Omiga_deg)
			- 0.00000087 * T * sind(Omiga_deg)) / 3600;
	gst_deg = epthilongGama_deg + GMST_deg;
	return 0;
}


/// <summary>
/// step 4 ˲ʱ����������ϵ ת��˲ʱƽ���� ����ϵ
/// ����ƽ�Ƴཻ�ǣ��ƾ��¶����ͽ����¶���˲ʱ�Ƴཻ�ǡ�
/// </summary>
/// <param name="JD">������</param>
/// <param name="accuracy">��ʾ����ľ���Ò?¿½ï¿?1ï¿?7 ��ֵΪ��normal�� �͡�high�� ������'N'�͡�H'��ʾһ�㾫�Ⱥ͸߾��� ��Ĭ��Ϊ�߾��ȼ���</param>
/// <param name="epthilongA_deg">ƽ�Ƴཻ��</param>
/// <param name="dertaPthi_deg">�ƾ��¶�</param>
/// <param name="dertaEpthilong_deg">�ͽ����¶�</param>
/// <param name="epthilong_deg">˲ʱ�Ƴཻ��</param>
/// <returns></returns>
int WGS84_J2000::nutationInLongitudeCaculate(double JD, double& epthilongA_deg, double& dertaPthi_deg, double& dertaEpthilong_deg, double& epthilong_deg)
{
	double T = (JD - 2451545) / 36525;
	double T2 = T * T;
	double T3 = T2 * T;
	double T4 = T3 * T;
	double T5 = T4 * T;
	//% ��������ƽ�����lunarMeanAnomaly_deg(l_deg) ̫��ƽ�����SolarMeanAnomaly_deg(solarl_deg)
	//	% ����γ�ȷ���lunarLatitudeAngle_deg(F_deg)  ����ƽ�Ǿ�diffLunarSolarElestialLongitude_deg(D_deg����ƽ�ƾ� - ̫��ƽ�ƾ���
	//		% ����������ƽ�ƾ�SolarAscendingNodeElestialLongitude_deg(Omiga_deg)
	double l_deg = (-0.00024470 * T4 + 0.051635 * T3 + 31.8792 * T2 + 1717915923.2178 * T + 485868.249036) / 3600;
	l_deg = fmod(l_deg, 360);
	double solarl_deg = (-0.00001149 * T4 + 0.000136 * T3 - 0.5532 * T2 + 129596581.0481 * T + 1287104.79305) / 3600;
	solarl_deg = fmod(solarl_deg, 360);
	double F_deg = (0.00000417 * T4 - 0.001037 * T3 - 12.7512 * T2 + 1739527262.8478 * T + 335779.526232) / 3600;
	F_deg = fmod(F_deg, 360);
	double D_deg = (-0.00003169 * T4 + 0.006593 * T3 - 6.3706 * T2 + 1602961601.2090 * T + 1072260.70369) / 3600;
	D_deg = fmod(D_deg, 360);
	double Omiga_deg = (-0.00005939 * T4 + 0.007702 * T3 + 7.4722 * T2 - 6962890.5431 * T + 450160.398036) / 3600;
	Omiga_deg = fmod(Omiga_deg, 360);
	Eigen::MatrixXd basicAngle_deg = Eigen::Matrix<double, 1, 5>{ l_deg,solarl_deg,F_deg,D_deg,Omiga_deg };

	epthilongA_deg = (-0.0000000434 * T5 - 0.000000576 * T4 + 0.00200340 * T3 - 0.0001831 * T2 - 46.836769 * T + 84381.406) / 3600;
	epthilongA_deg = epthilongA_deg - floor(epthilongA_deg / 360) * 360;
	//% IAU2000ģ����77��
	Eigen::MatrixXd elestialLonNutation = WGS84_J2000::IAU2000ModelParams;

	dertaPthi_deg = -3.75e-08;
	dertaEpthilong_deg = 0.388e-3 / 3600;
	for (int i = 0; i < 77; i++) {
		double sumAngle_deg = 0;
		for (int j = 0; j < 5; j++) {
			sumAngle_deg = sumAngle_deg + elestialLonNutation(i, j) * basicAngle_deg(j);
		}
		sumAngle_deg = sumAngle_deg - floor(sumAngle_deg / 360) * 360;
		dertaPthi_deg = dertaPthi_deg + ((elestialLonNutation(i, 5) + elestialLonNutation(i, 6) * T) * sind(sumAngle_deg ) + elestialLonNutation(i, 7) * cosd(sumAngle_deg)) * 1e-7 / 3600;
		dertaEpthilong_deg = dertaEpthilong_deg + ((elestialLonNutation(i, 8) + elestialLonNutation(i, 9) * T) * cosd(sumAngle_deg) + elestialLonNutation(i, 10) * sind(sumAngle_deg)) * 1e-7 / 3600;
	}
	epthilong_deg = epthilongA_deg + dertaEpthilong_deg;
	return 0;
}

/// <summary>
/// zA��thitaA��zetaA�������1�7, ����������
/// </summary>
/// <param name="JDTDB"></param>
/// <param name="zetaA"></param>
/// <param name="thitaA"></param>
/// <param name="zA"></param>
/// <returns></returns>
int WGS84_J2000::precessionAngle(double JDTDB, double& zetaA, double& thitaA, double& zA)
{
	double T = (JDTDB - 2451545) / 36525;
	double T2 = T * T;
	double T3 = T2 * T;
	//%
	//% zetaA = (2306.218 * T + 0.30188 * T2 + 0.017998 * T3) / 3600;
	//% zA = (2306.218 * T + 1.09468 * T2 + 0.018203) / 3600;
	//% thitaA = (2004.3109 * T - 0.42665 * T2 - 0.041833 * T3) / 3600;
	double T4 = T3 * T;
	double T5 = T4 * T;
	zetaA = (-0.0000003173 * T5 - 0.000005971 * T4 + 0.01801828 * T3 + 0.2988499 * T2 + 2306.083227 * T + 2.650545) / 3600;
	thitaA = (-0.0000001274 * T5 - 0.000007089 * T4 - 0.04182264 * T3 - 0.4294934 * T2 + 2004.191903 * T) / 3600;
	zA = (-0.0000002904 * T5 - 0.000028596 * T4 + 0.01826837 * T3 + 1.0927348 * T2 + 2306.077181 * T - 2.650545) / 3600;
	return 0;
}

/// <summary>
/// ͬʱ YMD2JD����Ϊ ������ת��Ϊ�����գ�
/// </summary>
/// <param name="yy"></param>
/// <param name="mm"></param>
/// <param name="dd"></param>
/// <returns></returns>
double WGS84_J2000::YMD2JD(double y, double m, double d)
{
	int B = 0;
	if (y > 1582 || (y == 1582 && m > 10) || (y == 1582 && m == 10 && d >= 15)) {
		B = 2 - floor(y / 100) + floor(y / 400);
	}
	return  floor(365.25 * (y + 4712)) + floor(30.6 * (m + 1)) + d - 63.5 + B;
}

/// <summary>
/// WGS84 ת J2000 
/// </summary>
/// <param name="BLH_deg_m">WGS84 ���</param>
/// <param name="UTC">1x3 Y,m,d</param>
/// <param name="xp">EARTH ORIENTATION PARAMETER Xp</param>
/// <param name="yp">EARTH ORIENTATION PARAMETER Yp</param>
/// <param name="dut1">Solar Weather Data dut1</param>
/// <param name="dat">Solar Weather Data dat</param>
/// <returns></returns>
Eigen::MatrixXd WGS84_J2000::WGS842J2000(Eigen::MatrixXd BLH_deg_m, Eigen::MatrixXd UTC, double xp, double yp, double dut1, double dat)
{
	//step 1 step1  WGS 84 ת����Э���������ϵ��1�7 
	Eigen::MatrixXd earthFixedXYZ = WGS84_J2000::WGS84TECEF(BLH_deg_m).transpose();
	//step 2  ���������Ï?1ï¿?7 ת��Ϊ˲ʱ�������� 
	// ����Ҫ�漰��ѯ ����ʱ�̵�  xp,yp , ���Բ�ѯEOP�ļ���ã����ص�ַ����http://celestrak.com/spacedata/
	earthFixedXYZ = ordinateSingleRotate(1, yp) * ordinateSingleRotate(2, xp) * earthFixedXYZ;
	//step 3  ˲ʱ��������ϵ ת��Ϊ ˲ʱ����������ϵ
	// �ò�����Ҫ�漰���������κ���ʱ�ǵļ��㣬���ڸ������κ���ʱ�� ���㷽�� �ܶ࣬�������1�7 һ����Ϊ��ȷ�ļ��㷽��������dut1 ��dat������EOP�ļ����С�EOP�ļ����ص�ַ���� http://celestrak.org/spacedata/
	double gst_deg, JDTDB;
	WGS84_J2000::utc2gst(UTC, dut1, dat, gst_deg, JDTDB);
	Eigen::MatrixXd xyz = ordinateSingleRotate(3, -1 * gst_deg) * earthFixedXYZ;
	//step 4 ˲ʱ����������ϵ ת��˲ʱƽ���� ����ϵ
	double epthilongA_deg, dertaPthi_deg, dertaEpthilong_deg, epthilong_deg;
	WGS84_J2000::nutationInLongitudeCaculate(JDTDB, epthilongA_deg, dertaPthi_deg, dertaEpthilong_deg, epthilong_deg);
	xyz = ordinateSingleRotate(1, -epthilongA_deg) * ordinateSingleRotate(3, dertaPthi_deg) * ordinateSingleRotate(1, dertaEpthilong_deg + epthilongA_deg) * xyz;
	//step5  ˲ʱƽ��������ϵת��ΪЭ����������ϵ��J2000��
	double zetaA, thitaA, zA;
	WGS84_J2000::precessionAngle(JDTDB, zetaA, thitaA, zA);
	xyz = ordinateSingleRotate(3, zetaA) * ordinateSingleRotate(2, -thitaA) * ordinateSingleRotate(3, zA) * xyz;
	return xyz.transpose();
	//return Eigen::MatrixXd();
}

Landpoint WGS84_J2000::WGS842J2000(Landpoint p, Eigen::MatrixXd UTC, double xp, double yp, double dut1, double dat)
{
	Eigen::MatrixXd blh = Eigen::MatrixXd(1, 3);
	blh << p.lon, p.lat, p.ati;
	blh = WGS84_J2000::WGS842J2000(blh, UTC, xp, yp, dut1, dat);
	return Landpoint{ blh(0,0),blh(0,1) ,blh(0,0) };
	//return Landpoint();
}
*/