/* +------------------------------------------------------------------------+
   |                     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/core/exceptions.h>
#include <mrpt/graphs/CNetworkOfPoses.h>
#include <mrpt/graphslam/ERD/CEmptyERD.h>
#include <mrpt/graphslam/ERD/CICPCriteriaERD.h>
#include <mrpt/graphslam/ERD/CLoopCloserERD.h>
#include <mrpt/graphslam/GSO/CLevMarqGSO.h>
#include <mrpt/graphslam/NRD/CEmptyNRD.h>
#include <mrpt/graphslam/NRD/CFixedIntervalsNRD.h>
#include <mrpt/graphslam/NRD/CICPCriteriaNRD.h>
#include <mrpt/poses/CPose3DPDFGaussianInf.h>
#include <mrpt/poses/CPosePDFGaussianInf.h>
#include <mrpt/system/filesystem.h>

#include <iostream>
#include <string>
#include <vector>

namespace mrpt::graphslam::apps
{
/**\brief Properties struct for both the Registration Decider and Optimizer
 * classes
 */
struct TRegistrationDeciderOrOptimizerProps
{
  TRegistrationDeciderOrOptimizerProps() = default;
  ~TRegistrationDeciderOrOptimizerProps() = default;
  /**\brief Name of the decider or optimizer class
   */
  std::string name;
  /**\brief General description of the decicder or optimizer class*/
  std::string description;
  /**\brief Class indicating if the current decider/optimizer class can be
   * used
   * in a multi-robot SLAM operation
   */
  bool is_mr_slam_class{false};
  bool is_slam_2d{true};
  bool is_slam_3d{false};
};

/**\brief Properties struct for the Registration Decider classes.
 *
 * \ingroup mrpt_graphslam_grp
 */
struct TRegistrationDeciderProps : public TRegistrationDeciderOrOptimizerProps
{
  TRegistrationDeciderProps() = default;
  ~TRegistrationDeciderProps() = default;
  /**\brief Type of decider.
   *
   * Available options are:
   * - node
   * - edge
   */
  std::string type;
  /**\brief Rawlog formats that the decider can be used in */
  std::string rawlog_format;
  /**\brief Measurements that the current decider class can utilize */
  std::vector<std::string> observations_used;
};

/**\brief Properties struct for the Optimizer classes.
 *
 * \ingroup mrpt_graphslam_grp
 */
struct TOptimizerProps : public TRegistrationDeciderOrOptimizerProps
{
  TOptimizerProps() = default;
  ~TOptimizerProps() = default;
};

/**\brief Class containing the declarations of supplementary methods that can
 * be used in application-related code.
 * Class instance can be handy for adding keeping the available
 * deciders/optimizers in a compact manner and for verifying whether a given
 * decider can be used.
 *
 * \ingroup mrpt_graphslam_grp
 */
template <class GRAPH_t>
struct TUserOptionsChecker
{
  /**\name handy typedefs for the creation of deciders/optimzer instances from
   * the corresponding strings
   */
  /**\{*/
  using constraint_t = typename GRAPH_t::constraint_t;
  using pose_t = typename GRAPH_t::constraint_t::type_value;
  using node_regs_t =
      std::map<std::string, mrpt::graphslam::deciders::CNodeRegistrationDecider<GRAPH_t>* (*)()>;
  using edge_regs_t =
      std::map<std::string, mrpt::graphslam::deciders::CEdgeRegistrationDecider<GRAPH_t>* (*)()>;
  using optimizers_t =
      std::map<std::string, mrpt::graphslam::optimizers::CGraphSlamOptimizer<GRAPH_t>* (*)()>;

  /**\}*/

  //
  // methods
  //
  /**\brief Constructor */
  TUserOptionsChecker();
  /**\brief Destructor */
  virtual ~TUserOptionsChecker();
  /**\brief Create the necessary mappings from strings to the corresponding
   * instance creation functors.
   *
   * Method is used for populating a map from string to instance creation
   * function. The decider/optimzer can then be spawned according to the user
   * selection.
   *
   */
  virtual void createDeciderOptimizerMappings();
  virtual void _createDeciderOptimizerMappings();
  /**\brief Populate the available decider, optimizer classes available in
   * user applications
   */
  virtual void populateDeciderOptimizerProperties();
  /**\brief Check if the given registrator decider exists in the vector of
   * deciders.
   * \param[in] given_reg String specifying the type of decider - This should
   * either be "node" or "edge"
   * \return True if it exists, false otherwise
   */
  virtual bool checkRegistrationDeciderExists(std::string given_reg, std::string reg_type) const;

  /**\brief Check if the given optimizer exists in the vector of optimizers.
   * \return True if it exists, false otherwise
   */
  virtual bool checkOptimizerExists(std::string given_opt) const;

  /**\brief Print the registration deciders vector in a formatted manner to
   * the
   * standard output
   * \param[in] reg_type Method prints both the node registration
   * and edge registration deciders of the given vector unless specified
   * otherwise. The available argument options are "node", "edge", "all"
   */
  virtual void dumpRegistrarsToConsole(std::string reg_type = "all") const;
  /**\brief Print the optimizers vector in a formatted manner to the standard
   * output.
   */
  virtual void dumpOptimizersToConsole() const;

  /**\name Methods for initializing decider/optimizer instances based on the
   * user
   * command line choices - http://stackoverflow.com/a/582456/2843583
   *
   * \warning Caller is responsible for deleting the initialized instances
   */
  /**\{*/
  template <class T>
  static mrpt::graphslam::deciders::CNodeRegistrationDecider<GRAPH_t>*
  createNodeRegistrationDecider()
  {
    return new T;
  }
  template <class T>
  static mrpt::graphslam::deciders::CEdgeRegistrationDecider<GRAPH_t>*
  createEdgeRegistrationDecider()
  {
    return new T;
  }
  template <class T>
  static mrpt::graphslam::optimizers::CGraphSlamOptimizer<GRAPH_t>* createGraphSlamOptimizer()
  {
    return new T;
  }
  /**}*/

  /**\name Mappings from registration decider/optimizer names to functors
   * for creating instances of the corresponding classes
   */
  /**\{*/
  node_regs_t node_regs_map;
  edge_regs_t edge_regs_map;
  optimizers_t optimizers_map;
  /**\}*/

  /**\name Vectors containing descriptions about the available
   * deciders/optimizers.
   *
   * Handy for displaying information to the user (e.g. in help text)
   */
  /**\{*/
  std::vector<TRegistrationDeciderProps*> regs_descriptions;
  std::vector<TOptimizerProps*> optimizers_descriptions;
  /**\}*/

  const std::string sep_header;
  const std::string sep_subheader;
};
}  // namespace mrpt::graphslam::apps
#include "TUserOptionsChecker_impl.h"
