/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2022, 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/CGraphSlamOptimizer.h>
#include <mrpt/graphslam/levmarq.h>
#include <mrpt/img/TColor.h>
#include <mrpt/obs/CSensoryFrame.h>
#include <mrpt/opengl/CRenderizable.h>
#include <mrpt/poses/CPose2D.h>
#include <mrpt/poses/CPose3D.h>

#include <cmath>
#include <iostream>
#include <map>
#include <string>

namespace mrpt::graphslam::optimizers
{
/**\brief Levenberg-Marquardt non-linear graph slam optimization scheme.
 *
 * ## Description
 *
 * Current decider optimizes the graph according to the
 * graphslam::optimize_spa_levmarq method. Refer to the latter for more
 * details on the implementation.
 *
 * ### .ini Configuration Parameters
 *
 * \htmlinclude graphslam-engine_config_params_preamble.txt
 *
 * - \b class_verbosity
 *   + \a Section       : OptimizerParameters
 *   + \a Default value : 1 (mrpt::system::LVL_INFO)
 *   + \a Required      : FALSE
 *
 * - \b optimization_on_second_thread
 *   + \a Section       : OptimizerParameters
 *   + \a Default value :  FALSE
 *   + \a Required      : FALSE
 *   + \a Description   : Specify whether to use a second thread to optimize
 *   the graph.
 *
 * - \b LC_min_nodeid_diff
 *  + \a Section       : GeneralConfiguration
 *  + \a Default value : 30
 *  + \a Required      : FALSE
 *  + \a Description   : Minimum NodeID difference for an edge to be considered
 *  a loop closure.
 *
 * - \b optimization_distance
 *  + \a Section       : OptimizerParameters
 *  + \a Default value : 5
 *  + \a Required      : FALSE
 *  + \a Description   : Positions of the nodes within the specified distance
 from the current
 *  graph node are optimized according to the corresponding constraints between
 *  them
 *
 * - \b verbose
 *  + \a Section       : OptimizerParameters
 *  + \a Default value : FALSE
 *  + \a Required      : FALSE
 *  + \a Description   : Refers to the Levenberg-Marquardt optimization.
 *  procedure
 *
 * - \b profiler
 *  + \a Section       : OptimizerParameters
 *  + \a Default value : FALSE
 *  + \a Required      : FALSE
 *  + \a Description   : Refers to the Levenberg-Marquardt optimization.
 *  procedure
 *
 * - \b max_iterations
 *  + \a Section       : OptimizerParameters
 *  + \a Default value : 100
 *  + \a Required      : FALSE
 *  + \a Description   : Refers to the Levenberg-Marquardt optimization. Sets
 *  the maximum number of iterations of the optimization scheme.
 *
 * - \b scale_hessian
 *  + \a Section       : OptimizerParameters
 *  + \a Default value : 0.2
 *  + \a Required      : FALSE
 *  + \a Description   : Refers to the Levenberg-Marquardt optimization.
 *
 * - \b tau
 *  + \a Section       : OptimizerParameters
 *  + \a Default value : 1e-3
 *  + \a Required      : FALSE
 *  + \a Description   : Refers to the Levenberg-Marquardt optimization.
 *
 *  \note For a detailed description of the optimization parameters of the
 *  Levenberg-Marquardt scheme, refer to
 *
 https://reference.mrpt.org/devel/group__mrpt__graphslam__grp.html#ga022f4a70be5ec7c432f46374e4bb9d66
 *
 *  \note For a detailed description of the graph visualization parameters
 *  refer to
 *
 https://reference.mrpt.org/devel/group__mrpt__opengl__grp.html#ga30efc9f6fcb49801e989d174e0f65a61

 *
 * \ingroup mrpt_graphslam_grp
 */
template <class GRAPH_T = typename mrpt::graphs::CNetworkOfPoses2DInf>
class CLevMarqGSO
	: public mrpt::graphslam::optimizers::CGraphSlamOptimizer<GRAPH_T>
{
   public:
	// Public methods
	//////////////////////////////////////////////////////////////
	/**\brief Handy typedefs */
	/**\{*/
	using constraint_t = typename GRAPH_T::constraint_t;
	/** type of underlying poses (2D/3D)*/
	using pose_t = typename GRAPH_T::constraint_t::type_value;
	using InfMat = mrpt::math::CMatrixFixed<
		double, constraint_t::state_length, constraint_t::state_length>;
	using grandpa = mrpt::graphslam::CRegistrationDeciderOrOptimizer<GRAPH_T>;
	using parent = mrpt::graphslam::optimizers::CGraphSlamOptimizer<GRAPH_T>;
	/**\}*/

	CLevMarqGSO();
	~CLevMarqGSO() override = default;

	bool updateState(
		mrpt::obs::CActionCollection::Ptr action,
		mrpt::obs::CSensoryFrame::Ptr observations,
		mrpt::obs::CObservation::Ptr observation) override;

	void initializeVisuals() override;
	void updateVisuals() override;
	/**\brief Get a list of the window events that happened since the last
	 * call.
	 */
	void notifyOfWindowEvents(
		const std::map<std::string, bool>& events_occurred) override;
	/**\brief Struct for holding the optimization-related variables in a
	 * compact form
	 */
	struct OptimizationParams : public mrpt::config::CLoadableOptions
	{
	   public:
		OptimizationParams();
		~OptimizationParams() override;

		void loadFromConfigFile(
			const mrpt::config::CConfigFileBase& source,
			const std::string& section) override;
		void dumpToTextStream(std::ostream& out) const override;

		mrpt::containers::yaml cfg;
		// True if optimization procedure is to run in a multithreading fashion
		bool optimization_on_second_thread;

		/**\brief optimize only for the nodes found in a certain distance from
		 * the current position. Optimize for the entire graph if set to1
		 */
		double optimization_distance;
		double offset_y_optimization_distance;
		int text_index_optimization_distance;
		mrpt::img::TColor optimization_distance_color;
		/**\brief Keystroke to toggle the optimization distance on/off */
		std::string keystroke_optimization_distance;
		/**\brief Keystroke to manually trigger a full graph optimization */
		std::string keystroke_optimize_graph;

		// nodeID difference for an edge to be considered loop closure
		int LC_min_nodeid_diff;

		// Map of TPairNodesID to their corresponding edge as recorded in the
		// last update of the optimizer state
		typename GRAPH_T::edges_map_t last_pair_nodes_to_edge;
	};

	/**\brief struct for holding the graph visualization-related variables in a
	 * compact form
	 */
	struct GraphVisualizationParams : public mrpt::config::CLoadableOptions
	{
	   public:
		GraphVisualizationParams();
		~GraphVisualizationParams() override;

		void loadFromConfigFile(
			const mrpt::config::CConfigFileBase& source,
			const std::string& section) override;
		void dumpToTextStream(std::ostream& out) const override;

		mrpt::containers::yaml cfg;
		bool visualize_optimized_graph;
		// textMessage parameters
		std::string keystroke_graph_toggle;	 // see Ctor for initialization
		std::string keystroke_graph_autofit;  // see Ctor for initialization
		int text_index_graph;
		double offset_y_graph;
	};

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

	bool justFullyOptimizedGraph() const override;

	/** Parameters relevant to the optimizatio nfo the graph. */
	OptimizationParams opt_params;
	/** Parameters relevant to the visualization of the graph. */
	GraphVisualizationParams viz_params;

   protected:
	/**\brief Feedback of the Levenberg-Marquardt graph optimization procedure.
	 *
	 */
	static void levMarqFeedback(
		const GRAPH_T& graph, const size_t iter, const size_t max_iter,
		const double cur_sq_error);

	/**\brief Optimize the given graph.
	 *
	 * Wrapper around the graphslam::optimize_spa_levmarq method
	 * \sa optimize_spa_levmarq, optimizeGraph
	 *
	 * \param[in] full_update Impose that method optimizes the whole graph
	 *
	 */
	void _optimizeGraph(bool is_full_update = false);
	/** \brief Wrapper around _optimizeGraph which first locks the section and
	 * then calls the _optimizeGraph method.
	 *
	 * Used in multithreaded optimization
	 * \sa _optimizeGraph()
	 */
	void optimizeGraph() override;
	/**\brief Check if a loop closure edge was added in the graph.
	 *
	 * Match the previously registered edges in the graph with the current. If
	 * there is a node difference *in any new edge* greater than
	 * \b LC_min_nodeid_diff (see .ini parameter) then new constraint is
	 * considered a Loop Closure
	 *
	 * \return True if \b any of the newly added edges is considered a loop
	 * closure
	 */
	bool checkForLoopClosures();
	/**\brief Decide whether to issue a full graph optimization
	 *
	 * In case N consecutive full optimizations have been issued, skip some of
	 * the next as they slow down the overall execution and they don't reduce
	 * the overall error
	 *
	 * \return True for issuing a full graph optimization, False otherwise
	 */
	bool checkForFullOptimization();
	/**\brief Initialize objects relateed to the Graph Visualization
	 */
	void initGraphVisualization();
	/**\brief Called internally for updating the visualization scene for the
	 * graph
	 * building procedure
	 */
	inline void updateGraphVisualization();
	/**\brief Toggle the graph visualization on and off.
	 */
	void toggleGraphVisualization();
	/**\brief Set the camera parameters of the CDisplayWindow3D so that the
	 * whole
	 * graph is viewed in the window.
	 *
	 * \warning Method assumes that the COpenGLinstance *is not locked* prior to
	 * this
	 * call, so make sure you have issued
	 * CDisplayWindow3D::unlockAccess3DScene() before calling this method.
	 */
	inline void fitGraphInView();

	/**\brief Initialize the Disk/Sphere used for visualizing the optimization
	 * distance.
	 */
	/**\{*/
	void initOptDistanceVisualization();
	/**\brief Setup the corresponding Disk/Sphere instance.
	 *
	 * Method overloads are used to overcome the C++ specialization
	 * restrictions
	 *
	 * \return Disk/Sphere instance for 2D/3D SLAM respectively
	 */
	/**\{*/
	mrpt::opengl::CRenderizable::Ptr initOptDistanceVisualizationInternal(
		const mrpt::poses::CPose2D& p_unused);
	mrpt::opengl::CRenderizable::Ptr initOptDistanceVisualizationInternal(
		const mrpt::poses::CPose3D& p_unused);
	/**\}*/

	/**\}*/

	/**\brief Update the position of the disk indicating the distance in which
	 * Levenberg-Marquardt graph optimization is executed
	 */
	inline void updateOptDistanceVisualization();
	/**\brief toggle the optimization distance object on and off
	 */
	void toggleOptDistanceVisualization();
	/**\brief Get a list of the nodeIDs whose position is within a certain
	 * distance to the specified nodeID
	 */
	void getNearbyNodesOf(
		std::set<mrpt::graphs::TNodeID>* nodes_set,
		const mrpt::graphs::TNodeID& cur_nodeID, double distance);

	// protected members
	//////////////////////////////////////////////////////////////

	bool m_first_time_call{false};
	bool m_has_read_config{false};
	bool registered_new_node;
	bool m_autozoom_active{true};

	// start optimizing the graph after a certain number of nodes has been
	// added (when m_graph->nodeCount() > m_last_total_num_of_nodes)
	size_t m_last_total_num_of_nodes{5};

	// Use second thread for graph optimization
	std::thread m_thread_optimize;

	/**\brief Enumeration that defines the behaviors towards using or ignoring a
	 * newly added loop closure to fully optimize the graph
	 */
	enum class OptimizationPolicy
	{
		IgnoreLoopClosures = 0,
		UseLoopClosures,

		Count
	};
	OptimizationPolicy m_optimization_policy;
	/**\name Smart Full-Optimization Command
	 *
	 * Instead of issuing a full optimization every time a loop closure is
	 * detected, ignore current loop closure when enough consecutive loop
	 * closures have already been utilised.
	 * This avoids the added computational cost that is needed for optimizing
	 * the graph without reducing the accuracy of the overall operation
	 */
	/**\{*/

	/**\brief Number of maximum cosecutive loop closures that are allowed to be
	 * issued.
	 *
	 * \sa m_curr_used_consec_lcs, m_max_ignored_consec_lcs
	 */
	size_t m_max_used_consec_lcs;
	/**\brief Number of consecutive loop closures that are currently registered
	 *
	 * \sa m_max_used_consec_lcs
	 */
	size_t m_curr_used_consec_lcs{0};
	/**\brief Number of consecutive loop closures to ignore after \b
	 * m_max_used_consec_lcs have already been issued.
	 *
	 * \sa m_curr_ignored_consec_lcs, m_max_used_consec_lcs
	 */
	size_t m_max_ignored_consec_lcs;
	/**\brief Consecutive Loop Closures that have currently been ignored
	 *
	 * \sa m_max_ignored_consec_lcs
	 */
	size_t m_curr_ignored_consec_lcs{0};

	/**\}*/

	/**\brief Indicates whether a full graph optimization was just issued.
	 */
	bool m_just_fully_optimized_graph{false};

	/**\brief Minimum number of nodes before we try optimizing the graph */
	size_t m_min_nodes_for_optimization{3};
};
}  // namespace mrpt::graphslam::optimizers
#include "CLevMarqGSO_impl.h"
