#include "stdafx.h"
#include "TBPImageAlgCls.h"
#include <QDateTime>
#include <QDebug>
#include <QString>
#include <cmath>

void TBPImageProcess(QString echofile, QString outImageFolder, QString imagePlanePath,long num_thread)
{
	std::shared_ptr<EchoL0Dataset> echoL0ds(new EchoL0Dataset);
	echoL0ds->Open(echofile);

	std::shared_ptr< SARSimulationImageL1Dataset> imagL1(new SARSimulationImageL1Dataset);
	imagL1->setCenterAngle(echoL0ds->getCenterAngle());
	imagL1->setCenterFreq(echoL0ds->getCenterFreq());
	imagL1->setNearRange(echoL0ds->getNearRange());
	imagL1->setRefRange((echoL0ds->getNearRange() + echoL0ds->getFarRange()) / 2);
	imagL1->setFarRange(echoL0ds->getFarRange());
	imagL1->setFs(echoL0ds->getFs());
	imagL1->setLookSide(echoL0ds->getLookSide());
	imagL1->OpenOrNew(outImageFolder, echoL0ds->getSimulationTaskName(), echoL0ds->getPluseCount(), echoL0ds->getPlusePoints());

	
	TBPImageAlgCls TBPimag;
	TBPimag.setEchoL0(echoL0ds);
	TBPimag.setImageL1(imagL1);
	TBPimag.setImagePlanePath(imagePlanePath);

	TBPimag.Process(num_thread);
}

void TBPImageAlgCls::setImagePlanePath(QString INimagePlanePath)
{
	this->imagePlanePath = INimagePlanePath;
}

QString TBPImageAlgCls::getImagePlanePath()
{
	return this->imagePlanePath;
}

void TBPImageAlgCls::setEchoL0(std::shared_ptr<EchoL0Dataset> inL0ds)
{
	this->L0ds = inL0ds;
}

void TBPImageAlgCls::setImageL1(std::shared_ptr<SARSimulationImageL1Dataset> inL1ds)
{
	this->L1ds = inL1ds;
}

std::shared_ptr<EchoL0Dataset> TBPImageAlgCls::getEchoL1()
{
	return  this->L0ds;
}

std::shared_ptr<SARSimulationImageL1Dataset> TBPImageAlgCls::getImageL0()
{
	return this->L1ds;
}

ErrorCode TBPImageAlgCls::Process(long num_thread)
{
	if (GPURUN) {

	}
	else {
		return	this->ProcessCPU(num_thread);
	}
}


ErrorCode TBPImageAlgCls::ProcessCPU(long num_thread)
{
	omp_set_num_threads(num_thread);
	// 常用参数
	long rowCount = this->L1ds->getrowCount();
	long colCount = this->L1ds->getcolCount();
	long pixelCount = rowCount * colCount;
	long PRFCount = this->L0ds->getPluseCount();
	long PlusePoints = this->L0ds->getPlusePoints();


	double Rnear = this->L1ds->getNearRange();
	double Rfar = this->L1ds->getFarRange();
	double fs = this->L1ds->getFs();
	double dx = LIGHTSPEED / 2 / fs;
	double factorj = this->L1ds->getCenterFreq() * 4 * M_PI / LIGHTSPEED * 1e9;


	Eigen::MatrixXcd echo = Eigen::MatrixXcd::Zero(PRFCount, PlusePoints);
	{
		std::shared_ptr<std::complex<double>> echodata = this->L0ds->getEchoArr();
		for (long i = 0; i < PRFCount; i++) {
			for (long j = 0; j < PlusePoints; j++) {
				echo(i, j) = echodata.get()[i * PlusePoints + j];
			}
		}
		echodata.reset();
	}

	Eigen::MatrixXd pixelX = Eigen::MatrixXd::Zero(rowCount, colCount);
	Eigen::MatrixXd pixelY = Eigen::MatrixXd::Zero(rowCount, colCount);
	Eigen::MatrixXd pixelZ = Eigen::MatrixXd::Zero(rowCount, colCount);

	Eigen::MatrixXd Pxs = Eigen::MatrixXd::Zero(this->L0ds->getPluseCount(), 1);
	Eigen::MatrixXd Pys = Eigen::MatrixXd::Zero(this->L0ds->getPluseCount(), 1);
	Eigen::MatrixXd Pzs = Eigen::MatrixXd::Zero(this->L0ds->getPluseCount(), 1);

	// 图像网格坐标 
	{
		std::shared_ptr<double> antpos = this->L0ds->getAntPos();
		double time = 0;
		double Px = 0;
		double Py = 0;
		double Pz = 0;
		double Vx = 0;
		double Vy = 0;
		double Vz = 0;
		double AntDirectX = 0;
		double AntDirectY = 0;
		double AntDirectZ = 0;
		double AVx = 0;
		double AVy = 0;
		double AVz = 0;

		double R = 0;
		double NormAnt = 0;

		for (long i = 0; i < rowCount; i++) {
			time = antpos.get()[i * 16 + 0];
			Px = antpos.get()[i * 16 + 1];
			Py = antpos.get()[i * 16 + 2];
			Pz = antpos.get()[i * 16 + 3];
			Vx = antpos.get()[i * 16 + 4];
			Vy = antpos.get()[i * 16 + 5];
			Vz = antpos.get()[i * 16 + 6];
			AntDirectX = antpos.get()[i * 16 + 7];
			AntDirectY = antpos.get()[i * 16 + 8];
			AntDirectZ = antpos.get()[i * 16 + 9];
			AVx = antpos.get()[i * 16 + 10];
			AVy = antpos.get()[i * 16 + 11];
			AVz = antpos.get()[i * 16 + 12];

			NormAnt = std::sqrt(AntDirectX * AntDirectX + AntDirectY * AntDirectY + AntDirectZ * AntDirectZ);
			AntDirectX = AntDirectX / NormAnt;
			AntDirectY = AntDirectY / NormAnt;
			AntDirectZ = AntDirectZ / NormAnt;// 归一化

			antpos.get()[i * 16 + 7] = AntDirectX;
			antpos.get()[i * 16 + 8] = AntDirectY;
			antpos.get()[i * 16 + 9] = AntDirectZ;
			Pxs(i, 0) = Px;
			Pys(i, 0) = Py;
			Pzs(i, 0) = Pz;

			for (long j = 0; j < colCount; j++) {
				R = j * dx + Rnear;
				pixelX(i, j) = Px + AntDirectX * R;
				pixelY(i, j) = Py + AntDirectY * R;
				pixelZ(i, j) = Pz + AntDirectZ * R;
			}
		}

		this->L1ds->saveAntPos(antpos);
		antpos.reset();
	}

	// BP成像
	long BlockLine = Memory1MB * 10 / 16 / rowCount;
	if (rowCount / BlockLine / num_thread < 3) {
		BlockLine = rowCount / num_thread / 3;
	}
	BlockLine = BlockLine > 10 ? BlockLine : 10;

	std::shared_ptr<std::complex<double>> imagarr = this->L1ds->getImageRaster();
	{
		for (long i = 0; i < pixelCount; i++) {
			imagarr.get()[i] = imagarr.get()[i];
		}
	}
	omp_lock_t lock; // 定义锁
	omp_init_lock(&lock); // 初始化锁

	long writeImageCount = 0;
	qDebug() << "block line:\t" << BlockLine;


	long startLine = 0;
	long processValue = 0;

#pragma omp parallel for
	for (startLine = 0; startLine < rowCount; startLine = startLine + BlockLine) { // 图像大小
		long stepLine = startLine + BlockLine < rowCount ? BlockLine : rowCount - startLine;
		long imageRowID = startLine; // 

		//Eigen::MatrixXd R = Eigen::MatrixXd::Zero(rowCount, 1);
		long pluseId = 0;
		std::complex<double> factPhas(0, 0);
		std::complex < double> sign(0, 0);
		Eigen::MatrixXd R = Eigen::MatrixXd::Zero(rowCount, 1);
		Eigen::MatrixXcd Rphi = Eigen::MatrixXd::Zero(rowCount, 1);

		long PluseIDs = 0;
		double mask = 0;

		for (long i = 0; i < stepLine; i++) { // 图像行
			imageRowID = startLine + i;
			for (long j = 0; j < colCount; j++) { //图像列
				R = ((pixelX(i, j) - Pxs.array()).array().pow(2) + (pixelY(i, j) - Pys.array()).array().pow(2) + (pixelZ(i, j) - Pzs.array()).array().pow(2)).array().sqrt();
				Rphi = Rphi.array() * 0;
				Rphi.imag() = R.array() * factorj;
				Rphi = Rphi.array().exp();

				for (long prfid = 0; prfid < rowCount; prfid++) { // 脉冲行
					PluseIDs = std::floor((R(prfid, 0) - Rnear) / dx);
					mask = (PluseIDs < 0 || PluseIDs >= PlusePoints) ? 0 : 1;
					PluseIDs = (PluseIDs < 0 || PluseIDs >= PlusePoints) ? 0 : PluseIDs;
					imagarr.get()[imageRowID * colCount + j] =
						imagarr.get()[imageRowID * colCount + j] +
						mask * echo(prfid, PluseIDs) * Rphi(prfid, 0);// 信号* 相位校正
				}
			}

		}
		omp_set_lock(&lock); // 保存文件
		processValue = processValue + BlockLine;
		this->L1ds->saveImageRaster(imagarr, 0, rowCount);
		qDebug() << QDateTime::currentDateTime().toString("yyyy-MM-dd HH:mm:ss.zzz").toUtf8().constData() << "\t" << processValue * 100.0 / rowCount << "%\t" << startLine << "\t-\t" << startLine + BlockLine << "\tend\t\t";
		omp_unset_lock(&lock); // 解锁
	}
	omp_destroy_lock(&lock); // 销毁锁

	this->L1ds->saveImageRaster(imagarr, 0, rowCount);
	this->L1ds->saveToXml();
	return ErrorCode::SUCCESS;
}

ErrorCode TBPImageAlgCls::ProcessGPU()
{
	// 常用参数
	long rowCount = this->L1ds->getrowCount();
	long colCount = this->L1ds->getcolCount();
	long pixelCount = rowCount * colCount;
	long PRFCount = this->L0ds->getPluseCount();
	long PlusePoints = this->L0ds->getPlusePoints();


	double Rnear = this->L1ds->getNearRange();
	double Rfar = this->L1ds->getFarRange();
	double fs = this->L1ds->getFs();
	double dx = LIGHTSPEED / 2 / fs;
	double factorj = this->L1ds->getCenterFreq() * 4 * M_PI / LIGHTSPEED * 1e9;

	std::shared_ptr<float> pixelX(new float[rowCount*colCount],delArrPtr);
	std::shared_ptr<float> pixelY(new float[rowCount*colCount],delArrPtr);
	std::shared_ptr<float> pixelZ(new float[rowCount*colCount],delArrPtr);

	std::shared_ptr<float>  Pxs (new float[this->L0ds->getPluseCount()]);
	std::shared_ptr<float>  Pys (new float[this->L0ds->getPluseCount()]);
	std::shared_ptr<float>  Pzs (new float[this->L0ds->getPluseCount()]);
	std::shared_ptr<float>  Vxs (new float[this->L0ds->getPluseCount()]);
	std::shared_ptr<float>  Vys (new float[this->L0ds->getPluseCount()]);
	std::shared_ptr<float>  Vzs (new float[this->L0ds->getPluseCount()]);



	// 图像网格坐标 
	{
		std::shared_ptr<double> antpos = this->L0ds->getAntPos();
		double time = 0;
		double Px = 0;
		double Py = 0;
		double Pz = 0;
		double Vx = 0;
		double Vy = 0;
		double Vz = 0;
		double AntDirectX = 0;
		double AntDirectY = 0;
		double AntDirectZ = 0;
		double AVx = 0;
		double AVy = 0;
		double AVz = 0;

		double R = 0;
		double NormAnt = 0;

		for (long i = 0; i < rowCount; i++) {
			time = antpos.get()[i * 16 + 0];
			Px = antpos.get()[i * 16 + 1];
			Py = antpos.get()[i * 16 + 2];
			Pz = antpos.get()[i * 16 + 3];
			Vx = antpos.get()[i * 16 + 4];
			Vy = antpos.get()[i * 16 + 5];
			Vz = antpos.get()[i * 16 + 6];
			AntDirectX = antpos.get()[i * 16 + 7];
			AntDirectY = antpos.get()[i * 16 + 8];
			AntDirectZ = antpos.get()[i * 16 + 9];
			AVx = antpos.get()[i * 16 + 10];
			AVy = antpos.get()[i * 16 + 11];
			AVz = antpos.get()[i * 16 + 12];

			NormAnt = std::sqrt(AntDirectX * AntDirectX + AntDirectY * AntDirectY + AntDirectZ * AntDirectZ);
			AntDirectX = AntDirectX / NormAnt;
			AntDirectY = AntDirectY / NormAnt;
			AntDirectZ = AntDirectZ / NormAnt;// 归一化

			antpos.get()[i * 16 + 7] = AntDirectX;
			antpos.get()[i * 16 + 8] = AntDirectY;
			antpos.get()[i * 16 + 9] = AntDirectZ;
	

			for (long j = 0; j < colCount; j++) {
				R = j * dx + Rnear;
	
			}
		}

		this->L1ds->saveAntPos(antpos);
		antpos.reset();
	}






	return ErrorCode::SUCCESS;
}