/* +------------------------------------------------------------------------+
   |                     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 "CPosePDFParticlesExtended.h"

#include <mrpt/math/distributions.h>
#include <mrpt/math/utils.h>
#include <mrpt/math/wrap2pi.h>
#include <mrpt/obs/CActionCollection.h>
#include <mrpt/obs/CActionRobotMovement2D.h>
#include <mrpt/obs/CObservationBeaconRanges.h>
#include <mrpt/poses/CPosePDFGaussian.h>
#include <mrpt/poses/CPoseRandomSampler.h>
#include <mrpt/random.h>
#include <mrpt/system/CTicTac.h>
#include <mrpt/system/os.h>

using namespace mrpt;
using namespace mrpt::obs;
using namespace mrpt::maps;
using namespace mrpt::math;
using namespace mrpt::system;
using namespace mrpt::bayes;
using namespace mrpt::random;
using namespace mrpt::poses;
using namespace std;

double likelihood_acumulation = 0;

IMPLEMENTS_SERIALIZABLE(CPosePDFParticlesExtended, CPosePDF, mrpt::poses)

/*---------------------------------------------------------------
  Constructor
  ---------------------------------------------------------------*/
CPosePDFParticlesExtended::CPosePDFParticlesExtended(size_t M)
{
  m_particles.resize(M);

  for (auto& m_particle : m_particles) m_particle.d.reset(new TExtendedCPose2D());

  static TExtendedCPose2D nullPose;
  resetDeterministic(nullPose);

  likelihood_acumulation = 0;
}

/*---------------------------------------------------------------
            operator =
  ---------------------------------------------------------------*/
void CPosePDFParticlesExtended::copyFrom(const CPosePDF& o)
{
  MRPT_START

  CParticleList::iterator itSrc, itDest;

  if (this == &o) return;  // It may be used sometimes

  if (o.GetRuntimeClass() == CLASS_ID(CPosePDFParticlesExtended))
  {
    auto* pdf = (CPosePDFParticlesExtended*)&o;

    // Both are particles:
    m_particles = pdf->m_particles;
  }
  else if (o.GetRuntimeClass() == CLASS_ID(CPosePDFGaussian))
  {
    auto* pdf = (CPosePDFGaussian*)&o;
    int M = (int)m_particles.size();
    std::vector<vector<double>> parts;
    std::vector<vector<double>>::iterator partsIt;

    getRandomGenerator().drawGaussianMultivariateMany(parts, M, pdf->cov);

    m_particles.clear();
    m_particles.resize(M);

    for (itDest = m_particles.begin(), partsIt = parts.begin(); itDest != m_particles.end();
         itDest++, partsIt++)
    {
      itDest->log_w = 0;
      itDest->d.reset(new TExtendedCPose2D());
      itDest->d->pose.x(pdf->mean.x() + (*partsIt)[0]);
      itDest->d->pose.x(pdf->mean.y() + (*partsIt)[1]);
      itDest->d->pose.phi(pdf->mean.phi() + (*partsIt)[2]);

      itDest->d->pose.normalizePhi();
    }
  }

  MRPT_END
}

/*---------------------------------------------------------------
  Destructor
  ---------------------------------------------------------------*/
CPosePDFParticlesExtended::~CPosePDFParticlesExtended() { clear(); }
/*---------------------------------------------------------------
      clear
  ---------------------------------------------------------------*/
void CPosePDFParticlesExtended::clear() { m_particles.clear(); }
/*---------------------------------------------------------------
            getEstimatedPose
  Returns an estimate of the pose, i.e. a "mean value", computed
    as a weighted average over all m_particles.
 ---------------------------------------------------------------*/
void CPosePDFParticlesExtended::getMean(CPose2D& est) const
{
  est = CPose2D(0, 0, 0);

  CPose2D p;
  size_t i, n = m_particles.size();
  double phi, w, W = 0;
  double W_phi_R = 0, W_phi_L = 0;
  double phi_R = 0, phi_L = 0;

  if (!n) return;

  // First: XY
  // -----------------------------------
  for (i = 0; i < n; i++)
  {
    p = m_particles[i].d->pose;
    w = exp(m_particles[i].log_w);
    W += w;

    est.x_incr(p.x() * w);
    est.y_incr(p.y() * w);

    // PHI is special:
    phi = p.phi();
    if (fabs(phi) > 1.5707963267948966192313216916398f)
    {
      // LEFT HALF: 0,2pi
      if (phi < 0) phi = (M_2PI + phi);

      phi_L += phi * w;
      W_phi_L += w;
    }
    else
    {
      // RIGHT HALF: -pi,pi
      phi_R += phi * w;
      W_phi_R += w;
    }
  }

  est *= (1 / W);

  // Next: PHI
  // -----------------------------------
  // The mean value from each side:
  if (W_phi_L > 0) phi_L /= W_phi_L;  // [0,2pi]
  if (W_phi_R > 0) phi_R /= W_phi_R;  // [-pi,pi]

  // Left side to [-pi,pi] again:
  if (phi_L > M_PI) phi_L = phi_L - M_2PI;

  // The total mean:
  est.phi(((phi_L * W_phi_L + phi_R * W_phi_R) / (W_phi_L + W_phi_R)));
}

/*---------------------------------------------------------------
            getEstimatedPoseState
 ---------------------------------------------------------------*/
TExtendedCPose2D CPosePDFParticlesExtended::getEstimatedPoseState() const
{
  TExtendedCPose2D est;
  CPose2D p;
  size_t i, n = m_particles.size();
  double phi;
  double w, W = 0;
  double W_phi_R = 0, W_phi_L = 0;
  double phi_R = 0, phi_L = 0;

  if (!n) return est;

  est.state.resize(m_particles[0].d->state.size());

  for (i = 0; i < n; i++) W += exp(m_particles[i].log_w);
  if (W == 0) W = 1;

  // First: XY
  // -----------------------------------
  for (i = 0; i < n; i++)
  {
    p = m_particles[i].d->pose;
    w = exp(m_particles[i].log_w) / W;

    est.pose.x_incr((p.x() * w));
    est.pose.y_incr((p.y() * w));

    CVectorDouble auxVec(m_particles[i].d->state);
    auxVec *= w;
    est.state += auxVec;

    // PHI is special:
    phi = p.phi();
    if (fabs(phi) > 1.5707963267948966192313216916398f)
    {
      // LEFT HALF: 0,2pi
      if (phi < 0) phi = (M_2PI + phi);

      phi_L += phi * w;
      W_phi_L += w;
    }
    else
    {
      // RIGHT HALF: -pi,pi
      phi_R += phi * w;
      W_phi_R += w;
    }
  }

  est.pose *= (1.0 / W);

  // Next: PHI
  // -----------------------------------
  // The mean value from each side:
  if (W_phi_L > 0) phi_L /= W_phi_L;  // [0,2pi]
  if (W_phi_R > 0) phi_R /= W_phi_R;  // [-pi,pi]

  // Left side to [-pi,pi] again:
  if (phi_L > M_PI) phi_L = phi_L - M_2PI;

  // The total mean:
  est.pose.phi(((phi_L * W_phi_L + phi_R * W_phi_R) / (W_phi_L + W_phi_R)));

  return est;
}

std::tuple<CMatrixDouble33, CPose2D> CPosePDFParticlesExtended::getCovarianceAndMean() const
{
  CMatrixDouble33 cov;
  CPose2D mean;
  getMean(mean);
  cov.setZero();

  size_t i, n = m_particles.size();
  double var_x = 0, var_y = 0, var_p = 0, var_xy = 0, var_xp = 0, var_yp = 0;
  double mean_phi = mean.phi();

  if (mean_phi < 0) mean_phi = M_2PI + mean_phi;

  double lin_w_sum = 0;

  for (i = 0; i < n; i++) lin_w_sum += exp(m_particles[i].log_w);
  if (lin_w_sum == 0) lin_w_sum = 1;

  for (i = 0; i < n; i++)
  {
    double w = exp(m_particles[i].log_w) / lin_w_sum;

    // Manage 1 PI range:
    double err_x = m_particles[i].d->pose.x() - mean.x();
    double err_y = m_particles[i].d->pose.y() - mean.y();
    double err_phi = wrapToPi(fabs(m_particles[i].d->pose.phi() - mean_phi));

    var_x += square(err_x) * w;
    var_y += square(err_y) * w;
    var_p += square(err_phi) * w;
    var_xy += err_x * err_y * w;
    var_xp += err_x * err_phi * w;
    var_yp += err_y * err_phi * w;
  }

  if (n >= 2)
  {
    // Unbiased estimation of variance:
    cov(0, 0) = var_x;
    cov(1, 1) = var_y;
    cov(2, 2) = var_p;

    cov(1, 0) = cov(0, 1) = var_xy;
    cov(2, 0) = cov(0, 2) = var_xp;
    cov(1, 2) = cov(2, 1) = var_yp;
  }
  return {cov, mean};
}

uint8_t CPosePDFParticlesExtended::serializeGetVersion() const { return 0; }
void CPosePDFParticlesExtended::serializeTo(mrpt::serialization::CArchive& out) const
{
  out.WriteAs<uint32_t>(m_particles.size());
  for (const auto& p : m_particles) out << p.log_w << p.d->pose << p.d->state;
}

void CPosePDFParticlesExtended::serializeFrom(mrpt::serialization::CArchive& in, uint8_t version)
{
  switch (version)
  {
    case 0:
    {
      CParticleList::iterator it;
      uint32_t n;

      // Delete previous content:
      m_particles.clear();

      // The data
      in >> n;
      m_particles.resize(n);
      for (it = m_particles.begin(); it != m_particles.end(); it++)
        in >> it->log_w >> it->d->pose >> it->d->state;
    }
    break;
    default:
      MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version);
  };
}

/*---------------------------------------------------------------
      prediction_and_update_pfStandardProposal
 ---------------------------------------------------------------*/
void CPosePDFParticlesExtended::offsetTransitionModel(double& val)
{
  if (getRandomGenerator().drawUniform(0.0, 1.0) < options.probabilityChangingBias)
  {
    val = getRandomGenerator().drawUniform(-options.changingBiasUnifRange, 1.0);
    //		val = min( val, 0.5f*options.changingBiasUnifRange );
    //		val = max( val,-0.5f*options.changingBiasUnifRange );
  }
}

/*---------------------------------------------------------------

      prediction_and_update_pfStandardProposal

 ---------------------------------------------------------------*/
void CPosePDFParticlesExtended::prediction_and_update_pfStandardProposal(
    const mrpt::obs::CActionCollection* actions,
    const mrpt::obs::CSensoryFrame* sf,
    const bayes::CParticleFilter::TParticleFilterOptions& PF_options)
{
  MRPT_START

  size_t i, M = m_particles.size();
  CActionRobotMovement2D::Ptr robotMovement;

  // ----------------------------------------------------------------------
  //						PREDICTION STAGE
  // ----------------------------------------------------------------------
  ASSERT_(actions != nullptr);

  // Find a robot movement estimation:
  robotMovement = actions->getBestMovementEstimation();

  // Assure one has been found:
  if (!robotMovement)
    THROW_EXCEPTION(
        "Action list does not contain any CActionRobotMovement2D derived "
        "object!");

  //  Prediction:
  // -----------------------------
  if (!PF_options.adaptiveSampleSize)
  {
    // ---------------------
    //   FIXES SAMPLE SIZE
    // ---------------------
    // Initialize random sample generator:
    mrpt::poses::CPoseRandomSampler poseSamplesGen;
    poseSamplesGen.setPosePDF(*robotMovement->poseChange);

    CPose2D increment_i;
    for (i = 0; i < M; i++)  // Update particle poses:
    {
      poseSamplesGen.drawSample(increment_i);

      // Add pose increment.
      m_particles[i].d->pose = m_particles[i].d->pose + increment_i;

      // Prediction of the BIAS "state vector":
      for (int k = 0; k < m_particles[i].d->state.size(); k++)
        offsetTransitionModel(m_particles[i].d->state[k]);
    }
  }  // end of fixed sample size
  else
  {
    THROW_EXCEPTION("Dynamic sample size not implemented in this class");
  }  // end of ADAPTIVE SAMPLE SIZE

  // ----------------------------------------------------------------------
  //						UPDATE STAGE
  // ----------------------------------------------------------------------
  M = m_particles.size();
  if (sf != nullptr)
  {
    // A map MUST be supplied!
    ASSERT_(options.metricMap || options.metricMaps.size() > 0);
    if (!options.metricMap) ASSERT_(options.metricMaps.size() == M);

    // Update particle's likelihood using the particle's pose:
    CParticleList::iterator it;
    for (it = m_particles.begin(), i = 0; it != m_particles.end(); it++, i++)
      it->log_w += auxiliarComputeObservationLikelihood(PF_options, this, i, sf, it->d.get()) *
                   PF_options.powFactor;
  };

  MRPT_END
}

/*---------------------------------------------------------------

      prediction_and_update_pfAuxiliaryPFOptimal

 ---------------------------------------------------------------*/
void CPosePDFParticlesExtended::prediction_and_update_pfAuxiliaryPFOptimal(
    const mrpt::obs::CActionCollection* actions,
    const mrpt::obs::CSensoryFrame* sf,
    const bayes::CParticleFilter::TParticleFilterOptions& PF_options)
{
  MRPT_START

  size_t i, k, N, M = m_particles.size();
  CActionRobotMovement2D::Ptr robotMovement;

  // ----------------------------------------------------------------------
  //							PRELIMINAR CHECKS:
  // ----------------------------------------------------------------------
  ASSERT_(sf != nullptr);
  ASSERT_(actions != nullptr);

  robotMovement = actions->getBestMovementEstimation();  // Find a robot movement estimation:
  if (!robotMovement)                                    // Assure one has been found:
    THROW_EXCEPTION(
        "Action list does not contain any CActionRobotMovement2D derived "
        "object!");

  ASSERT_(options.metricMap || options.metricMaps.size() > 0);
  if (!options.metricMap)
  {
    ASSERT_(options.metricMaps.size() == M);
  }

  // ----------------------------------------------------------------------
  //		0) Common part:  Prepare m_particles "draw" and compute
  // ----------------------------------------------------------------------
  // Initialize random sample generator:
  mrpt::poses::CPoseRandomSampler m_movementDrawer;
  m_movementDrawer.setPosePDF(*robotMovement->poseChange);

  CPose2D mean_movement;
  robotMovement->poseChange->getMean(mean_movement);

  m_pfAuxiliaryPFOptimal_estimatedProb.resize(M);

  // Prepare data for executing "fastDrawSample"
  CTicTac tictac;
  printf("[prepareFastDrawSample] Computing...");
  tictac.Tic();
  prepareFastDrawSample(PF_options, particlesEvaluator_AuxPFOptimal, &mean_movement, sf);
  printf("Done! in %.06f ms\n", tictac.Tac() * 1e3f);

#if 1 /** DEBUG **/
  printf(
      "[prepareFastDrawSample] max      (log) = %10.06f\n",
      math::maximum(m_pfAuxiliaryPFOptimal_estimatedProb));
  printf(
      "[prepareFastDrawSample] max-mean (log) = %10.06f\n",
      -math::mean(m_pfAuxiliaryPFOptimal_estimatedProb) +
          math::maximum(m_pfAuxiliaryPFOptimal_estimatedProb));
  printf(
      "[prepareFastDrawSample] max-min  (log) = %10.06f\n",
      -math::minimum(m_pfAuxiliaryPFOptimal_estimatedProb) +
          math::maximum(m_pfAuxiliaryPFOptimal_estimatedProb));
#endif /*****/

  // Now we have the vector "m_fastDrawProbability" filled out with:
  //     w[i]·p(zt|z^{t-1},x^{[i],t-1},X)
  //  where X is the robot pose prior (as implemented in
  //  the aux. function "particlesEvaluator_AuxPFOptimal"),

  // Precompute a list of "random" samples from the movement model:
  deque<TExtendedCPose2D*> newParticles;
  vector<double> newParticlesWeight;

  // We need the (aproximate) maximum likelihood value for each
  //  previous particle [i]:
  //
  //     max{ p( z^t | data^[i], x_(t-1)^[i], u_(t) ) }
  //
  // We store "-1" if the max. value has not been computed yet:
  vector<double> maxLikelihood(M, -1e300);
  CPose2D movementDraw;
  TExtendedCPose2D newPose, oldPose;
  double acceptanceProb, newPoseLikelihood, ratioLikLik;
  unsigned int timeoutCount, MAX_TIMEOUT = 1000;
  double bestNewPoseAccProb;
  TExtendedCPose2D bestNewPose;

  if (!PF_options.adaptiveSampleSize)
  {
    // ----------------------------------------------------------------------
    //						1) FIXED SAMPLE SIZE VERSION
    // ----------------------------------------------------------------------
    newParticles.resize(M);
    newParticlesWeight.resize(M);

    for (i = 0; i < M; i++)
    {
      // Generate a new particle:
      //   (a) Draw a "t-1" m_particles' index:
      // ----------------------------------------------------------------
      k = fastDrawSample(PF_options);
      oldPose = *m_particles[k].d;

      //   (b) Rejection-sampling: Draw a new robot pose from x[k],
      //       and accept it with probability p(zk|x) / maxLikelihood:
      // ----------------------------------------------------------------
      // Is the maxLikelihood already computed?
      double maxLik_k = maxLikelihood[k];
      CPose2D drawnSample;
      TExtendedCPose2D maxLikNewPose;

      if (maxLik_k < 0)
      {
        // NO -> Compute now and save it:
        for (unsigned int q = 0; q < PF_options.pfAuxFilterOptimal_MaximumSearchSamples; q++)
        {
          // Draw new robot pose:
          newPose.pose = oldPose.pose + m_movementDrawer.drawSample(drawnSample);
          // Prediction of the BIAS "state vector":
          newPose.state = oldPose.state;
          for (auto& s : newPose.state) offsetTransitionModel(s);

          // Likelihood:
          double lik = auxiliarComputeObservationLikelihood(PF_options, this, k, sf, &newPose);
          if (lik > maxLik_k)
          {
            maxLik_k = lik;
            maxLikNewPose = newPose;
          }
        }
        maxLikelihood[k] = maxLik_k;
      }

      // Rejection-sampling:
      timeoutCount = 0;
      bestNewPoseAccProb = 0;
      // static unsigned int	q =
      // PF_options.pfAuxFilterOptimal_MaximumSearchSamples;
      do
      {
        // Draw new robot pose:
        newPose.pose = oldPose.pose + m_movementDrawer.drawSample(drawnSample);
        // Prediction of the BIAS "state vector":
        newPose.state = oldPose.state;
        for (size_t q = 0; q < size_t(newPose.state.size()); q++)
          offsetTransitionModel(newPose.state[q]);

        // Compute acceptance probability:
        newPoseLikelihood = auxiliarComputeObservationLikelihood(PF_options, this, k, sf, &newPose);
        ratioLikLik = exp(newPoseLikelihood - maxLikelihood[k]);
        acceptanceProb = min(1.0, ratioLikLik);

        // Save the best "newPose" for the case of having a "timeout":
        if (acceptanceProb > bestNewPoseAccProb)
        {
          bestNewPoseAccProb = acceptanceProb;
          bestNewPose = newPose;
        }

        if (ratioLikLik > 1)
        {
          if (ratioLikLik > 1.2)
          {
            //  DEBUG
            // printf("\n[pfAuxiliaryPFOptimal] Warning!!
            // p(z|x)/p(z|x*)=%f\n",ratioLikLik);
          }
          maxLikelihood[k] = newPoseLikelihood;  //  :'-( !!!
        }

      } while (acceptanceProb < getRandomGenerator().drawUniform(0.0, 0.999) &&
               (++timeoutCount) < MAX_TIMEOUT);

      if (timeoutCount >= MAX_TIMEOUT) newPose = bestNewPose;

      // Insert the new particle!:
      newParticles[i] = new TExtendedCPose2D(newPose);
      // And its weight:
      newParticlesWeight[i] = 1;  // newPoseLikelihood /
                                  // m_pfAuxiliaryPFOptimal_estimatedProb[k];
    }                             // for i

  }  // end fixed sample size
  else
  {
    THROW_EXCEPTION("Not implemented for this class.");
  }  // end adaptive sample size

  // Substitute old by new particle set:
  // -------------------------------------------------
  N = newParticles.size();
  clear();  // Free old m_particles memory:
  m_particles.resize(N);

  CParticleList::iterator itDest;
  deque<TExtendedCPose2D*>::const_iterator itSrc;
  vector<double>::iterator itW;

  for (itDest = m_particles.begin(), itSrc = newParticles.begin(), itW = newParticlesWeight.begin();
       itDest != m_particles.end(); itDest++, itSrc++, itW++)
  {
    itDest->d.reset(*itSrc);
    itDest->log_w = *itW;
  }
  newParticles.clear();
  normalizeWeights();

  MRPT_END
}

/*---------------------------------------------------------------
              resetDeterministic
  Reset PDF to a single point and set the number of particles
 ---------------------------------------------------------------*/
void CPosePDFParticlesExtended::resetDeterministic(TExtendedCPose2D& location, int particlesCount)
{
  CParticleList::iterator it;

  if (particlesCount > 0)
  {
    clear();
    m_particles.resize(particlesCount);
    for (it = m_particles.begin(); it != m_particles.end(); it++)
      it->d.reset(new TExtendedCPose2D());
  }

  for (it = m_particles.begin(); it != m_particles.end(); it++)
  {
    *it->d = location;
    it->log_w = 0;
  }
}

/*---------------------------------------------------------------
            resetUniform
 ---------------------------------------------------------------*/
void CPosePDFParticlesExtended::resetUniform(
    float x_min,
    float x_max,
    float y_min,
    float y_max,
    CVectorFloat state_min,
    CVectorFloat state_max,
    float phi_min,
    float phi_max,
    int particlesCount)
{
  MRPT_START

  ASSERT_(state_min.size() == state_max.size());

  if (particlesCount > 0)
  {
    clear();
    m_particles.resize(particlesCount);
    for (int i = 0; i < particlesCount; i++) m_particles[i].d.reset(new TExtendedCPose2D());
  }

  size_t i, M = m_particles.size();
  for (i = 0; i < M; i++)
  {
    m_particles[i].d->pose.x(getRandomGenerator().drawUniform(x_min, x_max));
    m_particles[i].d->pose.y(getRandomGenerator().drawUniform(y_min, y_max));
    m_particles[i].d->pose.phi(getRandomGenerator().drawUniform(phi_min, phi_max));
    m_particles[i].d->state.resize(state_min.size());
    m_particles[i].d->state.resize(state_max.size());

    for (int k = 0; k < state_min.size(); k++)
      m_particles[i].d->state[k] = getRandomGenerator().drawUniform(state_min[k], state_max[k]);

    m_particles[i].log_w = 0;
  }

  MRPT_END
}

bool CPosePDFParticlesExtended::saveToTextFile(const std::string& file) const
{
  FILE* f = os::fopen(file.c_str(), "wt");
  if (!f) return false;

  for (const auto& m_particle : m_particles)
    os::fprintf(
        f, "%f %f %f %e\n", m_particle.d->pose.x(), m_particle.d->pose.y(),
        m_particle.d->pose.phi(), m_particle.log_w);

  os::fclose(f);
  return true;
}

CPose2D CPosePDFParticlesExtended::getParticlePose(int i) const { return m_particles[i].d->pose; }

void CPosePDFParticlesExtended::changeCoordinatesReference(const CPose3D& newReferenceBase_)
{
  CPose2D newReferenceBase = CPose2D(newReferenceBase_);
  CParticleList::iterator it;

  for (it = m_particles.begin(); it != m_particles.end(); it++)
    it->d->pose = newReferenceBase + it->d->pose;
}

void CPosePDFParticlesExtended::drawSingleSample(CPose2D& outPart) const
{
  float uni = getRandomGenerator().drawUniform(0.0f, 0.9999f);
  double cum = 0;
  CParticleList::const_iterator it;

  for (it = m_particles.begin(); it != m_particles.end(); it++)
  {
    cum += exp(it->log_w);
    if (uni <= cum)
    {
      outPart = it->d->pose;
      return;
    }
  }

  // Might not come here normally:
  outPart = (m_particles.end() - 1)->d->pose;
}

/*---------------------------------------------------------------
          drawManySamples
 ---------------------------------------------------------------*/
void CPosePDFParticlesExtended::drawManySamples(
    size_t N, std::vector<CVectorDouble>& outSamples) const
{
  CParticleFilter::TParticleFilterOptions PF_options;
  PF_options.adaptiveSampleSize = true;
  PF_options.resamplingMethod = CParticleFilter::prMultinomial;

  prepareFastDrawSample(PF_options);

  outSamples.resize(N);
  for (size_t i = 0; i < N; i++)
  {
    const TExtendedCPose2D* ptr = m_particles[fastDrawSample(PF_options)].d.get();

    // Copy pose:
    outSamples[i].resize(3);
    outSamples[i][0] = ptr->pose.x();
    outSamples[i][1] = ptr->pose.y();
    outSamples[i][2] = ptr->pose.phi();
  }
}

/*---------------------------------------------------------------
            +=
 ---------------------------------------------------------------*/
void CPosePDFParticlesExtended::operator+=(const CPose2D& Ap)
{
  CParticleList::iterator it;

  for (it = m_particles.begin(); it != m_particles.end(); it++) it->d->pose = it->d->pose + Ap;
}

/*---------------------------------------------------------------
          inverse
 ---------------------------------------------------------------*/
void CPosePDFParticlesExtended::inverse(CPosePDF& o) const
{
  ASSERT_(o.GetRuntimeClass() == CLASS_ID(CPosePDFParticlesExtended));
  auto* out = (CPosePDFParticlesExtended*)&o;

  out->copyFrom(*this);
  static CPose2D nullPose(0, 0, 0);

  for (auto& m_particle : out->m_particles) m_particle.d->pose = nullPose - m_particle.d->pose;
}

/*---------------------------------------------------------------
          getMostLikelyParticle
 ---------------------------------------------------------------*/
CPose2D CPosePDFParticlesExtended::getMostLikelyParticle() const
{
  CParticleList::const_iterator it, itMax = m_particles.begin();
  double max_w = -1e300;

  for (it = m_particles.begin(); it != m_particles.end(); it++)
  {
    if (it->log_w > max_w)
    {
      itMax = it;
      max_w = it->log_w;
    }
  }

  return itMax->d->pose;
}

/*---------------------------------------------------------------
          bayesianFusion
 ---------------------------------------------------------------*/
void CPosePDFParticlesExtended::bayesianFusion(
    [[maybe_unused]] const CPosePDF& p1,
    [[maybe_unused]] const CPosePDF& p2,
    [[maybe_unused]] const double minMahalanobisDistToDrop)
{
  THROW_EXCEPTION("Not implemented yet!");
}

/*---------------------------------------------------------------
          evaluatePDF_parzen
 ---------------------------------------------------------------*/
double CPosePDFParticlesExtended::evaluatePDF_parzen(
    float x, float y, float phi, float stdXY, float stdPhi) const
{
  double difPhi, ret = 0;

  for (const auto& m_particle : m_particles)
  {
    difPhi = wrapToPi(phi - m_particle.d->pose.phi());

    ret += exp(m_particle.log_w) *
           mrpt::math::normalPDF(
               sqrt(square(x - m_particle.d->pose.x()) + square(y - m_particle.d->pose.y())), 0,
               stdXY) *
           mrpt::math::normalPDF(fabs(difPhi), 0, stdPhi);
  }

  return ret;
}

/*---------------------------------------------------------------
          saveParzenPDFToTextFile
 ---------------------------------------------------------------*/
void CPosePDFParticlesExtended::saveParzenPDFToTextFile(
    const char* fileName,
    float x_min,
    float x_max,
    float y_min,
    float y_max,
    float phi,
    float stepSizeXY,
    float stdXY,
    float stdPhi) const
{
  FILE* f = os::fopen(fileName, "wt");
  if (!f) return;

  for (float y = y_min; y < y_max; y += stepSizeXY)
  {
    for (float x = x_min; x < x_max; x += stepSizeXY)
    {
      fprintf(f, "%f ", evaluatePDF_parzen(x, y, phi, stdXY, stdPhi));
    }  // y
    fprintf(f, "\n");
  }  // x

  os::fclose(f);
}

/*---------------------------------------------------------------
          TPredictionParams
 ---------------------------------------------------------------*/
CPosePDFParticlesExtended::TPredictionParams::TPredictionParams()
{
  metricMap = nullptr;

  KLD_minSampleSize = 250;
  KLD_maxSampleSize = 100000;
  KLD_binSize_XY = 0.2f;
  KLD_binSize_PHI = 5.0_deg;
  KLD_delta = 0.01f;
  KLD_epsilon = 0.02f;

  probabilityChangingBias = 0.1f;
  changingBiasUnifRange = 2.0f;
  mixtureProposalRatio = 0.10f;

  pfAuxFilterOptimal_MaximumSearchSamples = 20;
}

/*---------------------------------------------------------------
        auxiliarComputeObservationLikelihood
 ---------------------------------------------------------------*/
double CPosePDFParticlesExtended::auxiliarComputeObservationLikelihood(
    const bayes::CParticleFilter::TParticleFilterOptions& PF_options,
    const CParticleFilterCapable* obj,
    size_t particleIndexForMap,
    const CSensoryFrame* observation,
    const TExtendedCPose2D* x)
{
  double ret = 1;
  CMetricMap* map;  // The map:

  const auto* pdf = static_cast<const CPosePDFParticlesExtended*>(obj);

  if (pdf->options.metricMap)
    map = pdf->options.metricMap;
  else
  {
    ASSERT_(pdf->options.metricMaps.size() > particleIndexForMap);
    map = pdf->options.metricMaps[particleIndexForMap];
  }

  // For each observation:
  for (const auto& it : *observation)
  {
    const CObservation* obser = it.get();
    CObservationBeaconRanges obserDumm;

    // JLBC: 20/ABR/2007 -> UWB offset from extended state vector
    if (obser->GetRuntimeClass() == CLASS_ID(CObservationBeaconRanges))
    {
      auto* obs = (CObservationBeaconRanges*)obser;
      obserDumm = *obs;

      // Introduce bias:
      ASSERT_((int)obserDumm.sensedData.size() == (int)x->state.size());
      for (size_t k = 0; k < size_t(obserDumm.sensedData.size()); k++)
        obserDumm.sensedData[k].sensedDistance -= x->state[k];

      // Substitute:
      obser = &obserDumm;
    }

    // Compute the likelihood:
    ret += map->computeObservationLikelihood(*obser, mrpt::poses::CPose3D(x->pose));
  }

  // Done!
  return ret;
}

/*---------------------------------------------------------------
      particlesEvaluator_AuxPFOptimal
 ---------------------------------------------------------------*/
double CPosePDFParticlesExtended::particlesEvaluator_AuxPFOptimal(
    const bayes::CParticleFilter::TParticleFilterOptions& PF_options,
    const CParticleFilterCapable* obj,
    size_t index,
    const void* action,
    const void* observation)
{
  MRPT_START

  // Compute the quantity:
  //     w[i]·p(zt|z^{t-1},x^{[i],t-1},X)
  // See paper: [blanco2007...]

  // Take the previous particle weight:
  // --------------------------------------------
  const auto* pdf = static_cast<const CPosePDFParticlesExtended*>(obj);

  double ret = pdf->m_particles[index].log_w;

  // , take the mean of the posterior density:
  // --------------------------------------------
  TExtendedCPose2D x_predict = *pdf->m_particles[index].d;
  x_predict.pose = x_predict.pose + *static_cast<const CPose2D*>(action);

  // and compute the obs. likelihood:
  // --------------------------------------------
  return ret +
         (pdf->m_pfAuxiliaryPFOptimal_estimatedProb[index] = auxiliarComputeObservationLikelihood(
              PF_options, pdf, index, static_cast<const CSensoryFrame*>(observation), &x_predict));

  MRPT_END
}
