#include <time.h>
#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 "LookTableSimulationComputer.cuh"


extern  __device__ __host__ double getNumberDopplerCenterRate(double R, double r0, double r1, double r2, double r3, double r4, double reftime)
{
	double t=R / LIGHTSPEED - reftime;
	double dopplerCenterRate = r0 + r1*t + r2*t*t + r3*t*t*t + r4*t*t*t*t;
	return  dopplerCenterRate;
}

__device__ __host__ double getDopplerCenterRate(double Rx, double Ry, double Rz, double Vx, double Vy, double Vz, double fact_lamda)
{
	return -2*(Rx*Vx+Ry*Vy+Rz*Vz)* fact_lamda;
}

__device__ __host__ double getPolyfitNumber(double x, double a0, double a1, double a2, double a3, double a4, double a5)
{
	return a0 + a1 * x + a2 * x * x + a3 * x * x * x + a4 * x * x * x * x + a5 * x * x * x * x * x;
}


__global__ void Kernel_RDProcess_doppler(
	double* demX, double* demY, double* demZ,
	float* outRidx, float* outCidx,
	long pixelcount,
	double Xp0, double Yp0, double Zp0, double Xv0, double Yv0, double Zv0,
	double Xp1, double Yp1, double Zp1, double Xv1, double Yv1, double Zv1,
	double Xp2, double Yp2, double Zp2, double Xv2, double Yv2, double Zv2,
	double Xp3, double Yp3, double Zp3, double Xv3, double Yv3, double Zv3,
	double Xp4, double Yp4, double Zp4, double Xv4, double Yv4, double Zv4,
	double Xp5, double Yp5, double Zp5, double Xv5, double Yv5, double Zv5,
	double reftime, double r0, double r1, double r2, double r3, double r4,
	double starttime, double nearRange, double farRange,
	double PRF, double Fs,
	double fact_lamda
) {
	long  idx = blockIdx.x * blockDim.x + threadIdx.x;
	if (idx < pixelcount) {
		double demx = demX[idx];
		double demy = demY[idx];
		double demz = demZ[idx];
		float Rd_r = -1;
		float Rd_c = -1;
		double dt = 1.0 / PRF / 3;
		double Spx = 0, Spy = 0, Spz = 0, Svx = 0, Svy = 0, Svz = 0;
		double Rx = 0, Ry = 0, Rz = 0,R = 0;

		double dp1=0, dpn1=0, dp2=0, dpn2=0;
		
		double ti = 0;
		double inct = 0;
		for (long i = 0; i < 10000; i++) { // ×î´óµü´ú·¶Î§
			Spx = getPolyfitNumber(ti+dt, Xp0, Xp1, Xp2, Xp3, Xp4, Xp5);
			Spy = getPolyfitNumber(ti+dt, Yp0, Yp1, Yp2, Yp3, Yp4, Yp5);
			Spz = getPolyfitNumber(ti+dt, Zp0, Zp1, Zp2, Zp3, Zp4, Zp5);
			Svx = getPolyfitNumber(ti+dt, Xv0, Xv1, Xv2, Xv3, Xv4, Xv5);
			Svy = getPolyfitNumber(ti+dt, Yv0, Yv1, Yv2, Yv3, Yv4, Yv5);
			Svz = getPolyfitNumber(ti+dt, Zv0, Zv1, Zv2, Zv3, Zv4, Zv5);

			Rx = Spx - demx;
			Ry = Spy - demy;
			Rz = Spz - demz;
			R = sqrt(Rx * Rx + Ry * Ry + Rz * Rz);
			Rx = Rx / R;
			Ry = Ry / R;
			Rz = Rz / R;

			dp2 = getDopplerCenterRate(Rx, Ry, Rz, Svx, Svy, Svz, fact_lamda);
			dpn2 = getNumberDopplerCenterRate(R, r0, r1, r2, r3, r4, reftime);



			// ti
			Spx = getPolyfitNumber(ti, Xp0, Xp1, Xp2, Xp3, Xp4, Xp5);
			Spy = getPolyfitNumber(ti, Yp0, Yp1, Yp2, Yp3, Yp4, Yp5);
			Spz = getPolyfitNumber(ti, Zp0, Zp1, Zp2, Zp3, Zp4, Zp5);
			Svx = getPolyfitNumber(ti, Xv0, Xv1, Xv2, Xv3, Xv4, Xv5);
			Svy = getPolyfitNumber(ti, Yv0, Yv1, Yv2, Yv3, Yv4, Yv5);
			Svz = getPolyfitNumber(ti, Zv0, Zv1, Zv2, Zv3, Zv4, Zv5);
 
			Rx = Spx - demx;
			Ry = Spy - demy;
			Rz = Spz - demz;
			R = sqrt(Rx * Rx + Ry * Ry + Rz * Rz);
			Rx = Rx / R;
			Ry = Ry / R;
			Rz = Rz / R;

			dp1=getDopplerCenterRate(Rx, Ry, Rz, Svx, Svy, Svz, fact_lamda);
			dpn1 = getNumberDopplerCenterRate(R, r0, r1, r2, r3, r4, reftime);

			// iter
			inct = dt * (dp2 - dpn1) / (dp1 - dp2);

			//printf("ti: %10.6f\n", ti);
			//printf("inct: %10.6f\n", inct);
			//printf("demx: %10.6f\n", demx);
			//printf("demy: %10.6f\n", demy);
			//printf("demz: %10.6f\n", demz);
			//printf("Rd_r: %10.6f\n", Rd_r);
			//printf("Rd_c: %10.6f\n", Rd_c);
			//printf("dt:   %10.6f\n", dt);
			//printf("Spx:  %10.6f\n", Spx);
			//printf("Spy:  %10.6f\n", Spy);
			//printf("Spz:  %10.6f\n", Spz);
			//printf("Svx:  %10.6f\n", Svx);
			//printf("Svy:  %10.6f\n", Svy);
			//printf("Svz:  %10.6f\n", Svz);
			//printf("Rx:   %10.6f\n", Rx);
			//printf("Ry:   %10.6f\n", Ry);
			//printf("Rz:   %10.6f\n", Rz);
			//printf("R:    %10.6f\n", R);
			//printf("dp1:  %10.6f\n", dp1);
			//printf("dpn1: %10.6f\n", dpn1);
			//printf("dp2:  %10.6f\n", dp2);
			//printf("dpn2: %10.6f\n", dpn2);

			if (abs(inct) <= dt||isnan(inct) ) {
				
				Rd_r = (ti - starttime) * PRF;
				Rd_c =  Fs/LIGHTSPEED  * (R - nearRange)*2;
				//printf("ti: %10.6f,starttime:%10.6f,PRF:%10.6f,Rd_c:%10.6f,R:%10.6f\n", ti, starttime, PRF, Rd_c, R);
				outRidx[idx] = Rd_r;
				outCidx[idx] = Rd_c;//Rd_c;
				return;
			}
			ti = ti + inct;
		}
		outRidx[idx] = -1;
		outCidx[idx] = -1;
	}
}





void RDProcess_dopplerGPU(
	double* demX, double* demY, double* demZ,
	float* outRidx, float* outCidx,
	long rowcount, long colcount,
	double starttime, double nearRange, double farRange,
	double PRF, double Fs,
	double fact_lamda,
	double Xp0, double Yp0, double Zp0, double Xv0, double Yv0, double Zv0,
	double Xp1, double Yp1, double Zp1, double Xv1, double Yv1, double Zv1,
	double Xp2, double Yp2, double Zp2, double Xv2, double Yv2, double Zv2,
	double Xp3, double Yp3, double Zp3, double Xv3, double Yv3, double Zv3,
	double Xp4, double Yp4, double Zp4, double Xv4, double Yv4, double Zv4,
	double Xp5, double Yp5, double Zp5, double Xv5, double Yv5, double Zv5,
	double reftime, double r0, double r1, double r2, double r3, double r4

)
{
	long pixelcount = rowcount * colcount;
	int numBlocks = (pixelcount + BLOCK_SIZE - 1) / BLOCK_SIZE;
	Kernel_RDProcess_doppler << <numBlocks, BLOCK_SIZE >> > (
		demX, demY, demZ,
		outRidx, outCidx,
		pixelcount,
		Xp0, Yp0, Zp0, Xv0, Yv0, Zv0,
		Xp1, Yp1, Zp1, Xv1, Yv1, Zv1,
		Xp2, Yp2, Zp2, Xv2, Yv2, Zv2,
		Xp3, Yp3, Zp3, Xv3, Yv3, Zv3,
		Xp4, Yp4, Zp4, Xv4, Yv4, Zv4,
		Xp5, Yp5, Zp5, Xv5, Yv5, Zv5,
		reftime, r0, r1, r2, r3, r4,
		starttime,nearRange, farRange,
		PRF, Fs,
		fact_lamda
		);
	PrintLasterError("RD with doppler function");
	cudaDeviceSynchronize();
}






