/* +------------------------------------------------------------------------+
   |                     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/core/round.h>  // round()
#include <mrpt/maps/CReflectivityGridMap2D.h>
#include <mrpt/obs/CObservationReflectivity.h>
#include <mrpt/opengl/CSetOfObjects.h>
#include <mrpt/opengl/CTexturedPlane.h>
#include <mrpt/poses/CPose2D.h>
#include <mrpt/serialization/CArchive.h>
#include <mrpt/system/os.h>

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

//  =========== Begin of Map definition ============
MAP_DEFINITION_REGISTER(
    "mrpt::maps::CReflectivityGridMap2D,reflectivityMap", mrpt::maps::CReflectivityGridMap2D)

CReflectivityGridMap2D::TMapDefinition::TMapDefinition()

    = default;

void CReflectivityGridMap2D::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(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);

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

void CReflectivityGridMap2D::TMapDefinition::dumpToTextStream_map_specific(std::ostream& out) const
{
  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 CReflectivityGridMap2D::internal_CreateFromMapDefinition(
    const mrpt::maps::TMetricMapInitializer& _def)
{
  const CReflectivityGridMap2D::TMapDefinition& def =
      *dynamic_cast<const CReflectivityGridMap2D::TMapDefinition*>(&_def);
  auto obj =
      CReflectivityGridMap2D::Create(def.min_x, def.max_x, def.min_y, def.max_y, def.resolution);
  obj->insertionOptions = def.insertionOpts;
  return obj;
}
//  =========== End of Map definition Block =========

IMPLEMENTS_SERIALIZABLE(CReflectivityGridMap2D, CMetricMap, mrpt::maps)

// Lookup tables for log-odds
CLogOddsGridMapLUT<CReflectivityGridMap2D::cell_t> CReflectivityGridMap2D::m_logodd_lut;

/*---------------------------------------------------------------
            Constructor
  ---------------------------------------------------------------*/
CReflectivityGridMap2D::CReflectivityGridMap2D(
    double x_min, double x_max, double y_min, double y_max, double resolution) :
    CDynamicGrid<int8_t>(x_min, x_max, y_min, y_max, resolution), insertionOptions()
{
  internal_clear();
}

/*---------------------------------------------------------------
            clear
  ---------------------------------------------------------------*/
void CReflectivityGridMap2D::internal_clear() { fill(m_logodd_lut.p2l(0.5)); }
/*---------------------------------------------------------------
            isEmpty
  ---------------------------------------------------------------*/
bool CReflectivityGridMap2D::isEmpty() const { return false; }
/*---------------------------------------------------------------
            insertObservation
  ---------------------------------------------------------------*/
bool CReflectivityGridMap2D::internal_insertObservation(
    const CObservation& obs, const std::optional<const mrpt::poses::CPose3D>& robotPose)
{
  MRPT_START

  CPose2D robotPose2D;
  CPose3D robotPose3D;

  if (robotPose)
  {
    robotPose2D = CPose2D(*robotPose);
    robotPose3D = (*robotPose);
  }
  else
  {
    // Default values are (0,0,0)
  }

  if (IS_CLASS(obs, CObservationReflectivity))
  {
    /********************************************************************
          OBSERVATION TYPE: CObservationReflectivity
    ********************************************************************/
    const auto& o = dynamic_cast<const CObservationReflectivity&>(obs);

    if (o.channel != -1 && insertionOptions.channel != -1 && o.channel != insertionOptions.channel)
    {
      return false;  // Incorrect channel
    }

    CPose3D sensor_pose;
    sensor_pose.composeFrom(robotPose3D, o.sensorPose);

    // log-odd increment due to the observation:
    const cell_t logodd_observation = m_logodd_lut.p2l(o.reflectivityLevel);

    // Update cell, with saturation:
    cell_t* cell = cellByPos(sensor_pose.x(), sensor_pose.y());
    if (!cell)
    {
      // We need to resize the grid!
      const double new_x_min = std::min(m_x_min, sensor_pose.x());
      const double new_y_min = std::min(m_y_min, sensor_pose.y());
      const double new_x_max = std::min(m_x_max, sensor_pose.x());
      const double new_y_max = std::min(m_y_max, sensor_pose.y());

      const int8_t default_cell = m_logodd_lut.p2l(0.5);
      resize(new_x_min, new_x_max, new_y_min, new_y_max, default_cell, 2.0 /* addit. margin */);

      // Now we should get the cell:
      cell = cellByPos(sensor_pose.x(), sensor_pose.y());

      ASSERTMSG_(cell != nullptr, "cell==nullptr even after resizing grid!?");
    }

    const int cell_old = static_cast<int>(*cell);
    int cell_new = cell_old + static_cast<int>(logodd_observation);
    keep_min(cell_new, static_cast<int>(CELLTYPE_MAX) - 1);
    keep_max(cell_new, static_cast<int>(CELLTYPE_MIN) + 1);

    *cell = static_cast<cell_t>(cell_new);

    return true;  // Done!
  }               // end if "CObservationGasSensors"

  return false;

  MRPT_END
}

/*---------------------------------------------------------------
            computeObservationLikelihood
  ---------------------------------------------------------------*/
double CReflectivityGridMap2D::internal_computeObservationLikelihood(
    const CObservation& obs, const CPose3D& takenFrom) const
{
  MRPT_START

  if (IS_CLASS(obs, CObservationReflectivity))
  {
    /********************************************************************
          OBSERVATION TYPE: CObservationReflectivity
    ********************************************************************/
    const auto& o = dynamic_cast<const CObservationReflectivity&>(obs);

    if (o.channel != -1 && insertionOptions.channel != -1 && o.channel != insertionOptions.channel)
    {
      return 0;  // Incorrect channel
    }

    CPose3D sensor_pose;
    sensor_pose.composeFrom(takenFrom, o.sensorPose);

    const cell_t* cell = cellByPos(sensor_pose.x(), sensor_pose.y());
    if (!cell)
      return 0;  // out of the map..
    else
    {
      ASSERT_GE_(o.reflectivityLevel, 0);
      ASSERT_LE_(o.reflectivityLevel, 1);
      return -0.5 * square((m_logodd_lut.l2p(*cell) - o.reflectivityLevel) / o.sensorStdNoise);
    }
  }
  else
    return 0;

  MRPT_END
}

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

  // Map cells:
  const auto n = static_cast<uint32_t>(m_map.size());
  out << n;
  if (n) out.WriteBuffer(&m_map[0], n);

  // Save the insertion options
  out << insertionOptions.channel;  // v3

  out << genericMapParams;  // v1
}

void CReflectivityGridMap2D::serializeFrom(mrpt::serialization::CArchive& in, uint8_t version)
{
  switch (version)
  {
    case 0:
    case 1:
    case 2:
    case 3:
    {
      dyngridcommon_readFromStream(in, version < 2);

      // Map cells:
      uint32_t n;
      in >> n;
      m_map.resize(n);
      if (n) in.ReadBuffer(&m_map[0], n);

      // Load the insertion options:
      if (version >= 3) in >> insertionOptions.channel;

      if (version >= 1) in >> genericMapParams;
    }
    break;
    default:
      MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version);
  };
}

/*---------------------------------------------------------------
          TInsertionOptions
 ---------------------------------------------------------------*/
CReflectivityGridMap2D::TInsertionOptions::TInsertionOptions() = default;
void CReflectivityGridMap2D::TInsertionOptions::dumpToTextStream(std::ostream& out) const
{
  out << "\n----------- [CReflectivityGridMap2D::TInsertionOptions] "
         "------------ \n\n";

  LOADABLEOPTS_DUMP_VAR(channel, int);

  out << "\n";
}

/*---------------------------------------------------------------
          loadFromConfigFile
  ---------------------------------------------------------------*/
void CReflectivityGridMap2D::TInsertionOptions::loadFromConfigFile(
    [[maybe_unused]] const mrpt::config::CConfigFileBase& iniFile,
    [[maybe_unused]] const std::string& section)
{
  MRPT_LOAD_CONFIG_VAR(channel, int, iniFile, section);
}

/*---------------------------------------------------------------
          saveMetricMapRepresentationToFile
  ---------------------------------------------------------------*/
void CReflectivityGridMap2D::saveMetricMapRepresentationToFile(
    const std::string& filNamePrefix) const
{
  // Text matrix:
  saveToTextFile(filNamePrefix + std::string("_probability.txt"));
}

/*---------------------------------------------------------------
          getAsImage
  ---------------------------------------------------------------*/
void CReflectivityGridMap2D::getAsImage(CImage& img, bool verticalFlip, bool forceRGB) const
{
  if (!forceRGB)
  {  // 8bit gray-scale
    img.resize(m_size_x, m_size_y, CH_GRAY);
    const cell_t* srcPtr = &m_map[0];
    unsigned char* destPtr;
    for (unsigned int y = 0; y < m_size_y; y++)
    {
      if (!verticalFlip)
        destPtr = img(0, m_size_y - 1 - y);
      else
        destPtr = img(0, y);
      for (unsigned int x = 0; x < m_size_x; x++)
      {
        *destPtr++ = m_logodd_lut.l2p_255(*srcPtr++);
      }
    }
  }
  else
  {  // 24bit RGB:
    img.resize(m_size_x, m_size_y, CH_RGB);
    const cell_t* srcPtr = &m_map[0];
    unsigned char* destPtr;
    for (unsigned int y = 0; y < m_size_y; y++)
    {
      if (!verticalFlip)
        destPtr = img(0, m_size_y - 1 - y);
      else
        destPtr = img(0, y);
      for (unsigned int x = 0; x < m_size_x; x++)
      {
        uint8_t c = m_logodd_lut.l2p_255(*srcPtr++);
        *destPtr++ = c;
        *destPtr++ = c;
        *destPtr++ = c;
      }
    }
  }
}

void CReflectivityGridMap2D::getVisualizationInto(mrpt::opengl::CSetOfObjects& o) const
{
  if (!genericMapParams.enableSaveAs3DObject) return;

  MRPT_START

  opengl::CTexturedPlane::Ptr outObj = std::make_shared<opengl::CTexturedPlane>();

  outObj->setPlaneCorners(m_x_min, m_x_max, m_y_min, m_y_max);

  // Create the color & transparecy (alpha) images:
  CImage imgColor(m_size_x, m_size_y, CH_GRAY);
  CImage imgTrans(m_size_x, m_size_y, CH_GRAY);

  const cell_t* srcPtr = &m_map[0];
  unsigned char* destPtr_color;
  unsigned char* destPtr_trans;

  for (unsigned int y = 0; y < m_size_y; y++)
  {
    destPtr_color = imgColor(0, y);
    destPtr_trans = imgTrans(0, y);
    for (unsigned int x = 0; x < m_size_x; x++)
    {
      uint8_t cell255 = m_logodd_lut.l2p_255(*srcPtr++);
      *destPtr_color++ = cell255;

      int8_t auxC = (int8_t)((signed short)cell255) - 128;
      *destPtr_trans++ = auxC > 0 ? (auxC << 1) : ((-auxC) << 1);
    }
  }

  outObj->assignImage(imgColor, imgTrans);
  o.insert(outObj);

  MRPT_END
}

float CReflectivityGridMap2D::compute3DMatchingRatio(
    [[maybe_unused]] const mrpt::maps::CMetricMap* otherMap,
    [[maybe_unused]] const mrpt::poses::CPose3D& otherMapPose,
    [[maybe_unused]] const TMatchingRatioParams& params) const
{
  return 0;
}
