/*
 * Copyright (C) 2012 by
 *   MetraLabs GmbH (MLAB), GERMANY
 * and
 *   Neuroinformatics and Cognitive Robotics Labs (NICR) at TU Ilmenau, GERMANY
 * All rights reserved.
 *
 * Contact: info@mira-project.org
 *
 * Commercial Usage:
 *   Licensees holding valid commercial licenses may use this file in
 *   accordance with the commercial license agreement provided with the
 *   software or, alternatively, in accordance with the terms contained in
 *   a written agreement between you and MLAB or NICR.
 *
 * GNU General Public License Usage:
 *   Alternatively, this file may be used under the terms of the GNU
 *   General Public License version 3.0 as published by the Free Software
 *   Foundation and appearing in the file LICENSE.GPL3 included in the
 *   packaging of this file. Please review the following information to
 *   ensure the GNU General Public License version 3.0 requirements will be
 *   met: http://www.gnu.org/copyleft/gpl.html.
 *   Alternatively you may (at your option) use any later version of the GNU
 *   General Public License if such license has been publicly approved by
 *   MLAB and NICR (or its successors, if any).
 *
 * IN NO EVENT SHALL "MLAB" OR "NICR" BE LIABLE TO ANY PARTY FOR DIRECT,
 * INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES ARISING OUT OF
 * THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF "MLAB" OR
 * "NICR" HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 * "MLAB" AND "NICR" SPECIFICALLY DISCLAIM ANY WARRANTIES, INCLUDING,
 * BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
 * FITNESS FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER IS
 * ON AN "AS IS" BASIS, AND "MLAB" AND "NICR" HAVE NO OBLIGATION TO
 * PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS OR MODIFICATIONS.
 */

/**
 * @file Eigen
 *    Reflection adaptors for Eigen matrices to support de-/serialization.
 *
 * @author Tim Langner
 * @date   2010/08/02
 */

#ifndef _MIRA_EIGEN_
#define _MIRA_EIGEN_

#include <math/EigenFormat.h>
#include <math/Angle.h>
#include <math/YawPitchRoll.h>
#include <error/Exceptions.h>
#include <utils/MakeString.h>
#include <serialization/adapters/std/vector>
#include <serialization/BinarySerializer.h>
#include <serialization/PropertySerializer.h>
#include <serialization/JSONSerializer.h>
#include <serialization/IsTransparentSerializable.h>

namespace mira {

////////////////////////////////////////////////////////////////////////////////

/**
 * Non intrusive reflect for BinarySerializer and Eigen Matrices
 * @param iR The binary serializer
 * @param m The matrix
 */
template <typename Derived, typename Derived2>
void reflect(BinarySerializer<Derived>& iR, Eigen::MatrixBase<Derived2>& m)
{
	int rows = m.rows();
	int cols = m.cols();
	// store rows and cols
	iR.member("rows", rows, "");
	iR.member("cols", cols, "");
	// store data
	for (int r = 0; r < m.rows(); ++r)
		for (int c = 0; c < m.cols(); ++c)
			MIRA_MEMBER_WITH_ID(iR, "item", MakeString() << "item[" << r << "," << c << "]", m(r,c), "");
}

/**
 * Non intrusive reflect for BinaryDeserializer and Eigen Matrices
 * @param iR The binary deserializer
 * @param m The matrix
 */
template <typename Derived, typename Scalar, int Rows, int Cols, int Options, int MaxRows, int MaxCols>
void reflect(BinaryDeserializer<Derived>& iR, Eigen::Matrix<Scalar, Rows, Cols, Options, MaxRows, MaxCols>& m)
{
	int rows, cols;
	typedef Eigen::Matrix<Scalar, Rows, Cols, Options, MaxRows, MaxCols> MatrixType;

	// read rows and cols
	iR.member("rows", rows, "");
	iR.member("cols", cols, "");

	// If the its a dynamic matrix resize it
	if ( MatrixType::RowsAtCompileTime == Eigen::Dynamic || MatrixType::ColsAtCompileTime == Eigen::Dynamic )
		m.resize(rows, cols);

	// the rows and cols must match
	if ( rows != m.rows() || cols != m.cols() )
		MIRA_THROW(XIO, "Rows or columns do not match for fixed size matrix["
				<< m.rows() << "," << m.cols() << "] "
				<< "read: [" << rows << "," << cols << "]");

	// read the data
	for (int r = 0; r < m.rows(); ++r)
		for (int c = 0; c < m.cols(); ++c)
			MIRA_MEMBER_WITH_ID(iR, "item", MakeString() << "item[" << r << "," << c << "]", m(r,c), "");
}

////////////////////////////////////////////////////////////////////////////////

/**
 * Non intrusive reflect for JSONSerializer and Eigen Matrices
 * @param iR The JSON serializer
 * @param m The matrix
 */
template <typename Derived>
void reflect(JSONSerializer& iR, Eigen::MatrixBase<Derived>& m)
{
	std::vector<std::vector<typename Derived::Scalar>> data;
	data.resize(m.rows());
	// store data
	for (int r = 0; r < m.rows(); ++r)
	{
		data[r].resize(m.cols());
		for (int c = 0; c < m.cols(); ++c)
			data[r][c] = m(r,c);
	}
	iR.delegate(data);
}

/**
 * Non intrusive reflect for JSONDeserializer and Eigen Matrices
 * @param iR The JSON deserializer
 * @param m The matrix
 */
template <typename Scalar, int Rows, int Cols, int Options, int MaxRows, int MaxCols>
void reflect(JSONDeserializer& iR, Eigen::Matrix<Scalar, Rows, Cols, Options, MaxRows, MaxCols>& m)
{
	std::vector<std::vector<Scalar>> data;
	iR.delegate(data);

	int rows;
	rows = data.size();
	int cols = 0;
	if (rows > 0)
		cols = data[0].size();
	typedef Eigen::Matrix<Scalar, Rows, Cols, Options, MaxRows, MaxCols> MatrixType;

	// If the its a dynamic matrix resize it
	if ( MatrixType::RowsAtCompileTime == Eigen::Dynamic || MatrixType::ColsAtCompileTime == Eigen::Dynamic )
		m.resize(rows, cols);

	// the rows and cols must match
	if ( rows != m.rows() || cols != m.cols() )
		MIRA_THROW(XIO, "Rows or columns do not match for fixed size matrix["
				<< m.rows() << "," << m.cols() << "] "
				<< "read: [" << rows << "," << cols << "]");

	// read the data
	for (int r = 0; r < m.rows(); ++r)
		for (int c = 0; c < m.cols(); ++c)
			m(r,c) = data[r][c];
}

////////////////////////////////////////////////////////////////////////////////

/**
 * Non intrusive reflect for Eigen Matrices
 * @param iR The serializer
 * @param m The matrix
 */
template <typename Reflector, typename Derived>
void reflectRead(Reflector& r, Eigen::MatrixBase<Derived>& m)
{
	// use formatter to stream the matrix
	std::stringstream s;
	s << format(m, EigenFormat::matlab(17));
	std::string data = s.str();

	// delegate to the string
	r.delegate(data, REFLECT_CTRLFLAG_TEMP_TRACKING);
}

/**
 * Non intrusive reflect for Eigen Matrices
 * @param iR The deserializer
 * @param m The matrix
 */
template <typename Reflector, typename Derived>
void reflectWrite(Reflector& r, Eigen::MatrixBase<Derived>& m)
{
	// use formatter to load the matrix
	std::string data;

	// delegate to the string
	r.delegate(data, REFLECT_CTRLFLAG_TEMP_TRACKING);
	
	std::stringstream s(data);
	s >> format(m);
}

template <typename DerivedReflector, typename Derived>
void reflect(ReflectorInterface<DerivedReflector>& iR, Eigen::MatrixBase<Derived>& m)
{
	splitReflect(iR, m);
}

template<typename Derived, typename SerializerTag>
class IsTransparentSerializable<Eigen::MatrixBase<Derived>,SerializerTag> : public std::true_type {};

template<typename T, typename SerializerTag, int A, int B, int C, int D, int E>
class IsTransparentSerializable<Eigen::Matrix<T,A,B,C,D,E>,SerializerTag> : public std::true_type {};

////////////////////////////////////////////////////////////////////////////////

// specializations for 3- and 4-vectors to support the property serializer

template <typename T>
void reflect(PropertySerializer& r, Eigen::Matrix<T,3,1>& m)
{
	r.property("X", m[0], "");
	r.property("Y", m[1], "");
	r.property("Z", m[2], "");
}

template <typename T>
void reflect(PropertySerializer& r, Eigen::Matrix<T,4,1>& m)
{
	r.property("X", m[0], "");
	r.property("Y", m[1], "");
	r.property("Z", m[2], "");
	r.property("W", m[3], "");
}

////////////////////////////////////////////////////////////////////////////////

template <typename Reflector, typename T>
void reflectRead(Reflector& r, Eigen::Quaternion<T>& q)
{
	auto ypr = quaternionToYawPitchRoll(q);
	T yaw   = rad2deg(ypr.template get<0>());
	T pitch = rad2deg(ypr.template get<1>());
	T roll  = rad2deg(ypr.template get<2>());
	r.property("Yaw"  , yaw,   "The yaw angle (phi)");
	r.property("Pitch", pitch, "The pitch angle (theta)");
	r.property("Roll" , roll,  "The roll angle (psi)");
}

template <typename Reflector, typename T>
void reflectWrite(Reflector& r, Eigen::Quaternion<T>& q)
{
	T yaw, pitch, roll;
	r.property("Yaw"  , yaw,   "The yaw angle (phi)");
	r.property("Pitch", pitch, "The pitch angle (theta)");
	r.property("Roll" , roll,  "The roll angle (psi)");

	q = quaternionFromYawPitchRoll(deg2rad(yaw),deg2rad(pitch),deg2rad(roll));
}

/**
 * Non intrusive reflect for Eigen Quaternions
 * @param r The serializer
 * @param q The quaternion
 */
template <typename DerivedReflector, typename T>
void reflect(ReflectorInterface<DerivedReflector>& iR, Eigen::Quaternion<T>& q)
{
	splitReflect(iR, q);
}

/**
 * Non intrusive reflect for Eigen Quaternions for binary serialization.
 * The four members of the quaternion are stored as Vector4
 * @param r The serializer
 * @param q The quaternion
 */
template <typename Derived, typename T>
void reflect(BinarySerializer<Derived>& r, Eigen::Quaternion<T>& q)
{
	reflect(r, q.coeffs());
}

/**
 * Non intrusive reflect for Eigen Quaternions for binary deserialization
 * The four members of the quaternion are stored as Vector4
 * @param r The deserializer
 * @param m The quaternion
 */
template <typename Derived, typename T>
void reflect(BinaryDeserializer<Derived>& r, Eigen::Quaternion<T>& q)
{
	reflect(r, q.coeffs());
}

////////////////////////////////////////////////////////////////////////////////

} // namespace

#endif
