/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2023, 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/graphslam/GSO/CLevMarqGSO.h>

namespace mrpt::graphslam::optimizers
{
template <class GRAPH_T>
CLevMarqGSO<GRAPH_T>::CLevMarqGSO()
	: m_optimization_policy(OptimizationPolicy::UseLoopClosures)

{
	this->initializeLoggers("CLevMarqGSO");
}

template <class GRAPH_T>
bool CLevMarqGSO<GRAPH_T>::updateState(
	mrpt::obs::CActionCollection::Ptr action,
	mrpt::obs::CSensoryFrame::Ptr observations,
	mrpt::obs::CObservation::Ptr observation)
{
	MRPT_START
	if (this->m_graph->nodeCount() > m_last_total_num_of_nodes)
	{
		m_last_total_num_of_nodes = this->m_graph->nodeCount();
		registered_new_node = true;

		if (m_first_time_call)
		{
			opt_params.last_pair_nodes_to_edge = this->m_graph->edges;
			m_first_time_call = true;
		}

		if (opt_params.optimization_on_second_thread)
		{
			// join the previous optimization thread
			m_thread_optimize.join();

			// optimize the graph - run on a seperate thread
			m_thread_optimize = std::thread(&CLevMarqGSO::optimizeGraph, this);
		}
		else
		{  // single threaded implementation
			bool is_full_update = this->checkForFullOptimization();
			this->_optimizeGraph(is_full_update);
		}
	}

	return true;
	MRPT_END
}

template <class GRAPH_T>
void CLevMarqGSO<GRAPH_T>::initializeVisuals()
{
	MRPT_START
	ASSERTDEB_(m_has_read_config);
	parent::initializeVisuals();

	this->initGraphVisualization();
	this->initOptDistanceVisualization();

	MRPT_END
}

template <class GRAPH_T>
void CLevMarqGSO<GRAPH_T>::updateVisuals()
{
	MRPT_START
	parent::updateVisuals();

	if (opt_params.optimization_distance > 0)
	{
		this->updateOptDistanceVisualization();
	}

	this->updateGraphVisualization();

	MRPT_END
}

template <class GRAPH_T>
void CLevMarqGSO<GRAPH_T>::notifyOfWindowEvents(
	const std::map<std::string, bool>& events_occurred)
{
	MRPT_START
	using namespace std;
	parent::notifyOfWindowEvents(events_occurred);

	// I know the keys exists - I registered them explicitly

	// optimization_distance toggling
	if (opt_params.optimization_distance > 0)
	{
		if (events_occurred.find(opt_params.keystroke_optimization_distance)
				->second)
		{
			this->toggleOptDistanceVisualization();
		}

		if (events_occurred.find(opt_params.keystroke_optimize_graph)->second)
		{
			this->_optimizeGraph(/*is_full_update=*/true);
		}
	}

	// graph toggling
	if (events_occurred.find(viz_params.keystroke_graph_toggle)->second)
	{
		this->toggleGraphVisualization();
	}

	// if mouse event, let the user decide about the camera
	if (events_occurred.find("mouse_clicked")->second)
	{
		MRPT_LOG_DEBUG_STREAM("Mouse was clicked. Disabling autozoom.");
		m_autozoom_active = false;
	}

	// autofit the graph once
	if (events_occurred.find(viz_params.keystroke_graph_autofit)->second)
	{
		MRPT_LOG_DEBUG_STREAM("Autofit button was pressed");
		this->fitGraphInView();
	}

	MRPT_END
}  // end of notifyOfWindowEvents

template <class GRAPH_T>
inline void CLevMarqGSO<GRAPH_T>::initGraphVisualization()
{
	MRPT_START
	ASSERTDEBMSG_(this->m_win_manager, "No CWindowManager* is given");

	if (viz_params.visualize_optimized_graph)
	{
		this->m_win_observer->registerKeystroke(
			viz_params.keystroke_graph_toggle, "Toggle Graph visualization");
		this->m_win_observer->registerKeystroke(
			viz_params.keystroke_graph_autofit, "Fit Graph in view");

		this->m_win_manager->assignTextMessageParameters(
			/* offset_y*	= */ &viz_params.offset_y_graph,
			/* text_index* = */ &viz_params.text_index_graph);
	}

	MRPT_END
}
template <class GRAPH_T>
inline void CLevMarqGSO<GRAPH_T>::updateGraphVisualization()
{
	MRPT_START
	ASSERTDEBMSG_(this->m_win_manager, "No CWindowManager* is given");
	using namespace mrpt::opengl;

	this->logFmt(
		mrpt::system::LVL_DEBUG, "In the updateGraphVisualization function");

	// update the graph (clear and rewrite..)
	Scene::Ptr& scene = this->m_win->get3DSceneAndLock();

	// remove previous graph and insert the new instance
	// TODO - make this an incremental proocecure
	CRenderizable::Ptr prev_object = scene->getByName("optimized_graph");
	bool prev_visibility = true;
	if (prev_object)
	{  // set the visibility of the graph correctly
		prev_visibility = prev_object->isVisible();
	}
	scene->removeObject(prev_object);

	// CSetOfObjects::Ptr graph_obj =
	// graph_tools::graph_visualize(*this->m_graph, viz_params.cfg);
	CSetOfObjects::Ptr graph_obj = std::make_shared<CSetOfObjects>();
	this->m_graph->getAs3DObject(graph_obj, viz_params.cfg);

	graph_obj->setName("optimized_graph");
	graph_obj->setVisibility(prev_visibility);
	scene->insert(graph_obj);
	this->m_win->unlockAccess3DScene();

	this->m_win_manager->addTextMessage(
		5, -viz_params.offset_y_graph,
		format(
			"Optimized Graph: #nodes %d",
			static_cast<int>(this->m_graph->nodeCount())),
		mrpt::img::TColorf(0.0, 0.0, 0.0),
		/* unique_index = */ viz_params.text_index_graph);

	this->m_win->forceRepaint();

	if (m_autozoom_active) { this->fitGraphInView(); }

	MRPT_END
}

template <class GRAPH_T>
void CLevMarqGSO<GRAPH_T>::toggleGraphVisualization()
{
	MRPT_START
	using namespace mrpt::opengl;

	Scene::Ptr& scene = this->m_win->get3DSceneAndLock();

	CRenderizable::Ptr graph_obj = scene->getByName("optimized_graph");
	graph_obj->setVisibility(!graph_obj->isVisible());

	this->m_win->unlockAccess3DScene();
	this->m_win->forceRepaint();

	MRPT_END
}

template <class GRAPH_T>
void CLevMarqGSO<GRAPH_T>::fitGraphInView()
{
	MRPT_START
	using namespace mrpt::opengl;

	ASSERTDEBMSG_(
		this->m_win,
		"\nVisualization of data was requested but no CDisplayWindow3D pointer "
		"was given\n");

	// first fetch the graph object
	Scene::Ptr& scene = this->m_win->get3DSceneAndLock();
	CRenderizable::Ptr obj = scene->getByName("optimized_graph");
	CSetOfObjects::Ptr graph_obj =
		std::dynamic_pointer_cast<CSetOfObjects>(obj);
	this->m_win->unlockAccess3DScene();
	this->m_win->forceRepaint();

	// autofit it based on its grid
	CGridPlaneXY::Ptr obj_grid =
		graph_obj->CSetOfObjects::getByClass<CGridPlaneXY>();
	if (obj_grid)
	{
		float x_min, x_max, y_min, y_max;
		obj_grid->getPlaneLimits(x_min, x_max, y_min, y_max);
		const float z_min = obj_grid->getPlaneZcoord();
		this->m_win->setCameraPointingToPoint(
			0.5 * (x_min + x_max), 0.5 * (y_min + y_max), z_min);
		this->m_win->setCameraZoom(
			2.0f * std::max(10.0f, std::max(x_max - x_min, y_max - y_min)));
	}
	this->m_win->setCameraAzimuthDeg(60);
	this->m_win->setCameraElevationDeg(75);
	this->m_win->setCameraProjective(true);

	MRPT_END
}

template <class GRAPH_T>
void CLevMarqGSO<GRAPH_T>::initOptDistanceVisualization()
{
	MRPT_START
	using namespace mrpt::opengl;

	if (opt_params.optimization_distance > 0)
	{
		this->m_win_observer->registerKeystroke(
			opt_params.keystroke_optimization_distance,
			"Toggle optimization distance on/off");

		this->m_win_observer->registerKeystroke(
			opt_params.keystroke_optimize_graph,
			"Manually trigger a full graph optimization");
	}

	pose_t p;
	CRenderizable::Ptr obj = this->initOptDistanceVisualizationInternal(p);
	pose_t initial_pose;
	obj->setPose(initial_pose);
	obj->setName("optimization_distance_obj");

	Scene::Ptr scene = this->m_win->get3DSceneAndLock();
	scene->insert(obj);
	this->m_win->unlockAccess3DScene();
	this->m_win->forceRepaint();

	// optimization distance disk - textMessage
	this->m_win_manager->assignTextMessageParameters(
		&opt_params.offset_y_optimization_distance,
		&opt_params.text_index_optimization_distance);

	this->m_win_manager->addTextMessage(
		5, -opt_params.offset_y_optimization_distance,
		"Radius for graph optimization",
		mrpt::img::TColorf(opt_params.optimization_distance_color),
		/* unique_index = */ opt_params.text_index_optimization_distance);
	MRPT_END
}

template <class GRAPH_T>
mrpt::opengl::CRenderizable::Ptr
	CLevMarqGSO<GRAPH_T>::initOptDistanceVisualizationInternal(
		const mrpt::poses::CPose2D& p_unused)
{
	using namespace mrpt::opengl;

	CDisk::Ptr obj = std::make_shared<CDisk>();
	obj->setDiskRadius(
		opt_params.optimization_distance,
		opt_params.optimization_distance - 0.1);
	obj->setColor_u8(opt_params.optimization_distance_color);

	return obj;
}
template <class GRAPH_T>
mrpt::opengl::CRenderizable::Ptr
	CLevMarqGSO<GRAPH_T>::initOptDistanceVisualizationInternal(
		const mrpt::poses::CPose3D& p_unused)
{
	using namespace mrpt::opengl;

	CSphere::Ptr obj = std::make_shared<CSphere>();
	obj->setRadius(opt_params.optimization_distance);
	obj->setColor_u8(
		opt_params.optimization_distance_color.R,
		opt_params.optimization_distance_color.G,
		opt_params.optimization_distance_color.B,
		/*alpha = */ 60);

	return obj;
}

template <class GRAPH_T>
void CLevMarqGSO<GRAPH_T>::updateOptDistanceVisualization()
{
	MRPT_START
	ASSERTDEBMSG_(this->m_win_manager, "No CWindowManager* is given");
	using namespace mrpt::opengl;

	// update ICP_max_distance Disk
	Scene::Ptr scene = this->m_win->get3DSceneAndLock();

	CRenderizable::Ptr obj = scene->getByName("optimization_distance_obj");
	obj->setPose(this->m_graph->nodes.rbegin()->second);

	this->m_win->unlockAccess3DScene();
	this->m_win->forceRepaint();
	MRPT_END
}

// TODO - implement this
template <class GRAPH_T>
void CLevMarqGSO<GRAPH_T>::toggleOptDistanceVisualization()
{
	MRPT_START
	using namespace mrpt::opengl;

	Scene::Ptr scene = this->m_win->get3DSceneAndLock();

	CRenderizable::Ptr obj = scene->getByName("optimization_distance_obj");
	obj->setVisibility(!obj->isVisible());

	this->m_win->unlockAccess3DScene();
	this->m_win->forceRepaint();

	MRPT_END
}

template <class GRAPH_T>
void CLevMarqGSO<GRAPH_T>::optimizeGraph()
{
	MRPT_START
	using namespace std;

	MRPT_LOG_DEBUG_STREAM(
		"optimizeGraph:: ThreadID:" << endl
									<< "\t" << std::this_thread::get_id()
									<< endl
									<< "\t"
									<< "Trying to grab lock... ");

	std::lock_guard<std::mutex> graph_lock(*this->m_graph_section);
	this->_optimizeGraph();

	this->logFmt(mrpt::system::LVL_DEBUG, "2nd thread grabbed the lock..");

	MRPT_END
}

template <class GRAPH_T>
void CLevMarqGSO<GRAPH_T>::_optimizeGraph(bool is_full_update /*=false*/)
{
	MRPT_START
	this->m_time_logger.enter("CLevMarqGSO::_optimizeGraph");

	// if less than X nodes exist overall, do not try optimizing
	if (m_min_nodes_for_optimization > this->m_graph->nodes.size()) { return; }

	mrpt::system::CTicTac optimization_timer;
	optimization_timer.Tic();

	// set of nodes for which the optimization procedure will take place
	std::set<mrpt::graphs::TNodeID>* nodes_to_optimize;

	// fill in the nodes in certain distance to the current node, only if
	// is_full_update is not instructed
	if (is_full_update)
	{
		// nodes_to_optimize: List of nodes to optimize. nullptr -> all but the
		// root
		// node.
		nodes_to_optimize = nullptr;
	}
	else
	{
		nodes_to_optimize = new std::set<mrpt::graphs::TNodeID>;

		// I am certain that this shall not be called when nodeCount = 0, since
		// the
		// optimization procedure starts only after certain number of nodes has
		// been added
		this->getNearbyNodesOf(
			nodes_to_optimize, this->m_graph->nodeCount() - 1,
			opt_params.optimization_distance);
		nodes_to_optimize->insert(this->m_graph->nodeCount() - 1);
	}

	graphslam::TResultInfoSpaLevMarq levmarq_info;

	// Execute the optimization
	mrpt::graphslam::optimize_graph_spa_levmarq(
		*(this->m_graph), levmarq_info, nodes_to_optimize, opt_params.cfg,
		&CLevMarqGSO<GRAPH_T>::levMarqFeedback);  // functor feedback

	if (is_full_update) { m_just_fully_optimized_graph = true; }
	else
	{
		m_just_fully_optimized_graph = false;
	}

	double elapsed_time = optimization_timer.Tac();
	this->logFmt(
		mrpt::system::LVL_DEBUG, "Optimization of graph took: %fs",
		elapsed_time);

	// deleting the nodes_to_optimize set
	delete nodes_to_optimize;
	nodes_to_optimize = nullptr;

	this->m_time_logger.leave("CLevMarqGSO::_optimizeGraph");
	MRPT_END
}  // end of _optimizeGraph

template <class GRAPH_T>
bool CLevMarqGSO<GRAPH_T>::checkForLoopClosures()
{
	MRPT_START

	bool is_loop_closure = false;
	auto curr_pair_nodes_to_edge = this->m_graph->edges;

	// find the *node pairs* that exist in current but not the last
	// nodes_to_edge
	// map If the distance of any of these pairs is greater than
	// LC_min_nodeid_diff then consider this a loop closure
	typename GRAPH_T::edges_map_t::const_iterator search;
	mrpt::graphs::TPairNodeIDs curr_pair;

	for (auto it = curr_pair_nodes_to_edge.begin();
		 it != curr_pair_nodes_to_edge.end(); ++it)
	{
		search = opt_params.last_pair_nodes_to_edge.find(it->first);
		// if current node pair is not found in the last set...
		if (search == opt_params.last_pair_nodes_to_edge.end())
		{
			curr_pair = it->first;

			if (std::abs(
					static_cast<int>(curr_pair.first) -
					static_cast<int>(curr_pair.second)) >
				opt_params.LC_min_nodeid_diff)
			{
				this->logFmt(
					mrpt::system::LVL_DEBUG, "Registering loop closure... ");
				is_loop_closure = true;
				break;	// no need for more iterations
			}
		}
	}

	// update the pair_nodes_to_edge map
	opt_params.last_pair_nodes_to_edge = curr_pair_nodes_to_edge;
	return is_loop_closure;

	MRPT_END
}

template <class GRAPH_T>
bool CLevMarqGSO<GRAPH_T>::checkForFullOptimization()
{
	bool is_full_update = false;

	if (opt_params.optimization_distance == -1)
	{  // always optimize fully
		return true;
	}

	bool added_lc = this->checkForLoopClosures();

	// Decide on the LoopClosingAttitude I am in
	if (!added_lc)
	{  // reset both ignored and used counters
		if (m_curr_used_consec_lcs != 0 || m_curr_ignored_consec_lcs != 0)
		{
			MRPT_LOG_DEBUG_STREAM("No new Loop Closure found.");
		}

		m_curr_used_consec_lcs = 0;
		m_curr_ignored_consec_lcs = 0;
		m_optimization_policy = OptimizationPolicy::UseLoopClosures;

		return is_full_update;
	}
	else
	{  // lc found.
		// have I used enough consecutive loop closures?
		bool use_limit_reached =
			m_curr_used_consec_lcs == m_max_used_consec_lcs;
		// have I ignored enough consecutive loop closures?
		bool ignore_limit_reached =
			m_curr_ignored_consec_lcs == m_max_ignored_consec_lcs;

		// Have I reached any of the limits above?
		if (ignore_limit_reached || use_limit_reached)
		{
			m_curr_ignored_consec_lcs = 0;
			m_curr_used_consec_lcs = 0;

			// decide of the my policy on full optimization
			if (ignore_limit_reached)
			{
				m_optimization_policy = OptimizationPolicy::UseLoopClosures;
			}
			if (use_limit_reached)
			{
				m_optimization_policy = OptimizationPolicy::IgnoreLoopClosures;
			}
		}
		else
		{  // no limits reached yet.
			if (m_optimization_policy == OptimizationPolicy::UseLoopClosures)
			{
				m_curr_used_consec_lcs += 1;
			}
			else
			{
				m_curr_ignored_consec_lcs += 1;
			}
		}
	}

	// Decide on whether to fully optimize the graph based on the mode I am in
	if (m_optimization_policy == OptimizationPolicy::IgnoreLoopClosures)
	{
		is_full_update = false;
		MRPT_LOG_WARN_STREAM(
			"*PARTIAL* graph optimization.. ignoring new loop closure");
	}
	else
	{
		is_full_update = true;
		MRPT_LOG_WARN_STREAM("Commencing with *FULL* graph optimization... ");
	}
	return is_full_update;

}  // end of checkForFullOptimization

template <class GRAPH_T>
bool CLevMarqGSO<GRAPH_T>::justFullyOptimizedGraph() const
{
	return m_just_fully_optimized_graph;
}

template <class GRAPH_T>
void CLevMarqGSO<GRAPH_T>::levMarqFeedback(
	const GRAPH_T& graph, const size_t iter, const size_t max_iter,
	const double cur_sq_error)
{
}

template <class GRAPH_T>
void CLevMarqGSO<GRAPH_T>::getNearbyNodesOf(
	std::set<mrpt::graphs::TNodeID>* nodes_set,
	const mrpt::graphs::TNodeID& cur_nodeID, double distance)
{
	MRPT_START

	if (distance > 0)
	{
		// check all but the last node.
		for (mrpt::graphs::TNodeID nodeID = 0;
			 nodeID < this->m_graph->nodeCount() - 1; ++nodeID)
		{
			double curr_distance = this->m_graph->nodes[nodeID].distanceTo(
				this->m_graph->nodes[cur_nodeID]);
			if (curr_distance <= distance) { nodes_set->insert(nodeID); }
		}
	}
	else
	{  // check against all nodes
		this->m_graph->getAllNodes(*nodes_set);
	}

	MRPT_END
}

template <class GRAPH_T>
void CLevMarqGSO<GRAPH_T>::printParams() const
{
	parent::printParams();

	opt_params.dumpToConsole();
	viz_params.dumpToConsole();
}
template <class GRAPH_T>
void CLevMarqGSO<GRAPH_T>::loadParams(const std::string& source_fname)
{
	MRPT_START
	parent::loadParams(source_fname);

	opt_params.loadFromConfigFileName(source_fname, "OptimizerParameters");
	viz_params.loadFromConfigFileName(source_fname, "VisualizationParameters");

	mrpt::config::CConfigFile source(source_fname);

	// TODO - check that these work
	m_max_used_consec_lcs = source.read_int(
		"OptimizerParameters", "max_used_consecutive_loop_closures", 2, false);

	m_max_ignored_consec_lcs = source.read_int(
		"OptimizerParameters", "max_ignored_consecutive_loop_closures", 15,
		false);

	// set the logging level if given by the user
	// Minimum verbosity level of the logger
	int min_verbosity_level =
		source.read_int("OptimizerParameters", "class_verbosity", 1, false);
	this->setMinLoggingLevel(mrpt::system::VerbosityLevel(min_verbosity_level));

	MRPT_LOG_DEBUG("Successfully loaded Params. ");
	m_has_read_config = true;

	MRPT_END
}

template <class GRAPH_T>
void CLevMarqGSO<GRAPH_T>::getDescriptiveReport(std::string* report_str) const
{
	MRPT_START
	using namespace std;

	const static std::string report_sep(2, '\n');
	const static std::string header_sep(80, '#');

	// Report on graph
	stringstream class_props_ss;
	class_props_ss << "Levenberg Marquardt Optimization Summary: " << std::endl;
	class_props_ss << header_sep << std::endl;

	// time and output logging
	const std::string time_res = this->m_time_logger.getStatsAsText();
	const std::string output_res = this->getLogAsString();

	// merge the individual reports
	report_str->clear();
	parent::getDescriptiveReport(report_str);

	*report_str += class_props_ss.str();
	*report_str += report_sep;

	*report_str += time_res;
	*report_str += report_sep;

	*report_str += output_res;
	*report_str += report_sep;

	MRPT_END
}

template <class GRAPH_T>
CLevMarqGSO<GRAPH_T>::OptimizationParams::OptimizationParams()
	: optimization_distance_color(0, 201, 87),
	  keystroke_optimization_distance("u"),
	  keystroke_optimize_graph("w")
{
}
template <class GRAPH_T>
CLevMarqGSO<GRAPH_T>::OptimizationParams::~OptimizationParams() = default;
template <class GRAPH_T>
void CLevMarqGSO<GRAPH_T>::OptimizationParams::dumpToTextStream(
	std::ostream& out) const
{
	MRPT_START
	out << "-----------[ Levenberg-Marquardt Optimization ] -------\n";
	out << "Optimization on second thread  = "
		<< (optimization_on_second_thread ? "TRUE" : "FALSE") << std::endl;
	out << "Optimize nodes in distance     = " << optimization_distance << "\n";
	out << "Min. node difference for LC    = " << LC_min_nodeid_diff << "\n";
	// out << cfg.getAsString() << std::endl;
	MRPT_END
}
template <class GRAPH_T>
void CLevMarqGSO<GRAPH_T>::OptimizationParams::loadFromConfigFile(
	const mrpt::config::CConfigFileBase& source, const std::string& section)
{
	MRPT_START
	optimization_on_second_thread = source.read_bool(
		section, "optimization_on_second_thread", false, false);
	LC_min_nodeid_diff = source.read_int(
		"GeneralConfiguration", "LC_min_nodeid_diff", 30, false);
	optimization_distance =
		source.read_double(section, "optimization_distance", 5, false);
	// asert the previous value
	ASSERTDEBMSG_(
		optimization_distance == 1 || optimization_distance > 0,
		format(
			"Invalid value for optimization distance: %.2f",
			optimization_distance));

	// optimization parameters
	cfg["verbose"] = source.read_bool(section, "verbose", false, false);
	cfg["profiler"] = source.read_bool(section, "profiler", false, false);
	cfg["max_iterations"] =
		source.read_double(section, "max_iterations", 100, false);
	cfg["scale_hessian"] =
		source.read_double("Optimization", "scale_hessian", 0.2, false);
	cfg["tau"] = source.read_double(section, "tau", 1e-3, false);

	MRPT_END
}

template <class GRAPH_T>
CLevMarqGSO<GRAPH_T>::GraphVisualizationParams::GraphVisualizationParams()
	: keystroke_graph_toggle("s"), keystroke_graph_autofit("a")
{
}
template <class GRAPH_T>
CLevMarqGSO<GRAPH_T>::GraphVisualizationParams::~GraphVisualizationParams() =
	default;
template <class GRAPH_T>
void CLevMarqGSO<GRAPH_T>::GraphVisualizationParams::dumpToTextStream(
	std::ostream& out) const
{
	MRPT_START

	out << "-----------[ Graph Visualization Parameters ]-----------\n";
	out << mrpt::format(
		"Visualize optimized graph = %s\n",
		visualize_optimized_graph ? "TRUE" : "FALSE");

	// out << mrpt::format("%s", cfg.getAsString().c_str());

	std::cout << std::endl;

	MRPT_END
}
template <class GRAPH_T>
void CLevMarqGSO<GRAPH_T>::GraphVisualizationParams::loadFromConfigFile(
	const mrpt::config::CConfigFileBase& source, const std::string& section)
{
	MRPT_START

	visualize_optimized_graph =
		source.read_bool(section, "visualize_optimized_graph", true, false);

	cfg["show_ID_labels"] =
		source.read_bool(section, "optimized_show_ID_labels", false, false);
	cfg["show_ground_grid"] =
		source.read_double(section, "optimized_show_ground_grid", 1, false);
	cfg["show_edges"] =
		source.read_bool(section, "optimized_show_edges", true, false);
	cfg["edge_color"] =
		source.read_int(section, "optimized_edge_color", 1500, false);
	cfg["edge_width"] =
		source.read_double(section, "optimized_edge_width", 1.5, false);
	cfg["show_node_corners"] =
		source.read_bool(section, "optimized_show_node_corners", true, false);
	cfg["show_edge_rel_poses"] =
		source.read_bool(section, "optimized_show_edge_rel_poses", true, false);
	cfg["edge_rel_poses_color"] =
		source.read_int(section, "optimized_edge_rel_poses_color", 2000, false);
	cfg["nodes_edges_corner_scale"] = source.read_double(
		section, "optimized_nodes_edges_corner_scale", 0.4, false);
	cfg["nodes_corner_scale"] =
		source.read_double(section, "optimized_nodes_corner_scale", 0.7, false);
	cfg["nodes_point_size"] =
		source.read_int(section, "optimized_nodes_point_size", 5, false);
	cfg["nodes_point_color"] =
		source.read_int(section, "optimized_nodes_point_color", 3000, false);

	MRPT_END
}
}  // namespace mrpt::graphslam::optimizers
