/* +------------------------------------------------------------------------+
   |                     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 "hmtslam-precomp.h"  // Precomp header
//
#include <mrpt/io/CFileStream.h>
#include <mrpt/random.h>
#include <mrpt/system/CTicTac.h>
#include <mrpt/system/os.h>

#include <memory>

using namespace mrpt::slam;
using namespace mrpt::hmtslam;
using namespace mrpt::random;
using namespace mrpt::poses;
using namespace mrpt::system;
using namespace std;

/*---------------------------------------------------------------

						CHMTSLAM_TBI

	Topological Bayesian Inference (TBI) process within HMT-SLAM

  ---------------------------------------------------------------*/
void CHMTSLAM::thread_TBI()
{
	CHMTSLAM* obj = this;
	CTicTac tictac;

	// Seems that must be called in each thread??
	if (obj->m_options.random_seed)
		getRandomGenerator().randomize(obj->m_options.random_seed);
	else
		getRandomGenerator().randomize();

	try
	{
		// Start thread:
		// -------------------------
		MRPT_LOG_DEBUG_STREAM(
			"[thread_TBI] Thread started (ID=" << std::this_thread::get_id());

		// --------------------------------------------
		//    The main loop
		//  Executes until termination is signaled
		// --------------------------------------------
		while (!obj->m_terminateThreads)
		{
			std::this_thread::sleep_for(100ms);
		};	// end while execute thread

		// Finish thread:
		// -------------------------
		obj->logFmt(mrpt::system::LVL_DEBUG, "[thread_TBI] Thread finished");
		obj->m_terminationFlag_TBI = true;
	}
	catch (const std::exception& e)
	{
		obj->m_terminationFlag_TBI = true;

		// Release semaphores:

		obj->logFmt(mrpt::system::LVL_ERROR, "%s", e.what());

		// DEBUG: Terminate application:
		obj->m_terminateThreads = true;
	}
	catch (...)
	{
		obj->m_terminationFlag_TBI = true;

		obj->logFmt(
			mrpt::system::LVL_ERROR,
			"\n---------------------- EXCEPTION CAUGHT! ---------------------\n"
			" In CHierarchicalMappingFramework::thread_TBI. Unexpected runtime "
			"error!!\n");

		// Release semaphores:

		// DEBUG: Terminate application:
		obj->m_terminateThreads = true;
	}
}

/*---------------------------------------------------------------

						TBI_main_method

	Topological Bayesian Inference (TBI) process within HMT-SLAM

  ---------------------------------------------------------------*/
CHMTSLAM::TMessageLSLAMfromTBI::Ptr CHMTSLAM::TBI_main_method(
	CLocalMetricHypothesis* LMH, const CHMHMapNode::TNodeID& areaID)
{
	MRPT_START

	auto* obj = (CHMTSLAM*)LMH->m_parent.get();

	const THypothesisID LMH_ID = LMH->m_ID;

	// Lock the map:
	std::lock_guard<std::mutex>(obj->m_map_cs);

	TMessageLSLAMfromTBI::Ptr msg = std::make_shared<TMessageLSLAMfromTBI>();

	// Fill out easy data:
	msg->hypothesisID = LMH_ID;
	msg->cur_area = areaID;

	// get a pointer to the current area:
	const CHMHMapNode::Ptr currentArea = obj->m_map.getNodeByID(areaID);
	ASSERT_(currentArea);

	obj->logFmt(
		mrpt::system::LVL_DEBUG, "[TBI] Request for area id=%i\n", (int)areaID);

	// --------------------------------------------------------
	// 1) Use bounding-boxes to get a first list of candidates
	//    The candidates are saved in "msg->loopClosureData"
	// -------------------------------------------------------
	// But first: if the areas are within the LMH, then we have to update the
	// maps in the HMAP!
	if (LMH->m_neighbors.find(areaID) != LMH->m_neighbors.end())
	{
		// Update:
		LMH->updateAreaFromLMH(areaID);
	}

	for (auto a = obj->m_map.begin(); a != obj->m_map.end(); ++a)
	{
		// Only for other areas!
		if (a->first == areaID) continue;

		// Test hypothesis: LMH_ID
		if (a->second->m_hypotheses.has(LMH_ID))
		{
			// Not neighbors:
			if (a->second->isNeighbor(areaID, LMH_ID)) continue;

			// OK, check:
			// But first: if the areas are within the LMH, then we have to
			// update the maps in the HMAP!
			if (LMH->m_neighbors.find(a->first) != LMH->m_neighbors.end())
			{
				// Update:
				LMH->updateAreaFromLMH(a->first);
			}

			// Compute it:
			double match = obj->m_map.computeOverlapProbabilityBetweenNodes(
				areaID,	 // From
				a->first,  // To
				LMH_ID);

			obj->logFmt(
				mrpt::system::LVL_DEBUG, "[TBI] %i-%i -> overlap prob=%f\n",
				(int)areaID, (int)a->first, match);

			if (match > 0.9)
			{
				// Initialize the new entry in "msg->loopClosureData" for the
				// areas:
				//  "areaID" <-> "a->first"
				TMessageLSLAMfromTBI::TBI_info& tbi_info =
					msg->loopClosureData[a->first];

				tbi_info.log_lik = 0;
				tbi_info.delta_new_cur.clear();
			}
		}
	}  // end for each node in the graph.

	// ----------------------------------------------------
	// 2) Use the TBI engines
	// ----------------------------------------------------
	std::set<CHMHMapNode::TNodeID> lstNodesToErase;
	{
		std::lock_guard<std::mutex> lock(obj->m_topLCdets_cs);

		for (auto it = obj->m_topLCdets.begin(); it != obj->m_topLCdets.end();
			 ++it)
		{
			for (auto& candidate : msg->loopClosureData)
			{
				// If the current log_lik of this area is reaaaally low, we
				// could skip the computation with other LC detectors...
				// ----------------------------------------------------------------------------------------------------------------
				// TODO: ...

				// Proceed:
				// ----------------------------------------------------------------------------------------------------------------
				const CHMHMapNode::Ptr refArea =
					obj->m_map.getNodeByID(candidate.first);
				double this_log_lik;

				// get the output from this LC detector:
				CPose3DPDF::Ptr pdf = (*it)->computeTopologicalObservationModel(
					LMH->m_ID, currentArea, refArea, this_log_lik);

				// Add to the output:
				candidate.second.log_lik += this_log_lik;

				// This is because not all LC detector MUST return a pose PDF
				// (i.e. image-based detectors)
				if (pdf)
				{
					ASSERT_(IS_CLASS(*pdf, CPose3DPDFSOG));
					CPose3DPDFSOG::Ptr SOG =
						std::dynamic_pointer_cast<CPose3DPDFSOG>(pdf);

					// Mix (append) the modes, if any:
					if (SOG->size() > 0)
						candidate.second.delta_new_cur.appendFrom(*SOG);
					else
						lstNodesToErase.insert(candidate.first);
				}
			}  // end for each candidate area
		}  // end for each LC detector

	}  // end of m_topLCdets_cs lock

	// Delete candidates which had no PDF when they should.
	for (unsigned long it : lstNodesToErase)
		msg->loopClosureData.erase(it);

	obj->logFmt(
		mrpt::system::LVL_DEBUG, "[TBI_main] Done. %u candidates found.\n",
		(unsigned int)msg->loopClosureData.size());

	return msg;
	MRPT_END
}
