#include "GPUBaseTool.h"
#include "GPUBPTool.cuh"
#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 "GPUBPImageNet.cuh"

#ifndef MAX_ITER
#define EPSILON 1e-12
#define MAX_ITER 50
#endif

#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif


__global__ void kernel_TimeBPImageGridNet(
	double* antPx, double* antPy, double* antPz,
	double* antDirx, double* antDiry, double* antDirz,
	double* imgx, double* imgy, double* imgz,
	long prfcount, long freqpoints, double meanH,
	double Rnear, double dx) {

	long idx = blockIdx.x * blockDim.x + threadIdx.x;
	long pixelcount = prfcount * freqpoints;
	long prfid = idx / freqpoints;
	long Rid = idx % freqpoints;
	if (idx < pixelcount) {
		// 计算坐标
		Vector3 S = { antPx[prfid], antPy[prfid], antPz[prfid] }; // 卫星位置 (m)
		Vector3 ray = { antDirx[prfid], antDiry[prfid], antDirz[prfid] };      // 视线方向
		double H = meanH;                    // 平均高程
		double R = Rnear + dx * Rid;                   // 目标距离
		// 参数校验
		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);
			imgx[idx] = 0;
			imgy[idx] = 0;
			imgz[idx] = 0;
			return;
			//printf("idx=%d;prfid=%d;Rid=%d;S=[%f , %f ,%f ];ray=[%f ,%f ,%f ];H=%f;R=%f,imgP=[%f ,%f , %f ];Rextend\n",
			//	idx, prfid, Rid, S.x, S.y, S.z, ray.x, ray.y, ray.z, H, R,imgx[idx],imgy[idx],imgz[idx]);
			// 参数校验
			//return;
		}

		// Step 1: 计算交点T
		Vector3 T = compute_T(S, ray, H);
		if (isnan(T.x)) {
			imgx[idx] = 0;
			imgy[idx] = 0;
			imgz[idx] = 0;
			//printf("idx=%d;prfid=%d;Rid=%d;Tnan\n",
			//	idx, prfid, Rid, S.x, S.y, S.z, ray.x, ray.y, ray.z, H, R,T.x,T.y,T.z, imgx[idx], imgy[idx], imgz[idx]);
			return;
		}

		// Step 2: 计算目标点P

		Vector3 P;// = compute_P(S, T, R, H);
		{ // 计算P
			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);
			ez = vec_normalize(vec_cross(SO, ST)); // Z 轴
			ey = vec_normalize(vec_cross(ez, SO)); // Y 轴 与 ST 同向 --这个结论在星地几何约束，便是显然的；
			ex = vec_normalize(SO);  //X轴



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

			//printf("A=%f;B=%f;C=%f;D=%f;E=%f;F=%f;Q=%f;\
			//	S=[%f , %f ,%f ];\
			//	T=[%f , %f ,%f ];\
			//	ex=[%f , %f ,%f ];\
			//	ey=[%f , %f ,%f ];\
			//	ez=[%f , %f ,%f ];\
			//ray=[%f ,%f ,%f ];\
			//H=%f;R=%f;;\n",A,B,C,D,E,F,Q,
			//	S.x,S.y,S.z,
			//	T.x,T.y,T.z ,
			//	ex.x,ex.y,ex.z,
			//	ey.x,ey.y,ey.z,
			//	ez.x,ez.y,ez.z,
			//	ray.x, ray.y, ray.z,
			//	H, R);
			//	return;

			// 牛顿迭代法
			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) {
					//printf("iter=%d;check Q0=%f;Q=%f;dQ=%f;fQ=%f;dfQ=%f;break\n", iter, Q0, Q, dQ, fQ, dfQ);
					break;
				}
				else {
					dQ = (abs(dQ) < d2r * 3) ? dQ : (abs(dQ) / dQ * d2r * 3);
					Q = Q - dQ;
					//printf("iter=%d;check Q0=%f;Q=%f;dQ=%f;fQ=%f;dfQ=%f;\n", iter, Q0, Q, dQ, fQ, dfQ);
				}

			}
			//printf("check Q0=%f;Q=%f;\n", Q0, Q);
			double t1 = R * cos(Q);
			double t2 = R * sin(Q);
			P = Vector3{
			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) {
				P = Vector3{ 0,0,0 };
				imgx[idx] = 0;
				imgy[idx] = 0;
				imgz[idx] = 0;
				return;
			}
		}

		double Rt = sqrt(pow(S.x - T.x, 2) + pow(S.y - T.y, 2) + pow(S.z - T.z, 2));
		double Rp = sqrt(pow(S.x - P.x, 2) + pow(S.y - P.y, 2) + pow(S.z - P.z, 2));
		double Rop = sqrt(pow(P.x, 2) + pow(P.y, 2) + pow(P.z, 2));

		if (!isnan(P.x) && (Rop > WGS84_A * 0.3) && (Rop < WGS84_A * 3)) {

			imgx[idx] = P.x;
			imgy[idx] = P.y;
			imgz[idx] = P.z;
			return;
			//printf("idx=%d; S=[%f , %f ,%f ]; H=%f;R=%f;RP=%f;Rr=%f;imgT=[%f ,%f ,%f ];imgP=[%f ,%f , %f ]; \n",
			//	idx,   S.x, S.y, S.z,   H, R, Rp, Rt,T.x, T.y, T.z, P.x, P.y, P.z);
		}
		else {
			imgx[idx] = 0;
			imgy[idx] = 0;
			imgz[idx] = 0;
			printf("idx=%d; S=[%f , %f ,%f ]; H=%f;R=%f;RP=%f;Rr=%f;imgT=[%f ,%f ,%f ];imgP=[%f ,%f , %f ]; ERROR\n",
				idx, S.x, S.y, S.z, H, R, Rp, Rt, T.x, T.y, T.z, P.x, P.y, P.z);
			return;
		}
	}

}












void TIMEBPCreateImageGrid(double* antPx, double* antPy, double* antPz,
	double* antDirx, double* antDiry, double* antDirz,
	double* imgx, double* imgy, double* imgz,
	long prfcount, long freqpoints, double meanH,
	double Rnear, double dx 
)
{
	long pixelcount = prfcount * freqpoints;
	int grid_size = (pixelcount + BLOCK_SIZE - 1) / BLOCK_SIZE;

	kernel_TimeBPImageGridNet << <grid_size, BLOCK_SIZE >> > (
		antPx, antPy, antPz,
		antDirx, antDiry, antDirz,
		imgx, imgy, imgz,
		prfcount, freqpoints, meanH,
		Rnear, dx);
	PrintLasterError("TIMEBPCreateImageGrid");
	cudaDeviceSynchronize();
}