#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___
#define EPSILON 1e-12
#define MAX_ITER 50

#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif

 



__device__ double  computerR(double& Px, double& Py, double& Pz, double& Tx, double& Ty, double& Tz) {
	//double R= sqrt((Px - Tx) * (Px - Tx) + (Py - Ty) * (Py - Ty) + (Pz - Tz) * (Pz - Tz));
	//if (R > 900000) {
	//	printf("R=%f\n", R);
	//}
	//return R;

	return sqrt((Px - Tx) * (Px - Tx) + (Py - Ty) * (Py - Ty) + (Pz - Tz) * (Pz - Tz));
}



__device__ void updateBPImage(
	long prfid, long pixelidx,double R,double LRrange,
	cuComplex* TimeEchoArr, long prfcount, long pointCount,
	cuComplex* imgArr,  
	double startLamda, double Rnear, double dx, double RefRange, double Rfar
) {
	double ridx = (R - LRrange) / dx; // 获取范围
	if (ridx < 0 || ridx >= pointCount) {
		return;
	}
	else {}

	long Ridx0 = floor(ridx);
	long Ridx1 = ceil(ridx);

	long pid0 = prfid * pointCount + Ridx0;
	long pid1 = prfid * pointCount + Ridx1;

	cuComplex s0 = TimeEchoArr[pid0];
	cuComplex s1 = TimeEchoArr[pid1];


	if (isinf(s0.x) || isinf(s0.y) || isinf(s1.x) || isinf(s1.y)) {
		return;
	}

	cuComplex s = make_cuComplex(
		s0.x + (s1.x - s0.x) * (ridx-Ridx0),  // real
		s0.y + (s1.y - s0.y) * (ridx-Ridx0)   // imag
	);
	double phi = 4 * PI / startLamda * (R - RefRange);

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

	imgArr[pixelidx] = cuCaddf(imgArr[pixelidx], s);
	//imgArr[pixelidx] = cuCaddf(imgArr[pixelidx], make_cuComplex(1,1));
	return;
}




// 分块计算
__device__ void segmentBPImage(
	double* antPx, double* antPy, double* antPz,
	double Tx, double Ty, double Tz,
	cuComplex* TimeEchoArr, long prfcount, long pointCount,
	cuComplex* imgArr,  
	double startLamda, double Rnear, double dx, double RefRange, double Rfar,
	long startSegmentPrfId,long pixelID // 分段起始prfid
) {
	// 计算单条脉冲范围
	double Rrange = pointCount * dx;// 成像范围
	double LRrange = RefRange - Rrange / 2;//左范围
	double RRrange = RefRange + Rrange / 2;
 
	long currentprfid = 0;
	// 0
	currentprfid = startSegmentPrfId + 0;
	double Px = antPx[currentprfid];
	double Py = antPy[currentprfid];
	double Pz = antPz[currentprfid];
	double R0 = computerR(Px, Py, Pz, Tx, Ty, Tz);


	if (LRrange <= R0 && R0 <= RRrange) {
		updateBPImage(
			currentprfid, pixelID, R0, LRrange,
			TimeEchoArr, prfcount, pointCount,
			imgArr,
			startLamda, Rnear, dx, RefRange, Rfar
		);
	}

	// 10
	currentprfid = startSegmentPrfId + 10;
	Px = antPx[currentprfid];
	Py = antPy[currentprfid];
	Pz = antPz[currentprfid];
	double R10 = computerR(Px, Py, Pz, Tx, Ty, Tz);
	if (Rnear <= R10 && R10 <= RRrange) {
		updateBPImage(
			currentprfid, pixelID, R10, LRrange,
			TimeEchoArr, prfcount, pointCount,
			imgArr,
			startLamda, Rnear, dx, RefRange, Rfar
		);
	}


	//19
	currentprfid = startSegmentPrfId + 19;
	Px = antPx[currentprfid];
	Py = antPy[currentprfid];
	Pz = antPz[currentprfid];
	double R19 = computerR(Px, Py, Pz, Tx, Ty, Tz);
	if (Rnear <= R19 && R19 <= RRrange) {
		updateBPImage(
			currentprfid, pixelID, R19, LRrange,
			TimeEchoArr, prfcount, pointCount,
			imgArr,
			startLamda, Rnear, dx, RefRange, Rfar
		);
	}

	// 判断是否需要处理
	
	if (R0 < LRrange && R10 < LRrange && R19 < LRrange ) { // 越界、不处理
		return;
	}
	else if (R0 > RRrange && R10 > RRrange && R19 > RRrange) {// 越界、不处理
		return;
	}
	else {}




	double R = 0;
#pragma unroll
	for (long i = 1; i < 10; i++) {
		currentprfid = startSegmentPrfId + i;
		if (currentprfid < prfcount) {
			Px = antPx[currentprfid];
			Py = antPy[currentprfid];
			Pz = antPz[currentprfid];
			R = computerR(Px, Py, Pz, Tx, Ty, Tz);
			updateBPImage(
				currentprfid, pixelID, R, LRrange,
				TimeEchoArr, prfcount, pointCount,
				imgArr,
				startLamda, Rnear, dx, RefRange, Rfar
			);
		}
	}

#pragma unroll
	for (long i = 11; i < 20; i++) {
		currentprfid = startSegmentPrfId + i;
		if (currentprfid < prfcount) {
			Px = antPx[currentprfid];
			Py = antPy[currentprfid];
			Pz = antPz[currentprfid];
			R = computerR(Px, Py, Pz, Tx, Ty, Tz);
			updateBPImage(
				currentprfid, pixelID, R, LRrange,
				TimeEchoArr, prfcount, pointCount,
				imgArr,
				startLamda, Rnear, dx, RefRange, Rfar
			);
		}
	}
}





__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,double Rfar
) {
	long idx = blockIdx.x * blockDim.x + threadIdx.x;
	long pixelcount = imH * imW;
	if (idx < pixelcount) {
 
		double Tx = imgx[idx]; // 地面坐标点
		double Ty = imgy[idx];
		double Tz = imgz[idx];
		double Rrange = pointCount * dx;// 成像范围
		double LRrange = RefRange - Rrange / 2;//左范围
		double RRrange = RefRange + Rrange / 2;

		for (long segid = 0; segid < prfcount; segid = segid + 20) {			
			long seglen = prfcount - segid;
			if (seglen < 20) {
				for (long i = 1; i < 10; i++) {
					long currentprfid = segid + i;
					if (currentprfid < prfcount) {
						double Px = antPx[currentprfid];
						double Py = antPy[currentprfid];
						double Pz = antPz[currentprfid];
						double R = computerR(Px, Py, Pz, Tx, Ty, Tz);
						updateBPImage(
							currentprfid, idx, R, LRrange,
							TimeEchoArr, prfcount, pointCount,
							imgArr,
							startLamda, Rnear, dx, RefRange, Rfar
						);
					}
				}
			}
			else {
				// 判断范围
				segmentBPImage(
					antPx, antPy, antPz,
					Tx, Ty, Tz,
					TimeEchoArr, prfcount, pointCount,
					imgArr,
					startLamda, Rnear, dx, RefRange, Rfar,
					segid, idx
				);
			}

		}
	}
}





extern "C" {



 
	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,double Rfar
	)
	{
		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, Rfar
			);

		PrintLasterError("TimeBPImage");
		cudaDeviceSynchronize();

	}

}

#endif