/* +------------------------------------------------------------------------+
   |                     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

#ifdef HTMSLAM_HAS_FABMAP
#include <FabMapLibInterface.h>
#define THE_FABMAP static_cast<fabmap::FabMapInstance*>(m_fabmap)
#endif

using namespace mrpt;
using namespace mrpt::obs;
using namespace mrpt::poses;
using namespace mrpt::hmtslam;
using namespace std;

CTopLCDetector_FabMap::CTopLCDetector_FabMap(CHMTSLAM* hmtslam)
	: CTopLCDetectorBase(hmtslam), m_fabmap(nullptr)
{
#ifdef HTMSLAM_HAS_FABMAP
	// Use already loaded options:
	const CTopLCDetector_FabMap::TOptions* o =
		&m_hmtslam->m_options.TLC_fabmap_options;

	unsigned int nVocabSize;
	if (!fabmap::ParseOXV_PeekDimensions(
			o->vocab_path + o->vocabName + ".oxv", nVocabSize))
		THROW_EXCEPTION_FMT(
			"Error parsing vocabulary file: %s",
			std::string(o->vocab_path + o->vocabName + ".oxv").c_str())

	m_fabmap = new fabmap::FabMapInstance(
		o->vocab_path, o->vocabName, o->p_obs_given_exists, 0.1,
		o->p_at_new_place, nVocabSize, o->df_lik_smooth);

	// Restart msg:
	cout << "[CTopLCDetector_FabMap::constructor] Resetting FabMap" << endl;
	THE_FABMAP->hmtslam_restart();
#else
	THROW_EXCEPTION("Please, recompile MRPT with FabMap to use this class.");
#endif
}

CTopLCDetector_FabMap::~CTopLCDetector_FabMap()
{
#ifdef HTMSLAM_HAS_FABMAP
	delete THE_FABMAP;
	m_fabmap = nullptr;
#endif
}

/** This method must compute the topological observation model.
 * \param out_log_lik The output, a log-likelihood.
 * \return nullptr, or a PDF of the estimated translation between the two areas
 * (can be a multi-modal PDF).
 */
CPose3DPDF::Ptr CTopLCDetector_FabMap::computeTopologicalObservationModel(
	[[maybe_unused]] const THypothesisID& hypID,
	[[maybe_unused]] const CHMHMapNode::Ptr& currentArea,
	[[maybe_unused]] const CHMHMapNode::Ptr& refArea,
	[[maybe_unused]] double& out_log_lik)
{
	return CPose3DPDF::Ptr();
}

/** Hook method for being warned about the insertion of a new poses into the
 * maps.
 *  This should be independent of hypothesis IDs.
 */
void CTopLCDetector_FabMap::OnNewPose(
	[[maybe_unused]] const TPoseID& poseID,
	[[maybe_unused]] const CSensoryFrame* SF)
{
#ifdef HTMSLAM_HAS_FABMAP

	vector<string> lstObsImages;

	size_t n = 0;
	CObservationImage::Ptr obsIm;
	while ((obsIm = SF->getObservationByClass<CObservationImage>(n)))
	{
		string path;
		obsIm->image.getExternalStorageFileAbsolutePath(path);
		lstObsImages.push_back(path);
		n++;
	};

	if (lstObsImages.empty()) return;  // Not all poses must have images.

	cout << "[OnNewPose] Adding new pose: " << poseID
		 << " # of images: " << lstObsImages.size() << endl;
	THE_FABMAP->hmtslam_addNewPose(poseID, lstObsImages);
#endif
}

// Initialization
CTopLCDetector_FabMap::TOptions::TOptions()
	: vocab_path("./vocab"), vocabName("vocab_name")

{
}

//  Load parameters from configuration source
void CTopLCDetector_FabMap::TOptions::loadFromConfigFile(
	const mrpt::config::CConfigFileBase& iniFile, const std::string& section)
{
	MRPT_LOAD_CONFIG_VAR(vocab_path, string, iniFile, section);
	MRPT_LOAD_CONFIG_VAR(vocabName, string, iniFile, section);
	MRPT_LOAD_CONFIG_VAR(p_obs_given_exists, double, iniFile, section);
	MRPT_LOAD_CONFIG_VAR(p_at_new_place, double, iniFile, section);
	MRPT_LOAD_CONFIG_VAR(df_lik_smooth, double, iniFile, section);
}

//  This method must display clearly all the contents of the structure in
//  textual form, sending it to a CStream.
void CTopLCDetector_FabMap::TOptions::dumpToTextStream(std::ostream& out) const
{
	out << "\n----------- [CTopLCDetector_FabMap::TOptions] ------------ \n\n";

	LOADABLEOPTS_DUMP_VAR(vocab_path, string)
	LOADABLEOPTS_DUMP_VAR(vocabName, string)
	LOADABLEOPTS_DUMP_VAR(p_obs_given_exists, double)
	LOADABLEOPTS_DUMP_VAR(p_at_new_place, double)
	LOADABLEOPTS_DUMP_VAR(df_lik_smooth, double)
}
