/* +------------------------------------------------------------------------+
   |                     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

#include <mrpt/img/CImage.h>
#include <mrpt/math/CMatrixF.h>
#include <mrpt/math/CPolygon.h>
#include <mrpt/obs/CObservation.h>
#include <mrpt/obs/CObservation2DRangeScan.h>
#include <mrpt/opengl/pointcloud_adapters.h>
#include <mrpt/poses/CPose2D.h>
#include <mrpt/poses/CPose3D.h>
#include <mrpt/serialization/CSerializable.h>

namespace mrpt
{
namespace obs
{
//	namespace detail {
//		// Implemented in CObservationRGBD360_project3D_impl.h
//		template <class POINTMAP>
//		void unprojectInto(CObservationRGBD360 &src_obs,
// POINTMAP &dest_pointcloud, const bool takeIntoAccountSensorPoseOnRobot, const
// mrpt::poses::CPose3D * robotPoseInTheWorld, const bool PROJ3D_USE_LUT);
//	}

/** Declares a class derived from "CObservation" that
 *      encapsules an omnidirectional RGBD measurement from a set of RGBD
 *sensors.
 *  This kind of observations can carry one or more of these data fields:
 *    - 3D point cloud (as float's).
 *    - 2D range image (as a matrix): Each entry in the matrix
 *"rangeImage(ROW,COLUMN)" contains a distance or a depth (in meters), depending
 *on \a range_is_depth.
 *    - 2D intensity (grayscale or RGB) image (as a mrpt::img::CImage): For
 *SwissRanger cameras, a logarithmic A-law compression is used to convert the
 *original 16bit intensity to a more standard 8bit graylevel.
 *
 *  The coordinates of the 3D point cloud are in millimeters with respect to the
 *RGB camera origin of coordinates
 *
 *  The 2D images and matrices are stored as common images, with an up->down
 *rows order and left->right, as usual.
 *   Optionally, the intensity and confidence channels can be set to
 *delayed-load images for off-rawlog storage so it saves
 *   memory by having loaded in memory just the needed images. See the methods
 *load() and unload().
 *  Due to the intensive storage requirements of this kind of observations, this
 *observation is the only one in MRPT
 *   for which it's recommended to always call "load()" and "unload()" before
 *and after using the observation, *ONLY* when
 *   the observation was read from a rawlog dataset, in order to make sure that
 *all the externally stored data fields are
 *   loaded and ready in memory.
 *
 *  Classes that grab observations of this type are:
 *		- mrpt::hwdrivers::CSwissRanger3DCamera
 *		- mrpt::hwdrivers::CKinect
 *
 *
 *  3D point clouds can be generated at any moment after grabbing with
 *CObservationRGBD360::unprojectInto() and
 *CObservationRGBD360::unprojectInto(), provided the correct
 *   calibration parameters.
 *
 *  \note Starting at serialization version 3 (MRPT 0.9.1+), the 3D point cloud
 *and the rangeImage can both be stored externally to save rawlog space.
 *  \note Starting at serialization version 5 (MRPT 0.9.5+), the new field \a
 *range_is_depth
 *  \note Starting at serialization version 6 (MRPT 0.9.5+), the new field \a
 *intensityImageChannel
 *
 * \sa mrpt::hwdrivers::CSwissRanger3DCamera, mrpt::hwdrivers::COpenNI2_RGBD360,
 *CObservation
 * \ingroup mrpt_obs_grp
 */
class CObservationRGBD360 : public CObservation
{
  DEFINE_SERIALIZABLE(CObservationRGBD360, mrpt::obs)

 protected:
  /** If set to true, m_points3D_external_file is valid. */
  bool m_points3D_external_stored{false};
  /** 3D points are in CImage::getImagesPathBase()+ this_file_name */
  std::string m_points3D_external_file;

  /** If set to true, m_rangeImage_external_file is valid. */
  bool m_rangeImage_external_stored{false};
  /** rangeImage is in CImage::getImagesPathBase()+ this_file_name */
  std::string m_rangeImage_external_file;

 public:
  /** Default constructor */
  CObservationRGBD360();
  /** Destructor */
  ~CObservationRGBD360() override;

  static const unsigned int NUM_SENSORS = 2;

  mrpt::system::TTimeStamp timestamps[NUM_SENSORS];

  /** true means the field rangeImage contains valid data */
  bool hasRangeImage{false};
  /** If hasRangeImage=true, a matrix of floats with the range data as
   * captured by the camera (in meters) \sa range_is_depth */
  mrpt::math::CMatrix_u16 rangeImages[NUM_SENSORS];

  /** Units for integer depth values in rangeImages */
  float rangeUnits = 0.001f;

  /** Similar to calling "rangeImage.setSize(H,W)" but this method provides
   * memory pooling to speed-up the memory allocation. */
  void rangeImage_setSize(const int HEIGHT, const int WIDTH, const unsigned sensor_id);

  /** true means the field intensityImage contains valid data */
  bool hasIntensityImage{false};
  /** If hasIntensityImage=true, a color or gray-level intensity image of the
   * same size than "rangeImage" */
  mrpt::img::CImage intensityImages[NUM_SENSORS];
  /** Projection parameters of the 8 RGBD sensor. */
  mrpt::img::TCamera sensorParamss[NUM_SENSORS];

  /** The maximum range allowed by the device, in meters (e.g. 8.0m, 5.0m,...)
   */
  float maxRange{10.0f};
  /** The 6D pose of the sensor on the robot. */
  mrpt::poses::CPose3D sensorPose;
  /** The "sigma" error of the device in meters, used while inserting the scan
   * in an occupancy grid. */
  float stdError{0.01f};

  // See base class docs
  void getSensorPose(mrpt::poses::CPose3D& out_sensorPose) const override
  {
    out_sensorPose = sensorPose;
  }
  void setSensorPose(const mrpt::poses::CPose3D& newSensorPose) override
  {
    sensorPose = newSensorPose;
  }
  void getDescriptionAsText(std::ostream& o) const override;

};  // End of class def.

}  // namespace obs

}  // namespace mrpt
