/*
								+----------------------------------+
								|                                  |
								|  *** Common spline template ***  |
								|                                  |
								|   Copyright © -tHE SWINe- 2008   |
								|                                  |
								|             Spline.h             |
								|                                  |
								+----------------------------------+
*/

#pragma once
#ifndef __GENERIC_SPLINE_TEMPLATE_INCLUDED
#define __GENERIC_SPLINE_TEMPLATE_INCLUDED

/**
 *	@file lml/Spline.h
 *	@date 2008
 *	@author -tHE SWINe-
 *	@brief common spline template
 *
 *	@date 2009-01-15
 *
 *	fixed error in CSpline::ErasePoints() where cached lengths
 *	were erased instead of the points themselves
 *
 *	@date 2009-05-23
 *
 *	removed all instances of std::vector::reserve and replaced them by stl_ut::Reserve_*
 *
 *	@date 2009-10-20
 *
 *	fixed some warnings when compiling under VC 2005, implemented "Security
 *	Enhancements in the CRT" for VC 2008. compare against MyProjects_2009-10-19_
 *
 *	@date 2012-06-19
 *
 *	Moved multiple inclusion guard before file documentation comment.
 *
 *	@date 2014-03-18
 *
 *	Made difference between the key points the spline is defined on and the domain points
 *	the spline outputs (for e.g. Bezier splines those are the same, for rational bezier or
 *	e.g. Kochanek-Bartels, key-points are positions + parameters (weights, continuity, ...),
 *	domain points are only positions).
 *
 *	Added CSplineSampler_TimeTransform to be able to e.g. sample only a part of a spline,
 *	or to turn the direction of a spline.
 *
 */

#include "../NewFix.h"
#include "../CallStack.h"
#include <vector>
#include "../Numerical.h"

/**
 *	@brief spline traits class
 *	@tparam CSplineImpl is spline implementation type
 */
template <class CSplineImpl>
class CSplineTraits {
public:
	/**
	 *	@brief spline traits, stored as enum
	 */
	enum {
		b_has_explicit_time = CSplineImpl::b_has_explicit_time, /**< @brief if set, the spline keypoints have f_time member */
		b_has_rotation = CSplineImpl::b_has_rotation /**< @brief the spline keypoints do not have t_rotation member */
	};
};

/**
 *	@brief simple generic spline storage template
 *
 *	@tparam CPointClass is type of points the spline outputs
 *	@tparam CKeyPointClass is type of key points the spline is defined on
 *	@tparam CSplineImpl is a particular spline implementation
 */
template <class CPointClass, class CKeyPointClass, class CSplineImpl>
class CSplineBase : public CSplineImpl {
public:
	typedef CPointClass _TyPoint; /**< @brief type of points the spline outputs */
	typedef typename CPointClass::_TyScalar _TyScalar; /**< @brief type of scalar */
	typedef CKeyPointClass _TyKeyPoint; /**< @brief type of key points the spline is defined on */
	typedef CSplineTraits<CSplineImpl> _TyTraits; /**< @brief spline traits */

protected:
	/**
	 *	@brief tangent magnitude adaptor for integrator (gives arc length)
	 */
	class CTangentMag {
	protected:
		const CSplineBase &m_r_spline;
		size_t m_n_arc;

	public:
		inline CTangentMag(const CSplineBase &r_spline, size_t n_arc)
			:m_r_spline(r_spline), m_n_arc(n_arc)
		{}

		inline _TyScalar operator ()(_TyScalar f_t) const
		{
			return m_r_spline.v_Arc_Derivative(f_t, m_n_arc).f_Length();
		}
	};

public:
	/**
	 *	@brief default constructor (has no effect)
	 */
	inline CSplineBase()
	{}

	/**
	 *	@brief constructor
	 *	@param[in] r_point_list is list of spline points, it is copied locally
	 *	@note This function throws std::bad_alloc.
	 */
	inline CSplineBase(const std::vector<_TyKeyPoint> &r_point_list) // throw(std::bad_alloc)
	{
		m_point_list.insert(m_point_list.end(), r_point_list.begin(), r_point_list.end());
		// copy points
	}

	/**
	 *	@brief constructor
	 *
	 *	@param[in] p_point_begin is iterator pointing to the first spline point
	 *	@param[in] p_point_end is iterator pointing to one past the last spline point
	 *
	 *	@note This function throws std::bad_alloc.
	 */
	template <class CPointIter>
	inline CSplineBase(CPointIter p_point_begin, CPointIter p_point_end) // throw(std::bad_alloc)
	{
		m_point_list.insert(m_point_list.end(), p_point_begin, p_point_end);
		// copy points
	}

	/**
	 *	@brief copy-constructor
	 *	@param[in] r_other is spline to copy from
	 *	@return Returns reference to this.
	 *	@note This function throws std::bad_alloc.
	 */
	inline CSplineBase(const CSplineBase &r_other) // throw(std::bad_alloc)
	{
		m_point_list.insert(m_point_list.end(),
			r_other.m_point_list.begin(), r_other.m_point_list.end());
		// copy points
	}

	/**
	 *	@brief copy-operator
	 *	@param[in] r_other is spline to copy from
	 *	@return Returns reference to this.
	 *	@note This function throws std::bad_alloc.
	 */
	inline CSplineBase &operator =(const CSplineBase &r_other) // throw(std::bad_alloc)
	{
		m_point_list.clear();
		m_point_list.insert(m_point_list.end(),
			r_other.m_point_list.begin(), r_other.m_point_list.end());
		// copy points

		return *this;
	}

	/**
	 *	@brief gets number of spline keypoints
	 *	@return Returns number of spline keypoints.
	 */
	inline size_t n_Point_Num() const
	{
		return m_point_list.size();
	}

	/**
	 *	@brief determines whether spline contains any keypoints at all
	 *	@return Returns true if the spline contains one or more keypoints, otherwise returns false.
	 */
	inline bool b_Empty() const
	{
		return m_point_list.empty();
	}

	/**
	 *	@brief gets a spline keypoint
	 *	@param[in] n_index is zero-based index of the spline keypoint
	 *	@return Returns const reference to the selected spline keypoint.
	 */
	inline const _TyKeyPoint &r_Point(size_t n_index) const
	{
		return m_point_list[n_index];
	}

	/**
	 *	@brief gets a spline keypoint
	 *	@param[in] n_index is zero-based index of the spline keypoint
	 *	@return Returns reference to the selected spline keypoint.
	 *	@note This resets cached spline segment lengths.
	 */
	inline _TyKeyPoint &r_Point(size_t n_index)
	{
		return m_point_list[n_index];
	}

	/**
	 *	@brief erases keypoint inside a range
	 *
	 *	@param[in] n_begin is zero-based index of the first point to be erased
	 *	@param[in] n_end is zero-based index of one past the last point
	 *		to be erased, or -1 as all points until the end
	 */
	inline void Erase(size_t n_begin, size_t n_end)
	{
		if(n_end != size_t(-1)) {
			_ASSERTE(n_begin <= n_end);
			m_point_list.erase(m_point_list.begin() + n_begin,
				m_point_list.begin() + n_end);
		} else {
			m_point_list.erase(m_point_list.begin() + n_begin,
				m_point_list.end());
		}
		// erase points
	}

	/**
	 *	@brief erases a single keypoint
	 *	@param[in] n_point is zero-based index of the point to be erased
	 */
	inline void Erase(size_t n_point)
	{
		m_point_list.erase(m_point_list.begin() + n_point);
		// erase the selected point
	}

	/**
	 *	@brief erases all spline keypoint
	 */
	inline void Erase()
	{
		m_point_list.clear();
	}

	/**
	 *	@brief inserts a range of keypoints and places them at a specified location
	 *
	 *	@param[in] n_insert_before is zero-based index of spline keypoint
	 *		where to insert the new keypoints
	 *	@param[in] p_point_begin is iterator pointing to the first inserted keypoint
	 *	@param[in] p_point_end is iterator pointing to one past the last inserted keypoint
	 *
	 *	@note This function throws std::bad_alloc.
	 */
	template <class CPointIter>
	void Insert(size_t n_insert_before, CPointIter p_point_begin, CPointIter p_point_end) // throw(std::bad_alloc)
	{
		m_point_list.insert(m_point_list.begin() + n_insert_before, p_point_begin, p_point_end);
		// add new points
	}

	/**
	 *	@brief inserts an array of keypoints and places them at the end of the storage
	 *
	 *	@param[in] p_point_begin is iterator pointing to the first inserted keypoint
	 *	@param[in] p_point_end is iterator pointing to one past the last inserted keypoint
	 *
	 *	@note This function throws std::bad_alloc.
	 */
	template <class CPointIter>
	void InsertBack(CPointIter p_point_begin, CPointIter p_point_end) // throw(std::bad_alloc)
	{
		m_point_list.insert(m_point_list.end(), p_point_begin, p_point_end);
		// add new points
	}

	/**
	 *	@brief inserts a single keypoint at a specified location
	 *
	 *	@param[in] n_insert_before is zero-based index of spline keypoint
	 *		where to insert the new keypoint
	 *	@param[in] t_point is value of the keypoint to be inserted 
	 *
	 *	@note This function throws std::bad_alloc.
	 */
	inline void Insert(size_t n_insert_before, _TyKeyPoint t_point) // throw(std::bad_alloc)
	{
		m_point_list.insert(m_point_list.begin() + n_insert_before, t_point);
	}

	/**
	 *	@brief inserts a single keypoint at the end
	 *	@param[in] t_point is value of the keypoint to be inserted 
	 *	@note This function throws std::bad_alloc.
	 */
	inline void PushBack(_TyKeyPoint t_point) // throw(std::bad_alloc)
	{
		m_point_list.push_back(t_point);
	}

	/**
	 *	@brief calculates spline length
	 *	@param[in] f_max_error is maximum relative error
	 *	@return Returns spline length with the specified maximal error.
	 *	@note This calls f_Arc_Length() in a loop, not particularly efficient.
	 */
	_TyScalar f_Length(_TyScalar f_max_error = 1e-3f) const
	{
		size_t n_arc_num;
		if(!(n_arc_num = n_Arc_Num()))
			return 0;

		f_max_error /= n_arc_num; // this makes it more precise, not less
		// error multiplies with the number of arcs

		_TyScalar f_length = 0;
		for(size_t i = 0, n = n_arc_num; i < n; ++ i)
			f_length += f_Arc_Length(i, f_max_error);
		// sum up arc lengths, don't cache them

		return f_length;
	}

	/**
	 *	@brief calculates spline length and returns arc lengths
	 *
	 *	@param[out] r_arc_length is filled with arc lengths upon return
	 *	@param[in] f_max_error is maximum relative error
	 *
	 *	@return Returns spline length with the specified maximal error.
	 *
	 *	@note This calls f_Arc_Length() in a loop, not particularly efficient.
	 *	@note This function throws std::bad_alloc.
	 */
	_TyScalar f_Length(std::vector<_TyScalar> &r_arc_length, _TyScalar f_max_error = 1e-3f) const // throw(std::bad_alloc)
	{
		size_t n_arc_num;
		if(!(n_arc_num = n_Arc_Num()))
			return 0;

		f_max_error /= n_arc_num; // this makes it more precise, not less
		// error multiplies with the number of arcs

		_TyScalar f_length = 0;
		for(size_t i = 0, n = n_arc_num; i < n; ++ i) {
			_TyScalar f_arc = f_Arc_Length(i, f_max_error);
			r_arc_length.push_back(f_arc);
			f_length += f_arc;
		}
		// sum up arc lengths, don't cache them

		return f_length;
	}

	/**
	 *	@brief calculates spline arc length
	 *
	 *	@param[in] n_arc is zero-based index of spline arc
	 *	@param[in] f_max_error is maximum relative error
	 *
	 *	@return Returns length of the selected spline arc with the specified maximal error.
	 */
	_TyScalar f_Arc_Length(size_t n_arc, _TyScalar f_max_error = 1e-3f) const
	{
		_ASSERTE(n_arc < n_Arc_Num());
		return f_Integrate(CTangentMag(*this, n_arc), _TyScalar(0), _TyScalar(1), f_max_error);
	}

	/**
	 *	@brief calculates index of arc and a local position given position on spline
	 *
	 *	@param[in] f_t is position on the spline in range [0, 1]
	 *	@param[out] r_f_local_t is local position along the particular arc (range [0, 1])
	 *	@param[in] f_spline_length is total spline length
	 *	@param[in] r_arc_length is a list of lengths of all the spline arcs
	 *
	 *	@return Returns (zero-based) index of arc at the given position along the spline.
	 */
	size_t n_Arc(_TyScalar f_t, _TyScalar &r_f_local_t, _TyScalar f_spline_length,
		const std::vector<_TyScalar> &r_arc_length) const
	{
		size_t n_arc_num;
		if(!(n_arc_num = n_Arc_Num()))
			return size_t(-1);
		_ASSERTE(r_arc_length.size() == n_arc_num);

		if(f_t <= 0) {
			r_f_local_t = 0;
			return 0;
		} else if(f_t >= 1) {
			r_f_local_t = 1;
			return n_arc_num - 1;
		}
		// clamp t to range [0, 1]

		_TyScalar f_dist = f_t * f_spline_length/*f_Length(f_max_error)*/;
		for(size_t i = 0, n = n_arc_num; i < n; ++ i) {
			_TyScalar f_arc_len = r_arc_length[i]/*f_Arc_Length(i, f_max_error)*/;
			if(f_dist < f_arc_len) {
				r_f_local_t = f_dist / f_arc_len;
				return i;
			}
			f_dist -= f_arc_len;
		}
		// find arc by spline length

		r_f_local_t = 1;
		return n_arc_num - 1;
		// last arc was hit (should happen quite seldom)
	}

	/**
	 *	@brief gets number of spline arcs
	 *	@return Returns number of spline arcs.
	 *	@note this function is supposed to be implemented in the final spline class
	 */
	inline size_t n_Arc_Num() const
	{
		return CSplineImpl::n_Arc_Num(); // these do not have to be virtual anymore
	}

	/**
	 *	@brief calculates position on a spline arc
	 *
	 *	@param[in] f_t is position on a spline arc in range [0, 1]
	 *	@param[in] n_arc is zero-based index of spline arc
	 *
	 *	@return Returns point at the given position along the selected arc.
	 */
	_TyPoint v_Arc_Position(_TyScalar f_t, size_t n_arc) const // no need to be virtual
	{
		return CSplineImpl::v_Arc_Position(f_t, n_arc); // these do not have to be virtual anymore
	}

	/**
	 *	@brief calculates tangent vector on a spline arc
	 *
	 *	@param[in] f_t is position on a spline arc in range [0, 1]
	 *	@param[in] n_arc is zero-based index of spline arc
	 *
	 *	@return Returns tangent at the given position along the selected arc.
	 */
	_TyPoint v_Arc_Derivative(_TyScalar f_t, size_t n_arc) const // no need to be virtual
	{
		return CSplineImpl::v_Arc_Derivative(f_t, n_arc); // these do not have to be virtual anymore
	}
};

/**
 *	@brief simple spline interface template
 *
 *	@tparam CSplineImpl is a particular spline implementation
 */
template <class CSplineImpl>
class CSplineIface : public CSplineBase<typename CSplineImpl::_TyPoint,
	typename CSplineImpl::_TyKeyPoint, CSplineImpl> {
public:
	typedef typename CSplineImpl::_TyPoint _TyPoint; /**< @brief type of points the spline outputs */
	typedef typename CSplineImpl::_TyKeyPoint _TyKeyPoint; /**< @brief type of key points the spline is defined on */
	typedef CSplineImpl _TySplineImpl; /**< @brief spline implementation name */

public:
	/**
	 *	@copydoc CSplineBase::CSplineBase()
	 */
	inline CSplineIface()
		:CSplineBase<_TyPoint, _TyKeyPoint, _TySplineImpl>()
	{}

	/**
	 *	@copydoc CSplineBase::CSplineBase(const std::vector<_TyKeyPoint>&)
	 */
	inline CSplineIface(const std::vector<_TyKeyPoint> &r_point_list) // throw(std::bad_alloc)
		:CSplineBase<_TyPoint, _TyKeyPoint, _TySplineImpl>(r_point_list)
	{}

	/**
	 *	@copydoc CSplineBase::CSplineBase(CPointIter,CPointIter)
	 */
	template <class CPointIter>
	inline CSplineIface(CPointIter p_point_begin, CPointIter p_point_end) // throw(std::bad_alloc)
		:CSplineBase<_TyPoint, _TyKeyPoint, _TySplineImpl>(p_point_begin, p_point_end)
	{}

	/**
	 *	@copydoc CSplineBase::CSplineBase(const CSplineBase&)
	 */
	inline CSplineIface(const CSplineIface &r_other) // throw(std::bad_alloc)
		:CSplineBase<_TyPoint, _TyKeyPoint, _TySplineImpl>(r_other)
	{}

	/**
	 *	@copydoc CSplineBase::operator=()
	 */
	inline CSplineIface &operator =(const CSplineIface &r_other) // throw(std::bad_alloc)
	{
		*(CSplineBase<_TyPoint, _TyKeyPoint, _TySplineImpl>*)this = r_other;
		return *this;
	}

	/**
	 *	@brief calculates point at a given position along the spline (uniform subdivision)
	 *	@param[in] f_t is position on the spline in range [0, 1]
	 *	@return Returns point at the specified position along the spline.
	 *	@note This doesn't use any kind or approximation of constant step
	 *		subdivision, each spline segment is considered unit length
	 */
	_TyPoint v_Position_Uniform(_TyScalar f_t) const
	{
		size_t n_arc_num = CSplineBase<_TyPoint, _TyKeyPoint, _TySplineImpl>::n_Arc_Num();
		size_t n_arc = (f_t > 0)? ((f_t < 1)? size_t(f_t * n_arc_num) : n_arc_num - 1) : 0;
		_TyScalar f_arc_t = f_t * n_arc_num - n_arc;
		return CSplineBase<_TyPoint, _TyKeyPoint, _TySplineImpl>::v_Arc_Position(f_arc_t, n_arc);
	}

	/**
	 *	@brief calculates tangent vector at a given position along the spline (uniform subdivision)
	 *	@param[in] f_t is position on the spline in range [0, 1]
	 *	@return Returns tangent at the specified position along the spline.
	 *	@note This doesn't use any kind or approximation of constant step
	 *		subdivision, each spline segment is considered unit length
	 */
	_TyPoint v_Derivative_Uniform(_TyScalar f_t) const
	{
		size_t n_arc_num = CSplineBase<_TyPoint, _TyKeyPoint, _TySplineImpl>::n_Arc_Num();
		size_t n_arc = (f_t > 0)? ((f_t < 1)? size_t(f_t * n_arc_num) : n_arc_num - 1) : 0;
		_TyScalar f_arc_t = f_t * n_arc_num - n_arc;
		return CSplineBase<_TyPoint, _TyKeyPoint, _TySplineImpl>::v_Arc_Derivative(f_arc_t, n_arc);
	}

	/**
	 *	@brief calculates point at a given position along the spline (semi-constant step subdivision)
	 *	@param[in] f_t is position on the spline in range [0, 1]
	 *	@return Returns point at the specified position along the spline.
	 *	@note Semi-constant step means arc lengths are used to determine spline arc
	 *		on position f_t, but the arc itself is subdivided with constant step.
	 */
	_TyPoint v_Position_SCS(_TyScalar f_t, _TyScalar f_spline_length,
		const std::vector<_TyScalar> &r_arc_length) const
	{
		_TyScalar f_arc_t;
		size_t n_arc = CSplineBase<_TyPoint, _TyKeyPoint, _TySplineImpl>::n_Arc(f_t, f_arc_t, f_spline_length, r_arc_length);
		return CSplineBase<_TyPoint, _TyKeyPoint, _TySplineImpl>::v_Arc_Position(f_arc_t, n_arc);
	}

	/**
	 *	@brief calculates tangent vector at a given position along the spline (semi-constant step subdivision)
	 *	@param[in] f_t is position on the spline in range [0, 1]
	 *	@return Returns tangent at the specified position along the spline.
	 *	@note Semi-constant step means arc lengths are used to determine spline arc
	 *		on position f_t, but the arc itself is subdivided with constant step.
	 */
	_TyPoint v_Derivative_SCS(_TyScalar f_t, _TyScalar f_spline_length,
		const std::vector<_TyScalar> &r_arc_length) const
	{
		_TyScalar f_arc_t;
		size_t n_arc = CSplineBase<_TyPoint, _TyKeyPoint, _TySplineImpl>::n_Arc(f_t, f_arc_t, f_spline_length, r_arc_length);
		return CSplineBase<_TyPoint, _TyKeyPoint, _TySplineImpl>::v_Arc_Derivative(f_arc_t, n_arc);
	}

	// todo - add also a new f_Arc_Position() which will calculates position on the arc for constant step
	// todo - add a function that will train some polynomial function (least squares, wow) for constant step correction
	// todo - think about caching spline props, it is not very well done here (maybe split the spline to several classes, base for storage and containing interpolation interface, then more of those to support caching - or maybe even an external structure that would be filled and explicitly passed to the functions (problems with usability in other code, virtual functions and such
	// *** maybe create a simple spline interpolator / sampler interface and then template everything else away - flexible while also fast in scenarios where the spline type is known at compile-time))
};

/**
 *	@brief simple spline interpolator interface
 *	@tparam CPointClass is type of points the spline is defined on
 */
template <class CPointClass>
class CSplineSampler {
public:
	typedef CPointClass _TyPoint; /**< @brief type of points the spline is defined on */
	typedef typename CPointClass::_TyScalar _TyScalar; /**< @brief type of scalar */

public:
	/**
	 *	@brief calculates point at a given position along the spline
	 *	@param[in] f_t is position on the spline in range [0, 1]
	 *	@return Returns point at the specified position along the spline.
	 *	@note This interface does not specify subdivision step,
	 *		which is entirely dependent on the implementation.
	 */
	virtual _TyPoint v_Position(_TyScalar f_t) const = 0;

	/**
	 *	@brief calculates tangent vector at a given position along the spline
	 *	@param[in] f_t is position on the spline in range [0, 1]
	 *	@return Returns tangent at the specified position along the spline.
	 *	@note This interface does not specify subdivision step,
	 *		which is entirely dependent on the implementation.
	 */
	virtual _TyPoint v_Derivative(_TyScalar f_t) const = 0;

	// should there be more? spline length? something else?
};

/**
 *	@brief simple spline interpolator interface
 *	@tparam CPointClass is type of points the spline is defined on
 */
template <class CSplineClass>
class CSplineSampler_UniformStep : public CSplineSampler<typename CSplineClass::_TyPoint> {
public:
	typedef CSplineSampler_UniformStep<CSplineClass> _TySampler; /**< @brief this type */
	typedef CSplineClass _TySpline; /**< @brief type of the spline */
	typedef typename CSplineClass::_TyPoint _TyPoint; /**< @brief type of points the spline is defined on */
	typedef typename _TyPoint::_TyScalar _TyScalar; /**< @brief type of scalar */

protected:
	const _TySpline *m_p_spline; /**< @brief reference to the sampled spline */

public:
	/**
	 *	@brief default constructor; creates sampler on a spline
	 *	@param[in] r_spline is a spline to be sampled
	 */
	inline CSplineSampler_UniformStep(const _TySpline &r_spline)
		:m_p_spline(&r_spline)
	{}

	/**
	 *	@brief copy-constructor (copies spline reference only, spline data is not modified)
	 *	@param[in] r_other is sampler to copy from
	 */
	inline CSplineSampler_UniformStep(const _TySampler &r_other)
		:m_p_spline(r_other.m_p_spline)
	{}

	/**
	 *	@brief copy-operator (copies spline reference only, spline data is not modified)
	 *	@param[in] r_other is sampler to copy from
	 *	@return Returns reference to this.
	 */
	inline CSplineSampler_UniformStep &operator =(const _TySampler &r_other)
	{
		m_p_spline = r_other.m_p_spline;
		return *this;
	}

	/**
	 *	@brief calculates point at a given position along the spline
	 *	@param[in] f_t is position on the spline in range [0, 1]
	 *	@return Returns point at the specified position along the spline.
	 *	@note This interface does not specify subdivision step,
	 *		which is entirely dependent on the implementation.
	 */
	virtual _TyPoint v_Position(_TyScalar f_t) const
	{
		if(!m_p_spline->n_Arc_Num())
			return _TyPoint();
		return m_p_spline->v_Position_Uniform(f_t);
	}

	/**
	 *	@brief calculates tangent vector at a given position along the spline
	 *	@param[in] f_t is position on the spline in range [0, 1]
	 *	@return Returns tangent at the specified position along the spline.
	 *	@note This interface does not specify subdivision step,
	 *		which is entirely dependent on the implementation.
	 */
	virtual _TyPoint v_Derivative(_TyScalar f_t) const
	{
		if(!m_p_spline->n_Arc_Num())
			return _TyPoint();
		return m_p_spline->v_Derivative_Uniform(f_t);
	}
};

/**
 *	@brief simple spline interpolator interface
 *	@tparam CPointClass is type of points the spline is defined on
 */
template <class CSplineClass>
class CSplineSampler_ExplicitTime_UniformStep : public CSplineSampler<typename CSplineClass::_TyPoint> {
public:
	typedef CSplineSampler_UniformStep<CSplineClass> _TySampler; /**< @brief this type */
	typedef CSplineClass _TySpline; /**< @brief type of the spline */
	typedef typename CSplineClass::_TyPoint _TyPoint; /**< @brief type of points the spline is defined on */
	typedef typename _TyPoint::_TyScalar _TyScalar; /**< @brief type of scalar */

protected:
	const _TySpline *m_p_spline; /**< @brief reference to the sampled spline */

public:
	/**
	 *	@brief default constructor; creates sampler on a spline
	 *	@param[in] r_spline is a spline to be sampled
	 */
	inline CSplineSampler_ExplicitTime_UniformStep(const _TySpline &r_spline)
		:m_p_spline(&r_spline)
	{}

	/**
	 *	@brief copy-constructor (copies spline reference only, spline data is not modified)
	 *	@param[in] r_other is sampler to copy from
	 */
	inline CSplineSampler_ExplicitTime_UniformStep(const _TySampler &r_other)
		:m_p_spline(r_other.m_p_spline)
	{}

	/**
	 *	@brief copy-operator (copies spline reference only, spline data is not modified)
	 *	@param[in] r_other is sampler to copy from
	 *	@return Returns reference to this.
	 */
	inline CSplineSampler_ExplicitTime_UniformStep &operator =(const _TySampler &r_other)
	{
		m_p_spline = r_other.m_p_spline;
		return *this;
	}

	/**
	 *	@brief calculates point at a given position along the spline
	 *	@param[in] f_t is position on the spline (in spline time domain)
	 *	@return Returns point at the specified position along the spline.
	 *	@note This interface does not specify subdivision step,
	 *		which is entirely dependent on the implementation.
	 */
	virtual _TyPoint v_Position(_TyScalar f_t) const
	{
		if(!m_p_spline->n_Arc_Num())
			return _TyPoint();
		size_t n_arc = n_Find_Arc(f_t);
		return m_p_spline->v_Arc_Position(f_t, n_arc);
	}

	/**
	 *	@brief calculates tangent vector at a given position along the spline
	 *	@param[in] f_t is position on the spline (in spline time domain)
	 *	@return Returns tangent at the specified position along the spline.
	 *	@note This interface does not specify subdivision step,
	 *		which is entirely dependent on the implementation.
	 */
	virtual _TyPoint v_Derivative(_TyScalar f_t) const
	{
		if(!m_p_spline->n_Arc_Num())
			return _TyPoint();
		size_t n_arc = n_Find_Arc(f_t);
		return m_p_spline->v_Arc_Derivative(f_t, n_arc);
	}

	/**
	 *	@brief finds a spline arc, given sample time
	 *	@param[in,out] r_f_t is position on the spline (in spline time domain)
	 *		overwritten by arc position in range [0, 1]
	 *	@return Returns zero-based index of the selected spline arc.
	 */
	size_t n_Find_Arc(_TyScalar &r_f_t) const
	{
		_TyScalar f_t = r_f_t;
		if(f_t < 0)
			return 0;
		for(size_t i = 0, n = m_p_spline->n_Arc_Num(); i < n; ++ i) {
			_TyScalar f_len = m_p_spline->f_Arc_Time(i);
			if(f_t <= f_len || i + 1 == n) {
				r_f_t = f_t / f_len; // to be between 0 and 1
				return i;
			} else
				f_t -= f_len;
		}

		return size_t(-1);
	}
};

/**
 *	@brief simple spline interpolator interface
 *	@tparam CPointClass is type of points the spline is defined on
 */
template <class CSplineClass>
class CSplineSampler_SCSStep : public CSplineSampler<typename CSplineClass::_TyPoint> {
public:
	typedef CSplineSampler_SCSStep<CSplineClass> _TySampler; /**< @brief this type */
	typedef CSplineClass _TySpline; /**< @brief type of the spline */
	typedef typename CSplineClass::_TyPoint _TyPoint; /**< @brief type of points the spline is defined on */
	typedef typename _TyPoint::_TyScalar _TyScalar; /**< @brief scalar type */

protected:
	const _TySpline *m_p_spline; /**< @brief reference to the sampled spline */
	_TyScalar m_f_spline_length;
	std::vector<_TyScalar> m_arc_length;

public:
	/**
	 *	@brief default constructor; creates sampler on a spline
	 *	@param[in] r_spline is a spline to be sampled
	 *	@param[in] f_max_error is maximum relative error for arc length computation
	 *	@note This function throws std::bad_alloc.
	 */
	inline CSplineSampler_SCSStep(const _TySpline &r_spline,
		_TyScalar f_max_error = _TyScalar(1e-3)) // throw(std::bad_alloc)
		:m_p_spline(&r_spline), m_f_spline_length(0)
	{
		size_t n_arc_num = r_spline.n_Arc_Num();
		m_arc_length.resize(n_arc_num);
		for(size_t i = 0; i < n_arc_num; ++ i)
			m_f_spline_length += (m_arc_length[i] = r_spline.f_Arc_Length(i, f_max_error));
	}

	/**
	 *	@brief copy-constructor (copies spline reference only, spline data is not modified)
	 *	@param[in] r_other is sampler to copy from
	 *	@note This function throws std::bad_alloc.
	 */
	inline CSplineSampler_SCSStep(const _TySampler &r_other) // throw(std::bad_alloc)
		:m_p_spline(r_other.m_p_spline), m_f_spline_length(r_other.m_f_spline_length),
		m_arc_length(r_other.m_arc_length)
	{}

	/**
	 *	@brief copy-operator (copies spline reference only, spline data is not modified)
	 *	@param[in] r_other is sampler to copy from
	 *	@return Returns reference to this.
	 *	@note This function throws std::bad_alloc.
	 */
	inline CSplineSampler_SCSStep &operator =(const _TySampler &r_other) // throw(std::bad_alloc)
	{
		m_p_spline = r_other.m_p_spline;
		m_f_spline_length = r_other.m_f_spline_length;
		m_arc_length = r_other.m_arc_length;
		return *this;
	}

	/**
	 *	@brief calculates point at a given position along the spline
	 *	@param[in] f_t is position on the spline in range [0, 1]
	 *	@return Returns point at the specified position along the spline.
	 *	@note This interface does not specify subdivision step,
	 *		which is entirely dependent on the implementation.
	 */
	virtual _TyPoint v_Position(_TyScalar f_t) const
	{
		if(!m_p_spline->n_Arc_Num())
			return _TyPoint();
		return m_p_spline->v_Position_SCS(f_t, m_f_spline_length, m_arc_length);
	}

	/**
	 *	@brief calculates tangent vector at a given position along the spline
	 *	@param[in] f_t is position on the spline in range [0, 1]
	 *	@return Returns tangent at the specified position along the spline.
	 *	@note This interface does not specify subdivision step,
	 *		which is entirely dependent on the implementation.
	 */
	virtual _TyPoint v_Derivative(_TyScalar f_t) const
	{
		if(!m_p_spline->n_Arc_Num())
			return _TyPoint();
		return m_p_spline->v_Derivative_SCS(f_t, m_f_spline_length, m_arc_length);
	}
};

/**
 *	@brief time transformation spline sampler wrapper
 *	@tparam CBaseSampler is sampler this sampler is wrapped arround
 */
template <class CBaseSampler>
class CSplineSampler_TimeTransform : public CSplineSampler<typename CBaseSampler::_TyPoint> {
public:
	typedef CBaseSampler _TyBaseSampler; /**< @brief base sampler type */
	typedef typename _TyBaseSampler::_TySpline _TySpline; /**< @brief type of the spline */
	typedef CSplineSampler_TimeTransform<_TySpline> _TySampler; /**< @brief this type */
	typedef typename _TySpline::_TyPoint _TyPoint; /**< @brief type of points the spline is defined on */
	typedef typename _TyPoint::_TyScalar _TyScalar; /**< @brief scalar type */

protected:
	const CBaseSampler *m_p_sampler; /**< @brief base sampler reference */
	_TyScalar m_f_offset; /**< @brief time offset */
	_TyScalar m_f_scale; /**< @brief time scale */

public:
	/**
	 *	@brief default constructor; wraps a sampler with time transformation
	 *
	 *	To turn direction of the spline, use offset 1 and scale -1.
	 *	To sample from time <tt>a</tt> to time <tt>b</tt>, use offset
	 *	<tt>a</tt> and scale <tt>1 / (b - a)</tt>.
	 *
	 *	@param[in] r_sampler is reference to the parent sampler (the sampler must not be deleted)
	 *	@param[in] f_offset is time offset (default 0)
	 *	@param[in] f_scale is time scale (default 1)
	 */
	CSplineSampler_TimeTransform(const CBaseSampler &r_sampler, _TyScalar f_offset = 0, _TyScalar f_scale = 1)
		:m_p_sampler(&r_sampler), m_f_offset(f_offset), m_f_scale(f_scale)
	{}

	/**
	 *	@brief copy-constructor
	 *	@param[in] r_other is sampler wrapper to be copied
	 */
	CSplineSampler_TimeTransform(const CSplineSampler_TimeTransform &r_other)
		:m_p_sampler(r_other.m_p_sampler), m_f_offset(r_other.m_f_offset), m_f_scale(r_other.m_f_scale)
	{}

	/**
	 *	@brief copy-operator
	 *	@param[in] r_other is sampler wrapper to be copied
	 *	@return Returns reference to this.
	 */
	CSplineSampler_TimeTransform &operator =(const CSplineSampler_TimeTransform &r_other)
	{
		m_p_sampler = r_other.m_p_sampler;
		m_f_offset = r_other.m_f_offset;
		m_f_scale = r_other.m_f_scale;
		return *this;
	}

	/**
	 *	@brief gets time offset
	 *	@return Returns time offset.
	 */
	inline const _TyScalar f_Time_Offset() const
	{
		return m_f_offset;
	}

	/**
	 *	@brief gets time scale
	 *	@return Returns time scale.
	 */
	inline const _TyScalar f_Time_Scale() const
	{
		return m_f_scale;
	}

	/**
	 *	@brief gets time offset
	 *	@return Returns reference to the time offset.
	 */
	inline _TyScalar &f_Time_Offset()
	{
		return m_f_offset;
	}

	/**
	 *	@brief gets time scale
	 *	@return Returns reference to the time scale.
	 */
	inline _TyScalar &f_Time_Scale()
	{
		return m_f_scale;
	}

	/**
	 *	@brief copydoc CSplineSampler::v_Position()
	 */
	virtual _TyPoint v_Position(_TyScalar f_t) const
	{
		return m_p_sampler->v_Position(m_f_offset + m_f_scale * f_t);
	}

	/**
	 *	@brief copydoc CSplineSampler::v_Derivative()
	 */
	virtual _TyPoint v_Derivative(_TyScalar f_t) const
	{
		return m_p_sampler->v_Derivative(m_f_offset + m_f_scale * f_t);
	}
};

/**
 *	@brief spatial transformation spline sampler wrapper
 *	@tparam CBaseSampler is sampler this sampler is wrapped arround
 */
template <class CBaseSampler>
class CSplineSampler_Transform : public CSplineSampler<typename CBaseSampler::_TyPoint> {
public:
	typedef CBaseSampler _TyBaseSampler; /**< @brief base sampler type */
	typedef typename _TyBaseSampler::_TySpline _TySpline; /**< @brief type of the spline */
	typedef CSplineSampler_TimeTransform<_TySpline> _TySampler; /**< @brief this type */
	typedef typename _TySpline::_TyPoint _TyPoint; /**< @brief type of points the spline is defined on */
	typedef typename _TyPoint::_TyScalar _TyScalar; /**< @brief scalar type */

	typedef typename CMatrixTraits<_TyScalar, _TyPoint::n_row_num,
		_TyPoint::n_row_num>::_Ty _TyRotation; // todo - doc, commit
	typedef _TyPoint _TyOffset;

protected:
	const CBaseSampler *m_p_sampler; /**< @brief base sampler reference */
	_TyRotation m_t_rotation; /**< @brief rotation */
	_TyOffset m_v_offset; /**< @brief spatial offset */

public:
	/**
	 *	@brief default constructor; wraps a sampler with time transformation
	 *
	 *	To turn direction of the spline, use offset 1 and scale -1.
	 *	To sample from time <tt>a</tt> to time <tt>b</tt>, use offset
	 *	<tt>a</tt> and scale <tt>1 / (b - a)</tt>.
	 *
	 *	@param[in] r_sampler is reference to the parent sampler (the sampler must not be deleted)
	 *	@param[in] t_rotation is spatial rotation
	 *	@param[in] v_offset is spatial offset
	 */
	CSplineSampler_Transform(const CBaseSampler &r_sampler, _TyRotation t_rotation, _TyOffset v_offset)
		:m_p_sampler(&r_sampler), m_t_rotation(t_rotation), m_v_offset(v_offset)
	{}

	/**
	 *	@brief copy-constructor
	 *	@param[in] r_other is sampler wrapper to be copied
	 */
	CSplineSampler_Transform(const CSplineSampler_Transform &r_other)
		:m_p_sampler(r_other.m_p_sampler), m_t_rotation(r_other.m_t_rotation),
		m_v_offset(r_other.m_v_offset)
	{}

	/**
	 *	@brief copy-operator
	 *	@param[in] r_other is sampler wrapper to be copied
	 *	@return Returns reference to this.
	 */
	CSplineSampler_Transform &operator =(const CSplineSampler_Transform &r_other)
	{
		m_p_sampler = r_other.m_p_sampler;
		m_v_offset = r_other.m_v_offset;
		m_t_rotation = r_other.m_t_rotation;
		return *this;
	}

	inline const _TyRotation &t_Rotation() const
	{
		return m_t_rotation;
	}

	inline _TyRotation &t_Rotation()
	{
		return m_t_rotation;
	}

	inline const _TyPoint &v_Offset() const
	{
		return m_v_offset;
	}

	inline _TyPoint &v_Offset()
	{
		return m_v_offset;
	}

	/**
	 *	@brief copydoc CSplineSampler::v_Position()
	 */
	virtual _TyPoint v_Position(_TyScalar f_t) const
	{
		return m_t_rotation * m_p_sampler->v_Position(f_t) + m_v_offset;
	}

	/**
	 *	@brief copydoc CSplineSampler::v_Derivative()
	 */
	virtual _TyPoint v_Derivative(_TyScalar f_t) const
	{
		return m_t_rotation * m_p_sampler->v_Derivative(f_t) + m_v_offset;
	}
};

#endif // !__GENERIC_SPLINE_TEMPLATE_INCLUDED
