#include "model/processor/strategies/ScanConversion.h"
#include <QPixmap>
#include <QImage>
#include "model/processor/BIP.h"

ScanConversion::ScanConversion(const Context context,
							   const QString kernelPath,
							   const QObject *parent = Q_NULLPTR) :
	IProcessStrategy(context, kernelPath, "ScanConversion", parent),
	_kernelFunctor(KernelFunctor<Image2D, Image2D,
				   Buffer, Buffer, Buffer, Buffer, ScanConversion_t>(_kernel))
{
	_gridPixelXPos = Q_NULLPTR;
	_gridPixelZPos = Q_NULLPTR;
	_scanXPos = Q_NULLPTR;
	_scanZPos = Q_NULLPTR;

	_gridX = Q_NULLPTR;
	_gridZ = Q_NULLPTR;
	_queryX = Q_NULLPTR;
	_queryZ = Q_NULLPTR;

	_gridPixelR = Q_NULLPTR;
	_gridPixelTheta = Q_NULLPTR;
}

void ScanConversion::cpuProcess(ScenGenOutput_t params)
{
	auto context = _openCLHelper.getContext();
	auto queue = BIP::getInstance()->CLQueue;
	//virtual convex
	if(params.virtualConvex.value)
	{
		if(params.outputWidth.isUpdated|| params.outputHeight.isUpdated)
		{
			if(_gridPixelXPos)
				delete[] _gridPixelXPos;

			if(_gridPixelZPos)
				delete[] _gridPixelZPos;

			_gridPixelXPos = new myflt[params.outputWidth.value *
											 params.outputHeight.value];
			_gridPixelZPos = new myflt[params.outputWidth.value *
											 params.outputHeight.value];

			_width = params.outputWidth.value;
			_height = params.outputHeight.value;

			_kernelParameters.width = _width;
		}

		if(params.depth.isUpdated || params.startDepth.isUpdated ||
			params.fieldOfView.isUpdated || params.vcMaxTheta.isUpdated ||
			params.outputWidth.isUpdated || params.outputHeight.isUpdated)
		{
			auto maxZ = params.depth.value;
			auto minZ = params.startDepth.value * cosf(params.vcMaxTheta.value);
			auto maxX = params.fieldOfView.value / 2 + maxZ * sinf(params.vcMaxTheta.value);
			auto minX = -1 * maxX;

			auto baseZ = (maxZ - minZ) / (params.outputHeight.value - 1);
			auto baseX = (maxX - minX) / (params.outputWidth.value - 1);

			for(auto i = 0; i < params.outputHeight.value; i++)
			{
				auto temp = i * baseZ;
				for(auto j = 0; j < params.outputWidth.value; j++)
				{
					_gridPixelZPos[i * params.outputHeight.value + j] = minZ + temp;
					_gridPixelXPos[i * params.outputHeight.value + j] = minX + (baseX * j);
				}
			}

			auto virtualOriginZ = params.fieldOfView.value / 2 / tanf(params.vcMaxTheta.value);

			if(_gridPixelR)
				delete[] _gridPixelR;

			if(_gridPixelTheta)
				delete[] _gridPixelTheta;

			_gridPixelR = new myflt[params.outputWidth.value *
											 params.outputHeight.value];
			_gridPixelTheta = new myflt[params.outputWidth.value *
											 params.outputHeight.value];

			auto strTan = tanf(params.steering.value);

			for(auto i = 0; i < params.outputWidth.value *
				params.outputHeight.value; i++)
			{
				auto x = _gridPixelXPos[i];
				auto z = _gridPixelZPos[i];

				auto pixelTheta = atanf(x / (z + virtualOriginZ));

				if(pixelTheta >= -params.vcMaxTheta.value - abs(params.steering.value) &&
					pixelTheta <= params.vcMaxTheta.value + abs(params.steering.value))
				{
					if(params.steering.value == 0)
					{
						auto gridPixelAx = sqrtf(powf(x - virtualOriginZ * tanf(pixelTheta), 2) + powf(z, 2));
						if(gridPixelAx >= params.startDepth.value &&
							gridPixelAx <= params.depth.value)
						{
							_gridPixelR[i] = gridPixelAx;
							_gridPixelTheta[i] = pixelTheta;
						}
						else
						{
							_gridPixelR[i] = 0;
							_gridPixelTheta[i] = 0;
						}
					}
					else
					{
						auto a = virtualOriginZ * strTan;
						auto b = x * strTan + virtualOriginZ + z;
						auto c = x - z * strTan;
						auto interceptTheta = atanf((b + sqrtf(powf(b, 2) - 4 * a * c)) / (2 * a));
						if(interceptTheta > params.vcMaxTheta.value ||
							interceptTheta < -params.vcMaxTheta.value)
						{
							interceptTheta = atanf((b - sqrtf(powf(b, 2) - 4 * a * c)) / (2 * a));
							if(interceptTheta > params.vcMaxTheta.value ||
								interceptTheta < -params.vcMaxTheta.value)
							{
								_gridPixelR[i] = 0;
								_gridPixelTheta[i] = 0;
							}
							else
							{
								auto gridPixelAx = sqrtf(powf(x - virtualOriginZ * tanf(interceptTheta), 2) + powf(z, 2));
								if(gridPixelAx >= params.startDepth.value &&
									gridPixelAx <= params.depth.value)
								{
									_gridPixelR[i] = gridPixelAx;
									_gridPixelTheta[i] = interceptTheta;
								}
								else
								{
									_gridPixelR[i] = 0;
									_gridPixelTheta[i] = 0;
								}
							}
						}
						else
						{
							auto gridPixelAx = sqrtf(powf(x - virtualOriginZ * tanf(interceptTheta), 2) + powf(z, 2));
							if(gridPixelAx >= params.startDepth.value &&
								gridPixelAx <= params.depth.value)
							{
								_gridPixelR[i] = gridPixelAx;
								_gridPixelTheta[i] = interceptTheta;
							}
							else
							{
								_gridPixelR[i] = 0;
								_gridPixelTheta[i] = 0;
							}
						}
					}
				}
				else
				{
					_gridPixelR[i] = 0;
					_gridPixelTheta[i] = 0;
				}
			}

			if(_queryX)
				delete _queryX;

			if(_queryZ)
				delete _queryZ;
			_queryX = _openCLHelper.array2CLBuffer(context,
												   params.outputWidth.value *
												   params.outputHeight.value *
												   sizeof (myflt));
			_queryZ = _openCLHelper.array2CLBuffer(context,
												   params.outputWidth.value *
												   params.outputHeight.value *
												   sizeof (myflt));

			BIP::getInstance()->CLQueue.enqueueWriteBuffer(*_queryZ, CL_TRUE, 0,
									   params.outputWidth.value *
									   params.outputHeight.value *
									   sizeof (myflt),
									   _gridPixelR);
			queue.enqueueWriteBuffer(*_queryX, CL_TRUE, 0,
									   params.outputWidth.value *
									   params.outputHeight.value *
									   sizeof (myflt),
									   _gridPixelTheta);
		}

		if(params.minScanAz.isUpdated || params.rxLineNo.isUpdated ||
				params.rxLineDaz.isUpdated)
		{
			if(_scanXPos)
				delete [] _scanXPos;

			if(_gridX)
				delete _gridX;

			_scanXPos = new myflt[params.rxLineNo.value];
			for(auto i = 0; i < params.rxLineNo.value; i++)
			{
				_scanXPos[i] = params.minScanAz.value + i * params.rxLineDaz.value;
			}

			_gridX = _openCLHelper.array2CLBuffer(context,
												  static_cast<quint64>(params.rxLineNo.value)
												  * sizeof (myflt));

			queue.enqueueWriteBuffer(*_gridX, CL_TRUE, 0,
									 static_cast<unsigned long>(params.rxLineNo.value) * sizeof (myflt),
									 _scanXPos);

			_kernelParameters.gridXSize = params.rxLineNo.value;
		}

		if(params.rxFocusPointNo.isUpdated || params.minScanAx.isUpdated ||
				params.rxPointDax.isUpdated)
		{
			if(_scanZPos)
				delete[] _scanZPos;

			if(_gridZ)
				delete _gridZ;

			_scanZPos = new myflt [params.rxFocusPointNo.value];
			for(auto i = 0; i < params.rxFocusPointNo.value; i++)
			{
				_scanZPos[i] = params.minScanAx.value + i * params.rxPointDax.value;
			}

			_gridZ = _openCLHelper.array2CLBuffer(context,
												  static_cast<quint64>(params.rxFocusPointNo.value)
												  * sizeof (myflt));

			queue.enqueueWriteBuffer(*_gridZ, CL_TRUE, 0,
									 static_cast<unsigned long>(params.rxFocusPointNo.value) * sizeof (myflt),
									 _scanZPos);

			_kernelParameters.gridZSize = params.rxFocusPointNo.value;
		}
	}
	//linear non convex
	else
	{
		if(params.outputWidth.isUpdated || params.outputHeight.isUpdated)
		{
			if(_gridPixelXPos)
				delete[] _gridPixelXPos;

			if(_gridPixelZPos)
				delete[] _gridPixelZPos;

			_gridPixelXPos = new myflt[params.outputWidth.value *
											 params.outputHeight.value];
			_gridPixelZPos = new myflt[params.outputWidth.value *
											 params.outputHeight.value];

			if(_queryX)
				delete _queryX;

			if(_queryZ)
				delete _queryZ;

			_queryX = _openCLHelper.array2CLBuffer(context,
												   params.outputWidth.value *
												   params.outputHeight.value *
												   sizeof (myflt));
			_queryZ = _openCLHelper.array2CLBuffer(context,
												   params.outputWidth.value *
												   params.outputHeight.value *
												   sizeof (myflt));

			_width = params.outputWidth.value;
			_height = params.outputHeight.value;

			_kernelParameters.width = _width;
		}

		if(params.depth.isUpdated || params.startDepth.isUpdated ||
				params.fieldOfView.isUpdated || params.steering.isUpdated ||
				params.outputWidth.isUpdated || params.outputHeight.isUpdated)
		{
			auto maxZ = params.depth.value;
			auto minZ = params.startDepth.value;
			auto maxX = params.fieldOfView.value / 2;
			auto minX = -1 * maxX;

			auto baseZ = (maxZ - minZ) / (params.outputHeight.value - 1);
			auto baseX = (maxX - minX) / (params.outputWidth.value - 1);

			for(auto i = 0; i < params.outputHeight.value; i++)
			{
				auto temp = i * baseZ;
				auto tan_result = temp * tanf(params.steering.value);
				for(auto j = 0; j < params.outputWidth.value; j++)
				{
					_gridPixelZPos[i * params.outputHeight.value + j] = minZ + temp;
					_gridPixelXPos[i * params.outputHeight.value + j] = minX + (baseX * j) - tan_result;
				}
			}

			BIP::getInstance()->CLQueue.enqueueWriteBuffer(*_queryX, CL_TRUE, 0,
									   params.outputWidth.value *
									   params.outputHeight.value *
									   sizeof (myflt),
									   _gridPixelXPos);
			queue.enqueueWriteBuffer(*_queryZ, CL_TRUE, 0,
									   params.outputWidth.value *
									   params.outputHeight.value *
									   sizeof (myflt),
									   _gridPixelZPos);
		}

		if(params.fieldOfView.isUpdated || params.rxLineNo.isUpdated ||
				params.rxLineDaz.isUpdated)
		{
			if(_scanXPos)
				delete [] _scanXPos;

			if(_gridX)
				delete _gridX;

			_scanXPos = new myflt[params.rxLineNo.value];
			for(auto i = 0; i < params.rxLineNo.value; i++)
			{
				_scanXPos[i] = -params.fieldOfView.value / 2 + i * params.rxLineDaz.value;
			}

			_gridX = _openCLHelper.array2CLBuffer(context,
												  static_cast<quint64>(params.rxLineNo.value)
												  * sizeof (myflt));

			queue.enqueueWriteBuffer(*_gridX, CL_TRUE, 0,
									 static_cast<unsigned long>(params.rxLineNo.value) * sizeof (myflt),
									 _scanXPos);

			_kernelParameters.gridXSize = params.rxLineNo.value;
		}

		if(params.rxFocusPointNo.isUpdated || params.rxPointDax.isUpdated)
		{
			if(_scanZPos)
				delete[] _scanZPos;

			if(_gridZ)
				delete _gridZ;

			_scanZPos = new myflt [params.rxFocusPointNo.value];
			for(auto i = 0; i < params.rxFocusPointNo.value; i++)
			{
				_scanZPos[i] = params.startDepth.value + i * params.rxPointDax.value;
			}

			_gridZ = _openCLHelper.array2CLBuffer(context,
												  static_cast<quint64>(params.rxFocusPointNo.value)
												  * sizeof (myflt));

			queue.enqueueWriteBuffer(*_gridZ, CL_TRUE, 0,
									 static_cast<unsigned long>(params.rxFocusPointNo.value) * sizeof (myflt),
									 _scanZPos);

			_kernelParameters.gridZSize = params.rxFocusPointNo.value;
		}
	}
}

void ScanConversion::finalize()
{
	delete _gridPixelXPos;
	delete _gridPixelZPos;
	delete _scanXPos;
	delete _scanZPos;

	delete _gridX;
	delete _gridZ;
	delete _queryX;
	delete _queryZ;

	_gridPixelXPos = Q_NULLPTR;
	_gridPixelZPos = Q_NULLPTR;
	_scanXPos = Q_NULLPTR;
	_scanZPos = Q_NULLPTR;

	_gridX = Q_NULLPTR;
	_gridZ = Q_NULLPTR;
	_queryX = Q_NULLPTR;
	_queryZ = Q_NULLPTR;
}

Image* ScanConversion::processKernel(Image *frames, Buffer* scrathPad)
{
	Context context = _openCLHelper.getContext();
	auto format = frames->getImageInfo<CL_IMAGE_FORMAT>();
	auto imageOutput = new Image2D(context,
							CL_MEM_READ_WRITE,
							ImageFormat(format.image_channel_order, format.image_channel_data_type),
							_width,
							_height);
	cl::EnqueueArgs eargs(BIP::getInstance()->CLQueue, cl::NDRange(_width, _height));

	_openCLHelper.runKernelFunctor<Image2D, Image2D, Buffer, Buffer, Buffer,
			Buffer, ScanConversion_t>(_kernelFunctor,
									  eargs,
									  *(Image2D*)(frames),
									  *imageOutput,
									  *_gridX,
									  *_gridZ,
									  *_queryX,
									  *_queryZ,
									  _kernelParameters);
	delete frames;

	return imageOutput;
}