#include "stdafx.h"
#include "GeoOperator.h"
#include <iostream>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <time.h>
////#include <mkl.h>
#include <string>
#include <omp.h>
#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 <fstream>
#include <proj.h>
#include "GeoOperator.h"
#include <ogr_spatialref.h>  // OGRSpatialReference 用于空间参考转换
#include <gdal_alg.h>        // 用于 GDALWarp 操作
#include <ogr_spatialref.h>


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
	};
}


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;
}

void LLA2XYZ(const Landpoint& LLA, Point3& XYZ)
{
	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 };
	XYZ.x = (N + H) * cosB * cos(L);
	XYZ.y = (N + H) * cosB * sin(L);
	//result.z = (N * (1 - e * e) + H) * sin(B);
	XYZ.z = (N * (1 - 1 / f_inverse) * (1 - 1 / f_inverse) + H) * sinB;

}


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;
}


Landpoint   XYZ2LLA(const Landpoint& XYZ) {
	double tmpX = XYZ.lon;// 
	double temY = XYZ.lat;// 
	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;
}



void XYZ2BLH_FixedHeight(double x, double y, double z,double ati, Landpoint& point) {
	const double a = 6378137.0;       // WGS84长半轴 
	const double f = 1.0 / 298.257223563;
	const double e2 = 2 * f - f * f;      // 第一偏心率平方

	// 计算经度L (弧度)
	const double L_rad = std::atan2(y, x);
	point.lon = L_rad * 180.0 / M_PI; // 转为度

	const double p = std::sqrt(x * x + y * y);
	const double H = ati;       // 使用已知高度

	// 初始纬度估算（考虑已知高度H）
	double B_rad = std::atan2(z, p * (1 - e2 * (a / (a + H))));

	// 迭代计算纬度B（固定H）
	for (int i = 0; i < 10; ++i) {   // 已知H时迭代次数减少
		const double sin_B = std::sin(B_rad);
		const double N = a / std::sqrt(1 - e2 * sin_B * sin_B);
		const double delta = e2 * N / (N + H);  // 高度固定时的修正项

		const double B_new = std::atan2(z, p * (1 - delta));

		if (std::abs(B_new - B_rad) < 1e-9) {
			B_rad = B_new;
			break;
		}
		B_rad = B_new;
	}

	point.lat = B_rad * 180.0 / M_PI; // 弧度转度

	// 经度范围修正 [-180°, 180°]
	point.lon = std::fmod(point.lon + 360.0, 360.0);
	if (point.lon > 180.0) point.lon -= 360.0;
	point.ati = ati;
}


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
	};
}

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

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

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

double   operator /(Point3 a, Point3 b)
{
	return sqrt(pow(a.x, 2) + pow(a.y, 2)) / sqrt(pow(b.x, 2) + pow(b.y, 2));
}

double BASECONSTVARIABLEAPI getPixelSpacingInDegree(double pixelSpacingInMeter)
{
	return pixelSpacingInMeter / WGS84_A * r2d;
 
}

double BASECONSTVARIABLEAPI getPixelSpacingInMeter(double pixelSpacingInDegree)
{
	return pixelSpacingInDegree * WGS84_A * r2d;
}


Landpoint   getSlopeVector(const Landpoint& p0, const  Landpoint& p1, const  Landpoint& p2, const  Landpoint& p3, const Landpoint& p4,bool inLBH) {
	Landpoint n0, n1, n2, n3, n4;
	if (inLBH) {
		n0 = LLA2XYZ(p0);
		n1 = LLA2XYZ(p1);
		n2 = LLA2XYZ(p2);
		n3 = LLA2XYZ(p3);
		n4 = LLA2XYZ(p4);
	}
	else {
		n0 = p0;
		n1 = p1;
		n2 = p2;
		n3 = p3;
		n4 = 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  
	//qDebug() << 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;
}



double   distance(const Vector3D& p1, const Vector3D& p2)
{
	double dx = p1.x - p2.x;
	double dy = p1.y - p2.y;
	double dz = p1.z - p2.z;
	return std::sqrt(dx * dx + dy * dy + dz * dz);
}

double   pointToLineDistance(const Vector3D& point, const Vector3D& linePoint, const Vector3D& lineDirection)
{
	Vector3D pointToLine = { point.x - linePoint.x, point.y - linePoint.y, point.z - linePoint.z };

	// 璁＄畻鐐瑰埌鐩寸嚎鐨勬姇褰辩偣鐨勪綅缃?
	double t = (pointToLine.x * lineDirection.x + pointToLine.y * lineDirection.y + pointToLine.z * lineDirection.z) /
		(lineDirection.x * lineDirection.x + lineDirection.y * lineDirection.y + lineDirection.z * lineDirection.z);

	// 璁＄畻鎶曞奖鐐?
	Vector3D projection = { linePoint.x + t * lineDirection.x, linePoint.y + t * lineDirection.y, linePoint.z + t * lineDirection.z };

	// 璁＄畻鐐瑰埌鐩寸嚎鐨勮窛绂?
	return distance(point, projection);
}

Vector3D operator+(const Vector3D& p1, const Vector3D& p2)
{
	return  Vector3D{ p1.x + p2.x,p1.y + p2.y,p1.z + p2.z };
}

Vector3D operator-(const Vector3D& p1, const Vector3D& p2)
{
	return  Vector3D{ p1.x - p2.x,p1.y - p2.y,p1.z - p2.z };
}

bool operator==(const Vector3D& p1, const Vector3D& p2)
{
	return p1.x == p2.x && p1.y == p2.y && p1.z == p2.z;
}

Vector3D operator*(const Vector3D& p, double scale)
{
	return Vector3D{
		p.x * scale,
		p.y * scale,
		p.z * scale
	};
}

Vector3D operator*(double scale, const Vector3D& p)
{
	return Vector3D{
		p.x * scale,
		p.y * scale,
		p.z * scale
	};
}

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

double getCosAngle(const Vector3D& a, const Vector3D& b)
{
	double c = dot(a, b) / (getlength(a) * getlength(b));
	return acos(c > 1 ? 1 : c < -1 ? -1 : c) * r2d;
}

double dot(const Vector3D& p1, const Vector3D& p2)
{
	return p1.y * p2.y + p1.x * p2.x + p1.z * p2.z;
}

double getlength(const Vector3D& p1)
{
	return std::sqrt(std::pow(p1.x, 2) + std::pow(p1.y, 2) + std::pow(p1.z, 2));
}

Vector3D crossProduct(const Vector3D& a, const Vector3D& b)
{
	return Vector3D{
		a.y * b.z - a.z * b.y,//x 
		a.z * b.x - a.x * b.z,//y
		a.x * b.y - a.y * b.x//z
	};
}


/// <summary>
///      n1
///   n4 n0 n2           
///      n3
/// </summary>
Vector3D getSlopeVector(const Vector3D& n0, const Vector3D& n1, const Vector3D& n2, const Vector3D& n3, const Vector3D& n4)
{
	Vector3D n01 = n1 - n0, n02 = n2 - n0, n03 = n3 - n0, n04 = n4 - n0;
	return (crossProduct(n01, n04) + crossProduct(n04, n03) + crossProduct(n03, n02) + crossProduct(n02, n01)) * 0.25;
}


SphericalCoordinates   cartesianToSpherical(const CartesianCoordinates& cartesian)
{
	SphericalCoordinates spherical;

	spherical.r = std::sqrt(cartesian.x * cartesian.x + cartesian.y * cartesian.y + cartesian.z * cartesian.z);
	spherical.theta = std::acos(cartesian.z / spherical.r);
	spherical.phi = std::atan2(cartesian.y, cartesian.x);

	return spherical;
}

CartesianCoordinates   sphericalToCartesian(const SphericalCoordinates& spherical)
{
	CartesianCoordinates cartesian;

	cartesian.x = spherical.r * std::sin(spherical.theta) * std::cos(spherical.phi);
	cartesian.y = spherical.r * std::sin(spherical.theta) * std::sin(spherical.phi);
	cartesian.z = spherical.r * std::cos(spherical.theta);

	return cartesian;
}


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


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

bool BASECONSTVARIABLEAPI ConvertResolutionToDegrees(int sourceEPSG, double resolutionMeters, double refLon, double refLat, double& degreePerPixelX, double& degreePerPixelY){
	// 初始化源坐标系（平面投影）和目标坐标系（WGS84 经纬度）
	OGRSpatialReference sourceSRS, targetSRS;
	sourceSRS.importFromEPSG(sourceEPSG);   // 源坐标系需明确 EPSG 
	targetSRS.importFromEPSG(4326);         // 目标为 WGS84 经纬度 

	// 创建坐标转换器：经纬度 -> 平面坐标
	OGRCoordinateTransformation* toPlane = OGRCreateCoordinateTransformation(&targetSRS, &sourceSRS);
	if (!toPlane) return false;

	// 将参考点经纬度转换为平面坐标
	double x = refLon, y = refLat;
	if (!toPlane->Transform(1, &x, &y)) {
		OGRCoordinateTransformation::DestroyCT(toPlane);
		return false;
	}

	// 创建坐标转换器：平面坐标 -> 经纬度
	OGRCoordinateTransformation* toGeo = OGRCreateCoordinateTransformation(&sourceSRS, &targetSRS);
	if (!toGeo) {
		OGRCoordinateTransformation::DestroyCT(toPlane);
		return false;
	}

	// 计算东向分辨率（经度变化量）
	double eastX = x + resolutionMeters, eastY = y;
	double eastLon = eastX, eastLat = eastY;
	if (!toGeo->Transform(1, &eastLon, &eastLat)) {
		OGRCoordinateTransformation::DestroyCT(toPlane);
		OGRCoordinateTransformation::DestroyCT(toGeo);
		return false;
	}
	degreePerPixelX = (eastLon - refLon) / resolutionMeters;  // 经度方向每米对应度数 

	// 计算北向分辨率（纬度变化量）
	double northX = x, northY = y + resolutionMeters;
	double northLon = northX, northLat = northY;
	if (!toGeo->Transform(1, &northLon, &northLat)) {
		OGRCoordinateTransformation::DestroyCT(toPlane);
		OGRCoordinateTransformation::DestroyCT(toGeo);
		return false;
	}
	degreePerPixelY = (northLat - refLat) / resolutionMeters;  // 纬度方向每米对应度数 

	// 释放资源
	OGRCoordinateTransformation::DestroyCT(toPlane);
	OGRCoordinateTransformation::DestroyCT(toGeo);
	return true;
}