/* +------------------------------------------------------------------------+
   |                     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/img/TCamera.h>
#include <mrpt/obs/CObservation.h>
#include <mrpt/poses/CPose2D.h>
#include <mrpt/poses/CPose3D.h>
#include <mrpt/poses/CPose3DQuat.h>
#include <mrpt/serialization/CSerializable.h>

namespace mrpt::obs
{
struct TStereoImageFeatures
{
  std::pair<mrpt::img::TPixelCoordf, mrpt::img::TPixelCoordf> pixels;
  unsigned int ID;
};

/** Declares a class derived from "CObservation" that encapsules a pair of
 cameras and a set of matched image features extracted from them.
 *
 <b>NOTE:</b> The image features stored in this class are NOT supposed to be
 UNDISTORTED, but the TCamera members must provide their distortion params.
 A zero-vector of distortion params means a set of UNDISTORTED pixels.<br>
 * \sa CObservation
 * \ingroup mrpt_obs_grp
 */
class CObservationStereoImagesFeatures : public CObservation
{
  DEFINE_SERIALIZABLE(CObservationStereoImagesFeatures, mrpt::obs)

 public:
  CObservationStereoImagesFeatures() = default;
  /** Other constructor providing members initialization.
   */
  CObservationStereoImagesFeatures(
      const mrpt::img::TCamera& cLeft /*left camera*/,
      const mrpt::img::TCamera& cRight /*right camera*/,
      const mrpt::poses::CPose3DQuat& rCPose /*rightCameraPose*/,
      const mrpt::poses::CPose3DQuat& cPORobot /*cameraPoseOnRobot*/);

  /** A method for storing the set of observed features in a text file in the
   * format: <br>
   * ID ul vl ur vr <br>
   * being (ul,vl) and (ur,vr) the "x" and "y" coordinates for the left and
   * right feature, respectively.
   */
  void saveFeaturesToTextFile(const std::string& filename);

  // ------------------
  // Class Members
  // ------------------
  mrpt::img::TCamera cameraLeft, cameraRight;

  /** The pose of the right camera, relative to the left one:
   *  Note that for the Bumblebee stereo camera and using the conventional
   * reference coordinates for the left
   *   camera ("x" points to the right, "y" down), the "right" camera is
   * situated
   *   at position (BL, 0, 0) with q = [1 0 0 0], where BL is the BASELINE.
   */
  mrpt::poses::CPose3DQuat rightCameraPose;

  /** The pose of the LEFT camera, relative to the robot.
   */
  mrpt::poses::CPose3DQuat cameraPoseOnRobot;

  /** Vectors of image feature pairs (with ID).
   */
  std::vector<TStereoImageFeatures> theFeatures;

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

  /** A general method to change the sensor pose on the robot in a
   * mrpt::poses::CPose3D form.
   *  Note that most sensors will use the full (6D) CPose3DQuat, but see the
   * derived classes for more details or special cases.
   * \sa getSensorPose
   */
  inline void setSensorPose(const mrpt::poses::CPose3D& newSensorPose) override
  {
    cameraPoseOnRobot = mrpt::poses::CPose3DQuat(newSensorPose);
  }

  /** A general method to change the sensor pose on the robot in a CPose3DQuat
   * form.
   *  Note that most sensors will use the full (6D) CPose3DQuat, but see the
   * derived classes for more details or special cases.
   * \sa getSensorPose
   */
  inline void setSensorPose(const mrpt::poses::CPose3DQuat& newSensorPose)
  {
    cameraPoseOnRobot = newSensorPose;
  }
};  // End of class def.

}  // namespace mrpt::obs
