/* +------------------------------------------------------------------------+
   |                     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/gui/CDisplayWindow3D.h>
#include <mrpt/io/CFileGZInputStream.h>
#include <mrpt/io/CFileGZOutputStream.h>
#include <mrpt/maps/CLandmarksMap.h>
#include <mrpt/obs/CActionCollection.h>
#include <mrpt/obs/CActionRobotMovement3D.h>
#include <mrpt/obs/CObservationBearingRange.h>
#include <mrpt/obs/CSensoryFrame.h>
#include <mrpt/opengl/CGridPlaneXY.h>
#include <mrpt/opengl/CSetOfLines.h>
#include <mrpt/opengl/CSphere.h>
#include <mrpt/opengl/stock_objects.h>
#include <mrpt/poses/CPose3D.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::math;
using namespace mrpt::obs;
using namespace mrpt::maps;
using namespace mrpt::random;
using namespace mrpt::system;
using namespace mrpt::io;
using namespace mrpt::opengl;
using namespace mrpt::poses;
using namespace mrpt::config;
using namespace mrpt::serialization;
using namespace mrpt::vision;
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-landmarks - 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);

		const int random_seed = ini.read_int("Params", "random_seed", 0);

		if (random_seed != 0) getRandomGenerator().randomize(random_seed);
		else
			getRandomGenerator().randomize();

		// Set default values:
		unsigned int nLandmarks = 3;
		unsigned 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 odometryNoisePhi_std_deg = 0.01f;
		float odometryNoisePitch_std_deg = 0.01f;
		float odometryNoiseRoll_std_deg = 0.01f;

		float minSensorDistance = 0;
		float maxSensorDistance = 10;
		float fieldOfView_deg = 180.0f;

		float sensorPose_x = 0;
		float sensorPose_y = 0;
		float sensorPose_z = 0;
		float sensorPose_yaw_deg = 0;
		float sensorPose_pitch_deg = 0;
		float sensorPose_roll_deg = 0;

		float stdRange = 0.01f;
		float stdYaw_deg = 0.1f;
		float stdPitch_deg = 0.1f;

		bool sensorDetectsIDs = true;

		bool circularPath = true;
		bool random6DPath = false;
		size_t squarePathLength = 40;

		// Load params from INI:
		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(random6DPath, bool, ini, "Params");
		MRPT_LOAD_CONFIG_VAR(squarePathLength, int, ini, "Params");

		MRPT_LOAD_CONFIG_VAR(sensorDetectsIDs, bool, ini, "Params");

		MRPT_LOAD_CONFIG_VAR(odometryNoiseXY_std, float, ini, "Params");
		MRPT_LOAD_CONFIG_VAR(odometryNoisePhi_std_deg, float, ini, "Params");
		MRPT_LOAD_CONFIG_VAR(odometryNoisePitch_std_deg, float, ini, "Params");
		MRPT_LOAD_CONFIG_VAR(odometryNoiseRoll_std_deg, float, ini, "Params");

		MRPT_LOAD_CONFIG_VAR(sensorPose_x, float, ini, "Params");
		MRPT_LOAD_CONFIG_VAR(sensorPose_y, float, ini, "Params");
		MRPT_LOAD_CONFIG_VAR(sensorPose_z, float, ini, "Params");
		MRPT_LOAD_CONFIG_VAR(sensorPose_yaw_deg, float, ini, "Params");
		MRPT_LOAD_CONFIG_VAR(sensorPose_pitch_deg, float, ini, "Params");
		MRPT_LOAD_CONFIG_VAR(sensorPose_roll_deg, float, ini, "Params");

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

		bool show_in_3d = false;
		MRPT_LOAD_CONFIG_VAR(show_in_3d, bool, ini, "Params");

		MRPT_LOAD_CONFIG_VAR(stdRange, float, ini, "Params");
		MRPT_LOAD_CONFIG_VAR(stdYaw_deg, float, ini, "Params");
		MRPT_LOAD_CONFIG_VAR(stdPitch_deg, float, ini, "Params");

		float stdYaw = DEG2RAD(stdYaw_deg);
		float stdPitch = DEG2RAD(stdPitch_deg);

		float odometryNoisePhi_std = DEG2RAD(odometryNoisePhi_std_deg);
		float odometryNoisePitch_std = DEG2RAD(odometryNoisePitch_std_deg);
		float odometryNoiseRoll_std = DEG2RAD(odometryNoiseRoll_std_deg);

		float fieldOfView = DEG2RAD(fieldOfView_deg);

		CPose3D sensorPoseOnRobot(
			sensorPose_x, sensorPose_y, sensorPose_z,
			DEG2RAD(sensorPose_yaw_deg), DEG2RAD(sensorPose_pitch_deg),
			DEG2RAD(sensorPose_roll_deg));

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

		// ---------------------------------------------
		// Create the point-beacons:
		// ---------------------------------------------
		printf("Creating landmark map...");
		mrpt::maps::CLandmarksMap landmarkMap;
		int randomSetCount = 0;
		int uniqueIds = 1;

		// Process each of the "RANDOMSET_%i" found:
		do
		{
			string sectName = format("RANDOMSET_%i", ++randomSetCount);

			nLandmarks = 0;
			MRPT_LOAD_CONFIG_VAR(nLandmarks, uint64_t, ini, sectName);
			MRPT_LOAD_CONFIG_VAR(min_x, float, ini, sectName);
			MRPT_LOAD_CONFIG_VAR(max_x, float, ini, sectName);
			MRPT_LOAD_CONFIG_VAR(min_y, float, ini, sectName);
			MRPT_LOAD_CONFIG_VAR(max_y, float, ini, sectName);
			MRPT_LOAD_CONFIG_VAR(min_z, float, ini, sectName);
			MRPT_LOAD_CONFIG_VAR(max_z, float, ini, sectName);

			for (size_t i = 0; i < nLandmarks; i++)
			{
				CLandmark LM;
				CPointPDFGaussian pt3D;

				// Random coordinates:
				pt3D.mean = CPoint3D(
					getRandomGenerator().drawUniform(min_x, max_x),
					getRandomGenerator().drawUniform(min_y, max_y),
					getRandomGenerator().drawUniform(min_z, max_z));

				// Add:
				LM.createOneFeature();
				LM.features[0].type = featBeacon;
				LM.ID = uniqueIds++;
				LM.setPose(pt3D);

				landmarkMap.landmarks.push_back(LM);
			}

			if (nLandmarks)
				cout << nLandmarks << " generated for the 'randomset' "
					 << randomSetCount << endl;

		} while (nLandmarks);

		landmarkMap.saveToTextFile(
			format("%s/%s_ground_truth.txt", outDir.c_str(), outFile.c_str()));
		printf("Done!\n");

		// ---------------------------------------------
		// Simulate:
		// ---------------------------------------------
		size_t nWarningsNoSight = 0;

		CActionRobotMovement2D::TMotionModelOptions opts;
		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 = odometryNoisePhi_std;

		// Output rawlog, gz-compressed.
		CFileGZOutputStream fil(
			format("%s/%s", outDir.c_str(), outFile.c_str()));
		CPose3D realPose;

		const size_t N_STEPS_STOP_AT_THE_BEGINNING = 4;

		CMatrixDouble GT_path;

		for (size_t i = 0; i < nSteps; i++)
		{
			cout << "Generating step " << i << "...\n";
			CSensoryFrame SF;
			CActionCollection acts;

			CPose3D incPose3D;
			bool incPose_is_3D = random6DPath;

			if (i >= N_STEPS_STOP_AT_THE_BEGINNING)
			{
				if (random6DPath)
				{  // 3D path
					const double Ar = 3.0_deg;
					TPose3D Ap =
						TPose3D(0.20 * cos(Ar), 0.20 * sin(Ar), 0, Ar, 0, 0);
					// Ap.z  += getRandomGenerator().drawGaussian1D(0,0.05);
					Ap.yaw += getRandomGenerator().drawGaussian1D(0, 0.2_deg);
					Ap.pitch += getRandomGenerator().drawGaussian1D(0, 2.0_deg);
					Ap.roll += getRandomGenerator().drawGaussian1D(0, 4.0_deg);

					incPose3D = CPose3D(Ap);
				}
				else
				{  // 2D path:
					if (circularPath)
					{
						// Circular path:
						float Ar = 5.0_deg;
						incPose3D = CPose3D(
							CPose2D(0.20f * cos(Ar), 0.20f * sin(Ar), Ar));
					}
					else
					{
						// Square path:
						if ((i % squarePathLength) > (squarePathLength - 5))
							incPose3D =
								CPose3D(CPose2D(0, 0, DEG2RAD(90.0f / 4)));
						else
							incPose3D = CPose3D(CPose2D(0.20f, 0, 0));
					}
				}
			}
			else
			{
				// Robot is still at the beginning:
				incPose3D = CPose3D(0, 0, 0, 0, 0, 0);
			}

			// Simulate observations:
			CObservationBearingRange::Ptr obs =
				std::make_shared<CObservationBearingRange>();

			obs->minSensorDistance = minSensorDistance;
			obs->maxSensorDistance = maxSensorDistance;
			obs->fieldOfView_yaw = fieldOfView;
			obs->fieldOfView_pitch = fieldOfView;
			obs->sensorLocationOnRobot = sensorPoseOnRobot;

			landmarkMap.simulateRangeBearingReadings(
				realPose, sensorPoseOnRobot, *obs,
				sensorDetectsIDs,  // wheter to identy landmarks
				stdRange, stdYaw, stdPitch);

			// Keep the GT of the robot pose:
			GT_path.setSize(i + 1, 6);
			for (size_t k = 0; k < 6; k++)
				GT_path(i, k) = realPose[k];

			cout << obs->sensedData.size() << " landmarks in sight";

			if (obs->sensedData.empty()) nWarningsNoSight++;

			SF.push_back(obs);

			// Simulate odometry, from "incPose3D" with noise:
			if (!incPose_is_3D)
			{  // 2D odometry:
				CActionRobotMovement2D act;
				CPose2D incOdo(incPose3D);
				if (incPose3D.x() != 0 || incPose3D.y() != 0 ||
					incPose3D.yaw() != 0)
				{
					incOdo.x_incr(
						getRandomGenerator().drawGaussian1D_normalized() *
						odometryNoiseXY_std);
					incOdo.y_incr(
						getRandomGenerator().drawGaussian1D_normalized() *
						odometryNoiseXY_std);
					incOdo.phi_incr(
						getRandomGenerator().drawGaussian1D_normalized() *
						odometryNoisePhi_std);
				}
				act.computeFromOdometry(incOdo, opts);
				acts.insert(act);
			}
			else
			{  // 3D odometry:
				CActionRobotMovement3D act;
				act.estimationMethod = CActionRobotMovement3D::emOdometry;

				CPose3D noisyIncPose = incPose3D;

				if (incPose3D.x() != 0 || incPose3D.y() != 0 ||
					incPose3D.yaw() != 0)
				{
					noisyIncPose.x_incr(
						getRandomGenerator().drawGaussian1D_normalized() *
						odometryNoiseXY_std);
					noisyIncPose.y_incr(
						getRandomGenerator().drawGaussian1D_normalized() *
						odometryNoiseXY_std);
					noisyIncPose.z_incr(
						getRandomGenerator().drawGaussian1D_normalized() *
						odometryNoiseXY_std);
					noisyIncPose.setYawPitchRoll(
						noisyIncPose.yaw() +
							getRandomGenerator().drawGaussian1D_normalized() *
								odometryNoisePhi_std,
						noisyIncPose.pitch() +
							getRandomGenerator().drawGaussian1D_normalized() *
								odometryNoisePitch_std,
						noisyIncPose.roll() +
							getRandomGenerator().drawGaussian1D_normalized() *
								odometryNoiseRoll_std);
				}

				act.poseChange.mean = noisyIncPose;
				act.poseChange.cov.setIdentity();

				act.poseChange.cov(0, 0) = act.poseChange.cov(1, 1) =
					act.poseChange.cov(2, 2) = square(odometryNoiseXY_std);
				act.poseChange.cov(3, 3) = square(odometryNoisePhi_std);
				act.poseChange.cov(4, 4) = square(odometryNoisePitch_std);
				act.poseChange.cov(5, 5) = square(odometryNoiseRoll_std);

				acts.insert(act);
			}

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

			// Next pose:
			realPose = realPose + incPose3D;

			cout << endl;
		}

		// Save the ground truth for the robot poses as well:
		GT_path.saveToTextFile(
			format(
				"%s/%s_ground_truth_robot_path.txt", outDir.c_str(),
				outFile.c_str()),
			MATRIX_FORMAT_FIXED, true,
			"% Columns are: x(m) y(m) z(m) yaw(rad) pitch(rad) roll(rad)\n");

		cout << "Data saved to directory: " << outDir << endl;

		if (nWarningsNoSight)
			cout
				<< "WARNING: " << nWarningsNoSight
				<< " observations contained zero landmarks in the sensor range."
				<< endl;

		// Optionally, display in 3D:
		if (show_in_3d && GT_path.rows() > 1)
		{
#if MRPT_HAS_OPENGL_GLUT && MRPT_HAS_WXWIDGETS
			mrpt::gui::CDisplayWindow3D win("Final simulation", 400, 300);

			mrpt::opengl::COpenGLScene::Ptr& scene = win.get3DSceneAndLock();

			scene->insert(mrpt::opengl::CGridPlaneXY::Create(
				min_x - 10, max_x + 10, min_y - 10, max_y + 10, 0));
			scene->insert(mrpt::opengl::stock_objects::CornerXYZ());

			// Insert all landmarks:
			for (auto it = landmarkMap.landmarks.begin();
				 it != landmarkMap.landmarks.end(); ++it)
			{
				mrpt::opengl::CSphere::Ptr lm = mrpt::opengl::CSphere::Create();
				lm->setColor(1, 0, 0);
				lm->setRadius(0.1f);
				lm->setLocation(it->pose_mean);
				lm->setName(format("LM#%u", (unsigned)it->ID));
				// lm->enableShowName(true);
				scene->insert(lm);
			}

			// Insert all robot poses:
			const size_t N = GT_path.rows();
			mrpt::opengl::CSetOfLines::Ptr pathLines =
				mrpt::opengl::CSetOfLines::Create();
			pathLines->setColor(0, 0, 1, 0.5);
			pathLines->setLineWidth(3.0);
			pathLines->resize(N - 1);

			for (size_t i = 0; i < N - 1; i++)
				pathLines->setLineByIndex(
					i, GT_path(i, 0), GT_path(i, 1), GT_path(i, 2),
					GT_path(i + 1, 0), GT_path(i + 1, 1), GT_path(i + 1, 2));

			scene->insert(pathLines);

			for (size_t i = 0; i < N; i++)
			{
				mrpt::opengl::CSetOfObjects::Ptr corner =
					mrpt::opengl::stock_objects::CornerXYZ();
				corner->setScale(0.2f);
				corner->setPose(TPose3D(
					GT_path(i, 0), GT_path(i, 1), GT_path(i, 2), GT_path(i, 3),
					GT_path(i, 4), GT_path(i, 5)));
				scene->insert(corner);
			}

			win.unlockAccess3DScene();
			win.forceRepaint();

			cout << "Press any key or close the 3D window to exit." << endl;
			win.waitForKey();

#endif	// MRPT_HAS_OPENGL_GLUT  && MRPT_HAS_WXWIDGETS
		}
	}
	catch (const std::exception& e)
	{
		std::cout << mrpt::exception_to_str(e);
	}
	catch (...)
	{
		std::cout << "Untyped exception!";
	}
}
