#include "precompile.h"
#include "math_headers.h"
#include "../timer_wrapper.h"

void EigenSparseSubMatrix(EigenSparseMatrix& dst, const EigenSparseMatrix& src, const std::vector<int>& indices)
{
	dst.resize(src.rows(), indices.size());

	for (unsigned int i = 0; i != indices.size(); ++i)
	{
		dst.col(i) = src.col(indices[i]);
	}
}

void EigenConstructKKT(EigenSparseMatrix& KKT, const EigenSparseMatrix& G, const EigenSparseMatrix& N)
{
	assert(G.rows() == G.cols());
	assert(G.rows() == N.rows());

	EigenSparseMatrix KKT_left;
	KKT_left.resize(G.rows(), G.cols()+N.cols());
	KKT_left.middleCols(0, G.cols()) = G;
	KKT_left.middleCols(G.cols(), N.cols()) = N;
	KKT_left = KKT_left.transpose();

	KKT.setZero();
	KKT.resize(G.rows()+N.cols(), G.cols()+N.cols());
	KKT.middleCols(0, KKT_left.cols()) = KKT_left;
	KKT.middleCols(KKT_left.cols(), N.cols()) =  N;	
}

void EigenGroupBlockMatrices(EigenSparseMatrix& G, const EigenSparseMatrix& A, const EigenSparseMatrix& B, const EigenSparseMatrix& C, const EigenSparseMatrix& D)
{
	assert(A.rows() == B.rows());
	assert(C.rows() == D.rows());
	assert(A.cols() == C.cols());
	assert(B.cols() == D.cols());

	// group AB
	EigenSparseMatrix AB;
	AB.resize(A.rows(), A.cols()+B.cols());
	AB.middleCols(0, A.cols()) = A;
	AB.middleCols(A.cols(), B.cols()) = B;
	EigenSparseMatrix ABT = AB.transpose();

	// group CD
	EigenSparseMatrix CD;
	CD.resize(C.rows(), C.cols()+D.cols());
	CD.middleCols(0, C.cols()) = C;
	CD.middleCols(C.cols(), D.cols()) = D;
	EigenSparseMatrix CDT = CD.transpose();

	// group [ABT CDT]
	EigenSparseMatrix ABTCDT;
	ABTCDT.resize(ABT.rows(), ABT.cols() + CDT.cols());
	ABTCDT.middleCols(0, ABT.cols()) = ABT;
	ABTCDT.middleCols(ABT.cols(), CDT.cols()) = CDT;
	G = ABTCDT.transpose();
}
void EigenConcatMatricesVertically(EigenSparseMatrix& G, const EigenSparseMatrix& A, const EigenSparseMatrix& B) // Dst = [A;B]
{
	assert(A.cols() == B.cols());

	EigenSparseMatrix Gt(A.cols(), A.rows()+B.rows());
	Gt.middleCols(0, A.rows()) = A.transpose();
	Gt.middleCols(A.rows(), B.rows()) = B.transpose();

	G = Gt.transpose();
}
void EigenConcatMatricesHorizontally(EigenSparseMatrix& G, const EigenSparseMatrix& A, const EigenSparseMatrix& B) // Dst = [A B]
{
	assert(A.rows() == B.rows());
	G.resize(A.rows(), A.cols()+B.cols());
	G.middleCols(0, A.cols()) = A;
	G.middleCols(A.cols(), B.cols()) = B;
}

void EigenFullToSubVector(VectorX& dst, VectorX& src, const std::vector<int>& indices) // dst = src(indices)
{
	dst.resize(indices.size());
	for (unsigned int i = 0; i != indices.size(); ++i)
	{
		dst(i) = src(indices[i]);
	}
}
void EigenSubToFullVector(VectorX& dst, VectorX& src, const std::vector<int>& indices, int size) // dst=zeros(size,1); dst(indices) = src
{
	dst.resize(size);
	dst.setZero();
	for (unsigned int i = 0; i != indices.size(); ++i)
	{
		assert(indices[i] < size);
		dst(indices[i]) = src(i);
	}
}
void EigenVectorConcat(VectorX& dst, const VectorX& src1, const VectorX& src2)
{
	unsigned int size1 = src1.size();
	unsigned int size2 = src2.size();
	dst.resize(size1+size2);
	dst.block(0, 0, size1, 1) = src1;
	dst.block(size1, 0, size2, 1) = src2;
}
void EigenVectorDissect(VectorX& dst1, VectorX& dst2, const VectorX& src, unsigned int size1, unsigned int size2)
{
	dst1.resize(size1);
	dst2.resize(size2);

	dst1 = src.block(0, 0, size1, 1);
	dst2 = src.block(size1, 0, size2, 1);
}
void EigenVectorConcat(VectorX& dst, const VectorX& src1, const VectorX& src2, const VectorX& src3)
{
	unsigned int size1 = src1.size();
	unsigned int size2 = src2.size();
	unsigned int size3 = src3.size();
	dst.resize(size1+size2+size3);
	dst.block(0, 0, size1, 1) = src1;
	dst.block(size1, 0, size2, 1) = src2;
	dst.block(size1+size2, 0, size3, 1) = src3;
}
void EigenVectorDissect(VectorX& dst1, VectorX& dst2, VectorX& dst3, const VectorX& src, unsigned int size1, unsigned int size2, unsigned int size3)
{
	dst1.resize(size1);
	dst2.resize(size2);
	dst3.resize(size3);

	dst1 = src.block(0, 0, size1, 1);
	dst2 = src.block(size1, 0, size2, 1);
	dst3 = src.block(size1+size2, 0, size3, 1);
}

void EigenSparseMatrixToTriplets(const EigenSparseMatrix& A, std::vector<SparseMatrixTriplet>& A_triplets)
{
	A_triplets.clear();
	A_triplets.reserve(A.nonZeros());
	for (unsigned int col = 0; col != A.outerSize(); ++col)
	{
		for (EigenSparseMatrix::InnerIterator it(A, col); it; ++it)
		{
			A_triplets.push_back(SparseMatrixTriplet(it.row(), col, it.value()));
		}
	}
}

glm::vec3 Eigen2GLM(const EigenVector3& eigen_vector)
{
	return glm::vec3(eigen_vector[0], eigen_vector[1], eigen_vector[2]);
}
EigenVector3 GLM2Eigen(const glm::vec3& glm_vector)
{
	return EigenVector3(glm_vector[0], glm_vector[1], glm_vector[2]);
}

glm::vec2 Eigen2GLM2(const EigenVector2& eigen_vector)
{
	return glm::vec2(eigen_vector[0], eigen_vector[1]);
}
EigenVector2 GLM2Eigen2(const glm::vec2& glm_vector)
{
	return EigenVector2(glm_vector[0], glm_vector[1]);
}

bool comparator_sorting_by_value(const double_index_pair& l, const double_index_pair& r) {return l.first < r.first;}
bool comparator_sorting_by_index(const double_index_pair& l, const double_index_pair& r) {return l.second < r.second;}
bool comparator_sorting_by_value_int(const int_index_pair& l, const int_index_pair& r) {return l.first < r.first;}
bool comparator_sorting_by_index_int(const int_index_pair& l, const int_index_pair& r) {return l.second < r.second;}

// matrix3333
Matrix3333::Matrix3333()
{

}

Matrix3333::Matrix3333(const Matrix3333& other)
{
	for (unsigned int row = 0; row != 3; ++row)
	{
		for (unsigned int col = 0; col != 3; ++col)
		{
			mat[row][col] = other.mat[row][col];
		}
	}
}

void Matrix3333::SetZero()
{
	for (unsigned int row = 0; row != 3; ++row)
	{
		for (unsigned int col = 0; col != 3; ++col)
		{
			mat[row][col] = EigenMatrix3::Zero();
		}
	}
}

void Matrix3333::SetIdentity()
{
	for (unsigned int row = 0; row != 3; ++row)
	{
		for (unsigned int col = 0; col != 3; ++col)
		{
			mat[row][col] = EigenMatrix3::Zero();
			mat[row][col](row, col) = 1.0;
		}
	}
}

EigenMatrix3& Matrix3333::operator() (int row, int col)
{
	assert(row >= 0 && row < 3 && col >= 0 && col < 3);
	return mat[row][col];
}

Matrix3333 Matrix3333::operator+ (const Matrix3333& plus)
{
	Matrix3333 res;
	for (unsigned int row = 0; row != 3; ++row)
	{
		for (unsigned int col = 0; col != 3; ++col)
		{
			res.mat[row][col] = mat[row][col] + plus.mat[row][col];
		}
	}
	return res;
}

Matrix3333 Matrix3333::operator- (const Matrix3333& minus)
{
	Matrix3333 res;
	for (unsigned int row = 0; row != 3; ++row)
	{
		for (unsigned int col = 0; col != 3; ++col)
		{
			res.mat[row][col] = mat[row][col] - minus.mat[row][col];
		}
	}
	return res;
}

Matrix3333 Matrix3333::operator* (const EigenMatrix3& multi)
{
	Matrix3333 res;
	for (unsigned int i = 0; i < 3; ++i)
	{
		for (unsigned int j = 0; j < 3; ++j)
		{
			res.mat[i][j].setZero();
			for (unsigned int k = 0; k < 3; k++)
			{
				res.mat[i][j] += mat[i][k] * multi(k, j);
			}
		}
	}
	return res;
}

Matrix3333 operator* (const EigenMatrix3& multi1, Matrix3333& multi2)
{
	Matrix3333 res;
	for (unsigned int i = 0; i < 3; ++i)
	{
		for (unsigned int j = 0; j < 3; ++j)
		{
			res(i,j).setZero();
			for (unsigned int k = 0; k < 3; k++)
			{
				res(i,j) = multi1(i, k) * multi2(k, j);
			}
		}
	}
	return res;
}

Matrix3333 Matrix3333::operator* (ScalarType multi)
{
	Matrix3333 res;
	for (unsigned int i = 0; i < 3; ++i)
	{
		for (unsigned int j = 0; j < 3; ++j)
		{
			res.mat[i][j] = mat[i][j] * multi;
		}
	}
	return res;
}

Matrix3333 operator* (ScalarType multi1, Matrix3333& multi2)
{
	Matrix3333 res;
	for (unsigned int i = 0; i < 3; ++i)
	{
		for (unsigned int j = 0; j < 3; ++j)
		{
			res(i, j) = multi1 * multi2(i, j);
		}
	}
	return res;
}

Matrix3333 Matrix3333::transpose()
{
	Matrix3333 res;
	for (unsigned int i = 0; i < 3; ++i)
	{
		for (unsigned int j = 0; j < 3; ++j)
		{
			res(i, j) = mat[j][i];
		}
	}
	return res;
}
EigenMatrix3 Matrix3333::FrobeniusProduct(const EigenMatrix3& multi)
{
	EigenMatrix3 res;
	res.setZero();
	for (unsigned int i = 0; i < 3; ++i)
	{
		for (unsigned int j = 0; j < 3; ++j)
		{
			res += mat[i][j] * multi(i, j);
		}
	}
	return res;
}

// matrix2222
Matrix2222::Matrix2222()
{

}

Matrix2222::Matrix2222(const Matrix2222& other)
{
	for (unsigned int row = 0; row != 2; ++row)
	{
		for (unsigned int col = 0; col != 2; ++col)
		{
			mat[row][col] = other.mat[row][col];
		}
	}
}

void Matrix2222::SetZero()
{
	for (unsigned int row = 0; row != 2; ++row)
	{
		for (unsigned int col = 0; col != 2; ++col)
		{
			mat[row][col] = EigenMatrix2::Zero();
		}
	}
}

void Matrix2222::SetIdentity()
{
	for (unsigned int row = 0; row != 2; ++row)
	{
		for (unsigned int col = 0; col != 2; ++col)
		{
			mat[row][col] = EigenMatrix2::Zero();
			mat[row][col](row, col) = 1.0;
		}
	}
}

EigenMatrix2& Matrix2222::operator() (int row, int col)
{
	assert(row >= 0 && row < 2 && col >= 0 && col < 2);
	return mat[row][col];
}

Matrix2222 Matrix2222::operator+ (const Matrix2222& plus)
{
	Matrix2222 res;
	for (unsigned int row = 0; row != 2; ++row)
	{
		for (unsigned int col = 0; col != 2; ++col)
		{
			res.mat[row][col] = mat[row][col] + plus.mat[row][col];
		}
	}
	return res;
}

Matrix2222 Matrix2222::operator- (const Matrix2222& minus)
{
	Matrix2222 res;
	for (unsigned int row = 0; row != 2; ++row)
	{
		for (unsigned int col = 0; col != 2; ++col)
		{
			res.mat[row][col] = mat[row][col] - minus.mat[row][col];
		}
	}
	return res;
}

Matrix2222 Matrix2222::operator* (const EigenMatrix2& multi)
{
	Matrix2222 res;
	for (unsigned int i = 0; i < 2; ++i)
	{
		for (unsigned int j = 0; j < 2; ++j)
		{
			res.mat[i][j].setZero();
			for (unsigned int k = 0; k < 2; k++)
			{
				res.mat[i][j] += mat[i][k] * multi(k, j);
			}
		}
	}
	return res;
}
Matrix2222 operator* (const EigenMatrix2& multi1, Matrix2222& multi2)
{
	Matrix2222 res;
	for (unsigned int i = 0; i < 2; ++i)
	{
		for (unsigned int j = 0; j < 2; ++j)
		{
			res(i,j).setZero();
			for (unsigned int k = 0; k < 2; k++)
			{
				res(i,j) = multi1(i, k) * multi2(k, j);
			}
		}
	}
	return res;
}

Matrix2222 Matrix2222::operator* (ScalarType multi)
{
	Matrix2222 res;
	for (unsigned int i = 0; i < 2; ++i)
	{
		for (unsigned int j = 0; j < 2; ++j)
		{
			res.mat[i][j] = mat[i][j] * multi;
		}
	}
	return res;
}

Matrix2222 operator* (ScalarType multi1, Matrix2222& multi2)
{
	Matrix2222 res;
	for (unsigned int i = 0; i < 2; ++i)
	{
		for (unsigned int j = 0; j < 2; ++j)
		{
			res(i, j) = multi1 * multi2(i, j);
		}
	}
	return res;
}

Matrix2222 Matrix2222::transpose()
{
	Matrix2222 res;
	for (unsigned int i = 0; i < 2; ++i)
	{
		for (unsigned int j = 0; j < 2; ++j)
		{
			res(i, j) = mat[j][i];
		}
	}
	return res;
}
EigenMatrix2 Matrix2222::FrobeniusProduct(const EigenMatrix2& multi)
{
	EigenMatrix2 res;
	res.setZero();
	for (unsigned int i = 0; i < 2; ++i)
	{
		for (unsigned int j = 0; j < 2; ++j)
		{
			res += mat[i][j] * multi(i, j);
		}
	}
	return res;
}

void directProduct(Matrix3333& dst, const EigenMatrix3 src1, const EigenMatrix3& src2)
{
	for (unsigned int i = 0; i < 3; ++i)
	{
		for (unsigned int j = 0; j < 3; ++j)
		{
			dst(i, j) = src1(i, j) * src2;
		}
	}
}
void directProduct(Matrix2222& dst, const EigenMatrix2 src1, const EigenMatrix2& src2)
{
	for (unsigned int i = 0; i < 2; ++i)
	{
		for (unsigned int j = 0; j < 2; ++j)
		{
			dst(i, j) = src1(i, j) * src2;
		}
	}
}
