/* +------------------------------------------------------------------------+
   |                     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 "nav-precomp.h"  // Precomp header
//
#include <mrpt/config/CConfigFileBase.h>
#include <mrpt/kinematics/CVehicleVelCmd_DiffDriven.h>
#include <mrpt/kinematics/CVehicleVelCmd_Holo.h>
#include <mrpt/nav/reactive/CNavigatorManualSequence.h>
#include <mrpt/system/string_utils.h>

using namespace mrpt::nav;

CNavigatorManualSequence::CNavigatorManualSequence(CRobot2NavInterface& robot_interface) :
    CAbstractNavigator(robot_interface)
{
}

// Dtor:
CNavigatorManualSequence::~CNavigatorManualSequence() = default;
void CNavigatorManualSequence::saveConfigFile(mrpt::config::CConfigFileBase& c) const {}

void CNavigatorManualSequence::loadConfigFile(const mrpt::config::CConfigFileBase& c)
{
  const std::string s = "CNavigatorManualSequence";

  programmed_orders.clear();
  std::vector<std::string> lstKeys;
  c.getAllKeys(s, lstKeys);

  for (const auto& lstKey : lstKeys)
  {
    std::string str = c.read_string(s, lstKey, "", true);
    std::vector<std::string> toks;
    mrpt::system::tokenize(str, " \t\r\n", toks);
    ASSERTMSG_(
        toks.size() > 2, std::string("Wrong format while parsing CNavigatorManualSequence "
                                     "cfg file in entry: ") +
                             lstKey);

    const double t = atof(toks[0].c_str());
    TVelCmd krc;

    const size_t nComps = toks.size() - 1;
    switch (nComps)
    {
      case 2:
        krc.cmd_vel = std::make_shared<mrpt::kinematics::CVehicleVelCmd_DiffDriven>();
        break;
      case 4:
        krc.cmd_vel = mrpt::kinematics::CVehicleVelCmd_Holo::Create();
        break;
      default:
        THROW_EXCEPTION("Expected 2 or 4 velocity components!");
    };

    for (size_t j = 0; j < nComps; j++) krc.cmd_vel->setVelCmdElement(j, atof(toks[j + 1].c_str()));

    // insert:
    programmed_orders[t] = krc;
  }
}

void CNavigatorManualSequence::initialize()
{
  ASSERT_(!programmed_orders.empty());
  m_robot.resetNavigationTimer();
}

/** Overriden in this class to ignore the cancel/pause/... commands */
void CNavigatorManualSequence::navigationStep()
{
  if (programmed_orders.empty()) return;

  const double t = m_robot.getNavigationTime();

  if (t >= programmed_orders.begin()->first)
  {
    const TVelCmd& krc = programmed_orders.begin()->second;
    // Send cmd:
    logFmt(
        mrpt::system::LVL_DEBUG, "[CNavigatorManualSequence] Sending cmd: t=%f\n",
        programmed_orders.begin()->first);

    if (!this->changeSpeeds(*krc.cmd_vel))
    {
      this->stop(true /*not emergency*/);
      logFmt(mrpt::system::LVL_ERROR, "[CNavigatorManualSequence] **ERROR** sending cmd to robot.");
      return;
    }
    // remove:
    programmed_orders.erase(programmed_orders.begin());
  }
}
