#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>


 
// 向量运算
__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;
}

// 计算视线交点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);
    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 = fmax((-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 };
}

// 构建平面基底
extern __device__ __host__ void compute_basis(Vector3 S, Vector3 T, Vector3* e1, Vector3* e2) {
    Vector3 ST = vec_normalize(vec_sub(T, S));
    Vector3 SO = vec_normalize(vec_sub(Vector3 { 0, 0, 0 }, S)); // S->O方向

    *e1 = vec_normalize(vec_cross(ST, SO));
    *e2 = vec_normalize(vec_cross(*e1, ST));
}

// 牛顿迭代法
extern __device__ __host__ int newton_solve(Vector3 S, Vector3 e1, Vector3 e2,
    double R, double H, double* u, double* v) {
    double a_h = WGS84_A + H;

    for (int iter = 0; iter < MAX_ITER; ++iter) {
        Vector3 P = {
            S.x + e1.x * (*u) + e2.x * (*v),
            S.y + e1.y * (*u) + e2.y * (*v),
            S.z + e1.z * (*u) + e2.z * (*v)
        };

        // 残差计算
        double f1 = (P.x * P.x + P.y * P.y) / (a_h * a_h) + P.z * P.z / (WGS84_B * WGS84_B) - 1.0;
        double f2 = (*u) * (*u) + (*v) * (*v) - R * R;
        if (fabs(f1) < 1e-8 && fabs(f2) < 1e-8) return 1;

        // 雅可比矩阵
        double J11 = (2 * (S.x + e1.x * (*u) + e2.x * (*v)) * e1.x) / (a_h * a_h)
            + (2 * (S.z + e1.z * (*u) + e2.z * (*v)) * e1.z) / (WGS84_B * WGS84_B);
        double J12 = (2 * (S.x + e1.x * (*u) + e2.x * (*v)) * e2.x) / (a_h * a_h)
            + (2 * (S.z + e1.z * (*u) + e2.z * (*v)) * e2.z) / (WGS84_B * WGS84_B);
        double J21 = 2 * (*u);
        double J22 = 2 * (*v);

        // 矩阵求逆
        double det = J11 * J22 - J12 * J21;
        if (fabs(det) < 1e-12) break;

        double delta_u = (-J22 * f1 + J12 * f2) / det;
        double delta_v = (J21 * f1 - J11 * f2) / det;

        *u += delta_u;
        *v += delta_v;
    }
    return 0;
}

// 主计算函数A
extern __device__ __host__ Vector3 compute_P(Vector3 S, Vector3 T, double R, double H) {
    Vector3 e1, e2;
    compute_basis(S, T, &e1, &e2);

    // 计算参考角度方向
    Vector3 ST_vec = vec_sub(T, S);
    Vector3 SO_vec = vec_sub(Vector3 { 0, 0, 0 }, S);
    Vector3 ref_cross = vec_cross(SO_vec, ST_vec);
    double ref_sign = ref_cross.z; // 取Z分量判断方向

    Vector3 best_P = { NAN, NAN, NAN };
    double min_dist = INFINITY;

    // 圆周采样
    const int samples = 36;
    for (int i = 0; i < samples; ++i) {
        double angle = 2 * M_PI * i / samples;
        double u = R * cos(angle);
        double v = R * sin(angle);

        if (!newton_solve(S, e1, e2, R, H, &u, &v)) continue;

        Vector3 P = {
            S.x + e1.x * u + e2.x * v,
            S.y + e1.y * u + e2.y * v,
            S.z + e1.z * u + e2.z * v
        };

        // 椭球验证
        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 (fabs(check - 1.0) > 1e-6) continue;

        // 角度方向验证
        Vector3 SP_vec = vec_sub(P, S);
        Vector3 cur_cross = vec_cross(SP_vec, ST_vec);
        if (ref_sign * cur_cross.z < 0) continue;

        // 选择最近点
        double dist = vec_dot(vec_sub(P, T), vec_sub(P, T));
        if (dist < min_dist) {
            min_dist = dist;
            best_P = P;
        }
    }
    return best_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;
//}
//