/* +------------------------------------------------------------------------+
   |                     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/hmtslam/CRobotPosesGraph.h>
#include <mrpt/io/CFileStream.h>
#include <mrpt/poses/CPose3DPDFParticles.h>
#include <mrpt/random.h>
#include <mrpt/system/CTicTac.h>
#include <mrpt/system/os.h>

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

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

						perform_TLC

	Topological Loop Closure: Performs all the required operations
	to close a loop between two areas which have been determined
	to be the same.

  ---------------------------------------------------------------*/
void CHMTSLAM::perform_TLC(
	CLocalMetricHypothesis& LMH,
	const CHMHMapNode::TNodeID Ai,	// areaInLMH,
	const CHMHMapNode::TNodeID Ae,	// areaLoopClosure,
	const mrpt::poses::CPose3DPDFGaussian& pose_i_wrt_e)
{
	MRPT_START
	ASSERT_(Ai != Ae);

	std::lock_guard<std::mutex> locker(LMH.m_robotPosesGraph.lock);

	logFmt(
		mrpt::system::LVL_DEBUG, "[perform_TLC] TLC of areas: %u <-> %u \n",
		(unsigned)Ai, (unsigned)Ae);

	// * Verify a1 \in LMH & a2 \notin LMH
	// ----------------------------------------------------------------------
	ASSERT_(LMH.m_neighbors.count(Ai) == 1);
	ASSERT_(LMH.m_neighbors.count(Ae) == 0);

	// * Get the old pose reference of the area which will desapear:
	// ----------------------------------------------------------------------
	TPoseID poseID_Ai_origin;
	m_map.getNodeByID(Ai)->m_annotations.getElemental(
		NODE_ANNOTATION_REF_POSEID, poseID_Ai_origin, LMH.m_ID, true);
	//	TPoseID		poseID_new_origin;
	//	m_map.getNodeByID(Ae)->m_annotations.getElemental(
	// NODE_ANNOTATION_REF_POSEID,poseID_new_origin, LMH.m_ID, true);

	// * Make a list Li of pose IDs in Ai, which need a change in coordinates:
	// ----------------------------------------------------------------------------------------------
	// TPoseIDSet  Li;
	// for (map<TPoseID,CHMHMapNode::TNodeID>::const_iterator
	// it=LMH.m_nodeIDmemberships.begin();it!=LMH.m_nodeIDmemberships.end();++it)
	//	if(it->second==Ai)
	//		Li.insert(it->first);

	// * Change all poses in "Ai" from "p" to " 0 (-) pose1wrt2 (+) (p (-)
	// oldRef )"
	// ----------------------------------------------------------------------------------------------
	// For all the particles
	// for ( CLocalMetricHypothesis::CParticleList::const_iterator it =
	// LMH.m_particles.begin(); it!=LMH.m_particles.end(); ++it)
	//{
	//	// Touch only the affected poseIDs:
	//	for (TPoseIDSet::const_iterator n=Li.begin();n!=Li.end();++n)
	//	{
	//		map<TPoseID,CPose3D>::iterator itPos = it->d->robotPoses.find(*n);
	//		ASSERT_(itPos!=it->d->robotPoses.end());
	//		itPos->second = Apose_ei + (itPos->second - oldRef);
	//	}
	//}

	// Change coords in incr. partitioning as well:
	// CSimpleMap *SFseq =
	// LMH.m_robotPosesGraph.partitioner.getSequenceOfFrames();
	// for (std::map<uint32_t,TPoseID>::const_iterator
	// it=LMH.m_robotPosesGraph.idx2pose.begin();it!=LMH.m_robotPosesGraph.idx2pose.end();++it)
	//{
	//	// Only if the pose has been affected:
	//	if (Li.count(it->second)==0) continue;

	//	CPose3DPDF::Ptr		pdf;
	//	CSensoryFrame::Ptr	sf;
	//	SFseq->get( it->first, pdf, sf);

	//	// Copy from particles:
	//	CPose3DPDFParticles::Ptr pdfParts = CPose3DPDFParticles::Ptr(pdf);
	//	LMH.getPoseParticles( it->second, *pdfParts);
	//}

	// * Mark all poses in LMH \in Ai as being of "Ae":
	// ----------------------------------------------------------------------------------------------
	for (auto& m_nodeIDmembership : LMH.m_nodeIDmemberships)
		if (m_nodeIDmembership.second == Ai) m_nodeIDmembership.second = Ae;

	// * Replace "Ai" by "Ae" in the list of neighbors areas:
	// ----------------------------------------------------------------------------------------------
	LMH.m_neighbors.erase(LMH.m_neighbors.find(Ai));
	LMH.m_neighbors.insert(Ae);

	// * Reflect the change of coordinates in all existing arcs from/to "Ai",
	// which are not part of the LMH (list of neighbors)
	//     "p" to " 0 (-) pose1wrt2 (+) (p (-) oldRef )"...
	//  NOTE: Arcs to "Ai" must be replaced by arcs to "Ae"
	// ----------------------------------------------------------------------------------------------
	{
		TArcList lstArcs;
		m_map.getNodeByID(Ai)->getArcs(lstArcs, LMH.m_ID);
		for (auto arc = lstArcs.begin(); arc != lstArcs.end(); ++arc)
		{
			// TODO ...
		}
	}

	// * The node for "area Ai" must be deleted in the HMAP, and all its arcs.
	// ----------------------------------------------------------------------------------------------
	{
		TArcList lstArcs;
		m_map.getNodeByID(Ai)->getArcs(lstArcs);
		for (auto& lstArc : lstArcs)
			lstArc.reset();	 // The "delete" will automatically remove the entry
		// in "m_map". Other smrtpnts will be cleared too.

		m_map.getNodeByID(Ai).reset();	// The "delete" will automatically
		// remove the entry in "m_map"
	}

	// * Merge Ae into the LMH: This includes the change of coordinates to those
	// used now in "Ai" and the rest of neighbors!
	// ----------------------------------------------------------------------------------------------
	CPose3DPDFParticles pdfAiRef;
	LMH.getPoseParticles(poseID_Ai_origin, pdfAiRef);
	CPose3D AiRef;
	pdfAiRef.getMean(AiRef);
	const CPose3D& Apose_ie = pose_i_wrt_e.mean;
	// const CPose3D  Apose_ei = -Apose_ie;
	const CPose3D AeRefInLMH = AiRef + Apose_ie;

	CRobotPosesGraph::Ptr AePG =
		m_map.getNodeByID(Ae)->m_annotations.getAs<CRobotPosesGraph>(
			NODE_ANNOTATION_POSES_GRAPH, LMH.m_ID);

	// For each pose in Ae:
	for (auto it = AePG->begin(); it != AePG->end(); ++it)
	{
		const TPoseID poseId = it->first;
		const TPoseInfo& pi = it->second;

		//  Insert the observations into the metric maps:
		ASSERT_(LMH.m_particles.size() == pi.pdf.size());

		for (size_t i = 0; i < pi.pdf.size(); i++)
		{
			// Transport coordinates:
			const CPose3D p = CPose3D(pi.pdf.m_particles[i].d);
			LMH.m_particles[i].d->robotPoses[poseId] = AeRefInLMH + p;
			// pi.sf.insertObservationsInto( &LMH.m_particles[i].d->metricMaps,
			// pi.pdf.m_particles[i].d );
		}

		// Add node membership:
		LMH.m_nodeIDmemberships[poseId] = Ae;

		// Now, insert the SF
		LMH.m_SFs[poseId] = pi.sf;

		// Also add to the list of poses pending to be evaluated by the AA:
		LMH.m_posesPendingAddPartitioner.push_back(poseId);
	}

	// * The current pose of the robot should be in the new coordinate system:
	// ----------------------------------------------------------------------------------------------
	//  This is already done since the current pose is just another poseID and
	//  it's threaded like the others.

	// * Rebuild metric maps for consistency:
	// ----------------------------------------------------------------------------------------------
	LMH.rebuildMetricMaps();

	// * Update all the data in the node "Ae":
	// ----------------------------------------------------------------------------------------------
	LMH.updateAreaFromLMH(Ae);

	MRPT_END
}
