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

namespace mrpt::obs
{
/** An observation of the current (cumulative) odometry for a wheeled robot.
 * This provides the relative pose of the robot with respect to the `odom`
 * frame of reference, in "ROS parlance".
 *
 * This kind of observation more naturally fits the "observation-only" rawlog
 * format, since odometry increments are normally used in "sensory-frame based"
 * datasets. However, the user is free to use them whenever it is useful. Refer
 * to the [rawlog format description](rawlog_format.html).
 *
 * \sa CObservation, CActionRobotMovement2D
 * \ingroup mrpt_obs_grp
 */
class CObservationOdometry : public CObservation
{
  DEFINE_SERIALIZABLE(CObservationOdometry, mrpt::obs)

 public:
  /** Default ctor */
  CObservationOdometry();

  /** The absolute odometry measurement (IT IS NOT INCREMENTAL) */
  mrpt::poses::CPose2D odometry;

  /** "true" means that "encoderLeftTicks" and "encoderRightTicks" contain
   * valid values. */
  bool hasEncodersInfo{false};
  /** For differential-driven robots: The ticks count for each wheel in
   * ABSOLUTE VALUE (IT IS NOT INCREMENTAL) (positive means FORWARD, for both
   * wheels); \sa hasEncodersInfo  */
  int32_t encoderLeftTicks{0}, encoderRightTicks{0};

  /** "true" means that `velocityLocal` contains valid values. */
  bool hasVelocities{false};
  /** Velocity, in the robot (local) frame of reference (+X=forward). */
  mrpt::math::TTwist2D velocityLocal;

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

  // See base class docs:
  bool exportTxtSupported() const override { return true; }
  std::string exportTxtHeader() const override;
  std::string exportTxtDataRow() const override;

};  // End of class def.

}  // namespace mrpt::obs
