/*
Copyright 2011. All rights reserved.
Institute of Measurement and Control Systems
Karlsruhe Institute of Technology, Germany

Authors: Andreas Geiger

matrix is free software; you can redistribute it and/or modify it under the
terms of the GNU General Public License as published by the Free Software
Foundation; either version 2 of the License, or any later version.

matrix is distributed in the hope that it will be useful, but WITHOUT ANY
WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A
PARTICULAR PURPOSE. See the GNU General Public License for more details.

You should have received a copy of the GNU General Public License along with
matrix; if not, write to the Free Software Foundation, Inc., 51 Franklin
Street, Fifth Floor, Boston, MA 02110-1301, USA 
 */

#ifndef MATRIX_H
#define MATRIX_H

#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <iostream>
#include <vector>

#ifndef _MSC_VER
//#include <stdint.h>
typedef int int32_t;
#else
typedef __int8            int8_t;
typedef __int16           int16_t;
typedef __int32           int32_t;
typedef __int64           int64_t;
typedef unsigned __int8   uint8_t;
typedef unsigned __int16  uint16_t;
typedef unsigned __int32  uint32_t;
typedef unsigned __int64  uint64_t;
#endif

//#define endll endl << endl // double end line definition

//typedef long double FLOAT; //quad precision
typedef double FLOAT;      // double precision
//typedef float  FLOAT;    // single precision

class Matrix {

public:

	// constructor / deconstructor
	Matrix ();                                                  // init empty 0x0 matrix
	Matrix (const int32_t m,const int32_t n);                   // init empty mxn matrix
	Matrix (const int32_t m,const int32_t n,const FLOAT* val_); // init mxn matrix with values from array 'val'
	Matrix (const Matrix &M);                                   // creates deepcopy of M
	~Matrix ();

	// assignment operator, copies contents of M
	Matrix& operator= (const Matrix &M);

	// copies submatrix of M into array 'val', default values copy whole row/column/matrix
	void getData(FLOAT* val_,int32_t i1=0,int32_t j1=0,int32_t i2=-1,int32_t j2=-1);

	// set or get submatrices of current matrix
	Matrix getMat(int32_t i1,int32_t j1,int32_t i2=-1,int32_t j2=-1);
	void   setMat(const Matrix &M,const int32_t i,const int32_t j);

	//set mxn matrix with values from array 'val'
	void setVal(const int32_t m,const int32_t n,const FLOAT* val_);

	// set sub-matrix to scalar (default 0), -1 as end replaces whole row/column/matrix
	void setVal(FLOAT s,int32_t i1=0,int32_t j1=0,int32_t i2=-1,int32_t j2=-1);

	// set (part of) diagonal to scalar, -1 as end replaces whole diagonal
	void setDiag(FLOAT s,int32_t i1=0,int32_t i2=-1);

	// clear matrix
	void zero();

	// extract columns with given index
	Matrix extractCols (std::vector<int> idx);

	// create identity matrix
	static Matrix eye (const int32_t m);
	void          eye ();

	// create matrix with ones
	static Matrix ones(const int32_t m,const int32_t n);

	// create diagonal matrix with nx1 or 1xn matrix M as elements
	static Matrix diag(const Matrix &M);

	// returns the m-by-n matrix whose elements are taken column-wise from M
	static Matrix reshape(const Matrix &M,int32_t m,int32_t n);

	// create 3x3 rotation matrices (convention: http://en.wikipedia.org/wiki/Rotation_matrix)
	static Matrix rotMatX(const FLOAT &angle);
	static Matrix rotMatY(const FLOAT &angle);
	static Matrix rotMatZ(const FLOAT &angle);

	//homogenize 3x3 matrix or 3 vector to 4x4 matrix or 4 vector
	static Matrix homogenize(const Matrix &M);
	//de-homogenize a 4 vector
	static Matrix dehomogenize(const Matrix &M);
	//homogenize 3 vector to 4x4 translation matrix
	static Matrix homogenizeTranslation(const Matrix &M);
	//homogenize rotation and translation simultaniously
	static Matrix homogenizeRotTrans(const Matrix &R, const Matrix &t);

	// simple arithmetic operations
	Matrix  operator+ (const Matrix &M); // add matrix
	Matrix  operator- (const Matrix &M); // subtract matrix
	Matrix  operator* (const Matrix &M); // multiply with matrix
	Matrix  operator* (const FLOAT &s);  // multiply with scalar
	Matrix  operator/ (const Matrix &M); // divide elementwise by matrix (or vector)
	Matrix  operator/ (const FLOAT &s);  // divide by scalar
	Matrix  operator- ();                // negative matrix
	Matrix  operator~ ();                // transpose
	FLOAT   l2norm ();                   // euclidean norm (vectors) / frobenius norm (matrices)
	FLOAT   mean ();                     // mean of all elements in matrix

	// complex arithmetic operations
	static Matrix cross (const Matrix &a, const Matrix &b);    // cross product of two vectors
	static Matrix inv (const Matrix &M);                       // invert matrix M
	bool   inv ();                                             // invert this matrix
	FLOAT  det ();                                             // returns determinant of matrix
	bool   solve (const Matrix &M,FLOAT eps=1e-20);            // solve linear system M*x=B, replaces *this and M
	bool   lu(int32_t *idx, FLOAT &d, FLOAT eps=1e-20);        // replace *this by lower upper decomposition
	void   svd(Matrix &U,Matrix &W,Matrix &V);                 // singular value decomposition *this = U*diag(W)*V^T

	// print matrix to stream
	friend std::ostream& operator<< (std::ostream& out,const Matrix& M);

	// direct data access
	FLOAT   **val;
	int32_t   m,n;

private:

	void allocateMemory (const int32_t m_,const int32_t n_);
	void releaseMemory ();
	inline FLOAT pythag(FLOAT a,FLOAT b);

};

#endif // MATRIX_H