/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2022, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */

#include "slam-precomp.h"  // Precompiled headers
//
#include <mrpt/maps/CLandmark.h>
#include <mrpt/maps/CLandmarksMap.h>
#include <mrpt/math/TPose2D.h>
#include <mrpt/math/utils.h>
#include <mrpt/obs/CObservationBeaconRanges.h>
#include <mrpt/slam/CRejectionSamplingRangeOnlyLocalization.h>

using namespace mrpt::math;
using namespace mrpt::slam;
using namespace mrpt::maps;
using namespace mrpt::obs;
using namespace mrpt::random;
using namespace mrpt::poses;
using namespace std;

/*---------------------------------------------------------------
						Constructor
---------------------------------------------------------------*/
CRejectionSamplingRangeOnlyLocalization::
	CRejectionSamplingRangeOnlyLocalization()
	: m_oldPose(),

	  m_dataPerBeacon()
{
}

/*---------------------------------------------------------------
					RS_drawFromProposal
---------------------------------------------------------------*/
void CRejectionSamplingRangeOnlyLocalization::RS_drawFromProposal(
	CPose2D& outSample)
{
	MRPT_START

	// Use the first beacon data to draw a pose:
	if (m_dataPerBeacon.size() == 0)
		THROW_EXCEPTION(
			"There is no information from which to draw samples!! Use "
			"'setParams' with valid data!");

	ASSERT_(m_drawIndex < m_dataPerBeacon.size());

	float ang = getRandomGenerator().drawUniform(
		m_dataPerBeacon[m_drawIndex].minAngle,
		m_dataPerBeacon[m_drawIndex].maxAngle);
	float R = getRandomGenerator().drawGaussian1D(
		m_dataPerBeacon[m_drawIndex].radiusAtRobotPlane, m_sigmaRanges);

	// This is the point where the SENSOR is:
	outSample.x(m_dataPerBeacon[m_drawIndex].beaconPosition.x + cos(ang) * R);
	outSample.y(m_dataPerBeacon[m_drawIndex].beaconPosition.y + sin(ang) * R);

	outSample.phi(
		getRandomGenerator().drawGaussian1D(m_oldPose.phi(), 2.0_deg));

	// Compute the robot pose P.
	//	  P = SAMPLE - ROT · SENSOR_ON_ROBOT
	mrpt::math::TPoint2D on(
		m_dataPerBeacon[m_drawIndex].sensorOnRobot.x,
		m_dataPerBeacon[m_drawIndex].sensorOnRobot.y);
	mrpt::math::TPoint2D S(outSample.x(), outSample.y());
	on = mrpt::math::TPoint2D(mrpt::math::TPose2D(0, 0, outSample.phi()) + on);
	S = S - on;

	outSample.x(S.x);
	outSample.y(S.y);

	MRPT_END
}

/*---------------------------------------------------------------
					RS_observationLikelihood
---------------------------------------------------------------*/
double CRejectionSamplingRangeOnlyLocalization::RS_observationLikelihood(
	const CPose2D& x)
{
	MRPT_START

	double lik = 1.0;
	double m_sigmaRanges2 = square(m_sigmaRanges);

	// Evaluate the likelihood for all the observations but the "m_drawIndex":
	for (size_t i = 0; i < m_dataPerBeacon.size(); i++)
	{
		// TODO: height now includes the sensor "z"!!!...
		CPoint3D P(
			x +
			CPoint3D(
				m_dataPerBeacon[i].sensorOnRobot.x,
				m_dataPerBeacon[i].sensorOnRobot.y, 0));

		if (i != m_drawIndex)
			// Evalute:
			lik *=
				exp(-0.5 *
					square(
						m_dataPerBeacon[i].radiusAtRobotPlane -
						P.distanceTo(m_dataPerBeacon[i].beaconPosition)) /
					m_sigmaRanges2);
	}

	return lik;

	MRPT_END
}

/*---------------------------------------------------------------
					setParams
---------------------------------------------------------------*/
bool CRejectionSamplingRangeOnlyLocalization::setParams(
	const CLandmarksMap& beaconsMap,
	const CObservationBeaconRanges& observation, float sigmaRanges,
	const CPose2D& oldPose, float robot_z, bool autoCheckAngleRanges)

{
	MRPT_START

	// Various vars:
	m_z_robot = robot_z;
	m_oldPose = oldPose;
	m_sigmaRanges = sigmaRanges;  // TODO: observation.stdError;

	double xMin = 1e30, xMax = -1e30, yMin = 1e30, yMax = -1e30,
		   gridRes = 2 * m_sigmaRanges;
	float R;

	// Save each observed beacon data:
	m_dataPerBeacon.clear();

	// Minimum radius:
	std::deque<
		mrpt::obs::CObservationBeaconRanges::TMeasurement>::const_iterator it;
	size_t i;

	// For each observation:
	for (i = 0, it = observation.sensedData.begin();
		 it != observation.sensedData.end(); ++it, ++i)
	{
		// Is in the map?
		const CLandmark* lm = beaconsMap.landmarks.getByBeaconID(it->beaconID);
		if (lm != nullptr)
		{
			TDataPerBeacon data;

			data.sensorOnRobot = it->sensorLocationOnRobot.asTPoint();

			data.beaconPosition.x = lm->pose_mean.x;
			data.beaconPosition.y = lm->pose_mean.y;

			// First compute squared:
			data.radiusAtRobotPlane =
				square(it->sensedDistance) - square(lm->pose_mean.z - robot_z);

			if (data.radiusAtRobotPlane > 0)
			{
				data.radiusAtRobotPlane = sqrt(data.radiusAtRobotPlane);
				data.minAngle = -M_PIf;
				data.maxAngle = M_PIf;
				m_dataPerBeacon.push_back(data);

				// std::cout << "BEACON: " << m_dataPerBeacon.size() << "\n   "
				// <<data.beaconPosition << " R=" << data.radiusAtRobotPlane <<
				// "\n";

				// Keep max/min:
				xMin = min(
					xMin,
					data.beaconPosition.x -
						(data.radiusAtRobotPlane + 4 * m_sigmaRanges + 1.0));
				xMax = max(
					xMax,
					data.beaconPosition.x +
						(data.radiusAtRobotPlane + 4 * m_sigmaRanges + 1.0));
				yMin = min(
					yMin,
					data.beaconPosition.y -
						(data.radiusAtRobotPlane + 4 * m_sigmaRanges + 1.0));
				yMax = max(
					yMax,
					data.beaconPosition.y +
						(data.radiusAtRobotPlane + 4 * m_sigmaRanges + 1.0));
			}
			else
			{
				printf(
					"\nWARNING: Beacon range is shorter than distance between "
					"the robot and the beacon in the Z axis!!!: Ignoring this "
					"measurement\n");
			}
		}
	}  // end for

	// ------------------------------------------------------------------------
	// Precompute the min/max angles for potential samples, for each beacon:
	// ------------------------------------------------------------------------
	if (autoCheckAngleRanges && m_dataPerBeacon.size() > 1)
	{
		// Build the grid:
		// Each cell in the grid is a vector of bools, each one indicating
		// whether some samples from the i'th beacon falls there.
		mrpt::containers::CDynamicGrid<std::vector<bool>> grid(
			xMin, xMax, yMin, yMax, gridRes);
		grid.fill(std::vector<bool>(m_dataPerBeacon.size(), false));
		std::vector<bool>* cell;

		// The ngular step size:
		float Aa = 5.0_deg;

		// Fill the grid:
		for (i = 0; i < m_dataPerBeacon.size(); i++)
		{
			for (float a = -M_PIf; a <= M_PIf; a += Aa)
			{
				// Radius R - 3*SIGMA:
				R = m_dataPerBeacon[i].radiusAtRobotPlane -
					3.0f * m_sigmaRanges;
				R = max(R, 0.0f);
				cell = grid.cellByPos(
					m_dataPerBeacon[i].beaconPosition.x + cos(a) * R,
					m_dataPerBeacon[i].beaconPosition.y + sin(a) * R);
				ASSERT_(cell != nullptr);
				(*cell)[i] = true;
				// Radius R :
				R = m_dataPerBeacon[i].radiusAtRobotPlane;
				cell = grid.cellByPos(
					m_dataPerBeacon[i].beaconPosition.x + cos(a) * R,
					m_dataPerBeacon[i].beaconPosition.y + sin(a) * R);
				ASSERT_(cell != nullptr);
				(*cell)[i] = true;
				// Radius R + 3*SIGMA:
				R = m_dataPerBeacon[i].radiusAtRobotPlane +
					3.0f * m_sigmaRanges;
				cell = grid.cellByPos(
					m_dataPerBeacon[i].beaconPosition.x + cos(a) * R,
					m_dataPerBeacon[i].beaconPosition.y + sin(a) * R);
				ASSERT_(cell != nullptr);
				(*cell)[i] = true;
			}  // end for a
		}  // end for i

		// Check the angles:
		for (i = 0; i < m_dataPerBeacon.size(); i++)
		{
			float maxA = -M_PIf, minA = M_PIf;
			for (float a = -M_PIf; a < M_PIf; a += Aa)
			{
				// Radius R - 3*SIGMA:
				R = m_dataPerBeacon[i].radiusAtRobotPlane -
					3.0f * m_sigmaRanges;
				R = max(R, 0.0f);
				cell = grid.cellByPos(
					m_dataPerBeacon[i].beaconPosition.x + cos(a) * R,
					m_dataPerBeacon[i].beaconPosition.y + sin(a) * R);
				ASSERT_(cell != nullptr);
				if (std::count(cell->begin(), cell->end(), true) > 1)
				{
					maxA = max(maxA, a);
					minA = min(minA, a);
				}
				// Radius R :
				R = m_dataPerBeacon[i].radiusAtRobotPlane;
				cell = grid.cellByPos(
					m_dataPerBeacon[i].beaconPosition.x + cos(a) * R,
					m_dataPerBeacon[i].beaconPosition.y + sin(a) * R);
				ASSERT_(cell != nullptr);
				if (std::count(cell->begin(), cell->end(), true) > 1)
				{
					maxA = max(maxA, a);
					minA = min(minA, a);
				}
				// Radius R + 3*SIGMA:
				R = m_dataPerBeacon[i].radiusAtRobotPlane +
					3.0f * m_sigmaRanges;
				cell = grid.cellByPos(
					m_dataPerBeacon[i].beaconPosition.x + cos(a) * R,
					m_dataPerBeacon[i].beaconPosition.y + sin(a) * R);
				ASSERT_(cell != nullptr);
				if (std::count(cell->begin(), cell->end(), true) > 1)
				{
					maxA = max(maxA, a);
					minA = min(minA, a);
				}
			}  // end for a
			m_dataPerBeacon[i].minAngle = max(-M_PIf, minA - Aa);
			m_dataPerBeacon[i].maxAngle = min(M_PIf, maxA + Aa);
		}  // end for i
	}  // end if >1 beacons

	// Select best candidate for "m_drawIndex":
	float minCoberture = 1e30f;
	m_drawIndex = 0;
	for (i = 0; i < m_dataPerBeacon.size(); i++)
	{
		float c = m_dataPerBeacon[i].radiusAtRobotPlane *
			(m_dataPerBeacon[i].maxAngle - m_dataPerBeacon[i].minAngle);
		if (c < minCoberture)
		{
			minCoberture = c;
			m_drawIndex = i;
		}
	}  // end for i

	// End!
	return m_dataPerBeacon.size() != 0;
	MRPT_END
}
