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

#include <mrpt/serialization/CSerializable.h>

#include <map>

namespace mrpt::math
{
/** A (persistent) sequence of (x,y) coordinates, allowing queries of
 * intermediate points through spline interpolation, where possible.
 *  This class internally relies on mrpt::math::spline. Optionally the y
 * coordinate can be set as wrapped in ]-pi,pi].
 *  For querying interpolated points, see
 * \ sa mrpt::math::spline, mrpt::poses::CPose3DInterpolator
 * \ingroup interpolation_grp
 */
class CSplineInterpolator1D : public mrpt::serialization::CSerializable
{
	DEFINE_SERIALIZABLE(CSplineInterpolator1D, mrpt::math)

   private:
	/** The placeholders for the data */
	std::map<double, double> m_x2y;

	/** Whether to wrap "y" */
	bool m_wrap2pi;

   public:
	/** Constructor with optional initial values. */
	template <class VECTOR>
	inline CSplineInterpolator1D(
		const VECTOR& initial_x, const VECTOR& initial_y, bool wrap2pi = false)
		: m_wrap2pi(wrap2pi)
	{
		setXY(initial_x, initial_y);
	}

	/** Constructor */
	CSplineInterpolator1D(bool wrap2pi = false);

	/** If set to true, the interpolated data will be wrapped to ]-pi,pi] */
	void setWrap2pi(bool wrap) { m_wrap2pi = wrap; }
	/** Return the wrap property */
	bool getWrap2pi() { return m_wrap2pi; }
	/** Set all the data at once .
	 *  The vectors must have the same length.
	 */
	template <class VECTOR>
	void setXY(
		const VECTOR& x, const VECTOR& y, bool clearPreviousContent = true)
	{
		MRPT_START
		if (clearPreviousContent) m_x2y.clear();
		ASSERT_EQUAL_(x.size(), y.size());
		const size_t n = size_t(x.size());
		for (size_t i = 0; i < n; i++)
			m_x2y[x[i]] = y[i];
		MRPT_END
	}

	/** Append a new point: */
	void appendXY(double x, double y);

	/** Clears all stored points */
	void clear() { m_x2y.clear(); }
	/** Query an interpolation of the curve at some "x".
	 *   The result is stored in "y". If the "x" point is out of range,
	 * "valid_out" is set to false.
	 *  \return A reference to "y"
	 * \sa queryVector
	 */
	double& query(double x, double& y, bool& out_valid) const;

	/** As query, but for a whole vector at once.
	 *  \return false if there is at least one value that couldn't be
	 * interpolated (in this case the output is indeterminate).
	 * \sa query
	 */
	template <class VECTOR1, class VECTOR2>
	bool queryVector(const VECTOR1& x, VECTOR2& out_y) const
	{
		const size_t n = size_t(x.size());
		out_y.resize(n);
		bool valid, anyValid = false;
		for (size_t i = 0; i < n; i++)
		{
			query(x[i], out_y[i], valid);
			if (valid) anyValid = true;
		}
		return anyValid;
	}
};

}  // namespace mrpt::math
