/* +------------------------------------------------------------------------+
   |                     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/CLoadableOptions.h>
#include <mrpt/graphs/CNetworkOfPoses.h>

#include <string>

namespace mrpt::graphslam
{
/**\brief Holds the data of an information path.
 *
 * Path comprises of nodes (TNodeID type) and constraints between them.
 * Only consecutive nodes are connected by edges, thus \em path.
 *
 * \sa mrpt::deciders::CLoopCloserERD
 * \ingroup mrpt_graphslam_grp
 */
template <class GRAPH_T = typename mrpt::graphs::CNetworkOfPoses2DInf>
struct TUncertaintyPath : public mrpt::config::CLoadableOptions
{
  using constraint_t = typename GRAPH_T::constraint_t;
  using pose_t = typename constraint_t::type_value;
  using self_t = TUncertaintyPath<GRAPH_T>;

  TUncertaintyPath();
  TUncertaintyPath(const mrpt::graphs::TNodeID& starting_node);
  TUncertaintyPath(
      const mrpt::graphs::TNodeID& starting_node,
      const mrpt::graphs::TNodeID& ending_node,
      const constraint_t& edge);
  ~TUncertaintyPath() override = default;
  void clear();
  bool isEmpty() const;
  /**\brief Assert that the current path is between the given nodeIDs.
   *
   * Call to this method practically checks if the give nodes match the source
   * and destination nodeIDs.
   *
   * \note Assertions will be executed only in \b Debug builds
   *
   * \exception std::runtime_error in case the conditions don't hold
   */
  void assertIsBetweenNodeIDs(
      const mrpt::graphs::TNodeID& from, const mrpt::graphs::TNodeID& to) const;

  // no need to load anything..
  void loadFromConfigFile(
      const mrpt::config::CConfigFileBase& source, const std::string& section) override;
  void dumpToTextStream(std::ostream& out) const override;
  std::string getAsString() const;
  void getAsString(std::string* str) const;

  /**\brief Return the source node of this path */
  const mrpt::graphs::TNodeID& getSource() const;
  /**\brief Return the Destination node of this path */
  const mrpt::graphs::TNodeID& getDestination() const;
  double getDeterminant();
  /**\brief Test if the current path has a lower uncertainty than the other
   * path.
   *
   * \return True if the current path does have a lower uncertainty
   */
  bool hasLowerUncertaintyThan(const self_t& other) const;
  /**\brief add a new link in the current path.
   *
   * Add the node that the path traverses and the information matrix of
   * the extra link
   */
  void addToPath(const mrpt::graphs::TNodeID& node, const constraint_t& edge);
  self_t& operator+=(const self_t& other);
  // results...
  bool operator==(const self_t& other) const;
  bool operator!=(const self_t& other) const;

  friend std::ostream& operator<<(std::ostream& o, const self_t& obj)
  {
    o << obj.getAsString() << endl;
    return o;
  }

  /**\brief Nodes that the Path comprises of.
   *
   * Nodes in the path are added to the end of the vector.
   */
  std::vector<mrpt::graphs::TNodeID> nodes_traversed;
  /**\brief Current path position + corresponding covariance */
  constraint_t curr_pose_pdf;

  /**Determine whether the determinant of the Path is up-to-date and
   * can be directly fetched or has to be computed again */
  /**\{*/
  bool determinant_is_updated;
  double determinant_cached;
  /**\}*/
};
}  // namespace mrpt::graphslam
#include "TUncertaintyPath_impl.h"
