/* +------------------------------------------------------------------------+
   |                     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/poses/CPose2D.h>
#include <mrpt/poses/CPose3D.h>

#include <cstdint>
#include <iostream>
#include <sstream>
#include <string>

namespace mrpt::graphs::detail
{
/**\brief An edge hypothesis between two nodeIDs.
 *
 * Struct practically provides a wrapper around the GRAPH_T::constraint_t
 * instance. Represents a hypothesis for a potential, perhaps loop closing,
 * edge (i.e. a graph constraint/edge), between two nodeIDs of the graph.
 *
 * \sa mrpt::deciders::CLoopCloserERD
 * \ingroup mrpt_graphs_grp
 */
template <class GRAPH_T>
struct THypothesis
{
	/**\brief Handy typedefs */
	/**\{*/
	/**\brief type of graph constraints */
	using constraint_t = typename GRAPH_T::constraint_t;
	/**\brief type of underlying poses (2D/3D). */
	using pose_t = typename constraint_t::type_value;
	using self_t = THypothesis<GRAPH_T>;
	/**\}*/
	/**\brief Constructor */
	THypothesis();
	/**\brief Destructor */
	~THypothesis();
	/**\brief Return a string representation of the object at hand
	 *
	 */
	/**\{*/
	std::string getAsString(bool oneline = true) const;
	void getAsString(std::string* str, bool oneline = true) const;
	/**\}*/

	/**\brief Getter methods for the underlying edge
	 *
	 * \sa setEdge
	 */
	/**\{*/
	void getEdge(constraint_t* edge) const;
	constraint_t getEdge() const;
	/**\}*/

	/**\brief Setter method for the underlying edge
	 *
	 * \sa getEdge
	 */
	void setEdge(const constraint_t& edge);

	/**\brief Getter methods for the inverse of the underlying edge */
	/**\{*/
	void getInverseEdge(constraint_t* edge) const;
	constraint_t getInverseEdge() const;
	/**\}*/

	/**\brief Reverse the hypothesis.
	 *
	 * Reversing implies, at least, changing order of from/to nodes and
	 * reversing
	 * the underlying edge
	 */
	void inverseHypothesis();

	/**\brief ID of the current hypothesis */
	size_t id;
	/**\brief Starting node of the hypothesis */
	mrpt::graphs::TNodeID from;
	/**\brief Ending node of the hypothesis */
	mrpt::graphs::TNodeID to;
	/**\brief Field that specifies if the hypothesis is to be considered */
	bool is_valid{true};
	/**\brief Goodness value corresponding to the hypothesis edge
	 *
	 * \note For ICP edges this resolves to the CICP goodness measure for the
	 * alignment operation.
	 *
	 * \sa edge, mrpt::slam::CICP::goodness
	 */
	double goodness{0};
	/**\brief Compare the start and end nodes of two hypothesis
	 *
	 * \return True if ends match.
	 */
	bool sameEndsWith(const self_t& other) const;
	/**\brief Check if the start, end nodes are the ones given
	 *
	 * \return True if ends are the given ones
	 */
	bool hasEnds(
		const mrpt::graphs::TNodeID from, const mrpt::graphs::TNodeID to) const;

	/**\brief Handy operator for using THypothesis in std::set
	 */
	bool operator<(const self_t& other) const;

	inline friend std::ostream& operator<<(
		std::ostream& o, const THypothesis<GRAPH_T>& h)
	{
		o << h.getAsString(/*oneline=*/true) << std::endl;
		return o;
	}

   private:
	/**\brief Edge connecting the two nodes */
	constraint_t edge;
};
}  // namespace mrpt::graphs::detail
#include "THypothesis_impl.h"
