/* +------------------------------------------------------------------------+
   |                     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/obs/CActionCollection.h>
#include <mrpt/obs/CObservation.h>
#include <mrpt/obs/CSensoryFrame.h>

#include "CRegistrationDeciderOrOptimizer.h"

namespace mrpt::graphslam::deciders
{
/**\brief Interface for implementing node registration classes.
 *
 * CNodeRegistrationDecider provides the basic methods that have to exist in
 * every node registration decider class. For an example of inheriting from
 * this class see CFixedIntervalsNRD.
 *
 * \note As a naming convention, all the implemented node registration deciders
 * are suffixed with the NRD acronym.
 *
 * \ingroup mrpt_graphslam_grp
 */
template <class GRAPH_T>
class CNodeRegistrationDecider :
    public virtual mrpt::graphslam::CRegistrationDeciderOrOptimizer<GRAPH_T>
{
 public:
  /**\brief Handy typedefs */
  /**\{*/
  /**\brief Parent of current class */
  using parent_t = mrpt::graphslam::CRegistrationDeciderOrOptimizer<GRAPH_T>;
  /**\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 inf_mat_t =
      mrpt::math::CMatrixFixed<double, constraint_t::state_length, constraint_t::state_length>;
  /**\}*/

  /**\brief Default class constructor.*/
  CNodeRegistrationDecider();
  /**\brief Default class destructor.*/
  ~CNodeRegistrationDecider() override = default;
  /**\brief Getter method for fetching the currently estimated robot position.
   *
   * In single-robot situations this is most likely going to be the last
   * registered node position + an position/uncertainty increment from that
   * position
   */
  virtual global_pose_t getCurrentRobotPosEstimation() const;
  void getDescriptiveReport(std::string* report_str) const override;

 protected:
  /**\brief Reset the given PDF method and assign a fixed high-certainty
   * Covariance/Information matrix
   */
  void resetPDF(constraint_t* c);
  /**\brief Check whether a new node should be registered in the
   * graph.
   *
   * This should be the key-method in any implementation of this
   * interface. Should call registerNewNodeAtEnd method if the registration
   * condition is satisfied.
   *
   * \return True upon successful node registration in the graph
   */
  virtual bool checkRegistrationCondition();
  /**\brief Utility methods for adding new poses to the graph.
   */
  /**\{*/
  /** Add a new constraint at the end of the graph.
   * \param[in] constraint Constraint transformation from the latest
   * registered to the new node.
   *
   * \return True upon successful node registration.
   */
  bool registerNewNodeAtEnd(const typename GRAPH_T::constraint_t& constraint);
  /**\brief Same goal as the previous method - uses the m_since_prev_node_PDF
   * as the constraint at the end.
   */
  bool registerNewNodeAtEnd();
  /**\brief Get a global_pose_t and fill the NODE_ANNOTATIONS-related fields
   *
   * \note Users are encouraged to override this method in case they have
   * defined a new TNodeAnnotations struct and want to use this metadata in
   * the graph nodes.
   */
  virtual void addNodeAnnotsToPose(global_pose_t* pose) const;
  /**\}*/

  /**\brief Store the last registered NodeID.
   *
   * We don't store its pose since it will most likely change due to calls to
   * the
   * graph-optimization procedure / dijkstra_node_estimation
   */
  mrpt::graphs::TNodeID m_prev_registered_nodeID;
  /**\brief Tracking the PDF of the current position of the robot with
   * regards to the <b previous registered node</b>.
   */
  constraint_t m_since_prev_node_PDF;
  /**\brief Initial information matrix for paths
   *
   * Large values for this indicate that I am sure of the corresponding
   * (initial) pose
   */
  inf_mat_t m_init_inf_mat;
};
}  // namespace mrpt::graphslam::deciders
#include "CNodeRegistrationDecider_impl.h"
