/* +------------------------------------------------------------------------+
   |                     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 "poses-precomp.h"  // Precompiled headers
//
#include <mrpt/math/ops_containers.h>  // maximum()
#include <mrpt/poses/CPoint3D.h>
#include <mrpt/poses/CPointPDFParticles.h>
#include <mrpt/poses/CPose3D.h>
#include <mrpt/serialization/CArchive.h>
#include <mrpt/system/os.h>

#include <Eigen/Dense>

using namespace mrpt;
using namespace mrpt::poses;
using namespace mrpt::math;
using namespace mrpt::system;

IMPLEMENTS_SERIALIZABLE(CPointPDFParticles, CPointPDF, mrpt::poses)

CPointPDFParticles::CPointPDFParticles(size_t numParticles) { setSize(numParticles); }

/** Clear all the particles (free memory) */
void CPointPDFParticles::clear() { setSize(0); }
/*---------------------------------------------------------------
    setSize
  ---------------------------------------------------------------*/
void CPointPDFParticles::setSize(size_t numberParticles, const mrpt::math::TPoint3Df& defaultValue)
{
  // Free old particles: automatic via smart ptr
  m_particles.resize(numberParticles);
  for (auto& it : m_particles)
  {
    it.log_w = 0;
    it.d.reset(new TPoint3Df(defaultValue));
  }
}

/*---------------------------------------------------------------
            getMean
  Returns an estimate of the pose, (the mean, or mathematical expectation of the
 PDF)
 ---------------------------------------------------------------*/
void CPointPDFParticles::getMean(CPoint3D& p) const
{
  MRPT_START
  if (m_particles.empty()) THROW_EXCEPTION("Cannot compute mean since there are zero particles.");

  CParticleList::const_iterator it;
  double sumW = 0;
  double x = 0, y = 0, z = 0;
  for (it = m_particles.begin(); it != m_particles.end(); it++)
  {
    const double w = exp(it->log_w);
    x += it->d->x * w;
    y += it->d->y * w;
    z += it->d->z * w;
    sumW += w;
  }

  ASSERT_(sumW != 0);

  sumW = 1.0 / sumW;

  p.x(x * sumW);
  p.y(y * sumW);
  p.z(z * sumW);

  MRPT_END
}

std::tuple<CMatrixDouble33, CPoint3D> CPointPDFParticles::getCovarianceAndMean() const
{
  MRPT_START

  CPoint3D mean;
  CMatrixDouble33 cov;

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

    double err_x = m_particles[i].d->x - mean.x();
    double err_y = m_particles[i].d->y - mean.y();
    double err_phi = m_particles[i].d->z - mean.z();

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

uint8_t CPointPDFParticles::serializeGetVersion() const { return 0; }
void CPointPDFParticles::serializeTo(mrpt::serialization::CArchive& out) const
{
  uint32_t N = size();
  out << N;

  for (const auto& m_particle : m_particles)
    out << m_particle.log_w << m_particle.d->x << m_particle.d->y << m_particle.d->z;
}

void CPointPDFParticles::serializeFrom(mrpt::serialization::CArchive& in, uint8_t version)
{
  switch (version)
  {
    case 0:
    {
      uint32_t N;
      in >> N;
      setSize(N);

      for (auto& m_particle : m_particles)
        in >> m_particle.log_w >> m_particle.d->x >> m_particle.d->y >> m_particle.d->z;
    }
    break;
    default:
      MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version);
  };
}

void CPointPDFParticles::copyFrom(const CPointPDF& o)
{
  if (this == &o) return;  // It may be used sometimes

  // Convert to samples:
  THROW_EXCEPTION("NO");
}

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

  ---------------------------------------------------------------*/
bool CPointPDFParticles::saveToTextFile(const std::string& file) const
{
  MRPT_START

  FILE* f = os::fopen(file.c_str(), "wt");
  if (!f) return false;

  size_t i, N = m_particles.size();
  for (i = 0; i < N; i++)
    os::fprintf(
        f, "%f %f %f %e\n", m_particles[i].d->x, m_particles[i].d->y, m_particles[i].d->z,
        m_particles[i].log_w);

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

/*---------------------------------------------------------------
            changeCoordinatesReference
 ---------------------------------------------------------------*/
void CPointPDFParticles::changeCoordinatesReference(const CPose3D& newReferenceBase)
{
  TPoint3D pt;
  for (auto& m_particle : m_particles)
  {
    newReferenceBase.composePoint(
        m_particle.d->x, m_particle.d->y,
        m_particle.d->z,  // In
        pt.x, pt.y, pt.z  // Out
    );
    m_particle.d->x = d2f(pt.x);
    m_particle.d->y = d2f(pt.y);
    m_particle.d->z = d2f(pt.z);
  }
}

/*---------------------------------------------------------------
            computeKurtosis
 ---------------------------------------------------------------*/
double CPointPDFParticles::computeKurtosis()
{
  MRPT_START

  // kurtosis = \mu^4 / (\sigma^2) -3
  Eigen::Vector3d kurts, mu4, m, var;
  kurts.fill(0);
  mu4.fill(0);
  m.fill(0);
  var.fill(0);

  // Means:
  for (auto& m_particle : m_particles)
  {
    m[0] += m_particle.d->x;
    m[1] += m_particle.d->y;
    m[2] += m_particle.d->z;
  }
  m *= 1.0 / m_particles.size();

  // variances:
  for (auto& m_particle : m_particles)
  {
    var[0] += square(m_particle.d->x - m[0]);
    var[1] += square(m_particle.d->y - m[1]);
    var[2] += square(m_particle.d->z - m[2]);
  }
  var *= 1.0 / m_particles.size();
  var[0] = square(var[0]);
  var[1] = square(var[1]);
  var[2] = square(var[2]);

  // Moment:
  for (auto& m_particle : m_particles)
  {
    mu4[0] += pow(m_particle.d->x - m[0], 4.0);
    mu4[1] += pow(m_particle.d->y - m[1], 4.0);
    mu4[2] += pow(m_particle.d->z - m[2], 4.0);
  }
  mu4 *= 1.0 / m_particles.size();

  // Kurtosis's
  kurts.array() = mu4.array() / var.array();

  return math::maximum(kurts);

  MRPT_END
}

/*---------------------------------------------------------------
          drawSingleSample
  ---------------------------------------------------------------*/
void CPointPDFParticles::drawSingleSample([[maybe_unused]] CPoint3D& outSample) const
{
  THROW_EXCEPTION("TO DO!");
}

/*---------------------------------------------------------------
          bayesianFusion
 ---------------------------------------------------------------*/
void CPointPDFParticles::bayesianFusion(
    [[maybe_unused]] const CPointPDF& p1_,
    [[maybe_unused]] const CPointPDF& p2_,
    [[maybe_unused]] const double minMahalanobisDistToDrop)
{
  MRPT_START

  THROW_EXCEPTION("TODO!!!");

  MRPT_END
}
