/* +------------------------------------------------------------------------+
   |                     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/containers/stl_containers_utils.h>

// Implementattion file for TUncertaintyPath struct

namespace mrpt::graphslam
{
template <class GRAPH_T>
TUncertaintyPath<GRAPH_T>::TUncertaintyPath()
{
	this->clear();
}
template <class GRAPH_T>
TUncertaintyPath<GRAPH_T>::TUncertaintyPath(
	const mrpt::graphs::TNodeID& starting_node)
{
	this->clear();
	nodes_traversed.push_back(starting_node);
}
template <class GRAPH_T>
TUncertaintyPath<GRAPH_T>::TUncertaintyPath(
	const mrpt::graphs::TNodeID& starting_node,
	const mrpt::graphs::TNodeID& ending_node, const constraint_t& edge)
{
	this->clear();
	nodes_traversed.push_back(starting_node);
	this->addToPath(ending_node, edge);
}

template <class GRAPH_T>
void TUncertaintyPath<GRAPH_T>::clear()
{
	using namespace mrpt;
	using namespace mrpt::math;
	using namespace mrpt::poses;
	using namespace std;

	// clear the vector of traversed nodes
	nodes_traversed.clear();

	// clear the relative edge
	curr_pose_pdf.mean = pose_t();
	// by default the information matrix is set to the unit matrix
	auto init_path_mat = CMatrixDouble33::Identity();

	// put a really large number - we are certain of this position
	init_path_mat *= 10000;	 // TODO - justify this..
	curr_pose_pdf.cov_inv = init_path_mat;

	determinant_is_updated = false;
	determinant_cached = 0;

}  // end of clear

template <class GRAPH_T>
bool TUncertaintyPath<GRAPH_T>::isEmpty() const
{
	return *this == self_t();
}

template <class GRAPH_T>
void TUncertaintyPath<GRAPH_T>::assertIsBetweenNodeIDs(
	const mrpt::graphs::TNodeID& from, const mrpt::graphs::TNodeID& to) const
{
	ASSERTDEBMSG_(
		this->getSource() == from,
		format(
			"\nnodeID %lu is not the source of the path\n%s\n\n",
			static_cast<unsigned long>(from), this->getAsString().c_str()));
	ASSERTDEBMSG_(
		this->getDestination() == to,
		format(
			"\nnodeID %lu is not the destination of the path\n%s\n\n",
			static_cast<unsigned long>(to), this->getAsString().c_str()));
}

template <class GRAPH_T>
TUncertaintyPath<GRAPH_T>& TUncertaintyPath<GRAPH_T>::operator+=(
	const self_t& other)
{
	using namespace mrpt;
	using namespace mrpt::math;
	using namespace mrpt::poses;
	using namespace std;

	// other path should start where this ends
	ASSERTDEBMSG_(
		other.nodes_traversed.begin()[0] == this->nodes_traversed.rbegin()[0],
		"\"other\" instance must start from the nodeID that this "
		"TUncertaintyPath has ended.");
	ASSERTDEBMSG_(
		other.nodes_traversed.size(),
		"\"other\" instance doesn't have an initialized list of traversed "
		"nodes");
	ASSERTDEBMSG_(
		this->nodes_traversed.size(),
		"\"this\" instance doesn't have an initialized list of traversed "
		"nodes");

	//////// TODO Remove these - >>>>>>>>>>>>>>>>>>>>>
	// cout << string(20, '-') << "Aggregating 2 paths.."
	//<< string(20, '-') << endl;
	// this->dumpToConsole(); other.dumpToConsole();
	////// TODO Remove these - <<<<<<<<<<<<<<<<<<<<<

	// aggregate the two gaussian - mean & information matrix
	this->curr_pose_pdf += other.curr_pose_pdf;

	// add the traversed nodes
	this->nodes_traversed.insert(
		this->nodes_traversed.end(), other.nodes_traversed.begin() + 1,
		other.nodes_traversed.end());

	////// TODO Remove these - >>>>>>>>>>>>>>>>>>>>>
	// cout << std::string(10, '%') << endl << "AFTER Aggregation..." << endl;
	// this->dumpToConsole();
	// cout << string(50, '-') << endl;
	// mrpt::system::pause();
	////// TODO Remove these - <<<<<<<<<<<<<<<<<<<<<

	determinant_is_updated = false;
	return *this;
}
template <class GRAPH_T>
bool TUncertaintyPath<GRAPH_T>::operator==(const self_t& other) const
{
	using namespace mrpt;
	using namespace mrpt::math;
	using namespace mrpt::poses;
	using namespace std;

	// check if the traversed nodes are the same as well as the
	// CPoseGaussianInfs are the same..
	return (
		this->nodes_traversed == other.nodes_traversed &&
		this->curr_pose_pdf == other.curr_pose_pdf);
}
template <class GRAPH_T>
bool TUncertaintyPath<GRAPH_T>::operator!=(const self_t& other) const
{
	return !(*this == other);
}

template <class GRAPH_T>
void TUncertaintyPath<GRAPH_T>::addToPath(
	const mrpt::graphs::TNodeID& node, const constraint_t& edge)
{
	// update the path
	curr_pose_pdf += edge;

	// update the traversed nodes
	nodes_traversed.push_back(node);

	determinant_is_updated = false;
}

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

template <class GRAPH_T>
void TUncertaintyPath<GRAPH_T>::dumpToTextStream(std::ostream& out) const
{
	out << mrpt::format("%s\n", this->getAsString().c_str());
}

template <class GRAPH_T>
void TUncertaintyPath<GRAPH_T>::getAsString(std::string* str) const
{
	using namespace mrpt;
	using namespace mrpt::poses;
	using namespace std;
	using namespace mrpt::math;
	using namespace mrpt::containers;

	stringstream ss;
	string header_sep(30, '=');

	ss << "Path properties: " << endl;
	ss << header_sep << endl << endl;

	ss << "- CPosePDFGaussianInf: "
	   << (curr_pose_pdf.isInfType() ? "TRUE" : "FALSE") << endl;
	ss << "- Nodes list: \n\t< " << getSTLContainerAsString(nodes_traversed)
	   << "\b\b>" << endl;

	ss << endl;
	ss << curr_pose_pdf << endl;
	ss << endl;

	CMatrixDouble33 mat;
	if (curr_pose_pdf.isInfType()) { curr_pose_pdf.getInformationMatrix(mat); }
	else
	{
		curr_pose_pdf.getCovariance(mat);
	}
	ss << "Determinant: " << mat.det();

	*str = ss.str();
}
template <class GRAPH_T>
std::string TUncertaintyPath<GRAPH_T>::getAsString() const
{
	std::string s;
	this->getAsString(&s);
	return s;
}

template <class GRAPH_T>
const mrpt::graphs::TNodeID& TUncertaintyPath<GRAPH_T>::getSource() const
{
	return nodes_traversed.at(0);
}
template <class GRAPH_T>
const mrpt::graphs::TNodeID& TUncertaintyPath<GRAPH_T>::getDestination() const
{
	return nodes_traversed.back();
}

template <class GRAPH_T>
double TUncertaintyPath<GRAPH_T>::getDeterminant()
{
	using namespace mrpt;
	using namespace mrpt::math;
	using namespace mrpt::poses;
	using namespace std;

	// if determinant is up-to-date then return the cached version...
	if (determinant_is_updated) return determinant_cached;

	// update the cached version and return it.
	CMatrixDouble33 mat;
	if (curr_pose_pdf.isInfType()) { curr_pose_pdf.getInformationMatrix(mat); }
	else
	{
		curr_pose_pdf.getCovariance(mat);
	}
	double determinant = mat.det();

	determinant_cached = determinant;
	determinant_is_updated = true;

	return determinant;
}

template <class GRAPH_T>
bool TUncertaintyPath<GRAPH_T>::hasLowerUncertaintyThan(
	const self_t& other) const
{
	using namespace mrpt;
	using namespace mrpt::math;
	using namespace mrpt::poses;
	using namespace std;

	ASSERTDEBMSG_(
		(curr_pose_pdf.isInfType() && other.curr_pose_pdf.isInfType()) ||
			(!curr_pose_pdf.isInfType() && !other.curr_pose_pdf.isInfType()),
		mrpt::format("Constraints of given paths don't have the same "
					 "representation of uncertainty"));

	// If we are talking about information form matrices, the *higher* the
	// determinant the more confident we are.
	// if we are talking about covariances then the *lower*.
	bool has_lower = false;
	if (curr_pose_pdf.isInfType())
	{
		has_lower = this->getDeterminant() > other.getDeterminant();
	}
	else
	{
		has_lower = this->getDeterminant() < other.getDeterminant();
	}

	return has_lower;
}
}  // namespace mrpt::graphslam
