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

using namespace mrpt::graphslam::deciders;
using namespace std;

#include <sstream>

// Implementation of classes defined in the CNodeRegistrationDecider class
// template.
//
template <class GRAPH_T>
CNodeRegistrationDecider<GRAPH_T>::CNodeRegistrationDecider() :
    m_prev_registered_nodeID(mrpt::graphs::INVALID_NODEID)
{
  m_init_inf_mat.setIdentity();
  m_init_inf_mat *= 10000;
  this->resetPDF(&this->m_since_prev_node_PDF);
}

template <class GRAPH_T>
void CNodeRegistrationDecider<GRAPH_T>::getDescriptiveReport(std::string* report_str) const
{
  MRPT_START

  stringstream ss("");
  parent_t::getDescriptiveReport(report_str);

  ss << "Node Registration Decider Strategy [NRD]: " << endl;
  *report_str += ss.str();

  MRPT_END
}

template <class GRAPH_T>
bool CNodeRegistrationDecider<GRAPH_T>::checkRegistrationCondition()
{
  return false;
}

template <class GRAPH_T>
bool CNodeRegistrationDecider<GRAPH_T>::registerNewNodeAtEnd(
    const typename GRAPH_T::constraint_t& constraint)
{
  MRPT_START
  using namespace std;
  using mrpt::graphs::TNodeID;

  // register the initial node if it doesn't exist.
  // Runs only once.
  if (this->m_prev_registered_nodeID == mrpt::graphs::INVALID_NODEID)
  {  // root
    MRPT_LOG_WARN("Registering root node...");
    global_pose_t tmp_pose = this->getCurrentRobotPosEstimation();
    this->addNodeAnnotsToPose(&tmp_pose);

// make sure that this pair hasn't been registered yet.
#if _DEBUG
    std::pair<typename GRAPH_T::global_poses_t::const_iterator, bool> res =
#endif
        this->m_graph->nodes.insert(make_pair(this->m_graph->root, tmp_pose));
    ASSERTDEBMSG_(
        res.second, mrpt::format(
                        "nodeID \"%lu\" with pose \"%s\" seems to be "
                        "already registered.",
                        this->m_graph->root, tmp_pose.asString().c_str()));

    this->m_prev_registered_nodeID = this->m_graph->root;
  }

  // FROM nodeID
  TNodeID from = this->m_prev_registered_nodeID;
  // TO nodeID
  // In all cases this is going to be ONE AFTER the last registered nodeID
  TNodeID to = this->m_graph->nodeCount();

  // add the new pose.
  {
    global_pose_t tmp_pose = this->getCurrentRobotPosEstimation();
    this->addNodeAnnotsToPose(&tmp_pose);

#if _DEBUG
    std::pair<typename GRAPH_T::global_poses_t::const_iterator, bool> res =
#endif
        this->m_graph->nodes.insert(make_pair(to, tmp_pose));
    ASSERTDEBMSG_(
        res.second, mrpt::format(
                        "nodeID \"%lu\" with pose \"%s\" seems to be "
                        "already registered.",
                        to, tmp_pose.asString().c_str()));
    this->m_graph->insertEdgeAtEnd(from, to, constraint);
  }

  m_prev_registered_nodeID = to;

  MRPT_LOG_DEBUG_STREAM(
      "Registered new node:" << endl
                             << "\t" << from << " => " << to << endl
                             << "\tEdge: " << constraint.getMeanVal().asString());

  return true;
  MRPT_END
}

template <class GRAPH_T>
bool CNodeRegistrationDecider<GRAPH_T>::registerNewNodeAtEnd()
{
  bool res = this->registerNewNodeAtEnd(this->m_since_prev_node_PDF);

  // reset the PDF since the last registered node position
  this->resetPDF(&m_since_prev_node_PDF);

  return res;
}

template <class GRAPH_T>
void CNodeRegistrationDecider<GRAPH_T>::resetPDF(constraint_t* c)
{
  MRPT_START
  ASSERTDEB_(c);

  *c = constraint_t();
  ASSERTDEB_(c->isInfType());
  c->cov_inv = this->m_init_inf_mat;

  MRPT_END
}  // end of resetPDF

template <class GRAPH_T>
void CNodeRegistrationDecider<GRAPH_T>::addNodeAnnotsToPose(global_pose_t* pose) const
{
}

template <class GRAPH_T>
typename GRAPH_T::global_pose_t CNodeRegistrationDecider<GRAPH_T>::getCurrentRobotPosEstimation()
    const
{
  global_pose_t pose_out;

  if (this->m_prev_registered_nodeID != mrpt::graphs::INVALID_NODEID)
  {
    pose_out = this->m_graph->nodes.at(this->m_prev_registered_nodeID);
  }

  pose_out += m_since_prev_node_PDF.getMeanVal();
  return pose_out;
}
