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

#include <mrpt/math/CMatrixD.h>
#include <mrpt/poses/CPoseInterpolatorBase.h>
//#include <mrpt/math/eigen_extensions.h>
#include <mrpt/math/slerp.h>
#include <mrpt/math/wrap2pi.h>
#include <mrpt/poses/CPose2D.h>
#include <mrpt/poses/CPose3D.h>
#include <mrpt/poses/CPose3DPDFParticles.h>
#include <mrpt/poses/CPose3DQuat.h>
#include <mrpt/serialization/stl_serialization.h>
#include <mrpt/system/datetime.h>

#include <fstream>
#include <mrpt/math/interp_fit.hpp>

namespace mrpt::poses
{
template <int DIM>
CPoseInterpolatorBase<DIM>::CPoseInterpolatorBase() : maxTimeInterpolation(std::chrono::seconds(-1))

{
}

template <int DIM>
void CPoseInterpolatorBase<DIM>::clear()
{
  m_path.clear();
}

template <int DIM>
void CPoseInterpolatorBase<DIM>::insert(const mrpt::Clock::time_point& t, const cpose_t& p)
{
  m_path[t] = p.asTPose();
}
template <int DIM>
void CPoseInterpolatorBase<DIM>::insert(const mrpt::Clock::time_point& t, const pose_t& p)
{
  m_path[t] = p;
}

/*---------------------------------------------------------------
            interpolate
  ---------------------------------------------------------------*/
template <int DIM>
typename CPoseInterpolatorBase<DIM>::cpose_t& CPoseInterpolatorBase<DIM>::interpolate(
    const mrpt::Clock::time_point& t, cpose_t& out_interp, bool& out_valid_interp) const
{
  pose_t p;
  this->interpolate(t, p, out_valid_interp);
  out_interp = cpose_t(p);
  return out_interp;
}

template <int DIM>
typename CPoseInterpolatorBase<DIM>::pose_t& CPoseInterpolatorBase<DIM>::interpolate(
    const mrpt::Clock::time_point& t, pose_t& out_interp, bool& out_valid_interp) const
{
  // Default value in case of invalid interp
  for (size_t k = 0; k < pose_t::static_size; k++)
  {
    out_interp[k] = 0;
  }
  TTimePosePair p1, p2, p3, p4;
  p1.second = p2.second = p3.second = p4.second = out_interp;

  // We'll look for 4 consecutive time points.
  // Check if the selected method needs all 4 points or just the central 2 of
  // them:
  bool interp_method_requires_4pts;
  switch (m_method)
  {
    case imLinear2Neig:
    case imSplineSlerp:
    case imLinearSlerp:
      interp_method_requires_4pts = false;
      break;
    default:
      interp_method_requires_4pts = true;
      break;
  };

  // Out of range?
  auto it_ge1 = m_path.lower_bound(t);

  // Exact match?
  if (it_ge1 != m_path.end() && it_ge1->first == t)
  {
    out_interp = it_ge1->second;
    out_valid_interp = true;
    return out_interp;
  }

  // Are we in the beginning or the end of the path?
  if (it_ge1 == m_path.end() || it_ge1 == m_path.begin())
  {
    out_valid_interp = false;
    return out_interp;
  }  // end

  p3 = *it_ge1;  // Third pair
  auto it_ge2 = it_ge1;
  ++it_ge2;
  if (it_ge2 == m_path.end())
  {
    if (interp_method_requires_4pts)
    {
      out_valid_interp = false;
      return out_interp;
    }
  }
  else
  {
    p4 = *(it_ge2);  // Fourth pair
  }

  p2 = *(--it_ge1);  // Second pair

  if (it_ge1 == m_path.begin())
  {
    if (interp_method_requires_4pts)
    {
      out_valid_interp = false;
      return out_interp;
    }
  }
  else
  {
    p1 = *(--it_ge1);  // First pair
  }

  // Test if the difference between the desired timestamp and the next
  // timestamp is lower than a certain (configurable) value
  const mrpt::Clock::duration dt12 =
      interp_method_requires_4pts ? (p2.first - p1.first) : mrpt::Clock::duration(0);
  const mrpt::Clock::duration dt23 = (p3.first - p2.first);
  const mrpt::Clock::duration dt34 =
      interp_method_requires_4pts ? (p4.first - p3.first) : mrpt::Clock::duration(0);

  if (maxTimeInterpolation.count() > 0 &&
      (dt12 > maxTimeInterpolation || dt23 > maxTimeInterpolation || dt34 > maxTimeInterpolation))
  {
    out_valid_interp = false;
    return out_interp;
  }

  // Do interpolation:
  // ------------------------------------------
  // First Previous point:  p1
  // Second Previous point: p2
  // First Next point:	  p3
  // Second Next point:     p4
  // Time where to interpolate:  t

  impl_interpolation(p1, p2, p3, p4, m_method, t, out_interp);

  out_valid_interp = true;
  return out_interp;

}  // end interpolate

template <int DIM>
bool CPoseInterpolatorBase<DIM>::getPreviousPoseWithMinDistance(
    const mrpt::Clock::time_point& t, double distance, cpose_t& out_pose)
{
  pose_t p;
  bool ret = getPreviousPoseWithMinDistance(t, distance, p);
  out_pose = cpose_t(p);
  return ret;
}

template <int DIM>
bool CPoseInterpolatorBase<DIM>::getPreviousPoseWithMinDistance(
    const mrpt::Clock::time_point& t, double distance, pose_t& out_pose)
{
  if (m_path.size() == 0 || distance <= 0) return false;

  pose_t myPose;

  // Search for the desired timestamp
  auto it = m_path.find(t);
  if (it != m_path.end() && it != m_path.begin())
    myPose = it->second;
  else
    return false;

  double d = 0.0;
  do
  {
    --it;
    d = (point_t(myPose) - point_t(it->second)).norm();
  } while (d < distance && it != m_path.begin());

  if (d >= distance)
  {
    out_pose = it->second;
    return true;
  }
  else
    return false;
}  // end getPreviousPose

template <int DIM>
void CPoseInterpolatorBase<DIM>::setMaxTimeInterpolation(const mrpt::Clock::duration& time)
{
  ASSERT_(time.count() > 0);
  maxTimeInterpolation = time;
}

template <int DIM>
mrpt::Clock::duration CPoseInterpolatorBase<DIM>::getMaxTimeInterpolation()
{
  return maxTimeInterpolation;
}

template <int DIM>
bool CPoseInterpolatorBase<DIM>::saveToTextFile(const std::string& s) const
{
  try
  {
    std::ofstream f;
    f.open(s);
    if (!f.is_open()) return false;
    std::string str;
    for (auto i = m_path.begin(); i != m_path.end(); ++i)
    {
      const double t = mrpt::Clock::toDouble(i->first);
      const auto& p = i->second;

      str = mrpt::format("%.06f ", t);
      for (unsigned int k = 0; k < p.size(); k++) str += mrpt::format("%.06f ", p[k]);
      str += std::string("\n");

      f << str;
    }
    return true;
  }
  catch (...)
  {
    return false;
  }
}

template <int DIM>
bool CPoseInterpolatorBase<DIM>::saveToTextFile_TUM(const std::string& s) const
{
  try
  {
    std::ofstream f;
    f.open(s);
    if (!f.is_open()) return false;
    std::string str;
    for (auto i = m_path.begin(); i != m_path.end(); ++i)
    {
      const double t = mrpt::Clock::toDouble(i->first);
      const auto p = mrpt::poses::CPose3DQuat(mrpt::poses::CPose3D(i->second));

      // TUM format: timestamp x y z q_x q_y q_z q_w
      str = mrpt::format("%.06f", t);
      constexpr std::array<int, 7> idxs = {0, 1, 2, 4, 5, 6, 3};
      for (unsigned int k = 0; k < 7; k++) str += mrpt::format(" %.06f", p[idxs[k]]);
      str += std::string("\n");
      f << str;
    }
    return true;
  }
  catch (...)
  {
    return false;
  }
}

template <int DIM>
bool CPoseInterpolatorBase<DIM>::saveInterpolatedToTextFile(
    const std::string& s, const mrpt::Clock::duration& period) const
{
  using mrpt::Clock;
  try
  {
    std::ofstream f;
    f.open(s);
    if (!f.is_open()) return false;
    if (m_path.empty()) return true;

    std::string str;

    const Clock::time_point t_ini = m_path.begin()->first;
    const Clock::time_point t_end = m_path.rbegin()->first;

    pose_t p;
    bool valid;
    for (Clock::time_point t = t_ini; t <= t_end; t += period)
    {
      this->interpolate(t, p, valid);
      if (!valid) continue;

      str = mrpt::format("%.06f ", mrpt::Clock::toDouble(t));
      for (unsigned int k = 0; k < p.size(); k++) str += mrpt::format("%.06f ", p[k]);
      str += std::string("\n");
      f << str;
    }
    return true;
  }
  catch (...)
  {
    return false;
  }
}

template <int DIM>
bool CPoseInterpolatorBase<DIM>::loadFromTextFile(const std::string& s)
{
  MRPT_START

  clear();
  mrpt::math::CMatrixD M;

  try
  {
    M.loadFromTextFile(s);
  }
  catch (std::exception&)
  {
    return false;  // error loading file
  }

  // Check valid format:
  if (M.rows() == 0) return false;
  ASSERT_(M.cols() == pose_t::static_size + 1);

  // load into the path:
  const size_t N = M.rows();
  pose_t p;
  for (size_t i = 0; i < N; i++)
  {
    for (unsigned int k = 0; k < pose_t::static_size; k++) p[k] = M(i, k + 1);
    insert(mrpt::Clock::fromDouble(M(i, 0)), p);
  }
  return true;
  MRPT_END
}

template <int DIM>
bool CPoseInterpolatorBase<DIM>::loadFromTextFile_TUM(const std::string& s)
{
  MRPT_START

  clear();
  mrpt::math::CMatrixD M;

  try
  {
    M.loadFromTextFile(s);
  }
  catch (std::exception&)
  {
    return false;  // error loading file
  }

  // Check valid format:
  if (M.rows() == 0) return false;
  ASSERT_(M.cols() == 3 + 4 + 1);

  // load into the path:
  const size_t N = M.rows();
  constexpr std::array<int, 7> idxs = {0, 1, 2, 4, 5, 6, 3};
  mrpt::poses::CPose3DQuat p;
  for (size_t i = 0; i < N; i++)
  {
    for (unsigned int k = 0; k < 7; k++) p[idxs[k]] = M(i, k + 1);
    insert(mrpt::Clock::fromDouble(M(i, 0)), pose_t(mrpt::poses::CPose3D(p).asTPose()));
  }
  return true;
  MRPT_END
}

template <int DIM>
void CPoseInterpolatorBase<DIM>::getBoundingBox(point_t& Min, point_t& Max) const
{
  MRPT_START
  ASSERT_(!m_path.empty());

  for (unsigned int k = 0; k < point_t::static_size; k++)
  {
    Min[k] = std::numeric_limits<double>::max();
    Max[k] = -std::numeric_limits<double>::max();
  }

  for (auto p = m_path.begin(); p != m_path.end(); ++p)
  {
    for (unsigned int k = 0; k < point_t::static_size; k++)
    {
      mrpt::keep_min(Min[k], p->second[k]);
      mrpt::keep_max(Max[k], p->second[k]);
    }
  }
  MRPT_END
}

template <int DIM>
void CPoseInterpolatorBase<DIM>::setInterpolationMethod(mrpt::poses::TInterpolatorMethod method)
{
  m_method = method;
}

template <int DIM>
TInterpolatorMethod CPoseInterpolatorBase<DIM>::getInterpolationMethod() const
{
  return m_method;
}

template <int DIM>
void CPoseInterpolatorBase<DIM>::filter(unsigned int component, unsigned int samples)
{
  if (m_path.empty()) return;

  TPath aux;

  int ant, post;
  size_t nitems = size();

  post = (samples % 2) ? (unsigned int)(samples / 2) : samples / 2;
  ant = (unsigned int)(samples / 2);

  int k = 0;
  iterator it1, it2, it3;

  // int asamples;
  for (it1 = m_path.begin(); it1 != m_path.end(); ++it1, ++k)
  {
    it2 = m_path.begin();
    if (k - ant > 0) advance(it2, k - ant);

    if (k + post < (int)nitems)
    {
      it3 = m_path.begin();
      advance(it3, k + post + 1);
    }
    else
    {
      it3 = m_path.end();
    }

    unsigned int nsamples = distance(it2, it3);
    CPose3DPDFParticles particles(nsamples);
    for (unsigned int i = 0; it2 != it3; ++it2, ++i)
    {
      particles.m_particles[i].log_w = 0;
      particles.m_particles[i].d = it1->second;
      switch (component)
      {
        case 0:
          particles.m_particles[i].d.x = it2->second[0];
          break;
        case 1:
          particles.m_particles[i].d.y = it2->second[1];
          break;
        case 2:
          particles.m_particles[i].d.z = it2->second[2];
          break;
        case 3:
          particles.m_particles[i].d.yaw = it2->second[3];
          break;
        case 4:
          particles.m_particles[i].d.pitch = it2->second[4];
          break;
        case 5:
          particles.m_particles[i].d.roll = it2->second[5];
          break;
      }  // end switch
    }    // end for it2

    mrpt::poses::CPose3D auxPose;
    particles.getMean(auxPose);
    aux[it1->first] = pose_t(auxPose.asTPose());
  }  // end for it1
  m_path = aux;
}
}  // namespace mrpt::poses
