#include <iostream>
#include <memory>
#include <cmath>
#include <complex>
#include <device_launch_parameters.h>
#include <cuda_runtime.h>
#include <cublas_v2.h>
#include <cuComplex.h>

#include "BaseConstVariable.h"
#include "GPUTool.cuh"
#include "GPUBPTool.cuh"

#include <cmath>
#include <stdio.h>
#include <cassert>

#define EPSILON 1e-12
#define MAX_ITER 50

#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif

__device__ __host__  double angleBetweenVectors(Vector3 a, Vector3 b, bool returnDegrees ) {
	// 计算点积
	double dotProduct = a.x * b.x + a.y * b.y + a.z * b.z;

	// 计算模长
	double magA = std::sqrt(a.x * a.x + a.y * a.y + a.z * a.z);
	double magB = std::sqrt(b.x * b.x + b.y * b.y + b.z * b.z);

	// 处理零向量异常
	if (magA == 0.0 || magB == 0.0) {
		return NAN;
	}

	double cosTheta = dotProduct / (magA * magB);
	cosTheta = cosTheta < -1 ? -1 : cosTheta>1 ? 1 : cosTheta;  // 截断到[-1, 1]
	double angleRad = std::acos(cosTheta);
	return returnDegrees ? angleRad * 180.0 / M_PI : angleRad;
}

// 向量运算
__device__ __host__ Vector3 vec_sub(Vector3 a, Vector3 b) {
	return { a.x - b.x, a.y - b.y, a.z - b.z };
}

__device__ __host__ double vec_dot(Vector3 a, Vector3 b) {
	return a.x * b.x + a.y * b.y + a.z * b.z;
}

__device__ __host__ Vector3 vec_cross(Vector3 a, Vector3 b) {
	return { a.y * b.z - a.z * b.y,
			a.z * b.x - a.x * b.z,
			a.x * b.y - a.y * b.x };
}

__device__ __host__ Vector3 vec_normalize(Vector3 v) {
	double len = sqrt(vec_dot(v, v));
	return (len > 1e-12) ? Vector3{ v.x / len, v.y / len, v.z / len } : v;
}


// 标准正交基底坐标计算
__device__ __host__ Vector3 coordinates_orthonormal_basis(Vector3& A,
	Vector3& e1,
	Vector3& e2,
	Vector3& e3) {
	//// 验证基底正交性和单位长度（容差1e-10）
	//const double tolerance = 1e-10;
	//assert(fabs(dot(e1, e2)) < tolerance);
	//assert(fabs(dot(e1, e3)) < tolerance);
	//assert(fabs(dot(e2, e3)) < tolerance);
	//assert(fabs(norm(e1) - 1.0) < tolerance);
	//assert(fabs(norm(e2) - 1.0) < tolerance);
	//assert(fabs(norm(e3) - 1.0) < tolerance);

	// 计算投影坐标
	return Vector3{
			vec_dot(A, e1),
			vec_dot(A, e2),
			vec_dot(A, e3)
	};
}



// 计算视线交点T
extern __device__ __host__ Vector3 compute_T(Vector3 S, Vector3 ray, double H) {
	Vector3 dir = vec_normalize(ray);
	double a_h = WGS84_A + H;

	double A = (dir.x * dir.x + dir.y * dir.y) / (a_h * a_h) + dir.z * dir.z / (WGS84_B * WGS84_B); // A > 0 
	double B = 2.0 * (S.x * dir.x / (a_h * a_h) + S.y * dir.y / (a_h * a_h) + S.z * dir.z / (WGS84_B * WGS84_B));
	double C = (S.x * S.x + S.y * S.y) / (a_h * a_h) + S.z * S.z / (WGS84_B * WGS84_B) - 1.0;

	double disc = B * B - 4 * A * C;
	if (disc < 0) return Vector3{ NAN, NAN, NAN };

	double sqrt_d = sqrt(disc);
	double t = fmin((-B - sqrt_d) / (2 * A), (-B + sqrt_d) / (2 * A));// 取最小值
	return (t > 1e-6) ? Vector3{ S.x + dir.x * t, S.y + dir.y * t, S.z + dir.z * t }
	: Vector3{ NAN, NAN, NAN };
}

// 主计算函数A
extern __device__ __host__ Vector3 compute_P(Vector3 S, Vector3 T, double R, double H) {
	Vector3 ex, ey, ez; // 平面基函数
	Vector3 ST = vec_normalize(vec_sub(T, S));// S->T 
	Vector3 SO = vec_normalize(vec_sub(Vector3{ 0, 0, 0 }, S)); // S->O 
	Vector3 st1 = vec_sub(T, S);
	double R0 = sqrt(st1.x * st1.x + st1.y * st1.y + st1.z * st1.z);

	// S (Z .) --------Y 
	// |\   
	// | \
    // |  \
    // X   \
    // | -> T 
	// |   /    
	// |  /    
	// | /      
	// |/      
	// O


	ez = vec_normalize(vec_cross(SO, ST)); // Z 轴
	ey = vec_normalize(vec_cross(ez, SO)); // Y 轴 与 ST 同向 --这个结论在星地几何约束，便是显然的；
	ex = vec_normalize(SO);  //X轴
	// 大致的理论推导
	// 这里考虑 成像几何，所以点 P 一定在 ex-0-ey 平面上，所以t3=0;
	// 定义 SP 的向量与 ex的夹角为 theta , 目标长度为 t
	// t1=t*cos(Q);
	// t2=t*sin(Q);
	// h=(a+H)
	// Xp=Sx+t1*ex.x+t2*ey.x +t3*ez.x; //因为 t3=0；
	// Yp=Sy+t1*ex.y+t2*ey.y +t3*ez.y; 
	// Zp=Sz+t1*ex.z+t2*ey.z +t3*ez.z; 
	// Xp^2+Yp^2     Zp^2            Xp^2+Yp^2     Zp^2
	// ---------- + ------- = 1  ==> ---------- + ------- = 1
	//  (a+H)^2       b^2             h^2            b^2
	double h2 = (WGS84_A + H) * (WGS84_A + H);
	double b2 = WGS84_B * WGS84_B;
	double R2 = R * R;
	double A = R2 * ((ex.x * ex.x + ex.y * ex.y) / h2 + (ex.z * ex.z) / b2);
	double B = R2 * ((ex.x * ey.x + ex.y * ey.y) / h2 + (ex.z * ey.z) / b2) * 2;
	double C = R2 * ((ey.x * ey.x + ey.y * ey.y) / h2 + (ey.z * ey.z) / b2);
	double D = 1 - ((S.x * S.x + S.y * S.y) / h2 + (S.z * S.z) / b2);
	double E = 2 * R * ((S.x * ex.x + S.y * ex.y) / h2 + (S.z * ex.z) / b2);
	double F = 2 * R * ((S.x * ey.x + S.y * ey.y) / h2 + (S.z * ey.z) / b2);

	//  f(Q)=Acos^2(Q)+Bsin(Q)cos(Q)+Csin^2(Q)+E*cos(Q)+F*sin(Q)-D
	//  f'(Q)=(C−A)sin(2Q)+2Bcos(2Q)-Esin(Q)+Fcos(Q)

	// 牛顿迭代
	//                  f(Q)
	// Q(t+1)=Q  -  -----------   
	//                 f'(Q)

	// 求解初始值

	double Q0 = angleBetweenVectors(SO, ST, false);
	double dQ = 0;
	double fQ = 0;
	double dfQ = 0;
	double Q = R < R0 ? Q0 - 1e-3 : Q0 + 1e-3;


	// 牛顿迭代法
	for (int iter = 0; iter < MAX_ITER * 10; ++iter) {
		fQ = A * cos(Q) * cos(Q) + B * sin(Q) * cos(Q) + C * sin(Q) * sin(Q) + E * cos(Q) + F * sin(Q) - D;
		dfQ = (C - A) * sin(2 * Q) + B * cos(2 * Q) - E * sin(Q) + F * cos(Q);
		dQ = fQ / dfQ;
		if (abs(dQ) < 1e-8) {
			break;
		}
		else {
			dQ = abs(dQ) < d2r * 2 ? dQ : abs(dQ) / dQ * d2r;
			Q = Q - dQ;
		}
	}

	double t1 = R * cos(Q);
	double t2 = R * sin(Q);
	Vector3 P = {
	 S.x + t1 * ex.x + t2 * ey.x, //因为 t3=0；
	 S.y + t1 * ex.y + t2 * ey.y,
	 S.z + t1 * ex.z + t2 * ey.z,

	};



	// 椭球验证
	double check = (P.x * P.x + P.y * P.y) / ((WGS84_A + H) * (WGS84_A + H))
		+ P.z * P.z / (WGS84_B * WGS84_B);
	if (isnan(Q) || isinf(Q) || fabs(check - 1.0) > 1e-6) {
		return Vector3{ NAN,NAN,NAN };
		printf("check value =%f\n", fabs(check - 1.0));
	}

	return P;
}



//// 参数校验与主函数
//int main() {
//    Vector3 S = { -2.8e6, -4.2e6, 3.5e6 }; // 卫星位置 (m)
//    Vector3 ray = { 0.6, 0.4, -0.7 };      // 视线方向
//    double H = 500.0;                    // 平均高程
//    double R = 1000.0;                   // 目标距离
//
//    // 参数校验
//    if (R <= 0 || H < -WGS84_A * 0.1 || H > WGS84_A * 0.1) {
//        printf("参数错误：\n  H范围：±%.1f km\n  R必须>0\n", WGS84_A * 0.1 / 1000);
//        return 1;
//    }
//
//    // Step 1: 计算交点T
//    Vector3 T = compute_T(S, ray, H);
//    if (isnan(T.x)) {
//        printf("错误：视线未与椭球相交\n");
//        return 1;
//    }
//
//    // Step 2: 计算目标点P
//    Vector3 P = compute_P(S, T, R, H);
//
//    if (!isnan(P.x)) {
//        printf("计算结果：\n");
//        printf("P = (%.3f, %.3f, %.3f) m\n", P.x, P.y, P.z);
//
//        // 验证距离
//        Vector3 SP = vec_sub(P, S);
//        double dist = sqrt(vec_dot(SP, SP));
//        printf("实际距离：%.3f m (期望：%.1f m)\n", dist, R);
//
//        // 验证椭球
//        double check = (P.x * P.x + P.y * P.y) / ((WGS84_A + H) * (WGS84_A + H))
//            + P.z * P.z / (WGS84_B * WGS84_B);
//        printf("椭球验证：%.6f (期望：1.0)\n", check);
//
//        // 验证最近距离
//        Vector3 PT = vec_sub(P, T);
//        printf("到T点距离：%.3f m\n", sqrt(vec_dot(PT, PT)));
//    }
//    else {
//        printf("未找到有效解\n");
//    }
//
//    return 0;
//}
//