/* +------------------------------------------------------------------------+
   |                     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/config/CConfigFileBase.h>
#include <mrpt/config/CLoadableOptions.h>
#include <mrpt/graphslam/interfaces/CNodeRegistrationDecider.h>
#include <mrpt/graphslam/misc/CRangeScanOps.h>
#include <mrpt/graphslam/misc/TSlidingWindow.h>
#include <mrpt/obs/CActionCollection.h>
#include <mrpt/obs/CObservation2DRangeScan.h>
#include <mrpt/obs/CObservation3DRangeScan.h>
#include <mrpt/obs/CSensoryFrame.h>

#include <string>

namespace mrpt::graphslam::deciders
{
/**\brief ICP-based Fixed Intervals Node Registration
 *
 * ## Description
 *
 * Current Decider is meant for adding nodes in 2D datasets recorded using
 * a laser range finder or RGB-D camera (e.g. Kinect). No odometry data from
 * encoders is needed. Using ICP to match consecutive RangeScan measurements,
 * the decider keeps track of the pose transformation since the last registered
 * node. If the norm or the angle of the latter surpasses certain thresholds
 * (which are read from an external .ini file) then a new node is added to the
 * graph)
 * \sa loadParams, TParams::loadFromConfigFile
 *
 * Decider *does not guarantee* thread safety when accessing the GRAPH_T
 * resource. This is handled by the CGraphSlamEngine class.
 *
 * ### Specifications
 *
 * - Map type: 2D
 * - MRPT rawlog format: #1, #2
 * - Graph Type: CPosePDFGaussianInf
 * - Observations Used: CObservation2DRangeScan, CObservation3DRangeScan
 * - Node Registration Strategy: Fixed Intervals
 *
 * ### .ini Configuration Parameters
 *
 * \htmlinclude config_params_preamble.txt
 *
 * - \b class_verbosity
 *   + \a Section       : NodeRegistrationDeciderParameters
 *   + \a default value : 1 (mrpt::system::LVL_INFO)
 *   + \a Required      : FALSE
 *
 * - \b registration_max_distance
 *  + \a Section       : NodeRegistrationDeciderParameters
 *  + \a Default value : 0.5 // meters
 *  + \a Required      : FALSE
 *
 * - \b registration_max_angle
 *  + \a Section       : NodeRegistrationDeciderParameters
 *  + \a Default value : 10 // degrees
 *  + \a Required      : FALSE
 *
 * \note Since the decider inherits from the CRangeScanOps
 * class, it parses the configuration parameters of the latter as well from the
 * "ICP" section. Refer to the CRangeScanOps documentation for
 * its list of configuration
 * parameters
 *
 * \note Class contains an instance of the TSlidingWindow class and it parses
 * the configuration parameters of the latter from the
 * "NodeRegistrationDeciderParameters" section. Refer to TSlidingWindow
 * documentation for its list of configuration parameters
 *
 * \ingroup mrpt_graphslam_grp
 */
template <class GRAPH_T>
class CICPCriteriaNRD :
    public virtual mrpt::graphslam::deciders::CNodeRegistrationDecider<GRAPH_T>,
    public mrpt::graphslam::deciders::CRangeScanOps<GRAPH_T>
{
 public:
  /**\brief Handy typedefs */
  /**\{*/
  /**\brief type of graph constraints */
  using constraint_t = typename GRAPH_T::constraint_t;
  /**\brief type of underlying poses (2D/3D). */
  using pose_t = typename GRAPH_T::constraint_t::type_value;
  using global_pose_t = typename GRAPH_T::global_pose_t;

  using InfMat =
      mrpt::math::CMatrixFixed<double, constraint_t::state_length, constraint_t::state_length>;
  /**\brief Typedef for accessing methods of the RangeScanRegistrationDecider
   * parent class.
   */
  using range_ops_t = mrpt::graphslam::deciders::CRangeScanOps<GRAPH_T>;
  using decider_t = CICPCriteriaNRD<GRAPH_T>; /**< self type */
  /**\brief Node Registration Decider */
  using parent_t = mrpt::graphslam::deciders::CNodeRegistrationDecider<GRAPH_T>;
  /**\}*/

  CICPCriteriaNRD();
  ~CICPCriteriaNRD() override = default;

  void loadParams(const std::string& source_fname) override;
  void printParams() const override;
  void getDescriptiveReport(std::string* report_str) const override;

  /**\brief Update the decider state using the latest dataset measurements.
   *
   * \note Depending on the observations at hand, update of the state is
   * handled either by updateState2D, or by updateState3D methods. This
   * helps in separating the 2D, 3D RangeScans handling altogether, which in
   * turn simplifies the overall procedure
   *
   * Order of calls:
   * updateState (calls) ==> updateState2D/3D ==>
   * checkRegistrationCondition2D/3D ==> CheckRegistrationCondition
   *
   * \sa updateState2D, updateState3D
   */
  bool updateState(
      mrpt::obs::CActionCollection::Ptr action,
      mrpt::obs::CSensoryFrame::Ptr observations,
      mrpt::obs::CObservation::Ptr observation) override;
  /**\brief Specialized updateState method used solely when dealing with
   * 2DRangeScan information.
   * \sa updateState3D
   */
  bool updateState2D(mrpt::obs::CObservation2DRangeScan::Ptr observation);
  /**\brief Specialized updateState method used solely when dealing with
   * 3DRangeScan information.
   * \sa updateState2D
   */
  bool updateState3D(mrpt::obs::CObservation3DRangeScan::Ptr observation);

  struct TParams : public mrpt::config::CLoadableOptions
  {
   public:
    TParams(decider_t& d);
    ~TParams() override = default;

    decider_t& decider; /**< Reference to outer decider class */

    void loadFromConfigFile(
        const mrpt::config::CConfigFileBase& source, const std::string& section) override;
    void dumpToTextStream(std::ostream& out) const override;
    /** Maximum distance for new node registration */
    double registration_max_distance;
    /** Maximum angle difference for new node registration */
    double registration_max_angle;
  };

  TParams params;

 protected:
  bool checkRegistrationCondition() override;
  /**\brief Specialized checkRegistrationCondtion method used solely when
   * dealing with 2DRangeScan information
   * \sa checkRegistrationCondition3D
   */
  bool checkRegistrationCondition2D();
  /**\brief Specialized checkRegistrationCondition method used solely when
   * dealing with 3DRangeScan information
   * \sa checkRegistrationCondition2D
   */
  bool checkRegistrationCondition3D();

  bool m_is_using_3DScan;

  /**\brief handy laser scans to use in the class methods
   */
  /**\{ */
  /**\brief 2D LaserScan corresponding to the latest registered node in the
   * graph */
  mrpt::obs::CObservation2DRangeScan::Ptr m_last_laser_scan2D;
  /**\brief Current LaserScan. Set during the new measurements acquisition in
   * updateState method
   */
  mrpt::obs::CObservation2DRangeScan::Ptr m_curr_laser_scan2D;

  mrpt::obs::CObservation3DRangeScan::Ptr m_last_laser_scan3D;
  mrpt::obs::CObservation3DRangeScan::Ptr m_curr_laser_scan3D;
  /**\} */

  /**\brief Odometry rigid-body transformation since the last accepted
   * LaserScan.
   *
   * Decider can use it to smoothen the trajectory in the case of high noise
   * in the laser measurements
   */
  constraint_t m_latest_odometry_PDF;
  /**\brief pose_t estimation using only odometry information.
   * \note Utilized only in observation-only rawlogs.
   *
   */
  pose_t m_curr_odometry_only_pose;
  /**\brief pose_t estimation using only odometry information.
   * \note Utilized only in observation-only rawlogs.
   *
   * Resets next time an ICP edge/Odometry measurement is utilized for
   * updating the estimated robot position.
   */
  pose_t m_last_odometry_only_pose;
  /**\brief Keeps track of the last N measurements between the ICP edge and
   * the corresponding odometry measurements.
   *
   * Use the last odometry rigid body transformation instead of the
   * ICP edge if the mahalanobis distance between them is greater than this
   * limit.
   */
  TSlidingWindow m_mahal_distance_ICP_odom;

  // criteria for adding new a new node
  bool m_use_angle_difference_node_reg;
  bool m_use_distance_node_reg;

  /**How many times we used the ICP Edge instead of Odometry edge*/
  int m_times_used_ICP;
  /**How many times we used the Odometry Edge instead of the ICP edge */
  int m_times_used_odom;
};
}  // namespace mrpt::graphslam::deciders
#include "CICPCriteriaNRD_impl.h"
