/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */

#include "nav-precomp.h"  // Precomp header
//
#include <mrpt/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>
#include <iostream>

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)

constexpr unsigned NUM_FACTORS = 8U;

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++)
  {
    std::array<double, NUM_FACTORS> scores;  // 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.translation());

    // 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.translation());
    }

    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;
    }

    // Factor [7]: Heading mismatch: 1.0=perfect phi aligment, 0.0=180deg error
    // -------------------------------------------------------------------------
    uint32_t ptgStep = 0;
    ptg->getPathStepForDist(i, d * ptg->getRefDistance(), ptgStep);
    const auto ptgPose = ptg->getPathPose(i, ptgStep);
    scores[7] = 1.0 - std::abs(mrpt::math::angDistance(target.phi, ptgPose.phi) / M_PI);

    // 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
}
