/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2023, 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/core/round.h>
#include <mrpt/math/geometry.h>
#include <mrpt/math/ops_containers.h>
#include <mrpt/nav/holonomic/CHolonomicFullEval.h>
#include <mrpt/serialization/CArchive.h>
#include <mrpt/serialization/stl_serialization.h>

#include <Eigen/Dense>	// col(),...
#include <cmath>

using namespace mrpt;
using namespace mrpt::math;
using namespace mrpt::nav;
using namespace std;

IMPLEMENTS_SERIALIZABLE(
	CLogFileRecord_FullEval, CHolonomicLogFileRecord, mrpt::nav)
IMPLEMENTS_SERIALIZABLE(
	CHolonomicFullEval, CAbstractHolonomicReactiveMethod, mrpt::nav)

const unsigned NUM_FACTORS = 7U;

CHolonomicFullEval::CHolonomicFullEval(
	const mrpt::config::CConfigFileBase* INI_FILE)
	: CAbstractHolonomicReactiveMethod("CHolonomicFullEval"),
	  m_last_selected_sector(std::numeric_limits<unsigned int>::max())
{
	if (INI_FILE != nullptr) initialize(*INI_FILE);
}

void CHolonomicFullEval::saveConfigFile(mrpt::config::CConfigFileBase& c) const
{
	options.saveToConfigFile(c, getConfigFileSectionName());
}

void CHolonomicFullEval::initialize(const mrpt::config::CConfigFileBase& c)
{
	options.loadFromConfigFile(c, getConfigFileSectionName());
}

struct TGap
{
	int k_from{-1}, k_to{-1};
	double min_eval, max_eval;
	bool contains_target_k{false};
	/** Direction with the best evaluation inside the gap. */
	int k_best_eval{-1};

	TGap()
		: min_eval(std::numeric_limits<double>::max()),
		  max_eval(-std::numeric_limits<double>::max())

	{
	}
};

void CHolonomicFullEval::evalSingleTarget(
	unsigned int target_idx, const NavInput& ni, EvalOutput& eo)
{
	ASSERT_(target_idx < ni.targets.size());
	const auto target = ni.targets[target_idx];

	using mrpt::square;

	eo = EvalOutput();

	const auto ptg = getAssociatedPTG();
	const size_t nDirs = ni.obstacles.size();

	const double target_dir = ::atan2(target.y, target.x);
	const unsigned int target_k =
		CParameterizedTrajectoryGenerator::Alpha2index(target_dir, nDirs);
	const double target_dist = target.norm();

	m_dirs_scores.resize(nDirs, options.factorWeights.size() + 2);

	// TP-Obstacles in 2D:
	std::vector<mrpt::math::TPoint2D> obstacles_2d(nDirs);

	mrpt::obs::T2DScanProperties sp;
	sp.aperture = 2.0 * M_PI;
	sp.nRays = nDirs;
	sp.rightToLeft = true;
	const auto& sc_lut = m_sincos_lut.getSinCosForScan(sp);

	for (unsigned int i = 0; i < nDirs; i++)
	{
		obstacles_2d[i].x = ni.obstacles[i] * sc_lut.ccos[i];
		obstacles_2d[i].y = ni.obstacles[i] * sc_lut.csin[i];
	}

	// Sanity checks:
	ASSERT_EQUAL_(options.factorWeights.size(), NUM_FACTORS);
	ASSERT_GT_(nDirs, 3);

	for (unsigned int i = 0; i < nDirs; i++)
	{
		double scores[NUM_FACTORS];	 // scores for each criterion

		// Too close to obstacles? (unless target is in between obstacles and
		// the robot)
		if (ni.obstacles[i] < options.TOO_CLOSE_OBSTACLE &&
			!(i == target_k && ni.obstacles[i] > 1.02 * target_dist))
		{
			for (size_t l = 0; l < NUM_FACTORS; l++)
				m_dirs_scores(i, l) = .0;
			continue;
		}

		const double d = std::min(ni.obstacles[i], 0.95 * target_dist);

		// The TP-Space representative coordinates for this direction:
		const double x = d * sc_lut.ccos[i];
		const double y = d * sc_lut.csin[i];

		// Factor [0]: collision-free distance
		// -----------------------------------------------------
		if (mrpt::abs_diff(i, target_k) <= 1 &&
			target_dist < 1.0 - options.TOO_CLOSE_OBSTACLE &&
			ni.obstacles[i] > 1.05 * target_dist)
		{
			// Don't count obstacles ahead of the target.
			scores[0] =
				std::max(target_dist, ni.obstacles[i]) / (target_dist * 1.05);
		}
		else
		{
			scores[0] =
				std::max(0.0, ni.obstacles[i] - options.TOO_CLOSE_OBSTACLE);
		}

		// Discount "circular loop aparent free distance" here, but don't count
		// it for clearance, since those are not real obstacle points.
		if (ptg != nullptr)
		{
			const double max_real_freespace =
				ptg->getActualUnloopedPathLength(i);
			const double max_real_freespace_norm =
				max_real_freespace / ptg->getRefDistance();

			mrpt::keep_min(scores[0], max_real_freespace_norm);
		}

		// Factor [1]: Closest approach to target along straight line
		// (Euclidean)
		// -------------------------------------------
		mrpt::math::TSegment2D sg;
		sg.point1.x = 0;
		sg.point1.y = 0;
		sg.point2.x = x;
		sg.point2.y = y;

		// Range of attainable values: 0=passes thru target. 2=opposite
		// direction
		double min_dist_target_along_path = sg.distance(target);

		// Idea: if this segment is taking us *away* from target, don't make
		// the segment to start at (0,0), since all paths "running away"
		// will then have identical minimum distances to target. Use the
		// middle of the segment instead:
		const double endpt_dist_to_target = (target - TPoint2D(x, y)).norm();
		const double endpt_dist_to_target_norm =
			std::min(1.0, endpt_dist_to_target);

		if ((endpt_dist_to_target_norm > target_dist &&
			 endpt_dist_to_target_norm >= 0.95 * target_dist) &&
			/* the path does not get any closer to trg */
			min_dist_target_along_path >
				1.05 * std::min(target_dist, endpt_dist_to_target_norm))
		{
			// path takes us away or way blocked:
			sg.point1.x = x * 0.5;
			sg.point1.y = y * 0.5;
			min_dist_target_along_path = sg.distance(target);
		}

		scores[1] = 1.0 / (1.0 + square(min_dist_target_along_path));

		// Factor [2]: Distance of end collision-free point to target
		// (Euclidean)
		// Factor [5]: idem (except: no decimation afterwards)
		// -----------------------------------------------------
		scores[2] = std::sqrt(1.01 - endpt_dist_to_target_norm);
		scores[5] = scores[2];
		// the 1.01 instead of 1.0 is to be 100% sure we don't get a domain
		// error in sqrt()

		// Factor [3]: Stabilizing factor (hysteresis) to avoid quick switch
		// among very similar paths:
		// ------------------------------------------------------------------------------------------
		if (m_last_selected_sector != std::numeric_limits<unsigned int>::max())
		{
			// It's fine here to consider that -PI is far from +PI.
			const unsigned int hist_dist =
				mrpt::abs_diff(m_last_selected_sector, i);

			if (hist_dist >= options.HYSTERESIS_SECTOR_COUNT)
				scores[3] = square(
					1.0 -
					(hist_dist - options.HYSTERESIS_SECTOR_COUNT) /
						double(nDirs));
			else
				scores[3] = 1.0;
		}
		else
		{
			scores[3] = 1.0;
		}

		// Factor [4]: clearance to nearest obstacle along path
		// Use TP-obstacles instead of real obstacles in Workspace since
		// it's way faster, despite being an approximation:
		// -------------------------------------------------------------------
		{
			sg.point1.x = 0;
			sg.point1.y = 0;
			sg.point2.x = x;
			sg.point2.y = y;

			double& closest_obs = scores[4];
			closest_obs = 1.0;

			// eval obstacles within a certain region of this "i" direction only
			const int W = std::max(1, round(nDirs * 0.1));
			const int i_min = std::max(0, static_cast<int>(i) - W);
			const int i_max =
				std::min(static_cast<int>(nDirs) - 1, static_cast<int>(i) + W);
			for (int oi = i_min; oi <= i_max; oi++)
			{
				// "no obstacle" (norm_dist=1.0) doesn't count as a real obs:
				if (ni.obstacles[oi] >= 0.99) continue;
				mrpt::keep_min(closest_obs, sg.distance(obstacles_2d[oi]));
			}
		}

		// Factor [6]: Direct distance in "sectors":
		// -------------------------------------------------------------------
		scores[6] = 1.0 /
			(1.0 + mrpt::square((4.0 / nDirs) * mrpt::abs_diff(i, target_k)));

		// If target is not directly reachable for this i-th direction, decimate
		// its scorings:
		if (target_dist < 1.0 - options.TOO_CLOSE_OBSTACLE &&
			ni.obstacles[i] < 1.01 * target_dist)
		{
			// this direction cannot reach target, so assign a low score:
			scores[1] *= 0.1;
			scores[2] *= 0.1;
			scores[6] *= 0.1;
		}

		// Save stats for debugging:
		for (size_t l = 0; l < NUM_FACTORS; l++)
			m_dirs_scores(i, l) = scores[l];

	}  // end for each direction "i"

	// Normalize factors?
	ASSERT_(options.factorNormalizeOrNot.size() == NUM_FACTORS);
	for (size_t l = 0; l < NUM_FACTORS; l++)
	{
		if (!options.factorNormalizeOrNot[l]) continue;

		const double mmax = m_dirs_scores.col(l).maxCoeff();
		const double mmin = m_dirs_scores.col(l).minCoeff();
		const double span = mmax - mmin;
		if (span <= .0) continue;

		m_dirs_scores.col(l).array() -= mmin;
		m_dirs_scores.col(l).array() /= span;
	}

	// Phase 1: average of PHASE1_FACTORS and thresholding:
	// ----------------------------------------------------------------------
	const unsigned int NUM_PHASES = options.PHASE_FACTORS.size();
	ASSERT_(NUM_PHASES >= 1);

	std::vector<double> weights_sum_phase(NUM_PHASES, .0),
		weights_sum_phase_inv(NUM_PHASES);
	for (unsigned int i = 0; i < NUM_PHASES; i++)
	{
		for (unsigned int l : options.PHASE_FACTORS[i])
			weights_sum_phase[i] += options.factorWeights.at(l);
		ASSERT_(weights_sum_phase[i] > .0);
		weights_sum_phase_inv[i] = 1.0 / weights_sum_phase[i];
	}

	eo.phase_scores = std::vector<std::vector<double>>(
		NUM_PHASES, std::vector<double>(nDirs, .0));
	auto& phase_scores = eo.phase_scores;  // shortcut
	double last_phase_threshold = -1.0;	 // don't threshold for the first phase

	for (unsigned int phase_idx = 0; phase_idx < NUM_PHASES; phase_idx++)
	{
		double phase_min = std::numeric_limits<double>::max(), phase_max = .0;

		for (unsigned int i = 0; i < nDirs; i++)
		{
			double this_dir_eval = 0;

			if (ni.obstacles[i] <
					options.TOO_CLOSE_OBSTACLE ||  // Too close to obstacles ?
				(phase_idx > 0 &&
				 phase_scores[phase_idx - 1][i] <
					 last_phase_threshold)	// thresholding of the previous
				// phase
			)
			{
				this_dir_eval = .0;
			}
			else
			{
				// Weighted avrg of factors:
				for (unsigned int l : options.PHASE_FACTORS[phase_idx])
					this_dir_eval += options.factorWeights.at(l) *
						std::log(std::max(1e-6, m_dirs_scores(i, l)));

				this_dir_eval *= weights_sum_phase_inv[phase_idx];
				this_dir_eval = std::exp(this_dir_eval);
			}
			phase_scores[phase_idx][i] = this_dir_eval;

			mrpt::keep_max(phase_max, phase_scores[phase_idx][i]);
			mrpt::keep_min(phase_min, phase_scores[phase_idx][i]);

		}  // for each direction

		ASSERT_(options.PHASE_THRESHOLDS.size() == NUM_PHASES);
		ASSERT_(
			options.PHASE_THRESHOLDS[phase_idx] > .0 &&
			options.PHASE_THRESHOLDS[phase_idx] < 1.0);

		last_phase_threshold = options.PHASE_THRESHOLDS[phase_idx] * phase_max +
			(1.0 - options.PHASE_THRESHOLDS[phase_idx]) * phase_min;
	}  // end for each phase

	// Give a chance for a derived class to manipulate the final evaluations:
	auto& dirs_eval = phase_scores.back();

	postProcessDirectionEvaluations(dirs_eval, ni, target_idx);

	// Recalculate the threshold just in case the postProcess function above
	// changed things:
	{
		double phase_min = std::numeric_limits<double>::max(), phase_max = .0;
		for (unsigned int i = 0; i < nDirs; i++)
		{
			mrpt::keep_max(phase_max, dirs_eval[i]);
			mrpt::keep_min(phase_min, dirs_eval[i]);
		}
		last_phase_threshold = options.PHASE_THRESHOLDS.back() * phase_max +
			(1.0 - options.PHASE_THRESHOLDS.back()) * phase_min;
	}

	// Thresholding:
	for (unsigned int i = 0; i < nDirs; i++)
	{
		double& val = dirs_eval[i];
		if (val < last_phase_threshold) val = .0;
	}
}

void CHolonomicFullEval::navigate(const NavInput& ni, NavOutput& no)
{
	using mrpt::square;

	ASSERT_(ni.clearance != nullptr);
	ASSERT_(!ni.targets.empty());

	// Create a log record for returning data.
	auto log = std::make_shared<CLogFileRecord_FullEval>();
	no.logRecord = log;

	// Evaluate for each target:
	const size_t numTrgs = ni.targets.size();
	std::vector<EvalOutput> evals(numTrgs);
	for (unsigned int trg_idx = 0; trg_idx < numTrgs; trg_idx++)
	{
		evalSingleTarget(trg_idx, ni, evals[trg_idx]);
	}

	ASSERT_(!evals.empty());
	const auto nDirs = evals.front().phase_scores.back().size();
	ASSERT_EQUAL_(nDirs, ni.obstacles.size());

	// Now, sum all weights for the last stage for each target into an "overall"
	// score vector, one score per direction of motion:
	std::vector<double> overall_scores;
	overall_scores.assign(nDirs, .0);
	for (const auto& e : evals)
	{
		for (unsigned int i = 0; i < nDirs; i++)
			overall_scores[i] += e.phase_scores.back()[i];
	}
	// Normalize:
	for (unsigned int i = 0; i < nDirs; i++)
		overall_scores[i] *= (1.0 / numTrgs);

	// Search for best direction in the "overall score" vector:

	// Keep the GAP with the largest maximum value within;
	// then pick the MIDDLE point as the final selection.
	std::vector<TGap> gaps;
	std::size_t best_gap_idx = std::string::npos;
	{
		bool inside_gap = false;
		for (unsigned int i = 0; i < nDirs; i++)
		{
			const double val = overall_scores[i];
			if (val < 0.01)
			{
				// This direction didn't pass the cut threshold for the "last
				// phase":
				if (inside_gap)
				{
					// We just ended a gap:
					auto& active_gap = *gaps.rbegin();
					active_gap.k_to = i - 1;
					inside_gap = false;
				}
			}
			else
			{
				// higher or EQUAL to the treshold (equal is important just in
				// case we have a "flat" diagram...)
				if (!inside_gap)
				{
					// We just started a gap:
					TGap new_gap;
					new_gap.k_from = i;
					gaps.emplace_back(new_gap);
					inside_gap = true;
				}
			}

			if (inside_gap)
			{
				auto& active_gap = *gaps.rbegin();
				if (val >= active_gap.max_eval) { active_gap.k_best_eval = i; }
				mrpt::keep_max(active_gap.max_eval, val);
				mrpt::keep_min(active_gap.min_eval, val);

				if (best_gap_idx == std::string::npos ||
					val > gaps[best_gap_idx].max_eval)
				{
					best_gap_idx = gaps.size() - 1;
				}
			}
		}  // end for i

		// Handle the case where we end with an open, active gap:
		if (inside_gap)
		{
			auto& active_gap = *gaps.rbegin();
			active_gap.k_to = nDirs - 1;
		}
	}

	// Not heading to target: go thru the "middle" of the gap to maximize
	// clearance
	int best_dir_k = -1;
	double best_dir_eval = 0;

	// We may have no gaps if all paths are blocked by obstacles, for example:
	if (!gaps.empty())
	{
		ASSERT_(best_gap_idx < gaps.size());
		const TGap& best_gap = gaps[best_gap_idx];
		best_dir_k = best_gap.k_best_eval;
		best_dir_eval = overall_scores.at(best_dir_k);
	}

	// Prepare NavigationOutput data:
	if (best_dir_eval == .0)
	{
		// No way found!
		no.desiredDirection = 0;
		no.desiredSpeed = 0;
	}
	else
	{
		// A valid movement:
		const auto ptg = getAssociatedPTG();
		const double ptg_ref_dist = ptg ? ptg->getRefDistance() : 1.0;

		no.desiredDirection =
			CParameterizedTrajectoryGenerator::Index2alpha(best_dir_k, nDirs);

		// Speed control: Reduction factors
		// ---------------------------------------------
		const double targetNearnessFactor = m_enableApproachTargetSlowDown
			? std::min(
				  1.0,
				  ni.targets.front().norm() /
					  (options.TARGET_SLOW_APPROACHING_DISTANCE / ptg_ref_dist))
			: 1.0;

		const double obs_dist = ni.obstacles[best_dir_k];
		// Was: min with obs_clearance too.
		const double obs_dist_th = std::max(
			options.TOO_CLOSE_OBSTACLE,
			options.OBSTACLE_SLOW_DOWN_DISTANCE * ni.maxObstacleDist);
		double riskFactor = 1.0;
		if (obs_dist <= options.TOO_CLOSE_OBSTACLE) { riskFactor = 0.0; }
		else if (
			obs_dist < obs_dist_th && obs_dist_th > options.TOO_CLOSE_OBSTACLE)
		{
			riskFactor = (obs_dist - options.TOO_CLOSE_OBSTACLE) /
				(obs_dist_th - options.TOO_CLOSE_OBSTACLE);
		}
		no.desiredSpeed =
			ni.maxRobotSpeed * std::min(riskFactor, targetNearnessFactor);
	}

	m_last_selected_sector = best_dir_k;

	// LOG --------------------------
	if (log)
	{
		log->selectedTarget = 0;  // was: best_trg_idx
		log->selectedSector = best_dir_k;
		log->evaluation = best_dir_eval;
		// Copy the evaluation of first phases for (arbitrarily) the first
		// target, then overwrite the scores of its last phase with the OVERALL
		// phase scores:
		log->dirs_eval = evals.front().phase_scores;
		log->dirs_eval.back() = overall_scores;

		if (options.LOG_SCORE_MATRIX) { log->dirs_scores = m_dirs_scores; }
	}
}

unsigned int CHolonomicFullEval::direction2sector(
	const double a, const unsigned int N)
{
	const int idx = round(0.5 * (N * (1 + mrpt::math::wrapToPi(a) / M_PI) - 1));
	if (idx < 0) return 0;
	else
		return static_cast<unsigned int>(idx);
}

CLogFileRecord_FullEval::CLogFileRecord_FullEval() : dirs_scores() {}
uint8_t CLogFileRecord_FullEval::serializeGetVersion() const { return 3; }
void CLogFileRecord_FullEval::serializeTo(
	mrpt::serialization::CArchive& out) const
{
	out << CHolonomicLogFileRecord::dirs_eval << dirs_scores << selectedSector
		<< evaluation << selectedTarget /*v3*/;
}

/*---------------------------------------------------------------
					readFromStream
  ---------------------------------------------------------------*/
void CLogFileRecord_FullEval::serializeFrom(
	mrpt::serialization::CArchive& in, uint8_t version)
{
	switch (version)
	{
		case 0:
		case 1:
		case 2:
		case 3:
		{
			if (version >= 2) { in >> CHolonomicLogFileRecord::dirs_eval; }
			else
			{
				CHolonomicLogFileRecord::dirs_eval.resize(2);
				in >> CHolonomicLogFileRecord::dirs_eval[0];
				if (version >= 1)
				{
					in >> CHolonomicLogFileRecord::dirs_eval[1];
				}
			}
			in >> dirs_scores >> selectedSector >> evaluation;
			if (version >= 3) { in >> selectedTarget; }
			else
			{
				selectedTarget = 0;
			}
		}
		break;
		default: MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version);
	};
}

/*---------------------------------------------------------------
						TOptions
  ---------------------------------------------------------------*/
CHolonomicFullEval::TOptions::TOptions()
	: factorWeights{0.1, 0.5, 0.5, 0.01, 1, 1, 1},
	  factorNormalizeOrNot{0, 0, 0, 0, 1, 0, 0},
	  PHASE_FACTORS{{1, 2}, {4}, {0, 2}},
	  PHASE_THRESHOLDS{0.5, 0.6, 0.7}

{
}

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

	// Load from config text:
	MRPT_LOAD_CONFIG_VAR(TOO_CLOSE_OBSTACLE, double, c, s);
	MRPT_LOAD_CONFIG_VAR(TARGET_SLOW_APPROACHING_DISTANCE, double, c, s);
	MRPT_LOAD_CONFIG_VAR(OBSTACLE_SLOW_DOWN_DISTANCE, double, c, s);
	MRPT_LOAD_CONFIG_VAR(HYSTERESIS_SECTOR_COUNT, double, c, s);
	MRPT_LOAD_CONFIG_VAR(LOG_SCORE_MATRIX, bool, c, s);
	MRPT_LOAD_CONFIG_VAR(clearance_threshold_ratio, double, c, s);
	MRPT_LOAD_CONFIG_VAR(gap_width_ratio_threshold, double, c, s);

	c.read_vector(
		s, "factorWeights", std::vector<double>(), factorWeights, true);
	ASSERT_EQUAL_(factorWeights.size(), NUM_FACTORS);

	c.read_vector(
		s, "factorNormalizeOrNot", factorNormalizeOrNot, factorNormalizeOrNot);
	ASSERT_EQUAL_(factorNormalizeOrNot.size(), factorWeights.size());

	// Phases:
	int PHASE_COUNT = 0;
	MRPT_LOAD_CONFIG_VAR_NO_DEFAULT(PHASE_COUNT, int, c, s);

	PHASE_FACTORS.resize(PHASE_COUNT);
	PHASE_THRESHOLDS.resize(PHASE_COUNT);
	for (int i = 0; i < PHASE_COUNT; i++)
	{
		c.read_vector(
			s, mrpt::format("PHASE%i_FACTORS", i + 1), PHASE_FACTORS[i],
			PHASE_FACTORS[i], true);
		ASSERT_(!PHASE_FACTORS[i].empty());

		PHASE_THRESHOLDS[i] = c.read_double(
			s, mrpt::format("PHASE%i_THRESHOLD", i + 1), .0, true);
		ASSERT_(PHASE_THRESHOLDS[i] >= .0 && PHASE_THRESHOLDS[i] <= 1.0);
	}

	MRPT_END
}

void CHolonomicFullEval::TOptions::saveToConfigFile(
	mrpt::config::CConfigFileBase& c, const std::string& s) const
{
	MRPT_START

	const int WN = mrpt::config::MRPT_SAVE_NAME_PADDING(),
			  WV = mrpt::config::MRPT_SAVE_VALUE_PADDING();

	MRPT_SAVE_CONFIG_VAR_COMMENT(
		TOO_CLOSE_OBSTACLE,
		"Directions with collision-free distances below this threshold are not "
		"elegible.");
	MRPT_SAVE_CONFIG_VAR_COMMENT(
		TARGET_SLOW_APPROACHING_DISTANCE,
		"Start to reduce speed when closer than this to target.");
	MRPT_SAVE_CONFIG_VAR_COMMENT(
		OBSTACLE_SLOW_DOWN_DISTANCE,
		"Start to reduce speed when clearance is below this value ([0,1] ratio "
		"wrt obstacle reference/max distance)");
	MRPT_SAVE_CONFIG_VAR_COMMENT(
		HYSTERESIS_SECTOR_COUNT,
		"Range of `sectors` (directions) for hysteresis over successive "
		"timesteps");
	MRPT_SAVE_CONFIG_VAR_COMMENT(
		LOG_SCORE_MATRIX, "Save the entire score matrix in log files");
	MRPT_SAVE_CONFIG_VAR_COMMENT(
		clearance_threshold_ratio,
		"Ratio [0,1], times path_count, gives the minimum number of paths at "
		"each side of a target direction to be accepted as desired direction");
	MRPT_SAVE_CONFIG_VAR_COMMENT(
		gap_width_ratio_threshold,
		"Ratio [0,1], times path_count, gives the minimum gap width to accept "
		"a direct motion towards target.");

	ASSERT_EQUAL_(factorWeights.size(), NUM_FACTORS);
	c.write(
		s, "factorWeights",
		mrpt::system::sprintf_container("%.2f ", factorWeights), WN, WV,
		"[0]=Free space, [1]=Dist. in sectors, [2]=Closer to target "
		"(Euclidean), [3]=Hysteresis, [4]=clearance along path, [5]=Like [2] "
		"without decimation if path obstructed");
	c.write(
		s, "factorNormalizeOrNot",
		mrpt::system::sprintf_container("%u ", factorNormalizeOrNot), WN, WV,
		"Normalize factors or not (1/0)");

	c.write(
		s, "PHASE_COUNT", PHASE_FACTORS.size(), WN, WV,
		"Number of evaluation phases to run (params for each phase below)");

	for (unsigned int i = 0; i < PHASE_FACTORS.size(); i++)
	{
		c.write(
			s, mrpt::format("PHASE%u_THRESHOLD", i + 1), PHASE_THRESHOLDS[i],
			WN, WV,
			"Phase scores must be above this relative range threshold [0,1] to "
			"be considered in next phase (Default:`0.75`)");
		c.write(
			s, mrpt::format("PHASE%u_FACTORS", i + 1),
			mrpt::system::sprintf_container("%d ", PHASE_FACTORS[i]), WN, WV,
			"Indices of the factors above to be considered in this phase");
	}

	MRPT_END
}

uint8_t CHolonomicFullEval::serializeGetVersion() const { return 4; }
void CHolonomicFullEval::serializeTo(mrpt::serialization::CArchive& out) const
{
	// Params:
	out << options.factorWeights << options.HYSTERESIS_SECTOR_COUNT
		<< options.PHASE_FACTORS <<	 // v3
		options.TARGET_SLOW_APPROACHING_DISTANCE << options.TOO_CLOSE_OBSTACLE
		<< options.PHASE_THRESHOLDS	 // v3
		<< options.OBSTACLE_SLOW_DOWN_DISTANCE	// v1
		<< options.factorNormalizeOrNot	 // v2
		<< options.clearance_threshold_ratio
		<< options.gap_width_ratio_threshold  // v4:
		;
	// State:
	out << m_last_selected_sector;
}
void CHolonomicFullEval::serializeFrom(
	mrpt::serialization::CArchive& in, uint8_t version)
{
	switch (version)
	{
		case 0:
		case 1:
		case 2:
		case 3:
		case 4:
		{
			// Params:
			in >> options.factorWeights >> options.HYSTERESIS_SECTOR_COUNT;

			if (version >= 3) { in >> options.PHASE_FACTORS; }
			else
			{
				options.PHASE_THRESHOLDS.resize(2);
				in >> options.PHASE_FACTORS[0] >> options.PHASE_FACTORS[1];
			}
			in >> options.TARGET_SLOW_APPROACHING_DISTANCE >>
				options.TOO_CLOSE_OBSTACLE;

			if (version >= 3) { in >> options.PHASE_THRESHOLDS; }
			else
			{
				options.PHASE_THRESHOLDS.resize(1);
				in >> options.PHASE_THRESHOLDS[0];
			}

			if (version >= 1) in >> options.OBSTACLE_SLOW_DOWN_DISTANCE;
			if (version >= 2) in >> options.factorNormalizeOrNot;

			if (version >= 4)
			{
				in >> options.clearance_threshold_ratio >>
					options.gap_width_ratio_threshold;
			}

			// State:
			in >> m_last_selected_sector;
		}
		break;
		default: MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version);
	};
}

void CHolonomicFullEval::postProcessDirectionEvaluations(
	std::vector<double>& dir_evals, const NavInput& ni, unsigned int trg_idx)
{
	// Default: do nothing
}
