/* +------------------------------------------------------------------------+
   |                     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/bits_mem.h>
#include <mrpt/maps/CSimplePointsMap.h>
#include <mrpt/serialization/CArchive.h>

#include "CPointsMap_crtp_common.h"

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

//  =========== Begin of Map definition ============
MAP_DEFINITION_REGISTER("mrpt::maps::CSimplePointsMap,pointsMap", mrpt::maps::CSimplePointsMap)

CSimplePointsMap::TMapDefinition::TMapDefinition() = default;
void CSimplePointsMap::TMapDefinition::loadFromConfigFile_map_specific(
    const mrpt::config::CConfigFileBase& c, const std::string& s)
{
  insertionOpts.loadFromConfigFile(c, s + string("_insertOpts"));
  likelihoodOpts.loadFromConfigFile(c, s + string("_likelihoodOpts"));
  renderOpts.loadFromConfigFile(c, s + string("_renderOpts"));
}

void CSimplePointsMap::TMapDefinition::dumpToTextStream_map_specific(std::ostream& out) const
{
  this->insertionOpts.dumpToTextStream(out);
  this->likelihoodOpts.dumpToTextStream(out);
  this->renderOpts.dumpToTextStream(out);
}

mrpt::maps::CMetricMap::Ptr CSimplePointsMap::internal_CreateFromMapDefinition(
    const mrpt::maps::TMetricMapInitializer& _def)
{
  const CSimplePointsMap::TMapDefinition& def =
      *dynamic_cast<const CSimplePointsMap::TMapDefinition*>(&_def);
  auto obj = CSimplePointsMap::Create();
  obj->insertionOptions = def.insertionOpts;
  obj->likelihoodOptions = def.likelihoodOpts;
  obj->renderOptions = def.renderOpts;
  return obj;
}
//  =========== End of Map definition Block =========
IMPLEMENTS_SERIALIZABLE(CSimplePointsMap, CPointsMap, mrpt::maps)

void CSimplePointsMap::reserve(size_t newLength)
{
  m_x.reserve(newLength);
  m_y.reserve(newLength);
  m_z.reserve(newLength);
}

// Resizes all point buffers so they can hold the given number of points: newly
// created points are set to default values,
//  and old contents are not changed.
void CSimplePointsMap::resize(size_t newLength)
{
  this->reserve(newLength);  // to ensure 4N capacity
  m_x.resize(newLength, 0);
  m_y.resize(newLength, 0);
  m_z.resize(newLength, 0);
  mark_as_modified();
}

// Resizes all point buffers so they can hold the given number of points,
// *erasing* all previous contents
//  and leaving all points to default values.
void CSimplePointsMap::setSize(size_t newLength)
{
  this->reserve(newLength);  // to ensure 4N capacity
  m_x.assign(newLength, 0);
  m_y.assign(newLength, 0);
  m_z.assign(newLength, 0);
  mark_as_modified();
}

uint8_t CSimplePointsMap::serializeGetVersion() const { return 10; }
void CSimplePointsMap::serializeTo(mrpt::serialization::CArchive& out) const
{
  uint32_t n = m_x.size();

  // First, write the number of points:
  out << n;

  if (n > 0)
  {
    out.WriteBufferFixEndianness(&m_x[0], n);
    out.WriteBufferFixEndianness(&m_y[0], n);
    out.WriteBufferFixEndianness(&m_z[0], n);
  }
  out << genericMapParams;               // v9
  insertionOptions.writeToStream(out);   // v9
  likelihoodOptions.writeToStream(out);  // v5
  renderOptions.writeToStream(out);      // v10
}

/*---------------------------------------------------------------
          readFromStream
   Implements the reading from a CStream capability of
    CSerializable objects
  ---------------------------------------------------------------*/
void CSimplePointsMap::serializeFrom(mrpt::serialization::CArchive& in, uint8_t version)
{
  switch (version)
  {
    case 8:
    case 9:
    case 10:
    {
      mark_as_modified();

      // Read the number of points:
      uint32_t n;
      in >> n;

      this->resize(n);

      if (n > 0)
      {
        in.ReadBufferFixEndianness(&m_x[0], n);
        in.ReadBufferFixEndianness(&m_y[0], n);
        in.ReadBufferFixEndianness(&m_z[0], n);
      }
      if (version >= 9)
        in >> genericMapParams;
      else
      {
        bool disableSaveAs3DObject;
        in >> disableSaveAs3DObject;
        genericMapParams.enableSaveAs3DObject = !disableSaveAs3DObject;
      }
      insertionOptions.readFromStream(in);
      likelihoodOptions.readFromStream(in);
      if (version >= 10) renderOptions.readFromStream(in);
    }
    break;

    case 0:
    case 1:
    case 2:
    case 3:
    case 4:
    case 5:
    case 6:
    case 7:
    {
      mark_as_modified();

      // Read the number of points:
      uint32_t n;
      in >> n;

      this->resize(n);

      if (n > 0)
      {
        in.ReadBufferFixEndianness(&m_x[0], n);
        in.ReadBufferFixEndianness(&m_y[0], n);
        in.ReadBufferFixEndianness(&m_z[0], n);

        // Version 1: weights are also stored:
        // Version 4: Type becomes long int -> uint32_t for
        // portability!!
        if (version >= 1)
        {
          if (version >= 4)
          {
            if (version >= 7)
            {
              // Weights were removed from this class in v7 (MRPT
              // 0.9.5),
              //  so nothing else to do.
            }
            else
            {
              // Go on with old serialization format, but discard
              // weights:
              std::vector<uint32_t> dummy_pointWeight(n);
              in.ReadBufferFixEndianness(&dummy_pointWeight[0], n);
            }
          }
          else
          {
            std::vector<uint32_t> dummy_pointWeight(n);
            in.ReadBufferFixEndianness(&dummy_pointWeight[0], n);
          }
        }
      }

      if (version >= 2)
      {
        // version 2: options saved too
        in >> insertionOptions.minDistBetweenLaserPoints >>
            insertionOptions.addToExistingPointsMap >> insertionOptions.also_interpolate >>
            insertionOptions.disableDeletion >> insertionOptions.fuseWithExisting >>
            insertionOptions.isPlanarMap;

        if (version < 6)
        {
          bool old_matchStaticPointsOnly;
          in >> old_matchStaticPointsOnly;
        }

        in >> insertionOptions.maxDistForInterpolatePoints;

        {
          bool disableSaveAs3DObject;
          in >> disableSaveAs3DObject;
          genericMapParams.enableSaveAs3DObject = !disableSaveAs3DObject;
        }
      }

      if (version >= 3)
      {
        in >> insertionOptions.horizontalTolerance;
      }

      if (version >= 5)  // version 5: added likelihoodOptions
        likelihoodOptions.readFromStream(in);

      if (version >= 8)  // version 8: added insertInvalidPoints
        in >> insertionOptions.insertInvalidPoints;
    }
    break;
    default:
      MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version);
  };
}

/*---------------------------------------------------------------
          Clear
  ---------------------------------------------------------------*/
void CSimplePointsMap::internal_clear()
{
  // This swap() thing is the only way to really deallocate the memory.
  vector_strong_clear(m_x);
  vector_strong_clear(m_y);
  vector_strong_clear(m_z);

  mark_as_modified();
}

void CSimplePointsMap::insertPointFast(float x, float y, float z)
{
  m_x.push_back(x);
  m_y.push_back(y);
  m_z.push_back(z);
}

namespace mrpt::maps::detail
{
using mrpt::maps::CSimplePointsMap;

template <>
struct pointmap_traits<CSimplePointsMap>
{
  /** Helper method fot the generic implementation of
   * CPointsMap::loadFromRangeScan(), to be called only once before inserting
   * points - this is the place to reserve memory in lric for extra working
   * variables. */
  inline static void internal_loadFromRangeScan2D_init(
      [[maybe_unused]] CSimplePointsMap& me,
      [[maybe_unused]] mrpt::maps::CPointsMap::TLaserRange2DInsertContext& lric)
  {
  }
  /** Helper method fot the generic implementation of
   * CPointsMap::loadFromRangeScan(), to be called once per range data */
  inline static void internal_loadFromRangeScan2D_prepareOneRange(
      [[maybe_unused]] CSimplePointsMap& me,
      [[maybe_unused]] const float gx,
      [[maybe_unused]] const float gy,
      [[maybe_unused]] const float gz,
      [[maybe_unused]] mrpt::maps::CPointsMap::TLaserRange2DInsertContext& lric)
  {
  }
  /** Helper method fot the generic implementation of
   * CPointsMap::loadFromRangeScan(), to be called after each
   * "{x,y,z}.push_back(...);" */
  inline static void internal_loadFromRangeScan2D_postPushBack(
      [[maybe_unused]] CSimplePointsMap& me,
      [[maybe_unused]] mrpt::maps::CPointsMap::TLaserRange2DInsertContext& lric)
  {
  }

  /** Helper method fot the generic implementation of
   * CPointsMap::loadFromRangeScan(), to be called only once before inserting
   * points - this is the place to reserve memory in lric for extra working
   * variables. */
  inline static void internal_loadFromRangeScan3D_init(
      [[maybe_unused]] CSimplePointsMap& me,
      [[maybe_unused]] mrpt::maps::CPointsMap::TLaserRange3DInsertContext& lric)
  {
  }
  /** Helper method fot the generic implementation of
   * CPointsMap::loadFromRangeScan(), to be called once per range data */
  inline static void internal_loadFromRangeScan3D_prepareOneRange(
      [[maybe_unused]] CSimplePointsMap& me,
      [[maybe_unused]] const float gx,
      [[maybe_unused]] const float gy,
      [[maybe_unused]] const float gz,
      [[maybe_unused]] mrpt::maps::CPointsMap::TLaserRange3DInsertContext& lric)
  {
  }
  /** Helper method fot the generic implementation of
   * CPointsMap::loadFromRangeScan(), to be called after each
   * "{x,y,z}.push_back(...);" */
  inline static void internal_loadFromRangeScan3D_postPushBack(
      [[maybe_unused]] CSimplePointsMap& me,
      [[maybe_unused]] mrpt::maps::CPointsMap::TLaserRange3DInsertContext& lric)
  {
  }
  /** Helper method fot the generic implementation of
   * CPointsMap::loadFromRangeScan(), to be called once per range data, at the
   * end */
  inline static void internal_loadFromRangeScan3D_postOneRange(
      [[maybe_unused]] CSimplePointsMap& me,
      [[maybe_unused]] mrpt::maps::CPointsMap::TLaserRange3DInsertContext& lric)
  {
  }
};
}  // namespace mrpt::maps::detail
/** See CPointsMap::loadFromRangeScan() */
void CSimplePointsMap::loadFromRangeScan(
    const CObservation2DRangeScan& rangeScan,
    const std::optional<const mrpt::poses::CPose3D>& robotPose)
{
  mrpt::maps::detail::loadFromRangeImpl<CSimplePointsMap>::templ_loadFromRangeScan(
      *this, rangeScan, robotPose);
}

/** See CPointsMap::loadFromRangeScan() */
void CSimplePointsMap::loadFromRangeScan(
    const CObservation3DRangeScan& rangeScan,
    const std::optional<const mrpt::poses::CPose3D>& robotPose)
{
  mrpt::maps::detail::loadFromRangeImpl<CSimplePointsMap>::templ_loadFromRangeScan(
      *this, rangeScan, robotPose);
}

// ================================ PLY files import & export virtual methods
// ================================

/** In a base class, reserve memory to prepare subsequent calls to
 * PLY_import_set_vertex */
void CSimplePointsMap::PLY_import_set_vertex_count(size_t N) { this->setSize(N); }
