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

// implementation file for the THypothesis struct template

namespace mrpt::graphs::detail
{
template <class GRAPH_T>
THypothesis<GRAPH_T>::THypothesis() = default;

template <class GRAPH_T>
THypothesis<GRAPH_T>::~THypothesis() = default;

template <class GRAPH_T>
std::string THypothesis<GRAPH_T>::getAsString(bool oneline /*=true*/) const
{
  std::string str;
  this->getAsString(&str, oneline);
  return str;
}

template <class GRAPH_T>
void THypothesis<GRAPH_T>::getAsString(std::string* str, bool oneline /*=true*/) const
{
  ASSERTMSG_(str, "Given string pointer is not valid");

  using namespace std;

  stringstream ss;
  if (!oneline)
  {  // multiline report
    ss << "Hypothesis #" << id << endl;
    ss << from << " => " << to << endl;
    ss << edge << endl;
  }
  else
  {
    ss << "Hypothesis #" << id << "| ";
    ss << from << " => " << to << "| ";
    ss << edge.getMeanVal().asString();
    ss << "|goodness: " << goodness;
    ss << "|valid: " << is_valid;
  }

  *str = ss.str();
}

// TODO - test these
template <class GRAPH_T>
bool THypothesis<GRAPH_T>::sameEndsWith(const self_t& other) const
{
  return (this->from == other.from && this->to == other.to);
}

template <class GRAPH_T>
bool THypothesis<GRAPH_T>::hasEnds(mrpt::graphs::TNodeID from_in, mrpt::graphs::TNodeID to_in) const
{
  return (this->from == from_in && this->to == to_in);
}

template <class GRAPH_T>
void THypothesis<GRAPH_T>::getEdge(constraint_t* out_edge) const
{
  ASSERT_(out_edge);
  out_edge->copyFrom(edge);
}

template <class GRAPH_T>
typename GRAPH_T::constraint_t THypothesis<GRAPH_T>::getEdge() const
{
  return this->edge;
}

template <class GRAPH_T>
void THypothesis<GRAPH_T>::setEdge(const constraint_t& in_edge)
{
  edge.copyFrom(in_edge);
}

template <class GRAPH_T>
void THypothesis<GRAPH_T>::getInverseEdge(constraint_t* out_edge) const
{
  ASSERT_(out_edge);
  edge.inverse(*out_edge);
}

template <class GRAPH_T>
typename GRAPH_T::constraint_t THypothesis<GRAPH_T>::getInverseEdge() const
{
  constraint_t inverse_edge;
  this->getInverseEdge(&inverse_edge);

  return inverse_edge;
}

template <class GRAPH_T>
void THypothesis<GRAPH_T>::inverseHypothesis()
{
  // inverse the start/end nodes
  mrpt::graphs::TNodeID tmp = from;
  from = to;
  to = tmp;

  // inverse the edge
  constraint_t edge_tmp = this->getInverseEdge();
  this->edge.copyFrom(edge_tmp);
}

template <class GRAPH_T>
bool THypothesis<GRAPH_T>::operator<(const self_t& other) const
{
  return this->id < other.id;
}
}  // namespace mrpt::graphs::detail
