/* +------------------------------------------------------------------------+
   |                     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 "maps-precomp.h"  // Precomp header
//
#include <mrpt/bayes/CParticleFilter.h>
#include <mrpt/bayes/CParticleFilterCapable.h>
#include <mrpt/config/CConfigFileBase.h>
#include <mrpt/core/round.h>  // round()
#include <mrpt/io/CFileOutputStream.h>
#include <mrpt/maps/CBeaconMap.h>
#include <mrpt/math/data_utils.h>  // averageLogLikelihood()
#include <mrpt/math/geometry.h>
#include <mrpt/obs/CObservationBeaconRanges.h>
#include <mrpt/opengl/CGridPlaneXY.h>
#include <mrpt/opengl/COpenGLScene.h>
#include <mrpt/opengl/CSetOfObjects.h>
#include <mrpt/opengl/stock_objects.h>
#include <mrpt/random.h>
#include <mrpt/serialization/CArchive.h>
#include <mrpt/system/os.h>
#include <mrpt/system/string_utils.h>

#include <Eigen/Dense>

using namespace mrpt;
using namespace mrpt::maps;
using namespace mrpt::math;
using namespace mrpt::obs;
using namespace mrpt::random;
using namespace mrpt::poses;
using namespace mrpt::bayes;
using namespace mrpt::system;
using namespace mrpt::tfest;
using namespace std;

//  =========== Begin of Map definition ============
MAP_DEFINITION_REGISTER(
	"mrpt::maps::CBeaconMap,beaconMap", mrpt::maps::CBeaconMap)

CBeaconMap::TMapDefinition::TMapDefinition() = default;
void CBeaconMap::TMapDefinition::loadFromConfigFile_map_specific(
	const mrpt::config::CConfigFileBase& source,
	const std::string& sectionNamePrefix)
{
	// [<sectionNamePrefix>+"_creationOpts"]
	// const std::string sSectCreation =
	// sectionNamePrefix+string("_creationOpts");
	// MRPT_LOAD_CONFIG_VAR(resolution, float,   source,sSectCreation);

	insertionOpts.loadFromConfigFile(
		source, sectionNamePrefix + string("_insertOpts"));
	likelihoodOpts.loadFromConfigFile(
		source, sectionNamePrefix + string("_likelihoodOpts"));
}

void CBeaconMap::TMapDefinition::dumpToTextStream_map_specific(
	std::ostream& out) const
{
	// LOADABLEOPTS_DUMP_VAR(resolution     , float);

	this->insertionOpts.dumpToTextStream(out);
	this->likelihoodOpts.dumpToTextStream(out);
}

mrpt::maps::CMetricMap* CBeaconMap::internal_CreateFromMapDefinition(
	const mrpt::maps::TMetricMapInitializer& _def)
{
	const CBeaconMap::TMapDefinition& def =
		*dynamic_cast<const CBeaconMap::TMapDefinition*>(&_def);
	auto* obj = new CBeaconMap();
	obj->insertionOptions = def.insertionOpts;
	obj->likelihoodOptions = def.likelihoodOpts;
	return obj;
}
//  =========== End of Map definition Block =========

IMPLEMENTS_SERIALIZABLE(CBeaconMap, CMetricMap, mrpt::maps)

/*---------------------------------------------------------------
						Constructor
  ---------------------------------------------------------------*/
CBeaconMap::CBeaconMap() : m_beacons(), likelihoodOptions(), insertionOptions()
{
}

/*---------------------------------------------------------------
						clear
  ---------------------------------------------------------------*/
void CBeaconMap::internal_clear() { m_beacons.clear(); }
/*---------------------------------------------------------------
						getLandmarksCount
  ---------------------------------------------------------------*/
size_t CBeaconMap::size() const { return m_beacons.size(); }
/*---------------------------------------------------------------
	Resize
  ---------------------------------------------------------------*/
void CBeaconMap::resize(const size_t N) { m_beacons.resize(N); }
uint8_t CBeaconMap::serializeGetVersion() const { return 1; }
void CBeaconMap::serializeTo(mrpt::serialization::CArchive& out) const
{
	out << genericMapParams;  // v1

	// First, write the number of landmarks:
	const uint32_t n = m_beacons.size();
	out << n;
	// Write all landmarks:
	for (const auto& it : *this)
		out << it;
}

void CBeaconMap::serializeFrom(
	mrpt::serialization::CArchive& in, uint8_t version)
{
	switch (version)
	{
		case 0:
		case 1:
		{
			if (version >= 1) in >> genericMapParams;  // v1

			uint32_t n, i;

			// Delete previous content of map:
			clear();

			// Load from stream:
			// Read all landmarks:
			in >> n;
			m_beacons.resize(n);
			for (i = 0; i < n; i++)
				in >> m_beacons[i];
		}
		break;
		default: MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version);
	};
}

double CBeaconMap::internal_computeObservationLikelihood(
	const CObservation& obs, const CPose3D& robotPose3D) const
{
	MRPT_START

	/* ===============================================================================================================
		Refer to the papers:
		- IROS 2008, "Efficient Probabilistic Range-Only SLAM",
			http://www.mrpt.org/paper-ro-slam-with-sog/

		- ICRA 2008, "A Pure Probabilistic Approach to Range-Only SLAM",
			http://www.mrpt.org/tutorials/slam-algorithms/rangeonly_slam/
	   ===============================================================================================================
	   */

	if (CLASS_ID(CObservationBeaconRanges) == obs.GetRuntimeClass())
	{
		/********************************************************************

						OBSERVATION TYPE: CObservationBeaconRanges

				Lik. between "this" and "auxMap";

			********************************************************************/
		double ret = 0;
		const auto& o = dynamic_cast<const CObservationBeaconRanges&>(obs);
		const CBeacon* beac;
		CPoint3D sensor3D;

		for (auto& it_obs : o.sensedData)
		{
			// Look for the beacon in this map:
			beac = getBeaconByID(it_obs.beaconID);

			if (beac != nullptr && it_obs.sensedDistance > 0 &&
				!std::isnan(it_obs.sensedDistance))
			{
				float sensedRange = it_obs.sensedDistance;

				// FOUND: Compute the likelihood function:
				// -----------------------------------------------------
				// Compute the 3D position of the sensor:
				sensor3D = robotPose3D + it_obs.sensorLocationOnRobot;

				// Depending on the PDF type of the beacon in the map:
				switch (beac->m_typePDF)
				{
					// ------------------------------
					// PDF is MonteCarlo
					// ------------------------------
					case CBeacon::pdfMonteCarlo:
					{
						CPointPDFParticles::CParticleList::const_iterator it;
						CVectorDouble logWeights(
							beac->m_locationMC.m_particles.size());
						CVectorDouble logLiks(
							beac->m_locationMC.m_particles.size());
						CVectorDouble::iterator itLW, itLL;

						for (it = beac->m_locationMC.m_particles.begin(),
							itLW = logWeights.begin(), itLL = logLiks.begin();
							 it != beac->m_locationMC.m_particles.end();
							 ++it, ++itLW, ++itLL)
						{
							float expectedRange = sensor3D.distance3DTo(
								it->d->x, it->d->y, it->d->z);
							// expectedRange +=
							// float(0.1*(1-exp(-0.16*expectedRange)));

							*itLW = it->log_w;	// Linear weight of this
							// likelihood component
							*itLL = -0.5 *
								square(
									(sensedRange - expectedRange) /
									likelihoodOptions.rangeStd);
							// ret+= exp(
							// -0.5*square((sensedRange-expectedRange)/likelihoodOptions.rangeStd)
							// );
						}  // end for it

						if (logWeights.size())
							ret += math::averageLogLikelihood(
								logWeights, logLiks);  // A numerically-stable
						// method to average the
						// likelihoods
					}
					break;
					// ------------------------------
					// PDF is Gaussian
					// ------------------------------
					case CBeacon::pdfGauss:
					{
						// Compute the Jacobian H and varZ
						CMatrixFixed<double, 1, 3> H;
						float varZ, varR = square(likelihoodOptions.rangeStd);
						float Ax =
							beac->m_locationGauss.mean.x() - sensor3D.x();
						float Ay =
							beac->m_locationGauss.mean.y() - sensor3D.y();
						float Az =
							beac->m_locationGauss.mean.z() - sensor3D.z();
						H(0, 0) = Ax;
						H(0, 1) = Ay;
						H(0, 2) = Az;
						float expectedRange =
							sensor3D.distanceTo(beac->m_locationGauss.mean);
						H.asEigen() *=
							1.0 / expectedRange;  // sqrt(Ax*Ax+Ay*Ay+Az*Az);

						varZ = mrpt::math::multiply_HCHt_scalar(
							H, beac->m_locationGauss.cov);

						varZ += varR;

						// Compute the mean expected range (add bias!):
						// expectedRange +=
						// float(0.1*(1-exp(-0.16*expectedRange)));

						// Compute the likelihood:
						//   lik \propto exp( -0.5* ( ^z - z  )^2 / varZ );
						//   log_lik = -0.5* ( ^z - z  )^2 / varZ
						ret +=
							-0.5 * square(sensedRange - expectedRange) / varZ;
					}
					break;
					// ------------------------------
					// PDF is SOG
					// ------------------------------
					case CBeacon::pdfSOG:
					{
						CMatrixDouble13 H;
						CVectorDouble logWeights(beac->m_locationSOG.size());
						CVectorDouble logLiks(beac->m_locationSOG.size());
						CVectorDouble::iterator itLW, itLL;
						CPointPDFSOG::const_iterator it;
						// For each Gaussian mode:
						for (it = beac->m_locationSOG.begin(),
							itLW = logWeights.begin(), itLL = logLiks.begin();
							 it != beac->m_locationSOG.end();
							 ++it, ++itLW, ++itLL)
						{
							// Compute the Jacobian H and varZ
							double varZ,
								varR = square(likelihoodOptions.rangeStd);
							double Ax = it->val.mean.x() - sensor3D.x();
							double Ay = it->val.mean.y() - sensor3D.y();
							double Az = it->val.mean.z() - sensor3D.z();
							H(0, 0) = Ax;
							H(0, 1) = Ay;
							H(0, 2) = Az;
							double expectedRange =
								sensor3D.distanceTo(it->val.mean);
							H.asEigen() *= 1.0 /
								expectedRange;	// sqrt(Ax*Ax+Ay*Ay+Az*Az);

							varZ = mrpt::math::multiply_HCHt_scalar(
								H, it->val.cov);
							varZ += varR;

							// Compute the mean expected range (add bias!):
							// expectedRange +=
							// float(0.1*(1-exp(-0.16*expectedRange)));

							// Compute the likelihood:
							*itLW = it->log_w;	// log-weight of this likelihood
							// component
							*itLL = -0.5 * square(sensedRange - expectedRange) /
								varZ;
						}  // end for each mode

						// Accumulate to the overall (log) likelihood value:
						if (logWeights.size())
							ret += math::averageLogLikelihood(
								logWeights,
								logLiks);  // log( linear_lik / sumW );
					}
					break;

					default: THROW_EXCEPTION("Invalid beac->m_typePDF!!!");
				};
			}
			else
			{
				// If not found, a uniform distribution:
				if (o.maxSensorDistance != o.minSensorDistance)
					ret +=
						log(1.0 / (o.maxSensorDistance - o.minSensorDistance));
			}
		}  // for each sensed beacon "it"

		// printf("ret: %e\n",ret);
		MRPT_CHECK_NORMAL_NUMBER(ret);
		return ret;

	}  // end of likelihood of CObservationBeaconRanges
	else
	{
		/********************************************************************
					OBSERVATION TYPE: Unknown
		********************************************************************/
		return 0;
	}
	MRPT_END
}

/*---------------------------------------------------------------
						insertObservation
  ---------------------------------------------------------------*/
bool CBeaconMap::internal_insertObservation(
	const mrpt::obs::CObservation& obs,
	const std::optional<const mrpt::poses::CPose3D>& robotPose)
{
	MRPT_START

	CPose2D robotPose2D;
	CPose3D robotPose3D;

	if (robotPose)
	{
		robotPose2D = CPose2D(*robotPose);
		robotPose3D = (*robotPose);
	}
	else
	{
		// Default values are (0,0,0)
	}

	if (CLASS_ID(CObservationBeaconRanges) == obs.GetRuntimeClass())
	{
		/********************************************************************
						OBSERVATION TYPE: CObservationBeaconRanges
		 ********************************************************************/

		/* ===============================================================================================================
		   Refer to the papers:
		   - IROS 2008, "Efficient Probabilistic Range-Only SLAM",
			   https://www.mrpt.org/paper-ro-slam-with-sog/

		   - ICRA 2008, "A Pure Probabilistic Approach to Range-Only SLAM",
			   https://www.mrpt.org/tutorials/slam-algorithms/rangeonly_slam/
		  ===============================================================================================================
		  */

		// Here we fuse OR create the beacon position PDF:
		// --------------------------------------------------------
		const auto& o = static_cast<const CObservationBeaconRanges&>(obs);

		for (const auto& it : o.sensedData)
		{
			CPoint3D sensorPnt(robotPose3D + it.sensorLocationOnRobot);
			float sensedRange = it.sensedDistance;
			unsigned int sensedID = it.beaconID;

			CBeacon* beac = getBeaconByID(sensedID);

			if (sensedRange > 0)  // Only sensible range values!
			{
				if (!beac)
				{
					// ======================================
					//                INSERT
					// ======================================
					CBeacon newBeac;
					newBeac.m_ID = sensedID;

					if (insertionOptions.insertAsMonteCarlo)
					{
						// Insert as a new set of samples:
						// ------------------------------------------------

						newBeac.m_typePDF = CBeacon::pdfMonteCarlo;

						size_t numParts = round(
							insertionOptions.MC_numSamplesPerMeter *
							sensedRange);
						ASSERT_(
							insertionOptions.minElevation_deg <=
							insertionOptions.maxElevation_deg);
						double minA =
							DEG2RAD(insertionOptions.minElevation_deg);
						double maxA =
							DEG2RAD(insertionOptions.maxElevation_deg);
						newBeac.m_locationMC.setSize(numParts);
						for (auto& m_particle :
							 newBeac.m_locationMC.m_particles)
						{
							double th =
								getRandomGenerator().drawUniform(-M_PI, M_PI);
							double el =
								getRandomGenerator().drawUniform(minA, maxA);
							double R = getRandomGenerator().drawGaussian1D(
								sensedRange, likelihoodOptions.rangeStd);
							m_particle.d->x =
								sensorPnt.x() + R * cos(th) * cos(el);
							m_particle.d->y =
								sensorPnt.y() + R * sin(th) * cos(el);
							m_particle.d->z = sensorPnt.z() + R * sin(el);
						}  // end for itP
					}
					else
					{
						// Insert as a Sum of Gaussians:
						// ------------------------------------------------
						newBeac.m_typePDF = CBeacon::pdfSOG;
						CBeacon::generateRingSOG(
							sensedRange,  // Sensed range
							newBeac.m_locationSOG,	// Output SOG
							this,  // My CBeaconMap, for options.
							sensorPnt  // Sensor point
						);
					}

					// and insert it:
					m_beacons.push_back(newBeac);

				}  // end insert
				else
				{
					// ======================================
					//					FUSE
					// ======================================
					switch (beac->m_typePDF)
					{
						// ------------------------------
						// FUSE: PDF is MonteCarlo
						// ------------------------------
						case CBeacon::pdfMonteCarlo:
						{
							double maxW = -1e308, sumW = 0;
							// Update weights:
							// --------------------
							for (auto& p : beac->m_locationMC.m_particles)
							{
								float expectedRange = sensorPnt.distance3DTo(
									p.d->x, p.d->y, p.d->z);
								p.log_w += -0.5 *
									square(
										(sensedRange - expectedRange) /
										likelihoodOptions.rangeStd);
								maxW = max(p.log_w, maxW);
								sumW += exp(p.log_w);
							}

							// Perform resampling (SIR filter) or not (simply
							// accumulate weights)??
							// ------------------------------------------------------------------------
							if (insertionOptions.MC_performResampling)
							{
								// Yes, perform an auxiliary PF SIR here:
								// ---------------------------------------------
								if (beac->m_locationMC.ESS() < 0.5)
								{
									// We must resample:
									// Make a list with the log weights:
									vector<double> log_ws;
									vector<size_t> indxs;
									beac->m_locationMC.getWeights(log_ws);

									// And compute the resampled indexes:
									CParticleFilterCapable::computeResampling(
										CParticleFilter::prSystematic, log_ws,
										indxs);

									// Replace with the new samples:
									beac->m_locationMC.performSubstitution(
										indxs);

									// Determine if this is a 2D beacon map:
									bool is2D =
										(insertionOptions.minElevation_deg ==
										 insertionOptions.maxElevation_deg);
									float noiseStd =
										insertionOptions
											.MC_afterResamplingNoise;

									// AND, add a small noise:
									CPointPDFParticles::CParticleList::iterator
										itSample;
									for (itSample = beac->m_locationMC
														.m_particles.begin();
										 itSample !=
										 beac->m_locationMC.m_particles.end();
										 ++itSample)
									{
										itSample->d->x +=
											getRandomGenerator().drawGaussian1D(
												0, noiseStd);
										itSample->d->y +=
											getRandomGenerator().drawGaussian1D(
												0, noiseStd);
										if (!is2D)
											itSample->d->z +=
												getRandomGenerator()
													.drawGaussian1D(
														0, noiseStd);
									}
								}
							}  // end "do resample"
							else
							{
								// Do not resample:
								// ---------------------------------------------

								// Remove very very very unlikely particles:
								// -------------------------------------------
								for (auto it_p =
										 beac->m_locationMC.m_particles.begin();
									 it_p !=
									 beac->m_locationMC.m_particles.end();)
								{
									if (it_p->log_w <
										(maxW -
										 insertionOptions
											 .MC_thresholdNegligible))
									{
										it_p->d.reset();
										it_p = beac->m_locationMC.m_particles
												   .erase(it_p);
									}
									else
										++it_p;
								}
							}  // end "do not resample"

							// Normalize weights:
							//  log_w = log( exp(log_w)/sumW ) ->
							//  log_w -= log(sumW);
							// -----------------------------------------
							sumW = log(sumW);
							for (auto& p : beac->m_locationMC.m_particles)
								p.log_w -= sumW;

							// Is the moment to turn into a Gaussian??
							// -------------------------------------------
							auto [COV, MEAN] =
								beac->m_locationMC.getCovarianceAndMean();

							double D1 = sqrt(COV(0, 0));
							double D2 = sqrt(COV(1, 1));
							double D3 = sqrt(COV(2, 2));

							double mxVar = max3(D1, D2, D3);

							if (mxVar < insertionOptions.MC_maxStdToGauss)
							{
								// Collapse into Gaussian:
								beac->m_locationMC
									.clear();  // Erase prev. samples

								// Assure a non-null covariance!
								CMatrixDouble COV2 = CMatrixDouble(COV);
								COV2.setSize(2, 2);
								if (COV2.det() == 0)
								{
									COV.setIdentity();
									COV *= square(0.01f);
									if (insertionOptions.minElevation_deg ==
										insertionOptions.maxElevation_deg)
										COV(2, 2) = 0;	// We are in a 2D map:
								}

								beac->m_typePDF =
									CBeacon::pdfGauss;	// Pass to gaussian.
								beac->m_locationGauss.mean = MEAN;
								beac->m_locationGauss.cov = COV;
							}
						}
						break;

						// ------------------------------
						// FUSE: PDF is Gaussian:
						// ------------------------------
						case CBeacon::pdfGauss:
						{
							// Compute the mean expected range:
							float expectedRange = sensorPnt.distanceTo(
								beac->m_locationGauss.mean);
							float varR = square(likelihoodOptions.rangeStd);
							// bool	useEKF_or_KF = true;

							// if (useEKF_or_KF)
							{
								// EKF method:
								// ---------------------
								// Add bias:
								// expectedRange +=
								// float(0.1*(1-exp(-0.16*expectedRange)));

								// An EKF for updating the Gaussian:
								float y = sensedRange - expectedRange;

								// Compute the Jacobian H and varZ
								CMatrixDouble13 H;
								double varZ;
								double Ax =
									(beac->m_locationGauss.mean.x() -
									 sensorPnt.x());
								double Ay =
									(beac->m_locationGauss.mean.y() -
									 sensorPnt.y());
								double Az =
									(beac->m_locationGauss.mean.z() -
									 sensorPnt.z());
								H(0, 0) = Ax;
								H(0, 1) = Ay;
								H(0, 2) = Az;
								H.asEigen() *= 1.0 /
									expectedRange;	// sqrt(Ax*Ax+Ay*Ay+Az*Az);
								varZ = mrpt::math::multiply_HCHt_scalar(
									H, beac->m_locationGauss.cov);
								varZ += varR;

								Eigen::Vector3d K =
									beac->m_locationGauss.cov.asEigen() *
									H.transpose();
								K *= 1.0 / varZ;

								// Update stage of the EKF:
								beac->m_locationGauss.mean.x_incr(K(0, 0) * y);
								beac->m_locationGauss.mean.y_incr(K(1, 0) * y);
								beac->m_locationGauss.mean.z_incr(K(2, 0) * y);

								beac->m_locationGauss.cov =
									((Eigen::Matrix<double, 3, 3>::Identity() -
									  K * H.asEigen()) *
									 beac->m_locationGauss.cov.asEigen())
										.eval();
							}
						}
						break;
						// ------------------------------
						// FUSE: PDF is SOG
						// ------------------------------
						case CBeacon::pdfSOG:
						{
							// Compute the mean expected range for this mode:
							float varR = square(likelihoodOptions.rangeStd);

							// For each Gaussian mode:
							//  1) Update its weight (using the likelihood of
							//  the observation linearized at the mean)
							//  2) Update its mean/cov (as in the simple EKF)
							double max_w = -1e9;
							for (auto& mode : beac->m_locationSOG)
							{
								double expectedRange =
									sensorPnt.distanceTo(mode.val.mean);

								// An EKF for updating the Gaussian:
								double y = sensedRange - expectedRange;

								// Compute the Jacobian H and varZ
								CMatrixDouble13 H;
								double varZ;
								double Ax = (mode.val.mean.x() - sensorPnt.x());
								double Ay = (mode.val.mean.y() - sensorPnt.y());
								double Az = (mode.val.mean.z() - sensorPnt.z());
								H(0, 0) = Ax;
								H(0, 1) = Ay;
								H(0, 2) = Az;
								H.asEigen() *= 1.0 / expectedRange;
								varZ = mrpt::math::multiply_HCHt_scalar(
									H, mode.val.cov);
								varZ += varR;
								Eigen::Vector3d K =
									mode.val.cov.asEigen() * H.transpose();
								K *= 1.0 / varZ;

								// Update stage of the EKF:
								mode.val.mean.x_incr(K(0, 0) * y);
								mode.val.mean.y_incr(K(1, 0) * y);
								mode.val.mean.z_incr(K(2, 0) * y);

								mode.val.cov = (Eigen::Matrix3d::Identity() -
												K * H.asEigen()) *
									mode.val.cov.asEigen();

								// Update the weight of this mode:
								// ----------------------------------
								mode.log_w += -0.5 * square(y) / varZ;

								// keep the maximum mode weight
								max_w = max(max_w, mode.log_w);
							}  // end for each mode

							// Remove modes with negligible weights:
							// -----------------------------------------------------------
							for (auto it_p = beac->m_locationSOG.begin();
								 it_p != beac->m_locationSOG.end();)
							{
								if (max_w - it_p->log_w >
									insertionOptions.SOG_thresholdNegligible)
									it_p = beac->m_locationSOG.erase(it_p);
								else
									++it_p;
							}

							// Normalize the weights:
							beac->m_locationSOG.normalizeWeights();

							// Should we pass this beacon to a single Gaussian
							// mode?
							// -----------------------------------------------------------
							const auto [curCov, curMean] =
								beac->m_locationSOG.getCovarianceAndMean();

							double D1 = sqrt(curCov(0, 0));
							double D2 = sqrt(curCov(1, 1));
							double D3 = sqrt(curCov(2, 2));
							float maxDiag = max3(D1, D2, D3);

							if (maxDiag < 0.10f)
							{
								// Yes, transform:
								beac->m_locationGauss.mean = curMean;
								beac->m_locationGauss.cov = curCov;
								beac->m_typePDF = CBeacon::pdfGauss;
								// Done!
							}
						}
						break;
						default: THROW_EXCEPTION("Invalid beac->m_typePDF!!!");
					};

				}  // end fuse
			}  // end if range makes sense
		}  // end for each observation

		// DONE!!
		// Observation was successfully inserted into the map
		return true;
	}
	else
	{
		return false;
	}

	MRPT_END
}

/*---------------------------------------------------------------
				determineMatching2D
  ---------------------------------------------------------------*/
void CBeaconMap::determineMatching2D(
	const mrpt::maps::CMetricMap* otherMap, const CPose2D& otherMapPose,
	TMatchingPairList& correspondences,
	[[maybe_unused]] const TMatchingParams& params,
	TMatchingExtraResults& extraResults) const
{
	MRPT_START
	extraResults = TMatchingExtraResults();

	CBeaconMap auxMap;
	CPose3D otherMapPose3D(otherMapPose);

	// Check the other map class:
	ASSERT_(otherMap->GetRuntimeClass() == CLASS_ID(CBeaconMap));
	const auto* otherMap2 = dynamic_cast<const CBeaconMap*>(otherMap);
	vector<bool> otherCorrespondences;

	// Coordinates change:
	auxMap.changeCoordinatesReference(otherMapPose3D, otherMap2);

	// Use the 3D matching method:
	computeMatchingWith3DLandmarks(
		otherMap2, correspondences, extraResults.correspondencesRatio,
		otherCorrespondences);

	MRPT_END
}

/*---------------------------------------------------------------
				changeCoordinatesReference
  ---------------------------------------------------------------*/
void CBeaconMap::changeCoordinatesReference(const CPose3D& newOrg)
{
	// Change the reference of each individual beacon:
	for (auto& m_beacon : m_beacons)
		m_beacon.changeCoordinatesReference(newOrg);
}

/*---------------------------------------------------------------
				changeCoordinatesReference
  ---------------------------------------------------------------*/
void CBeaconMap::changeCoordinatesReference(
	const CPose3D& newOrg, const mrpt::maps::CBeaconMap* otherMap)
{
	// In this object we cannot apply any special speed-up: Just copy and change
	// coordinates:
	(*this) = *otherMap;
	changeCoordinatesReference(newOrg);
}

/*---------------------------------------------------------------
						computeMatchingWith3DLandmarks
  ---------------------------------------------------------------*/
void CBeaconMap::computeMatchingWith3DLandmarks(
	const mrpt::maps::CBeaconMap* anotherMap,
	TMatchingPairList& correspondences, float& correspondencesRatio,
	vector<bool>& otherCorrespondences) const
{
	MRPT_START

	TSequenceBeacons::const_iterator thisIt, otherIt;
	size_t nThis, nOther;
	unsigned int j, k;
	TMatchingPair match;
	CPointPDFGaussian pointPDF_k, pointPDF_j;
	vector<bool> thisLandmarkAssigned;

	// Get the number of landmarks:
	nThis = m_beacons.size();
	nOther = anotherMap->m_beacons.size();

	// Initially no LM has a correspondence:
	thisLandmarkAssigned.resize(nThis, false);

	// Initially, set all landmarks without correspondences:
	correspondences.clear();
	otherCorrespondences.clear();
	otherCorrespondences.resize(nOther, false);
	correspondencesRatio = 0;

	for (k = 0, otherIt = anotherMap->m_beacons.begin();
		 otherIt != anotherMap->m_beacons.end(); ++otherIt, ++k)
	{
		for (j = 0, thisIt = m_beacons.begin(); thisIt != m_beacons.end();
			 ++thisIt, ++j)
		{
			// Is it a correspondence?
			if ((otherIt)->m_ID == (thisIt)->m_ID)
			{
				// If a previous correspondence for this LM was found, discard
				// this one!
				if (!thisLandmarkAssigned[j])
				{
					thisLandmarkAssigned[j] = true;

					// OK: A correspondence found!!
					otherCorrespondences[k] = true;

					match.globalIdx = j;

					CPoint3D mean_j = m_beacons[j].getMeanVal();

					match.global.x = mean_j.x();
					match.global.y = mean_j.y();
					match.global.z = mean_j.z();

					CPoint3D mean_k = anotherMap->m_beacons[k].getMeanVal();
					match.localIdx = k;
					match.local.x = mean_k.x();
					match.local.y = mean_k.y();
					match.local.z = mean_k.z();

					correspondences.push_back(match);
				}
			}

		}  // end of "otherIt" is SIFT

	}  // end of other it., k

	// Compute the corrs ratio:
	correspondencesRatio = 2.0f * correspondences.size() / d2f(nThis + nOther);

	MRPT_END
}

/*---------------------------------------------------------------
						saveToMATLABScript3D
  ---------------------------------------------------------------*/
bool CBeaconMap::saveToMATLABScript3D(
	const string& file, [[maybe_unused]] const char* style,
	[[maybe_unused]] float confInterval) const
{
	FILE* f = os::fopen(file.c_str(), "wt");
	if (!f) return false;

	// Header:
	os::fprintf(
		f, "%%-------------------------------------------------------\n");
	os::fprintf(f, "%% File automatically generated using the MRPT method:\n");
	os::fprintf(f, "%%   'CBeaconMap::saveToMATLABScript3D'\n");
	os::fprintf(f, "%%\n");
	os::fprintf(f, "%%                        ~ MRPT ~\n");
	os::fprintf(
		f, "%%  Jose Luis Blanco Claraco, University of Malaga @ 2006\n");
	os::fprintf(f, "%%  http://www.isa.uma.es/ \n");
	os::fprintf(
		f, "%%-------------------------------------------------------\n\n");

	// Main code:
	os::fprintf(f, "hold on;\n\n");
	std::vector<std::string> strs;
	string s;

	for (const auto& m_beacon : m_beacons)
	{
		m_beacon.getAsMatlabDrawCommands(strs);
		mrpt::system::stringListAsString(strs, s);
		os::fprintf(f, "%s", s.c_str());
	}

	os::fprintf(f, "axis equal;grid on;");

	os::fclose(f);
	return true;
}

void CBeaconMap::TLikelihoodOptions::dumpToTextStream(std::ostream& out) const
{
	out << "\n----------- [CBeaconMap::TLikelihoodOptions] ------------ \n\n";
	out << mrpt::format(
		"rangeStd                                = %f\n", rangeStd);
	out << "\n";
}

/*---------------------------------------------------------------
					loadFromConfigFile
  ---------------------------------------------------------------*/
void CBeaconMap::TLikelihoodOptions::loadFromConfigFile(
	const mrpt::config::CConfigFileBase& iniFile, const string& section)
{
	rangeStd = iniFile.read_float(section.c_str(), "rangeStd", rangeStd);
}

void CBeaconMap::TInsertionOptions::dumpToTextStream(std::ostream& out) const
{
	out << "\n----------- [CBeaconMap::TInsertionOptions] ------------ \n\n";

	out << mrpt::format(
		"insertAsMonteCarlo                      = %c\n",
		insertAsMonteCarlo ? 'Y' : 'N');
	out << mrpt::format(
		"minElevation_deg                        = %.03f\n", minElevation_deg);
	out << mrpt::format(
		"maxElevation_deg                        = %.03f\n", maxElevation_deg);
	out << mrpt::format(
		"MC_numSamplesPerMeter                   = %d\n",
		MC_numSamplesPerMeter);
	out << mrpt::format(
		"MC_maxStdToGauss                        = %.03f\n", MC_maxStdToGauss);
	out << mrpt::format(
		"MC_thresholdNegligible                  = %.03f\n",
		MC_thresholdNegligible);
	out << mrpt::format(
		"MC_performResampling                    = %c\n",
		MC_performResampling ? 'Y' : 'N');
	out << mrpt::format(
		"MC_afterResamplingNoise                 = %.03f\n",
		MC_afterResamplingNoise);
	out << mrpt::format(
		"SOG_thresholdNegligible                 = %.03f\n",
		SOG_thresholdNegligible);
	out << mrpt::format(
		"SOG_maxDistBetweenGaussians             = %.03f\n",
		SOG_maxDistBetweenGaussians);
	out << mrpt::format(
		"SOG_separationConstant                  = %.03f\n",
		SOG_separationConstant);

	out << "\n";
}

/*---------------------------------------------------------------
					loadFromConfigFile
  ---------------------------------------------------------------*/
void CBeaconMap::TInsertionOptions::loadFromConfigFile(
	const mrpt::config::CConfigFileBase& iniFile, const string& section)
{
	MRPT_LOAD_CONFIG_VAR(insertAsMonteCarlo, bool, iniFile, section.c_str());
	MRPT_LOAD_CONFIG_VAR(maxElevation_deg, double, iniFile, section.c_str());
	MRPT_LOAD_CONFIG_VAR(minElevation_deg, double, iniFile, section.c_str());
	MRPT_LOAD_CONFIG_VAR(MC_numSamplesPerMeter, int, iniFile, section.c_str());
	MRPT_LOAD_CONFIG_VAR(MC_maxStdToGauss, float, iniFile, section.c_str());
	MRPT_LOAD_CONFIG_VAR(
		MC_thresholdNegligible, double, iniFile, section.c_str());
	MRPT_LOAD_CONFIG_VAR(MC_performResampling, bool, iniFile, section.c_str());
	MRPT_LOAD_CONFIG_VAR(
		MC_afterResamplingNoise, float, iniFile, section.c_str());
	MRPT_LOAD_CONFIG_VAR(
		SOG_thresholdNegligible, float, iniFile, section.c_str());
	MRPT_LOAD_CONFIG_VAR(
		SOG_maxDistBetweenGaussians, float, iniFile, section.c_str());
	MRPT_LOAD_CONFIG_VAR(
		SOG_separationConstant, float, iniFile, section.c_str());
}

/*---------------------------------------------------------------
					 isEmpty
  ---------------------------------------------------------------*/
bool CBeaconMap::isEmpty() const { return size() == 0; }
/*---------------------------------------------------------------
					 simulateBeaconReadings
  ---------------------------------------------------------------*/
void CBeaconMap::simulateBeaconReadings(
	const CPose3D& in_robotPose, const CPoint3D& in_sensorLocationOnRobot,
	CObservationBeaconRanges& out_Observations) const
{
	TSequenceBeacons::const_iterator it;
	mrpt::obs::CObservationBeaconRanges::TMeasurement newMeas;
	CPoint3D point3D, beacon3D;
	CPointPDFGaussian beaconPDF;

	// Compute the 3D position of the sensor:
	point3D = in_robotPose + in_sensorLocationOnRobot;

	// Clear output data:
	out_Observations.sensedData.clear();

	// For each BEACON landmark in the map:
	for (it = m_beacons.begin(); it != m_beacons.end(); ++it)
	{
		it->getMean(beacon3D);

		float range = point3D.distanceTo(beacon3D);

		if (range < out_Observations.maxSensorDistance &&
			range > out_Observations.minSensorDistance)
		{
			// Add noise:
			range += getRandomGenerator().drawGaussian1D(
				0, out_Observations.stdError);

			// Fill out:
			newMeas.beaconID = it->m_ID;
			newMeas.sensorLocationOnRobot = in_sensorLocationOnRobot;
			newMeas.sensedDistance = range;

			// Insert:
			out_Observations.sensedData.push_back(newMeas);
		}
	}  // end for it
	// Done!
}

/*---------------------------------------------------------------
					 saveMetricMapRepresentationToFile
  ---------------------------------------------------------------*/
void CBeaconMap::saveMetricMapRepresentationToFile(
	const string& filNamePrefix) const
{
	MRPT_START

	// Matlab:
	string fil1(filNamePrefix + string("_3D.m"));
	saveToMATLABScript3D(fil1);

	// 3D Scene:
	opengl::COpenGLScene scene;
	const opengl::CSetOfObjects::Ptr obj3D = getVisualization();
	auto objGround = opengl::CGridPlaneXY::Create(
		-100.0f, 100.0f, -100.0f, 100.0f, .0f, 1.f);

	scene.insert(obj3D);
	scene.insert(objGround);

	string fil2(filNamePrefix + string("_3D.3Dscene"));
	scene.saveToFile(fil2);

	// Textual representation:
	string fil3(filNamePrefix + string("_covs.txt"));
	saveToTextFile(fil3);

	// Total number of particles / modes:
	string fil4(filNamePrefix + string("_population.txt"));
	{
		FILE* f = os::fopen(fil4.c_str(), "wt");
		if (f)
		{
			size_t nParts = 0, nGaussians = 0;

			for (const auto& m_beacon : m_beacons)
			{
				switch (m_beacon.m_typePDF)
				{
					case CBeacon::pdfMonteCarlo:
						nParts += m_beacon.m_locationMC.size();
						break;
					case CBeacon::pdfSOG:
						nGaussians += m_beacon.m_locationSOG.size();
						break;
					case CBeacon::pdfGauss: nGaussians++; break;
				};
			}

			fprintf(
				f, "%u %u", static_cast<unsigned>(nParts),
				static_cast<unsigned>(nGaussians));
			os::fclose(f);
		}
	}

	MRPT_END
}

void CBeaconMap::getVisualizationInto(mrpt::opengl::CSetOfObjects& o) const
{
	MRPT_START

	if (!genericMapParams.enableSaveAs3DObject) return;

	// ------------------------------------------------
	//  Add the XYZ corner for the current area:
	// ------------------------------------------------
	o.insert(opengl::stock_objects::CornerXYZ());

	// Save 3D ellipsoids or whatever representation:
	for (const auto& m_beacon : m_beacons)
		m_beacon.getVisualizationInto(o);

	MRPT_END
}

/*---------------------------------------------------------------
   Computes the ratio in [0,1] of correspondences between "this" and the
 "otherMap" map, whose 6D pose relative to "this" is "otherMapPose"
 *   In the case of a multi-metric map, this returns the average between the
 maps. This method always return 0 for grid maps.
 * \param  otherMap					  [IN] The other map to compute the matching
 with.
 * \param  otherMapPose				  [IN] The 6D pose of the other map as seen
 from "this".
 * \param  maxDistForCorr			  [IN] The minimum distance between 2
 non-probabilistic map elements for counting them as a correspondence.
 * \param  maxMahaDistForCorr		  [IN] The minimum Mahalanobis distance
 between 2 probabilistic map elements for counting them as a correspondence.
 *
 * \return The matching ratio [0,1]
 * \sa computeMatchingWith2D
 ----------------------------------------------------------------*/
float CBeaconMap::compute3DMatchingRatio(
	const mrpt::maps::CMetricMap* otherMap2,
	const mrpt::poses::CPose3D& otherMapPose,
	const TMatchingRatioParams& params) const
{
	MRPT_START

	// Compare to a similar map only:
	const CBeaconMap* otherMap = nullptr;

	if (otherMap2->GetRuntimeClass() == CLASS_ID(CBeaconMap))
		otherMap = dynamic_cast<const CBeaconMap*>(otherMap2);

	if (!otherMap) return 0;

	TMatchingPairList matchList;
	vector<bool> otherCorrespondences;
	float out_corrsRatio;

	CBeaconMap modMap;

	modMap.changeCoordinatesReference(otherMapPose, otherMap);

	computeMatchingWith3DLandmarks(
		&modMap, matchList, out_corrsRatio, otherCorrespondences);

	return out_corrsRatio;

	MRPT_END
}

/*---------------------------------------------------------------
					getBeaconByID
 ---------------------------------------------------------------*/
const CBeacon* CBeaconMap::getBeaconByID(CBeacon::TBeaconID id) const
{
	for (const auto& m_beacon : m_beacons)
		if (m_beacon.m_ID == id) return &m_beacon;
	return nullptr;
}

/*---------------------------------------------------------------
					getBeaconByID
 ---------------------------------------------------------------*/
CBeacon* CBeaconMap::getBeaconByID(CBeacon::TBeaconID id)
{
	for (auto& m_beacon : m_beacons)
		if (m_beacon.m_ID == id) return &m_beacon;
	return nullptr;
}

/*---------------------------------------------------------------
					saveToTextFile
- VX VY VZ: Variances of each dimension (C11, C22, C33)
- DET2D DET3D: Determinant of the 2D and 3D covariance matrixes.
- C12, C13, C23: Cross covariances
 ---------------------------------------------------------------*/
void CBeaconMap::saveToTextFile(const string& fil) const
{
	MRPT_START
	FILE* f = os::fopen(fil.c_str(), "wt");
	ASSERT_(f != nullptr);

	for (const auto& m_beacon : m_beacons)
	{
		const auto [C, p] = m_beacon.getCovarianceAndMean();

		float D3 = C.det();
		float D2 = C(0, 0) * C(1, 1) - square(C(0, 1));
		os::fprintf(
			f, "%i %f %f %f %e %e %e %e %e %e %e %e\n",
			static_cast<int>(m_beacon.m_ID), p.x(), p.y(), p.z(), C(0, 0),
			C(1, 1), C(2, 2), D2, D3, C(0, 1), C(1, 2), C(1, 2));
	}

	os::fclose(f);
	MRPT_END
}
