#ifndef _MATH_HEADERS_H_
#define _MATH_HEADERS_H_

// std
#include <algorithm>

// eigen
#include <Eigen\Core>
#include <Eigen\Dense>
#include <Eigen\Sparse>

// glm
#include <glm.hpp>
#include <gtc\matrix_transform.hpp>
#include <gtx\color_space.hpp>

// global header
#include "global_headers.h"

// eigen vectors and matrices
typedef int IndexType;
// 3d
typedef Eigen::Matrix<ScalarType, 3, 3, 0, 3 ,3> EigenMatrix3;
typedef Eigen::Matrix<ScalarType, 3, 1, 0, 3 ,1> EigenVector3;
typedef Eigen::Matrix<ScalarType, 1, 3, 0, 1, 3> EigenRowVector3;
typedef Eigen::Matrix<ScalarType, 2, 3, 0, 2, 3> EigenMatrix2x3;
typedef Eigen::Matrix<ScalarType, -1, 3, 0, -1 ,3> EigenMatrixnx3;
// 2d
typedef Eigen::Matrix<ScalarType, 2, 2, 0, 2 ,2> EigenMatrix2;
typedef Eigen::Matrix<ScalarType, 2, 1, 0, 2 ,1> EigenVector2;
typedef Eigen::Matrix<ScalarType, 1, 2, 0, 1, 2> EigenRowVector2;
typedef Eigen::Matrix<ScalarType, 2, 2, 0, 2, 2> EigenMatrix2x2;
typedef Eigen::Matrix<ScalarType, -1, 2, 0, -1 ,2> EigenMatrixnx2;
// dynamic 
typedef Eigen::Matrix<ScalarType, Eigen::Dynamic, 1> VectorX;
typedef Eigen::Matrix<ScalarType, Eigen::Dynamic, Eigen::Dynamic> EigenMatrix;
typedef Eigen::MatrixXi EigenMatrixi;
typedef Eigen::SparseMatrix<ScalarType> EigenSparseMatrix;
typedef Eigen::Triplet<ScalarType,IndexType> SparseMatrixTriplet;
// quaternion
typedef Eigen::AngleAxis<ScalarType> EigenAngleAxis;
typedef Eigen::Quaternion<ScalarType, Eigen::DontAlign> EigenQuaternion;

// eigen vector accessor
#define block_vector(a) block<3,1>(3*(a), 0)
void EigenSparseSubMatrix(EigenSparseMatrix& dst, const EigenSparseMatrix& src, const std::vector<int>& indices);
void EigenConstructKKT(EigenSparseMatrix& KKT, const EigenSparseMatrix& G, const EigenSparseMatrix& N); // construct [G N; N' 0]
void EigenGroupBlockMatrices(EigenSparseMatrix& G, const EigenSparseMatrix& A, const EigenSparseMatrix& B, const EigenSparseMatrix& C, const EigenSparseMatrix& D); // G = [A B; C D]
void EigenConcatMatricesVertically(EigenSparseMatrix& G, const EigenSparseMatrix& A, const EigenSparseMatrix& B); // Dst = [A;B]
void EigenConcatMatricesHorizontally(EigenSparseMatrix& G, const EigenSparseMatrix& A, const EigenSparseMatrix& B); // Dst = [A B]
void EigenFullToSubVector(VectorX& dst, VectorX& src, const std::vector<int>& indices); // dst = src(indices)
void EigenSubToFullVector(VectorX& dst, VectorX& src, const std::vector<int>& indices, int size); // dst=zeros(size,1); dst(indices) = src
void EigenVectorConcat(VectorX& dst, const VectorX& src1, const VectorX& src2);
void EigenVectorDissect(VectorX& dst1, VectorX& dst2, const VectorX& src, unsigned int size1, unsigned int size2);
void EigenVectorConcat(VectorX& dst, const VectorX& src1, const VectorX& src2, const VectorX& src3);
void EigenVectorDissect(VectorX& dst1, VectorX& dst2, VectorX& dst3, const VectorX& src, unsigned int size1, unsigned int size2, unsigned int size3);

// eigen sparse matrix to triplets
void EigenSparseMatrixToTriplets(const EigenSparseMatrix& A, std::vector<SparseMatrixTriplet>& A_triplets);


// eigen 2 glm, glm 2 eigen
glm::vec3 Eigen2GLM(const EigenVector3& eigen_vector);
EigenVector3 GLM2Eigen(const glm::vec3& glm_vector);
glm::vec2 Eigen2GLM2(const EigenVector2& eigen_vector);
EigenVector2 GLM2Eigen2(const glm::vec2& glm_vector);

typedef std::pair<ScalarType, int> double_index_pair;
typedef std::pair<int, int> int_index_pair;
bool comparator_sorting_by_value(const double_index_pair& l, const double_index_pair& r);
bool comparator_sorting_by_index(const double_index_pair& l, const double_index_pair& r);
bool comparator_sorting_by_value_int(const int_index_pair& l, const int_index_pair& r);
bool comparator_sorting_by_index_int(const int_index_pair& l, const int_index_pair& r);

class Matrix3333 // 3x3 matrix: each element is a 3x3 matrix
{
public:
	Matrix3333();
	Matrix3333(const Matrix3333& other);
	~Matrix3333() {}

	void SetZero(); // [0 0 0; 0 0 0; 0 0 0]; 0 = 3x3 zeros
	void SetIdentity(); //[I 0 0; 0 I 0; 0 0 I]; 0 = 3x3 zeros, I = 3x3 identity

	// operators
	EigenMatrix3& operator() (int row, int col);
	Matrix3333 operator+ (const Matrix3333& plus);
	Matrix3333 operator- (const Matrix3333& minus);
	Matrix3333 operator* (const EigenMatrix3& multi);
	friend Matrix3333 operator* (const EigenMatrix3& multi1, Matrix3333& multi2);
	Matrix3333 operator* (ScalarType multi);
	friend Matrix3333 operator* (ScalarType multi1, Matrix3333& multi2);
	Matrix3333 transpose();	
	EigenMatrix3 FrobeniusProduct(const EigenMatrix3& multi); // this operator is commutative

protected:
	
	EigenMatrix3 mat[3][3];
};

class Matrix2222 // 2x2 matrix: each element is a 2x2 matrix
{
public:
	Matrix2222();
	Matrix2222(const Matrix2222& other);
	~Matrix2222() {}

	void SetZero(); // [0 0; 0 0]; 0 = 2x2 zeros
	void SetIdentity(); //[I 0; 0 I;]; 0 = 2x2 zeros, I = 2x2 identity

	// operators and basic functions
	EigenMatrix2& operator() (int row, int col);
	Matrix2222 operator+ (const Matrix2222& plus);
	Matrix2222 operator- (const Matrix2222& minus);
	Matrix2222 operator* (const EigenMatrix2& multi);
	friend Matrix2222 operator* (const EigenMatrix2& multi1, Matrix2222& multi2);
	Matrix2222 operator* (ScalarType multi);
	friend Matrix2222 operator* (ScalarType multi1, Matrix2222& multi2);
	Matrix2222 transpose();	
	EigenMatrix2 FrobeniusProduct(const EigenMatrix2& multi); // this operator is commutative

protected:
	
	EigenMatrix2 mat[2][2];
};

// dst = src1 \kron src2
void directProduct(Matrix3333& dst, const EigenMatrix3 src1, const EigenMatrix3& src2);
void directProduct(Matrix2222& dst, const EigenMatrix2 src1, const EigenMatrix2& src2);

#endif