/* +------------------------------------------------------------------------+
   |                     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          |
   +------------------------------------------------------------------------+ */

// implementation of the TUserOptionsChecker class template methods
namespace mrpt::graphslam::apps
{
template <class GRAPH_t>
TUserOptionsChecker<GRAPH_t>::TUserOptionsChecker()
	: sep_header(40, '='), sep_subheader(20, '-')
{
}

template <class GRAPH_t>
TUserOptionsChecker<GRAPH_t>::~TUserOptionsChecker()
{
	using namespace std;

	// release the instances holding the descriptions of the available
	// deciders/optimizers
	for (auto it = regs_descriptions.begin(); it != regs_descriptions.end();
		 ++it)
	{
		delete *it;
	}
	for (auto it = optimizers_descriptions.begin();
		 it != optimizers_descriptions.end(); ++it)
	{
		delete *it;
	}
}

template <class GRAPH_t>
void TUserOptionsChecker<GRAPH_t>::createDeciderOptimizerMappings()
{
	MRPT_START

	using namespace mrpt::graphslam::deciders;
	using namespace mrpt::graphslam::optimizers;
	using namespace mrpt::graphs;
	using namespace mrpt::poses;

	// node registration deciders
	node_regs_map["CEmptyNRD"] =
		&createNodeRegistrationDecider<CEmptyNRD<GRAPH_t>>;
	node_regs_map["CFixedIntervalsNRD"] =
		&createNodeRegistrationDecider<CFixedIntervalsNRD<GRAPH_t>>;
	// edge registration deciders
	edge_regs_map["CEmptyERD"] =
		&createEdgeRegistrationDecider<CEmptyERD<GRAPH_t>>;
	// optimizers
	optimizers_map["CLevMarqGSO"] =
		&createGraphSlamOptimizer<CLevMarqGSO<GRAPH_t>>;
	optimizers_map["CEmptyGSO"] =
		&createGraphSlamOptimizer<CLevMarqGSO<GRAPH_t>>;

	// create the decider optimizer, specific to the GRAPH_T template type
	this->_createDeciderOptimizerMappings();

	MRPT_END
}

template <class GRAPH_t>
void TUserOptionsChecker<GRAPH_t>::_createDeciderOptimizerMappings()
{
}

// deciders/optpimizers specific to the 2D SLAM cases
template <>
inline void TUserOptionsChecker<
	mrpt::graphs::CNetworkOfPoses2DInf>::_createDeciderOptimizerMappings()
{
	using namespace mrpt::graphs;

	node_regs_map["CICPCriteriaNRD"] =
		&createNodeRegistrationDecider<CICPCriteriaNRD<CNetworkOfPoses2DInf>>;
	edge_regs_map["CICPCriteriaERD"] =
		&createEdgeRegistrationDecider<CICPCriteriaERD<CNetworkOfPoses2DInf>>;
	edge_regs_map["CLoopCloserERD"] =
		&createEdgeRegistrationDecider<CLoopCloserERD<CNetworkOfPoses2DInf>>;
}
template <>
inline void TUserOptionsChecker<
	mrpt::graphs::CNetworkOfPoses2DInf_NA>::_createDeciderOptimizerMappings()
{
	using namespace mrpt::graphs;

	node_regs_map["CICPCriteriaNRD"] = &createNodeRegistrationDecider<
		CICPCriteriaNRD<CNetworkOfPoses2DInf_NA>>;
	edge_regs_map["CICPCriteriaERD"] = &createEdgeRegistrationDecider<
		CICPCriteriaERD<CNetworkOfPoses2DInf_NA>>;
	edge_regs_map["CLoopCloserERD"] =
		&createEdgeRegistrationDecider<CLoopCloserERD<CNetworkOfPoses2DInf_NA>>;
}

// deciders/optpimizers specific to the 3D SLAM cases
template <>
inline void TUserOptionsChecker<
	mrpt::graphs::CNetworkOfPoses3DInf>::_createDeciderOptimizerMappings()
{
}

template <class GRAPH_t>
void TUserOptionsChecker<GRAPH_t>::dumpRegistrarsToConsole(
	std::string reg_type /*="all"*/) const
{
	MRPT_START
	using namespace std;
	using namespace mrpt;

	ASSERTDEBMSG_(
		(system::strCmpI(reg_type, "node") ||
		 system::strCmpI(reg_type, "edge") || system::strCmpI(reg_type, "all")),
		format(
			"Registrar string '%s' does not match a known registrar name.\n"
			"Specify 'node' 'edge' or 'all'",
			reg_type.c_str()));

	if (system::strCmpI(reg_type, "node") || system::strCmpI(reg_type, "edge"))
	{
		cout << endl
			 << "Available " << system::upperCase(reg_type)
			 << " Registration Deciders: " << endl;
		cout << sep_header << endl;

		for (auto dec_it = regs_descriptions.begin();
			 dec_it != regs_descriptions.end(); ++dec_it)
		{
			TRegistrationDeciderProps* dec = *dec_it;
			if (system::strCmpI(dec->type, reg_type))
			{
				cout << dec->name << endl;
				cout << sep_subheader << endl;
				cout << "\t- "
					 << "Description: " << dec->description << endl;
				cout << "\t- "
					 << "Rawlog Format: " << dec->rawlog_format << endl;
				cout << "\t- "
					 << "Observations that can be used: " << endl;
				cout << "\t- "
					 << "Multi-robot SLAM capable decider: "
					 << (dec->is_mr_slam_class ? "TRUE" : "FALSE") << endl;
				cout << "\t- "
					 << "SLAM Type: " << endl;
				if (dec->is_slam_2d)
				{
					cout << "\t\t+ "
						 << "2D" << endl;
				}
				if (dec->is_slam_3d)
				{
					cout << "\t\t+ "
						 << "3D" << endl;
				}
				cout << endl;
				for (auto obs_it = dec->observations_used.begin();
					 obs_it != dec->observations_used.end(); ++obs_it)
				{
					cout << "\t\t+ " << *obs_it << endl;
				}
			}
		}
	}
	else
	{  // print both
		dumpRegistrarsToConsole("node");
		dumpRegistrarsToConsole("edge");
	}

	MRPT_END
}

template <class GRAPH_t>
void TUserOptionsChecker<GRAPH_t>::dumpOptimizersToConsole() const
{
	MRPT_START

	using namespace std;

	cout << endl << "Available GraphSlam Optimizer classes: " << endl;
	cout << sep_header << endl;

	for (auto opt_it = optimizers_descriptions.begin();
		 opt_it != optimizers_descriptions.end(); ++opt_it)
	{
		TOptimizerProps* opt = *opt_it;
		cout << opt->name << endl;
		cout << sep_subheader << endl;
		cout << "\t- "
			 << "Description: " << opt->description << endl;

		cout << "\t- "
			 << "Multi-robot SLAM capable optimizer: "
			 << (opt->is_mr_slam_class ? "TRUE" : "FALSE");
		cout << "\t- "
			 << "SLAM Type: " << endl;
		if (opt->is_slam_2d)
		{
			cout << "\t\t+ "
				 << "2D" << endl;
		}
		if (opt->is_slam_3d)
		{
			cout << "\t\t+ "
				 << "3D" << endl;
		}
	}
	MRPT_END
}

template <class GRAPH_t>
bool TUserOptionsChecker<GRAPH_t>::checkRegistrationDeciderExists(
	std::string given_reg, std::string reg_type) const
{
	MRPT_START

	using namespace std;
	using namespace mrpt;
	using namespace mrpt::poses;

	ASSERTDEBMSG_(
		(system::strCmpI(reg_type, "node") ||
		 system::strCmpI(reg_type, "edge")),
		format(
			"Registrar string \"%s\" does not match a known registrar name.\n"
			"Specify 'node' or 'edge' ",
			reg_type.c_str()));
	bool found = false;

	for (auto dec_it = regs_descriptions.begin();
		 dec_it != regs_descriptions.end(); ++dec_it)
	{
		TRegistrationDeciderProps* dec = *dec_it;

		// TODO - check this
		// if decider is not suitable for the selected SLAM type, ignore.
		pose_t p;
		if ((!dec->is_slam_2d && IS_CLASS(p, CPose2D)) ||
			(!dec->is_slam_3d && IS_CLASS(p, CPose3D)))
		{ continue; }

		if (system::strCmpI(dec->type, reg_type))
		{
			if (system::strCmpI(dec->name, given_reg))
			{
				found = true;
				return found;
			}
		}
	}

	return found;
	MRPT_END
}

template <class GRAPH_t>
bool TUserOptionsChecker<GRAPH_t>::checkOptimizerExists(
	std::string given_opt) const
{
	MRPT_START
	using namespace std;
	using namespace mrpt;
	using namespace mrpt::poses;

	bool found = false;

	for (auto opt_it = optimizers_descriptions.begin();
		 opt_it != optimizers_descriptions.end(); ++opt_it)
	{
		TOptimizerProps* opt = *opt_it;

		// if optimizer is not suitable for the selected SLAM type, ignore.
		pose_t p;
		if ((!opt->is_slam_2d && IS_CLASS(p, CPose2D)) &&
			(!opt->is_slam_3d && IS_CLASS(p, CPose3D)))
		{ continue; }

		if (system::strCmpI(opt->name, given_opt))
		{
			found = true;
			return found;
		}
	}

	return found;
	MRPT_END
}

template <class GRAPH_t>
void TUserOptionsChecker<GRAPH_t>::populateDeciderOptimizerProperties()
{
	MRPT_START

	// reset the vectors - in case they contain any elements
	regs_descriptions.clear();
	optimizers_descriptions.clear();

	// registering the available deciders
	{  // CFixedIntervalsNRD
		auto* dec = new TRegistrationDeciderProps;
		dec->name = "CFixedIntervalsNRD";
		dec->description =
			"Register a new node if the distance from the previous node "
			"surpasses a predefined distance threshold. Uses odometry "
			"information for estimating the robot movement";
		dec->type = "Node";
		dec->rawlog_format = "Both";
		dec->observations_used.emplace_back(
			"CActionRobotMovement2D - Format #1");
		dec->observations_used.emplace_back("CObservationOdometry - Format #2");
		dec->is_slam_2d = true;
		dec->is_slam_3d = true;

		regs_descriptions.push_back(dec);
	}
	{  // CICPCriteriaNRD
		auto* dec = new TRegistrationDeciderProps;
		dec->name = "CICPCriteriaNRD";
		dec->description =
			"Register a new node if the distance from the previous node "
			"surpasses a predefined distance threshold. Uses 2D/3D RangeScans "
			"alignment for estimating the robot movement";
		dec->type = "Node";
		dec->rawlog_format = "#2 - Observation-only";
		dec->observations_used.emplace_back(
			"CObservation2DRangeScan - Format #2");
		dec->observations_used.emplace_back(
			"CObservation3DRangeScan - Format #2");
		dec->is_slam_2d = true;

		regs_descriptions.push_back(dec);
	}
	{  // CEmptyNRD
		auto* dec = new TRegistrationDeciderProps;
		dec->name = "CEmptyNRD";
		dec->description =
			"Empty Decider - does nothing when its class methods are called";
		dec->type = "Node";
		dec->rawlog_format = "Both";
		dec->is_mr_slam_class = true;
		dec->is_slam_2d = true;
		dec->is_slam_3d = true;

		regs_descriptions.push_back(dec);
	}
	{  // CICPCriteriaERD
		auto* dec = new TRegistrationDeciderProps;
		dec->name = "CICPCriteriaERD";
		dec->description =
			"Register a new edge by aligning the provided 2D/3D RangeScans of "
			"2 nodes. Uses the goodness of the ICP Alignment as the criterium "
			"for adding a new edge";
		dec->type = "Edge";
		dec->rawlog_format = "Both";
		dec->observations_used.emplace_back(
			"CObservation2DRangeScan - Format #1, #2");
		dec->observations_used.emplace_back(
			"CObservation3DRangeScan - Format #2");
		dec->is_slam_2d = true;

		regs_descriptions.push_back(dec);
	}
	{  // CEmptyERD
		auto* dec = new TRegistrationDeciderProps;
		dec->name = "CEmptyERD";
		dec->description =
			"Empty Decider - does nothing when its class methods are called";
		dec->type = "Edge";
		dec->rawlog_format = "Both";
		dec->is_mr_slam_class = true;
		dec->is_slam_2d = true;
		dec->is_slam_3d = true;

		regs_descriptions.push_back(dec);
	}
	{  // CLoopCloserERD
		auto* dec = new TRegistrationDeciderProps;
		dec->name = "CLoopCloserERD";
		dec->description =
			"Partition the map and register *sets* of edges based on the "
			"Pairwise consistency matrix of each set.";
		dec->type = "Edge";
		dec->rawlog_format = "Both";
		dec->observations_used.emplace_back(
			"CObservation2DRangeScan - Format #1, #2");
		dec->is_slam_2d = true;

		regs_descriptions.push_back(dec);
	}

	// registering the available optimizers
	{  // CEmptyGSO
		auto* opt = new TOptimizerProps;
		opt->name = "CEmptyGSO";
		opt->description =
			"Empty Optimizer - does nothing when its class methods are called";
		opt->is_mr_slam_class = true;
		opt->is_slam_2d = true;
		opt->is_slam_3d = true;

		optimizers_descriptions.push_back(opt);
	}

	{  // CLevMarqGSO
		auto* opt = new TOptimizerProps;
		opt->name = "CLevMarqGSO";
		opt->description = "Levenberg-Marqurdt non-linear graphSLAM solver";
		opt->is_mr_slam_class = true;
		opt->is_slam_2d = true;
		opt->is_slam_3d = true;

		optimizers_descriptions.push_back(opt);
	}

	MRPT_END
}
}  // namespace mrpt::graphslam::apps
