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

#include "graphslam-precomp.h"  // Precompiled headers
//
#include <mrpt/graphslam/misc/CEdgeCounter.h>

// implementation file of CEdgeCounter class
using namespace mrpt::graphslam::detail;

CEdgeCounter::CEdgeCounter() { this->clearAllEdges(); }
void CEdgeCounter::setWindowManagerPtr(mrpt::graphslam::CWindowManager* win_manager)
{
  MRPT_START
  ASSERTDEB_(win_manager);
  ASSERTDEB_(win_manager->win);

  m_win_manager = win_manager;
  m_win = m_win_manager->win;

  MRPT_END
}

void CEdgeCounter::setRemovedEdges(int removed_edges)
{
  m_unique_edges = this->getTotalNumOfEdges() - removed_edges;
}

void CEdgeCounter::setLoopClosureEdgesManually(int num_loop_closures)
{
  m_num_loop_closures = num_loop_closures;
}

int CEdgeCounter::getLoopClosureEdges() const { return m_num_loop_closures; }
int CEdgeCounter::getTotalNumOfEdges() const
{
  int sum;
  this->getTotalNumOfEdges(&sum);

  return sum;
}

void CEdgeCounter::getTotalNumOfEdges(int* total_num_edges) const
{
  ASSERTDEB_(total_num_edges);
  int sum = 0;

  for (const auto& it : m_name_to_edges_num)
  {
    sum += it.second;
  }
  *total_num_edges = sum;
}

int CEdgeCounter::getNumForEdgeType(const std::string& name) const
{
  auto search = m_name_to_edges_num.find(name);
  if (search != m_name_to_edges_num.end())
  {
    return search->second;
  }
  else
  {
    THROW_EXCEPTION("No edge with such name exists");
  }
}

void CEdgeCounter::getNumForEdgeType(const std::string& name, int* total_num)
{
  auto search = m_name_to_edges_num.find(name);
  if (search != m_name_to_edges_num.end())
  {
    *total_num = search->second;
  }
  else
  {
    THROW_EXCEPTION("No edge with such name exists");
  }
}

void CEdgeCounter::setEdgesManually(const std::string& name, int num_of_edges)
{
  auto search = m_name_to_edges_num.find(name);
  if (search != m_name_to_edges_num.end())
  {
    search->second = num_of_edges;
  }
  else
  {
    std::string str_err = "No edge with such name exists.";
    THROW_EXCEPTION(str_err);
  }
  // Update the visualization if the user has already set the vizualization
  // parameters
  if (m_has_read_textmessage_params && m_win_manager)
  {
    this->updateTextMessages();
  }
}

void CEdgeCounter::addEdge(
    const std::string& name, bool is_loop_closure /* =false */, bool is_new /* =false */)
{
  auto search = m_name_to_edges_num.find(name);
  if (search != m_name_to_edges_num.end())
  {
    (search->second)++;  // increment to the found element

    // specify warning if is_new = true
    if (is_new)
    {
      std::string str_err = mrpt::format(
          "Specified edge type [%s] already exists but is_new is also "
          "specified!",
          name.c_str());
      THROW_EXCEPTION(str_err);
      // std::stringstream ss_warn;
      // ss_warn << "Commencing with the increment normally" << std::endl;
    }
    if (is_loop_closure)
    {
      // throw if user has also specified the is new boolean flag
      if (is_new)
      {
        std::string str_err =
            "Both is_new and is_loop_closure flags are true. "
            "Exiting...";
        THROW_EXCEPTION(str_err);
      }
      m_num_loop_closures++;
    }
  }
  else
  {
    if (is_new)
    {
      m_name_to_edges_num[name] = 1;
    }
    else
    {
      std::string str_err =
          "No edge with such name exists. Specify is_new parameter if "
          "you want to add it";
      THROW_EXCEPTION(str_err);
    }
  }

  // Update the visualization if the user has already set the vizualization
  // parameters
  if (m_has_read_textmessage_params && m_win_manager)
  {
    updateTextMessages();
  }
}

void CEdgeCounter::addEdgeType(const std::string& name)
{
  auto search = m_name_to_edges_num.find(name);

  if (search != m_name_to_edges_num.end())
  {
    THROW_EXCEPTION(mrpt::format("Specified edge type [%s] already exists", name.c_str()));
  }
  else
  {
    m_name_to_edges_num[name] = 0;
  }
}

void CEdgeCounter::clearAllEdges()
{
  m_num_loop_closures = 0;

  m_name_to_edges_num.clear();
  m_name_to_offset_y.clear();
  m_name_to_text_index.clear();

  m_has_read_textmessage_params = false;
  m_display_total_edges = false;
  m_display_loop_closures = false;
}

void CEdgeCounter::dumpToConsole() const
{
  std::string str(getAsString());
  std::cout << str << std::endl;
}

void CEdgeCounter::getAsString(std::string* str_out) const
{
  std::stringstream ss_out;
  std::string sep(80, '#');
  ss_out << "Summary of Edges: " << std::endl;
  ss_out << sep << std::endl;

  ss_out << "\tTotal registered edges: " << this->getTotalNumOfEdges() << std::endl;
  ss_out << "\tUnique edges (after removal of multiple edges connecting the "
            "same nodes): "
         << m_unique_edges << std::endl;

  for (const auto& it : m_name_to_edges_num)
  {
    ss_out << "\t" << it.first << " edges: " << it.second << std::endl;
  }
  ss_out << "\tLoop closure edges: " << this->getLoopClosureEdges() << std::endl;

  // dump the contents to the provided string
  *str_out = ss_out.str();
}

std::string CEdgeCounter::getAsString() const
{
  std::string str;
  this->getAsString(&str);
  return str;
}

// VISUALIZATION RELATED METHODS
// ////////////////////////////

void CEdgeCounter::setTextMessageParams(
    const std::map<std::string, double>& name_to_offset_y,
    const std::map<std::string, int>& name_to_text_index)
{
  ASSERTDEBMSG_(
      m_win_manager,
      "Visualization of data was requested but no CWindowManager pointer was "
      "provided");
  ASSERTDEB_EQUAL_(name_to_offset_y.size(), name_to_text_index.size());

  for (const auto& it : name_to_offset_y)
  {
    std::string name = it.first;

    // check if name already exist, otherwise throw exception
    auto search = m_name_to_edges_num.find(name);
    if (search == m_name_to_edges_num.end())
    {
      std::stringstream ss_err;
      ss_err << "Name " << name << " is not recognized as an Edge type." << std::endl;
      THROW_EXCEPTION(ss_err.str());
    }
    // name exists ...

    double offset_y = it.second;
    int text_index = name_to_text_index.find(name)->second;

    m_name_to_offset_y[name] = offset_y;
    m_name_to_text_index[name] = text_index;
  }

  m_has_read_textmessage_params = true;
}

void CEdgeCounter::setTextMessageParams(
    const std::map<std::string, double>& name_to_offset_y,
    const std::map<std::string, int>& name_to_text_index,
    double offset_y_total_edges,
    int text_index_total_edges,
    double offset_y_loop_closures,
    int text_index_loop_closures)
{
  // set the parameters for total edges / loop closures
  m_display_total_edges = true;
  m_display_loop_closures = true;

  m_offset_y_total_edges = offset_y_total_edges;
  m_offset_y_loop_closures = offset_y_loop_closures;

  m_text_index_total_edges = text_index_total_edges;
  m_text_index_loop_closures = text_index_loop_closures;

  // pass execution to the other setTextMessageParams
  this->setTextMessageParams(name_to_offset_y, name_to_text_index);
}

void CEdgeCounter::updateTextMessages() const
{
  ASSERTDEB_(m_win_manager);
  ASSERTDEB_(m_has_read_textmessage_params);
  ASSERTDEB_EQUAL_(m_name_to_offset_y.size(), m_name_to_text_index.size());

  // Add text message for the total amount of edges
  std::stringstream title;
  title << "Total edges: " << this->getTotalNumOfEdges();
  // if (m_unique_edges) {
  // title << " |Unique: " << m_unique_edges << std::endl;
  //}
  if (m_display_total_edges && m_win_manager)
  {
    m_win_manager->addTextMessage(
        5, -m_offset_y_total_edges, title.str(), mrpt::img::TColorf(1.0, 1.0, 1.0),
        /* unique_index = */ m_text_index_total_edges);
  }

  // add a textMessage for every stored edge type
  for (const auto& it : m_name_to_offset_y)
  {
    std::string name = it.first;
    double offset_y = it.second;
    int text_index = m_name_to_text_index.find(name)->second;
    int edges_num = m_name_to_edges_num.find(name)->second;

    std::stringstream ss;
    ss << "  " << name << ": " << edges_num << std::endl;
    m_win_manager->addTextMessage(
        5, -offset_y, ss.str(), mrpt::img::TColorf(1.0, 1.0, 1.0),
        /* unique_index = */ text_index);
  }

  // add text message for the loop closures
  if (m_display_loop_closures)
  {
    std::stringstream ss;
    ss << "  "
       << "Loop closures: " << m_num_loop_closures << std::endl;
    m_win_manager->addTextMessage(
        5, -m_offset_y_loop_closures, ss.str(), mrpt::img::TColorf(1.0, 1.0, 1.0),
        /* unique_index = */ m_text_index_loop_closures);
  }

  m_win->forceRepaint();
}
