/* +------------------------------------------------------------------------+
   |                     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 "maps-precomp.h"  // Precomp header
//
#include <mrpt/maps/CHeightGridMap2D_MRF.h>
#include <mrpt/poses/CPose2D.h>
#include <mrpt/poses/CPose3D.h>

using namespace mrpt;
using namespace mrpt::maps;
using namespace mrpt::obs;
using namespace mrpt::poses;
using namespace std;
using namespace mrpt::math;

//  =========== Begin of Map definition ============
MAP_DEFINITION_REGISTER(
    "mrpt::maps::CHeightGridMap2D_MRF,dem_mrf", mrpt::maps::CHeightGridMap2D_MRF)

CHeightGridMap2D_MRF::TMapDefinition::TMapDefinition() = default;

void CHeightGridMap2D_MRF::TMapDefinition::loadFromConfigFile_map_specific(
    const mrpt::config::CConfigFileBase& source, const std::string& sectionNamePrefix)
{
  // [<sectionNamePrefix>+"_creationOpts"]
  const std::string sSectCreation = sectionNamePrefix + string("_creationOpts");
  MRPT_LOAD_CONFIG_VAR(run_map_estimation_at_ctor, bool, source, sSectCreation);
  MRPT_LOAD_CONFIG_VAR(min_x, double, source, sSectCreation);
  MRPT_LOAD_CONFIG_VAR(max_x, double, source, sSectCreation);
  MRPT_LOAD_CONFIG_VAR(min_y, double, source, sSectCreation);
  MRPT_LOAD_CONFIG_VAR(max_y, double, source, sSectCreation);
  MRPT_LOAD_CONFIG_VAR(resolution, double, source, sSectCreation);
  mapType =
      source.read_enum<CHeightGridMap2D_MRF::TMapRepresentation>(sSectCreation, "mapType", mapType);

  insertionOpts.loadFromConfigFile(source, sectionNamePrefix + string("_insertOpts"));
}

void CHeightGridMap2D_MRF::TMapDefinition::dumpToTextStream_map_specific(std::ostream& out) const
{
  out << mrpt::format(
      "MAP TYPE                                  = %s\n",
      mrpt::typemeta::TEnumType<CHeightGridMap2D_MRF::TMapRepresentation>::value2name(mapType)
          .c_str());
  LOADABLEOPTS_DUMP_VAR(run_map_estimation_at_ctor, bool);
  LOADABLEOPTS_DUMP_VAR(min_x, double);
  LOADABLEOPTS_DUMP_VAR(max_x, double);
  LOADABLEOPTS_DUMP_VAR(min_y, double);
  LOADABLEOPTS_DUMP_VAR(max_y, double);
  LOADABLEOPTS_DUMP_VAR(resolution, double);

  this->insertionOpts.dumpToTextStream(out);
}

mrpt::maps::CMetricMap::Ptr CHeightGridMap2D_MRF::internal_CreateFromMapDefinition(
    const mrpt::maps::TMetricMapInitializer& _def)
{
  const CHeightGridMap2D_MRF::TMapDefinition& def =
      *dynamic_cast<const CHeightGridMap2D_MRF::TMapDefinition*>(&_def);
  auto obj = CHeightGridMap2D_MRF::Create(
      def.mapType, def.min_x, def.max_x, def.min_y, def.max_y, def.resolution,
      def.run_map_estimation_at_ctor);
  obj->insertionOptions = def.insertionOpts;
  return obj;
}
//  =========== End of Map definition Block =========

IMPLEMENTS_SERIALIZABLE(CHeightGridMap2D_MRF, CRandomFieldGridMap2D, mrpt::maps)

// Constructor
CHeightGridMap2D_MRF::CHeightGridMap2D_MRF(
    TMapRepresentation mapType,
    double x_min,
    double x_max,
    double y_min,
    double y_max,
    double resolution,
    bool run_first_map_estimation_now) :
    CRandomFieldGridMap2D(mapType, x_min, x_max, y_min, y_max, resolution), insertionOptions()
{
  m_rfgm_run_update_upon_clear = run_first_map_estimation_now;
  // Set the grid to initial values (and adjusts the KF covariance matrix!)
  //  Also, calling clear() is mandatory to end initialization of our base
  //  class (read note in CRandomFieldGridMap2D::CRandomFieldGridMap2D)
  CMetricMap::clear();
}

bool CHeightGridMap2D_MRF::insertIndividualPoint(
    const double x,
    const double y,
    const double z,
    const CHeightGridMap2D_Base::TPointInsertParams& params)
{
  const TRandomFieldCell* cell = cellByPos(x, y);
  if (!cell) return false;
  this->insertIndividualReading(
      z, mrpt::math::TPoint2D(x, y), params.update_map_after_insertion, true /*time invariant*/,
      params.pt_z_std);
  return true;
}
double CHeightGridMap2D_MRF::dem_get_resolution() const { return m_resolution; }
size_t CHeightGridMap2D_MRF::dem_get_size_x() const { return m_size_x; }
size_t CHeightGridMap2D_MRF::dem_get_size_y() const { return m_size_y; }
bool CHeightGridMap2D_MRF::dem_get_z_by_cell(size_t cx, size_t cy, double& z_out) const
{
  const TRandomFieldCell* cell = cellByIndex(cx, cy);
  if (cell && cell->kf_mean())
  {
    z_out = cell->kf_mean();
    return true;
  }
  else
    return false;
}
bool CHeightGridMap2D_MRF::dem_get_z(const double x, const double y, double& z_out) const
{
  const TRandomFieldCell* cell = cellByPos(x, y);
  if (cell && cell->kf_mean())
  {
    z_out = cell->kf_mean();
    return true;
  }
  else
    return false;
}
void CHeightGridMap2D_MRF::dem_update_map() { this->updateMapEstimation(); }
void CHeightGridMap2D_MRF::internal_clear()
{
  // Just do the generic clear:
  CRandomFieldGridMap2D::internal_clear();
  // Anything else special for this derived class?
}

bool CHeightGridMap2D_MRF::internal_insertObservation(
    const CObservation& obs, const std::optional<const mrpt::poses::CPose3D>& robotPose)
{
  return dem_internal_insertObservation(obs, robotPose);
}

/*---------------------------------------------------------------
            computeObservationLikelihood
  ---------------------------------------------------------------*/
double CHeightGridMap2D_MRF::internal_computeObservationLikelihood(
    [[maybe_unused]] const CObservation& obs, [[maybe_unused]] const CPose3D& takenFrom) const
{
  THROW_EXCEPTION("Not implemented yet!");
}

uint8_t CHeightGridMap2D_MRF::serializeGetVersion() const { return 0; }
void CHeightGridMap2D_MRF::serializeTo(mrpt::serialization::CArchive& out) const
{
  dyngridcommon_writeToStream(out);

  // To assure compatibility: The size of each cell:
  auto n = static_cast<uint32_t>(sizeof(TRandomFieldCell));
  out << n;

  // Save the map contents:
  n = static_cast<uint32_t>(m_map.size());
  out << n;

// Save the "m_map": This requires special handling for big endian systems:
#if MRPT_IS_BIG_ENDIAN
  for (uint32_t i = 0; i < n; i++)
  {
    out << m_map[i].kf_mean() << m_map[i].dm_mean() << m_map[i].dmv_var_mean;
  }
#else
  // Little endian: just write all at once:
  out.WriteBuffer(&m_map[0], sizeof(m_map[0]) * m_map.size());
#endif

  // Save the insertion options:
  out << uint8_t(m_mapType) << m_cov << m_stackedCov;

  out << insertionOptions.sigma << insertionOptions.cutoffRadius << insertionOptions.R_min
      << insertionOptions.R_max << insertionOptions.KF_covSigma
      << insertionOptions.KF_initialCellStd << insertionOptions.KF_observationModelNoise
      << insertionOptions.KF_defaultCellMeanValue << insertionOptions.KF_W_size;

  out << m_average_normreadings_mean << m_average_normreadings_var
      << uint64_t(m_average_normreadings_count);

  out << genericMapParams;
}

void CHeightGridMap2D_MRF::serializeFrom(mrpt::serialization::CArchive& in, uint8_t version)
{
  switch (version)
  {
    case 0:
    {
      dyngridcommon_readFromStream(in);

      // To assure compatibility: The size of each cell:
      uint32_t n;
      in >> n;

      ASSERT_EQUAL_(n, static_cast<uint32_t>(sizeof(TRandomFieldCell)));
      // Load the map contents:
      in >> n;
      m_map.resize(n);

// Read the note in writeToStream()
#if MRPT_IS_BIG_ENDIAN
      for (uint32_t i = 0; i < n; i++)
        in >> m_map[i].kf_mean() >> m_map[i].dm_mean() >> m_map[i].dmv_var_mean;
#else
      // Little endian: just read all at once:
      in.ReadBuffer(&m_map[0], sizeof(m_map[0]) * m_map.size());
#endif

      {
        uint8_t i;
        in >> i;
        m_mapType = TMapRepresentation(i);

        in >> m_cov >> m_stackedCov;

        in >> insertionOptions.sigma >> insertionOptions.cutoffRadius >> insertionOptions.R_min >>
            insertionOptions.R_max >> insertionOptions.KF_covSigma >>
            insertionOptions.KF_initialCellStd >> insertionOptions.KF_observationModelNoise >>
            insertionOptions.KF_defaultCellMeanValue >> insertionOptions.KF_W_size;
      }

      {
        uint64_t N;
        in >> m_average_normreadings_mean >> m_average_normreadings_var >> N;
        m_average_normreadings_count = N;
      }

      in >> genericMapParams;

      m_hasToRecoverMeanAndCov = true;
    }
    break;
    default:
      MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version);
  };
}

/*---------------------------------------------------------------
          TInsertionOptions
 ---------------------------------------------------------------*/
CHeightGridMap2D_MRF::TInsertionOptions::TInsertionOptions() = default;
void CHeightGridMap2D_MRF::TInsertionOptions::dumpToTextStream(std::ostream& out) const
{
  out << "\n----------- [CHeightGridMap2D_MRF::TInsertionOptions] "
         "------------ "
         "\n\n";
  out << "[TInsertionOptions.Common] ------------ \n\n";
  internal_dumpToTextStream_common(out);  // Common params to all random fields maps:

  out << "\n";
}

/*---------------------------------------------------------------
          loadFromConfigFile
  ---------------------------------------------------------------*/
void CHeightGridMap2D_MRF::TInsertionOptions::loadFromConfigFile(
    const mrpt::config::CConfigFileBase& iniFile, const std::string& section)
{
  // Common data fields for all random fields maps:
  internal_loadFromConfigFile_common(iniFile, section);

  // Specific data fields for this class:
  // ...
}

void CHeightGridMap2D_MRF::getVisualizationInto(mrpt::opengl::CSetOfObjects& o) const
{
  MRPT_START
  if (!genericMapParams.enableSaveAs3DObject) return;
  CRandomFieldGridMap2D::getVisualizationInto(o);
  MRPT_END
}

/*---------------------------------------------------------------
            getAs3DObject
---------------------------------------------------------------*/
void CHeightGridMap2D_MRF::getAs3DObject(
    opengl::CSetOfObjects& meanObj, opengl::CSetOfObjects& varObj) const
{
  MRPT_START
  if (!genericMapParams.enableSaveAs3DObject) return;
  CRandomFieldGridMap2D::getAs3DObject(meanObj, varObj);
  MRPT_END
}
