/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2024, 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,
    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,
    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));
  }
}
