/* +------------------------------------------------------------------------+
   |                     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/graphs/CNetworkOfPoses.h>
#include <mrpt/graphslam/misc/CWindowManager.h>
#include <mrpt/obs/CActionCollection.h>
#include <mrpt/obs/CObservation.h>
#include <mrpt/obs/CSensoryFrame.h>
#include <mrpt/system/COutputLogger.h>
#include <mrpt/system/CTimeLogger.h>

#include <map>
#include <string>

namespace mrpt::graphslam
{
/**\brief Interface for implementing node/edge registration deciders or
 * optimizer classes.
 *
 * Specific interfaces - for implementing node/edge deciders / optimizers -
 * can inherit from CRegistrationDeciderOrOptimizer so that they can make use
 * of the generic methods defined in the latter.
 *
 * \note \b Convention: For the already implemented deciders/optimizers the
 * following naming convention has been used:
 * - NRD: Node Registration Decider class
 * - ERD: Edge Registration Decider class
 * - GSO: GraphSlam Optimizer class
 *
 * \ingroup mrpt_graphslam_grp
 */
template <class GRAPH_T = typename mrpt::graphs::CNetworkOfPoses2DInf>
class CRegistrationDeciderOrOptimizer : public mrpt::system::COutputLogger
{
 public:
  CRegistrationDeciderOrOptimizer() = default;
  ~CRegistrationDeciderOrOptimizer() override = default;
  /**\brief Generic method for fetching the incremental action-observations
   * (or
   * observation-only) measurements
   *
   * \return True if operation was successful. Criteria for Success depend on
   * the decider/optimizer implementing this method
   */
  virtual bool updateState(
      mrpt::obs::CActionCollection::Ptr action,
      mrpt::obs::CSensoryFrame::Ptr observations,
      mrpt::obs::CObservation::Ptr observation) = 0;
  /**\brief Fetch a CWindowManager pointer.
   *
   * CWindowManager instance should contain a CDisplayWindow3D* and,
   * optionally, a CWindowObserver pointer so that interaction with the
   * window is possible
   */
  virtual void setWindowManagerPtr(mrpt::graphslam::CWindowManager* win_manager);

  /**\brief Fetch a std::mutex for locking the GRAPH_T
   * resource.
   *
   * Handy for realising multithreading in the derived classes.
   *
   * \warning Beware that prior to the decider/optimizer public method call,
   * the CCriticalSection will already be locked from CGraphSlamEngine_t
   * instance, but this isn't effective in multithreaded implementations where
   * the decider/optimizer itself has to lock the function at which the extra
   * thread runs.
   */
  virtual void setCriticalSectionPtr(std::mutex* graph_section);
  /**\brief Initialize visual objects in CDisplayWindow (e.g. \em add an
   * object to scene).
   *
   * \exception std::exception If the method is called without having first
   * provided a CDisplayWindow3D* to the class instance
   *
   * \sa setWindowManagerPtr, updateVisuals
   */
  virtual void initializeVisuals();
  /**\brief Update the relevant visual features in CDisplayWindow.
   *
   *\exception std::exception If the method is called without having first
   * provided a CDisplayWindow3D* to the class instance
   *
   * \sa setWindowManagerPtr, initializeVisuals
   */
  virtual void updateVisuals();
  /**\brief Get a list of the window events that happened since the last call.
   *
   * Method in derived classes is automatically called from the
   * CGraphSlamEngine_t instance. After that, decider/optimizer should just
   * fetch the parameters that it is interested in.
   */
  virtual void notifyOfWindowEvents(const std::map<std::string, bool>& events_occurred);
  /**\brief Load the necessary for the decider/optimizer configuration
   * parameters.
   */
  virtual void loadParams(const std::string& source_fname);
  /**\brief Print the problem parameters - relevant to the decider/optimizer
   * to the
   * screen in a unified/compact way.
   */
  virtual void printParams() const;
  /**\brief Fill the provided string with a detailed report of the
   * decider/optimizer state.
   *
   * Report should include (part of) the following:
   * - Timing of important methods
   * - Properties fo class at the current time
   * - Logging of commands until current time
   */
  virtual void getDescriptiveReport(std::string* report_str) const;

  /**\brief Fetch the graph on which the decider/optimizer will work on.
   *
   */
  virtual void setGraphPtr(GRAPH_T* graph);

  /**\brief Initialize the COutputLogger, CTimeLogger instances given the
   * name of the decider/optimizer at hand
   */
  virtual void initializeLoggers(const std::string& name);
  virtual void setClassName(const std::string& name);
  bool isMultiRobotSlamClass();

  std::string getClassName() const { return m_class_name; };

 protected:
  /**\brief Handy function for making all the visuals assertions in a
   * compact manner
   */
  virtual void assertVisualsVars();
  /**\brief Pointer to the graph that is under construction */
  GRAPH_T* m_graph = nullptr;
  std::mutex* m_graph_section = nullptr;

  /** \name Visuals-related variables methods
   */
  /**\{*/
  /**\brief Pointer to the CWindowManager object used to store
   * visuals-related instances
   */
  mrpt::graphslam::CWindowManager* m_win_manager = nullptr;
  /**\brief Window to use */
  mrpt::gui::CDisplayWindow3D* m_win = nullptr;
  /**\brief CWindowObserver object for monitoring various visual-oriented
   * events.*/
  mrpt::graphslam::CWindowObserver* m_win_observer = nullptr;
  bool m_initialized_visuals = false;
  /**\}*/

  /**\brief Time logger instance */
  mrpt::system::CTimeLogger m_time_logger;
  /**\brief Name of the class instance */
  std::string m_class_name = "CRegistrationDeciderOrOptimizer";
  /**\brief Boolean indicating if the current class can be used in
   * multi-robot SLAM operations
   */
  bool is_mr_slam_class = false;
};
}  // namespace mrpt::graphslam
#include "CRegistrationDeciderOrOptimizer_impl.h"
