/* +------------------------------------------------------------------------+
   |                     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 "apps-precomp.h"  // Precompiled headers
//
#include <mrpt/apps/MonteCarloLocalization_App.h>
#include <mrpt/bayes/CParticleFilter.h>
#include <mrpt/config/CConfigFile.h>
#include <mrpt/gui/CDisplayWindow3D.h>
#include <mrpt/gui/CDisplayWindowPlots.h>
#include <mrpt/io/CFileGZInputStream.h>
#include <mrpt/io/CFileGZOutputStream.h>
#include <mrpt/io/CFileOutputStream.h>
#include <mrpt/io/vector_loadsave.h>
#include <mrpt/maps/CMultiMetricMap.h>
#include <mrpt/maps/COccupancyGridMap2D.h>
#include <mrpt/maps/CSimpleMap.h>
#include <mrpt/maps/CSimplePointsMap.h>
#include <mrpt/math/data_utils.h>
#include <mrpt/math/distributions.h>
#include <mrpt/math/ops_vectors.h>  // << for vector<>
#include <mrpt/math/utils.h>
#include <mrpt/obs/CActionCollection.h>
#include <mrpt/obs/CActionRobotMovement2D.h>
#include <mrpt/obs/CActionRobotMovement3D.h>
#include <mrpt/obs/CObservationOdometry.h>
#include <mrpt/obs/CRawlog.h>
#include <mrpt/opengl/CDisk.h>
#include <mrpt/opengl/CEllipsoid2D.h>
#include <mrpt/opengl/CEllipsoid3D.h>
#include <mrpt/opengl/CGridPlaneXY.h>
#include <mrpt/opengl/CPointCloud.h>
#include <mrpt/opengl/stock_objects.h>
#include <mrpt/poses/CPose2D.h>
#include <mrpt/poses/CPose2DInterpolator.h>
#include <mrpt/random.h>
#include <mrpt/serialization/CArchive.h>
#include <mrpt/slam/CMonteCarloLocalization2D.h>
#include <mrpt/slam/CMonteCarloLocalization3D.h>
#include <mrpt/system/CTicTac.h>
#include <mrpt/system/filesystem.h>
#include <mrpt/system/os.h>

#include <Eigen/Dense>

using namespace mrpt::apps;

// ---------------------------------------
//   MonteCarloLocalization_Base
// ---------------------------------------
MonteCarloLocalization_Base::MonteCarloLocalization_Base()
{
  // Set logger display name:
  this->setLoggerName("MonteCarloLocalization_Base");
}

void MonteCarloLocalization_Base::initialize(int argc, const char** argv)
{
  MRPT_START

  MRPT_LOG_INFO_FMT(
      " pf-localization - Part of the MRPT\n"
      " MRPT C++ Library: %s - Sources timestamp: %s\n\n",
      mrpt::system::MRPT_getVersion().c_str(), mrpt::system::MRPT_getCompilationDate().c_str());

  // Process arguments:
  if (argc < 2)
  {
    THROW_EXCEPTION_FMT("Usage: %s", impl_get_usage().c_str());
  }

  // Config file:
  const std::string configFile = std::string(argv[1]);

  ASSERT_FILE_EXISTS_(configFile);
  params.setContent(mrpt::io::file_get_contents(configFile));

  impl_initialize(argc, argv);

  MRPT_END
}

// All these "using"s, to easily reuse old code from the pf-localization app:
using namespace mrpt;
using namespace mrpt::slam;
using namespace mrpt::maps;
using namespace mrpt::opengl;
using namespace mrpt::gui;
using namespace mrpt::math;
using namespace mrpt::system;
using namespace mrpt::random;
using namespace mrpt::poses;
using namespace mrpt::bayes;
using namespace mrpt::obs;
using namespace mrpt::io;
using namespace mrpt::img;
using namespace mrpt::config;
using namespace mrpt::serialization;
using namespace std;

// Auxiliary trait classes for template implementations below:
template <class PDF>
struct pf2gauss_t
{
};

template <>
struct pf2gauss_t<mrpt::slam::CMonteCarloLocalization2D>
{
  using type = mrpt::poses::CPosePDFGaussian;
  static constexpr bool PF_IS_3D = false;

  static void resetOnFreeSpace(
      CMonteCarloLocalization2D& pdf,
      mrpt::maps::CMultiMetricMap& metricMap,
      size_t PARTICLE_COUNT,
      const mrpt::math::TPose3D& init_min,
      const mrpt::math::TPose3D& init_max)
  {
    pdf.resetUniformFreeSpace(
        metricMap.mapByClass<COccupancyGridMap2D>().get(), 0.7f, PARTICLE_COUNT, init_min.x,
        init_max.x, init_min.y, init_max.y, init_min.yaw, init_max.yaw);
  }

  static void resetUniform(
      CMonteCarloLocalization2D& pdf,
      size_t PARTICLE_COUNT,
      const mrpt::math::TPose3D& init_min,
      const mrpt::math::TPose3D& init_max)
  {
    pdf.resetUniform(
        init_min.x, init_max.x, init_min.y, init_max.y, init_min.yaw, init_max.yaw, PARTICLE_COUNT);
  }
};
template <>
struct pf2gauss_t<CMonteCarloLocalization3D>
{
  using type = CPose3DPDFGaussian;
  static constexpr bool PF_IS_3D = true;

  static void resetOnFreeSpace(
      [[maybe_unused]] CMonteCarloLocalization3D& pdf,
      [[maybe_unused]] mrpt::maps::CMultiMetricMap& metricMap,
      [[maybe_unused]] size_t PARTICLE_COUNT,
      [[maybe_unused]] const mrpt::math::TPose3D& init_min,
      [[maybe_unused]] const mrpt::math::TPose3D& init_max)
  {
    THROW_EXCEPTION("init_PDF_mode=0 not supported for 3D particles");
  }

  static void resetUniform(
      CMonteCarloLocalization3D& pdf,
      size_t PARTICLE_COUNT,
      const mrpt::math::TPose3D& init_min,
      const mrpt::math::TPose3D& init_max)
  {
    pdf.resetUniform(init_min, init_max, PARTICLE_COUNT);
  }
};

template <class MONTECARLO_TYPE>
void MonteCarloLocalization_Base::do_pf_localization()
{
  auto& cfg = this->params;  // rename for reusing old code

  // Number of initial particles (if size>1, run the experiments N times)
  std::vector<int> particles_count;

  // Load parameters:

  // Mandatory entries:
  cfg.read_vector(
      sect, "particles_count", std::vector<int>(1, 0), particles_count,
      /*Fail if not found*/ true);
  string OUT_DIR_PREFIX = cfg.read_string(sect, "logOutput_dir", "", /*Fail if not found*/ true);

  // Non-mandatory entries:
  string MAP_FILE = cfg.read_string(sect, "map_file", "");
  size_t rawlog_offset = cfg.read_int(sect, "rawlog_offset", 0);
  string GT_FILE = cfg.read_string(sect, "ground_truth_path_file", "");
  const auto NUM_REPS = cfg.read_uint64_t(sect, "experimentRepetitions", 1);
  int SCENE3D_FREQ = cfg.read_int(sect, "3DSceneFrequency", 10);
  bool SCENE3D_FOLLOW = cfg.read_bool(sect, "3DSceneFollowRobot", true);
  unsigned int testConvergenceAt = cfg.read_int(sect, "experimentTestConvergenceAtStep", -1);

  bool SAVE_STATS_ONLY = cfg.read_bool(sect, "SAVE_STATS_ONLY", false);
  bool DO_RELIABILITY_ESTIMATE = false;
  bool DO_SCAN_LIKELIHOOD_DEBUG = false;
  MRPT_LOAD_CONFIG_VAR(DO_RELIABILITY_ESTIMATE, bool, cfg, sect);
  MRPT_LOAD_CONFIG_VAR(DO_SCAN_LIKELIHOOD_DEBUG, bool, cfg, sect);

  bool SHOW_PROGRESS_3D_REAL_TIME = cfg.read_bool(sect, "SHOW_PROGRESS_3D_REAL_TIME", false);
  int SHOW_PROGRESS_3D_REAL_TIME_DELAY_MS =
      cfg.read_int(sect, "SHOW_PROGRESS_3D_REAL_TIME_DELAY_MS", 1);
  double STATS_CONF_INTERVAL = cfg.read_double(sect, "STATS_CONF_INTERVAL", 0.2);

  CPose2D initial_odo;
  initial_odo.x(cfg.read_double(sect, "initial_odo_x", 0));
  initial_odo.y(cfg.read_double(sect, "initial_odo_y", 0));
  initial_odo.phi(cfg.read_double(sect, "initial_odo_phi", 0));

#if !MRPT_HAS_WXWIDGETS
  SHOW_PROGRESS_3D_REAL_TIME = false;
#endif

  // Default odometry uncertainty parameters in "actOdom2D_params" depending
  // on how fast the robot moves, etc...
  //  Only used for observations-only rawlogs:
  CActionRobotMovement2D::TMotionModelOptions actOdom2D_params;
  actOdom2D_params.modelSelection = CActionRobotMovement2D::mmGaussian;
  actOdom2D_params.gaussianModel.minStdXY =
      cfg.read_double("DummyOdometryParams", "minStdXY", 0.04);
  actOdom2D_params.gaussianModel.minStdPHI =
      DEG2RAD(cfg.read_double("DummyOdometryParams", "minStdPHI", 2.0));

  CActionRobotMovement3D::TMotionModelOptions actOdom3D_params;
  actOdom3D_params.mm6DOFModel.additional_std_XYZ =
      cfg.read_double("DummyOdometryParams", "additional_std_XYZ", 0.01);
  actOdom3D_params.mm6DOFModel.additional_std_angle =
      DEG2RAD(cfg.read_double("DummyOdometryParams", "additional_std_angle", 0.1));

  // PF-algorithm Options:
  // ---------------------------
  CParticleFilter::TParticleFilterOptions pfOptions;
  pfOptions.loadFromConfigFile(cfg, "PF_options");

  // PDF Options:
  // ------------------
  TMonteCarloLocalizationParams pdfPredictionOptions;
  pdfPredictionOptions.KLD_params.loadFromConfigFile(cfg, "KLD_options");

  // Metric map options:
  // -----------------------------
  TSetOfMetricMapInitializers mapList;
  mapList.loadFromConfigFile(cfg, "MetricMap");

  MRPT_LOG_INFO_STREAM(
      "" << endl
         << "MAP_FILE       : " << MAP_FILE << endl
         << "GT_FILE        : " << GT_FILE << endl
         << "OUT_DIR_PREFIX : " << OUT_DIR_PREFIX << endl
         << "particles_count :" << particles_count << endl);

  // --------------------------------------------------------------------
  //						EXPERIMENT PREPARATION
  // --------------------------------------------------------------------
  CTicTac tictac;
  CSimpleMap simpleMap;
  CParticleFilter::TParticleFilterStats PF_stats;

  // Load the set of metric maps to consider in the experiments:
  auto metricMap = CMultiMetricMap::Create();
  metricMap->setListOfMaps(mapList);

  {
    std::stringstream ss;
    pfOptions.dumpToTextStream(ss);
    mapList.dumpToTextStream(ss);

    MRPT_LOG_INFO_STREAM(ss.str());
  }

  getRandomGenerator().randomize();

  // Load the map (if any):
  // -------------------------
  if (MAP_FILE.size())
  {
    ASSERT_(fileExists(MAP_FILE));

    // Detect file extension:
    // -----------------------------
    string mapExt =
        lowerCase(extractFileExtension(MAP_FILE, true));  // Ignore possible .gz extensions

    if (!mapExt.compare("simplemap"))
    {
      // It's a ".simplemap":
      // -------------------------
      MRPT_LOG_INFO_STREAM("Loading '.simplemap' file: " << MAP_FILE);
      {
        CFileGZInputStream f(MAP_FILE);
        archiveFrom(f) >> simpleMap;
      }
      MRPT_LOG_INFO_STREAM("Map loaded ok, with " << simpleMap.size() << " frames.");

      ASSERT_(simpleMap.size() > 0);

      // Build metric map:
      // ------------------------------
      MRPT_LOG_INFO("Building metric map(s) from '.simplemap'...");
      metricMap->loadFromSimpleMap(simpleMap);
      MRPT_LOG_INFO("Done.");
    }
    else if (!mapExt.compare("gridmap"))
    {
      // It's a ".gridmap":
      // -------------------------
      MRPT_LOG_INFO("Loading gridmap from '.gridmap'...");
      auto grid = metricMap->mapByClass<COccupancyGridMap2D>();
      ASSERT_(grid);
      {
        CFileGZInputStream f(MAP_FILE);
        archiveFrom(f) >> (*grid);
      }
      MRPT_LOG_INFO("Done.");
    }
    else
    {
      THROW_EXCEPTION_FMT("Map file has unknown extension: '%s'", mapExt.c_str());
    }
  }

  // Load the Ground Truth:
  CMatrixDouble GT(0, 0);
  if (fileExists(GT_FILE))
  {
    MRPT_LOG_INFO("Loading ground truth file...");
    GT.loadFromTextFile(GT_FILE);
    MRPT_LOG_INFO("Done.");

    prepareGT(GT);
  }
  else
    MRPT_LOG_INFO("Ground truth file: not available.");

  // PDF initialization uniform distribution limits:
  const auto init_min = mrpt::math::TPose3D(
      cfg.read_double(sect, "init_PDF_min_x", 0), cfg.read_double(sect, "init_PDF_min_y", 0),
      cfg.read_double(sect, "init_PDF_min_z", 0),
      mrpt::DEG2RAD(cfg.read_double(sect, "init_PDF_min_yaw_deg", -180.0)),
      mrpt::DEG2RAD(cfg.read_double(sect, "init_PDF_min_pitch_deg", 0)),
      mrpt::DEG2RAD(cfg.read_double(sect, "init_PDF_min_roll_deg", 0)));

  const auto init_max = mrpt::math::TPose3D(
      cfg.read_double(sect, "init_PDF_max_x", 0), cfg.read_double(sect, "init_PDF_max_y", 0),
      cfg.read_double(sect, "init_PDF_max_z", 0),
      mrpt::DEG2RAD(cfg.read_double(sect, "init_PDF_max_yaw_deg", +180.0)),
      mrpt::DEG2RAD(cfg.read_double(sect, "init_PDF_max_pitch_deg", 0)),
      mrpt::DEG2RAD(cfg.read_double(sect, "init_PDF_max_roll_deg", 0)));

  MRPT_LOG_INFO_STREAM(
      "Initial PDF limits:\n Min=" << init_min.asString() << "\n Max=" << init_max.asString());

  // Gridmap / area of initial uncertainty:
  COccupancyGridMap2D::TEntropyInfo gridInfo;
  if (auto grid = metricMap->mapByClass<COccupancyGridMap2D>(); grid)
  {
    grid->computeEntropy(gridInfo);
    MRPT_LOG_INFO_FMT(
        "The gridmap has %.04fm2 observed area, %u observed cells.", gridInfo.effectiveMappedArea,
        (unsigned)gridInfo.effectiveMappedCells);
  }
  else
  {
    gridInfo.effectiveMappedArea = (init_max.x - init_min.x) * (init_max.y - init_min.y);
  }

  for (int PARTICLE_COUNT : particles_count)
  {
    MRPT_LOG_INFO_FMT(
        "Initial PDF: %f particles/m2", PARTICLE_COUNT / gridInfo.effectiveMappedArea);

    // Global stats for all the experiment loops:
    int nConvergenceTests = 0, nConvergenceOK = 0;
    CVectorDouble convergenceErrors;
    std::mutex convergenceErrors_mtx;

    // --------------------------------------------------------------------
    //					EXPERIMENT REPETITIONS LOOP
    // --------------------------------------------------------------------

    // Generate list of repetition indices:
    std::vector<unsigned int> rep_indices(NUM_REPS);
    std::iota(rep_indices.begin(), rep_indices.end(), 0);

    // Run each repetition in parallel:
    // GCC 7 still has not implemented C++17 for_each(std::execution::par
    // So: home-made parallel loops:

    // std::for_each(std::execution::par, rep_indices.begin(),
    // rep_indices.end(),
    auto run_localization_code = [&](const size_t repetition)
    {
      CVectorDouble indivConvergenceErrors, executionTimes, odoError;
      MRPT_LOG_INFO_STREAM(
          "====== RUNNING FOR " << PARTICLE_COUNT << " INITIAL PARTICLES  - Repetition "
                                << 1 + repetition << " / " << NUM_REPS);

      // Create 3D window if requested:
      CDisplayWindow3D::Ptr win3D;
      if (SHOW_PROGRESS_3D_REAL_TIME)
      {
        win3D = std::make_shared<CDisplayWindow3D>("pf-localization - The MRPT project", 1000, 600);
        win3D->setCameraAzimuthDeg(-45);
      }

      // Create the 3D scene and get the map only once, later we'll modify
      // only the particles, etc..
      Scene scene;
      if (SCENE3D_FREQ > 0 || SHOW_PROGRESS_3D_REAL_TIME)
      {
        mrpt::math::TBoundingBoxf bbox({-50, -50, 0}, {50, 50, 0});
        if (auto pts = metricMap->getAsSimplePointsMap(); pts) bbox = pts->boundingBox();

        scene.insert(mrpt::opengl::CGridPlaneXY::Create(
            bbox.min.x, bbox.max.x, bbox.min.y, bbox.max.y, 0, 5));

        if (win3D)
          win3D->setCameraZoom(2 * std::max(bbox.max.x - bbox.min.x, bbox.max.y - bbox.min.y));

        scene.insert(metricMap->getVisualization());
      }

      // The experiment directory is:
      string sOUT_DIR_PARTS, sOUT_DIR_3D;
      const auto sOUT_DIR = format(
          "%s_%03u_%07i", OUT_DIR_PREFIX.c_str(), static_cast<unsigned int>(repetition),
          PARTICLE_COUNT);
      MRPT_LOG_INFO_FMT("Creating directory: %s", sOUT_DIR.c_str());
      deleteFilesInDirectory(sOUT_DIR);
      createDirectory(sOUT_DIR);
      ASSERT_DIRECTORY_EXISTS_(sOUT_DIR);

      if (!SAVE_STATS_ONLY)
      {
        sOUT_DIR_PARTS = format("%s/particles", sOUT_DIR.c_str());
        sOUT_DIR_3D = format("%s/3D", sOUT_DIR.c_str());

        MRPT_LOG_INFO_FMT("Creating directory: %s", sOUT_DIR_PARTS.c_str());
        deleteFilesInDirectory(sOUT_DIR_PARTS);
        createDirectory(sOUT_DIR_PARTS);
        ASSERT_DIRECTORY_EXISTS_(sOUT_DIR_PARTS);

        MRPT_LOG_INFO_FMT("Creating directory: %s", sOUT_DIR_3D.c_str());
        deleteFilesInDirectory(sOUT_DIR_3D);
        createDirectory(sOUT_DIR_3D);
        ASSERT_DIRECTORY_EXISTS_(sOUT_DIR_3D);

        using namespace std::string_literals;
        metricMap->saveMetricMapRepresentationToFile(sOUT_DIR + "/map"s);
      }

      MONTECARLO_TYPE pdf;

      // PDF Options:
      pdf.options = pdfPredictionOptions;

      pdf.options.metricMap = metricMap;

      // Create the PF object:
      CParticleFilter PF;
      PF.m_options = pfOptions;

      size_t step = 0;
      size_t rawlogEntry = 0;

      // Initialize the PDF:
      // -----------------------------
      tictac.Tic();
      if (!cfg.read_bool(
              sect, "init_PDF_mode", false,
              /*Fail if not found*/ true))
      {
        // Reset uniform on free space:
        pf2gauss_t<MONTECARLO_TYPE>::resetOnFreeSpace(
            pdf, *metricMap, PARTICLE_COUNT, init_min, init_max);
      }
      else
      {
        // Reset uniform:
        pf2gauss_t<MONTECARLO_TYPE>::resetUniform(pdf, PARTICLE_COUNT, init_min, init_max);
      }

      MRPT_LOG_INFO_FMT(
          "PDF of %u particles initialized in %.03fms", PARTICLE_COUNT, 1000 * tictac.Tac());

      pdf.saveToTextFile(format("%s/particles_0_initial.txt", sOUT_DIR_PARTS.c_str()));

      // -----------------------------
      //		Particle filter
      // -----------------------------
      CPose2D odometryEstimation = initial_odo;

      using PDF_MEAN_TYPE = typename MONTECARLO_TYPE::type_value;

      PDF_MEAN_TYPE pdfEstimation;

      bool end = false;

      CFileOutputStream f_cov_est, f_pf_stats, f_odo_est;

      if (!SAVE_STATS_ONLY)
      {
        f_cov_est.open(sOUT_DIR.c_str() + string("/cov_est.txt"));
        f_pf_stats.open(sOUT_DIR.c_str() + string("/PF_stats.txt"));
        f_odo_est.open(sOUT_DIR.c_str() + string("/odo_est.txt"));
      }

      Clock::time_point cur_obs_timestamp;
      CPose2D last_used_abs_odo(0, 0, 0), pending_most_recent_odo(0, 0, 0);

      for (; !end; step++)
      {
        // Finish if ESC is pushed:
        if (allow_quit_on_esc_key && os::kbhit())
          if (os::getch() == 27)
          {
            end = true;
            break;
          }

        // Load pose change from the rawlog:
        // ----------------------------------------
        CActionCollection::Ptr action;
        CSensoryFrame::Ptr observations;
        CObservation::Ptr obs;

        if (!impl_get_next_observations(action, observations, obs))
        {
          // EOF
          end = true;
          break;
        }

        // Determine if we are reading a Act-SF or an Obs-only
        // rawlog:
        if (obs)
        {
          // It's an observation-only rawlog: build an auxiliary
          // pair of action-SF, since
          //  montecarlo-localization only accepts those pairs as
          //  input:

          // If it's an odometry reading, don't feed it to the PF.
          // Instead,
          // store its value for use as an "action" together with
          // the next actual observation:
          if (IS_CLASS(*obs, CObservationOdometry))
          {
            auto obs_odo = std::dynamic_pointer_cast<CObservationOdometry>(obs);
            pending_most_recent_odo = obs_odo->odometry;
            static bool is_1st_odo = true;
            if (is_1st_odo)
            {
              is_1st_odo = false;
              last_used_abs_odo = pending_most_recent_odo;
            }
            continue;
          }
          else
          {
            // SF: Just one observation:
            // ------------------------------------------------------
            observations = std::make_shared<CSensoryFrame>();
            observations->insert(obs);

            // ActionCollection: Just one action with a dummy
            // odometry
            // ------------------------------------------------------
            action = std::make_shared<CActionCollection>();

            if (pf2gauss_t<MONTECARLO_TYPE>::PF_IS_3D)
            {
              CActionRobotMovement3D actOdom3D;

              const CPose3D odo_incr = CPose3D(pending_most_recent_odo - last_used_abs_odo);
              last_used_abs_odo = pending_most_recent_odo;

              actOdom3D.computeFromOdometry(odo_incr, actOdom3D_params);
              action->insert(actOdom3D);
            }
            else
            {
              CActionRobotMovement2D actOdom2D;

              const CPose2D odo_incr = pending_most_recent_odo - last_used_abs_odo;
              last_used_abs_odo = pending_most_recent_odo;

              actOdom2D.computeFromOdometry(odo_incr, actOdom2D_params);
              action->insert(actOdom2D);
            }
          }
        }
        else
        {
          // Already in Act-SF format, nothing else to do!
        }

        CPose2D expectedPose;  // Ground truth

        if (observations->size() > 0)
          cur_obs_timestamp = observations->getObservationByIndex(0)->timestamp;
        else if (obs)
          cur_obs_timestamp = obs->timestamp;

        // Test for end condition if we are testing convergence:
        if (step == testConvergenceAt)
        {
          nConvergenceTests++;

          // Convergence??
          if (sqrt(pdf.getCovariance().det()) < 2)
          {
            if (pdfEstimation.distanceTo(expectedPose) < 2) nConvergenceOK++;
          }
          end = true;
        }

        if (step < rawlog_offset) continue;

        // Do not execute the PF at "step=0", to let the initial
        // PDF to be reflected in the logs.
        if (step > rawlog_offset)
        {
          // Show 3D?
          if (SHOW_PROGRESS_3D_REAL_TIME)
          {
            const auto [cov, meanPose] = pdf.getCovarianceAndMean();

            if (rawlogEntry >= 2)
              getGroundTruth(expectedPose, rawlogEntry - 2, GT, cur_obs_timestamp);

            // The particles' cov:
            {
              CRenderizable::Ptr ellip = scene.getByName("parts_cov");
              if (!ellip)
              {
                if (pf2gauss_t<MONTECARLO_TYPE>::PF_IS_3D)
                {
                  auto el = CEllipsoid3D::Create();
                  ellip = mrpt::ptr_cast<CRenderizable>::from(el);
                  el->setLineWidth(2);
                  el->setQuantiles(3);
                  el->enableDrawSolid3D(false);
                }
                else
                {
                  auto el = CEllipsoid2D::Create();
                  ellip = mrpt::ptr_cast<CRenderizable>::from(el);
                  el->setLineWidth(2);
                  el->setQuantiles(3);
                  el->enableDrawSolid3D(false);
                }
                ellip->setName("parts_cov");
                ellip->setColor(1, 0, 0, 0.6);
                scene.insert(ellip);
              }
              else
              {
                if (pf2gauss_t<MONTECARLO_TYPE>::PF_IS_3D)
                {
                  mrpt::ptr_cast<CEllipsoid3D>::from(ellip)->setCovMatrix(
                      cov.template blockCopy<3, 3>());
                }
                else
                {
                  mrpt::ptr_cast<CEllipsoid2D>::from(ellip)->setCovMatrix(
                      cov.template blockCopy<2, 2>());
                }
              }
              double ellipse_z = mrpt::poses::CPose3D(meanPose).z() + 0.01;

              ellip->setLocation(meanPose.x(), meanPose.y(), ellipse_z);
            }

            Scene::Ptr ptrSceneWin = win3D->get3DSceneAndLock();

            win3D->setCameraPointingToPoint(meanPose.x(), meanPose.y(), 0);

            win3D->addTextMessage(
                10, 10,
                mrpt::format(
                    "timestamp: %s",
                    mrpt::system::dateTimeLocalToString(cur_obs_timestamp).c_str()),
                6001);

            win3D->addTextMessage(
                10, 33, mrpt::format("#particles= %7u", static_cast<unsigned int>(pdf.size())),
                6002);

            win3D->addTextMessage(
                10, 55, mrpt::format("mean pose (x y phi_deg)= %s", meanPose.asString().c_str()),
                6003);

            *ptrSceneWin = scene;
            win3D->unlockAccess3DScene();

            // Update:
            win3D->forceRepaint();

            std::this_thread::sleep_for(
                std::chrono::milliseconds(SHOW_PROGRESS_3D_REAL_TIME_DELAY_MS));
          }  // end show 3D real-time

          // ----------------------------------------
          // RUN ONE STEP OF THE PARTICLE FILTER:
          // ----------------------------------------
          tictac.Tic();
          if (!SAVE_STATS_ONLY)
          {
            MRPT_LOG_INFO_STREAM(
                "Step " << step << ". Executing ParticleFilter on " << pdf.particlesCount()
                        << " particles...");
          }

          PF.executeOn(
              pdf,
              action.get(),        // Action
              observations.get(),  // Obs.
              &PF_stats            // Output statistics
          );

          double run_time = tictac.Tac();
          executionTimes.push_back(run_time);
          if (!SAVE_STATS_ONLY)
          {
            MRPT_LOG_INFO_FMT("Done in %.03fms, ESS=%f", 1e3 * run_time, pdf.ESS());
          }
        }

        // Avrg. error:
        // ----------------------------------------
        CActionRobotMovement2D::Ptr best_mov_estim = action->getBestMovementEstimation();
        if (best_mov_estim)
        {
          odometryEstimation = odometryEstimation + best_mov_estim->poseChange->getMeanVal();
        }

        pdf.getMean(pdfEstimation);

        // save estimated mean in history:
        if (cur_obs_timestamp != INVALID_TIMESTAMP && fill_out_estimated_path)
        {
          out_estimated_path.insert(
              cur_obs_timestamp, mrpt::math::TPose3D(pdfEstimation.asTPose()));
        }

        getGroundTruth(expectedPose, rawlogEntry, GT, cur_obs_timestamp);

        if (expectedPose.x() != 0 || expectedPose.y() != 0 || expectedPose.phi() != 0)
        {  // Averaged error to GT
          double sumW = 0;
          double locErr = 0;
          for (size_t k = 0; k < pdf.size(); k++) sumW += exp(pdf.getW(k));
          for (size_t k = 0; k < pdf.size(); k++)
          {
            const auto pk = pdf.getParticlePose(k);
            locErr += mrpt::hypot_fast(expectedPose.x() - pk.x, expectedPose.y() - pk.y) *
                      exp(pdf.getW(k)) / sumW;
          }
          convergenceErrors_mtx.lock();
          convergenceErrors.push_back(locErr);
          convergenceErrors_mtx.unlock();

          indivConvergenceErrors.push_back(locErr);
          odoError.push_back(expectedPose.distanceTo(odometryEstimation));
        }

        const auto [cov, meanPose] = pdf.getCovarianceAndMean();
        const auto current_pdf_gaussian = typename pf2gauss_t<MONTECARLO_TYPE>::type(meanPose, cov);

        // Text output:
        // ----------------------------------------
        if (!SAVE_STATS_ONLY)
        {
          MRPT_LOG_INFO_STREAM("Odometry estimation: " << odometryEstimation);
          MRPT_LOG_INFO_STREAM(
              "PDF estimation: " << pdfEstimation << ", ESS (B.R.)= " << PF_stats.ESS_beforeResample
                                 << " tr(cov): " << std::sqrt(current_pdf_gaussian.cov.trace()));

          if (GT.rows() > 0) MRPT_LOG_INFO_STREAM("Ground truth: " << expectedPose);
        }

        // Evaluate the "reliability" of the pose estimation
        // (for now, only for 2D laser scans + grid maps)
        double obs_reliability_estim = .0;
        if (DO_RELIABILITY_ESTIMATE)
        {
          // We need: a gridmap & a 2D LIDAR:
          CObservation2DRangeScan::Ptr obs_scan;
          if (observations)
            obs_scan =
                observations->getObservationByClass<CObservation2DRangeScan>(0);  // Get the 0'th
          // scan, if
          // several are
          // present.
          COccupancyGridMap2D::Ptr gridmap = metricMap->mapByClass<COccupancyGridMap2D>();
          if (obs_scan && gridmap)  // We have both, go on:
          {
            // Simulate scan + uncertainty:
            COccupancyGridMap2D::TLaserSimulUncertaintyParams ssu_params;
            COccupancyGridMap2D::TLaserSimulUncertaintyResult ssu_out;
            ssu_params.method = COccupancyGridMap2D::sumUnscented;
            // ssu_params.UT_alpha = 0.99;
            // obs_scan->stdError = 0.07;
            // obs_scan->maxRange = 10.0;

            ssu_params.robotPose = CPosePDFGaussian(current_pdf_gaussian);
            ssu_params.aperture = obs_scan->aperture;
            ssu_params.rangeNoiseStd = obs_scan->stdError;
            ssu_params.nRays = obs_scan->getScanSize();
            ssu_params.rightToLeft = obs_scan->rightToLeft;
            ssu_params.sensorPose = obs_scan->sensorPose;
            ssu_params.maxRange = obs_scan->maxRange;

            gridmap->laserScanSimulatorWithUncertainty(ssu_params, ssu_out);

            // Evaluate reliability:
            CObservation2DRangeScanWithUncertainty::TEvalParams evalParams;
            // evalParams.prob_outliers = 0.40;
            // evalParams.max_prediction_std_dev = 1.0;
            obs_reliability_estim =
                ssu_out.scanWithUncert.evaluateScanLikelihood(*obs_scan, evalParams);

            if (DO_SCAN_LIKELIHOOD_DEBUG)
            {
              static mrpt::gui::CDisplayWindowPlots win;

              std::vector<float> ranges_mean, ranges_obs;
              for (size_t i = 0; i < obs_scan->getScanSize(); i++)
              {
                ranges_mean.push_back(ssu_out.scanWithUncert.rangeScan.getScanRange(i));
                ranges_obs.push_back(obs_scan->getScanRange(i));
              }

              win.plot(ranges_mean, "3k-", "mean");
              win.plot(ranges_obs, "r-", "obs");

              Eigen::VectorXd ci1 = ssu_out.scanWithUncert.rangesMean.asEigen() +
                                    3 * ssu_out.scanWithUncert.rangesCovar.asEigen()
                                            .diagonal()
                                            .array()
                                            .sqrt()
                                            .matrix();
              Eigen::VectorXd ci2 = ssu_out.scanWithUncert.rangesMean.asEigen() -
                                    3 * ssu_out.scanWithUncert.rangesCovar.asEigen()
                                            .diagonal()
                                            .array()
                                            .sqrt()
                                            .matrix();
              win.plot(ci1, "k-", "CI+");
              win.plot(ci2, "k-", "CI-");

              win.setWindowTitle(mrpt::format("obs_reliability_estim: %f", obs_reliability_estim));
              win.axis_fit();
            }
          }
          MRPT_LOG_INFO_STREAM("Reliability measure [0-1]: " << obs_reliability_estim);
        }

        if (!SAVE_STATS_ONLY)
        {
          f_cov_est.printf("%e\n", sqrt(cov.det()));
          f_pf_stats.printf(
              "%u %e %e %f %f\n", (unsigned int)pdf.size(), PF_stats.ESS_beforeResample,
              PF_stats.weightsVariance_beforeResample, obs_reliability_estim,
              sqrt(current_pdf_gaussian.cov.det()));
          f_odo_est.printf(
              "%f %f %f\n", odometryEstimation.x(), odometryEstimation.y(),
              odometryEstimation.phi());
        }

        if ((!SAVE_STATS_ONLY && SCENE3D_FREQ > 0) || SHOW_PROGRESS_3D_REAL_TIME)
        {
          // Generate 3D scene:
          // ------------------------------

          // The Ground Truth (GT):
          if (GT.rows() > 0)
          {
            CRenderizable::Ptr GTpt = scene.getByName("GT");
            if (!GTpt)
            {
              GTpt = std::make_shared<CDisk>();
              GTpt = std::make_shared<CDisk>();
              GTpt->setName("GT");
              GTpt->setColor(0, 0, 0, 0.9);

              mrpt::ptr_cast<CDisk>::from(GTpt)->setDiskRadius(0.04f);
              scene.insert(GTpt);
            }

            GTpt->setPose(expectedPose);
          }

          // The particles:
          {
            CRenderizable::Ptr parts = scene.getByName("particles");
            if (parts) scene.removeObject(parts);

            auto p = pdf.template getAs3DObject<CSetOfObjects::Ptr>();
            p->setName("particles");
            scene.insert(p);
          }

          // The laser scan:
          {
            CRenderizable::Ptr scanPts = scene.getByName("scan");
            if (!scanPts)
            {
              scanPts = std::make_shared<CPointCloud>();
              scanPts->setName("scan");
              scanPts->setColor(1, 0, 0, 0.9);
              mrpt::ptr_cast<CPointCloud>::from(scanPts)->enableColorFromZ(false);
              mrpt::ptr_cast<CPointCloud>::from(scanPts)->setPointSize(4);
              scene.insert(scanPts);
            }

            CSimplePointsMap map;

            CPose3D robotPose3D(meanPose);

            map.clear();
            observations->insertObservationsInto(map);

            mrpt::ptr_cast<CPointCloud>::from(scanPts)->loadFromPointsMap(&map);
            mrpt::ptr_cast<CPointCloud>::from(scanPts)->setPose(robotPose3D);
          }

          // The camera:
          scene.enableFollowCamera(SCENE3D_FOLLOW);

          // Views:
          Viewport::Ptr view1 = scene.getViewport("main");
          {
            CCamera& cam = view1->getCamera();
            cam.setAzimuthDegrees(-90);
            cam.setElevationDegrees(90);
            cam.setPointingAt(meanPose);
            cam.setZoomDistance(5);
            cam.setOrthogonal();
          }
        }

        if (!SAVE_STATS_ONLY && SCENE3D_FREQ != -1 && ((step + 1) % SCENE3D_FREQ) == 0)
        {
          // Save 3D scene:
          CFileGZOutputStream f(
              format("%s/progress_%05u.3Dscene", sOUT_DIR_3D.c_str(), (unsigned)step));
          archiveFrom(f) << scene;

          // Generate text files for matlab:
          // ------------------------------------
          pdf.saveToTextFile(
              format("%s/particles_%05u.txt", sOUT_DIR_PARTS.c_str(), (unsigned)step));
        }

      };  // while rawlogEntries

      indivConvergenceErrors.saveToTextFile(sOUT_DIR + "/GT_error.txt");
      odoError.saveToTextFile(sOUT_DIR + "/ODO_error.txt");
      executionTimes.saveToTextFile(sOUT_DIR + "/exec_times.txt");

      if (win3D && NUM_REPS == 1) mrpt::system::pause();
    };  // for repetitions

    CTicTac tictacGlobal;
    tictacGlobal.Tic();

    const auto max_num_threads = std::thread::hardware_concurrency();
    size_t runs_per_thread = NUM_REPS;
    if (max_num_threads > 1)
      runs_per_thread =
          static_cast<size_t>(std::ceil((NUM_REPS) / static_cast<double>(max_num_threads)));

    MRPT_LOG_INFO_STREAM(
        "Running " << NUM_REPS << " repetitions, on max " << max_num_threads
                   << " parallel threads: " << runs_per_thread << " runs/thread.");

    std::vector<std::thread> running_tasks;
    for (size_t r = 0; r < NUM_REPS; r += runs_per_thread)
    {
      auto runner = [&](size_t i_start, size_t i_end)
      {
        if (i_end > NUM_REPS) i_end = NUM_REPS;  // sanity check
        for (size_t i = i_start; i < i_end; i++) run_localization_code(i);
      };

      running_tasks.emplace_back(runner, r, r + runs_per_thread);
      std::this_thread::sleep_for(std::chrono::milliseconds(100));
    }

    // Wait for all threads to end:
    for (auto& t : running_tasks)
      if (t.joinable()) t.join();

    double repetitionTime = tictacGlobal.Tac();

    // Avr. error:
    double covergenceErrorMean = 0, convergenceErrorsMin = 0, convergenceErrorsMax = 0;
    if (!convergenceErrors.empty())
      math::confidenceIntervals(
          convergenceErrors, covergenceErrorMean, convergenceErrorsMin, convergenceErrorsMax,
          STATS_CONF_INTERVAL);

    // Save overall results:
    {
      CFileOutputStream f(format("%s_SUMMARY.txt", OUT_DIR_PREFIX.c_str()), OpenMode::APPEND);

      f.printf(
          "%% Ratio_covergence_success  #particles  "
          "average_time_per_execution  convergence_mean_error "
          "convergence_error_conf_int_inf "
          "convergence_error_conf_int_sup "
          "\n");
      if (!nConvergenceTests) nConvergenceTests = 1;
      f.printf(
          "%f %u %f %f %f %f\n", ((double)nConvergenceOK) / nConvergenceTests, PARTICLE_COUNT,
          repetitionTime / NUM_REPS, covergenceErrorMean, convergenceErrorsMin,
          convergenceErrorsMax);
    }

    MRPT_LOG_INFO_FMT("Total execution time: %.06f sec", repetitionTime);

  }  // end of loop for different # of particles
}

void MonteCarloLocalization_Base::getGroundTruth(
    mrpt::poses::CPose2D& expectedPose,
    size_t rawlogEntry,
    const mrpt::math::CMatrixDouble& GT,
    const Clock::time_point& cur_time)
{
  // Either:
  // - time x y phi
  // or
  // - time x y z yaw pitch roll

  if (GT.cols() == 4 || GT.cols() == 7)
  {
    bool GT_index_is_time;

    // First column can be: timestamps, or rawlogentries:
    //  Auto-figure it out:
    if (GT.rows() > 2)
    {
      GT_index_is_time = floor(GT(0, 0)) != GT(0, 0) && floor(GT(1, 0)) != GT(1, 0);
    }
    else
    {
      GT_index_is_time = false;
    }

    if (GT_index_is_time)
    {
      // Look for the timestamp:
      using namespace std::chrono_literals;

      bool interp_ok = false;
      GT_path.interpolate(cur_time, expectedPose, interp_ok);
      if (!interp_ok)
      {
        /*
        cerr << format(
          "GT time not found: %f\n",
          mrpt::Clock::toDouble(cur_time));
          */
      }
    }
    else
    {
      // Look for the rawlogEntry:
      size_t k, N = GT.rows();
      for (k = 0; k < N; k++)
      {
        if (GT(k, 0) == rawlogEntry) break;
      }

      if (k < N)
      {
        expectedPose.x(GT(k, 1));
        expectedPose.y(GT(k, 2));
        expectedPose.phi(GT(k, 3));
      }
    }
  }
  else if (GT.cols() == 3)
  {
    if ((int)rawlogEntry < GT.rows())
    {
      expectedPose.x(GT(rawlogEntry, 0));
      expectedPose.y(GT(rawlogEntry, 1));
      expectedPose.phi(GT(rawlogEntry, 2));
    }
  }
  else if (GT.cols() > 0)
    THROW_EXCEPTION("Unexpected number of columns in ground truth file");
}

void MonteCarloLocalization_Base::prepareGT(const mrpt::math::CMatrixDouble& GT)
{
  // Either:
  // - time x y phi
  // or
  // - time x y z yaw pitch roll
  if (GT.cols() == 4 || GT.cols() == 7)
  {
    const bool GT_is_3D = (GT.cols() == 7);
    bool GT_index_is_time = false;

    // First column can be: timestamps, or rawlogentries:
    //  Auto-figure it out:
    if (GT.rows() > 2)
    {
      GT_index_is_time = floor(GT(0, 0)) != GT(0, 0) && floor(GT(1, 0)) != GT(1, 0);
    }

    if (GT_index_is_time)
    {
      // Look for the timestamp:
      using namespace std::chrono_literals;
      GT_path.setMaxTimeInterpolation(200ms);

      for (int i = 0; i < GT.rows(); i++)
      {
        GT_path.insert(
            mrpt::Clock::fromDouble(GT(i, 0)),
            TPose2D(GT(i, 1), GT(i, 2), GT(i, GT_is_3D ? 4 : 3)));
      }
    }
  }
}

void MonteCarloLocalization_Base::run()
{
  MRPT_START

  // Detect 2D vs 3D particle filter?
  const bool is_3D = params.read_bool(sect, "use_3D_poses", false);

  if (is_3D)
  {
    MRPT_LOG_INFO("Running for: CMonteCarloLocalization3D");
    do_pf_localization<CMonteCarloLocalization3D>();
  }
  else
  {
    MRPT_LOG_INFO("Running for: CMonteCarloLocalization2D");
    do_pf_localization<CMonteCarloLocalization2D>();
  }

  MRPT_END
}

// ---------------------------------------
//   MonteCarloLocalization_Rawlog
// ---------------------------------------
MonteCarloLocalization_Rawlog::MonteCarloLocalization_Rawlog()
{
  setLoggerName("MonteCarloLocalization_Rawlog");
}

void MonteCarloLocalization_Rawlog::impl_initialize(int argc, const char** argv)
{
  MRPT_START
  // Rawlog file: from args. line or from config file:
  if (argc == 3)
    m_rawlogFileName = std::string(argv[2]);
  else
    m_rawlogFileName = params.read_string(sect, "rawlog_file", std::string("log.rawlog"), true);

  m_rawlog_offset = params.read_int(sect, "rawlog_offset", 0);

  ASSERT_FILE_EXISTS_(m_rawlogFileName);

  MRPT_END
}
