/* +------------------------------------------------------------------------+
   |                     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/containers/printf_vector.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::poses;
using namespace mrpt::obs;
using namespace std;

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

						areaAbstraction

	Area Abstraction (AA) process within HMT-SLAM

  ---------------------------------------------------------------*/
CHMTSLAM::TMessageLSLAMfromAA::Ptr CHMTSLAM::areaAbstraction(
	CLocalMetricHypothesis* LMH, const TPoseIDList& newPoseIDs)
{
	MRPT_START

	ASSERT_(!newPoseIDs.empty());
	ASSERT_(LMH);
	CHMTSLAM* obj = LMH->m_parent.get();
	ASSERT_(obj);

	// The output results:
	TMessageLSLAMfromAA::Ptr resMsg = std::make_shared<TMessageLSLAMfromAA>();

	// Process msg:
	THypothesisID LMH_ID = LMH->m_ID;
	resMsg->hypothesisID = LMH_ID;

	for (unsigned long newPoseID : newPoseIDs)
	{
		// Add a new node to the graph:
		obj->logFmt(
			mrpt::system::LVL_DEBUG, "[thread_AA] Processing new pose ID: %u\n",
			static_cast<unsigned>(newPoseID));

		// Get SF & pose pdf for the new pose.
		const CSensoryFrame* sf;
		CPose3DPDFParticles::Ptr posePDF =
			std::make_shared<CPose3DPDFParticles>();

		{
			// std::lock_guard<std::mutex>	lock( LMH->m_lock ); // We are
			// already within the LMH's lock!

			// SF:
			auto itSFs = LMH->m_SFs.find(newPoseID);
			ASSERT_(itSFs != LMH->m_SFs.end());
			sf = &itSFs->second;

			// Pose PDF:
			LMH->getPoseParticles(newPoseID, *posePDF);
		}  // end of LMH's critical section lock

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

			// Add to the graph partitioner:
			LMH->m_robotPosesGraph.partitioner.options =
				obj->m_options.AA_options;

			unsigned int newIdx =
				LMH->m_robotPosesGraph.partitioner.addMapFrame(*sf, *posePDF);
			LMH->m_robotPosesGraph.idx2pose[newIdx] = newPoseID;
		}  // end of critical section lock on "m_robotPosesGraph.lock"
	}  // end for each new ID

	vector<std::vector<uint32_t>> partitions;
	{
		std::lock_guard<std::mutex> locker(LMH->m_robotPosesGraph.lock);
		LMH->m_robotPosesGraph.partitioner.updatePartitions(partitions);
	}

	// Send result to LSLAM
	resMsg->partitions.resize(partitions.size());
	vector<TPoseIDList>::iterator itDest;
	vector<std::vector<uint32_t>>::const_iterator itSrc;
	for (itDest = resMsg->partitions.begin(), itSrc = partitions.begin();
		 itSrc != partitions.end(); itSrc++, itDest++)
	{
		itDest->resize(itSrc->size());
		std::vector<uint32_t>::const_iterator it1;
		TPoseIDList::iterator it2;
		for (it1 = itSrc->begin(), it2 = itDest->begin(); it1 != itSrc->end();
			 it1++, it2++)
			*it2 = LMH->m_robotPosesGraph.idx2pose[*it1];
	}

	resMsg->dumpToConsole();

	return resMsg;

	MRPT_END
}

void CHMTSLAM::TMessageLSLAMfromAA::dumpToConsole() const
{
	cout << format(
		"Hypo ID: %i has %i partitions:\n", (int)hypothesisID,
		(int)partitions.size());

	for (const auto& partition : partitions)
	{
		mrpt::containers::printf_vector("%i", partition);
		cout << endl;
	}
}
