/* +------------------------------------------------------------------------+
   |                     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 <mrpt/config/CConfigFile.h>
#include <mrpt/maps/COccupancyGridMap2D.h>
#include <mrpt/maps/CSimplePointsMap.h>
#include <mrpt/obs/CObservation2DRangeScan.h>
#include <mrpt/poses/CPose2D.h>
#include <mrpt/system/CTicTac.h>
#include <mrpt/system/filesystem.h>
#include <mrpt/system/os.h>

#include <iostream>

using namespace mrpt;
using namespace mrpt::system;
using namespace mrpt::obs;
using namespace mrpt::maps;
using namespace mrpt::poses;
using namespace std;

#include <mrpt/examples_config.h>
string myDataDir(
	MRPT_EXAMPLES_BASE_DIRECTORY +
	string("maps_gridmap_likelihood_characterization/"));

// Default .ini file:
string iniFile(myDataDir + string("config_likelihood.ini"));

// ------------------------------------------------------
//				theMainThing
// ------------------------------------------------------
void theMainThing()
{
	COccupancyGridMap2D gridMap;
	CObservation2DRangeScan obsScan;
	CTicTac tictac;

	// Load the grid map from file
	// ----------------------------------------------
	obsScan.aperture = M_2PIf;

	ASSERT_(mrpt::system::fileExists(iniFile));
	mrpt::config::CConfigFile cfgFile(iniFile);

	string bmp = cfgFile.read_string("Params", "bitmap_file", "", true);
	float res =
		cfgFile.read_float("Params", "evaluation_grid_resolution", 0.1f, true);

	float evalgrid_x_min = 0, evalgrid_x_max = 0;
	float evalgrid_y_min = 0, evalgrid_y_max = 0;

	MRPT_LOAD_CONFIG_VAR_NO_DEFAULT(evalgrid_x_min, float, cfgFile, "Params");
	MRPT_LOAD_CONFIG_VAR_NO_DEFAULT(evalgrid_x_max, float, cfgFile, "Params");
	MRPT_LOAD_CONFIG_VAR_NO_DEFAULT(evalgrid_y_min, float, cfgFile, "Params");
	MRPT_LOAD_CONFIG_VAR_NO_DEFAULT(evalgrid_y_max, float, cfgFile, "Params");

	int type_experiment = 0;
	MRPT_LOAD_CONFIG_VAR_NO_DEFAULT(type_experiment, int, cfgFile, "Params");

	// Gridmap:
	string GRIDMAP_FILE(myDataDir + bmp);

	gridMap.loadFromBitmapFile(GRIDMAP_FILE, 0.05f);
	gridMap.saveAsBitmapFile("./out_gridmap.png");

	if (type_experiment == 0)
	{
		// Simulate scan:
		// ----------------------------------------------
		obsScan.aperture = M_PIf;
		obsScan.maxRange = 80.0f;
		obsScan.rightToLeft = true;
		gridMap.laserScanSimulator(obsScan, CPose2D(0, 0, 0), 0.5f, 180);
	}
	else
	{
		// Lik. of each 2D  position
		// ------------------------------------
		obsScan.aperture = 0;
		obsScan.maxRange = 80.0f;
		obsScan.rightToLeft = true;
		obsScan.resizeScanAndAssign(1, 0.0, true);
	}

	// Set options:
	// ----------------------------------------------
	gridMap.likelihoodOptions.loadFromConfigFile(cfgFile, "LikelihoodOptions");
	gridMap.likelihoodOptions.dumpToConsole();

	// Perform simulation:
	// ----------------------------------------------
	printf("Performing simulation (saving to out_lik.txt)...");

	FILE* f = os::fopen("out_lik.txt", "wt");
	ASSERT_(f);

	CSimplePointsMap pointsMap;
	pointsMap.clear();
	pointsMap.insertPoint(0, 0);

	tictac.Tic();

	float phi = (float)0.0_deg;

	auto nullPose = CPose3D(0, 0, 0, phi, 0, 0);

	for (float y = evalgrid_y_min; y < evalgrid_y_max; y += res)
	{
		for (float x = evalgrid_x_min; x < evalgrid_x_max; x += res)
		{
			nullPose.x(x);
			nullPose.y(y);
			fprintf(
				f, "%e ",
				gridMap.computeObservationLikelihood(obsScan, nullPose));
		}  // for y
		fprintf(f, "\n");
	}  // for x

	printf("Done!\n");

	printf("Time:%fms\n", 1000.0f * tictac.Tac());

	os::fclose(f);
}

int main()
{
	try
	{
		theMainThing();

		return 0;
	}
	catch (const std::exception& e)
	{
		std::cout << "MRPT exception:\n" << e.what() << std::endl;
		return -1;
	}
	catch (...)
	{
		printf("Runtime exception!!");
		return -1;
	}
}
