/* +------------------------------------------------------------------------+
|                     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/io/CFileGZInputStream.h>
#include <mrpt/io/CFileGZOutputStream.h>
#include <mrpt/io/CFileInputStream.h>
#include <mrpt/maps/CPointsMapXYZI.h>
#include <mrpt/obs/CObservation3DRangeScan.h>
#include <mrpt/opengl/CPointCloudColoured.h>
#include <mrpt/serialization/aligned_serialization.h>
#include <mrpt/system/filesystem.h>
#include <mrpt/system/os.h>

#include <iostream>

#include "CPointsMap_crtp_common.h"

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

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

CPointsMapXYZI::TMapDefinition::TMapDefinition() = default;

void CPointsMapXYZI::TMapDefinition::loadFromConfigFile_map_specific(
    const CConfigFileBase& source, const std::string& sectionNamePrefix)
{
  insertionOpts.loadFromConfigFile(source, sectionNamePrefix + string("_insertOpts"));
  likelihoodOpts.loadFromConfigFile(source, sectionNamePrefix + string("_likelihoodOpts"));
}

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

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

IMPLEMENTS_SERIALIZABLE(CPointsMapXYZI, CPointsMap, mrpt::maps)

void CPointsMapXYZI::reserve(size_t newLength)
{
  m_x.reserve(newLength);
  m_y.reserve(newLength);
  m_z.reserve(newLength);
  m_intensity.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 CPointsMapXYZI::resize(size_t newLength)
{
  m_x.resize(newLength, 0);
  m_y.resize(newLength, 0);
  m_z.resize(newLength, 0);
  m_intensity.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 CPointsMapXYZI::setSize(size_t newLength)
{
  m_x.assign(newLength, 0);
  m_y.assign(newLength, 0);
  m_z.assign(newLength, 0);
  m_intensity.assign(newLength, 0);
  mark_as_modified();
}

uint8_t CPointsMapXYZI::serializeGetVersion() const { return 0; }
void CPointsMapXYZI::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.WriteBufferFixEndianness(&m_intensity[0], n);
  }
  insertionOptions.writeToStream(out);
  likelihoodOptions.writeToStream(out);
}

void CPointsMapXYZI::serializeFrom(mrpt::serialization::CArchive& in, uint8_t version)
{
  switch (version)
  {
    case 0:
    {
      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);
        in.ReadBufferFixEndianness(&m_intensity[0], n);
      }
      insertionOptions.readFromStream(in);
      likelihoodOptions.readFromStream(in);
    }
    break;
    default:
      MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version);
  };
}

void CPointsMapXYZI::internal_clear()
{
  vector_strong_clear(m_x);
  vector_strong_clear(m_y);
  vector_strong_clear(m_z);
  vector_strong_clear(m_intensity);
  mark_as_modified();
}

void CPointsMapXYZI::setPointRGB(size_t index, float x, float y, float z, float R, float G, float B)
{
  if (index >= m_x.size()) THROW_EXCEPTION("Index out of bounds");
  m_x[index] = x;
  m_y[index] = y;
  m_z[index] = z;
  m_intensity[index] = R;
  mark_as_modified();
}

void CPointsMapXYZI::setPointIntensity(size_t index, float I)
{
  if (index >= m_intensity.size()) THROW_EXCEPTION("Index out of bounds");
  this->m_intensity[index] = I;
  // mark_as_modified();  // No need to rebuild KD-trees, etc...
}

void CPointsMapXYZI::insertPointFast(float x, float y, float z)
{
  m_x.push_back(x);
  m_y.push_back(y);
  m_z.push_back(z);
  // mark_as_modified(); Don't, this is the "XXXFast()" method
}

void CPointsMapXYZI::insertPointRGB(
    float x, float y, float z, float R_intensity, float G_ignored, float B_ignored)
{
  m_x.push_back(x);
  m_y.push_back(y);
  m_z.push_back(z);
  m_intensity.push_back(R_intensity);
  mark_as_modified();
}

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

  auto obj = mrpt::opengl::CPointCloudColoured::Create();

  obj->loadFromPointsMap(this);
  obj->setColor(1, 1, 1, 1.0);
  obj->setPointSize(this->renderOptions.point_size);

  o.insert(obj);
}

void CPointsMapXYZI::getPointRGB(
    size_t index, float& x, float& y, float& z, float& R, float& G, float& B) const
{
  ASSERT_LT_(index, m_x.size());
  ASSERT_LT_(index, m_intensity.size());

  x = m_x[index];
  y = m_y[index];
  z = m_z[index];
  R = G = B = m_intensity[index];
}

float CPointsMapXYZI::getPointIntensity(size_t index) const
{
  ASSERT_LT_(index, m_intensity.size());
  return m_intensity[index];
}

bool CPointsMapXYZI::saveXYZI_to_text_file(const std::string& file) const
{
  FILE* f = os::fopen(file.c_str(), "wt");
  if (!f) return false;
  for (unsigned int i = 0; i < m_x.size(); i++)
    os::fprintf(f, "%f %f %f %f\n", m_x[i], m_y[i], m_z[i], m_intensity[i]);
  os::fclose(f);
  return true;
}

bool CPointsMapXYZI::loadXYZI_from_text_file(const std::string& file)
{
  MRPT_START

  // Clear current map:
  mark_as_modified();
  this->clear();

  std::ifstream f;
  f.open(file);
  if (!f.is_open()) return false;

  while (!f.eof())
  {
    std::string line;
    std::getline(f, line);

    std::stringstream ss(line);

    float x, y, z, i;
    if (!(ss >> x >> y >> z >> i))
    {
      break;
    }

    insertPointFast(x, y, z);
    m_intensity.push_back(i);
  }

  return true;

  MRPT_END
}

/*---------------------------------------------------------------
addFrom_classSpecific
---------------------------------------------------------------*/
void CPointsMapXYZI::addFrom_classSpecific(
    const CPointsMap& anotherMap, size_t nPreviousPoints, const bool filterOutPointsAtZero)
{
  const size_t nOther = anotherMap.size();

  // Specific data for this class:
  const auto* anotheMap_col = dynamic_cast<const CPointsMapXYZI*>(&anotherMap);

  if (anotheMap_col)
  {
    for (size_t i = 0, j = nPreviousPoints; i < nOther; i++)
    {
      if (filterOutPointsAtZero && anotheMap_col->getPointsBufferRef_x()[i] == 0 &&
          anotheMap_col->getPointsBufferRef_y()[i] == 0 &&
          anotheMap_col->getPointsBufferRef_z()[i] == 0)
        continue;

      m_intensity[j] = anotheMap_col->m_intensity[i];
      j++;
    }
  }
}

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

template <>
struct pointmap_traits<CPointsMapXYZI>
{
  /** 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(
      CPointsMapXYZI& me, mrpt::maps::CPointsMap::TLaserRange2DInsertContext& lric)
  {
    // lric.fVars: not needed
  }

  /** Helper method fot the generic implementation of
   * CPointsMap::loadFromRangeScan(), to be called once per range data */
  inline static void internal_loadFromRangeScan2D_prepareOneRange(
      CPointsMapXYZI& me,
      [[maybe_unused]] const float gx,
      [[maybe_unused]] const float gy,
      [[maybe_unused]] const float gz,
      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(
      CPointsMapXYZI& me, mrpt::maps::CPointsMap::TLaserRange2DInsertContext& lric)
  {
    float pI = 1.0f;
    me.m_intensity.push_back(pI);
  }

  /** 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(
      CPointsMapXYZI& me, mrpt::maps::CPointsMap::TLaserRange3DInsertContext& lric)
  {
    // Not used.
  }

  /** Helper method fot the generic implementation of
   * CPointsMap::loadFromRangeScan(), to be called once per range data */
  inline static void internal_loadFromRangeScan3D_prepareOneRange(
      CPointsMapXYZI& me,
      [[maybe_unused]] const float gx,
      [[maybe_unused]] const float gy,
      [[maybe_unused]] const float gz,
      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(
      CPointsMapXYZI& me, mrpt::maps::CPointsMap::TLaserRange3DInsertContext& lric)
  {
    const float pI = 1.0f;
    me.m_intensity.push_back(pI);
  }

  /** 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(
      CPointsMapXYZI& me, mrpt::maps::CPointsMap::TLaserRange3DInsertContext& lric)
  {
  }
};
}  // namespace mrpt::maps::detail
/** See CPointsMap::loadFromRangeScan() */
void CPointsMapXYZI::loadFromRangeScan(
    const CObservation2DRangeScan& rangeScan,
    const std::optional<const mrpt::poses::CPose3D>& robotPose)
{
  mrpt::maps::detail::loadFromRangeImpl<CPointsMapXYZI>::templ_loadFromRangeScan(
      *this, rangeScan, robotPose);
}

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

// ====PLY files import & export virtual methods
void CPointsMapXYZI::PLY_import_set_vertex_count(size_t N) { this->setSize(N); }

void CPointsMapXYZI::PLY_import_set_vertex(
    size_t idx, const mrpt::math::TPoint3Df& pt, const TColorf* pt_color)
{
  if (pt_color)
    this->setPointRGB(idx, pt.x, pt.y, pt.z, pt_color->R, pt_color->G, pt_color->B);
  else
    this->setPoint(idx, pt.x, pt.y, pt.z);
}

void CPointsMapXYZI::PLY_export_get_vertex(
    size_t idx, mrpt::math::TPoint3Df& pt, bool& pt_has_color, TColorf& pt_color) const
{
  pt_has_color = true;

  pt.x = m_x[idx];
  pt.y = m_y[idx];
  pt.z = m_z[idx];
  pt_color.R = pt_color.G = pt_color.B = m_intensity[idx];
}

bool CPointsMapXYZI::loadFromKittiVelodyneFile(const std::string& filename)
{
  try
  {
    mrpt::io::CFileGZInputStream f_gz;
    mrpt::io::CFileInputStream f_normal;
    mrpt::io::CStream* f = nullptr;

    if (std::string("gz") == mrpt::system::extractFileExtension(filename))
    {
      if (f_gz.open(filename)) f = &f_gz;
    }
    else
    {
      if (f_normal.open(filename)) f = &f_normal;
    }
    if (!f) THROW_EXCEPTION_FMT("Could not open thefile: `%s`", filename.c_str());

    this->clear();
    this->reserve(10000);

    for (;;)
    {
      constexpr std::size_t nToRead = sizeof(float) * 4;
      float xyzi[4];
      std::size_t nRead = f->Read(&xyzi, nToRead);
      if (nRead == 0)
        break;  // EOF
      else if (nRead == nToRead)
      {
        m_x.push_back(xyzi[0]);
        m_y.push_back(xyzi[1]);
        m_z.push_back(xyzi[2]);
        m_intensity.push_back(xyzi[3]);
      }
      else
        throw std::runtime_error(
            "Unexpected EOF at the middle of a XYZI record "
            "(truncated or corrupted file?)");
    }
    this->mark_as_modified();
    return true;
  }
  catch (const std::exception& e)
  {
    std::cerr << "[loadFromKittiVelodyneFile] " << e.what() << std::endl;
    return false;
  }
}

bool CPointsMapXYZI::saveToKittiVelodyneFile(const std::string& filename) const
{
  try
  {
    mrpt::io::CFileGZOutputStream f(filename);

    for (size_t i = 0; i < m_x.size(); i++)
    {
      const float xyzi[4] = {m_x[i], m_y[i], m_z[i], m_intensity[i]};
      const auto toWrite = sizeof(float) * 4;
      std::size_t nWr = f.Write(&xyzi, toWrite);
      ASSERT_EQUAL_(nWr, toWrite);
    }
    return true;
  }
  catch (const std::exception& e)
  {
    std::cerr << "[saveToKittiVelodyneFile] " << e.what() << std::endl;
    return false;
  }
}
