/* +------------------------------------------------------------------------+
   |                     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 "slam-precomp.h"  // Precompiled headers
//
#include <mrpt/slam/COccupancyGridMapFeatureExtractor.h>

using namespace mrpt;
using namespace mrpt::maps;
using namespace mrpt::slam;
using namespace mrpt::poses;
using namespace mrpt::img;
using namespace mrpt::system;

void COccupancyGridMapFeatureExtractor::uncached_extractFeatures(
	const mrpt::maps::COccupancyGridMap2D& grid,
	mrpt::maps::CLandmarksMap& outMap, const size_t number_of_features,
	const mrpt::vision::TDescriptorType descriptors,
	const mrpt::vision::CFeatureExtraction::TOptions& feat_options)
{
	MRPT_START

	// get the gridmap as an image:
	CImage img;
	grid.getAsImageFiltered(img, true /*vertical flip*/, false /* force RGB */);

	// Detect features:
	vision::CFeatureExtraction fExt;
	vision::CFeatureList lstFeatures;

	fExt.options = feat_options;
	fExt.options.patchSize = 0;	 // Do NOT extract patch

	// Detect interest points:
	fExt.detectFeatures(img, lstFeatures, 0 /* Init ID */, number_of_features);

	// Extract descriptors:
	if (descriptors != mrpt::vision::descAny)
		fExt.computeDescriptors(img, lstFeatures, descriptors);

	// Copy all the features to a map of landmarks:
	for (auto& ft : lstFeatures)
	{
		CLandmark lm;
		lm.ID = ft.keypoint.ID;
		lm.features.resize(1);

		lm.features[0] = ft;  // Insert the full feature there:

		lm.pose_mean.x =
			grid.getXMin() + (ft.keypoint.pt.x + 0.5f) * grid.getResolution();
		lm.pose_mean.y =
			grid.getYMin() + (ft.keypoint.pt.y + 0.5f) * grid.getResolution();
		lm.pose_mean.z = 0;

		lm.pose_cov_11 = lm.pose_cov_22 = lm.pose_cov_33 =
			square(grid.getResolution());
		lm.pose_cov_12 = lm.pose_cov_13 = lm.pose_cov_23 = 0;

		lm.seenTimesCount = 1;

		outMap.landmarks.push_back(lm);
	}

	MRPT_END_WITH_CLEAN_UP(try {
		grid.saveMetricMapRepresentationToFile(
			"__DEBUG_DUMP_GRIDMAP_ON_EXCEPTION");
	} catch (...){});
}

/*---------------------------------------------------------------
						extractFeatures
  ---------------------------------------------------------------*/
void COccupancyGridMapFeatureExtractor::extractFeatures(
	const mrpt::maps::COccupancyGridMap2D& grid,
	mrpt::maps::CLandmarksMap& outMap, const size_t number_of_features,
	const mrpt::vision::TDescriptorType descriptors,
	const mrpt::vision::CFeatureExtraction::TOptions& feat_options)
{
#if 0
	// Un-cashed version:
	uncached_extractFeatures(grid,outMap,number_of_features,descriptors,feat_options);
#else
	// Use cache mechanism:

	auto it = m_cache.find(&grid);
	if (it == m_cache.end())
	{
		// We have to recompute the features:
		CLandmarksMap::Ptr theMap = std::make_shared<CLandmarksMap>();

		uncached_extractFeatures(
			grid, *theMap, number_of_features, descriptors, feat_options);

		outMap = *theMap;

		// Insert into the cache:
		m_cache[&grid] = theMap;
	}
	else
	{
		// Already in the cache:
		outMap = *(it->second);
	}

#endif
}

// This will receive the events from maps in order to purge the cache.
void COccupancyGridMapFeatureExtractor::OnEvent(const mrptEvent& e)
{
	const COccupancyGridMap2D* src = nullptr;

	// Upon map change or destruction, remove from our cache:
	if (e.isOfType<mrptEventOnDestroy>())
		src = static_cast<const COccupancyGridMap2D*>(
			static_cast<const mrptEventOnDestroy*>(&e)->source_object);
	if (e.isOfType<mrptEventMetricMapClear>())
		src = static_cast<const COccupancyGridMap2D*>(
			static_cast<const mrptEventMetricMapClear*>(&e)->source_map);
	if (e.isOfType<mrptEventMetricMapInsert>())
		src = static_cast<const COccupancyGridMap2D*>(
			static_cast<const mrptEventMetricMapClear*>(&e)->source_map);

	if (src)
	{
		// Remove from cache:
		m_cache.erase(src);

		// Unsubscribe:
		this->observeEnd(*const_cast<COccupancyGridMap2D*>(src));
	}
}
