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

/*---------------------------------------------------------------------------------
 APPLICATION: Range-Only Localization with Particle Filter
 AUTHORs: The MAPIR group

 DESCRIPTION: Localization with PF, with an extended model to
			  cope with unknown, dynamic biases in the beacon
			  ranges. Refer to the paper:
 https://www.mrpt.org/paperuwb-particle-filter-localization/
 Demo config file:
 https://github.com/MRPT/mrpt/blob/master/share/mrpt/config_files/ro-localization/UWB_localization_demo.ini

 Antonio J. Ortiz de Galisteo worked hard in the first version of this program.
  Gracias!! :-)
 ---------------------------------------------------------------------------------
 */

#include <mrpt/bayes/CParticleFilter.h>
#include <mrpt/config/CConfigFile.h>
#include <mrpt/gui/CDisplayWindow3D.h>
#include <mrpt/io/CFileGZInputStream.h>
#include <mrpt/io/CFileGZOutputStream.h>
#include <mrpt/io/CFileStream.h>
#include <mrpt/maps/CLandmarksMap.h>
#include <mrpt/maps/CSimplePointsMap.h>
#include <mrpt/math/CMatrixF.h>
#include <mrpt/math/data_utils.h>
#include <mrpt/obs/CObservationBeaconRanges.h>
#include <mrpt/obs/CObservationGPS.h>
#include <mrpt/obs/CRawlog.h>
#include <mrpt/opengl/CAxis.h>
#include <mrpt/opengl/CDisk.h>
#include <mrpt/opengl/CEllipsoid2D.h>
#include <mrpt/opengl/CGridPlaneXY.h>
#include <mrpt/opengl/CPointCloud.h>
#include <mrpt/opengl/CSphere.h>
#include <mrpt/random.h>
#include <mrpt/serialization/CArchive.h>
#include <mrpt/system/filesystem.h>
#include <mrpt/system/os.h>

using namespace mrpt;
using namespace mrpt::obs;
using namespace mrpt::maps;
using namespace mrpt::bayes;
using namespace mrpt::poses;
using namespace mrpt::opengl;
using namespace mrpt::gui;
using namespace mrpt::io;
using namespace mrpt::serialization;
using namespace mrpt::math;
using namespace mrpt::random;
using namespace mrpt::system;
using namespace std;

#include "CPosePDFParticlesExtended.h"

#if MRPT_HAS_OPENGL_GLUT && MRPT_HAS_WXWIDGETS
#define SHOW_REAL_TIME_3D
#endif

#define STORE_3D

// ------------------------------------------------------
//				Configuration
// ------------------------------------------------------
std::unique_ptr<mrpt::config::CConfigFile> iniFile;
std::string iniFileName;

// extern double	likelihood_acumulation;

// ------------------------------------------------------
//				TestParticlesLocalization
// ------------------------------------------------------
void TestParticlesLocalization()
{
	// Load configuration!
	// ------------------------------------------

	// PF-algorithm Options:
	// ---------------------------
	CParticleFilter::TParticleFilterOptions pfOptions;
	pfOptions.loadFromConfigFile(*iniFile, "PF_options");

	int NUM_REPS =
		iniFile->read_int("ro-localization", "experimentRepetitions", 1);
	std::string RAWLOG_FILE =
		iniFile->read_string("ro-localization", "rawlog_file", "NOT FOUND!");
	std::string MAP_FILE =
		iniFile->read_string("ro-localization", "gridmap_file", "NOT FOUND!");
	std::string OUT_DIR_PREFIX = iniFile->read_string(
		"ro-localization", "logOutput_dir", "RO-LOCALIZATION_OUT");
	int SCENE3D_FREQ =
		iniFile->read_int("ro-localization", "3DSceneFrequency", 10);
	std::string GT_FILE =
		iniFile->read_string("ro-localization", "groundTruthFile", "");

	CMatrixF aux(1, 1);

	float Pc_range_ini = 0.05f;
	float Pc_range_end = 0.05f;
	float Pc_range_step = 0.05f;

	MRPT_LOAD_CONFIG_VAR(Pc_range_ini, float, (*iniFile), "ro-localization");
	MRPT_LOAD_CONFIG_VAR(Pc_range_end, float, (*iniFile), "ro-localization");
	MRPT_LOAD_CONFIG_VAR(Pc_range_step, float, (*iniFile), "ro-localization");

	uint64_t random_seed = 0;
	MRPT_LOAD_CONFIG_VAR(random_seed, uint64_t, (*iniFile), "ro-localization");

	bool SHOW_3D_FRANCO_POSITION = false;
	bool SAVE_3D_TO_VIDEO = false;

	MRPT_LOAD_CONFIG_VAR(
		SHOW_3D_FRANCO_POSITION, bool, (*iniFile), "ro-localization");
	MRPT_LOAD_CONFIG_VAR(SAVE_3D_TO_VIDEO, bool, (*iniFile), "ro-localization");

	ASSERT_FILE_EXISTS_(RAWLOG_FILE);

	// Load GT:
	CMatrixF groundTruth;
	if (!GT_FILE.empty()) groundTruth.loadFromTextFile(GT_FILE);

	// Real ranges estimated from GT:
	std::string OUT_DIR;

	// --------------------------------------------------------------------
	//						EXPERIMENT PREPARATION
	// --------------------------------------------------------------------
	CTicTac tictac, tictacGlobal;
	size_t rawlogEntry, rawlogEntries;
	CParticleFilter::TParticleFilterStats PF_stats;

	// Load the set of metric maps to consider in the experiments:
	CMultiMetricMap metricMap;
	TSetOfMetricMapInitializers mapList;
	mapList.loadFromConfigFile(*iniFile, "MetricMap");
	metricMap.setListOfMaps(mapList);
	mapList.dumpToConsole();
	pfOptions.dumpToConsole();

	// Init PSRNG:
	if (random_seed) getRandomGenerator().randomize(random_seed);
	else
		getRandomGenerator().randomize();

	// --------------------------------------------------------------------
	//					EXPERIMENT REPETITIONS LOOP
	// --------------------------------------------------------------------

	// --------------------------
	// Load the rawlog:
	// --------------------------
	printf("Loading the rawlog file...");
	CRawlog rawlog;
	rawlog.loadFromRawLogFile(RAWLOG_FILE);

	rawlogEntries = rawlog.size();
	std::cout << "rawlog Entries: " << rawlogEntries << "\n";
	printf("OK\n");

	// Number of beacons in the fixed, reference map:
	size_t nBeaconsInMap = 0;
	auto beacMap = metricMap.mapByClass<CLandmarksMap>();
	if (beacMap) nBeaconsInMap = beacMap->size();

	CMatrixDouble franco_matrix(rawlogEntries, 3);
	CMatrixDouble particle_matrix(rawlogEntries, 12 + nBeaconsInMap);
	CMatrixDouble real_ranges(rawlogEntries, nBeaconsInMap);
	CMatrixDouble real_offsets(rawlogEntries, 1 + nBeaconsInMap);

	CPose3D sensorPoseOnRobot;
	bool sensorPoseOnRobot_valid = false;

	tictacGlobal.Tic();
	for (float range_Pc = Pc_range_ini; range_Pc <= Pc_range_end;
		 range_Pc += Pc_range_step)
	{
		// The experiment directory is:
		OUT_DIR = format("%s_Pc_%.06f", OUT_DIR_PREFIX.c_str(), range_Pc);
		printf("Creating directory: %s\n", OUT_DIR.c_str());
		mrpt::system::createDirectory(OUT_DIR);

		CVectorDouble vector_errs_xy;

		for (int repetition = 0; repetition < NUM_REPS; repetition++)
		{
			unsigned int PARTICLE_COUNT = pfOptions.sampleSize;
			CPosePDFParticlesExtended pdf(PARTICLE_COUNT);
			// PDF Options:
			// ------------------
			pdf.options.KLD_binSize_PHI = DEG2RAD(
				iniFile->read_float("KLD_options", "KLD_binSize_PHI_deg", 5));
			pdf.options.KLD_binSize_XY =
				iniFile->read_float("KLD_options", "KLD_binSize_XY", 0.2f);
			pdf.options.KLD_delta =
				iniFile->read_float("KLD_options", "KLD_delta", 0.01f);
			pdf.options.KLD_epsilon =
				iniFile->read_float("KLD_options", "KLD_epsilon", 0.02f);
			pdf.options.KLD_maxSampleSize =
				iniFile->read_int("KLD_options", "KLD_maxSampleSize", 100000);
			pdf.options.KLD_minSampleSize =
				iniFile->read_int("KLD_options", "KLD_minSampleSize", 250);

			pdf.options.probabilityChangingBias = iniFile->read_float(
				"RO_MODEL", "probabilityChangingBias", 0.1f);
			pdf.options.changingBiasUnifRange =
				iniFile->read_float("RO_MODEL", "changingBiasUnifRange", 2.0f);
			pdf.options.mixtureProposalRatio =
				iniFile->read_float("RO_MODEL", "mixtureProposalRatio", 0.10f);

			size_t real_offsets_rows = 0;
			double accum_xy_err_sqr = 0;
			if (Pc_range_ini != Pc_range_end)
			{
				pdf.options.probabilityChangingBias = range_Pc;
			}

			pdf.options.metricMap = &metricMap;

			// Create Directory
			OUT_DIR = format(
				"%s_Pc_%.06f/REP_%03u", OUT_DIR_PREFIX.c_str(), range_Pc,
				(unsigned int)repetition);
			printf("Creating directory: %s\n", OUT_DIR.c_str());

			mrpt::system::deleteFilesInDirectory(OUT_DIR);
			mrpt::system::createDirectory(OUT_DIR);

			// Save the landmarks for plot in matlab:
			if (beacMap)
				beacMap->saveToMATLABScript2D(
					format("%s/plot_landmarks_map.m", OUT_DIR.c_str()));

			CPose2D odometryEstimation(
				iniFile->read_float("OdometryEstimation", "odometry_X", 0),
				iniFile->read_float("OdometryEstimation", "odometry_Y", 0),
				DEG2RAD(iniFile->read_float(
					"OdometryEstimation", "odometry_PHI", 0)));

			CPose2D initialPoseExperiment(odometryEstimation);

			CParticleFilter PF;
			PF.m_options = pfOptions;

			int step = 0;
			rawlogEntry = 0;

			// Initialize the PDF:
			// -----------------------------
			tictac.Tic();
			CVectorFloat state_min(nBeaconsInMap, 0.0f);
			CVectorFloat state_max(nBeaconsInMap, 0.0f);

			pdf.resetUniform(
				initialPoseExperiment.x() - 0.5,
				initialPoseExperiment.x() + 0.5,
				initialPoseExperiment.y() - 0.5,
				initialPoseExperiment.y() + 0.5, state_min, state_max, -M_PIf,
				M_PIf, PARTICLE_COUNT);

			printf(
				"PDF of %u particles initialized in %.03fms\n", PARTICLE_COUNT,
				1000 * tictac.Tac());

// 3D World
#ifdef STORE_3D
			Scene::Ptr scene = std::make_shared<Scene>();

#ifdef SHOW_REAL_TIME_3D
			CDisplayWindow3D window("ro-localization - Part of MRPT");
			window.setPos(50, 50);
			window.resize(800, 500);
			window.setCameraElevationDeg(90);
			window.setCameraZoom(11);
			window.setCameraAzimuthDeg(-90);
			window.setCameraPointingToPoint(6.0, 3.0, 0);

			window.setCameraProjective(false);

			if (SAVE_3D_TO_VIDEO) { window.grabImagesStart(); }

			Scene::Ptr sceneTR = window.get3DSceneAndLock();
#endif
			// World Axis
			{
				opengl::CAxis::Ptr obj =
					std::make_shared<opengl::CAxis>(-20, -10, -1, 20, 10, 4, 1);
				obj->enableTickMarks();
				obj->setColor(0, 0, 0);
#ifdef SHOW_REAL_TIME_3D
				sceneTR->insert(obj);
#endif
				scene->insert(obj);
			}

			// Grid2D
			if (mrpt::system::fileExists(MAP_FILE))
			{
				COccupancyGridMap2D grid2d;
				{
					CFileGZInputStream f(MAP_FILE);
					archiveFrom(f) >> grid2d;
				}

#ifdef SHOW_REAL_TIME_3D
				{
					auto obj = grid2d.getVisualization();
					obj->setLocation(
						initialPoseExperiment.x(), initialPoseExperiment.y(),
						0);
					sceneTR->insert(obj);
				}
#endif
				auto obj = grid2d.getVisualization();
				scene->insert(obj);
			}
			else
			// Floor
			{
				opengl::CGridPlaneXY::Ptr obj =
					std::make_shared<opengl::CGridPlaneXY>(
						-20, 20, -10, 10, 0, 0.5);
				obj->setColor(0.4f, 0.4f, 0.4f);
#ifdef SHOW_REAL_TIME_3D
				sceneTR->insert(obj);
#endif
				scene->insert(obj);
			}

			// The beacons
			if (beacMap)
			{
				for (size_t k = 0; k < beacMap->size(); k++)
				{
					auto parts = opengl::CSphere::Create();
					parts->setColor(1, 0, 0);
					parts->setLocation(beacMap->landmarks.get(k)->pose_mean);
					parts->setRadius(0.2f);

					parts->setName(format(" B%i", int(k + 1)));
					parts->enableShowName();
					scene->insert(parts);
#ifdef SHOW_REAL_TIME_3D
					sceneTR->insert(parts);
#endif
				}
			}

#ifdef SHOW_REAL_TIME_3D
			window.unlockAccess3DScene();
			window.forceRepaint();
#endif
#endif
			// -----------------------------
			//		Particle filter
			// -----------------------------
			CActionCollection::Ptr action;
			CSensoryFrame::Ptr observations;
			CPose2D pdfEstimation;

			while (rawlogEntry < (rawlogEntries - 1))
			{
				// Load pose change from the rawlog:
				// ----------------------------------------
				cout << endl << "RAWLOG_ENTRY: " << rawlogEntry << endl << endl;

				if (!rawlog.getActionObservationPair(
						action, observations, rawlogEntry))
					break;	// end of rawlog.

				//-------------------FRANCO DATA--------------------
				{
					CObservationBeaconRanges::Ptr beaconPose =
						observations
							->getObservationByClass<CObservationBeaconRanges>();
					if (beaconPose)
					{
						franco_matrix(step, 0) =
							beaconPose->auxEstimatePose.x();
						franco_matrix(step, 1) =
							beaconPose->auxEstimatePose.y();
						if (beaconPose->auxEstimatePose.phi() > 180)
							franco_matrix(step, 2) = 1e10;
						else if (beaconPose->auxEstimatePose.phi() < -180)
							franco_matrix(step, 2) = -1e10;
						else
							franco_matrix(step, 2) =
								beaconPose->auxEstimatePose.phi();
					}
					else
					{
						franco_matrix(step, 0) = -6;
						franco_matrix(step, 1) = -6;
						franco_matrix(step, 1) = -6;
					}
				}
				//--------------------------------------------------
				// PARTICLE FILTER:
				// ----------------------------------------
				tictac.Tic();
				printf(
					"Executing ParticleFilter on %u particles....",
					(unsigned int)pdf.particlesCount());

				PF.executeOn(
					pdf,
					action.get(),  // Action
					observations.get(),	 // Obs.
					&PF_stats  // Output statistics
				);
				printf(
					" Done! in %.03fms, ESS=%f\n", 1000.0f * tictac.Tac(),
					pdf.ESS());

				// Text output:
				// ----------------------------------------
				odometryEstimation = odometryEstimation +
					action->getBestMovementEstimation()
						->poseChange->getMeanVal();
				pdf.getMean(pdfEstimation);

				CPose2D GT_Pose;
				if (groundTruth.rows() > step)
				{
					GT_Pose = CPose2D(
						groundTruth(step, 1), groundTruth(step, 2),
						groundTruth(step, 3));
					GT_Pose = initialPoseExperiment + GT_Pose;

					// Add error:
					accum_xy_err_sqr += GT_Pose.sqrDistanceTo(pdfEstimation);

					// Create real_ranges row:
					// ----------------------------
					CObservationBeaconRanges::Ptr beaconPose =
						observations
							->getObservationByClass<CObservationBeaconRanges>();
					if (!sensorPoseOnRobot_valid)
					{
						if (beaconPose)
						{
							beaconPose->getSensorPose(sensorPoseOnRobot);
							sensorPoseOnRobot_valid = true;
						}
					}

					if (sensorPoseOnRobot_valid)
					{
						CPose3D sensorPose = GT_Pose + sensorPoseOnRobot;
						bool any_valid = false;
						for (size_t q = 0; q < nBeaconsInMap; q++)
						{
							mrpt::maps::CLandmark* lm =
								beacMap->landmarks.get(q);
							// Real range
							double R_real =
								sensorPose.distanceTo(lm->pose_mean);
							real_ranges(step, q) = R_real;

							// Real offset:
							if (beaconPose)
							{
								float R =
									beaconPose->getSensedRangeByBeaconID(q + 1);
								if (R > 0)
								{
									any_valid = true;
									real_offsets(real_offsets_rows, 1 + q) =
										R - R_real;
								}
							}
						}
						if (any_valid)
						{
							real_offsets(real_offsets_rows, 0) = step;
							real_offsets_rows++;
						}
					}

				}  // end GT

				TExtendedCPose2D meanState = pdf.getEstimatedPoseState();

				// likelihood_acumulation += pdfEstimation.distanceTo(CPoint2D
				// (groundTruth(step,1),groundTruth(step,2)));

				std::cout << "    Odometry est: " << odometryEstimation << "\n";
				std::cout << "         PDF est: " << pdfEstimation
						  << ", ESS (B.R.)= " << PF_stats.ESS_beforeResample
						  << " Cov:\n";

				CMatrixDouble cov;
				pdf.getCovariance(cov);

				std::cout << cov << "\n sqrt(Det.Cov) = " << sqrt(cov.det())
						  << "\n";

				//-----------------PARTICLE DATA-----------------
				particle_matrix(step, 0) = pdfEstimation.x();
				particle_matrix(step, 1) = pdfEstimation.y();
				particle_matrix(step, 2) = pdfEstimation.phi();

				particle_matrix(step, 3) = cov(0, 0);
				particle_matrix(step, 4) = cov(1, 1);
				particle_matrix(step, 5) = cov(2, 2);
				particle_matrix(step, 6) = cov(0, 1);
				particle_matrix(step, 7) = cov(0, 2);
				particle_matrix(step, 8) = cov(1, 2);

				particle_matrix(step, 9) = GT_Pose.x();
				particle_matrix(step, 10) = GT_Pose.y();
				particle_matrix(step, 11) = GT_Pose.phi();

				ASSERT_EQUAL_(
					size_t(meanState.state.size()), size_t(nBeaconsInMap));
				for (size_t l = 0; l < nBeaconsInMap; l++)
					particle_matrix(step, 12 + l) = meanState.state[l];

#ifdef STORE_3D
				// Generate 3D scene:
				// ------------------------------
				{
					const auto [C, meanPose] = pdf.getCovarianceAndMean();

#ifdef SHOW_REAL_TIME_3D
					sceneTR = window.get3DSceneAndLock();
#endif

// The particles:
#ifdef SHOW_REAL_TIME_3D
					opengl::CPointCloud::Ptr parts;

					{
						auto obj = sceneTR->getByName("part");
						if (!obj)
						{
							parts = opengl::CPointCloud::Create();
							scene->insert(parts);
#ifdef SHOW_REAL_TIME_3D
							sceneTR->insert(parts);
#endif
						}
						else
							parts = std::dynamic_pointer_cast<CPointCloud>(obj);
					}
#else
					opengl::CPointCloud::Ptr parts =
						std::make_shared<opengl::CPointCloud>();
					opengl::CRenderizable::Ptr obj;
#endif

					parts->setColor(0, 0, 1);
					parts->setName("part");
					parts->enableColorFromZ(false);

					parts->resize(pdf.size());

					for (size_t i = 0; i < pdf.size(); i++)
						parts->setPoint(
							i, pdf.m_particles[i].d->pose.x(),
							pdf.m_particles[i].d->pose.y(), 0);

// The particles' cov:
#ifdef SHOW_REAL_TIME_3D
					opengl::CEllipsoid2D::Ptr ellip;
					{
						auto obj = sceneTR->getByName("cov");
						if (!obj)
						{
							ellip = std::make_shared<opengl::CEllipsoid2D>();
							scene->insert(ellip);
#ifdef SHOW_REAL_TIME_3D
							sceneTR->insert(ellip);
#endif
						}
						else
							ellip =
								std::dynamic_pointer_cast<CEllipsoid2D>(obj);
					}
#else
					opengl::CEllipsoid2D::Ptr ellip =
						std::make_shared<opengl::CEllipsoid2D>();
#endif

					ellip->setColor(1, 0, 0, 0.6);
					ellip->setLocation(meanPose.x(), meanPose.y(), 0.05);

					ellip->setLineWidth(2);
					ellip->setQuantiles(3);
					ellip->setCovMatrix(C.blockCopy<2, 2>(0, 0));
					ellip->setName("cov");

#ifdef SHOW_REAL_TIME_3D
					// The laser scan:
					opengl::CPointCloud::Ptr scanPts;
					{
						auto obj = sceneTR->getByName("laser");
						if (!obj)
						{
							scanPts = opengl::CPointCloud::Create();
							scene->insert(scanPts);
							sceneTR->insert(scanPts);
						}
						else
							scanPts =
								std::dynamic_pointer_cast<CPointCloud>(obj);
					}

					scanPts->setColor(1, 0, 0, 0.9);
					scanPts->enableColorFromZ(false);
					scanPts->setPointSize(3);
					scanPts->setName("laser");

					CSimplePointsMap map;
					CPose3D robotPose3D(meanPose);
					observations->insertObservationsInto(map, robotPose3D);
					scanPts->loadFromPointsMap(&map);

					// Beacon range spheres:
					if (auto dist = observations->getObservationByClass<
									CObservationBeaconRanges>();
						beacMap && dist && !dist->sensedData.empty())
					{
						for (auto& k : dist->sensedData)
						{
							string beacon_name =
								format("ring%u", unsigned(k.beaconID));
							const mrpt::maps::CLandmark* lm =
								beacMap->landmarks.getByBeaconID(k.beaconID);
							if (lm)
							{
#ifdef SHOW_REAL_TIME_3D
								opengl::CDisk::Ptr sphere;
								{
									auto obj = sceneTR->getByName(beacon_name);
									if (!obj)
									{
										sphere =
											std::make_shared<opengl::CDisk>();
										scene->insert(sphere);
#ifdef SHOW_REAL_TIME_3D
										sceneTR->insert(sphere);
#endif
									}
									else
										sphere =
											std::dynamic_pointer_cast<CDisk>(
												obj);
								}
#else
								opengl::CSphere::Ptr sphere =
									std::make_shared<opengl::CSphere>();
								opengl::CRenderizable::Ptr obj;
#endif

								sphere->setColor(0, 0, 1, 0.3);
								sphere->setSlicesCount(40);
								sphere->setName(beacon_name);

								sphere->setLocation(
									lm->pose_mean.x, lm->pose_mean.y, 0.05);

								float R = square(k.sensedDistance) -
									square(k.sensorLocationOnRobot.z() -
										   lm->pose_mean.z);

								if (R > 0) R = sqrt(R);
								else
									R = 0.08f;
								sphere->setDiskRadius(R + 0.08f, R - 0.08f);
							}
						}
					}  // for each beacon

					// Franco Position
					if (SHOW_3D_FRANCO_POSITION)
					{
						opengl::CRenderizable::Ptr obj =
							sceneTR->getByName("franc");
						opengl::CSphere::Ptr sphere;

						if (!obj) sphere = std::make_shared<opengl::CSphere>();
						else
							sphere =
								std::dynamic_pointer_cast<opengl::CSphere>(obj);
						sphere->setColor(0, 1, 0);
						sphere->setRadius(0.05f);
						sphere->setName("franc");

						if (auto dist = observations->getObservationByClass<
										CObservationBeaconRanges>();
							dist)
						{
							sphere->setLocation(
								dist->auxEstimatePose.x(),
								dist->auxEstimatePose.y(), 0.05f);
						}

						if (!obj)
						{
							scene->insert(sphere);
							sceneTR->insert(sphere);
						}
					}
#endif

					// Mean of particles
					{
#ifdef SHOW_REAL_TIME_3D
						opengl::CSphere::Ptr sphere;

						{
							auto obj = sceneTR->getByName("mean_parts");
							if (!obj)
							{
								sphere = std::make_shared<opengl::CSphere>();
								scene->insert(sphere);
#ifdef SHOW_REAL_TIME_3D
								sceneTR->insert(sphere);
#endif
							}
							else
								sphere =
									std::dynamic_pointer_cast<opengl::CSphere>(
										obj);
						}

#else
						opengl::CSphere::Ptr sphere =
							std::make_shared<opengl::CSphere>();
#endif
						sphere->setColor(0, 0, 1);
						sphere->setRadius(0.05f);
						sphere->setName("mean_parts");

						sphere->setLocation(
							pdfEstimation.x(), pdfEstimation.y(), 0.05);
					}

					// groundTruth
					if (groundTruth.rows() > step)
					{
#ifdef SHOW_REAL_TIME_3D
						opengl::CSphere::Ptr sphere;

						{
							auto obj = sceneTR->getByName("GT");
							if (!obj)
							{
								sphere = std::make_shared<opengl::CSphere>();
								scene->insert(sphere);
#ifdef SHOW_REAL_TIME_3D
								sceneTR->insert(sphere);
#endif
							}
							else
								sphere =
									std::dynamic_pointer_cast<opengl::CSphere>(
										obj);
						}
#else
						opengl::CSphere::Ptr sphere =
							std::make_shared<opengl::CSphere>();
#endif
						sphere->setColor(0, 0, 0);
						sphere->setRadius(0.10f);
						sphere->setName("GT");

						sphere->setLocation(GT_Pose.x(), GT_Pose.y(), 0.05);

						cout << "GT robot pose: " << GT_Pose << endl;
					}

#ifdef SHOW_REAL_TIME_3D
					// GPS pose
					{
						auto o = observations
									 ->getObservationByClass<CObservationGPS>();
						if (o && beacMap)
						{
							opengl::CEllipsoid2D::Ptr sphere;
							double x, y;

							{
								auto obj = sceneTR->getByName("gps");
								if (!obj)
								{
									sphere = std::make_shared<
										opengl::CEllipsoid2D>();
									scene->insert(sphere);
									sceneTR->insert(sphere);
								}
								else
									sphere = std::dynamic_pointer_cast<
										opengl::CEllipsoid2D>(obj);
							}

							sphere->setColor(0, 1, 1, 0.5);
							sphere->setName("gps");
							if (o->hasMsgClass<
									mrpt::obs::gnss::Message_NMEA_GGA>())
							{
								const mrpt::obs::gnss::Message_NMEA_GGA& gga =
									o->getMsgByClass<
										mrpt::obs::gnss::Message_NMEA_GGA>();
								x = DEG2RAD(
										(gga.fields.longitude_degrees -
										 beacMap->likelihoodOptions.GPSOrigin
											 .longitude)) *
									6371000 * 1.03;
								y = DEG2RAD(
										(gga.fields.latitude_degrees -
										 beacMap->likelihoodOptions.GPSOrigin
											 .latitude)) *
									6371000 * 1.15;
								sphere->setLocation(
									(x *
										 cos(beacMap->likelihoodOptions
												 .GPSOrigin.ang) +
									 y *
										 sin(beacMap->likelihoodOptions
												 .GPSOrigin.ang) +
									 beacMap->likelihoodOptions.GPSOrigin
										 .x_shift),
									(-x *
										 sin(beacMap->likelihoodOptions
												 .GPSOrigin.ang) +
									 y *
										 cos(beacMap->likelihoodOptions
												 .GPSOrigin.ang) +
									 beacMap->likelihoodOptions.GPSOrigin
										 .y_shift),
									0);
							}
							auto r = CMatrixDouble22::Zero();
							r(1, 1) = 9;
							r(0, 0) = 9;
							sphere->setCovMatrix(r);
						}
					}
					{
						CObservationGPS::Ptr o =
							observations
								->getObservationByClass<CObservationGPS>();
						if (o)
						{
							opengl::CSphere::Ptr sphere;
							double x, y;

							{
								auto obj = sceneTR->getByName("gps_CENTER");
								if (!obj)
								{
									sphere =
										std::make_shared<opengl::CSphere>();
									scene->insert(sphere);
									sceneTR->insert(sphere);
								}
								else
									sphere = std::dynamic_pointer_cast<
										opengl::CSphere>(obj);
							}
							sphere->setColor(0, 1, 1);
							sphere->setName("gps_CENTER");
							if (o->hasMsgClass<
									mrpt::obs::gnss::Message_NMEA_GGA>())
							{
								const mrpt::obs::gnss::Message_NMEA_GGA& gga =
									o->getMsgByClass<
										mrpt::obs::gnss::Message_NMEA_GGA>();
								x = DEG2RAD(
										(gga.fields.longitude_degrees -
										 beacMap->likelihoodOptions.GPSOrigin
											 .longitude)) *
									6371000 * 1.03;	 //*9000000;
								y = DEG2RAD(
										(gga.fields.latitude_degrees -
										 beacMap->likelihoodOptions.GPSOrigin
											 .latitude)) *
									6371000 * 1.15;	 //*12000000;
								sphere->setLocation(
									(x *
										 cos(beacMap->likelihoodOptions
												 .GPSOrigin.ang) +
									 y *
										 sin(beacMap->likelihoodOptions
												 .GPSOrigin.ang) +
									 beacMap->likelihoodOptions.GPSOrigin
										 .x_shift),
									(-x *
										 sin(beacMap->likelihoodOptions
												 .GPSOrigin.ang) +
									 y *
										 cos(beacMap->likelihoodOptions
												 .GPSOrigin.ang) +
									 beacMap->likelihoodOptions.GPSOrigin
										 .y_shift),
									0);
							}
							sphere->setRadius(0.5);
						}
					}

					window.unlockAccess3DScene();
					window.forceRepaint();
					std::this_thread::sleep_for(2ms);  // Time to refresh
#endif
				}
#endif

				if ((step % SCENE3D_FREQ) == 0)
					scene->saveToFile(format(
						"%s/3Dscene_%03u.3Dscene", OUT_DIR.c_str(), step));

				step++;
			}  // while rawlogEntries
			// aux(0,0)=(float)likelihood_acumulation;
			// aux.saveToTextFile( format("%s/LIKELIHOOD.txt",OUT_DIR.c_str())
			// );

			franco_matrix.setSize(step, 3);
			franco_matrix.saveToTextFile(
				format("%s/franco_matrix.txt", OUT_DIR.c_str()));

			particle_matrix.setSize(step, particle_matrix.cols());
			particle_matrix.saveToTextFile(
				format("%s/particle_matrix.txt", OUT_DIR.c_str()),
				MATRIX_FORMAT_FIXED);

			real_ranges.setSize(step, real_ranges.cols());
			real_ranges.saveToTextFile(
				format("%s/GT_ranges.txt", OUT_DIR.c_str()),
				MATRIX_FORMAT_FIXED);

			real_offsets.setSize(real_offsets_rows, real_offsets.cols());
			real_offsets.saveToTextFile(
				format("%s/GT_offsets.txt", OUT_DIR.c_str()),
				MATRIX_FORMAT_FIXED);

			// Error stats:
			if (step > 0)
			{
				double avrg_xy_err = sqrt(accum_xy_err_sqr / step);
				vector_errs_xy.push_back(avrg_xy_err);
			}

		}  // repetitions

		// Average errors:
		if (Pc_range_ini != Pc_range_end)
		{
			CFileStream fo(
				format(
					"Pc_%.06f_%.06f_RESULTs.txt", Pc_range_ini, Pc_range_end),
				fomAppend);
			double err_mean, err_std;
			mrpt::math::meanAndStd(vector_errs_xy, err_mean, err_std);
			fo.printf("%f %f %f\n", range_Pc, err_mean, err_std);
		}

	}  // for Pc

	printf("\n TOTAL EXECUTION TIME = %.06f sec\n\n", tictacGlobal.Tac());

	// mrpt::system::pause();
}

// ------------------------------------------------------
//						MAIN
// ------------------------------------------------------
int main(int argc, char** argv)
{
	try
	{
		printf(" ro-localization - Version 0.1 - Part of the MRPT\n");
		printf(
			" MRPT C++ Library: %s - Sources timestamp: %s\n",
			MRPT_getVersion().c_str(), MRPT_getCompilationDate().c_str());
		printf(
			"------------------------------------------------------------------"
			"-\n");

		// Process arguments:
		if (argc < 2)
		{
			printf("Usage: %s <config.ini>\n\n", argv[0]);
			// pause();
			return -1;
		}

		iniFileName = argv[1];

		iniFile = std::make_unique<mrpt::config::CConfigFile>(iniFileName);

		TestParticlesLocalization();

		return 0;
	}
	catch (exception& e)
	{
		cout << "Caught MRPT exception:\n" << mrpt::exception_to_str(e) << endl;
		// pause();
		return -1;
	}
	catch (...)
	{
		printf("Untyped exception!!");
		// pause();
		return -1;
	}
}
