/* +------------------------------------------------------------------------+
   |                     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/CHolonomicND.h>
#include <mrpt/serialization/CArchive.h>

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

IMPLEMENTS_SERIALIZABLE(CLogFileRecord_ND, CHolonomicLogFileRecord, mrpt::nav)
IMPLEMENTS_SERIALIZABLE(CHolonomicND, CAbstractHolonomicReactiveMethod, mrpt::nav)

/**  Initialize the parameters of the navigator, from some
 *    configuration file, or default values if filename is set to NULL.
 */
CHolonomicND::CHolonomicND(const mrpt::config::CConfigFileBase* INI_FILE) :
    CAbstractHolonomicReactiveMethod("CHolonomicND"),
    m_last_selected_sector(std::numeric_limits<unsigned int>::max())
{
  if (INI_FILE != nullptr) initialize(*INI_FILE);
}

void CHolonomicND::initialize(const mrpt::config::CConfigFileBase& INI_FILE)
{
  options.loadFromConfigFile(INI_FILE, getConfigFileSectionName());
}
void CHolonomicND::saveConfigFile(mrpt::config::CConfigFileBase& c) const
{
  options.saveToConfigFile(c, getConfigFileSectionName());
}

/*---------------------------------------------------------------
            Navigate
  ---------------------------------------------------------------*/
void CHolonomicND::navigate(const NavInput& ni, NavOutput& no)
{
  const auto ptg = getAssociatedPTG();
  const double ptg_ref_dist = ptg ? ptg->getRefDistance() : 1.0;

  TGapArray gaps;
  TSituations situation;
  unsigned int selectedSector;
  double riskEvaluation;
  double evaluation;

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

  // Search gaps:
  gaps.clear();
  ASSERT_(!ni.targets.empty());
  const auto trg = *ni.targets.rbegin();

  gapsEstimator(ni.obstacles, trg, gaps);

  // Select best gap:
  searchBestGap(
      ni.obstacles, 1.0 /* max obs range*/, gaps, trg, selectedSector, evaluation, situation,
      riskEvaluation, *log);

  if (situation == SITUATION_NO_WAY_FOUND)
  {
    // No way found!
    no.desiredDirection = 0;
    no.desiredSpeed = 0;
  }
  else
  {
    // A valid movement:
    no.desiredDirection =
        CParameterizedTrajectoryGenerator::Index2alpha(selectedSector, ni.obstacles.size());

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

    const double riskFactor = std::min(1.0, riskEvaluation / options.RISK_EVALUATION_DISTANCE);
    no.desiredSpeed = ni.maxRobotSpeed * std::min(riskFactor, targetNearnessFactor);
  }

  m_last_selected_sector = selectedSector;

  // LOG --------------------------
  if (log)
  {
    // gaps:
    {
      int i, n = gaps.size();
      log->gaps_ini.resize(n);
      log->gaps_end.resize(n);
      for (i = 0; i < n; i++)
      {
        log->gaps_ini[i] = gaps[i].ini;
        log->gaps_end[i] = gaps[i].end;
      }
    }
    // Selection:
    log->selectedSector = selectedSector;
    log->evaluation = evaluation;
    log->situation = situation;
    log->riskEvaluation = riskEvaluation;
  }
}

/*---------------------------------------------------------------
        Find gaps in the obtacles (Beta version)
  ---------------------------------------------------------------*/
void CHolonomicND::gapsEstimator(
    const std::vector<double>& obstacles, const mrpt::math::TPoint2D& target, TGapArray& gaps_out)
{
  const size_t n = obstacles.size();
  ASSERT_(n > 2);

  // ================ Parameters ================
  const int GAPS_MIN_WIDTH = ceil(n * 0.01);  // was: 3
  const double GAPS_MIN_DEPTH_CONSIDERED = 0.6;
  const double GAPS_MAX_RELATIVE_DEPTH = 0.5;
  // ============================================

  // Find the maximum distances to obstacles:
  // ----------------------------------------------------------
  double overall_max_dist = std::numeric_limits<float>::min(),
         overall_min_dist = std::numeric_limits<float>::max();
  for (size_t i = 1; i < (n - 1); i++)
  {
    mrpt::keep_max(overall_max_dist, obstacles[i]);
    mrpt::keep_min(overall_min_dist, obstacles[i]);
  }
  double max_depth = overall_max_dist - overall_min_dist;

  //  Build list of "GAPS":
  // --------------------------------------------------------
  TGapArray gaps_temp;
  gaps_temp.reserve(150);

  for (double threshold_ratio = 0.95; threshold_ratio >= 0.05; threshold_ratio -= 0.05)
  {
    const double dist_threshold =
        threshold_ratio * overall_max_dist +
        (1.0f - threshold_ratio) * min(target.norm(), GAPS_MIN_DEPTH_CONSIDERED);

    bool is_inside = false;
    size_t sec_ini = 0, sec_end = 0;
    double maxDist = 0.;

    for (size_t i = 0; i < n; i++)
    {
      if (!is_inside && (obstacles[i] >= dist_threshold))  // A gap begins
      {
        sec_ini = i;
        maxDist = obstacles[i];
        is_inside = true;
      }
      else if (is_inside && (i == (n - 1) || obstacles[i] < dist_threshold))  // A gap ends
      {
        if (obstacles[i] < dist_threshold)
          sec_end = i - 1;
        else
          sec_end = i;

        is_inside = false;

        if ((sec_end - sec_ini) >= (size_t)GAPS_MIN_WIDTH)
        {
          // Add new gap:
          gaps_temp.resize(gaps_temp.size() + 1);
          TGap& newGap = *gaps_temp.rbegin();

          newGap.ini = sec_ini;
          newGap.end = sec_end;
          newGap.minDistance = min(obstacles[sec_ini], obstacles[sec_end]);
          newGap.maxDistance = maxDist;
        }
      }

      if (is_inside) maxDist = std::max(maxDist, obstacles[i]);
    }
  }

  // Start to filter the gap list
  //--------------------------------------------------------------

  const size_t nTempGaps = gaps_temp.size();

  std::vector<bool> delete_gaps;
  delete_gaps.assign(nTempGaps, false);

  // First, remove redundant gaps
  for (size_t i = 0; i < nTempGaps; i++)
  {
    if (delete_gaps[i] == 1) continue;

    for (size_t j = i + 1; j < nTempGaps; j++)
    {
      if (gaps_temp[i].ini == gaps_temp[j].ini || gaps_temp[i].end == gaps_temp[j].end)
        delete_gaps[j] = true;
    }
  }

  // Remove gaps with a big depth
  for (size_t i = 0; i < nTempGaps; i++)
  {
    if (delete_gaps[i] == 1) continue;

    if ((gaps_temp[i].maxDistance - gaps_temp[i].minDistance) > max_depth * GAPS_MAX_RELATIVE_DEPTH)
      delete_gaps[i] = true;
  }

  // Delete gaps which contain more than one other gaps
  for (size_t i = 0; i < nTempGaps; i++)
  {
    if (delete_gaps[i]) continue;

    unsigned int inner_gap_count = 0;

    for (unsigned int j = 0; j < nTempGaps; j++)
    {
      if (i == j || delete_gaps[j]) continue;

      // j is inside of i?
      if (gaps_temp[j].ini >= gaps_temp[i].ini && gaps_temp[j].end <= gaps_temp[i].end)
        if (++inner_gap_count > 1)
        {
          delete_gaps[i] = true;
          break;
        }
    }
  }

  // Delete gaps included in other gaps
  for (size_t i = 0; i < nTempGaps; i++)
  {
    if (delete_gaps[i]) continue;

    for (unsigned int j = 0; j < nTempGaps; j++)
    {
      if (i == j || delete_gaps[j]) continue;
      if (gaps_temp[i].ini <= gaps_temp[j].ini && gaps_temp[i].end >= gaps_temp[j].end)
        delete_gaps[j] = true;
    }
  }

  // Copy as result only those gaps not marked for deletion:
  // --------------------------------------------------------
  gaps_out.clear();
  gaps_out.reserve(nTempGaps / 2);
  for (size_t i = 0; i < nTempGaps; i++)
  {
    if (delete_gaps[i]) continue;

    // Compute the representative direction ("sector") for this gap:
    calcRepresentativeSectorForGap(gaps_temp[i], target, obstacles);

    gaps_out.push_back(gaps_temp[i]);
  }
}

/*---------------------------------------------------------------
            Search the best gap.
  ---------------------------------------------------------------*/
void CHolonomicND::searchBestGap(
    const std::vector<double>& obstacles,
    const double maxObsRange,
    const TGapArray& in_gaps,
    const mrpt::math::TPoint2D& target,
    unsigned int& out_selDirection,
    double& out_selEvaluation,
    TSituations& out_situation,
    double& out_riskEvaluation,
    CLogFileRecord_ND& log)
{
  // For evaluating the "risk":
  unsigned int min_risk_eval_sector = 0;
  unsigned int max_risk_eval_sector = obstacles.size() - 1;
  const unsigned int target_sector = direction2sector(atan2(target.y, target.x), obstacles.size());
  const double target_dist = std::max(0.01, target.norm());
  // (Risk is evaluated at the end, for all the situations)

  // D1 : Straight path?
  // --------------------------------------------------------
  const int freeSectorsNearTarget = ceil(0.02 * obstacles.size());
  bool theyAreFree = true, caseD1 = false;
  if (target_sector > static_cast<unsigned int>(freeSectorsNearTarget) &&
      target_sector < static_cast<unsigned int>(obstacles.size() - freeSectorsNearTarget))
  {
    const double min_free_dist = std::min(1.05 * target_dist, 0.95 * maxObsRange);
    for (int j = -freeSectorsNearTarget; theyAreFree && j <= freeSectorsNearTarget; j++)
      if (obstacles[(int(target_sector) + j) % obstacles.size()] < min_free_dist)
        theyAreFree = false;
    caseD1 = theyAreFree;
  }

  if (caseD1)
  {
    // S1: Move straight towards target:
    out_selDirection = target_sector;

    // In case of several paths, the shortest:
    out_selEvaluation = 1.0 + std::max(0.0, (maxObsRange - target_dist) / maxObsRange);
    out_situation = SITUATION_TARGET_DIRECTLY;
  }
  else
  {
    // Evaluate all gaps (if any):
    std::vector<double> gaps_evaluation;
    int selected_gap = -1;
    double selected_gap_eval = -100;

    evaluateGaps(obstacles, maxObsRange, in_gaps, target_sector, target_dist, gaps_evaluation);

    log.gaps_eval = gaps_evaluation;

    // D2: is there any gap "beyond" the target (and not too far away)?
    // (Not used)
    // ----------------------------------------------------------------

    // unsigned int dist;
    // for ( unsigned int i=0;i<in_gaps.size();i++ )
    //{
    //	dist = mrpt::abs_diff(target_sector,
    // in_gaps[i].representative_sector );
    //	if (dist > 0.5*obstacles.size())
    //		dist = obstacles.size() - dist;
    //
    //	if ( in_gaps[i].maxDistance >= target_dist && dist <=
    //(int)floor(options.MAX_SECTOR_DIST_FOR_D2_PERCENT * obstacles.size())
    //)
    //
    //		if ( gaps_evaluation[i]>selected_gap_eval )
    //		{
    //			selected_gap_eval = gaps_evaluation[i];
    //			selected_gap = i;
    //		}
    //}

    // Keep the best gaps (if none was picked up to this point)
    if (selected_gap == -1)
      for (unsigned int i = 0; i < in_gaps.size(); i++)
        if (gaps_evaluation[i] > selected_gap_eval)
        {
          selected_gap_eval = gaps_evaluation[i];
          selected_gap = i;
        }

    //  D3: Wasn't a good enough gap (or there were none)?
    // ----------------------------------------------------------
    if (selected_gap_eval <= 0)
    {
      // S2: No way found
      // ------------------------------------------------------
      out_selDirection = 0;
      out_selEvaluation = 0.0;  // Worst case
      out_situation = SITUATION_NO_WAY_FOUND;
    }
    else
    {
      // The selected gap:
      const TGap& gap = in_gaps[selected_gap];

      const unsigned int sectors_to_be_wide =
          round(options.WIDE_GAP_SIZE_PERCENT * obstacles.size());

      out_selDirection = in_gaps[selected_gap].representative_sector;
      out_selEvaluation = selected_gap_eval;

      // D4: Is it a WIDE gap?
      // -----------------------------------------------------
      if ((gap.end - gap.ini) < sectors_to_be_wide)
      {
        // S3: Narrow gap
        // -------------------------------------------
        out_situation = SITUATION_SMALL_GAP;
      }
      else
      {
        // S4: Wide gap
        // -------------------------------------------
        out_situation = SITUATION_WIDE_GAP;
      }

      // Evaluate the risk only within the gap:
      min_risk_eval_sector = gap.ini;
      max_risk_eval_sector = gap.end;
    }
  }

  // Evaluate short-term minimum distance to obstacles, in a small interval
  // around the selected direction:
  const unsigned int risk_eval_nsectors =
      round(options.RISK_EVALUATION_SECTORS_PERCENT * obstacles.size());
  const unsigned int sec_ini = std::max(
      min_risk_eval_sector,
      risk_eval_nsectors < out_selDirection ? out_selDirection - risk_eval_nsectors : 0);
  const unsigned int sec_fin =
      std::min(max_risk_eval_sector, out_selDirection + risk_eval_nsectors);

  out_riskEvaluation = 0.0;
  for (unsigned int i = sec_ini; i <= sec_fin; i++) out_riskEvaluation += obstacles[i];
  out_riskEvaluation /= (sec_fin - sec_ini + 1);
}

/*---------------------------------------------------------------
  Fills in the representative sector
    field in the gap structure:
  ---------------------------------------------------------------*/
void CHolonomicND::calcRepresentativeSectorForGap(
    TGap& gap, const mrpt::math::TPoint2D& target, const std::vector<double>& obstacles)
{
  int sector;
  const unsigned int sectors_to_be_wide = round(options.WIDE_GAP_SIZE_PERCENT * obstacles.size());
  const unsigned int target_sector = direction2sector(atan2(target.y, target.x), obstacles.size());

  if ((gap.end - gap.ini) < sectors_to_be_wide)  // Select the intermediate sector
  {
#if 1
    sector = round(0.5f * gap.ini + 0.5f * gap.end);
#else
    float min_dist_obs_near_ini = 1, min_dist_obs_near_end = 1;
    int i;
    for (i = gap.ini; i >= max(0, gap.ini - 2); i--)
      min_dist_obs_near_ini = min(min_dist_obs_near_ini, obstacles[i]);
    for (i = gap.end; i <= min((int)obstacles.size() - 1, gap.end + 2); i++)
      min_dist_obs_near_end = min(min_dist_obs_near_end, obstacles[i]);
    sector = round(
        (min_dist_obs_near_ini * gap.ini + min_dist_obs_near_end * gap.end) /
        (min_dist_obs_near_ini + min_dist_obs_near_end));
#endif
  }
  else  // Select a sector close to the target but spaced
  // "sectors_to_be_wide/2" from it
  {
    unsigned int dist_ini = mrpt::abs_diff(target_sector, gap.ini);
    unsigned int dist_end = mrpt::abs_diff(target_sector, gap.end);

    if (dist_ini > 0.5 * obstacles.size()) dist_ini = obstacles.size() - dist_ini;
    if (dist_end > 0.5 * obstacles.size()) dist_end = obstacles.size() - dist_end;

    int dir;
    if (dist_ini < dist_end)
    {
      sector = gap.ini;
      dir = +1;
    }
    else
    {
      sector = gap.end;
      dir = -1;
    }

    sector = sector + dir * static_cast<int>(sectors_to_be_wide) / 2;
  }

  keep_max(sector, 0);
  keep_min(sector, static_cast<int>(obstacles.size()) - 1);

  gap.representative_sector = sector;
}

/*---------------------------------------------------------------
            Evaluate each gap
  ---------------------------------------------------------------*/
void CHolonomicND::evaluateGaps(
    const std::vector<double>& obstacles,
    const double maxObsRange,
    const TGapArray& gaps,
    const unsigned int target_sector,
    const float target_dist,
    std::vector<double>& out_gaps_evaluation)
{
  out_gaps_evaluation.resize(gaps.size());

  const double targetAng =
      CParameterizedTrajectoryGenerator::Index2alpha(target_sector, obstacles.size());
  const double target_x = target_dist * cos(targetAng);
  const double target_y = target_dist * sin(targetAng);

  for (unsigned int i = 0; i < gaps.size(); i++)
  {
    // Short cut:
    const TGap* gap = &gaps[i];

    const float d = min3(obstacles[gap->representative_sector], maxObsRange, 0.95 * target_dist);

    // The TP-Space representative coordinates for this gap:
    const double phi = CParameterizedTrajectoryGenerator::Index2alpha(
        gap->representative_sector, obstacles.size());
    const double x = d * cos(phi);
    const double y = d * sin(phi);

    // Factor #1: Maximum reachable distance with this PTG:
    // -----------------------------------------------------
    // It computes the average free distance of the gap:
    float meanDist = 0.f;
    for (unsigned int j = gap->ini; j <= gap->end; j++) meanDist += obstacles[j];
    meanDist /= (gap->end - gap->ini + 1);

    double factor1;
    if (mrpt::abs_diff(gap->representative_sector, target_sector) <= 1 && target_dist < 1)
      factor1 = std::min(target_dist, meanDist) / target_dist;
    else
      factor1 = meanDist;

    // Factor #2: Distance to target in "sectors"
    // -------------------------------------------
    unsigned int dif = mrpt::abs_diff(target_sector, gap->representative_sector);

    // Handle the -PI,PI circular topology:
    if (dif > 0.5 * obstacles.size()) dif = obstacles.size() - dif;

    const double factor2 = exp(-square(dif / (obstacles.size() * 0.25)));

    // Factor #3: Punish paths that take us far away wrt the target:  **** I
    // don't understand it *********
    // -----------------------------------------------------
    double closestX, closestY;
    double dist_eucl = math::minimumDistanceFromPointToSegment(
        target_x, target_y,  // Point
        0, 0, x, y,          // Segment
        closestX, closestY   // Out
    );

    const float factor3 = (maxObsRange - std::min(maxObsRange, dist_eucl)) / maxObsRange;

    // Factor #4: Stabilizing factor (hysteresis) to avoid quick switch
    // among very similar paths:
    // ------------------------------------------------------------------------------------------
    double factor_AntiCab;

    if (m_last_selected_sector != std::numeric_limits<unsigned int>::max())
    {
      unsigned int dist = mrpt::abs_diff(m_last_selected_sector, gap->representative_sector);

      if (dist > unsigned(0.1 * obstacles.size()))
        factor_AntiCab = 0.0;
      else
        factor_AntiCab = 1.0;
    }
    else
    {
      factor_AntiCab = 0;
    }

    ASSERT_(options.factorWeights.size() == 4);

    if (obstacles[gap->representative_sector] <
        options.TOO_CLOSE_OBSTACLE)  // Too close to obstacles
      out_gaps_evaluation[i] = 0;
    else
      out_gaps_evaluation[i] =
          (options.factorWeights[0] * factor1 + options.factorWeights[1] * factor2 +
           options.factorWeights[2] * factor3 + options.factorWeights[3] * factor_AntiCab) /
          (math::sum(options.factorWeights));
  }  // for each gap
}

unsigned int CHolonomicND::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);
}

uint8_t CLogFileRecord_ND::serializeGetVersion() const { return 1; }
void CLogFileRecord_ND::serializeTo(mrpt::serialization::CArchive& out) const
{
  out << gaps_ini << gaps_end << gaps_eval;
  out << selectedSector << evaluation << riskEvaluation << (uint32_t)situation;
}

void CLogFileRecord_ND::serializeFrom(mrpt::serialization::CArchive& in, uint8_t version)
{
  switch (version)
  {
    case 0:
    {
      int32_t n;

      in >> n;
      gaps_ini.resize(n);
      gaps_end.resize(n);
      in.ReadBuffer(&(*gaps_ini.begin()), sizeof(gaps_ini[0]) * n);
      in.ReadBuffer(&(*gaps_end.begin()), sizeof(gaps_end[0]) * n);

      in >> n;
      gaps_eval.resize(n);
      in.ReadBuffer(&(*gaps_eval.begin()), sizeof(gaps_eval[0]) * n);

      in >> selectedSector >> evaluation >> riskEvaluation >> n;

      situation = (CHolonomicND::TSituations)n;
    }
    break;
    case 1:
    {
      uint32_t n;
      in >> gaps_ini >> gaps_end >> gaps_eval;
      in >> selectedSector >> evaluation >> riskEvaluation >> n;
      situation = (CHolonomicND::TSituations)n;
    }
    break;
    default:
      MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version);
  };
}

/*---------------------------------------------------------------
            TOptions
  ---------------------------------------------------------------*/
CHolonomicND::TOptions::TOptions() : factorWeights{1.0, 0.5, 2.0, 0.4} {}
void CHolonomicND::TOptions::loadFromConfigFile(
    const mrpt::config::CConfigFileBase& source, const std::string& section)
{
  MRPT_START

  // Load from config text:
  MRPT_LOAD_CONFIG_VAR(WIDE_GAP_SIZE_PERCENT, double, source, section);
  MRPT_LOAD_CONFIG_VAR(MAX_SECTOR_DIST_FOR_D2_PERCENT, double, source, section);
  MRPT_LOAD_CONFIG_VAR(RISK_EVALUATION_SECTORS_PERCENT, double, source, section);
  MRPT_LOAD_CONFIG_VAR(RISK_EVALUATION_DISTANCE, double, source, section);
  MRPT_LOAD_CONFIG_VAR(TOO_CLOSE_OBSTACLE, double, source, section);
  MRPT_LOAD_CONFIG_VAR(TARGET_SLOW_APPROACHING_DISTANCE, double, source, section);

  source.read_vector(section, "factorWeights", std::vector<double>(), factorWeights, true);
  ASSERT_(factorWeights.size() == 4);

  MRPT_END
}

void CHolonomicND::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(WIDE_GAP_SIZE_PERCENT, "");
  MRPT_SAVE_CONFIG_VAR_COMMENT(MAX_SECTOR_DIST_FOR_D2_PERCENT, "");
  MRPT_SAVE_CONFIG_VAR_COMMENT(RISK_EVALUATION_SECTORS_PERCENT, "");
  MRPT_SAVE_CONFIG_VAR_COMMENT(RISK_EVALUATION_DISTANCE, "In normalized ps-meters [0,1]");
  MRPT_SAVE_CONFIG_VAR_COMMENT(TOO_CLOSE_OBSTACLE, "For stopping gradually");
  MRPT_SAVE_CONFIG_VAR_COMMENT(TARGET_SLOW_APPROACHING_DISTANCE, "In normalized ps-meters");

  ASSERT_EQUAL_(factorWeights.size(), 4);
  c.write(
      s, "factorWeights",
      mrpt::format(
          "%.2f %.2f %.2f %.2f", factorWeights[0], factorWeights[1], factorWeights[2],
          factorWeights[3]),
      WN, WV,
      "[0]=Free space, [1]=Dist. in sectors, [2]=Closer to target "
      "(Euclidean), [3]=Hysteresis");

  MRPT_END
}

uint8_t CHolonomicND::serializeGetVersion() const { return 0; }
void CHolonomicND::serializeTo(mrpt::serialization::CArchive& out) const
{
  // Params:
  out << options.factorWeights << options.MAX_SECTOR_DIST_FOR_D2_PERCENT
      << options.RISK_EVALUATION_DISTANCE << options.RISK_EVALUATION_SECTORS_PERCENT
      << options.TARGET_SLOW_APPROACHING_DISTANCE << options.TOO_CLOSE_OBSTACLE
      << options.WIDE_GAP_SIZE_PERCENT;
  // State:
  out << m_last_selected_sector;
}
void CHolonomicND::serializeFrom(mrpt::serialization::CArchive& in, uint8_t version)
{
  switch (version)
  {
    case 0:
    {
      // Params:
      in >> options.factorWeights >> options.MAX_SECTOR_DIST_FOR_D2_PERCENT >>
          options.RISK_EVALUATION_DISTANCE >> options.RISK_EVALUATION_SECTORS_PERCENT >>
          options.TARGET_SLOW_APPROACHING_DISTANCE >> options.TOO_CLOSE_OBSTACLE >>
          options.WIDE_GAP_SIZE_PERCENT;
      // State:
      in >> m_last_selected_sector;
    }
    break;
    default:
      MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version);
  };
}
