/* +------------------------------------------------------------------------+
   |                     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/containers/copy_container_typecasting.h>
#include <mrpt/containers/printf_vector.h>
#include <mrpt/core/lock_helper.h>
#include <mrpt/io/CFileGZOutputStream.h>
#include <mrpt/io/CMemoryStream.h>
#include <mrpt/maps/CPointCloudFilterByDistance.h>
#include <mrpt/math/geometry.h>
#include <mrpt/math/ops_containers.h>  // sum()
#include <mrpt/math/wrap2pi.h>
#include <mrpt/nav/reactive/CAbstractPTGBasedReactive.h>
#include <mrpt/serialization/CArchive.h>
#include <mrpt/system/filesystem.h>

#include <array>
#include <iomanip>
#include <limits>

using namespace mrpt;
using namespace mrpt::io;
using namespace mrpt::poses;
using namespace mrpt::math;
using namespace mrpt::system;
using namespace mrpt::nav;
using namespace mrpt::serialization;
using namespace std;

// ------ CAbstractPTGBasedReactive::TNavigationParamsPTG -----
std::string CAbstractPTGBasedReactive::TNavigationParamsPTG::getAsText() const
{
	std::string s =
		CWaypointsNavigator::TNavigationParamsWaypoints::getAsText();
	s += "restrict_PTG_indices: ";
	s += mrpt::containers::sprintf_vector("%u ", this->restrict_PTG_indices);
	s += "\n";
	return s;
}

bool CAbstractPTGBasedReactive::TNavigationParamsPTG::isEqual(
	const CAbstractNavigator::TNavigationParamsBase& rhs) const
{
	auto o =
		dynamic_cast<const CAbstractPTGBasedReactive::TNavigationParamsPTG*>(
			&rhs);
	return o != nullptr &&
		CWaypointsNavigator::TNavigationParamsWaypoints::isEqual(rhs) &&
		restrict_PTG_indices == o->restrict_PTG_indices;
}

// Ctor:
CAbstractPTGBasedReactive::CAbstractPTGBasedReactive(
	CRobot2NavInterface& react_iterf_impl, bool enableConsoleOutput,
	bool enableLogFile, const std::string& sLogDir)
	: CWaypointsNavigator(react_iterf_impl),
	  m_enableConsoleOutput(enableConsoleOutput),
	  m_navlogfiles_dir(sLogDir)
{
	this->enableLogFile(enableLogFile);
}

void CAbstractPTGBasedReactive::preDestructor()
{
	m_closing_navigator = true;

	// Wait to end of navigation (multi-thread...)
	m_nav_cs.lock();
	m_nav_cs.unlock();

	// Just in case.
	try
	{
		// Call base class method, NOT the generic, virtual one,
		// to avoid problems if we are in the dtor, while the vtable
		// is being destroyed.
		CAbstractNavigator::stop(false /*not emergency*/);
	}
	catch (...)
	{
	}

	m_logFile.reset();

	// Free holonomic method:
	this->deleteHolonomicObjects();
}

CAbstractPTGBasedReactive::~CAbstractPTGBasedReactive()
{
	this->preDestructor();	// ensure the robot is stopped; free dynamic objects
}

/** \callergraph */
void CAbstractPTGBasedReactive::initialize()
{
	auto lck = mrpt::lockHelper(m_nav_cs);

	m_infoPerPTG_timestamp = INVALID_TIMESTAMP;

	ASSERT_(m_multiobjopt);
	m_multiobjopt->clear();

	// Compute collision grids:
	STEP1_InitPTGs();
}

/*---------------------------------------------------------------
						enableLogFile
  ---------------------------------------------------------------*/
void CAbstractPTGBasedReactive::enableLogFile(bool enable)
{
	auto lck = mrpt::lockHelper(m_nav_cs);

	try
	{
		// Disable:
		// -------------------------------
		if (!enable)
		{
			if (m_logFile)
			{
				MRPT_LOG_DEBUG(
					"[CAbstractPTGBasedReactive::enableLogFile] Stopping "
					"logging.");
				m_logFile.reset();	// Close file:
			}
			else
				return;	 // Already disabled.
		}
		else
		{  // Enable
			// -------------------------------
			if (m_logFile) return;	// Already enabled:

			// Open file, find the first free file-name.
			MRPT_LOG_DEBUG_FMT(
				"[CAbstractPTGBasedReactive::enableLogFile] Creating rnav log "
				"directory: %s",
				m_navlogfiles_dir.c_str());
			mrpt::system::createDirectory(m_navlogfiles_dir);
			if (!mrpt::system::directoryExists(m_navlogfiles_dir))
			{
				THROW_EXCEPTION_FMT(
					"Could not create directory for navigation logs: `%s`",
					m_navlogfiles_dir.c_str());
			}

			std::string filToOpen;
			for (unsigned int nFile = 0;; nFile++)
			{
				filToOpen = mrpt::format(
					"%s/log_%03u.reactivenavlog", m_navlogfiles_dir.c_str(),
					nFile);
				if (!system::fileExists(filToOpen)) break;
			}

			// Open log file:
			{
				std::unique_ptr<CFileGZOutputStream> fil(
					new CFileGZOutputStream);
				bool ok = fil->open(filToOpen, 1 /* compress level */);
				if (!ok)
				{
					THROW_EXCEPTION_FMT(
						"Error opening log file: `%s`", filToOpen.c_str());
				}
				else
				{
					m_logFile = std::move(fil);
				}
			}

			MRPT_LOG_DEBUG(mrpt::format(
				"[CAbstractPTGBasedReactive::enableLogFile] Logging to "
				"file `%s`",
				filToOpen.c_str()));
		}
	}
	catch (const std::exception& e)
	{
		MRPT_LOG_ERROR_FMT(
			"[CAbstractPTGBasedReactive::enableLogFile] Exception: %s",
			e.what());
	}
}

void CAbstractPTGBasedReactive::getLastLogRecord(CLogFileRecord& o)
{
	auto lck = mrpt::lockHelper(m_critZoneLastLog);
	o = lastLogRecord;
}

void CAbstractPTGBasedReactive::deleteHolonomicObjects()
{
	m_holonomicMethod.clear();
}

void CAbstractPTGBasedReactive::setHolonomicMethod(
	const std::string& method, const mrpt::config::CConfigFileBase& ini)
{
	auto lck = mrpt::lockHelper(m_nav_cs);

	this->deleteHolonomicObjects();
	const size_t nPTGs = this->getPTG_count();
	ASSERT_(nPTGs != 0);
	m_holonomicMethod.resize(nPTGs);

	for (size_t i = 0; i < nPTGs; i++)
	{
		m_holonomicMethod[i] =
			CAbstractHolonomicReactiveMethod::Factory(method);
		if (!m_holonomicMethod[i])
			THROW_EXCEPTION_FMT(
				"Non-registered holonomic method className=`%s`",
				method.c_str());

		m_holonomicMethod[i]->setAssociatedPTG(this->getPTG(i));
		m_holonomicMethod[i]->initialize(ini);	// load params
	}
}

/** The main method: executes one time-iteration of the reactive navigation
 * algorithm.
 * \callergraph */
void CAbstractPTGBasedReactive::performNavigationStep()
{
	mrpt::system::CTimeLoggerEntry tle(
		m_navProfiler, "CAbstractPTGBasedReactive::performNavigationStep()");

	// Security tests:
	if (m_closing_navigator) return;  // Are we closing in the main thread?
	if (!m_init_done)
		THROW_EXCEPTION("Have you called loadConfigFile() before?");
	ASSERT_(m_navigationParams);
	const size_t nPTGs = this->getPTG_count();

	// Whether to worry about log files:
	const bool fill_log_record = (m_logFile || m_enableKeepLogRecords);
	CLogFileRecord newLogRec;
	newLogRec.infoPerPTG.resize(nPTGs + 1); /* +1: [N] is the "NOP cmdvel"
											   option; not to be present in all
											   log entries. */

	// At the beginning of each log file, add an introductory block explaining
	// which PTGs are we using:
	{
		if (m_logFile &&
			m_logFile.get() != m_prev_logfile)	// Only the first time
		{
			m_prev_logfile = m_logFile.get();
			for (size_t i = 0; i < nPTGs; i++)
			{
				// If we make a direct copy (=) we will store the entire, heavy,
				// collision grid.
				// Let's just store the parameters of each PTG by serializing
				// it, so paths can be reconstructed
				// by invoking initialize()
				mrpt::io::CMemoryStream buf;
				auto arch = archiveFrom(buf);
				arch << *this->getPTG(i);
				buf.Seek(0);
				newLogRec.infoPerPTG[i].ptg = std::dynamic_pointer_cast<
					mrpt::nav::CParameterizedTrajectoryGenerator>(
					arch.ReadObject());
			}
		}
	}

	CTimeLoggerEntry tle1(m_timelogger, "navigationStep");

	try
	{
		totalExecutionTime.Tic();  // Start timer

		const mrpt::system::TTimeStamp tim_start_iteration =
			mrpt::system::now();

		// Compute target location relative to current robot pose:
		// ---------------------------------------------------------------------
		// Detect changes in target since last iteration (for NOP):
		const bool target_changed_since_last_iteration =
			(!m_copy_prev_navParams) ||
			!(*m_copy_prev_navParams == *m_navigationParams);
		if (target_changed_since_last_iteration)
			m_copy_prev_navParams = m_navigationParams->clone();

		// Load the list of target(s) from the navigationParam user command.
		// Semantic is: any of the target is good. If several targets are
		// reachable, head to latest one.
		std::vector<CAbstractNavigator::TargetInfo> targets;
		{
			auto p = dynamic_cast<
				const CWaypointsNavigator::TNavigationParamsWaypoints*>(
				m_navigationParams.get());
			if (p && !p->multiple_targets.empty())
			{ targets = p->multiple_targets; }
			else
			{
				targets.push_back(m_navigationParams->target);
			}
		}
		const size_t nTargets = targets.size();	 // Normally = 1, will be >1 if
		// we want the robot local
		// planner to be "smarter" in
		// skipping dynamic obstacles.
		ASSERT_(nTargets >= 1);

		STEP1_InitPTGs();  // Will only recompute if
		// "m_PTGsMustBeReInitialized==true"

		// STEP2: Load the obstacles and sort them in height bands.
		// -----------------------------------------------------------------------------
		bool sense_ok;
		{
			mrpt::system::CTimeLoggerEntry tle2(
				m_navProfiler,
				"CAbstractPTGBasedReactive::performNavigationStep().STEP2_"
				"SenseObstacles()");

			sense_ok = STEP2_SenseObstacles();
		}

		if (!sense_ok)
		{
			doEmergencyStop(
				"Error while loading and sorting the obstacles. Robot will be "
				"stopped.");
			if (fill_log_record)
			{
				CPose2D rel_cur_pose_wrt_last_vel_cmd_NOP,
					rel_pose_PTG_origin_wrt_sense_NOP;
				STEP8_GenerateLogRecord(
					newLogRec,
					std::vector<mrpt::math::TPose2D>() /*no targets*/,
					-1,	 // best_ptg_idx,
					m_robot.getEmergencyStopCmd(), nPTGs,
					false,	// best_is_NOP_cmdvel,
					rel_cur_pose_wrt_last_vel_cmd_NOP.asTPose(),
					rel_pose_PTG_origin_wrt_sense_NOP.asTPose(),
					0,	// executionTimeValue,
					0,	// tim_changeSpeed,
					tim_start_iteration);
			}
			return;
		}

		// ------- start of motion decision zone ---------
		executionTime.Tic();

		// Round #1: As usual, pure reactive, evaluate all PTGs and all
		// directions from scratch.
		// =========

		mrpt::math::TPose2D rel_pose_PTG_origin_wrt_sense(0, 0, 0),
			relPoseSense(0, 0, 0), relPoseVelCmd(0, 0, 0);
		if (params_abstract_ptg_navigator.use_delays_model)
		{
			/*
			 *                                          Delays model
			 *
			 * Event:          OBSTACLES_SENSED         RNAV_ITERATION_STARTS
			 * GET_ROBOT_POSE_VEL         VEL_CMD_SENT_TO_ROBOT
			 * Timestamp:  (m_WS_Obstacles_timestamp)   (tim_start_iteration)
			 * (m_curPoseVelTimestamp)     ("tim_send_cmd_vel")
			 * Delay                       |
			 * <---+--------------->|<--------------+-------->| |
			 * estimator:                        |                |
			 * |                                   |
			 *                timoff_obstacles <-   |
			 * +--> timoff_curPoseVelAge           |
			 *                                                    |<---------------------------------+--------------->|
			 *                                                                                       +-->
			 * timoff_sendVelCmd_avr (estimation)
			 *
			 *                                                                                                |<-------------------->|
			 *                                                                                                   tim_changeSpeed_avr
			 * (estim)
			 *
			 *                             |<-----------------------------------------------|-------------------------->|
			 *  Relative poses:                              relPoseSense
			 * relPoseVelCmd
			 *  Time offsets (signed):                       timoff_pose2sense
			 * timoff_pose2VelCmd
			 */
			const double timoff_obstacles = mrpt::system::timeDifference(
				tim_start_iteration, m_WS_Obstacles_timestamp);
			timoff_obstacles_avr.filter(timoff_obstacles);
			newLogRec.values["timoff_obstacles"] = timoff_obstacles;
			newLogRec.values["timoff_obstacles_avr"] =
				timoff_obstacles_avr.getLastOutput();
			newLogRec.timestamps["obstacles"] = m_WS_Obstacles_timestamp;

			const double timoff_curPoseVelAge = mrpt::system::timeDifference(
				tim_start_iteration, m_curPoseVel.timestamp);
			timoff_curPoseAndSpeed_avr.filter(timoff_curPoseVelAge);
			newLogRec.values["timoff_curPoseVelAge"] = timoff_curPoseVelAge;
			newLogRec.values["timoff_curPoseVelAge_avr"] =
				timoff_curPoseAndSpeed_avr.getLastOutput();

			// time offset estimations:
			const double timoff_pose2sense =
				timoff_obstacles - timoff_curPoseVelAge;

			double timoff_pose2VelCmd;
			timoff_pose2VelCmd = timoff_sendVelCmd_avr.getLastOutput() +
				0.5 * tim_changeSpeed_avr.getLastOutput() -
				timoff_curPoseVelAge;
			newLogRec.values["timoff_pose2sense"] = timoff_pose2sense;
			newLogRec.values["timoff_pose2VelCmd"] = timoff_pose2VelCmd;

			if (std::abs(timoff_pose2sense) > 1.25)
				MRPT_LOG_WARN_FMT(
					"timoff_pose2sense=%e is too large! Path extrapolation may "
					"be not accurate.",
					timoff_pose2sense);
			if (std::abs(timoff_pose2VelCmd) > 1.25)
				MRPT_LOG_WARN_FMT(
					"timoff_pose2VelCmd=%e is too large! Path extrapolation "
					"may be not accurate.",
					timoff_pose2VelCmd);

			// Path extrapolation: robot relative poses along current path
			// estimation:
			relPoseSense = m_curPoseVel.velLocal * timoff_pose2sense;
			relPoseVelCmd = m_curPoseVel.velLocal * timoff_pose2VelCmd;

			// relative pose for PTGs:
			rel_pose_PTG_origin_wrt_sense = relPoseVelCmd - relPoseSense;

			// logging:
			newLogRec.relPoseSense = relPoseSense;
			newLogRec.relPoseVelCmd = relPoseVelCmd;
		}
		else
		{
			// No delays model: poses to their default values.
		}

		for (auto& t : targets)
		{
			if (t.target_frame_id != m_curPoseVel.pose_frame_id)
			{
				auto frame_tf = m_frame_tf.lock();
				if (!frame_tf)
				{
					THROW_EXCEPTION_FMT(
						"Different frame_id's but no frame_tf object "
						"attached (or it expired)!: target_frame_id=`%s` != "
						"pose_frame_id=`%s`",
						t.target_frame_id.c_str(),
						m_curPoseVel.pose_frame_id.c_str());
				}
				mrpt::math::TPose2D robot_frame2map_frame;
				frame_tf->lookupTransform(
					t.target_frame_id, m_curPoseVel.pose_frame_id,
					robot_frame2map_frame);

				MRPT_LOG_DEBUG_FMT(
					"frame_tf: target_frame_id=`%s` as seen from "
					"pose_frame_id=`%s` = %s",
					t.target_frame_id.c_str(),
					m_curPoseVel.pose_frame_id.c_str(),
					robot_frame2map_frame.asString().c_str());

				t.target_coords = robot_frame2map_frame + t.target_coords;
				t.target_frame_id =
					m_curPoseVel.pose_frame_id;	 // Now the coordinates are in
				// the same frame than robot
				// pose
			}
		}

		std::vector<TPose2D> relTargets;
		const auto curPoseExtrapol = (m_curPoseVel.pose + relPoseVelCmd);
		std::transform(
			targets.begin(), targets.end(),	 // in
			std::back_inserter(relTargets),	 // out
			[curPoseExtrapol](const CAbstractNavigator::TargetInfo& e) {
				return e.target_coords - curPoseExtrapol;
			});
		ASSERT_EQUAL_(relTargets.size(), targets.size());

		// Use the distance to the first target as reference:
		const double relTargetDist = relTargets.begin()->norm();

		// PTG dynamic state
		/** Allow PTGs to be responsive to target location, dynamics, etc. */
		CParameterizedTrajectoryGenerator::TNavDynamicState ptg_dynState;

		ptg_dynState.curVelLocal = m_curPoseVel.velLocal;
		ptg_dynState.relTarget = relTargets[0];
		ptg_dynState.targetRelSpeed =
			m_navigationParams->target.targetDesiredRelSpeed;

		newLogRec.navDynState = ptg_dynState;

		{
			mrpt::system::CTimeLoggerEntry tle2(
				m_navProfiler,
				"CAbstractPTGBasedReactive::performNavigationStep().update_PTG_"
				"dynamic_states");

			for (size_t i = 0; i < nPTGs; i++)
				getPTG(i)->updateNavDynamicState(ptg_dynState);
		}

		m_infoPerPTG.assign(nPTGs + 1, TInfoPerPTG());	// reset contents
		m_infoPerPTG_timestamp = tim_start_iteration;
		vector<TCandidateMovementPTG> candidate_movs(
			nPTGs + 1);	 // the last extra one is for the evaluation of "NOP
		// motion command" choice.

		for (size_t indexPTG = 0; indexPTG < nPTGs; indexPTG++)
		{
			mrpt::system::CTimeLoggerEntry tle2(
				m_navProfiler,
				"CAbstractPTGBasedReactive::performNavigationStep().eval_"
				"regular_PTG");

			CParameterizedTrajectoryGenerator* ptg = getPTG(indexPTG);
			TInfoPerPTG& ipf = m_infoPerPTG[indexPTG];

			// Ensure the method knows about its associated PTG:
			auto holoMethod = this->getHoloMethod(indexPTG);
			ASSERT_(holoMethod);
			holoMethod->setAssociatedPTG(this->getPTG(indexPTG));

			// The picked movement in TP-Space (to be determined by holonomic
			// method below)
			TCandidateMovementPTG& cm = candidate_movs[indexPTG];

			ASSERT_(m_navigationParams);
			build_movement_candidate(
				ptg, indexPTG, relTargets, rel_pose_PTG_origin_wrt_sense, ipf,
				cm, newLogRec, false /* this is a regular PTG reactive case */,
				*holoMethod, tim_start_iteration, *m_navigationParams);
		}  // end for each PTG

		// check for collision, which is reflected by ALL TP-Obstacles being
		// zero:
		bool is_all_ptg_collision = true;
		for (size_t indexPTG = 0; indexPTG < nPTGs; indexPTG++)
		{
			bool is_collision = true;
			const auto& obs = m_infoPerPTG[indexPTG].TP_Obstacles;
			for (const auto o : obs)
			{
				if (o != 0)
				{
					is_collision = false;
					break;
				}
			}
			if (!is_collision)
			{
				is_all_ptg_collision = false;
				break;
			}
		}
		if (is_all_ptg_collision)
		{
			m_pending_events.emplace_back(std::bind(
				&CRobot2NavInterface::sendApparentCollisionEvent,
				std::ref(m_robot)));
		}

		// Round #2: Evaluate dont sending any new velocity command ("NOP"
		// motion)
		// =========
		bool NOP_not_too_old = true;
		bool NOP_not_too_close_and_have_to_slowdown = true;
		double NOP_max_time = -1.0, NOP_At = -1.0;
		double slowdowndist = .0;
		CParameterizedTrajectoryGenerator* last_sent_ptg =
			(m_lastSentVelCmd.isValid() && m_lastSentVelCmd.ptg_index >= 0)
			? getPTG(m_lastSentVelCmd.ptg_index)
			: nullptr;
		if (last_sent_ptg)
		{
			// So supportSpeedAtTarget() below is evaluated in the correct
			// context:
			last_sent_ptg->updateNavDynamicState(m_lastSentVelCmd.ptg_dynState);
		}

		// This approach is only possible if:
		const bool can_do_nop_motion =
			(m_lastSentVelCmd.isValid() &&
			 !target_changed_since_last_iteration && last_sent_ptg &&
			 last_sent_ptg->supportVelCmdNOP()) &&
			(NOP_not_too_old =
				 (NOP_At = mrpt::system::timeDifference(
					  m_lastSentVelCmd.tim_send_cmd_vel, tim_start_iteration)) <
				 (NOP_max_time = last_sent_ptg->maxTimeInVelCmdNOP(
									 m_lastSentVelCmd.ptg_alpha_index) /
					  std::max(0.1, m_lastSentVelCmd.speed_scale))) &&
			(NOP_not_too_close_and_have_to_slowdown =
				 (last_sent_ptg->supportSpeedAtTarget() ||
				  (relTargetDist >
				   (slowdowndist = m_holonomicMethod[m_lastSentVelCmd.ptg_index]
									   ->getTargetApproachSlowDownDistance())
				   // slowdowndist is assigned here, inside the if()
				   // to be sure the index in m_lastSentVelCmd is valid!
				   )));

		if (!NOP_not_too_old)
		{
			newLogRec.additional_debug_msgs["PTG_cont"] = mrpt::format(
				"PTG-continuation not allowed: previous command timed-out "
				"(At=%.03f > Max_At=%.03f)",
				NOP_At, NOP_max_time);
		}
		if (!NOP_not_too_close_and_have_to_slowdown)
		{
			newLogRec.additional_debug_msgs["PTG_cont_trgdst"] = mrpt::format(
				"PTG-continuation not allowed: target too close and must start "
				"slow-down (trgDist=%.03f < SlowDownDist=%.03f)",
				relTargetDist, slowdowndist);
		}

		mrpt::math::TPose2D rel_cur_pose_wrt_last_vel_cmd_NOP(0, 0, 0),
			rel_pose_PTG_origin_wrt_sense_NOP(0, 0, 0);

		if (can_do_nop_motion)
		{
			mrpt::system::CTimeLoggerEntry tle2(
				m_navProfiler,
				"CAbstractPTGBasedReactive::performNavigationStep().eval_NOP");

			// Add the estimation of how long it takes to run the changeSpeeds()
			// callback (usually a tiny period):
			const mrpt::system::TTimeStamp tim_send_cmd_vel_corrected =
				mrpt::system::timestampAdd(
					m_lastSentVelCmd.tim_send_cmd_vel,
					tim_changeSpeed_avr.getLastOutput());

			// Note: we use (uncorrected) raw odometry as basis to the following
			// calculation since it's normally
			// smoother than particle filter-based localization data, more
			// accurate in the middle/long term,
			// but not in the short term:
			mrpt::math::TPose2D robot_pose_at_send_cmd, robot_odom_at_send_cmd;
			bool valid_odom, valid_pose;

			m_latestOdomPoses.interpolate(
				tim_send_cmd_vel_corrected, robot_odom_at_send_cmd, valid_odom);
			m_latestPoses.interpolate(
				tim_send_cmd_vel_corrected, robot_pose_at_send_cmd, valid_pose);

			if (valid_odom && valid_pose)
			{
				ASSERT_(last_sent_ptg != nullptr);

				std::vector<TPose2D> relTargets_NOPs;
				std::transform(
					targets.begin(), targets.end(),	 // in
					std::back_inserter(relTargets_NOPs),  // out
					[robot_pose_at_send_cmd](
						const CAbstractNavigator::TargetInfo& e) {
						return e.target_coords - robot_pose_at_send_cmd;
					});
				ASSERT_EQUAL_(relTargets_NOPs.size(), targets.size());

				rel_pose_PTG_origin_wrt_sense_NOP = robot_odom_at_send_cmd -
					(m_curPoseVel.rawOdometry + relPoseSense);
				rel_cur_pose_wrt_last_vel_cmd_NOP =
					m_curPoseVel.rawOdometry - robot_odom_at_send_cmd;

				// Update PTG response to dynamic params:
				last_sent_ptg->updateNavDynamicState(
					m_lastSentVelCmd.ptg_dynState);

				if (fill_log_record)
				{
					newLogRec.additional_debug_msgs
						["rel_cur_pose_wrt_last_vel_cmd_NOP(interp)"] =
						rel_cur_pose_wrt_last_vel_cmd_NOP.asString();
					newLogRec.additional_debug_msgs
						["robot_odom_at_send_cmd(interp)"] =
						robot_odom_at_send_cmd.asString();
				}

				// No need to call setAssociatedPTG(), already correctly
				// associated above.

				ASSERT_(m_navigationParams);
				build_movement_candidate(
					last_sent_ptg, m_lastSentVelCmd.ptg_index, relTargets_NOPs,
					rel_pose_PTG_origin_wrt_sense_NOP, m_infoPerPTG[nPTGs],
					candidate_movs[nPTGs], newLogRec,
					true /* this is the PTG continuation (NOP) choice */,
					*getHoloMethod(m_lastSentVelCmd.ptg_index),
					tim_start_iteration, *m_navigationParams,
					rel_cur_pose_wrt_last_vel_cmd_NOP);

			}  // end valid interpolated origin pose
			else
			{
				// Can't interpolate pose, hence can't evaluate NOP:
				candidate_movs[nPTGs].speed =
					-0.01;	// <0 means inviable movement
			}
		}  // end can_do_NOP_motion

		// Evaluate all the candidates and pick the "best" one, using
		// the user-defined multiobjective optimizer
		// --------------------------------------------------------------
		CMultiObjectiveMotionOptimizerBase::TResultInfo mo_info;
		ASSERT_(m_multiobjopt);
		int best_ptg_idx = m_multiobjopt->decide(candidate_movs, mo_info);

		if (fill_log_record)
		{
			if (mo_info.final_evaluation.size() == newLogRec.infoPerPTG.size())
			{
				for (unsigned int i = 0; i < newLogRec.infoPerPTG.size(); i++)
				{
					newLogRec.infoPerPTG[i].evaluation =
						mo_info.final_evaluation[i];
				}
			}
			int idx = 0;
			for (const auto& le : mo_info.log_entries)
			{
				newLogRec.additional_debug_msgs[mrpt::format(
					"MultiObjMotOptmzr_msg%03i", idx++)] = le;
			}
		}

		// Pick best movement (or null if none is good)
		const TCandidateMovementPTG* selectedHolonomicMovement = nullptr;
		if (best_ptg_idx >= 0)
		{ selectedHolonomicMovement = &candidate_movs[best_ptg_idx]; }

		// If the selected PTG is (N+1), it means the NOP cmd. vel is selected
		// as the best alternative, i.e. do NOT send any new motion command.
		const bool best_is_NOP_cmdvel = (best_ptg_idx == int(nPTGs));

		// ---------------------------------------------------------------------
		//				SEND MOVEMENT COMMAND TO THE ROBOT
		// ---------------------------------------------------------------------
		mrpt::kinematics::CVehicleVelCmd::Ptr new_vel_cmd;
		if (best_is_NOP_cmdvel)
		{
			// Notify the robot that we want it to keep executing the last
			// cmdvel:
			if (!this->changeSpeedsNOP())
			{
				doEmergencyStop(
					"\nERROR calling changeSpeedsNOP()!! Stopping robot and "
					"finishing navigation\n");
				if (fill_log_record)
				{
					STEP8_GenerateLogRecord(
						newLogRec, relTargets, best_ptg_idx,
						m_robot.getEmergencyStopCmd(), nPTGs,
						best_is_NOP_cmdvel, rel_cur_pose_wrt_last_vel_cmd_NOP,
						rel_pose_PTG_origin_wrt_sense_NOP,
						0,	// executionTimeValue,
						0,	// tim_changeSpeed,
						tim_start_iteration);
				}
				return;
			}
		}
		else
		{
			// Make sure the dynamic state of a NOP cmd has not overwritten the
			// state for a "regular" PTG choice:
			for (size_t i = 0; i < nPTGs; i++)
			{
				getPTG(i)->updateNavDynamicState(ptg_dynState);
			}

			// STEP7: Get the non-holonomic movement command.
			// ---------------------------------------------------------------------
			double cmd_vel_speed_ratio = 1.0;
			if (selectedHolonomicMovement)
			{
				CTimeLoggerEntry tle2(
					m_timelogger, "navigationStep.selectedHolonomicMovement");
				cmd_vel_speed_ratio =
					generate_vel_cmd(*selectedHolonomicMovement, new_vel_cmd);
				ASSERT_(new_vel_cmd);
			}

			if (!new_vel_cmd /* which means best_PTG_eval==.0*/ ||
				new_vel_cmd->isStopCmd())
			{
				MRPT_LOG_DEBUG(
					"Best velocity command is STOP (no way found), calling "
					"robot.stop()");
				this->stop(true /* emergency */);  // don't call
				// doEmergencyStop() here
				// since that will stop
				// navigation completely
				new_vel_cmd = m_robot.getEmergencyStopCmd();
				m_lastSentVelCmd.reset();
			}
			else
			{
				mrpt::system::TTimeStamp tim_send_cmd_vel;
				{
					mrpt::system::CTimeLoggerEntry tle2(
						m_timlog_delays, "changeSpeeds()");
					tim_send_cmd_vel = mrpt::system::now();
					newLogRec.timestamps["tim_send_cmd_vel"] = tim_send_cmd_vel;
					if (!this->changeSpeeds(*new_vel_cmd))
					{
						doEmergencyStop(
							"\nERROR calling changeSpeeds()!! Stopping robot "
							"and finishing navigation\n");
						if (fill_log_record)
						{
							new_vel_cmd = m_robot.getEmergencyStopCmd();
							STEP8_GenerateLogRecord(
								newLogRec, relTargets, best_ptg_idx,
								new_vel_cmd, nPTGs, best_is_NOP_cmdvel,
								rel_cur_pose_wrt_last_vel_cmd_NOP,
								rel_pose_PTG_origin_wrt_sense_NOP,
								0,	// executionTimeValue,
								0,	// tim_changeSpeed,
								tim_start_iteration);
						}
						return;
					}
				}
				// Save last sent cmd:
				m_lastSentVelCmd.speed_scale = cmd_vel_speed_ratio;
				m_lastSentVelCmd.ptg_index = best_ptg_idx;
				m_lastSentVelCmd.ptg_alpha_index = selectedHolonomicMovement
					? selectedHolonomicMovement->PTG->alpha2index(
						  selectedHolonomicMovement->direction)
					: 0;
				m_lastSentVelCmd.original_holo_eval =
					selectedHolonomicMovement->props.at("holo_stage_eval");

				m_lastSentVelCmd.colfreedist_move_k = best_ptg_idx >= 0
					? m_infoPerPTG[best_ptg_idx]
						  .TP_Obstacles[m_lastSentVelCmd.ptg_alpha_index]
					: .0;
				m_lastSentVelCmd.was_slowdown =
					(selectedHolonomicMovement->props.at("is_slowdown") != 0.0);

				m_lastSentVelCmd.poseVel = m_curPoseVel;
				m_lastSentVelCmd.tim_send_cmd_vel = tim_send_cmd_vel;
				m_lastSentVelCmd.ptg_dynState = ptg_dynState;

				// Update delay model:
				const double timoff_sendVelCmd = mrpt::system::timeDifference(
					tim_start_iteration, tim_send_cmd_vel);
				timoff_sendVelCmd_avr.filter(timoff_sendVelCmd);
				newLogRec.values["timoff_sendVelCmd"] = timoff_sendVelCmd;
				newLogRec.values["timoff_sendVelCmd_avr"] =
					timoff_sendVelCmd_avr.getLastOutput();
			}
		}

		// ------- end of motion decision zone ---------

		// Statistics:
		// ----------------------------------------------------
		const double executionTimeValue = executionTime.Tac();
		meanExecutionTime.filter(executionTimeValue);
		meanTotalExecutionTime.filter(totalExecutionTime.Tac());

		const double tim_changeSpeed =
			m_timlog_delays.getLastTime("changeSpeeds()");
		tim_changeSpeed_avr.filter(tim_changeSpeed);

		// Running period estim:
		const double tNow = mrpt::Clock::nowDouble();
		const double period_tim = tNow - m_lastExecutionTimestamp;
		m_lastExecutionTimestamp = tNow;
		if (period_tim > 1.5 * meanExecutionPeriod.getLastOutput() &&
			period_tim < 2.0 /* Ignore long periods, likely nav pauses */
		)
		{
			MRPT_LOG_WARN_FMT(
				"Timing warning: Suspicious executionPeriod=%.03f ms is >150%% "
				"the average of %.03f ms",
				1e3 * period_tim, meanExecutionPeriod.getLastOutput() * 1e3);
		}
		meanExecutionPeriod.filter(period_tim);

		if (m_enableConsoleOutput)
		{
			MRPT_LOG_DEBUG(mrpt::format(
				"CMD: %s "
				"speedScale=%.04f "
				"T=%.01lfms Exec:%.01lfms|%.01lfms "
				"PTG#%i\n",
				new_vel_cmd ? new_vel_cmd->asString().c_str() : "NOP",
				selectedHolonomicMovement ? selectedHolonomicMovement->speed
										  : .0,
				1000.0 * meanExecutionPeriod.getLastOutput(),
				1000.0 * meanExecutionTime.getLastOutput(),
				1000.0 * meanTotalExecutionTime.getLastOutput(), best_ptg_idx));
		}
		if (fill_log_record)
		{
			STEP8_GenerateLogRecord(
				newLogRec, relTargets, best_ptg_idx, new_vel_cmd, nPTGs,
				best_is_NOP_cmdvel, rel_cur_pose_wrt_last_vel_cmd_NOP,
				rel_pose_PTG_origin_wrt_sense_NOP, executionTimeValue,
				tim_changeSpeed, tim_start_iteration);
		}
	}
	catch (const std::exception& e)
	{
		doEmergencyStop(
			std::string("[CAbstractPTGBasedReactive::performNavigationStep] "
						"Stopping robot and finishing navigation due to "
						"exception:\n") +
			std::string(e.what()));
	}
	catch (...)
	{
		doEmergencyStop(
			"[CAbstractPTGBasedReactive::performNavigationStep] Stopping robot "
			"and finishing navigation due to untyped exception.");
	}
}

/** \callergraph */
void CAbstractPTGBasedReactive::STEP8_GenerateLogRecord(
	CLogFileRecord& newLogRec, const std::vector<TPose2D>& relTargets,
	int nSelectedPTG, const mrpt::kinematics::CVehicleVelCmd::Ptr& new_vel_cmd,
	const int nPTGs, const bool best_is_NOP_cmdvel,
	const mrpt::math::TPose2D& rel_cur_pose_wrt_last_vel_cmd_NOP,
	const mrpt::math::TPose2D& rel_pose_PTG_origin_wrt_sense_NOP,
	const double executionTimeValue, const double tim_changeSpeed,
	const mrpt::system::TTimeStamp& tim_start_iteration)
{
	// ---------------------------------------
	// STEP8: Generate log record
	// ---------------------------------------
	mrpt::system::CTimeLoggerEntry tle(
		m_navProfiler, "CAbstractPTGBasedReactive::STEP8_GenerateLogRecord()");

	m_timelogger.enter("navigationStep.populate_log_info");

	this->loggingGetWSObstaclesAndShape(newLogRec);

	newLogRec.robotPoseLocalization = m_curPoseVel.pose;
	newLogRec.robotPoseOdometry = m_curPoseVel.rawOdometry;
	newLogRec.WS_targets_relative = relTargets;
	newLogRec.nSelectedPTG = nSelectedPTG;
	newLogRec.cur_vel = m_curPoseVel.velGlobal;
	newLogRec.cur_vel_local = m_curPoseVel.velLocal;
	newLogRec.cmd_vel = new_vel_cmd;
	newLogRec.values["estimatedExecutionPeriod"] =
		meanExecutionPeriod.getLastOutput();
	newLogRec.values["executionTime"] = executionTimeValue;
	newLogRec.values["executionTime_avr"] = meanExecutionTime.getLastOutput();
	newLogRec.values["time_changeSpeeds()"] = tim_changeSpeed;
	newLogRec.values["time_changeSpeeds()_avr"] =
		tim_changeSpeed_avr.getLastOutput();
	newLogRec.values["CWaypointsNavigator::navigationStep()"] =
		m_timlog_delays.getLastTime("CWaypointsNavigator::navigationStep()");
	newLogRec.values["CAbstractNavigator::navigationStep()"] =
		m_timlog_delays.getLastTime("CAbstractNavigator::navigationStep()");
	newLogRec.timestamps["tim_start_iteration"] = tim_start_iteration;
	newLogRec.timestamps["curPoseAndVel"] = m_curPoseVel.timestamp;
	newLogRec.nPTGs = nPTGs;

	// NOP mode  stuff:
	newLogRec.rel_cur_pose_wrt_last_vel_cmd_NOP =
		rel_cur_pose_wrt_last_vel_cmd_NOP;
	newLogRec.rel_pose_PTG_origin_wrt_sense_NOP =
		rel_pose_PTG_origin_wrt_sense_NOP;
	newLogRec.ptg_index_NOP =
		best_is_NOP_cmdvel ? m_lastSentVelCmd.ptg_index : -1;
	newLogRec.ptg_last_k_NOP = m_lastSentVelCmd.ptg_alpha_index;
	newLogRec.ptg_last_navDynState = m_lastSentVelCmd.ptg_dynState;

	m_timelogger.leave("navigationStep.populate_log_info");

	//  Save to log file:
	// --------------------------------------
	{
		mrpt::system::CTimeLoggerEntry tle2(
			m_timelogger, "navigationStep.write_log_file");
		if (m_logFile) archiveFrom(*m_logFile) << newLogRec;
	}
	// Set as last log record
	{
		auto lck = mrpt::lockHelper(m_critZoneLastLog);
		lastLogRecord = newLogRec;	// COPY
	}
}

/** \callergraph */
void CAbstractPTGBasedReactive::calc_move_candidate_scores(
	TCandidateMovementPTG& cm, const std::vector<double>& in_TPObstacles,
	const mrpt::nav::ClearanceDiagram& in_clearance,
	const std::vector<mrpt::math::TPose2D>& WS_Targets,
	const std::vector<CAbstractPTGBasedReactive::PTGTarget>& TP_Targets,
	CLogFileRecord::TInfoPerPTG& log, CLogFileRecord& newLogRec,
	const bool this_is_PTG_continuation,
	const mrpt::math::TPose2D& rel_cur_pose_wrt_last_vel_cmd_NOP,
	const unsigned int ptg_idx4weights,
	const mrpt::system::TTimeStamp tim_start_iteration,
	const mrpt::nav::CHolonomicLogFileRecord::Ptr& hlfr)
{
	MRPT_START
	mrpt::system::CTimeLoggerEntry tle(
		m_navProfiler,
		"CAbstractPTGBasedReactive::calc_move_candidate_scores()");

	const double ref_dist = cm.PTG->getRefDistance();

	// Replaced by: TP_Targets[i].*
	// const double   target_dir    = (TP_Target.x!=0 || TP_Target.y!=0) ?
	// atan2( TP_Target.y, TP_Target.x) : 0.0;
	// const int      target_k = static_cast<int>( cm.PTG->alpha2index(
	// target_dir ) );
	// const double   target_d_norm   = TP_Target.norm();

	// We need to evaluate the movement wrt to ONE target of the possibly many
	// input ones.
	// Policy: use the target whose TP-Space direction is closer to this
	// candidate direction:
	size_t selected_trg_idx = 0;
	{
		double best_trg_angdist = std::numeric_limits<double>::max();
		for (size_t i = 0; i < TP_Targets.size(); i++)
		{
			const double angdist = std::abs(mrpt::math::angDistance(
				TP_Targets[i].target_alpha, cm.direction));
			if (angdist < best_trg_angdist)
			{
				best_trg_angdist = angdist;
				selected_trg_idx = i;
			}
		}
	}
	ASSERT_(selected_trg_idx < WS_Targets.size());
	const auto WS_Target = WS_Targets[selected_trg_idx];
	const auto TP_Target = TP_Targets[selected_trg_idx];

	const double target_d_norm = TP_Target.target_dist;

	// Picked movement direction:
	const int move_k = static_cast<int>(cm.PTG->alpha2index(cm.direction));
	const double target_WS_d = WS_Target.norm();

	// Coordinates of the trajectory end for the given PTG and "alpha":
	const double d = std::min(in_TPObstacles[move_k], 0.99 * target_d_norm);
	uint32_t nStep;
	bool pt_in_range = cm.PTG->getPathStepForDist(move_k, d, nStep);
	ASSERT_(pt_in_range);
	const mrpt::math::TPose2D pose = cm.PTG->getPathPose(move_k, nStep);

	// Make sure that the target slow-down is honored, as seen in real-world
	// Euclidean space
	// (as opposed to TP-Space, where the holo methods are evaluated)
	if (m_navigationParams &&
		m_navigationParams->target.targetDesiredRelSpeed < 1.0 &&
		!m_holonomicMethod.empty() && getHoloMethod(0) != nullptr &&
		!cm.PTG->supportSpeedAtTarget()	 // If the PTG is able to handle the
		// slow-down on its own, dont change
		// speed here
	)
	{
		const double TARGET_SLOW_APPROACHING_DISTANCE =
			getHoloMethod(0)->getTargetApproachSlowDownDistance();

		const double Vf =
			m_navigationParams->target.targetDesiredRelSpeed;  // [0,1]

		const double f = std::min(
			1.0,
			Vf + target_WS_d * (1.0 - Vf) / TARGET_SLOW_APPROACHING_DISTANCE);
		if (f < cm.speed)
		{
			newLogRec.additional_debug_msgs["PTG_eval.speed"] = mrpt::format(
				"Relative speed reduced %.03f->%.03f based on Euclidean "
				"nearness to target.",
				cm.speed, f);
			cm.speed = f;
		}
	}

	// Start storing params in the candidate move structure:
	cm.props["ptg_idx"] = ptg_idx4weights;
	cm.props["ref_dist"] = ref_dist;
	cm.props["target_dir"] = TP_Target.target_alpha;
	cm.props["target_k"] = TP_Target.target_k;
	cm.props["target_d_norm"] = target_d_norm;
	cm.props["move_k"] = move_k;
	double& move_cur_d = cm.props["move_cur_d"] =
		0;	// current robot path normalized distance over path (0 unless in a
	// NOP cmd)
	cm.props["is_PTG_cont"] = this_is_PTG_continuation ? 1 : 0;
	cm.props["num_paths"] = in_TPObstacles.size();
	cm.props["WS_target_x"] = WS_Target.x;
	cm.props["WS_target_y"] = WS_Target.y;
	cm.props["robpose_x"] = pose.x;
	cm.props["robpose_y"] = pose.y;
	cm.props["robpose_phi"] = pose.phi;
	cm.props["ptg_priority"] = cm.PTG->getScorePriority() *
		cm.PTG->evalPathRelativePriority(TP_Target.target_k, target_d_norm);
	const bool is_slowdown = this_is_PTG_continuation
		? m_lastSentVelCmd.was_slowdown
		: (cm.PTG->supportSpeedAtTarget() && TP_Target.target_k == move_k &&
		   target_d_norm < 0.99 * in_TPObstacles[move_k]);
	cm.props["is_slowdown"] = is_slowdown ? 1 : 0;
	cm.props["holo_stage_eval"] = this_is_PTG_continuation
		? m_lastSentVelCmd.original_holo_eval
		: (hlfr && !hlfr->dirs_eval.empty() &&
		   hlfr->dirs_eval.rbegin()->size() == in_TPObstacles.size())
			? hlfr->dirs_eval.rbegin()->at(move_k)
			: .0;
	// Factor 1: Free distance for the chosen PTG and "alpha" in the TP-Space:
	// ----------------------------------------------------------------------
	double& colfree = cm.props["collision_free_distance"];

	// distance to collision:
	colfree = in_TPObstacles[move_k];  // we'll next substract here the
	// already-traveled distance, for NOP
	// motion candidates.

	// Special case for NOP motion cmd:
	// consider only the empty space *after* the current robot pose, which is
	// not at the origin.

	if (this_is_PTG_continuation)
	{
		int cur_k = 0;
		double cur_norm_d = .0;
		bool is_exact, is_time_based = false;
		uint32_t cur_ptg_step = 0;

		// Use: time-based prediction for shorter distances, PTG inverse
		// mapping-based for longer ranges:
		const double maxD = params_abstract_ptg_navigator
								.max_dist_for_timebased_path_prediction;
		if (std::abs(rel_cur_pose_wrt_last_vel_cmd_NOP.x) > maxD ||
			std::abs(rel_cur_pose_wrt_last_vel_cmd_NOP.y) > maxD)
		{
			is_exact = cm.PTG->inverseMap_WS2TP(
				rel_cur_pose_wrt_last_vel_cmd_NOP.x,
				rel_cur_pose_wrt_last_vel_cmd_NOP.y, cur_k, cur_norm_d);
		}
		else
		{
			// Use time:
			is_time_based = true;
			is_exact = true;  // well, sort of...
			const double NOP_At =
				m_lastSentVelCmd.speed_scale *
				mrpt::system::timeDifference(
					m_lastSentVelCmd.tim_send_cmd_vel, tim_start_iteration);
			newLogRec.additional_debug_msgs["PTG_eval.NOP_At"] =
				mrpt::format("%.06f s", NOP_At);
			cur_k = move_k;
			cur_ptg_step = mrpt::round(NOP_At / cm.PTG->getPathStepDuration());
			cur_norm_d = cm.PTG->getPathDist(cur_k, cur_ptg_step) /
				cm.PTG->getRefDistance();
			{
				const double cur_a = cm.PTG->index2alpha(cur_k);
				log.TP_Robot.x = cos(cur_a) * cur_norm_d;
				log.TP_Robot.y = sin(cur_a) * cur_norm_d;
				cm.starting_robot_dir = cur_a;
				cm.starting_robot_dist = cur_norm_d;
			}
		}

		if (!is_exact)
		{
			// Don't trust this step: we are not 100% sure of the robot pose in
			// TP-Space for this "PTG continuation" step:
			cm.speed = -0.01;  // this enforces a 0 global evaluation score
			newLogRec.additional_debug_msgs["PTG_eval"] =
				"PTG-continuation not allowed, cur. pose out of PTG domain.";
			return;
		}
		bool WS_point_is_unique = true;
		if (!is_time_based)
		{
			bool ok1 = cm.PTG->getPathStepForDist(
				m_lastSentVelCmd.ptg_alpha_index,
				cur_norm_d * cm.PTG->getRefDistance(), cur_ptg_step);
			if (ok1)
			{
				// Check bijective:
				WS_point_is_unique = cm.PTG->isBijectiveAt(cur_k, cur_ptg_step);
				const uint32_t predicted_step =
					mrpt::system::timeDifference(
						m_lastSentVelCmd.tim_send_cmd_vel,
						mrpt::system::now()) /
					cm.PTG->getPathStepDuration();
				WS_point_is_unique = WS_point_is_unique &&
					cm.PTG->isBijectiveAt(move_k, predicted_step);
				newLogRec.additional_debug_msgs["PTG_eval.bijective"] =
					mrpt::format(
						"isBijectiveAt(): k=%i step=%i -> %s",
						static_cast<int>(cur_k), static_cast<int>(cur_ptg_step),
						WS_point_is_unique ? "yes" : "no");

				if (!WS_point_is_unique)
				{
					// Don't trust direction:
					cur_k = move_k;
					cur_ptg_step = predicted_step;
					cur_norm_d = cm.PTG->getPathDist(cur_k, cur_ptg_step);
				}
				{
					const double cur_a = cm.PTG->index2alpha(cur_k);
					log.TP_Robot.x = cos(cur_a) * cur_norm_d;
					log.TP_Robot.y = sin(cur_a) * cur_norm_d;
					cm.starting_robot_dir = cur_a;
					cm.starting_robot_dist = cur_norm_d;
				}

				const mrpt::math::TPose2D predicted_rel_pose =
					cm.PTG->getPathPose(
						m_lastSentVelCmd.ptg_alpha_index, cur_ptg_step);
				const auto predicted_pose_global =
					m_lastSentVelCmd.poseVel.rawOdometry + predicted_rel_pose;
				const double predicted2real_dist = mrpt::hypot_fast(
					predicted_pose_global.x - m_curPoseVel.rawOdometry.x,
					predicted_pose_global.y - m_curPoseVel.rawOdometry.y);
				newLogRec.additional_debug_msgs["PTG_eval.lastCmdPose(raw)"] =
					m_lastSentVelCmd.poseVel.pose.asString();
				newLogRec.additional_debug_msgs["PTG_eval.PTGcont"] =
					mrpt::format(
						"mismatchDistance=%.03f cm", 1e2 * predicted2real_dist);

				if (predicted2real_dist >
						params_abstract_ptg_navigator
							.max_distance_predicted_actual_path &&
					(!is_slowdown ||
					 (target_d_norm - cur_norm_d) * ref_dist > 2.0 /*meters*/))
				{
					cm.speed =
						-0.01;	// this enforces a 0 global evaluation score
					newLogRec.additional_debug_msgs["PTG_eval"] =
						"PTG-continuation not allowed, mismatchDistance above "
						"threshold.";
					return;
				}
			}
			else
			{
				cm.speed = -0.01;  // this enforces a 0 global evaluation score
				newLogRec.additional_debug_msgs["PTG_eval"] =
					"PTG-continuation not allowed, couldn't get PTG step for "
					"cur. robot pose.";
				return;
			}
		}

		// Path following isn't perfect: we can't be 100% sure of whether the
		// robot followed exactly
		// the intended path (`kDirection`), or if it's actually a bit shifted,
		// as reported in `cur_k`.
		// Take the least favorable case.
		// [Update] Do this only when the PTG gave us a unique-mapped WS<->TPS
		// point:

		colfree = WS_point_is_unique
			? std::min(in_TPObstacles[move_k], in_TPObstacles[cur_k])
			: in_TPObstacles[move_k];

		// Only discount free space if there was a real obstacle, not the "end
		// of path" due to limited refDistance.
		if (colfree < 0.99) { colfree -= cur_norm_d; }

		// Save estimated robot pose over path as a parameter for scores:
		move_cur_d = cur_norm_d;
	}

	// Factor4: Decrease in euclidean distance between (x,y) and the target:
	//  Moving away of the target is negatively valued.
	cm.props["dist_eucl_final"] =
		mrpt::hypot_fast(WS_Target.x - pose.x, WS_Target.y - pose.y);

	// dist_eucl_min: Use PTG clearance methods to evaluate the real-world
	// (WorkSpace) minimum distance to target:
	{
		using map_d2d_t = std::map<double, double>;
		map_d2d_t pathDists;
		const double D = cm.PTG->getRefDistance();
		const int num_steps = ceil(D * 2.0);
		for (int i = 0; i < num_steps; i++)
		{
			pathDists[i / double(num_steps)] =
				100.0;	// default normalized distance to target (a huge value)
		}

		cm.PTG->evalClearanceSingleObstacle(
			WS_Target.x, WS_Target.y, move_k, pathDists,
			false /*treat point as target, not obstacle*/);

		const auto it = std::min_element(
			pathDists.begin(), pathDists.end(),
			[colfree](map_d2d_t::value_type& l, map_d2d_t::value_type& r)
				-> bool { return (l.second < r.second) && l.first < colfree; });
		cm.props["dist_eucl_min"] = (it != pathDists.end())
			? it->second * cm.PTG->getRefDistance()
			: 100.0;
	}

	// Factor5: Hysteresis:
	// -----------------------------------------------------
	double& hysteresis = cm.props["hysteresis"];
	hysteresis = .0;

	if (cm.PTG->supportVelCmdNOP())
	{ hysteresis = this_is_PTG_continuation ? 1.0 : 0.; }
	else if (m_last_vel_cmd)
	{
		mrpt::kinematics::CVehicleVelCmd::Ptr desired_cmd;
		desired_cmd = cm.PTG->directionToMotionCommand(move_k);
		const mrpt::kinematics::CVehicleVelCmd* ptr1 = m_last_vel_cmd.get();
		const mrpt::kinematics::CVehicleVelCmd* ptr2 = desired_cmd.get();
		if (typeid(*ptr1) == typeid(*ptr2))
		{
			ASSERT_EQUAL_(
				m_last_vel_cmd->getVelCmdLength(),
				desired_cmd->getVelCmdLength());

			double simil_score = 0.5;
			for (size_t i = 0; i < desired_cmd->getVelCmdLength(); i++)
			{
				const double scr =
					exp(-std::abs(
							desired_cmd->getVelCmdElement(i) -
							m_last_vel_cmd->getVelCmdElement(i)) /
						0.20);
				mrpt::keep_min(simil_score, scr);
			}
			hysteresis = simil_score;
		}
	}

	// Factor6: clearance
	// -----------------------------------------------------
	// clearance indicators that may be useful in deciding the best motion:
	double& clearance = cm.props["clearance"];
	clearance = in_clearance.getClearance(
		move_k, target_d_norm * 1.01, false /* spot, dont interpolate */);
	cm.props["clearance_50p"] = in_clearance.getClearance(
		move_k, target_d_norm * 0.5, false /* spot, dont interpolate */);
	cm.props["clearance_path"] = in_clearance.getClearance(
		move_k, target_d_norm * 0.9, true /* average */);
	cm.props["clearance_path_50p"] = in_clearance.getClearance(
		move_k, target_d_norm * 0.5, true /* average */);

	// Factor: ETA (Estimated Time of Arrival to target or to closest obstacle,
	// whatever it's first)
	// -----------------------------------------------------
	double& eta = cm.props["eta"];
	eta = .0;
	if (cm.PTG && cm.speed > .0)  // for valid cases only
	{
		// OK, we have a direct path to target without collisions.
		const double path_len_meters = d * ref_dist;

		// Calculate their ETA
		uint32_t target_step;
		bool valid_step =
			cm.PTG->getPathStepForDist(move_k, path_len_meters, target_step);
		if (valid_step)
		{
			eta = cm.PTG->getPathStepDuration() *
				target_step /* PTG original time to get to target point */
				* cm.speed /* times the speed scale factor*/;

			double discount_time = .0;
			if (this_is_PTG_continuation)
			{
				// Heuristic: discount the time already executed.
				// Note that hm.speed above scales the overall path time using
				// the current speed scale, not the exact
				// integration over the past timesteps. It's an approximation,
				// probably good enough...
				discount_time = mrpt::system::timeDifference(
					m_lastSentVelCmd.tim_send_cmd_vel, tim_start_iteration);
			}
			eta -= discount_time;  // This could even become negative if the
			// approximation is poor...
		}
	}

	MRPT_END
}

double CAbstractPTGBasedReactive::generate_vel_cmd(
	const TCandidateMovementPTG& in_movement,
	mrpt::kinematics::CVehicleVelCmd::Ptr& new_vel_cmd)
{
	mrpt::system::CTimeLoggerEntry tle(m_timelogger, "generate_vel_cmd");
	double cmdvel_speed_scale = 1.0;
	try
	{
		if (in_movement.speed == 0)
		{
			// The robot will stop:
			new_vel_cmd =
				in_movement.PTG->getSupportedKinematicVelocityCommand();
			new_vel_cmd->setToStop();
		}
		else
		{
			const bool is_slowdown = in_movement.props.count("is_slowdown") != 0
				? in_movement.props.at("is_slowdown") != 0
				: false;

			// Take the normalized movement command:
			new_vel_cmd = in_movement.PTG->directionToMotionCommand(
				in_movement.PTG->alpha2index(in_movement.direction));

			// Scale holonomic speeds to real-world one:
			if (!is_slowdown)
			{
				new_vel_cmd->cmdVel_scale(in_movement.speed);
				cmdvel_speed_scale *= in_movement.speed;

				if (!m_last_vel_cmd)  // first iteration? Use default values:
					m_last_vel_cmd =
						in_movement.PTG->getSupportedKinematicVelocityCommand();

				// Honor user speed limits & "blending":
				const double beta = meanExecutionPeriod.getLastOutput() /
					(meanExecutionPeriod.getLastOutput() +
					 params_abstract_ptg_navigator.speedfilter_tau);
				cmdvel_speed_scale *= new_vel_cmd->cmdVel_limits(
					*m_last_vel_cmd, beta,
					params_abstract_ptg_navigator.robot_absolute_speed_limits);
			}
		}

		m_last_vel_cmd = new_vel_cmd;  // Save for filtering in next step
	}
	catch (const std::exception& e)
	{
		MRPT_LOG_ERROR_STREAM(
			"[CAbstractPTGBasedReactive::generate_vel_cmd] Exception: "
			<< e.what());
	}
	return cmdvel_speed_scale;
}

/** \callergraph */
bool CAbstractPTGBasedReactive::impl_waypoint_is_reachable(
	const mrpt::math::TPoint2D& wp) const
{
	MRPT_START

	const size_t N = this->getPTG_count();
	if (m_infoPerPTG.size() < N ||
		m_infoPerPTG_timestamp == INVALID_TIMESTAMP ||
		mrpt::system::timeDifference(
			m_infoPerPTG_timestamp, mrpt::system::now()) > 0.5)
		return false;  // We didn't run yet or obstacle info is old

	for (size_t i = 0; i < N; i++)
	{
		const CParameterizedTrajectoryGenerator* ptg = getPTG(i);

		const std::vector<double>& tp_obs =
			m_infoPerPTG[i].TP_Obstacles;  // normalized distances
		if (tp_obs.size() != ptg->getPathCount())
			continue;  // May be this PTG has not been used so far? (Target out
		// of domain,...)

		int wp_k;
		double wp_norm_d;
		bool is_into_domain =
			ptg->inverseMap_WS2TP(wp.x, wp.y, wp_k, wp_norm_d);
		if (!is_into_domain) continue;

		ASSERT_(wp_k < int(tp_obs.size()));

		const double collision_free_dist = tp_obs[wp_k];
		if (collision_free_dist > 1.01 * wp_norm_d)
			return true;  // free path found to target
	}

	return false;  // no way found
	MRPT_END
}

/** \callergraph */
bool CAbstractPTGBasedReactive::STEP2_SenseObstacles()
{
	return implementSenseObstacles(m_WS_Obstacles_timestamp);
}

/** \callergraph */
void CAbstractPTGBasedReactive::onStartNewNavigation()
{
	m_last_curPoseVelUpdate_robot_time = -1e9;
	m_lastSentVelCmd.reset();

	CWaypointsNavigator::onStartNewNavigation();  // Call base method we
	// override
}

/** \callergraph */
void CAbstractPTGBasedReactive::build_movement_candidate(
	CParameterizedTrajectoryGenerator* ptg, const size_t indexPTG,
	const std::vector<mrpt::math::TPose2D>& relTargets,
	const mrpt::math::TPose2D& rel_pose_PTG_origin_wrt_sense, TInfoPerPTG& ipf,
	TCandidateMovementPTG& cm, CLogFileRecord& newLogRec,
	const bool this_is_PTG_continuation,
	mrpt::nav::CAbstractHolonomicReactiveMethod& holoMethod,
	const mrpt::system::TTimeStamp tim_start_iteration,
	const TNavigationParams& navp,
	const mrpt::math::TPose2D& rel_cur_pose_wrt_last_vel_cmd_NOP)
{
	mrpt::system::CTimeLoggerEntry tle(
		m_navProfiler, "CAbstractPTGBasedReactive::build_movement_candidate()");

	ASSERT_(ptg);

	const size_t idx_in_log_infoPerPTGs =
		this_is_PTG_continuation ? getPTG_count() : indexPTG;

	CHolonomicLogFileRecord::Ptr HLFR;
	cm.PTG = ptg;

	// If the user doesn't want to use this PTG, just mark it as invalid:
	ipf.targets.clear();
	bool use_this_ptg = true;
	{
		const auto* navpPTG = dynamic_cast<const TNavigationParamsPTG*>(&navp);
		if (navpPTG && !navpPTG->restrict_PTG_indices.empty())
		{
			use_this_ptg = false;
			for (size_t i = 0;
				 i < navpPTG->restrict_PTG_indices.size() && !use_this_ptg; i++)
			{
				if (navpPTG->restrict_PTG_indices[i] == indexPTG)
					use_this_ptg = true;
			}
		}
	}

	double timeForTPObsTransformation = .0, timeForHolonomicMethod = .0;

	// Normal PTG validity filter: check if target falls into the PTG domain:
	bool any_TPTarget_is_valid = false;
	if (use_this_ptg)
	{
		for (const auto& trg : relTargets)
		{
			PTGTarget ptg_target;

			ptg_target.valid_TP = ptg->inverseMap_WS2TP(
				trg.x, trg.y, ptg_target.target_k, ptg_target.target_dist);
			if (!ptg_target.valid_TP) continue;

			any_TPTarget_is_valid = true;
			ptg_target.target_alpha = ptg->index2alpha(ptg_target.target_k);
			ptg_target.TP_Target.x =
				cos(ptg_target.target_alpha) * ptg_target.target_dist;
			ptg_target.TP_Target.y =
				sin(ptg_target.target_alpha) * ptg_target.target_dist;

			ipf.targets.emplace_back(ptg_target);
		}
	}

	if (!any_TPTarget_is_valid)
	{
		newLogRec.additional_debug_msgs[mrpt::format(
			"mov_candidate_%u", static_cast<unsigned int>(indexPTG))] =
			"PTG discarded since target(s) is(are) out of domain.";
	}
	else
	{
		//  STEP3(b): Build TP-Obstacles
		// -----------------------------------------------------------------------------
		{
			tictac.Tic();

			// Initialize TP-Obstacles:
			const size_t Ki = ptg->getAlphaValuesCount();
			ptg->initTPObstacles(ipf.TP_Obstacles);
			if (params_abstract_ptg_navigator.evaluate_clearance)
			{ ptg->initClearanceDiagram(ipf.clearance); }

			// Implementation-dependent conversion:
			STEP3_WSpaceToTPSpace(
				indexPTG, ipf.TP_Obstacles, ipf.clearance,
				mrpt::math::TPose2D(0, 0, 0) - rel_pose_PTG_origin_wrt_sense,
				params_abstract_ptg_navigator.evaluate_clearance);

			if (params_abstract_ptg_navigator.evaluate_clearance)
			{ ptg->updateClearancePost(ipf.clearance, ipf.TP_Obstacles); }

			// Distances in TP-Space are normalized to [0,1]:
			const double _refD = 1.0 / ptg->getRefDistance();
			for (size_t i = 0; i < Ki; i++)
				ipf.TP_Obstacles[i] *= _refD;

			timeForTPObsTransformation = tictac.Tac();
			if (m_timelogger.isEnabled())
				m_timelogger.registerUserMeasure(
					"navigationStep.STEP3_WSpaceToTPSpace",
					timeForTPObsTransformation);
		}

		//  STEP4: Holonomic navigation method
		// -----------------------------------------------------------------------------
		if (!this_is_PTG_continuation)
		{
			tictac.Tic();

			// Slow down if we are approaching the final target, etc.
			holoMethod.enableApproachTargetSlowDown(
				navp.target.targetDesiredRelSpeed < .11);

			// Prepare holonomic algorithm call:
			CAbstractHolonomicReactiveMethod::NavInput ni;
			ni.clearance = &ipf.clearance;
			ni.maxObstacleDist = 1.0;
			ni.maxRobotSpeed = 1.0;	 // So, we use a normalized max speed here.
			ni.obstacles = ipf.TP_Obstacles;  // Normalized [0,1]

			ni.targets.clear();	 // Normalized [0,1]
			for (const auto& t : ipf.targets)
			{
				ni.targets.push_back(t.TP_Target);
			}

			CAbstractHolonomicReactiveMethod::NavOutput no;

			holoMethod.navigate(ni, no);

			// Extract resuls:
			cm.direction = no.desiredDirection;
			cm.speed = no.desiredSpeed;
			HLFR = no.logRecord;

			// Security: Scale down the velocity when heading towards obstacles,
			//  such that it's assured that we never go thru an obstacle!
			const int kDirection =
				static_cast<int>(cm.PTG->alpha2index(cm.direction));
			double obsFreeNormalizedDistance = ipf.TP_Obstacles[kDirection];

			// Take into account the future robot pose after NOP motion
			// iterations to slow down accordingly *now*
			if (ptg->supportVelCmdNOP())
			{
				const double v = mrpt::hypot_fast(
					m_curPoseVel.velLocal.vx, m_curPoseVel.velLocal.vy);
				const double d = v * ptg->maxTimeInVelCmdNOP(kDirection);
				obsFreeNormalizedDistance = std::min(
					obsFreeNormalizedDistance,
					std::max(0.90, obsFreeNormalizedDistance - d));
			}

			double velScale = 1.0;
			ASSERT_(
				params_abstract_ptg_navigator.secure_distance_end >
				params_abstract_ptg_navigator.secure_distance_start);
			if (obsFreeNormalizedDistance <
				params_abstract_ptg_navigator.secure_distance_end)
			{
				if (obsFreeNormalizedDistance <=
					params_abstract_ptg_navigator.secure_distance_start)
					velScale = 0.0;	 // security stop
				else
					velScale =
						(obsFreeNormalizedDistance -
						 params_abstract_ptg_navigator.secure_distance_start) /
						(params_abstract_ptg_navigator.secure_distance_end -
						 params_abstract_ptg_navigator.secure_distance_start);
			}

			// Scale:
			cm.speed *= velScale;

			timeForHolonomicMethod = tictac.Tac();
			if (m_timelogger.isEnabled())
				m_timelogger.registerUserMeasure(
					"navigationStep.STEP4_HolonomicMethod",
					timeForHolonomicMethod);
		}
		else
		{
			// "NOP cmdvel" case: don't need to re-run holo algorithm, just keep
			// the last selection:
			cm.direction = ptg->index2alpha(m_lastSentVelCmd.ptg_alpha_index);
			cm.speed = 1.0;	 // Not used.
		}

		// STEP5: Evaluate each movement to assign them a "evaluation" value.
		// ---------------------------------------------------------------------
		{
			CTimeLoggerEntry tle2(
				m_timelogger, "navigationStep.calc_move_candidate_scores");

			calc_move_candidate_scores(
				cm, ipf.TP_Obstacles, ipf.clearance, relTargets, ipf.targets,
				newLogRec.infoPerPTG[idx_in_log_infoPerPTGs], newLogRec,
				this_is_PTG_continuation, rel_cur_pose_wrt_last_vel_cmd_NOP,
				indexPTG, tim_start_iteration, HLFR);

			// Store NOP related extra vars:
			cm.props["original_col_free_dist"] = this_is_PTG_continuation
				? m_lastSentVelCmd.colfreedist_move_k
				: .0;

			//  SAVE LOG
			newLogRec.infoPerPTG[idx_in_log_infoPerPTGs].evalFactors = cm.props;
		}

	}  // end "valid_TP"

	// Logging:
	const bool fill_log_record =
		(m_logFile != nullptr || m_enableKeepLogRecords);
	if (fill_log_record)
	{
		CLogFileRecord::TInfoPerPTG& ipp =
			newLogRec.infoPerPTG[idx_in_log_infoPerPTGs];
		if (!this_is_PTG_continuation) ipp.PTG_desc = ptg->getDescription();
		else
			ipp.PTG_desc = mrpt::format(
				"NOP cmdvel (prev PTG idx=%u)",
				static_cast<unsigned int>(m_lastSentVelCmd.ptg_index));

		mrpt::containers::copy_container_typecasting(
			ipf.TP_Obstacles, ipp.TP_Obstacles);
		ipp.clearance = ipf.clearance;
		ipp.TP_Targets.clear();
		for (const auto& t : ipf.targets)
		{
			ipp.TP_Targets.push_back(t.TP_Target);
		}
		ipp.HLFR = HLFR;
		ipp.desiredDirection = cm.direction;
		ipp.desiredSpeed = cm.speed;
		ipp.timeForTPObsTransformation = timeForTPObsTransformation;
		ipp.timeForHolonomicMethod = timeForHolonomicMethod;
	}
}

void CAbstractPTGBasedReactive::TAbstractPTGNavigatorParams::loadFromConfigFile(
	const mrpt::config::CConfigFileBase& c, const std::string& s)
{
	MRPT_START

	robot_absolute_speed_limits.loadConfigFile(c, s);

	MRPT_LOAD_CONFIG_VAR_REQUIRED_CS(holonomic_method, string);
	MRPT_LOAD_CONFIG_VAR_REQUIRED_CS(motion_decider_method, string);
	MRPT_LOAD_CONFIG_VAR_REQUIRED_CS(ref_distance, double);
	MRPT_LOAD_CONFIG_VAR_CS(speedfilter_tau, double);
	MRPT_LOAD_CONFIG_VAR_CS(secure_distance_start, double);
	MRPT_LOAD_CONFIG_VAR_CS(secure_distance_end, double);
	MRPT_LOAD_CONFIG_VAR_CS(use_delays_model, bool);
	MRPT_LOAD_CONFIG_VAR_CS(max_distance_predicted_actual_path, double);
	MRPT_LOAD_CONFIG_VAR_CS(
		min_normalized_free_space_for_ptg_continuation, double);
	MRPT_LOAD_CONFIG_VAR_CS(enable_obstacle_filtering, bool);
	MRPT_LOAD_CONFIG_VAR_CS(evaluate_clearance, bool);
	MRPT_LOAD_CONFIG_VAR_CS(max_dist_for_timebased_path_prediction, double);

	MRPT_END
}

void CAbstractPTGBasedReactive::TAbstractPTGNavigatorParams::saveToConfigFile(
	mrpt::config::CConfigFileBase& c, const std::string& s) const
{
	robot_absolute_speed_limits.saveToConfigFile(c, s);

	// Build list of known holo methods:
	string lstHoloStr = "# List of known classes:\n";
	{
		const auto lst = mrpt::rtti::getAllRegisteredClassesChildrenOf(
			CLASS_ID(CAbstractHolonomicReactiveMethod));
		for (const auto& cl : lst)
			lstHoloStr +=
				string("# - `") + string(cl->className) + string("`\n");
	}
	MRPT_SAVE_CONFIG_VAR_COMMENT(
		holonomic_method,
		string("C++ class name of the holonomic navigation method to run in "
			   "the transformed TP-Space.\n") +
			lstHoloStr);

	// Build list of known decider methods:
	string lstDecidersStr = "# List of known classes:\n";
	{
		const auto lst = mrpt::rtti::getAllRegisteredClassesChildrenOf(
			CLASS_ID(CMultiObjectiveMotionOptimizerBase));
		for (const auto& cl : lst)
			lstDecidersStr +=
				string("# - `") + string(cl->className) + string("`\n");
	}
	MRPT_SAVE_CONFIG_VAR_COMMENT(
		motion_decider_method,
		string("C++ class name of the motion decider method.\n") +
			lstDecidersStr);

	MRPT_SAVE_CONFIG_VAR_COMMENT(
		ref_distance,
		"Maximum distance up to obstacles will be considered (D_{max} in "
		"papers).");
	MRPT_SAVE_CONFIG_VAR_COMMENT(
		speedfilter_tau,
		"Time constant (in seconds) for the low-pass filter applied to "
		"kinematic velocity commands (default=0: no filtering)");
	MRPT_SAVE_CONFIG_VAR_COMMENT(
		secure_distance_start,
		"In normalized distance [0,1], start/end of a ramp function that "
		"scales the holonomic navigator output velocity.");
	MRPT_SAVE_CONFIG_VAR_COMMENT(
		secure_distance_end,
		"In normalized distance [0,1], start/end of a ramp function that "
		"scales the holonomic navigator output velocity.");
	MRPT_SAVE_CONFIG_VAR_COMMENT(
		use_delays_model,
		"Whether to use robot pose inter/extrapolation to improve accuracy "
		"(Default:false)");
	MRPT_SAVE_CONFIG_VAR_COMMENT(
		max_distance_predicted_actual_path,
		"Max distance [meters] to discard current PTG and issue a new vel cmd "
		"(default= 0.05)");
	MRPT_SAVE_CONFIG_VAR_COMMENT(
		min_normalized_free_space_for_ptg_continuation,
		"Min normalized dist [0,1] after current pose in a PTG continuation to "
		"allow it.");
	MRPT_SAVE_CONFIG_VAR_COMMENT(
		enable_obstacle_filtering,
		"Enabled obstacle filtering (params in its own section)");
	MRPT_SAVE_CONFIG_VAR_COMMENT(
		evaluate_clearance,
		"Enable exact computation of clearance (default=false)");
	MRPT_SAVE_CONFIG_VAR_COMMENT(
		max_dist_for_timebased_path_prediction,
		"Max dist [meters] to use time-based path prediction for NOP "
		"evaluation");
}

CAbstractPTGBasedReactive::TAbstractPTGNavigatorParams::
	TAbstractPTGNavigatorParams()
	: holonomic_method(),
	  ptg_cache_files_directory("."),

	  robot_absolute_speed_limits()

{
}

void CAbstractPTGBasedReactive::loadConfigFile(
	const mrpt::config::CConfigFileBase& c)
{
	MRPT_START
	m_PTGsMustBeReInitialized = true;

	// At this point, we have been called from the derived class, who must be
	// already loaded all its specific params, including PTGs.

	// Load my params:
	params_abstract_ptg_navigator.loadFromConfigFile(
		c, "CAbstractPTGBasedReactive");

	// Filtering:
	if (params_abstract_ptg_navigator.enable_obstacle_filtering)
	{
		auto* filter = new mrpt::maps::CPointCloudFilterByDistance;
		m_WS_filter = mrpt::maps::CPointCloudFilterBase::Ptr(filter);
		filter->options.loadFromConfigFile(c, "CPointCloudFilterByDistance");
	}
	else
	{
		m_WS_filter.reset();
	}

	// Movement chooser:
	m_multiobjopt = CMultiObjectiveMotionOptimizerBase::Factory(
		params_abstract_ptg_navigator.motion_decider_method);
	if (!m_multiobjopt)
		THROW_EXCEPTION_FMT(
			"Non-registered CMultiObjectiveMotionOptimizerBase className=`%s`",
			params_abstract_ptg_navigator.motion_decider_method.c_str());

	m_multiobjopt->loadConfigFile(c);

	// Holo method:
	this->setHolonomicMethod(params_abstract_ptg_navigator.holonomic_method, c);
	ASSERT_(!m_holonomicMethod.empty());
	CWaypointsNavigator::loadConfigFile(c);	 // Load parent params

	m_init_done =
		true;  // If we reached this point without an exception, all is good.
	MRPT_END
}

void CAbstractPTGBasedReactive::saveConfigFile(
	mrpt::config::CConfigFileBase& c) const
{
	CWaypointsNavigator::saveConfigFile(c);
	params_abstract_ptg_navigator.saveToConfigFile(
		c, "CAbstractPTGBasedReactive");

	// Filtering:
	{
		mrpt::maps::CPointCloudFilterByDistance filter;
		filter.options.saveToConfigFile(c, "CPointCloudFilterByDistance");
	}

	// Holo method:
	if (!m_holonomicMethod.empty() && m_holonomicMethod[0])
	{
		// Save my current settings:
		m_holonomicMethod[0]->saveConfigFile(c);
	}
	else
	{
		// save options of ALL known methods:
		const auto lst = mrpt::rtti::getAllRegisteredClassesChildrenOf(
			CLASS_ID(CAbstractHolonomicReactiveMethod));
		for (const auto& cl : lst)
		{
			mrpt::rtti::CObject::Ptr obj = cl->createObject();
			auto* holo =
				dynamic_cast<CAbstractHolonomicReactiveMethod*>(obj.get());
			if (holo) { holo->saveConfigFile(c); }
		}
	}

	// Decider method:
	if (m_multiobjopt)
	{
		// Save my current settings:
		m_multiobjopt->saveConfigFile(c);
	}
	else
	{
		// save options of ALL known methods:
		const auto lst = mrpt::rtti::getAllRegisteredClassesChildrenOf(
			CLASS_ID(CMultiObjectiveMotionOptimizerBase));
		for (const auto& cl : lst)
		{
			mrpt::rtti::CObject::Ptr obj = cl->createObject();
			auto* momo =
				dynamic_cast<CMultiObjectiveMotionOptimizerBase*>(obj.get());
			if (momo) { momo->saveConfigFile(c); }
		}
	}
}

void CAbstractPTGBasedReactive::setTargetApproachSlowDownDistance(
	const double dist)
{
	for (auto& o : m_holonomicMethod)
	{
		o->setTargetApproachSlowDownDistance(dist);
	}
}

double CAbstractPTGBasedReactive::getTargetApproachSlowDownDistance() const
{
	ASSERT_(!m_holonomicMethod.empty());
	return m_holonomicMethod[0]->getTargetApproachSlowDownDistance();
}

CAbstractHolonomicReactiveMethod* CAbstractPTGBasedReactive::getHoloMethod(
	int idx)
{
	return m_holonomicMethod[idx].get();
}
