/* +------------------------------------------------------------------------+
   |                     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          |
   +------------------------------------------------------------------------+ */

#include <mrpt/math/CMatrixF.h>
#include <mrpt/math/CMatrixFixed.h>
#include <mrpt/math/ops_matrices.h>
#include <mrpt/math/ops_vectors.h>
#include <mrpt/math/utils.h>
#include <mrpt/system/CTicTac.h>

#include <Eigen/Dense>
#include <iostream>

using namespace mrpt;
using namespace mrpt::math;
using namespace mrpt::system;
using namespace std;

#include <mrpt/examples_config.h>
string myDataDir(MRPT_EXAMPLES_BASE_DIRECTORY + string("math_matrix_example/"));

// ------------------------------------------------------
//				TestChol
// ------------------------------------------------------
void TestChol()
{
	CMatrixFloat A, B;
	A.loadFromTextFile(myDataDir + string("in_for_cholesky.txt"));
	A.chol(B);

	cout << "Cholesky decomposition result:" << endl << B;
}

void TestInitMatrix()
{
	// Initialize a matrix from a C array:
	const double numbers[] = {1, 2, 3, 4, 5, 6};
	CMatrixDouble M(2, 3, numbers);
	cout << "Initialized matrix (I): " << endl << M << endl;

	const double numbers2[] = {0.5, 4.5, 6.7, 8.9, 15.2};
	CVectorDouble v1;
	loadVector(v1, numbers2);
	cout << "Initialized double vector: " << v1 << endl;

	std::vector<int> v2;
	loadVector(v2, numbers2);
	cout << "Initialized int vector: " << v2 << endl;

	/*	// I/O Test
		CMatrixD  B(M);
		CFileOutputStream("mat.bin") << B;
		CMatrixD  A;
		CFileInputStream("mat.bin") >> A;
		cout << "B:" << endl << B;
		cout << "A:" << endl << A;
	*/
}

void TestHCH()
{
	CMatrixFloat H, C, RES;

	cout << "reading H.txt...";
	H.loadFromTextFile(myDataDir + string("H.txt"));
	cout << "ok" << endl;

	cout << "reading C.txt...";
	C.loadFromTextFile(myDataDir + string("C.txt"));
	cout << "ok" << endl;

	// RES = H * C * H'
	mrpt::math::multiply_HCHt(H, C, RES);
	cout << "Saving RES.txt ...";
	RES.saveToTextFile("RES.txt");
	cout << "ok" << endl;

	// The same for a column vector:
	H.loadFromTextFile(myDataDir + string("H_col.txt"));
	cout << "H*C*(H') = " << mrpt::math::multiply_HCHt_scalar(H, C) << endl;
	cout << "Should be= 31.434 " << endl;

	// The same for a row vector:
	H.loadFromTextFile(myDataDir + string("H_row.txt"));
	cout << "Loaded H: " << endl << H;
	cout << "H*C*(H') = " << mrpt::math::multiply_HCHt_scalar(H, C) << endl;
	cout << "Should be= 31.434" << endl;
}

void TestMatrixTemplate()
{
	CTicTac tictac;
	CMatrixDouble M;

	// --------------------------------------
	M.loadFromTextFile(myDataDir + string("matrixA.txt"));
	cout << M << "\n";

	CMatrixDouble eigenVectors;
	std::vector<double> eigenValues;
	M.eig(eigenVectors, eigenValues);
	cout << "eigenVectors:\n"
		 << eigenVectors << "\n Eigenvalues:\n"
		 << eigenValues;

	CMatrixDouble D;
	D.setDiagonal(eigenValues);

	CMatrixDouble RES;
	RES = M.asEigen() * D.asEigen() * M.transpose();
	cout << "RES:\n" << RES;
}

void TestMatrices()
{
	CMatrixFloat m, l;
	CTicTac tictac;
	double t;

	m.setSize(4, 4);
	m(0, 0) = 4;
	m(0, 1) = -2;
	m(0, 2) = -1;
	m(0, 3) = 0;
	m(1, 0) = -2;
	m(1, 1) = 4;
	m(1, 2) = 0;
	m(1, 3) = -1;
	m(2, 0) = -1;
	m(2, 1) = 0;
	m(2, 2) = 4;
	m(2, 3) = -2;
	m(3, 0) = 0;
	m(3, 1) = -1;
	m(3, 2) = -2;
	m(3, 3) = 4;

	cout << "Matrix:\n" << m << endl;

	// I/O test through a text file:
	m.saveToTextFile("matrix1.txt");
	tictac.Tic();
	l.loadFromTextFile(myDataDir + string("matrix1.txt"));
	t = tictac.Tac();
	cout << "Read (text file) in " << 1e6 * t << "us:\n" << l << endl;
	mrpt::math::laplacian(m, l);

	cout << "Laplacian:\n" << l << endl;
}

void TestCov()
{
	// Initialize a matrix from a C array:
	const double numbers[] = {1, 2, 3, 10, 4, 5, 6, 14, 10, -5, -3, 1};
	CMatrixDouble Mdyn(4, 3, numbers);
	CMatrixFixed<double, 4, 3> Mfix(numbers);

	vector<CVectorDouble> samples(4);
	for (size_t i = 0; i < 4; i++)
	{
		samples[i].resize(3);
		for (size_t j = 0; j < 3; j++)
			samples[i][j] = Mdyn(i, j);
	}

	cout << "COV (vector of vectors): " << endl
		 << mrpt::math::covVector<vector<CVectorDouble>, Eigen::MatrixXd>(
				samples)
		 << endl;
	cout << "COV (mat fix): " << endl << mrpt::math::cov(Mfix) << endl;
	cout << "COV (mat dyn): " << endl << mrpt::math::cov(Mdyn) << endl;
}

// ------------------------------------------------------
//						MAIN
// ------------------------------------------------------
int main()
{
	try
	{
		TestInitMatrix();
		TestMatrixTemplate();
		TestMatrices();
		TestHCH();
		TestChol();
		TestCov();

		return 0;
	}
	catch (exception& e)
	{
		cout << "MRPT exception caught: " << e.what() << endl;
		return -1;
	}
}
