#include "model/algorithms/ScanConversionAlg.h"

#include <QtMath>

void ScanConversionAlg::linear(Input_t params, QVector<float> &scanXPos, QVector<float> &scanZPos,
							QVector<float> &gridPixelXPos, QVector<float> &gridPixelZPos)
{
	//just to map to X Z convention
	auto frameMaxXScan = params.bMaxScanAz;
	auto frameMinXScan = params.bMinScanAz;
	auto frameMaxZScan = params.bMaxScanAx;
	auto frameMinZScan = params.bMinScanAx;

	auto finalPixelXNo = params.outputWidth;
	auto finalPixelZNo = params.outputHeight;

	auto pixelXStep = (frameMaxXScan - frameMinXScan) / (finalPixelXNo - 1);
	auto pixelZStep = (frameMaxZScan - frameMinZScan) / (finalPixelZNo - 1);

	gridPixelXPos.reserve(finalPixelXNo * finalPixelZNo);
	gridPixelZPos.reserve(finalPixelXNo * finalPixelZNo);

	auto cap = gridPixelXPos.capacity();

	for(auto i = 0UL; i < finalPixelZNo; i++)
	{
		auto temp = frameMinZScan + i * pixelZStep;
		for(auto j = 0UL; j < finalPixelXNo; j++)
		{
			gridPixelZPos.push_back(temp / cosf(params.steering));
			gridPixelXPos.push_back((frameMinXScan + j * pixelXStep) -
									(temp * tanf(params.steering)));
		}
	}

	for(auto i = 0; i < params.rxLineNo; i++)
	{
		scanXPos.push_back(params.minScanAz + i * params.rxLineDaz);
	}

	for(auto i = 0; i < params.rxFocusPointNo; i++)
	{
		scanZPos.push_back(params.minScanAx + i * params.rxPointDax);
	}
}

/*************************************************************************************************/
void ScanConversionAlg::virtualConvex(Input_t params, QVector<float>& scanXPos,
									  QVector<float>& scanZPos, QVector<float>& gridPixelXPos,
									  QVector<float>& gridPixelZPos)
{
	auto finalPixelXNo = params.outputWidth;
	auto finalPixelZNo = params.outputHeight;

	auto minAbsScanAz = 0.0f;
	if (params.bMaxScanAz > 0 && params.bMinScanAz < 0)
	{
		minAbsScanAz = 0;
	}
	else
	{
		minAbsScanAz = qMin(abs(params.bMinScanAz), abs(params.bMaxScanAz));
	}

	auto maxAbsScanAz = qMax(abs(params.bMinScanAz), abs(params.bMaxScanAz));
	auto frameMaxZScan = params.bMaxScanAx *
			cosf(minAbsScanAz / params.fieldOfView * 2 * params.vcMaxTheta);
	auto frameMinZScan = params.bMinScanAx *
			cosf(maxAbsScanAz / params.fieldOfView * 2 * params.vcMaxTheta);
	auto frameMaxXScan = params.bMaxScanAz + params.bMaxScanAx *
			sinf(params.bMaxScanAz / params.fieldOfView * 2 * params.vcMaxTheta);
	auto frameMinXScan = params.bMinScanAz + params.bMaxScanAx *
			sinf(params.bMinScanAz / params.fieldOfView * 2 * params.vcMaxTheta);


	auto pixelXStep = (frameMaxXScan - frameMinXScan) / (finalPixelXNo - 1);
	auto pixelZStep = (frameMaxZScan - frameMinZScan) / (finalPixelZNo - 1);

	auto pixelXPos = QVector<float>();
	auto pixelZPos = QVector<float>();

	pixelXPos.reserve(finalPixelXNo * finalPixelZNo);
	pixelZPos.reserve(finalPixelXNo * finalPixelZNo);

	for(auto i = 0UL; i < finalPixelZNo; i++)
	{
		for(auto j = 0UL; j < finalPixelXNo; j++)
		{
			pixelZPos.push_back(frameMinZScan + i * pixelZStep);
			pixelXPos.push_back(frameMinXScan + j * pixelXStep);
		}
	}

	virtualScanConversion(pixelXPos, pixelZPos, finalPixelXNo, finalPixelZNo, params.steering,
						  params.virtualOriginalZ, params.startDepth, params.depth,
						  params.vcMaxTheta, gridPixelZPos, gridPixelXPos);

	//scanTheta
	for(auto i = 0; i < params.rxLineNo; i++)
	{
		scanXPos.push_back(params.minScanAz + i * params.rxLineDaz);
	}

	//scanR
	for(auto i = 0; i < params.rxFocusPointNo; i++)
	{
		scanZPos.push_back(params.minScanAx + i * params.rxPointDax);
	}
}

/*************************************************************************************************/
void ScanConversionAlg::convex(Input_t params, QVector<float>& scanXPos, QVector<float>& scanZPos,
							   QVector<float>& gridPixelXPos, QVector<float>& gridPixelZPos)
{
	auto finalPixelXNo = params.outputWidth;
	auto finalPixelZNo = params.outputHeight;

	auto frameMaxXScan = params.probe.radius * sinf(params.bMaxScanAz) +
			params.bMaxScanAx * sinf(params.bMaxScanAz / params.fieldOfView * params.angle);
	auto frameMinXScan = params.probe.radius * sinf(params.bMinScanAz) +
			params.bMaxScanAx * sinf(params.bMinScanAz / params.fieldOfView * params.angle);

	auto minAbsScanAz = 0.0f;
	if (params.bMaxScanAz > 0 && params.bMinScanAz < 0)
	{
		minAbsScanAz = 0;
	}
	else
	{
		minAbsScanAz = qMin(abs(params.bMinScanAz), abs(params.bMaxScanAz));
	}
	auto maxAbsScanAz = qMax(abs(params.bMinScanAz), abs(params.bMaxScanAz));

	auto frameMaxZScan = params.probe.radius * cosf(minAbsScanAz) +
			params.bMaxScanAx * cosf(minAbsScanAz / params.fieldOfView * params.angle);
	auto frameMinZScan = params.probe.radius * cosf(maxAbsScanAz) +
			params.bMinScanAx * cosf(maxAbsScanAz / params.fieldOfView * params.angle);

	auto pixelXStep = (frameMaxXScan - frameMinXScan) / (finalPixelXNo - 1);
	auto pixelZStep = (frameMaxZScan - frameMinZScan) / (finalPixelZNo - 1);

	auto pixelXPos = QVector<float>();
	auto pixelZPos = QVector<float>();

	pixelXPos.reserve(finalPixelXNo * finalPixelZNo);
	pixelZPos.reserve(finalPixelXNo * finalPixelZNo);

	for(auto i = 0UL; i < finalPixelZNo; i++)
	{
		for(auto j = 0UL; j < finalPixelXNo; j++)
		{
			pixelZPos.push_back(frameMinZScan + i * pixelZStep);
			pixelXPos.push_back(frameMinXScan + j * pixelXStep);
		}
	}

	convexScanConversion(pixelXPos, pixelZPos, finalPixelXNo, finalPixelZNo, params.steering,
						 params.angle, params.fieldOfView, params.probe.radius, params.startDepth,
						 params.depth, gridPixelZPos, gridPixelXPos);


	//scanTheta
	for(auto i = 0; i < params.rxLineNo; i++)
	{
		scanXPos.push_back(params.minScanAz + i * params.rxLineDaz + params.probe.radius);
	}

	//scanR
	for(auto i = 0; i < params.rxFocusPointNo; i++)
	{
		scanZPos.push_back(params.minScanAx + i * params.rxPointDax);
	}
}

/*************************************************************************************************/
void ScanConversionAlg::scanConversion(Input_t params, QVector<float>& scanXPos,
									   QVector<float>& scanZPos, QVector<float>& gridPixelXPos,
									   QVector<float>& gridPixelZPos)
{
	//convex
	if(!params.probe.linear)
	{
		convex(params, scanXPos, scanZPos, gridPixelXPos, gridPixelZPos);
	}
	//virtual convex
	else if(params.virtualConvex)
	{
		virtualConvex(params, scanXPos, scanZPos, gridPixelXPos, gridPixelZPos);
	}
	//linear
	else
	{
		linear(params, scanXPos, scanZPos, gridPixelXPos, gridPixelZPos);
	}
}

/*************************************************************************************************/
void ScanConversionAlg::virtualScanConversion(QVector<float> pixelXPos, QVector<float> pixelZPos,
											  uint width, uint height, float steering,
											  float virtualOriginalZ, float startDepth, float depth,
											  float vcMaxTheta, QVector<float>& gridPixelR,
											  QVector<float>& gridPixelTheta)
{
	for(auto i = 0U; i < width * height; i++)
	{
		auto x = pixelXPos[i];
		auto z = pixelZPos[i];

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

		if(pixelTheta >= -vcMaxTheta - abs(steering) &&
			pixelTheta <= vcMaxTheta + abs(steering))
		{
			if(steering == 0.0f)
			{
				auto gridPixelAx = sqrtf(powf(x - virtualOriginalZ * tanf(pixelTheta), 2) +
										 powf(z, 2));

				if(gridPixelAx >= startDepth &&	gridPixelAx <= depth)
				{
					gridPixelR.push_back(gridPixelAx);
					gridPixelTheta.push_back(pixelTheta);
				}
				else
				{
					gridPixelR.push_back(0);
					gridPixelTheta.push_back(0);
				}
			}
			else
			{
				auto strTan = tanf(steering);
				auto a = virtualOriginalZ * strTan;
				auto b = x * strTan + virtualOriginalZ + z;
				auto c = x - z * strTan;
				auto interceptTheta = atanf((b + sqrtf(powf(b, 2) - 4 * a * c)) / (2 * a));
				if(interceptTheta > vcMaxTheta || interceptTheta < -vcMaxTheta)
				{
					interceptTheta = atanf((b - sqrtf(powf(b, 2) - 4 * a * c)) / (2 * a));
					if(interceptTheta > vcMaxTheta || interceptTheta < -vcMaxTheta)
					{
						gridPixelR.push_back(0);
						gridPixelTheta.push_back(0);
					}
					else
					{
						auto gridPixelAx =
								sqrtf(powf(x - virtualOriginalZ * tanf(interceptTheta), 2) +
									  powf(z, 2));
						if(gridPixelAx >= startDepth &&	gridPixelAx <= depth)
						{
							gridPixelR.push_back(gridPixelAx);
							gridPixelTheta.push_back(interceptTheta);
						}
						else
						{
							gridPixelR.push_back(0);
							gridPixelTheta.push_back(0);
						}
					}
				}
				else
				{
					auto gridPixelAx =
							sqrtf(powf(x - virtualOriginalZ * tanf(interceptTheta), 2) +
								  powf(z, 2));
					if(gridPixelAx >= startDepth &&	gridPixelAx <= depth)
					{
						gridPixelR.push_back(gridPixelAx);
						gridPixelTheta.push_back(interceptTheta);
					}
					else
					{
						gridPixelR.push_back(0);
						gridPixelTheta.push_back(0);
					}
				}
			}//steering = 0
		}
		else
		{
			gridPixelR.push_back(0);
			gridPixelTheta.push_back(0);
		}
	}
}

/*************************************************************************************************/
void ScanConversionAlg::convexScanConversion(QVector<float> pixelXPos, QVector<float> pixelZPos,
											 uint width, uint height, float steering, float angle,
											 float fieldOfView, float probeRadius, float startDepth,
											 float depth, QVector<float>& gridPixelR,
											 QVector<float>& gridPixelTheta)
{
	auto virtualOriginalZ  = probeRadius *
			(cosf(fieldOfView / 2) - sinf(fieldOfView / 2) / tanf(angle / 2));
	auto virtualOriginalZ2 = powf(virtualOriginalZ, 2);

	auto maxR = probeRadius + depth;
	auto minR = probeRadius - virtualOriginalZ + startDepth;

	auto minTheta = -angle / 2 - abs(steering);
	auto maxTheta = angle / 2 + abs(steering);

	auto maxInterceptTheta = fieldOfView / 2;
	auto radius2 = powf(probeRadius, 2);

	if(steering == 0.0f)
	{
		for (auto i = 0U; i < width * height; i++)
		{
			auto x = pixelXPos[i];
			auto z = pixelZPos[i];

			auto pixelTheta = atan2f(x, z - virtualOriginalZ);
			auto pixelR = sqrtf(powf(x, 2) + powf(z - virtualOriginalZ, 2));

			if(pixelR >= minR && pixelR <= maxR && pixelTheta >= minTheta && pixelTheta <= maxTheta)
			{
				auto interceptTheta = 0.0f;
				auto interceptX = 0.0f;
				auto interceptZ = 0.0f;
				auto alpha = 0.0f;
				auto beta = 0.0f;

				if(x == 0.0f)
					interceptTheta = 0;
				else
				{
					alpha = virtualOriginalZ;
					beta = (virtualOriginalZ - z) / x;
					interceptX = (alpha * beta +
								  sqrtf(-1 * powf(alpha, 2) + (powf(beta, 2) + 1) * radius2)) /
							(powf(beta, 2) + 1);
					interceptZ = alpha - beta * interceptX;
					interceptTheta = atan2f(interceptX, interceptZ);
				}

				if(interceptTheta >  maxInterceptTheta || interceptTheta < -maxInterceptTheta)
				{
					interceptX = (alpha * beta -
								  sqrtf(-1 * powf(alpha, 2) + (powf(beta, 2) + 1) * radius2)) /
							(powf(beta, 2) + 1);
					interceptZ = alpha - beta * interceptX;
					interceptTheta = atan2f(interceptX, interceptZ);

					if(interceptTheta > maxInterceptTheta || interceptTheta < -maxInterceptTheta)
					{
						gridPixelR.push_back(0);
						gridPixelTheta.push_back(0);
					}
					else
					{
						auto gridPixelAx = sqrtf(powf(x - probeRadius * sinf(interceptTheta), 2) +
												 powf(z - probeRadius * cosf(interceptTheta), 2));
						if(gridPixelAx >= startDepth && gridPixelAx <= depth)
						{
							gridPixelR.push_back(probeRadius + gridPixelAx);
							gridPixelTheta.push_back(interceptTheta);
						}
						else
						{
							gridPixelR.push_back(0);
							gridPixelTheta.push_back(0);
						}
					}
				}
				else
				{
					auto gridPixelAx = sqrtf(powf(x - probeRadius * sinf(interceptTheta), 2) +
											 powf(z - probeRadius * cosf(interceptTheta), 2));
					if(gridPixelAx >= startDepth && gridPixelAx <= depth)
					{
						gridPixelR.push_back(probeRadius + gridPixelAx);
						gridPixelTheta.push_back(interceptTheta);
					}
					else
					{
						gridPixelR.push_back(0);
						gridPixelTheta.push_back(0);
					}
				}
			}
			else
			{
				gridPixelR.push_back(0);
				gridPixelTheta.push_back(0);
			}
		}
	}
	else
	{
		for (auto i = 0U; i < width * height; i++)
		{
			auto x = pixelXPos[i];
			auto z = pixelZPos[i];

			auto pixelTheta = atan2f(x, z - virtualOriginalZ);
			auto pixelR = sqrtf(powf(x, 2) + powf(z - virtualOriginalZ, 2));

			if(pixelR >= minR && pixelR <= maxR && pixelTheta >= minTheta && pixelTheta <= maxTheta)
			{
				auto strSin = 2 * sinf(steering);
				auto ro = pixelR / strSin;
				auto xo = ro * cosf(steering - pixelTheta);
				auto zo = ro * sinf(steering - pixelTheta) + virtualOriginalZ;
				if(zo == 0.0f)
				{
					auto interceptX = (radius2 - virtualOriginalZ2) / 2 / xo;
					auto interceptZ = sqrtf(radius2 - powf(interceptX , 2));
					auto interceptTheta = atan2f(interceptX, interceptZ);
					if (interceptTheta > maxInterceptTheta || interceptTheta < -maxInterceptTheta)
					{
						interceptZ = -sqrtf(radius2 - powf(interceptX , 2));
						interceptTheta = atan2f(interceptX, interceptZ);
						if(interceptTheta >  maxInterceptTheta ||
								interceptTheta < -maxInterceptTheta)
						{
							gridPixelR.push_back(0);
							gridPixelTheta.push_back(0);
						}
						else
						{
							auto gridPixelAx =
									sqrtf(powf(x - probeRadius * sinf(interceptTheta), 2) +
										  powf(z - probeRadius * cosf(interceptTheta), 2));
							if(gridPixelAx >= startDepth && gridPixelAx <= depth)
							{
								gridPixelR.push_back(probeRadius + gridPixelAx);
								gridPixelTheta.push_back(interceptTheta);
							}
							else
							{
								gridPixelR.push_back(0);
								gridPixelTheta.push_back(0);
							}
						}
					}
					else
					{
						auto gridPixelAx = 	sqrtf(powf(x - probeRadius * sinf(interceptTheta), 2) +
												  powf(z - probeRadius * cosf(interceptTheta), 2));
						if(gridPixelAx >= startDepth && gridPixelAx <= depth)
						{
							gridPixelR.push_back(probeRadius + gridPixelAx);
							gridPixelTheta.push_back(interceptTheta);
						}
						else
						{
							gridPixelR.push_back(0);
							gridPixelTheta.push_back(0);
						}
					}
				}
				else
				{
					auto alpha = (radius2 - virtualOriginalZ2 + 2 * zo * virtualOriginalZ) / 2 / zo;
					auto beta = xo / zo;

					auto interceptX = (alpha * beta +
									   sqrtf(-1 * powf(alpha, 2) + (powf(beta, 2) + 1) * radius2)) /
							(powf(beta, 2) + 1);
					auto interceptZ = alpha - beta * interceptX;
					auto interceptTheta = atan2f(interceptX, interceptZ);

					if(interceptTheta > maxInterceptTheta || interceptTheta < -maxInterceptTheta)
					{
						interceptX = (alpha * beta -
									  sqrtf(-1 * powf(alpha, 2) + (powf(beta, 2) + 1) * radius2)) /
								(powf(beta, 2) + 1);
						interceptZ = alpha - beta * interceptX;
						interceptTheta = atan2f(interceptX, interceptZ);
						if(interceptTheta > maxInterceptTheta || interceptTheta < -maxInterceptTheta)
						{
							gridPixelR.push_back(0);
							gridPixelTheta.push_back(0);
						}
						else
						{
							auto gridPixelAx = 	sqrtf(powf(x - probeRadius * sinf(interceptTheta), 2) +
													  powf(z - probeRadius * cosf(interceptTheta), 2));
							if(gridPixelAx >= startDepth && gridPixelAx <= depth)
							{
								gridPixelR.push_back(probeRadius + gridPixelAx);
								gridPixelTheta.push_back(interceptTheta);
							}
							else
							{
								gridPixelR.push_back(0);
								gridPixelTheta.push_back(0);
							}
						}
					}
					else
					{
						auto gridPixelAx = 	sqrtf(powf(x - probeRadius * sinf(interceptTheta), 2) +
												  powf(z - probeRadius * cosf(interceptTheta), 2));
						if(gridPixelAx >= startDepth && gridPixelAx <= depth)
						{
							gridPixelR.push_back(probeRadius + gridPixelAx);
							gridPixelTheta.push_back(interceptTheta);
						}
						else
						{
							gridPixelR.push_back(0);
							gridPixelTheta.push_back(0);
						}
					}
				}
			}
			else
			{
				gridPixelR.push_back(0);
				gridPixelTheta.push_back(0);
			}
		}
	}
}