#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 "GPUTBPImage.cuh"
#include "GPUBPTool.cuh"

#ifdef __CUDANVCC___





__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, double RefRange) {

	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[idx], antPy[idx], antPz[idx] }; // 卫星位置 (m)
		Vector3 ray = { antDirx[idx], antDiry[idx], antDirz[idx] };      // 视线方向
		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 ;
		    }
		
		    // Step 1: 计算交点T
		    Vector3 T = compute_T(S, ray, H);
		    if (isnan(T.x)) {
				imgx[idx] = 0;
				imgy[idx] = 0;
				imgz[idx] = 0;
		        return ;
		    }
		
		    // Step 2: 计算目标点P
		    Vector3 P = compute_P(S, T, R, H);
		
		    if (!isnan(P.x)) {

				imgx[idx] = P.x;
				imgy[idx] = P.y;
				imgz[idx] = P.z;
 
		    }
		    else {
				imgx[idx] = 0;
				imgy[idx] = 0;
				imgz[idx] = 0;
		        //printf("未找到有效解\n");
		    }
	}

}


__global__ void kernel_pixelTimeBP(
	double* antPx, double* antPy, double* antPz,
	double* imgx, double* imgy, double* imgz,
	cuComplex* TimeEchoArr, long prfcount, long pointCount,
	cuComplex* imgArr, long imH, long imW,
	double startLamda, double Rnear, double dx,double RefRange
) {
	long idx = blockIdx.x * blockDim.x + threadIdx.x;
	long pixelcount = imH * imW;
	if (idx < pixelcount) {

		double Tx = imgx[idx], Ty = imgy[idx], Tz = imgz[idx], PR = 0;
		double Px = 0, Py = 0, Pz = 0;
		double Rid = -1;
		long maxPointIdx = pointCount - 1;
		long pid = 0;
		long pid0 = 0;
		long pid1 = 0;
		double phi = 0;
		cuComplex s0=make_cuComplex(0,0), s1=make_cuComplex(0,0);
		cuComplex s = make_cuComplex(0, 0);
		cuComplex phiCorr = make_cuComplex(0, 0);
		for (long spid = 0; spid < prfcount; spid = spid + 8) {

#pragma unroll
			for (long sid = 0; sid < 8; sid++)
			{
				pid = spid + sid;
				if (pid < prfcount) {}
				else {
					continue;
				}

				Px = antPx[pid];
				Py = antPy[pid];
				Pz = antPz[pid];

				PR = sqrt((Px - Tx) * (Px - Tx) + (Py - Ty) * (Py - Ty) + (Pz - Tz) * (Pz - Tz));
				Rid = (PR - Rnear) / dx; // 行数

				if (Rid<0 || Rid>maxPointIdx) {
					continue;
				}
				else {}

				pid0 = floor(Rid);
				pid1 = ceil(Rid);

				if (pid1<0 || pid0<0 || pid0>maxPointIdx || pid1>maxPointIdx) {
					continue;
				}
				else {}
				// 线性插值
				s0 = TimeEchoArr[pid0];
				s1 = TimeEchoArr[pid1];

				s.x = s0.x * (Rid - pid0) + s1.x * (pid1 - Rid);
				s.y = s0.y * (Rid - pid0) + s1.y * (pid1 - Rid);

				// 相位校正

				phi = 4 * LIGHTSPEED/startLamda* (PR - RefRange) ; // 4PI/lamda * R

				// exp(ix)=cos(x)+isin(x)
				phiCorr.x = cos(phi);
				phiCorr.y = sin(phi);
				
				s = cuCmulf(s, phiCorr); // 校正后

				imgArr[idx] = cuCaddf(imgArr[idx], s);
			}
		}
	}
}





extern "C" {
	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, double RefRange)
	{
		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, RefRange);
		PrintLasterError("TIMEBPCreateImageGrid");
		cudaDeviceSynchronize();
	}



 
	void TimeBPImage(double* antPx, double* antPy, double* antPz,
		double* imgx, double* imgy, double* imgz,
		cuComplex* TimeEchoArr, long prfcount, long pointCount,
		cuComplex* imgArr, long imH, long imW,
		double startLamda, double Rnear, double dx, double RefRange
	)
	{
		long pixelcount = imH * imW;
		int grid_size = (pixelcount + BLOCK_SIZE - 1) / BLOCK_SIZE;

		kernel_pixelTimeBP << <grid_size, BLOCK_SIZE >> > (
			antPx, antPy, antPz,
			imgx, imgy, imgz,
			TimeEchoArr, prfcount, pointCount,
			imgArr, imH, imW,
			startLamda, Rnear, dx, RefRange
			);

		PrintLasterError("TimeBPImage");
		cudaDeviceSynchronize();

	}

}

#endif