/* +------------------------------------------------------------------------+
   |                     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/math/CMatrixD.h>
#include <mrpt/math/CMatrixDynamic.h>
#include <mrpt/math/CMatrixF.h>
#include <mrpt/math/CMatrixFixed.h>
#include <mrpt/serialization/CArchive.h>

/** \file matrix_serialization.h
 * This file implements matrix/vector text and binary serialization */
namespace mrpt::math
{
/** \addtogroup container_ops_grp
 *  @{ */

/** @name Operators for binary streaming of MRPT matrices
	@{ */

/** Read operator from a CStream. The format is compatible with that of CMatrixF
 * & CMatrixD */
template <size_t NROWS, size_t NCOLS>
mrpt::serialization::CArchive& operator>>(
	mrpt::serialization::CArchive& in, CMatrixFixed<float, NROWS, NCOLS>& M)
{
	CMatrixF aux;
	in.ReadObject(&aux);
	ASSERTMSG_(
		M.cols() == aux.cols() && M.rows() == aux.rows(),
		format(
			"Size mismatch: deserialized is %ux%u, expected is %ux%u",
			(unsigned)aux.rows(), (unsigned)aux.cols(), (unsigned)NROWS,
			(unsigned)NCOLS));
	M = aux;
	return in;
}
/** Read operator from a CStream. The format is compatible with that of CMatrixF
 * & CMatrixD */
template <size_t NROWS, size_t NCOLS>
mrpt::serialization::CArchive& operator>>(
	mrpt::serialization::CArchive& in, CMatrixFixed<double, NROWS, NCOLS>& M)
{
	CMatrixD aux;
	in.ReadObject(&aux);
	ASSERTMSG_(
		M.cols() == aux.cols() && M.rows() == aux.rows(),
		format(
			"Size mismatch: deserialized is %ux%u, expected is %ux%u",
			(unsigned)aux.rows(), (unsigned)aux.cols(), (unsigned)NROWS,
			(unsigned)NCOLS));
	M = aux;
	return in;
}

/** Write operator for writing into a CStream. The format is compatible with
 * that of CMatrixF & CMatrixD */
template <size_t NROWS, size_t NCOLS>
mrpt::serialization::CArchive& operator<<(
	mrpt::serialization::CArchive& out,
	const CMatrixFixed<float, NROWS, NCOLS>& M)
{
	CMatrixF aux;
	aux = M;
	out.WriteObject(&aux);
	return out;
}
/** Write operator for writing into a CStream. The format is compatible with
 * that of CMatrixF & CMatrixD */
template <size_t NROWS, size_t NCOLS>
mrpt::serialization::CArchive& operator<<(
	mrpt::serialization::CArchive& out,
	const CMatrixFixed<double, NROWS, NCOLS>& M)
{
	CMatrixD aux;
	aux = M;
	out.WriteObject(&aux);
	return out;
}

/** @} */  // end MRPT matrices stream operators

/** @name Operators for text streaming of MRPT matrices
	@{ */

/** Binary serialization of symmetric matrices, saving the space of duplicated
 * values. \sa serializeSymmetricMatrixTo() */
template <typename MAT>
void deserializeSymmetricMatrixFrom(MAT& m, mrpt::serialization::CArchive& in)
{
	ASSERT_EQUAL_(m.rows(), m.cols());
	auto N = m.cols();
	for (decltype(N) i = 0; i < N; i++)
		in >> m(i, i);
	for (decltype(N) r = 0; r < N - 1; r++)
	{
		for (decltype(N) c = r + 1; c < N; c++)
		{
			typename MAT::Scalar x;
			in >> x;
			m(r, c) = m(c, r) = x;
		}
	}
}

/** Binary serialization of symmetric matrices, saving the space of duplicated
 * values. \sa deserializeSymmetricMatrixFrom() */
template <typename MAT>
void serializeSymmetricMatrixTo(MAT& m, mrpt::serialization::CArchive& out)
{
	ASSERT_EQUAL_(m.rows(), m.cols());
	auto N = m.cols();
	for (decltype(N) i = 0; i < N; i++)
		out << m(i, i);
	for (decltype(N) r = 0; r < N - 1; r++)
		for (decltype(N) c = r + 1; c < N; c++)
			out << m(r, c);
}

/** @} */  // end MRPT matrices stream operators

/**  @} */	// end of grouping
}  // namespace mrpt::math
