//#####################################################################
// Copyright 2003-2004, Zhaosheng Bao, Ronald Fedkiw, Geoffrey Irving, Neil Molino, Duc Nguyen, Eftychios Sifakis, Joseph Teran, Rachel Weinstein.
// This file is part of PhysBAM whose distribution is governed by the license contained in the accompanying file PHYSBAM_COPYRIGHT.txt.
//#####################################################################
// Class MATRIX_2X2
//#####################################################################
#ifndef __MATRIX_2X2__
#define __MATRIX_2X2__

#include <assert.h>
#include "VECTOR_2D.h"
#include "VECTOR_3D.h"
#include "SYMMETRIC_MATRIX_2X2.h"
#include "../Math_Tools/sqr.h"
#include "../Math_Tools/constants.h"
#include "../Math_Tools/exchange.h"
namespace PhysBAM
{

template<class T>
class MATRIX_2X2
{
public:
	T x[4];

	MATRIX_2X2()
	{
		for (int i = 0; i < 4; i++) x[i] = 0;
	}

	MATRIX_2X2 (const MATRIX_2X2<T>& matrix_input)
	{
		for (int i = 0; i < 4; i++) x[i] = matrix_input.x[i];
	}

	MATRIX_2X2 (const SYMMETRIC_MATRIX_2X2<T>& matrix_input)
	{
		x[0] = matrix_input.x11;
		x[1] = x[2] = matrix_input.x21;
		x[3] = matrix_input.x22;
	}

	MATRIX_2X2 (const UPPER_TRIANGULAR_MATRIX_2X2<T>& matrix_input)
	{
		x[0] = matrix_input.x11;
		x[2] = matrix_input.x12;
		x[3] = matrix_input.x22;
		x[1] = 0;
	}

	MATRIX_2X2 (const T x11, const T x21, const T x12, const T x22)
	{
		x[0] = x11;
		x[1] = x21;
		x[2] = x12;
		x[3] = x22;
	}

	MATRIX_2X2 (const VECTOR_2D<T> & column1, const VECTOR_2D<T> & column2)
	{
		x[0] = column1.x;
		x[1] = column1.y;
		x[2] = column2.x;
		x[3] = column2.y;
	}

	T& operator() (const int i, const int j)
	{
		assert (i >= 1 && i <= 2);
		assert (j >= 1 && j <= 2);
		return x[i - 1 + 2 * (j - 1)];
	}

	const T& operator() (const int i, const int j) const
	{
		assert (i >= 1 && i <= 2);
		assert (j >= 1 && j <= 2);
		return x[i - 1 + 2 * (j - 1)];
	}

	VECTOR_2D<T>& Column (const int j)
	{
		assert (1 <= j && j <= 2);
		return * (VECTOR_2D<T>*) (x + 2 * (j - 1));
	}

	const VECTOR_2D<T>& Column (const int j) const
	{
		assert (1 <= j && j <= 2);
		return * (VECTOR_2D<T>*) (x + 2 * (j - 1));
	}

	MATRIX_2X2<T> operator-() const
	{
		return MATRIX_2X2<T> (-x[0], -x[1], -x[2], -x[3]);
	}

	MATRIX_2X2<T>& operator+= (const MATRIX_2X2<T>& A)
	{
		for (int i = 0; i < 4; i++) x[i] += A.x[i];

		return *this;
	}

	MATRIX_2X2<T>& operator+= (const T a)
	{
		x[0] += a;
		x[3] += a;
		return *this;
	}

	MATRIX_2X2<T>& operator-= (const MATRIX_2X2<T>& A)
	{
		for (int i = 0; i < 4; i++) x[i] -= A.x[i];

		return *this;
	}

	MATRIX_2X2<T>& operator-= (const T a)
	{
		x[0] -= a;
		x[3] -= a;
		return *this;
	}

	MATRIX_2X2<T>& operator*= (const MATRIX_2X2<T>& A)
	{
		return *this = *this * A;
	}

	MATRIX_2X2<T>& operator*= (const T a)
	{
		for (int i = 0; i < 4; i++) x[i] *= a;

		return *this;
	}

	MATRIX_2X2<T>& operator/= (const T a)
	{
		assert (a != 0);
		T s = 1 / a;

		for (int i = 0; i < 4; i++) x[i] *= s;

		return *this;
	}

	MATRIX_2X2<T> operator+ (const MATRIX_2X2<T>& A) const
	{
		return MATRIX_2X2<T> (x[0] + A.x[0], x[1] + A.x[1], x[2] + A.x[2], x[3] + A.x[3]);
	}

	MATRIX_2X2<T> operator+ (const T a) const
	{
		return MATRIX_2X2<T> (x[0] + a, x[1], x[2], x[3] + a);
	}

	MATRIX_2X2<T> operator- (const MATRIX_2X2<T>& A) const
	{
		return MATRIX_2X2<T> (x[0] - A.x[0], x[1] - A.x[1], x[2] - A.x[2], x[3] - A.x[3]);
	}

	MATRIX_2X2<T> operator- (const T a) const
	{
		return MATRIX_2X2<T> (x[0] - a, x[1], x[2], x[3] - a);
	}

	MATRIX_2X2<T> operator* (const MATRIX_2X2<T>& A) const
	{
		return MATRIX_2X2<T> (x[0] * A.x[0] + x[2] * A.x[1], x[1] * A.x[0] + x[3] * A.x[1], x[0] * A.x[2] + x[2] * A.x[3], x[1] * A.x[2] + x[3] * A.x[3]);
	}

	MATRIX_2X2<T> operator* (const T a) const
	{
		return MATRIX_2X2<T> (a * x[0], a * x[1], a * x[2], a * x[3]);
	}

	MATRIX_2X2<T> operator/ (const T a) const
	{
		assert (a != 0);
		T s = 1 / a;
		return MATRIX_2X2<T> (s * x[0], s * x[1], s * x[2], s * x[3]);
	}

	VECTOR_2D<T> operator* (const VECTOR_2D<T> & v) const
	{
		return VECTOR_2D<T> (x[0] * v.x + x[2] * v.y, x[1] * v.x + x[3] * v.y);
	}

	T Determinant() const
	{
		return x[0] * x[3] - x[1] * x[2];
	}

	void Invert()
	{
		*this = Inverse();
	}

	MATRIX_2X2<T> Inverse() const
	{
		T one_over_determinant = 1 / (x[0] * x[3] - x[1] * x[2]);
		return MATRIX_2X2<T> (one_over_determinant * x[3], -one_over_determinant * x[1], -one_over_determinant * x[2], one_over_determinant * x[0]);
	}

	MATRIX_2X2<T> Inverse_Transpose() const
	{
		return Inverse().Transposed();
	}

	VECTOR_2D<T> Solve_Linear_System (const VECTOR_2D<T>& b) const
	{
		T one_over_determinant = 1 / (x[0] * x[3] - x[1] * x[2]);
		return one_over_determinant * VECTOR_2D<T> (x[3] * b.x - x[2] * b.y, x[0] * b.y - x[1] * b.x);
	}

	void Transpose()
	{
		exchange (x[1], x[2]);
	}

	MATRIX_2X2<T> Transposed() const
	{
		return MATRIX_2X2<T> (x[0], x[2], x[1], x[3]);
	}

	T Trace() const
	{
		return x[0] + x[3];
	}

	MATRIX_2X2<T> Cofactor_Matrix() const
	{
		return MATRIX_2X2<T> (x[3], -x[2], -x[1], x[0]);
	}

	SYMMETRIC_MATRIX_2X2<T> Normal_Equations_Matrix() const
	{
		return SYMMETRIC_MATRIX_2X2<T> (x[0] * x[0] + x[1] * x[1], x[0] * x[2] + x[1] * x[3], x[2] * x[2] + x[3] * x[3]);
	}

	SYMMETRIC_MATRIX_2X2<T> Outer_Product_Matrix() const
	{
		return SYMMETRIC_MATRIX_2X2<T> (x[0] * x[0] + x[2] * x[2], x[0] * x[1] + x[2] * x[3], x[1] * x[1] + x[3] * x[3]);
	}

	SYMMETRIC_MATRIX_2X2<T> Symmetric_Part() const
	{
		return SYMMETRIC_MATRIX_2X2<T> (x[0], (T).5 * (x[1] + x[2]), x[3]);
	}

	SYMMETRIC_MATRIX_2X2<T> Twice_Symmetric_Part() const
	{
		return SYMMETRIC_MATRIX_2X2<T> (2 * x[0], x[1] + x[2], 2 * x[3]);
	}

	DIAGONAL_MATRIX_2X2<T> Diagonal_Part() const
	{
		return DIAGONAL_MATRIX_2X2<T> (x[0], x[3]);
	}

	static MATRIX_2X2<T> Transpose (const MATRIX_2X2<T>& A)
	{
		return MATRIX_2X2<T> (A.x[0], A.x[2], A.x[1], A.x[3]);
	}

	static MATRIX_2X2<T> Identity_Matrix()
	{
		return MATRIX_2X2<T> (1, 0, 0, 1);
	}

	static MATRIX_2X2<T> Diagonal_Matrix (const T d)
	{
		return MATRIX_2X2<T> (d, 0, 0, d);
	}

	T Antisymmetric_Part_Cross_Product_Vector_As_Scalar() const
	{
		return (T).5 * (x[1] - x[2]);
	}

	VECTOR_3D<T> Antisymmetric_Part_Cross_Product_Vector() const
	{
		return VECTOR_3D<T> (0, 0, (T).5 * (x[1] - x[2]));
	}

	static MATRIX_2X2<T> Rotation_Matrix (const T radians)
	{
		T c = cos (radians), s = sin (radians);
		return MATRIX_2X2<T> (c, s, -s, c);
	}

	static MATRIX_2X2<T> Derivative_Rotation_Matrix (const T radians)
	{
		T c = cos (radians), s = sin (radians);
		return MATRIX_2X2<T> (-s, c, -c, -s);
	}

	static MATRIX_2X2<T> Outer_Product (const VECTOR_2D<T>& u, const VECTOR_2D<T>& v)
	{
		return MATRIX_2X2<T> (u.x * v.x, u.y * v.x, u.x * v.y, u.y * v.y);
	}

	T Frobenius_Norm() const
	{
		return sqrt (sqr (x[0]) + sqr (x[1]) + sqr (x[2]) + sqr (x[3]));
	}

	T Max_Abs_Element() const
	{
		return max (fabs (x[0]), fabs (x[1]), fabs (x[2]), fabs (x[3]));
	}

	MATRIX_2X2<T> operator* (const DIAGONAL_MATRIX_2X2<T>& A) const
	{
		return MATRIX_2X2<T> (x[0] * A.x11, x[1] * A.x11, x[2] * A.x22, x[3] * A.x22);
	}

	MATRIX_2X2<T> operator* (const UPPER_TRIANGULAR_MATRIX_2X2<T>& A) const
	{
		return MATRIX_2X2<T> (x[0] * A.x11, x[1] * A.x11, x[0] * A.x12 + x[2] * A.x22, x[1] * A.x12 + x[3] * A.x22);
	}

	MATRIX_2X2<T> operator* (const SYMMETRIC_MATRIX_2X2<T>& A) const
	{
		return MATRIX_2X2<T> (x[0] * A.x11 + x[2] * A.x21, x[1] * A.x11 + x[3] * A.x21, x[0] * A.x21 + x[2] * A.x22, x[1] * A.x21 + x[3] * A.x22);
	}

	MATRIX_2X2<T> Multiply_With_Transpose (const UPPER_TRIANGULAR_MATRIX_2X2<T>& A) const
	{
		return MATRIX_2X2<T> (x[0] * A.x11 + x[2] * A.x12, x[1] * A.x11 + x[3] * A.x12, x[2] * A.x22, x[3] * A.x22);
	}

	MATRIX_2X2<T> Multiply_With_Transpose (const MATRIX_2X2<T>& A) const
	{
		return MATRIX_2X2<T> (x[0] * A.x[0] + x[2] * A.x[2], x[1] * A.x[0] + x[3] * A.x[2], x[0] * A.x[1] + x[2] * A.x[3], x[1] * A.x[1] + x[3] * A.x[3]);
	}

	MATRIX_2X2<T> Transpose_Times (const MATRIX_2X2<T>& A) const
	{
		return MATRIX_2X2<T> (x[0] * A.x[0] + x[1] * A.x[1], x[2] * A.x[0] + x[3] * A.x[1], x[0] * A.x[2] + x[1] * A.x[3], x[2] * A.x[2] + x[3] * A.x[3]);
	}

	VECTOR_2D<T> Transpose_Times (const VECTOR_2D<T>& v) const
	{
		return VECTOR_2D<T> (x[0] * v.x + x[1] * v.y, x[2] * v.x + x[3] * v.y);
	}

	UPPER_TRIANGULAR_MATRIX_2X2<T> R_From_QR_Factorization() const
	{
		if (x[1] == 0) return UPPER_TRIANGULAR_MATRIX_2X2<T> (x[0], x[2], x[3]);

		T c, s;

		if (fabs (x[1]) > fabs (x[0]))
		{
			T t = -x[0] / x[1];
			s = 1 / sqrt (1 + t * t);
			c = s * t;
		}
		else
		{
			T t = -x[1] / x[0];
			c = 1 / sqrt (1 + t * t);
			s = c * t;
		}

		return UPPER_TRIANGULAR_MATRIX_2X2<T> (c * x[0] - s * x[1], c * x[2] - s * x[3], s * x[2] + c * x[3]);
	}

	void Indefinite_Polar_Decomposition (MATRIX_2X2<T>& Q, SYMMETRIC_MATRIX_2X2<T>& S) const
	{
		T x03 = x[0] + x[3], cosine, sine;

		if (x03 == 0)
		{
			cosine = 0;
			sine = 1;
		}
		else
		{
			T t = (x[1] - x[2]) / x03;
			cosine = 1 / sqrt (1 + t * t);
			sine = t * cosine;
		}

		Q = MATRIX_2X2<T> (cosine, sine, -sine, cosine);
		S = SYMMETRIC_MATRIX_2X2<T> (Q.x[0] * x[0] + Q.x[1] * x[1], Q.x[0] * x[2] + Q.x[1] * x[3], Q.x[2] * x[2] + Q.x[3] * x[3]);
	}

	void Fast_Singular_Value_Decomposition (MATRIX_2X2<T>& U, DIAGONAL_MATRIX_2X2<T>& singular_values, MATRIX_2X2<T>& V) const
	{
		MATRIX_2X2<T> Q;
		SYMMETRIC_MATRIX_2X2<T> S;
		Indefinite_Polar_Decomposition (Q, S);
		S.Solve_Eigenproblem (singular_values, V);

		if (singular_values.x22 < 0 && fabs (singular_values.x22) >= fabs (singular_values.x11))
		{
			singular_values = DIAGONAL_MATRIX_2X2<T> (-singular_values.x22, -singular_values.x11);
			Q = -Q;
			V = MATRIX_2X2<T> (V.x[2], V.x[3], -V.x[0], -V.x[1]);
		}

		U = Q * V;
	}

	static T Determinant_Differential (const MATRIX_2X2<T>& A, const MATRIX_2X2<T>& dA)
	{
		return dA.x[0] * A.x[3] + A.x[0] * dA.x[3] - dA.x[1] * A.x[2] - A.x[1] * dA.x[2];
	}

	static MATRIX_2X2<T> Cofactor_Differential (const MATRIX_2X2<T>& dA)
	{
		return dA.Cofactor_Matrix();
	}

	T Triangle_Minimum_Altitude() const
	{
		return Determinant() / sqrt (max (Column (1).Magnitude_Squared(), Column (2).Magnitude_Squared(), (Column (1) - Column (2)).Magnitude_Squared()));
	}

//#####################################################################
};
// global functions
template<class T>
inline MATRIX_2X2<T> operator* (const T a, const MATRIX_2X2<T>& A)
{
	return MATRIX_2X2<T> (a * A.x[0], a * A.x[1], a * A.x[2], a * A.x[3]);
}

template<class T>
inline VECTOR_2D<T> operator* (const VECTOR_2D<T>& v, const MATRIX_2X2<T>& A)
{
	return VECTOR_2D<T> (v.x * A.x[0] + v.y * A.x[2], v.x * A.x[1] + v.y * A.x[3]);
}

template<class T>
inline MATRIX_2X2<T> operator* (const DIAGONAL_MATRIX_2X2<T>& A, const MATRIX_2X2<T>& B)
{
	return MATRIX_2X2<T> (A.x11 * B.x[0], A.x22 * B.x[1], A.x11 * B.x[2], A.x22 * B.x[3]);
}

template<class T>
inline MATRIX_2X2<T> operator* (const UPPER_TRIANGULAR_MATRIX_2X2<T>& A, const MATRIX_2X2<T>& B)
{
	return MATRIX_2X2<T> (A.x11 * B.x[0] + A.x12 * B.x[1], A.x22 * B.x[1], A.x11 * B.x[2] + A.x12 * B.x[3], A.x22 * B.x[3]);
}

template<class T>
inline std::istream& operator>> (std::istream& input_stream, MATRIX_2X2<T>& A)
{
	for (int i = 0; i < 2; i++) for (int j = 0; j < 2; j++) input_stream >> A.x[i + j * 2];

	return input_stream;
}

template<class T>
inline std::ostream& operator<< (std::ostream& output_stream, const MATRIX_2X2<T>& A)
{
	for (int i = 0; i < 2; i++)
	{
		for (int j = 0; j < 2; j++) output_stream << A.x[i + j * 2] << " ";

		output_stream << std::endl;
	}

	return output_stream;
}
}
#endif