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

#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());
	}
}
