/* +------------------------------------------------------------------------+
   |                     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/gui/CDisplayWindow3D.h>
#include <mrpt/maps/CSimplePointsMap.h>
#include <mrpt/math/geometry.h>
#include <mrpt/opengl/CPointCloud.h>
#include <mrpt/opengl/CSetOfLines.h>
#include <mrpt/opengl/stock_objects.h>
#include <mrpt/poses/CPose2D.h>
#include <mrpt/poses/CPosePDFGaussian.h>
#include <mrpt/poses/CPosePDFSOG.h>
#include <mrpt/random.h>
#include <mrpt/system/CTicTac.h>
#include <mrpt/system/CTimeLogger.h>
#include <mrpt/tfest/se2.h>

#include <iostream>

// Method explained in paper:
//  J.L. Blanco, J. Gonzalez-Jimenez, J.A. Fernandez-Madrigal,
//   "A Robust, Multi-Hypothesis Approach to Matching Occupancy Grid Maps",
//    Robotica, 2013.
// http://dx.doi.org/10.1017/S0263574712000732

// ============= PARAMETERS ===================
const size_t NUM_OBSERVATIONS_TO_SIMUL = 10;
const size_t RANSAC_MINIMUM_INLIERS = 9;  // Min. # of inliers to accept

#define LOAD_MAP_FROM_FILE 0  // 1: load from "sMAP_FILE", 0: random map.
#define SHOW_POINT_LABELS 0

const float normalizationStd = 0.15f;  // 1 sigma noise (meters)
const float ransac_mahalanobisDistanceThreshold = 5.0f;
const size_t MINIMUM_RANSAC_ITERS = 100000;

#if !LOAD_MAP_FROM_FILE
const size_t NUM_MAP_FEATS = 100;
const double MAP_SIZE_X = 50;
const double MAP_SIZE_Y = 25;
#else
// Expected format of the 2D map is, for each line (one per landmark):
//  ID X Y
const std::string sMAP_FILE = string("./DLRMap.txt");
#endif
// ==============================================

using namespace mrpt;
using namespace mrpt::math;
using namespace mrpt::random;
using namespace mrpt::maps;
using namespace mrpt::tfest;
using namespace mrpt::img;
using namespace std;

struct TObs
{
	size_t ID;	// Ground truth ID
	double x, y;
};

// ------------------------------------------------------
//				TestRANSAC
// ------------------------------------------------------
void TestRANSAC()
{
	mrpt::gui::CDisplayWindow3D win(
		"MRPT example: ransac-data-association", 800, 600);

	mrpt::system::CTimeLogger timelog;	// For dumping stats at the end
	mrpt::system::CTicTac timer;

	getRandomGenerator().randomize();  // randomize with time

	// --------------------------------
	// Load feature map:
	// --------------------------------
	CSimplePointsMap the_map;
#if LOAD_MAP_FROM_FILE
	{
		CMatrixDouble M;
		M.loadFromTextFile(sMAP_FILE);	// Launch except. on error
		ASSERT_(M.cols() == 3 && M.rows() > 2)

		const size_t nPts = M.rows();
		the_map.resize(nPts);
		for (size_t i = 0; i < nPts; i++)
			the_map.setPoint(i, M(i, 1), M(i, 2));
	}
#else
	// Generate random MAP:
	the_map.resize(NUM_MAP_FEATS);
	for (size_t i = 0; i < NUM_MAP_FEATS; i++)
	{
		the_map.setPoint(
			i, getRandomGenerator().drawUniform(0, MAP_SIZE_X),
			getRandomGenerator().drawUniform(0, MAP_SIZE_Y));
	}
#endif

	const size_t nMapPts = the_map.size();
	cout << "Loaded/generated map with " << nMapPts << " landmarks.\n";

	const size_t nObs = NUM_OBSERVATIONS_TO_SIMUL;

	mrpt::opengl::CPointCloud::Ptr gl_obs_map =
		mrpt::opengl::CPointCloud::Create();
	mrpt::opengl::CPointCloud::Ptr gl_result =
		mrpt::opengl::CPointCloud::Create();
	mrpt::opengl::CSetOfObjects::Ptr gl_obs =
		mrpt::opengl::CSetOfObjects::Create();
	mrpt::opengl::CSetOfObjects::Ptr gl_obs_txts =
		mrpt::opengl::CSetOfObjects::Create();
	mrpt::opengl::CSetOfLines::Ptr gl_lines =
		mrpt::opengl::CSetOfLines::Create();
	{
		mrpt::opengl::COpenGLScene::Ptr& scene = win.get3DSceneAndLock();

		scene->getViewport("main")->setCustomBackgroundColor(
			TColorf(0.8f, 0.8f, 0.8f));
		win.setCameraPointingToPoint(MAP_SIZE_X * 0.5, MAP_SIZE_Y * 0.5, 0);
		win.setCameraZoom(2 * MAP_SIZE_X);

		//
		scene->insert(mrpt::opengl::stock_objects::CornerXYZ());

		//
		mrpt::opengl::CPointCloud::Ptr gl_map =
			mrpt::opengl::CPointCloud::Create();
		gl_map->loadFromPointsMap(&the_map);
		gl_map->setColor(0, 0, 1);
		gl_map->setPointSize(3);

		scene->insert(gl_map);

#if SHOW_POINT_LABELS
		for (size_t i = 0; i < the_map.size(); i++)
		{
			mrpt::opengl::CText::Ptr gl_txt = mrpt::opengl::CText::Create(
				mrpt::format("%u", static_cast<unsigned int>(i)));
			double x, y;
			the_map.getPoint(i, x, y);
			gl_txt->setLocation(x + 0.05, y + 0.05, 0.01);

			scene->insert(gl_txt);
		}
#endif

		//
		scene->insert(gl_lines);

		//
		gl_obs_map->setColor(1, 0, 0);
		gl_obs_map->setPointSize(5);

		gl_result->setColor(0, 1, 0);
		gl_result->setPointSize(4);

		//
		gl_obs->insert(mrpt::opengl::stock_objects::CornerXYZ(0.6f));
		gl_obs->insert(gl_obs_map);
		gl_obs->insert(gl_obs_txts);
		scene->insert(gl_obs);
		scene->insert(gl_result);

		win.unlockAccess3DScene();
		win.repaint();
	}

	// Repeat for each set of observations in the input file
	while (win.isOpen())
	{
		// Read the observations themselves:
		vector<TObs> observations;
		observations.resize(nObs);

		const mrpt::poses::CPose2D GT_pose(
			mrpt::random::getRandomGenerator().drawUniform(
				-10, 10 + MAP_SIZE_X),
			mrpt::random::getRandomGenerator().drawUniform(
				-10, 10 + MAP_SIZE_Y),
			mrpt::random::getRandomGenerator().drawUniform(-M_PI, M_PI));

		const mrpt::poses::CPose2D GT_pose_inv = -GT_pose;

		std::vector<std::pair<size_t, float>> idxs;
		the_map.kdTreeRadiusSearch2D(GT_pose.x(), GT_pose.y(), 1000, idxs);
		ASSERT_(idxs.size() >= nObs);

		for (size_t i = 0; i < nObs; i++)
		{
			double gx, gy;
			the_map.getPoint(idxs[i].first, gx, gy);

			double lx, ly;
			GT_pose_inv.composePoint(gx, gy, lx, ly);

			observations[i].ID = idxs[i].first;
			observations[i].x = lx +
				mrpt::random::getRandomGenerator().drawGaussian1D(
					0, normalizationStd);
			observations[i].y = ly +
				mrpt::random::getRandomGenerator().drawGaussian1D(
					0, normalizationStd);
		}

		// ----------------------------------------------------
		// Generate list of individual-compatible pairings
		// ----------------------------------------------------
		TMatchingPairList all_correspondences;

		all_correspondences.reserve(nMapPts * nObs);

		// ALL possibilities:
		for (size_t j = 0; j < nObs; j++)
		{
			TMatchingPair match;

			match.localIdx = j;
			match.local.x = observations[j].x;
			match.local.y = observations[j].y;

			for (size_t i = 0; i < nMapPts; i++)
			{
				match.globalIdx = i;
				the_map.getPoint(i, match.global.x, match.global.y);

				all_correspondences.push_back(match);
			}
		}
		cout << "Generated " << all_correspondences.size()
			 << " potential pairings.\n";

		// ----------------------------------------------------
		//  Run RANSAC-based D-A
		// ----------------------------------------------------
		timelog.enter("robustRigidTransformation");
		timer.Tic();

		mrpt::tfest::TSE2RobustParams params;
		mrpt::tfest::TSE2RobustResult results;

		params.ransac_minSetSize =
			RANSAC_MINIMUM_INLIERS;	 // ransac_minSetSize (to add the solution
		// to the SOG)
		params.ransac_maxSetSize =
			all_correspondences
				.size();  // ransac_maxSetSize: Test with all data points
		params.ransac_mahalanobisDistanceThreshold =
			ransac_mahalanobisDistanceThreshold;
		params.ransac_nSimulations = 0;	 // 0=auto
		params.ransac_fuseByCorrsMatch = true;
		params.ransac_fuseMaxDiffXY = 0.01f;
		params.ransac_fuseMaxDiffPhi = 0.1_deg;
		params.ransac_algorithmForLandmarks = true;
		params.probability_find_good_model = 0.999999;
		params.ransac_min_nSimulations =
			MINIMUM_RANSAC_ITERS;  // (a lower limit to the auto-detected value
		// of ransac_nSimulations)
		params.verbose = true;

		// Run ransac data-association:
		mrpt::tfest::se2_l2_robust(
			all_correspondences, normalizationStd, params, results);

		timelog.leave("robustRigidTransformation");

		mrpt::poses::CPosePDFSOG& best_poses = results.transformation;
		TMatchingPairList& out_best_pairings = results.largestSubSet;

		const double tim = timer.Tac();
		cout << "RANSAC time: " << mrpt::system::formatTimeInterval(tim)
			 << endl;

		cout << "# of SOG modes: " << best_poses.size() << endl;
		cout << "Best match has " << out_best_pairings.size() << " features:\n";
		for (size_t i = 0; i < out_best_pairings.size(); i++)
			cout << out_best_pairings[i].globalIdx << " <-> "
				 << out_best_pairings[i].localIdx << endl;
		cout << endl;

		// Generate "association vector":
		vector<int> obs2map_pairings(nObs, -1);
		for (size_t i = 0; i < out_best_pairings.size(); i++)
			obs2map_pairings[out_best_pairings[i].localIdx] =
				out_best_pairings[i].globalIdx == ((unsigned int)-1)
				? -1
				: out_best_pairings[i].globalIdx;

		cout << "1\n";
		for (size_t i = 0; i < nObs; i++)
			cout << obs2map_pairings[i] << " ";
		cout << endl;

		gl_result->clear();

		// Reconstruct the SE(2) transformation for these pairings:
		mrpt::poses::CPosePDFGaussian solution_pose;
		mrpt::tfest::se2_l2(out_best_pairings, solution_pose);

		// Normalized covariance: scale!
		solution_pose.cov *= square(normalizationStd);

		cout << "Solution pose: " << solution_pose.mean << endl;
		cout << "Ground truth pose: " << GT_pose << endl;

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

			win.addTextMessage(
				5, 5,
				"Blue: map landmarks | Red: Observations | White lines: Found "
				"correspondences",
				0);

			//
			gl_obs_map->clear();
			for (size_t k = 0; k < nObs; k++)
				gl_obs_map->insertPoint(
					observations[k].x, observations[k].y, 0.0);

			gl_obs->setPose(solution_pose.mean);

#if SHOW_POINT_LABELS
			gl_obs_txts->clear();
			for (size_t i = 0; i < nObs; i++)
			{
				mrpt::opengl::CText::Ptr gl_txt = mrpt::opengl::CText::Create(
					mrpt::format("%u", static_cast<unsigned int>(i)));
				const double x = observations[i].x;
				const double y = observations[i].y;
				gl_txt->setLocation(x + 0.05, y + 0.05, 0.01);
				gl_obs_txts->insert(gl_txt);
			}
#endif

			//
			gl_lines->clear();
			double sqerr = 0;
			size_t nPairs = 0;
			for (size_t k = 0; k < nObs; k++)
			{
				int map_idx = obs2map_pairings[k];
				if (map_idx < 0) continue;
				nPairs++;

				double map_x, map_y;
				the_map.getPoint(map_idx, map_x, map_y);

				double obs_x, obs_y;
				solution_pose.mean.composePoint(
					observations[k].x, observations[k].y, obs_x, obs_y);

				const double z = 0;

				gl_lines->appendLine(map_x, map_y, 0, obs_x, obs_y, z);

				sqerr += mrpt::math::distanceSqrBetweenPoints<double>(
					map_x, map_y, obs_x, obs_y);
			}

			win.addTextMessage(
				5, 20, "Ground truth pose    : " + GT_pose.asString(), 1);
			win.addTextMessage(
				5, 35,
				"RANSAC estimated pose: " + solution_pose.mean.asString() +
					mrpt::format(" | RMSE=%f", (nPairs ? sqerr / nPairs : 0.0)),
				2);

			win.unlockAccess3DScene();
			win.repaint();

			cout << "nPairings: " << nPairs
				 << " RMSE = " << (nPairs ? sqerr / nPairs : 0.0) << endl;

			win.waitForKey();
		}

	}  // end of for each set of observations
}

// ------------------------------------------------------
//						MAIN
// ------------------------------------------------------
int main()
{
	try
	{
		TestRANSAC();
		return 0;
	}
	catch (const std::exception& e)
	{
		std::cerr << "MRPT error: " << mrpt::exception_to_str(e) << std::endl;
		return -1;
	}
	catch (...)
	{
		printf("Untyped exception!!");
		return -1;
	}
}
