/* +------------------------------------------------------------------------+
   |                     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          |
   +------------------------------------------------------------------------+ */
#pragma once

/** \file Include this file in your user application only if you have libLAS
 * installed in your system */
#include <mrpt/maps/CPointsMap.h>

#include <fstream>
#include <iostream>
#include <liblas/liblas.hpp>
#include <liblas/reader.hpp>
#include <liblas/writer.hpp>
#include <string>

namespace mrpt
{
/** \ingroup mrpt_maps_grp */
namespace maps
{
/** \addtogroup mrpt_maps_liblas_grp libLAS interface for CPointsMap (in
 * #include <mrpt/maps/CPointsMaps_liblas.h>)
 *  \ingroup mrpt_maps_grp
 * @{ */

/** Optional settings for saveLASFile() */
struct LAS_WriteParams
{
  // None.
};

/** Optional settings for loadLASFile() */
struct LAS_LoadParams
{
  // None.
};

/** Extra information gathered from the LAS file header */
struct LAS_HeaderInfo
{
  std::string FileSignature;
  std::string SystemIdentifier;
  std::string SoftwareIdentifier;
  std::string project_guid;
  /** Proj.4 string describing the Spatial Reference System. */
  std::string spatial_reference_proj4;
  /** Creation date (Year number) */
  uint16_t creation_year;
  /** Creation day of year */
  uint16_t creation_DOY;

  LAS_HeaderInfo() : creation_year(0), creation_DOY(0) {}
};

/** Save the point cloud as an ASPRS LAS binary file (requires MRPT built
 * against liblas). Refer to http://www.liblas.org/
 * \return false on any error */
template <class POINTSMAP>
bool saveLASFile(
    const POINTSMAP& ptmap,
    const std::string& filename,
    const LAS_WriteParams& params = LAS_WriteParams())
{
  std::ofstream ofs;
  ofs.open(filename.c_str(), std::ios::out | std::ios::binary);

  if (!ofs.is_open())
  {
    std::cerr << "[saveLASFile] Couldn't write to file: " << filename << std::endl;
    return false;
  }

  // Fill-in header:
  // ---------------------------------
  liblas::Header header;
  const size_t nPts = ptmap.size();

  header.SetPointRecordsCount(nPts);

  // Create writer:
  // ---------------------------------
  liblas::Writer writer(ofs, header);

  const bool has_color = ptmap.hasColorPoints();
  const float col_fract = 255.0f;

  liblas::Point pt(&header);
  liblas::Color col;
  for (size_t i = 0; i < nPts; i++)
  {
    float x, y, z, R, G, B;
    ptmap.getPoint(i, x, y, z, R, G, B);

    pt.SetX(x);
    pt.SetY(y);
    pt.SetZ(z);

    if (has_color)
    {
      col.SetRed(static_cast<uint16_t>(R * col_fract));
      col.SetGreen(static_cast<uint16_t>(G * col_fract));
      col.SetBlue(static_cast<uint16_t>(B * col_fract));
      pt.SetColor(col);
    }

    if (!writer.WritePoint(pt))
    {
      std::cerr << "[saveLASFile] liblas returned error writing point #" << i << " to file.\n";
      return false;
    }
  }
  return true;  // All ok.
}

/** Load the point cloud from an ASPRS LAS binary file (requires MRPT built
 * against liblas). Refer to http://www.liblas.org/
 * \note Color (RGB) information will be taken into account if using the
 * derived class mrpt::maps::CColouredPointsMap
 * \return false on any error */
template <class POINTSMAP>
bool loadLASFile(
    POINTSMAP& ptmap,
    const std::string& filename,
    LAS_HeaderInfo& out_headerInfo,
    const LAS_LoadParams& params = LAS_LoadParams())
{
  using namespace std;
  ptmap.clear();

  std::ifstream ifs;
  ifs.open(filename.c_str(), std::ios::in | std::ios::binary);

  if (!ifs.is_open())
  {
    std::cerr << "[loadLASFile] Couldn't open file: " << filename << std::endl;
    return false;
  }

  // Create LAS reader:
  // ---------------------
  liblas::Reader reader(ifs);

  // Parse header info:
  // ---------------------
  liblas::Header const& header = reader.GetHeader();
  const size_t nPts = header.GetPointRecordsCount();
  ptmap.reserve(nPts);

  out_headerInfo.FileSignature = header.GetFileSignature();
  out_headerInfo.SystemIdentifier = header.GetSystemId();
  out_headerInfo.SoftwareIdentifier = header.GetSoftwareId();
#if LIBLAS_VERSION_NUM < 1800
  out_headerInfo.project_guid = header.GetProjectId().to_string();
#else
  out_headerInfo.project_guid = boost::lexical_cast<std::string>(header.GetProjectId());
#endif
  out_headerInfo.spatial_reference_proj4 = header.GetSRS().GetProj4();
  out_headerInfo.creation_year = header.GetCreationYear();
  out_headerInfo.creation_DOY = header.GetCreationDOY();

  // Load points:
  // ---------------------
  const bool has_color = ptmap.hasColorPoints();
  const float col_fract = 1.0f / 255.0f;
  while (reader.ReadNextPoint())
  {
    liblas::Point const& p = reader.GetPoint();

    if (has_color)
    {
      liblas::Color const& col = p.GetColor();
      ptmap.insertPointRGB(
          p.GetX(), p.GetY(), p.GetZ(), col.GetRed() * col_fract, col.GetGreen() * col_fract,
          col.GetBlue() * col_fract);
    }
    else
    {
      ptmap.insertPoint(p.GetX(), p.GetY(), p.GetZ());
    }
  }

  if (ptmap.size() != nPts)
    cerr << "[loadLASFile] Header says point count is " << nPts << " but only " << ptmap.size()
         << " were really parsed in.\n";

  ptmap.mark_as_modified();

  return true;  // All ok.
}
/** @} */
}  // namespace maps
}  // namespace mrpt
