/* +------------------------------------------------------------------------+
   |                     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/io/CFileStream.h>
#include <mrpt/maps/CBeaconMap.h>
#include <mrpt/maps/CLandmarksMap.h>
#include <mrpt/maps/CMultiMetricMapPDF.h>
#include <mrpt/maps/COccupancyGridMap2D.h>
#include <mrpt/maps/CSimplePointsMap.h>
#include <mrpt/math/utils.h>
#include <mrpt/obs/CActionCollection.h>
#include <mrpt/obs/CActionRobotMovement2D.h>
#include <mrpt/obs/CActionRobotMovement3D.h>
#include <mrpt/obs/CObservationBeaconRanges.h>
#include <mrpt/poses/CPosePDFGaussian.h>
#include <mrpt/poses/CPosePDFGrid.h>
#include <mrpt/random.h>
#include <mrpt/slam/PF_aux_structs.h>
#include <mrpt/system/CTicTac.h>

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

namespace mrpt::slam
{
/** Fills out a "TPoseBin2D" variable, given a path hypotesis and (if not set to
 * nullptr) a new pose appended at the end, using the KLD params in "options".
 */
template <>
void KLF_loadBinFromParticle(
	detail::TPoseBin2D& outBin, const TKLDParams& opts,
	const mrpt::maps::CRBPFParticleData* currentParticleValue,
	const TPose3D* newPoseToBeInserted)
{
	// 2D pose approx: Use the latest pose only:
	if (newPoseToBeInserted)
	{
		outBin.x = round(newPoseToBeInserted->x / opts.KLD_binSize_XY);
		outBin.y = round(newPoseToBeInserted->y / opts.KLD_binSize_XY);
		outBin.phi = round(newPoseToBeInserted->yaw / opts.KLD_binSize_PHI);
	}
	else
	{
		ASSERT_(
			currentParticleValue && !currentParticleValue->robotPath.empty());
		const TPose3D& p = *currentParticleValue->robotPath.rbegin();
		outBin.x = round(p.x / opts.KLD_binSize_XY);
		outBin.y = round(p.y / opts.KLD_binSize_XY);
		outBin.phi = round(p.yaw / opts.KLD_binSize_PHI);
	}
}

/** Fills out a "TPathBin2D" variable, given a path hypotesis and (if not set to
 * nullptr) a new pose appended at the end, using the KLD params in "options".
 */
template <>
void KLF_loadBinFromParticle(
	detail::TPathBin2D& outBin, const TKLDParams& opts,
	const mrpt::maps::CRBPFParticleData* currentParticleValue,
	const TPose3D* newPoseToBeInserted)
{
	const size_t lenBinPath = (currentParticleValue != nullptr)
		? currentParticleValue->robotPath.size()
		: 0;

	// Set the output bin dimensionality:
	outBin.bins.resize(lenBinPath + (newPoseToBeInserted != nullptr ? 1 : 0));

	// Is a path provided??
	if (currentParticleValue != nullptr)
		for (size_t i = 0; i < lenBinPath; ++i)	 // Fill the bin data:
		{
			outBin.bins[i].x = round(
				currentParticleValue->robotPath[i].x / opts.KLD_binSize_XY);
			outBin.bins[i].y = round(
				currentParticleValue->robotPath[i].y / opts.KLD_binSize_XY);
			outBin.bins[i].phi = round(
				currentParticleValue->robotPath[i].yaw / opts.KLD_binSize_PHI);
		}

	// Is a newPose provided??
	if (newPoseToBeInserted != nullptr)
	{
		// And append the last pose: the new one:
		outBin.bins[lenBinPath].x =
			round(newPoseToBeInserted->x / opts.KLD_binSize_XY);
		outBin.bins[lenBinPath].y =
			round(newPoseToBeInserted->y / opts.KLD_binSize_XY);
		outBin.bins[lenBinPath].phi =
			round(newPoseToBeInserted->yaw / opts.KLD_binSize_PHI);
	}
}
}  // namespace mrpt::slam

// Include this AFTER specializations:
#include <mrpt/slam/PF_implementations.h>

/** Auxiliary for optimal sampling in RO-SLAM */
struct TAuxRangeMeasInfo
{
	TAuxRangeMeasInfo()
		: sensorLocationOnRobot(),

		  beaconID(INVALID_BEACON_ID)

	{
	}

	CPoint3D sensorLocationOnRobot;
	float sensedDistance{0};
	int64_t beaconID;
	size_t nGaussiansInMap{
		0};	 // Number of Gaussian modes in the map representation

	/** Auxiliary for optimal sampling in RO-SLAM */
	static bool cmp_Asc(const TAuxRangeMeasInfo& a, const TAuxRangeMeasInfo& b)
	{
		return a.nGaussiansInMap < b.nGaussiansInMap;
	}
};

/*----------------------------------------------------------------------------------
			prediction_and_update_pfAuxiliaryPFOptimal

  See paper reference in "PF_SLAM_implementation_pfAuxiliaryPFOptimal"
 ----------------------------------------------------------------------------------*/
void CMultiMetricMapPDF::prediction_and_update_pfAuxiliaryPFOptimal(
	const mrpt::obs::CActionCollection* actions,
	const mrpt::obs::CSensoryFrame* sf,
	const bayes::CParticleFilter::TParticleFilterOptions& PF_options)
{
	MRPT_START

	PF_SLAM_implementation_pfAuxiliaryPFOptimal<mrpt::slam::detail::TPoseBin2D>(
		actions, sf, PF_options, options.KLD_params);

	MRPT_END
}

/*----------------------------------------------------------------------------------
			PF_SLAM_implementation_pfAuxiliaryPFStandard
 ----------------------------------------------------------------------------------*/
void CMultiMetricMapPDF::prediction_and_update_pfAuxiliaryPFStandard(
	const mrpt::obs::CActionCollection* actions,
	const mrpt::obs::CSensoryFrame* sf,
	const bayes::CParticleFilter::TParticleFilterOptions& PF_options)
{
	MRPT_START

	PF_SLAM_implementation_pfAuxiliaryPFStandard<
		mrpt::slam::detail::TPoseBin2D>(
		actions, sf, PF_options, options.KLD_params);

	MRPT_END
}

/*----------------------------------------------------------------------------------
			prediction_and_update_pfOptimalProposal

For grid-maps:
==============
 Approximation by Grissetti et al:  Use scan matching to approximate
   the observation model by a Gaussian:
  See: "Improved Grid-based SLAM with Rao-Blackwellized PF by Adaptive Proposals
		   and Selective Resampling" (G. Grisetti, C. Stachniss, W. Burgard)

For beacon maps:
===============
  (JLBC: Method under development)

 ----------------------------------------------------------------------------------*/
void CMultiMetricMapPDF::prediction_and_update_pfOptimalProposal(
	const mrpt::obs::CActionCollection* actions,
	const mrpt::obs::CSensoryFrame* sf,
	const bayes::CParticleFilter::TParticleFilterOptions& PF_options)
{
	MRPT_START

	// ----------------------------------------------------------------------
	//						PREDICTION STAGE
	// ----------------------------------------------------------------------
	CVectorDouble rndSamples;
	bool updateStageAlreadyDone = false;
	CPose3D initialPose, incrPose, finalPose;

	// ICP used if "pfOptimalProposal_mapSelection" = 0 or 1
	CICP icp(
		options.icp_params);  // Set our ICP params instead of default ones.
	CICP::TReturnInfo icpInfo;

	CParticleList::iterator partIt;

	ASSERT_(sf != nullptr);
	// Find a robot movement estimation:
	CPose3D motionModelMeanIncr;  // The mean motion increment:
	CPoseRandomSampler robotActionSampler;
	{
		CActionRobotMovement2D::Ptr robotMovement2D =
			actions->getBestMovementEstimation();

		// If there is no 2D action, look for a 3D action:
		if (robotMovement2D)
		{
			robotActionSampler.setPosePDF(*robotMovement2D->poseChange);
			motionModelMeanIncr =
				mrpt::poses::CPose3D(robotMovement2D->poseChange->getMeanVal());
		}
		else
		{
			CActionRobotMovement3D::Ptr robotMovement3D =
				actions->getActionByClass<CActionRobotMovement3D>();
			if (robotMovement3D)
			{
				robotActionSampler.setPosePDF(robotMovement3D->poseChange);
				robotMovement3D->poseChange.getMean(motionModelMeanIncr);
			}
			else
			{
				motionModelMeanIncr.setFromValues(0, 0, 0);
			}
		}
	}

	// Average map will need to be updated after this:
	averageMapIsUpdated = false;

	// --------------------------------------------------------------------------------------
	//  Prediction:
	//
	//  Compute a new mean and covariance by sampling around the mean of the
	//  input "action"
	// --------------------------------------------------------------------------------------
	MRPT_LOG_DEBUG("Stage 1) Prediction start.");
	const size_t M = m_particles.size();

	// To be computed as an average from all m_particles:
	size_t particleWithHighestW = 0;
	for (size_t i = 0; i < M; i++)
		if (getW(i) > getW(particleWithHighestW)) particleWithHighestW = i;

	//   The paths MUST already contain the starting location for each particle:
	ASSERT_(!m_particles[0].d->robotPath.empty());
	// Build the local map of points for ICP:
	CSimplePointsMap localMapPoints;
	CLandmarksMap localMapLandmarks;
	bool built_map_points = false;
	bool built_map_lms = false;

	// Update particle poses:
	size_t i;
	for (i = 0, partIt = m_particles.begin(); partIt != m_particles.end();
		 partIt++, i++)
	{
		double extra_log_lik = 0;  // Used for the optimal_PF with ICP

		// Set initial robot pose estimation for this particle:
		const CPose3D ith_last_pose = CPose3D(
			*partIt->d->robotPath.rbegin());  // The last robot pose in the path

		CPose3D initialPoseEstimation = ith_last_pose + motionModelMeanIncr;

		// Use ICP with the map associated to particle?
		if (options.pfOptimalProposal_mapSelection == 0 ||
			options.pfOptimalProposal_mapSelection == 1 ||
			options.pfOptimalProposal_mapSelection == 3)
		{
			CPosePDFGaussian icpEstimation;

			// Configure the matchings that will take place in the ICP process:
			auto partMap = partIt->d->mapTillNow;
			const auto numPtMaps = partMap.countMapsByClass<CSimplePointsMap>();

			ASSERT_(numPtMaps == 0 || numPtMaps == 1);

			CMetricMap* map_to_align_to = nullptr;

			if (options.pfOptimalProposal_mapSelection == 0)  // Grid map
			{
				auto grid = partMap.mapByClass<COccupancyGridMap2D>();
				ASSERT_(grid);

				// Build local map of points.
				if (!built_map_points)
				{
					built_map_points = true;

					localMapPoints.insertionOptions.minDistBetweenLaserPoints =
						0.02f;
					localMapPoints.insertionOptions.isPlanarMap = true;
					sf->insertObservationsInto(localMapPoints);
				}

				map_to_align_to = grid.get();
			}
			// Map of points
			else if (options.pfOptimalProposal_mapSelection == 3)
			{
				auto ptsMap = partMap.mapByClass<CSimplePointsMap>();
				ASSERT_(ptsMap);

				// Build local map of points.
				if (!built_map_points)
				{
					built_map_points = true;

					localMapPoints.insertionOptions.minDistBetweenLaserPoints =
						0.02f;
					localMapPoints.insertionOptions.isPlanarMap = true;
					sf->insertObservationsInto(localMapPoints);
				}

				map_to_align_to = ptsMap.get();
			}
			else
			{
				auto lmMap = partMap.mapByClass<CLandmarksMap>();
				ASSERT_(lmMap);

				// Build local map of LMs.
				if (!built_map_lms)
				{
					built_map_lms = true;
					sf->insertObservationsInto(localMapLandmarks);
				}

				map_to_align_to = lmMap.get();
			}

			ASSERT_(map_to_align_to != nullptr);

			// Use ICP to align to each particle's map:
			{
				CPosePDF::Ptr alignEst = icp.Align(
					map_to_align_to, &localMapPoints,
					CPose2D(initialPoseEstimation), icpInfo);
				icpEstimation.copyFrom(*alignEst);
			}

			if (i == particleWithHighestW)
			{
				newInfoIndex = 1 - icpInfo.goodness;  // newStaticPointsRatio;
				// //* icpInfo.goodness;
			}

			// Set the gaussian pose:
			CPose3DPDFGaussian finalEstimatedPoseGauss(icpEstimation);

			MRPT_LOG_DEBUG_FMT(
				"gridICP[particle %u]: %.02f%%", static_cast<unsigned int>(i),
				100 * icpInfo.goodness);
			if (icpInfo.goodness < options.ICPGlobalAlign_MinQuality &&
				SFs.size())
			{
				MRPT_LOG_WARN_FMT(
					"gridICP[particle %u]: %.02f%% -> Using odometry instead!",
					(unsigned int)i, 100 * icpInfo.goodness);
				icpEstimation.mean = CPose2D(initialPoseEstimation);
			}

			// Use real ICP covariance (with a minimum level):
			keep_max(finalEstimatedPoseGauss.cov(0, 0), square(0.002));
			keep_max(finalEstimatedPoseGauss.cov(1, 1), square(0.002));
			keep_max(finalEstimatedPoseGauss.cov(2, 2), square(0.1_deg));

			// Generate gaussian-distributed 2D-pose increments according to
			// "finalEstimatedPoseGauss":
			// ------------------------------------------------------------
			finalPose =
				finalEstimatedPoseGauss.mean;  // Add to the new robot pose:
			getRandomGenerator().drawGaussianMultivariate(
				rndSamples, finalEstimatedPoseGauss.cov);
			// Add noise:
			finalPose.setFromValues(
				finalPose.x() + rndSamples[0], finalPose.y() + rndSamples[1],
				finalPose.z(), finalPose.yaw() + rndSamples[2],
				finalPose.pitch(), finalPose.roll());
		}
		else if (options.pfOptimalProposal_mapSelection == 2)
		{
			//     Perform optimal sampling with the beacon map:
			//  Described in paper: IROS 2008
			// --------------------------------------------------------
			auto beacMap = partIt->d->mapTillNow.mapByClass<CBeaconMap>();

			// We'll also update the weight of the particle here
			updateStageAlreadyDone = true;

			// =================================================================
			// SUMMARY:
			// For each beacon measurement in the SF, stored in
			// "lstObservedRanges",
			//  compute the SOG of the observation model, and multiply all of
			//  them (fuse) in "fusedObsModels". The result will hopefully be a
			//  very small PDF where to draw a sample from for the new robot
			//  pose.
			// =================================================================
			bool methodSOGorGrid = false;  // TRUE=SOG
			CPoint3D newDrawnPosition;
			float firstEstimateRobotHeading = std::numeric_limits<float>::max();

			// The parameters to discard too far gaussians:
			CPoint3D centerPositionPrior(ith_last_pose);
			float centerPositionPriorRadius = 2.0f;

			if (!robotActionSampler.isPrepared())
			{
				firstEstimateRobotHeading = ith_last_pose.yaw();
				// If the map is empty: There is no solution!:
				// THROW_EXCEPTION("There is no odometry & the initial beacon
				// map is empty: RO-SLAM has no solution -> ABORTED!!!");

				if (!beacMap->size())
				{
					// First iteration only...
					MRPT_LOG_WARN(
						"[RO-SLAM] Optimal filtering without map & "
						"odometry...this message should appear only the "
						"first iteration!!");
				}
				else
				{
					// To make RO-SLAM to have a solution, arbitrarily fix one
					// of the beacons so
					//  unbiguity dissapears.
					if (beacMap->get(0).m_typePDF == CBeacon::pdfSOG)
					{
						MRPT_LOG_WARN(
							"[RO-SLAM] Optimal filtering without map & "
							"odometry->FIXING ONE BEACON!");
						ASSERT_(beacMap->get(0).m_locationSOG.size() > 0);
						CPoint3D fixedBeacon(
							beacMap->get(0).m_locationSOG[0].val.mean);

						// Pass to gaussian without uncertainty:
						beacMap->get(0).m_typePDF = CBeacon::pdfGauss;
						beacMap->get(0).m_locationSOG.clear();
						beacMap->get(0).m_locationGauss.mean = fixedBeacon;
						beacMap->get(0).m_locationGauss.cov.setDiagonal(
							3, 1e-6);
					}
				}
			}  // end if there is no odometry

			// 1. Make the list of beacon IDs-ranges:
			// -------------------------------------------
			deque<TAuxRangeMeasInfo> lstObservedRanges;

			for (const auto& itObs : *sf)
			{
				if (IS_CLASS(*itObs, CObservationBeaconRanges))
				{
					const auto* obs =
						static_cast<const CObservationBeaconRanges*>(
							itObs.get());
					deque<CObservationBeaconRanges::TMeasurement>::
						const_iterator itRanges;
					for (itRanges = obs->sensedData.begin();
						 itRanges != obs->sensedData.end(); itRanges++)
					{
						ASSERT_(itRanges->beaconID != INVALID_BEACON_ID);
						// only add those in the map:
						for (auto itBeacs = beacMap->begin();
							 itBeacs != beacMap->end(); ++itBeacs)
						{
							if ((itBeacs)->m_ID == itRanges->beaconID)
							{
								TAuxRangeMeasInfo newMeas;
								newMeas.beaconID = itRanges->beaconID;
								newMeas.sensedDistance =
									itRanges->sensedDistance;
								newMeas.sensorLocationOnRobot =
									itRanges->sensorLocationOnRobot;

								ASSERT_(
									(itBeacs)->m_typePDF == CBeacon::pdfGauss ||
									(itBeacs)->m_typePDF == CBeacon::pdfSOG);
								newMeas.nGaussiansInMap =
									(itBeacs)->m_typePDF == CBeacon::pdfSOG
									? (itBeacs)->m_locationSOG.size()
									: 1 /*pdfGauss*/;

								lstObservedRanges.push_back(newMeas);
								break;	// Next observation
							}
						}
					}
				}
			}

			// ASSERT_( lstObservedRanges.size()>=2 );

			// Sort ascending ranges: Smallest ranges first -> the problem is
			// easiest!
			sort(
				lstObservedRanges.begin(), lstObservedRanges.end(),
				&TAuxRangeMeasInfo::cmp_Asc);

			if (methodSOGorGrid)
			{
				CPointPDFSOG fusedObsModels;  //<- p(z_t|x_t) for all the range
				// measurements (fused)
				fusedObsModels.clear();

				// 0. OPTIONAL: Create a "prior" as a first mode in
				// "fusedObsModels"
				//       using odometry. If there is no odometry, we
				//       *absolutely* need
				//       at least one well-localized beacon at the beginning, or
				//       the symmetry cannot be broken!
				// -----------------------------------------------------------------------------------------------
				if (robotActionSampler.isPrepared())
				{
					CPointPDFSOG::TGaussianMode newMode;
					newMode.log_w = 0;

					CPose3D auxPose = ith_last_pose +
						motionModelMeanIncr;  // CPose3D(robotMovement->poseChange->getEstimatedPose()));
					firstEstimateRobotHeading = auxPose.yaw();

					newMode.val.mean = CPoint3D(auxPose);

					// Uncertainty in z is null:
					// CMatrixF poseCOV =
					// robotMovement->poseChange->getEstimatedCovariance();
					CMatrixD poseCOV;
					robotActionSampler.getOriginalPDFCov2D(poseCOV);

					poseCOV.setSize(2, 2);
					poseCOV.setSize(3, 3);
					newMode.val.cov = poseCOV;
					fusedObsModels.push_back(newMode);	// Add it:
				}

				// 2. Generate the optimal proposal by fusing obs models
				// -------------------------------------------------------------
				for (auto itBeacs = beacMap->begin(); itBeacs != beacMap->end();
					 ++itBeacs)
				{
					// for each observed beacon (by its ID), generate
					// observation model:
					for (auto& lstObservedRange : lstObservedRanges)
					{
						if ((itBeacs)->m_ID == lstObservedRange.beaconID)
						{
							// Match:
							float sensedRange = lstObservedRange.sensedDistance;

							CPointPDFSOG newObsModel;
							(itBeacs)->generateObservationModelDistribution(
								sensedRange, newObsModel,
								beacMap.get(),	// The beacon map, for options
								lstObservedRange
									.sensorLocationOnRobot,	 // Sensor
								// location on
								// robot
								centerPositionPrior, centerPositionPriorRadius);

							if (!fusedObsModels.size())
							{
								// This is the first one: Just copy the obs.
								// model here:
								fusedObsModels = newObsModel;
							}
							else
							{
								// Fuse with existing:
								CPointPDFSOG tempFused(0);
								tempFused.bayesianFusion(
									fusedObsModels, newObsModel,
									3  // minMahalanobisDistToDrop
								);
								fusedObsModels = tempFused;
							}

							// Remove modes with negligible weights:
							// -----------------------------------------------------------

							{
								cout << "#modes bef: " << fusedObsModels.size()
									 << " ESS: " << fusedObsModels.ESS()
									 << endl;
								double max_w = -1e100;
								// int idx;

								CPointPDFSOG::iterator it;

								for (it = fusedObsModels.begin();
									 it != fusedObsModels.end(); it++)
									max_w =
										max(max_w, (it)->log_w);  // keep the
								// maximum
								// mode weight

								for (it = fusedObsModels.begin();
									 it != fusedObsModels.end();)
								{
									if (max_w - (it)->log_w >
										20)	 // Remove the mode:
										it = fusedObsModels.erase(it);
									else
										it++;
								}

								cout
									<< "#modes after: " << fusedObsModels.size()
									<< endl;
							}

							// Shall we simplify the PDF?
							// -----------------------------------------------------------
							CMatrixDouble currentCov;
							fusedObsModels.getCovariance(currentCov);
							ASSERT_(
								currentCov(0, 0) > 0 && currentCov(1, 1) > 0);
							if (sqrt(currentCov(0, 0)) < 0.10f &&
								sqrt(currentCov(1, 1)) < 0.10f &&
								sqrt(currentCov(2, 2)) < 0.10f)
							{
								// Approximate by a single gaussian!
								CPoint3D currentMean;
								fusedObsModels.getMean(currentMean);
								fusedObsModels[0].log_w = 0;
								fusedObsModels[0].val.mean = currentMean;
								fusedObsModels[0].val.cov = currentCov;

								fusedObsModels.resize(1);
							}

							{ /*
								 CMatrixD  evalGrid;
								 fusedObsModels.evaluatePDFInArea(-3,3,-3,3,0.1,0,evalGrid,
								 true);
								 evalGrid *= 1.0/evalGrid.maxCoeff();
								 CImage imgF(evalGrid, true);
								 static int autoCount=0;
								 imgF.saveToFile(format("debug_%04i.png",autoCount++));*/
							}
						}
					}  // end for itObs (in lstObservedRanges)

				}  // end for itBeacs

				if (beacMap->size())
					fusedObsModels.drawSingleSample(newDrawnPosition);
			}
			else
			{
				// =============================
				//         GRID METHOD
				// =============================

				float grid_min_x = ith_last_pose.x() - 0.5f;
				float grid_max_x = ith_last_pose.x() + 0.5f;
				float grid_min_y = ith_last_pose.y() - 0.5f;
				float grid_max_y = ith_last_pose.y() + 0.5f;
				float grid_resXY = 0.02f;

				bool repeatGridCalculation = false;

				do
				{
					auto pdfGrid = std::make_unique<CPosePDFGrid>(
						grid_min_x, grid_max_x, grid_min_y, grid_max_y,
						grid_resXY, 180.0_deg, 0, 0);

					pdfGrid->uniformDistribution();

					// Fuse all the observation models in the grid:
					// -----------------------------------------------------
					for (auto itBeacs = beacMap->begin();
						 itBeacs != beacMap->end(); ++itBeacs)
					{
						// for each observed beacon (by its ID), generate
						// observation model:
						for (auto& lstObservedRange : lstObservedRanges)
						{
							if ((itBeacs)->m_ID == lstObservedRange.beaconID)
							{
								// Match:
								float sensedRange =
									lstObservedRange.sensedDistance;

								/** /
																CPointPDFSOG
								newObsModel;
																(itBeacs)->generateObservationModelDistribution(
																	sensedRange,
																	newObsModel,
																	beacMap,
								// The beacon map, for options
																	itObs->sensorLocationOnRobot,//
								Sensor location on robot
																	centerPositionPrior,
																	centerPositionPriorRadius
								);
								/ **/
								for (size_t idxX = 0;
									 idxX < pdfGrid->getSizeX(); idxX++)
								{
									float grid_x = pdfGrid->idx2x(idxX);
									for (size_t idxY = 0;
										 idxY < pdfGrid->getSizeY(); idxY++)
									{
										double grid_y = pdfGrid->idx2y(idxY);

										// Evaluate obs model:
										double* cell =
											pdfGrid->getByIndex(idxX, idxY, 0);

										//
										double lik =
											1;	// newObsModel.evaluatePDF(CPoint3D(grid_x,grid_y,0),true);
										switch ((itBeacs)->m_typePDF)
										{
											case CBeacon::pdfSOG:
											{
												CPointPDFSOG* sog =
													&(itBeacs)->m_locationSOG;
												double sensorSTD2 = square(
													beacMap->likelihoodOptions
														.rangeStd);

												CPointPDFSOG::iterator it;
												for (it = sog->begin();
													 it != sog->end(); it++)
												{
													lik *= exp(
														-0.5 *
														square(
															sensedRange -
															(it)->val.mean.distance2DTo(
																grid_x +
																	lstObservedRange
																		.sensorLocationOnRobot
																		.x(),
																grid_y +
																	lstObservedRange
																		.sensorLocationOnRobot
																		.y())) /
														sensorSTD2);
												}
											}
											break;
											default:
												break;	// THROW_EXCEPTION("NO");
										}

										(*cell) *= lik;

									}  // for idxY
								}  // for idxX
								pdfGrid->normalize();
							}  // end if match
						}  // end for itObs
					}  // end for beacons in map

					// Draw the single pose from the grid:
					if (beacMap->size())
					{
						// Take the most likely cell:
						float maxW = -1e10f;
						float maxX = 0, maxY = 0;
						for (size_t idxX = 0; idxX < pdfGrid->getSizeX();
							 idxX++)
						{
							// float grid_x = pdfGrid->idx2x(idxX);
							for (size_t idxY = 0; idxY < pdfGrid->getSizeY();
								 idxY++)
							{
								// float grid_y = pdfGrid->idx2y(idxY);

								// Evaluate obs model:
								float c = *pdfGrid->getByIndex(idxX, idxY, 0);
								if (c > maxW)
								{
									maxW = c;
									maxX = pdfGrid->idx2x(idxX);
									maxY = pdfGrid->idx2y(idxY);
								}
							}
						}
						newDrawnPosition.x(maxX);
						newDrawnPosition.y(maxY);

						if (grid_resXY > 0.01f)
						{
							grid_min_x = maxX - 0.03f;
							grid_max_x = maxX + 0.03f;
							grid_min_y = maxY - 0.03f;
							grid_max_y = maxY + 0.03f;
							grid_resXY = 0.002f;
							repeatGridCalculation = true;
						}
						else
							repeatGridCalculation = false;
					}
					pdfGrid.reset();

				} while (repeatGridCalculation);

				// newDrawnPosition has the pose:
			}

			// 3. Get the new "finalPose" of this particle as a random sample
			// from the
			//  optimal proposal:
			// ---------------------------------------------------------------------------
			if (!beacMap->size())
			{
				// If we are in the first iteration (no map yet!) just stay
				// still:
				finalPose = ith_last_pose;
			}
			else
			{
				cout << "Drawn: " << newDrawnPosition << endl;
				// cout << "Final cov was:\n" <<
				// fusedObsModels.getEstimatedCovariance() << endl << endl;

				// Make sure it was initialized
				ASSERT_(
					firstEstimateRobotHeading !=
					std::numeric_limits<float>::max());

				finalPose.setFromValues(
					newDrawnPosition.x(), newDrawnPosition.y(),
					newDrawnPosition.z(), firstEstimateRobotHeading, 0, 0);
			}
			/** \todo If there are 2+ sensors on the robot, compute phi?
			 */

			// 4. Update the weight:
			//     In optimal sampling this is the expectation of the
			//     observation likelihood: This is the observation likelihood
			//     averaged
			//     over the whole optimal proposal.
			// ---------------------------------------------------------------------------
			double weightUpdate = 0;
			partIt->log_w += PF_options.powFactor * weightUpdate;
		}
		else
		{
			// By default:
			// Generate gaussian-distributed 2D-pose increments according to
			// mean-cov:
			if (!robotActionSampler.isPrepared())
				THROW_EXCEPTION(
					"Action list does not contain any CActionRobotMovement2D "
					"or CActionRobotMovement3D object!");

			robotActionSampler.drawSample(incrPose);

			finalPose = ith_last_pose + incrPose;
		}

		// Insert as the new pose in the path:
		partIt->d->robotPath.push_back(finalPose.asTPose());

		// ----------------------------------------------------------------------
		//						UPDATE STAGE
		// ----------------------------------------------------------------------
		if (!updateStageAlreadyDone)
		{
			partIt->log_w += PF_options.powFactor *
				(PF_SLAM_computeObservationLikelihoodForParticle(
					 PF_options, i, *sf, finalPose) +
				 extra_log_lik);
		}  // if update not already done...

	}  // end of for each particle "i" & "partIt"

	MRPT_LOG_DEBUG("Stage 1) Prediction done.");

	MRPT_END
}

/*---------------------------------------------------------------
			prediction_and_update_pfStandardProposal
 ---------------------------------------------------------------*/
void CMultiMetricMapPDF::prediction_and_update_pfStandardProposal(
	const mrpt::obs::CActionCollection* actions,
	const mrpt::obs::CSensoryFrame* sf,
	const bayes::CParticleFilter::TParticleFilterOptions& PF_options)
{
	MRPT_START

	PF_SLAM_implementation_pfStandardProposal<mrpt::slam::detail::TPoseBin2D>(
		actions, sf, PF_options, options.KLD_params);

	// Average map will need to be updated after this:
	averageMapIsUpdated = false;

	MRPT_END
}

// Specialization for my kind of particles:
void CMultiMetricMapPDF::
	PF_SLAM_implementation_custom_update_particle_with_new_pose(
		CRBPFParticleData* particleData, const TPose3D& newPose) const
{
	particleData->robotPath.push_back(newPose);
}

// Specialization for RBPF maps:
bool CMultiMetricMapPDF::PF_SLAM_implementation_doWeHaveValidObservations(
	const CMultiMetricMapPDF::CParticleList& particles,
	const CSensoryFrame* sf) const
{
	if (sf == nullptr) return false;
	ASSERT_(!particles.empty());
	return particles.begin()
		->d.get()
		->mapTillNow.canComputeObservationsLikelihood(*sf);
}

/** Do not move the particles until the map is populated.  */
bool CMultiMetricMapPDF::PF_SLAM_implementation_skipRobotMovement() const
{
	return 0 == getNumberOfObservationsInSimplemap();
}

/*---------------------------------------------------------------
 Evaluate the observation likelihood for one
   particle at a given location
 ---------------------------------------------------------------*/
double CMultiMetricMapPDF::PF_SLAM_computeObservationLikelihoodForParticle(
	[[maybe_unused]] const CParticleFilter::TParticleFilterOptions& PF_options,
	const size_t particleIndexForMap, const CSensoryFrame& observation,
	const CPose3D& x) const
{
	auto* map = const_cast<CMultiMetricMap*>(
		&m_particles[particleIndexForMap].d->mapTillNow);
	double ret = 0;
	for (const auto& it : observation)
		ret += map->computeObservationLikelihood(*it, x);
	return ret;
}
