/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2023, 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/containers/vector_with_small_size_optimization.h>
#include <mrpt/core/exceptions.h>  // ASSERT_()
#include <mrpt/math/MatrixVectorBase.h>
#include <mrpt/math/math_frwds.h>
#include <mrpt/math/matrix_size_t.h>
#include <mrpt/serialization/serialization_frwds.h>
#include <mrpt/typemeta/TTypeName.h>

#include <cstring>	// memset()
#include <type_traits>

namespace mrpt::math
{
/** Template for column vectors of dynamic size, compatible with Eigen.
 *
 * \note For a complete introduction to Matrices and vectors in MRPT, see:
 *http://www.mrpt.org/Matrices_vectors_arrays_and_Linear_Algebra_MRPT_and_Eigen_classes
 * \sa CVectorDynamic, CMatrixFixed, CVectorFixed
 * \ingroup mrpt_math_grp
 */
template <class T>
class CVectorDynamic : public MatrixVectorBase<T, CVectorDynamic<T>>
{
   protected:
	static constexpr size_t small_size = 16;
	using vec_t = mrpt::containers::vector_with_small_size_optimization<
		T, small_size, MRPT_MAX_STATIC_ALIGN_BYTES>;

	vec_t m_data;

   public:
	/** @name Matrix type definitions
	 * @{ */
	/** The type of the matrix elements */
	using value_type = T;
	using Scalar = T;
	using Index = int;
	using reference = T&;
	using const_reference = const T&;
	using size_type = int;
	using difference_type = std::ptrdiff_t;
	constexpr static int RowsAtCompileTime = -1;
	constexpr static int ColsAtCompileTime = 1;
	constexpr static int is_mrpt_type = 1;
	using eigen_t = Eigen::Matrix<T, -1, 1, 0, -1, 1>;
	/** @} */

	/** @name Iterators interface
	 * @{ */
	using iterator = typename vec_t::iterator;
	using const_iterator = typename vec_t::const_iterator;
	iterator begin() { return m_data.begin(); }
	iterator end() { return m_data.end(); }
	const_iterator begin() const { return m_data.begin(); }
	const_iterator end() const { return m_data.end(); }
	const_iterator cbegin() const { return m_data.begin(); }
	const_iterator cend() const { return m_data.end(); }
	/** @} */

	/** Internal use only: It reallocs the memory for the 2D matrix, maintaining
	 * the previous contents if posible.
	 */
	void realloc(size_t new_len, bool newElementsToZero = false)
	{
		const auto old_len = m_data.size();
		if (new_len == old_len) return;
		m_data.resize(new_len);
		if (newElementsToZero && new_len > old_len)
		{
			if constexpr (std::is_trivial_v<T>)
				::memset(&m_data[old_len], 0, sizeof(T) * (new_len - old_len));
			else
				for (size_t k = old_len; k < new_len; k++)
					m_data[k] = T();
		}
	}

   public:
	void swap(CVectorDynamic<T>& o) { m_data.swap(o.m_data); }

	CVectorDynamic() = default;

	/** Initializes to a vector of "N" zeros */
	CVectorDynamic(size_t N, bool initZero = true) { realloc(N, initZero); }

	/** Copy (casting from if needed) from another matrix  */
	template <typename U>
	explicit CVectorDynamic(const CVectorDynamic<U>& m)
	{
		(*this) = m;
	}

	/** Ctor from a fixed-size vector */
	template <std::size_t ROWS>
	explicit CVectorDynamic(const CMatrixFixed<T, ROWS, 1>& v)
	{
		(*this) = v;
	}

	/** Constructor from a given size and a C array. The array length must match
	 *cols x row.
	 * \code
	 *  const double numbers[] = {
	 *    1,2,3,
	 *    4,5,6 };
	 *  CMatrixDouble   M(3,2, numbers);
	 * \endcode
	 */
	template <
		typename ARRAY, typename = std::enable_if_t<std::is_array_v<ARRAY>>>
	CVectorDynamic(const ARRAY& data)
	{
		std::size_t N = std::size(data);
		ASSERTMSG_(N != 0, "CVectorDynamic ctor: Empty array!");
		realloc(N);
		for (size_t i = 0; i < N; i++)
			m_data[i] = static_cast<T>(data[i]);
	}

	/** Convert from Eigen matrix */
	template <class Derived>
	explicit CVectorDynamic(const Eigen::MatrixBase<Derived>& m)
	{
		*this = m;
	}

	/** Number of rows in the vector */
	size_type rows() const { return m_data.size(); }

	/** Number of columns in the matrix (always 1) */
	size_type cols() const { return 1; }

	/** Get a 2-vector with [NROWS NCOLS] (as in MATLAB command size(x)) */
	size_type size() const { return m_data.size(); }

	bool empty() const { return m_data.empty(); }

	/** Changes the size of matrix, maintaining the previous contents. */
	void setSize(size_t row, size_t col, bool zeroNewElements = false)
	{
		ASSERT_(col == 1);
		realloc(row, zeroNewElements);
	}
	void resize(std::size_t N, bool zeroNewElements = false)
	{
		setSize(N, 1, zeroNewElements);
	}

	template <class MAT>
	void fromVectorLike(const MAT& m)
	{
		MRPT_START
		ASSERT_EQUAL_(m.cols(), 1U);
		setSize(m.rows(), 1);
		for (Index r = 0; r < rows(); r++)
			(*this)[r] = m(r, 0);
		MRPT_END
	}

	/** Assignment operator from another matrix (possibly of a different type)
	 */
	template <typename U>
	CVectorDynamic& operator=(const CMatrixDynamic<U>& m)
	{
		MRPT_START
		fromVectorLike(m);
		return *this;
		MRPT_END
	}

	/** Assignment from an Eigen matrix */
	template <class Derived>
	CVectorDynamic& operator=(const Eigen::MatrixBase<Derived>& m)
	{
		MRPT_START
		fromVectorLike(m);
		return *this;
		MRPT_END
	}

	/** Assignment from a fixed-size vector */
	template <std::size_t ROWS>
	CVectorDynamic& operator=(const CMatrixFixed<T, ROWS, 1>& v)
	{
		MRPT_START
		fromVectorLike(v);
		return *this;
		MRPT_END
	}

	void push_back(const T& val)
	{
		m_data.resize(m_data.size() + 1);
		m_data.back() = val;
	}

	/** const segmentCopy(): Returns a *copy* of the given vector segment */
	template <int LEN>
	CMatrixFixed<Scalar, LEN, 1> segmentCopy(int start = 0) const
	{
		CMatrixFixed<Scalar, LEN, 1> v;
		for (int i = 0; i < LEN; i++)
			v[i] = m_data[start + i];
		return v;
	}

	/** const segmentCopy(): Returns a *copy* of the given vector segment (non
	 * templated version, dynamic length) */
	CVectorDynamic<Scalar> segmentCopy(int start, int LEN) const
	{
		CVectorDynamic<Scalar> v;
		v.resize(LEN);
		for (int i = 0; i < LEN; i++)
			v[i] = m_data[start + i];
		return v;
	}

	/** Subscript operator to get/set individual elements
	 */
	inline T& operator()(size_t row, size_t col)
	{
#if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
		if (row >= m_data.size() || col > 0)
			THROW_EXCEPTION(format(
				"Indexes (%lu,%lu) out of range. Vector is %lux%lu",
				static_cast<unsigned long>(row),
				static_cast<unsigned long>(col),
				static_cast<unsigned long>(m_data.size()),
				static_cast<unsigned long>(1)));
#endif
		return m_data[row];
	}

	/** Subscript operator to get individual elements
	 */
	inline const T& operator()(size_t row, size_t col) const
	{
#if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
		if (row >= m_data.size() || col > 0)
			THROW_EXCEPTION(format(
				"Indexes (%lu,%lu) out of range. Vector is %lux%lu",
				static_cast<unsigned long>(row),
				static_cast<unsigned long>(col),
				static_cast<unsigned long>(m_data.size()),
				static_cast<unsigned long>(1)));
#endif
		return m_data[row];
	}

	/** Subscript operator to get/set an individual element from a row or column
	 * matrix.
	 * \exception std::exception If the object is not a column or row matrix.
	 */
	inline T& operator[](size_t ith)
	{
#if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
		if (ith >= m_data.size())
			THROW_EXCEPTION_FMT(
				"Index %u out of range!", static_cast<unsigned>(ith));
#endif
		return m_data[ith];
	}

	/// \overload
	inline const T& operator[](size_t ith) const
	{
#if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
		if (ith >= m_data.size())
			THROW_EXCEPTION_FMT(
				"Index %u out of range!", static_cast<unsigned>(ith));
#endif
		return m_data[ith];
	}

	/** Get as an Eigen-compatible Eigen::Map object  */
	template <
		typename EIGEN_VECTOR = eigen_t,
		typename EIGEN_MAP = Eigen::Map<
			EIGEN_VECTOR, MRPT_MAX_ALIGN_BYTES, Eigen::InnerStride<1>>>
	EIGEN_MAP asEigen()
	{
		return EIGEN_MAP(&m_data[0], m_data.size());
	}
	/** \overload (const version) */
	template <
		typename EIGEN_VECTOR = Eigen::Matrix<T, -1, 1, 0, -1, 1>,
		typename EIGEN_MAP = Eigen::Map<
			const EIGEN_VECTOR, MRPT_MAX_ALIGN_BYTES, Eigen::InnerStride<1>>>
	EIGEN_MAP asEigen() const
	{
		return EIGEN_MAP(&m_data[0], m_data.size());
	}

	template <typename T2>
	CVectorDynamic<T2> cast() const;
};

using CVectorFloat = CVectorDynamic<float>;
using CVectorDouble = CVectorDynamic<double>;

mrpt::serialization::CArchive& operator<<(
	mrpt::serialization::CArchive& s, const CVectorFloat& a);
mrpt::serialization::CArchive& operator<<(
	mrpt::serialization::CArchive& s, const CVectorDouble& a);
mrpt::serialization::CArchive& operator>>(
	mrpt::serialization::CArchive& in, CVectorDouble& a);
mrpt::serialization::CArchive& operator>>(
	mrpt::serialization::CArchive& in, CVectorFloat& a);

}  // namespace mrpt::math

namespace mrpt::typemeta
{
// Extensions to mrpt::typemeta::TTypeName for matrices:
template <typename T>
struct TTypeName<mrpt::math::CVectorDynamic<T>>
{
	static auto get()
	{
		return literal("CVectorDynamic<") + TTypeName<T>::get() + literal(">");
	}
};
}  // namespace mrpt::typemeta
