/* +------------------------------------------------------------------------+
   |                     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          |
   +------------------------------------------------------------------------+ */

#include <gtest/gtest.h>
#include <mrpt/config/CConfigFile.h>
#include <mrpt/io/CFileGZInputStream.h>
#include <mrpt/maps/CMultiMetricMap.h>
#include <mrpt/maps/COccupancyGridMap2D.h>
#include <mrpt/maps/CSimpleMap.h>
#include <mrpt/obs/CRawlog.h>
#include <mrpt/random.h>
#include <mrpt/serialization/CArchive.h>
#include <mrpt/slam/CMonteCarloLocalization2D.h>
#include <mrpt/system/filesystem.h>
#include <mrpt/system/os.h>
#include <test_mrpt_common.h>

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

void run_test_pf_localization(CPose2D& meanPose, CMatrixDouble33& cov)
{
	// ------------------------------------------------------
	// The code below is a simplification of the program "pf-localization"
	// ------------------------------------------------------
	const string ini_fil =
		UNITTEST_BASEDIR() + string("/tests/montecarlo_test1.ini");
	if (!mrpt::system::fileExists(ini_fil))
	{
		cerr << "WARNING: Skipping test due to missing file: " << ini_fil
			 << "\n";
		return;
	}

	CConfigFile iniFile(ini_fil);
	std::vector<int>
		particles_count;  // Number of initial particles (if size>1, run
	// the experiments N times)

	// Load configuration:
	// -----------------------------------------
	string iniSectionName("LocalizationExperiment");

	// Mandatory entries:
	iniFile.read_vector(
		iniSectionName, "particles_count", std::vector<int>(1, 0),
		particles_count,
		/*Fail if not found*/ true);
	string RAWLOG_FILE = iniFile.read_string(
		iniSectionName, "rawlog_file", "", /*Fail if not found*/ true);

	RAWLOG_FILE = UNITTEST_BASEDIR() + string("/") + RAWLOG_FILE;

	// Non-mandatory entries:
	string MAP_FILE = iniFile.read_string(iniSectionName, "map_file", "");

	MAP_FILE = UNITTEST_BASEDIR() + string("/") + MAP_FILE;

	size_t rawlog_offset = iniFile.read_int(iniSectionName, "rawlog_offset", 0);
	int NUM_REPS = iniFile.read_int(iniSectionName, "experimentRepetitions", 1);

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

	// PDF Options:
	// ------------------
	TMonteCarloLocalizationParams pdfPredictionOptions;
	pdfPredictionOptions.KLD_params.loadFromConfigFile(iniFile, "KLD_options");

	// Metric map options:
	// -----------------------------
	TSetOfMetricMapInitializers mapList;
	mapList.loadFromConfigFile(iniFile, "MetricMap");

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

	// Load the set of metric maps to consider in the experiments:
	auto metricMap = CMultiMetricMap::Create();
	metricMap->setListOfMaps(mapList);

	getRandomGenerator().randomize();

	// Load the map (if any):
	// -------------------------
	if (MAP_FILE.size())
	{
		ASSERT_(fileExists(MAP_FILE));

		// Detect file extension:
		// -----------------------------
		string mapExt = lowerCase(extractFileExtension(
			MAP_FILE, true));  // Ignore possible .gz extensions

		if (!mapExt.compare("simplemap"))
		{
			// It's a ".simplemap":
			// -------------------------
			CFileGZInputStream f(MAP_FILE);
			mrpt::serialization::archiveFrom(f) >> simpleMap;
			ASSERT_(simpleMap.size() > 0);
			// Build metric map:
			metricMap->loadFromProbabilisticPosesAndObservations(simpleMap);
		}
		else
		{
			THROW_EXCEPTION_FMT(
				"Map file has unknown extension: '%s'", mapExt.c_str());
		}
	}

	// --------------------------
	// Load the rawlog:
	// --------------------------
	rawlog.loadFromRawLogFile(RAWLOG_FILE);
	rawlogEntries = rawlog.size();

	for (int PARTICLE_COUNT : particles_count)
	{
		// Global stats for all the experiment loops:
		vector<double> covergenceErrors;
		covergenceErrors.reserve(NUM_REPS);
		// --------------------------------------------------------------------
		//					EXPERIMENT REPETITIONS LOOP
		// --------------------------------------------------------------------
		tictacGlobal.Tic();
		for (int repetition = 0; repetition < NUM_REPS; repetition++)
		{
			CMonteCarloLocalization2D pdf(PARTICLE_COUNT);

			// PDF Options:
			pdf.options = pdfPredictionOptions;

			pdf.options.metricMap = metricMap;

			// Create the PF object:
			CParticleFilter PF;
			PF.m_options = pfOptions;

			size_t step = 0;
			rawlogEntry = 0;

			// Initialize the PDF:
			// -----------------------------
			tictac.Tic();
			if (!iniFile.read_bool(
					iniSectionName, "init_PDF_mode", false,
					/*Fail if not found*/ true))
			{
				auto grid = metricMap->mapByClass<COccupancyGridMap2D>();
				pdf.resetUniformFreeSpace(
					grid.get(), 0.7f, PARTICLE_COUNT,
					iniFile.read_float(
						iniSectionName, "init_PDF_min_x", 0, true),
					iniFile.read_float(
						iniSectionName, "init_PDF_max_x", 0, true),
					iniFile.read_float(
						iniSectionName, "init_PDF_min_y", 0, true),
					iniFile.read_float(
						iniSectionName, "init_PDF_max_y", 0, true),
					DEG2RAD(iniFile.read_float(
						iniSectionName, "init_PDF_min_phi_deg", -180)),
					DEG2RAD(iniFile.read_float(
						iniSectionName, "init_PDF_max_phi_deg", 180)));
			}
			else
				pdf.resetUniform(
					iniFile.read_float(
						iniSectionName, "init_PDF_min_x", 0, true),
					iniFile.read_float(
						iniSectionName, "init_PDF_max_x", 0, true),
					iniFile.read_float(
						iniSectionName, "init_PDF_min_y", 0, true),
					iniFile.read_float(
						iniSectionName, "init_PDF_max_y", 0, true),
					DEG2RAD(iniFile.read_float(
						iniSectionName, "init_PDF_min_phi_deg", -180)),
					DEG2RAD(iniFile.read_float(
						iniSectionName, "init_PDF_max_phi_deg", 180)),
					PARTICLE_COUNT);

			// -----------------------------
			//		Particle filter
			// -----------------------------
			CActionCollection::Ptr action;
			CSensoryFrame::Ptr observations;
			bool end = false;

			// TTimeStamp cur_obs_timestamp;

			while (rawlogEntry < (rawlogEntries - 1) && !end)
			{
				// Load pose change from the rawlog:
				// ----------------------------------------
				if (!rawlog.getActionObservationPair(
						action, observations, rawlogEntry))
					THROW_EXCEPTION("End of rawlog");

				CPose2D expectedPose;  // Ground truth

				//				if (observations->size()>0)
				//					cur_obs_timestamp =
				// observations->getObservationByIndex(0)->timestamp;

				if (step >= rawlog_offset)
				{
					// Do not execute the PF at "step=0", to let the initial PDF
					// to be
					//   reflected in the logs.
					if (step > rawlog_offset)
					{
						// ----------------------------------------
						// RUN ONE STEP OF THE PARTICLE FILTER:
						// ----------------------------------------
						tictac.Tic();

						PF.executeOn(
							pdf,
							action.get(),  // Action
							observations.get(),	 // Obs.
							&PF_stats  // Output statistics
						);
					}

					const auto [C, M] = pdf.getCovarianceAndMean();
					cov = C;
					meanPose = M;

				}  // end if rawlog_offset

				step++;

			};	// while rawlogEntries
		}  // for repetitions
	}  // end of loop for different # of particles
}

// TEST =================
TEST(MonteCarlo2D, RunSampleDataset)
{
	try
	{
		// Actual ending point:
		const CPose2D GT_endpose(15.904, -10.010, 4.93_deg);

		// Placeholder for results:
		CPose2D meanPose;
		CMatrixDouble33 cov;

		// Invoke test:
		// Give it 3 opportunities, since it might fail once for bad luck, or
		// even twice in an extreme bad luck:
		for (int op = 0; op < 3; op++)
		{
			run_test_pf_localization(meanPose, cov);

			const double final_pf_cov_trace = cov.trace();
			const CPose2D final_pf_pose = meanPose;

			bool pass1 = (final_pf_pose - GT_endpose).norm() < 0.10;
			bool pass2 = final_pf_cov_trace < 0.01;

			if (pass1 && pass2) return;	 // OK!

			// else: give it another try...
			cout << "\n*Warning: Test failed. Will give it another chance, "
					"since "
					"after all it's nondeterministic!\n";
		}

		FAIL() << "Failed to converge after 3 opportunities!!" << endl;
	}
	catch (const std::exception& e)
	{
		FAIL() << mrpt::exception_to_str(e);
	}
}
