/* +------------------------------------------------------------------------+
   |                     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/io/CFileGZOutputStream.h>
#include <mrpt/io/CFileOutputStream.h>
#include <mrpt/maps/CBeaconMap.h>
#include <mrpt/obs/CActionCollection.h>
#include <mrpt/obs/CActionRobotMovement2D.h>
#include <mrpt/obs/CObservationBeaconRanges.h>
#include <mrpt/obs/CSensoryFrame.h>
#include <mrpt/poses/CPoint3D.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::system;
using namespace mrpt::math;
using namespace mrpt::io;
using namespace mrpt::poses;
using namespace mrpt::config;
using namespace mrpt::obs;
using namespace mrpt::maps;
using namespace mrpt::random;
using namespace std;

int main(int argc, char** argv)
{
	try
	{
		bool showHelp = argc > 1 && !os::_strcmp(argv[1], "--help");
		bool showVersion = argc > 1 && !os::_strcmp(argv[1], "--version");

		printf(" simul-beacons - Part of the MRPT\n");
		printf(
			" MRPT C++ Library: %s - Sources timestamp: %s\n",
			MRPT_getVersion().c_str(), MRPT_getCompilationDate().c_str());

		if (showVersion) return 0;	// Program end

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

		string INI_FILENAME = std::string(argv[1]);
		ASSERT_FILE_EXISTS_(INI_FILENAME);

		CConfigFile ini(INI_FILENAME);

		getRandomGenerator().randomize();

		int i;
		char auxStr[2000];

		// Set default values:
		int nBeacons = 3;
		int nSteps = 100;
		std::string outFile("out.rawlog");
		std::string outDir("OUT");

		float min_x = -5;
		float max_x = 5;
		float min_y = -5;
		float max_y = 5;
		float min_z = 0;
		float max_z = 0;

		float odometryNoiseXY_std = 0.001f;
		float odometryBias_Y = 0;

		float minSensorDistance = 0;
		float maxSensorDistance = 10;
		float stdError = 0.04f;
		bool circularPath = true;
		int squarePathLength = 40;

		float ratio_outliers = 0;
		float ratio_outliers_first_step = 0;
		float outlier_uniform_min = 0;
		float outlier_uniform_max = 5.0;

		// Load params from INI:
		MRPT_LOAD_CONFIG_VAR(nBeacons, int, ini, "Params");
		MRPT_LOAD_CONFIG_VAR(outFile, string, ini, "Params");
		MRPT_LOAD_CONFIG_VAR(outDir, string, ini, "Params");
		MRPT_LOAD_CONFIG_VAR(nSteps, int, ini, "Params");
		MRPT_LOAD_CONFIG_VAR(circularPath, bool, ini, "Params");
		MRPT_LOAD_CONFIG_VAR(squarePathLength, int, ini, "Params");

		MRPT_LOAD_CONFIG_VAR(ratio_outliers, float, ini, "Params");
		MRPT_LOAD_CONFIG_VAR(ratio_outliers_first_step, float, ini, "Params");
		MRPT_LOAD_CONFIG_VAR(outlier_uniform_min, float, ini, "Params");
		MRPT_LOAD_CONFIG_VAR(outlier_uniform_max, float, ini, "Params");

		MRPT_LOAD_CONFIG_VAR(min_x, float, ini, "Params");
		MRPT_LOAD_CONFIG_VAR(max_x, float, ini, "Params");
		MRPT_LOAD_CONFIG_VAR(min_y, float, ini, "Params");
		MRPT_LOAD_CONFIG_VAR(max_y, float, ini, "Params");
		MRPT_LOAD_CONFIG_VAR(min_z, float, ini, "Params");

		MRPT_LOAD_CONFIG_VAR(odometryNoiseXY_std, float, ini, "Params");
		MRPT_LOAD_CONFIG_VAR(odometryBias_Y, float, ini, "Params");

		MRPT_LOAD_CONFIG_VAR(minSensorDistance, float, ini, "Params");
		MRPT_LOAD_CONFIG_VAR(maxSensorDistance, float, ini, "Params");
		MRPT_LOAD_CONFIG_VAR(stdError, float, ini, "Params");

		// Create out dir:
		mrpt::system::createDirectory(outDir);

		// ---------------------------------------------
		// Create the point-beacons:
		// ---------------------------------------------
		printf("Creating beacon map...");
		mrpt::maps::CBeaconMap beaconMap;
		for (i = 0; i < nBeacons; i++)
		{
			CBeacon b;
			TPoint3D pt3D;

			// Random coordinates:
			pt3D.x = getRandomGenerator().drawUniform(min_x, max_x);
			pt3D.y = getRandomGenerator().drawUniform(min_y, max_y);
			pt3D.z = getRandomGenerator().drawUniform(min_z, max_z);

			// Add:
			b.m_typePDF = CBeacon::pdfMonteCarlo;
			b.m_locationMC.setSize(1, TPoint3Df(pt3D.x, pt3D.y, pt3D.z));
			b.m_ID = i;
			beaconMap.push_back(b);
		}

		os::sprintf(auxStr, sizeof(auxStr), "%s/outSimMap.txt", outDir.c_str());
		beaconMap.saveToTextFile(auxStr);
		printf("Done!\n");

		// beaconMap.simulateBeaconReadings(  );

		// ---------------------------------------------
		// Simulate:
		// ---------------------------------------------
		CActionRobotMovement2D::TMotionModelOptions opts;
		CPoint3D null3D(0, 0, 0);
		opts.modelSelection = CActionRobotMovement2D::mmGaussian;
		opts.gaussianModel.a1 = 0;
		opts.gaussianModel.a2 = 0;
		opts.gaussianModel.a3 = 0;
		opts.gaussianModel.a4 = 0;
		opts.gaussianModel.minStdXY = odometryNoiseXY_std;
		opts.gaussianModel.minStdPHI = DEG2RAD(0.002f);

		os::sprintf(
			auxStr, sizeof(auxStr), "%s/%s", outDir.c_str(), outFile.c_str());
		CFileOutputStream fil(auxStr);
		CPose2D realPose;
		CPose2D incPose;
		int stopSteps = 4;

		for (i = 0; i < nSteps; i++)
		{
			printf("Generating step %i...", i);
			CSensoryFrame SF;
			CActionCollection acts;
			CActionRobotMovement2D act;
			CPose3D pose3D(realPose);

			if (i >= stopSteps)
			{
				if (circularPath)
				{
					// Circular path:
					float Ar = 5.0_deg;
					incPose = CPose2D(0.20f * cos(Ar), 0.20f * sin(Ar), Ar);
				}
				else
				{
					// Square path:
					if ((i % squarePathLength) > (squarePathLength - 5))
						incPose = CPose2D(0, 0, DEG2RAD(90.0f / 4));
					else
						incPose = CPose2D(0.20f, 0, 0);
				}
			}
			else
				incPose = CPose2D(0, 0, 0);

			// Simulate observations:
			CObservationBeaconRanges::Ptr obs =
				std::make_shared<CObservationBeaconRanges>();
			obs->minSensorDistance = minSensorDistance;
			obs->maxSensorDistance = maxSensorDistance;
			obs->stdError = stdError;

			beaconMap.simulateBeaconReadings(pose3D, null3D, *obs);

			// Corrupt with ourliers:
			float probability_corrupt =
				i == 0 ? ratio_outliers_first_step : ratio_outliers;
			for (auto& q : obs->sensedData)
			{
				if (getRandomGenerator().drawUniform(0.0f, 1.0f) <
					probability_corrupt)
				{
					q.sensedDistance += getRandomGenerator().drawUniform(
						outlier_uniform_min, outlier_uniform_max);
					if (q.sensedDistance < 0) q.sensedDistance = 0;
				}
			}

			std::cout << obs->sensedData.size() << " beacons at range";

			SF.push_back(obs);

			// Simulate actions:
			CPose2D incOdo(incPose);
			if (incPose.x() != 0 || incPose.y() != 0 || incPose.phi() != 0)
			{
				incOdo.x_incr(
					getRandomGenerator().drawGaussian1D_normalized() *
					odometryNoiseXY_std);
				incOdo.y_incr(
					odometryBias_Y +
					getRandomGenerator().drawGaussian1D_normalized() *
						odometryNoiseXY_std);
			}
			act.computeFromOdometry(incOdo, opts);
			acts.insert(act);

			// Save:
			mrpt::serialization::archiveFrom(fil) << SF << acts;

			// Next pose:
			realPose = realPose + incPose;

			printf("\n");
		}

		cout << "Data saved to directory: " << outDir << endl;
	}
	catch (const std::exception& e)
	{
		std::cout << mrpt::exception_to_str(e);
	}
	catch (...)
	{
		std::cout << "Untyped exception!";
	}
}
