/* +------------------------------------------------------------------------+
   |                     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 "math-precomp.h"  // Precompiled headers
//
#include <mrpt/math/CSplineInterpolator1D.h>  // for CSplineInterpolator1D
#include <mrpt/math/CVectorDynamic.h>
#include <mrpt/serialization/CSerializable.h>      // for CSerializable, CSeriali...
#include <mrpt/serialization/stl_serialization.h>  // for operator<<, operator>>

#include <map>
#include <mrpt/math/interp_fit.hpp>  // for spline

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

// This must be added to any CSerializable class implementation file.
IMPLEMENTS_SERIALIZABLE(CSplineInterpolator1D, CSerializable, mrpt::math)

/*---------------------------------------------------------------
            Constructor
  ---------------------------------------------------------------*/
CSplineInterpolator1D::CSplineInterpolator1D(bool wrap2pi) : m_wrap2pi(wrap2pi) {}

/*---------------------------------------------------------------
            appendXY
  ---------------------------------------------------------------*/
void CSplineInterpolator1D::appendXY(double x, double y) { m_x2y[x] = y; }
/*---------------------------------------------------------------
            query
  ---------------------------------------------------------------*/
double& CSplineInterpolator1D::query(double x, double& y, bool& out_valid) const
{
  out_valid = false;
  y = 0;

  std::pair<double, double> p1, p2, p3, p4;

  auto it_ge1 = m_x2y.lower_bound(x);

  // Exact match?
  if (it_ge1 != m_x2y.end() && it_ge1->first == x)
  {
    y = it_ge1->second;
    out_valid = true;
    return y;
  }

  // Are we in the beginning or the end of the path?
  if (it_ge1 == m_x2y.end() || it_ge1 == m_x2y.begin())
  {
    return y;
  }

  p3 = *it_ge1;  // Third pair
  ++it_ge1;
  if (it_ge1 == m_x2y.end())
  {
    return y;
  }
  p4 = *it_ge1;  // Fourth pair

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

  if (it_ge1 == m_x2y.begin())
  {
    return y;
  }

  p1 = *(--it_ge1);  // First pair

  // ---------------------------------------
  //    SPLINE INTERPOLATION
  // ---------------------------------------
  CVectorDouble xs(4);
  xs[0] = p1.first;
  xs[1] = p2.first;
  xs[2] = p3.first;
  xs[3] = p4.first;

  CVectorDouble ys(4);
  ys[0] = p1.second;
  ys[1] = p2.second;
  ys[2] = p3.second;
  ys[3] = p4.second;

  out_valid = true;
  return y = math::spline(x, xs, ys, m_wrap2pi);
}

uint8_t CSplineInterpolator1D::serializeGetVersion() const { return 0; }
void CSplineInterpolator1D::serializeTo(mrpt::serialization::CArchive& out) const
{
  out << m_x2y << m_wrap2pi;
}

void CSplineInterpolator1D::serializeFrom(mrpt::serialization::CArchive& in, uint8_t version)
{
  switch (version)
  {
    case 0:  // floats
    {
      in >> m_x2y >> m_wrap2pi;
    }
    break;
    default:
      MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version);
  };
}
