#ifndef MATRIX2D2_HH
#define MATRIX2D2_HH
//////////////////////////////////////////////////////////////////////////
//! file="amma/Geometry/AnGeo2/Matrix2d.hh"
//! lib=Mag2
//! userlevel=Normal
//! author="Radek Marik"
//! docentry="Geometry.2-D;Basic Types.Numerical.Specialised"
//! date="26.04.1994"
//! example=exMat2d.cc
//! rcsid="$Id: Matrix2d.hh,v 1.11 2000/10/04 12:48:52 ees1cg Exp $"

#include "amma/StdType.hh"
#include "amma/Index.hh"
#include "amma/SBfAcc.hh"

class Point2dC;
class Vector2dC;
class VecMat2dC;
class MatrixC;
class ostream;
class istream;

// ------------------------------------------------------------------
// ******** Matrix2d2C **********************************************
// ------------------------------------------------------------------

//: The class Matrix2d2C provides a quick implementation of a 2x2 matrix.
//  The 'C' convention of indexing from 0 is followed.

class Matrix2d2C
{
public:
  /* Type definitions.
   * ================= */
  
  enum InputDataT {ROWS, COLUMNS};
  //: The kind of input data.
    
public:
  
  // Constructors, assigment, and destructor.
  /* ----------------------------------------*/
  
  inline Matrix2d2C();
  //: Creates the zero matrix.
  
  Matrix2d2C(RealT b00, RealT b01, RealT b10, RealT b11);
  //: Creates the matrix from 4 numbers.
  
  Matrix2d2C(const Matrix2d2C & mat);
  //: Copy constructor.

  Matrix2d2C(const Point2dC & p0,
	     const Point2dC & p1,
	     const InputDataT dType = ROWS);
  //: Creates a new matrix and assign the values according to the points
  //: coordinates. 
  // If 'dType' is equal to ROWS, the points are treated
  // as rows of the matrix. If 'dType' is equal to COLUMNS, the points
  // are treated as columns of the matrix.
  
  
  Matrix2d2C(const MatrixC & mat);
  //: Creates the matrix from the upper-left submatrix of the matrix 'mat'.
  
  const Matrix2d2C & operator=(const Matrix2d2C & mat);
  //: Assigment.
  
  // Tests of states of a matrix.
  /* ----------------------------*/
  
  BooleanT operator==(const Matrix2d2C & mat) const;
  //: Returns TRUE if all elements of this matrix are equal to the related
  //: elements of the matrix 'mat'.
  
  inline BooleanT IsSymmetric() const;
  //: Returns TRUE if this matrix is symmetric.
  
  // Setting of matrix elements.
  /* ---------------------------*/
  
  void Set(const Matrix2d2C & mat);
  //: Sets all elements of the matrix according to the matrix 'mat'.
  
  inline void Set(RealT b00, RealT b01, RealT b10, RealT b11);
  //: Sets the elements of this matrix according to the values 'b00',
  //: 'b01', 'b10', and 'b11'.
  
  inline Matrix2d2C & SetZero();
  //: Sets all elements to be 0.0.
  
  Matrix2d2C & SetRow(IndexT i, const Point2dC & p);
  //: Sets the elements of the row 'i' according to the elements of 
  //: the point 'p'.
  
  Matrix2d2C & SetColumn(IndexT i, const Point2dC & p);
  //: Sets the elements of the column 'i' according to the elements
  //: of the point 'p'.
  
  // Access to the parameters of the object.
  /* ---------------------------------------*/
  
  inline SizeT RDim() const;
  //: Returns the number of rows.
  
  inline SizeT CDim() const;
  //: Returns the number of columns.
  
  inline const Matrix2d2C & Matrix() const;
  //: Access to the constant matrix.
  
  inline Matrix2d2C & Matrix();                     
  //: Access to the matrix.
  
  inline const SizeBufferAccessC<RealT>  operator[](IndexT r) const;
  //: Access to the row of the constant matrix.
  
  inline SizeBufferAccessC<RealT>  operator[](IndexT r);
  //: Access to the row of the matrix.
  
  inline RealT operator()(IndexT i, IndexT j) const; 
  //: Returns the value of the element a[ij].
  
  inline RealT & operator()(IndexT i, IndexT j); 
  //: Access to the value of the element a[ij].
  
  inline const RealT & A00() const;
  //: Access to the value of the constant element a00.
 
  inline const RealT & A01() const;
  //: Access to the value of the constant element a01.
  
  inline const RealT & A10() const;
  //: Access to the value of the constant element a10.
  
  inline const RealT & A11() const;
  //: Access to the value of the constant element a11.

  inline RealT & A00();
  //: Access to the value of the element a00.
  
  inline RealT & A01();
  //: Access to the value of the element a01.
  
  inline RealT & A10();
  //: Access to the value of the element a10.
  
  inline RealT & A11();
  //: Access to the value of the element a11.
  
  RealT G(IndexT i, IndexT j) const;
  //: Returns the value of the element (i,j).
  
  Vector2dC Column(IndexT i) const;
  //: Returns the i-th column of this matrix.
  
  Vector2dC Row(IndexT i) const;
  //: Returns the i-th row of this matrix.
  
  Vector2dC Diagonal() const;
  //: Returns the vector of main diagonal item.
  
  // Arithmetic operations
  /* ---------------------*/
  
  inline const Matrix2d2C & operator+=(const Matrix2d2C & mat);
  //: Adds the matrix 'mat' to this matrix.
  
  inline const Matrix2d2C & operator-=(const Matrix2d2C & mat);
  //: Subtracts the matrix 'mat' from this matrix.
  
  inline const Matrix2d2C & operator*=(RealT lambda);
  //: Multiplies all element of this matrix by the scalar 'lambda'.

  inline const Matrix2d2C & operator/=(RealT lambda);
  //: Divides all element of this matrix by the scalar 'lambda'.
  
  Matrix2d2C operator+(const Matrix2d2C & mat) const;
  //: Adds the matrix 'mat' and this matrix.
  
  Matrix2d2C operator-(const Matrix2d2C & mat) const;
  //: Subtracts the matrix 'mat' from this matrix.
  
  Matrix2d2C operator*(RealT lambda) const;
  //: Multiplies all element of this matrix by the scalar 'lambda'.
  
  Matrix2d2C operator/(RealT lambda) const;
  //: Divides all element of this matrix by the scalar 'lambda'.
  
  Matrix2d2C operator*(const Matrix2d2C & mat) const;
  //: Multiplies this matrix by the matrix 'mat'.

  Vector2dC operator*(const Vector2dC & v) const;
  //: Multiplies this matrix and the vector 'v'.
  
  Point2dC operator*(const Point2dC & p) const;
  //: Multiplies this matrix and the point 'p'.
  
  
  // Matrix operators.
  // =================
  
  inline RealT Sum() const;
  //: Returns the sum of all elements of this matrix.
  
  inline RealT Sum2() const;
  //: Returns the sum of square values of all elements of this matrix.
  
  inline Matrix2d2C Sqr() const;
  //: Returns the matrix which elements are squares of the elements of this
  //: matrix.
  
  inline Matrix2d2C T() const;
  //: Returns the transposition of this matrix.
  
  Matrix2d2C I() const;
  //: Returns the inversion of this matrix.
  
  inline RealT Det() const;
  //: Returns the value of the determinant of this matrix.
  
  inline RealT Tr() const;
  //: Returns the sum of the diagonal elements of this matrix.
  
  inline Matrix2d2C EigenValues() const;
  //: Returns the diagonal matrix containing the eigenvalues of this matrix.
  // The matrix must be symmetric. The element A00() will contain the bigger
  // eigenvalue according to its absolute value.
  
  inline Matrix2d2C SingularValues() const;
  //: Returns the diagonal matrix containing the singular values
  //: of this matrix. 
  // The element A00() will contain the bigger
  // singular value according to its absolute value.
  
  VecMat2dC EigenSystem() const;
  //: Returns the vector with eigenvalues of this symmetric matrix and
  //: the matrix whose columns are related eigenvectors of this matrix.
  // The eigenvalues are ordered according to their absolute values.
  // The eigenvectors are normalized to be unit. The first eigenvector
  // will point to the I. or IV. quadrant.
  
  inline void SVD(Matrix2d2C & u, Matrix2d2C & s, Matrix2d2C & v) const;
  //: Singular Value Decomposition.
  // Updates the matrices 'u', 's', 'v' to be the solution of the singular
  // value decomposition of this matrix 'm = u * s * v.T()', where 'u' 
  // and 'v' are orthogonal matrices and 's' is the diagonal matrix
  // containing singular values ordered according to their absolute values.
  // The first eigenvector of both matrices will point to the I. 
  // or IV. quadrant.
  
  Point2dC LinearSolver(const Point2dC & rightSide) const;
  //: Returns the solution of 2 linear equations determined 
  //: by the matrix and the right side vector 'rightSide'
  
  static Matrix2d2C ProductT(const Vector2dC & a);
  //: Returns the matrix that is result of a*a.T().
  
  // Error checking.
  // ===============
  
  inline void ErrOutOfRange(IndexT i, const char * functionName) const;
  //: Triggers the error handling if the index 'i' is smaller than 0
  //: or bigger than 1. 
  // The name of the calling function 'functionName' is passed to the error handler.
  
protected:
  
  inline Matrix2d2C EigenVectors(const Matrix2d2C & eigenVal) const;
  //: Calculate the Eigen Vectors
  // Returns the matrix which columns are eigenvectors related
  // to the eigenvalues
  // of this matrix stored in the diagonal matrix 'eigenVal'. 
  // The eigenvectors are normalize to be unit. The first eigenvector
  // will point to the I. or IV. quadrant.
  
private:
  
  // Object representation.
  // ======================
  enum {dim = 2}; // the dimension of matrix
  
  RealT a[dim][dim]; // The matrix elements.

  friend ostream & operator<<(ostream & outS, const Matrix2d2C & matrix);
  friend istream & operator>>(istream & inS, Matrix2d2C & matrix);
};    

class MatrixC;

Matrix2d2C
DirInvCovariance(const Point2dC & point,
                 const Point2dC & center,
                 const Point2dC & boundaryPoint,
                 RealT sigma1,
                 RealT sigma2);

Matrix2d2C ToMatrix2d2(const MatrixC & matrix);

ostream & operator<<(ostream & outS, const Matrix2d2C & m);
// Sends the matrix 'm' to the output stream 'outS'.

istream & operator>>(istream & inS, Matrix2d2C & m);
// Reads and sets the elements of the matrix 'm' from the input stream 'inS'.

namespace StdMath {
  inline 
  void SetToZero(Matrix2d2C &dat)
  { dat = Matrix2d2C(); } 
  //: Set 'dat' to zero.
}


#include "amma/Error.hh"
#include "amma/StdMath.hh"
#include "amma/UsefulFn.hh"
#include "amma/Vector2d.hh"

// -----------------------------------------------------------------
// ******** Matrix2dC **********************************************
// -----------------------------------------------------------------

inline 
void 
Matrix2d2C::ErrOutOfRange(IndexT i,
                          const char * functionName) const
//========================================================
{
  if ((i < 0) || (i >= (IntT) dim))
  {
    errAMMA << "the index out of range <0," << (dim-1) << ">: " << i;
    errAMMA.Function(functionName).Exit();
  }
}

inline 
SizeT 
Matrix2d2C::RDim() const
//======================
{ return dim; }

inline 
SizeT 
Matrix2d2C::CDim() const
//======================
{ return dim; }

inline 
RealT
Matrix2d2C::operator()(IndexT i, IndexT j) const
//======================================================================
{
  #ifdef AMMA_CHECK
    ErrOutOfRange(i, "Matrix2d2C::operator()(IndexT i, IndexT) const");
    ErrOutOfRange(j, "Matrix2d2C::operator()(IndexT, IndexT j) const");
  #endif
  return a[i.V()][j.V()];
}

inline 
RealT &
Matrix2d2C::operator()(IndexT i, IndexT j)
//================================================================
{
  #ifdef AMMA_CHECK
    ErrOutOfRange(i, "Matrix2d2C::operator()(IndexT i, IndexT) const");
    ErrOutOfRange(j, "Matrix2d2C::operator()(IndexT, IndexT j) const");
  #endif
  return a[i.V()][j.V()];
}

inline 
BooleanT 
Matrix2d2C::IsSymmetric() const
//=============================
{ return a[0][1] == a[1][0]; }

inline
Matrix2d2C &
Matrix2d2C::SetZero()
//===================
{
  a[0][0] = 0.0;
  a[0][1] = 0.0;

  a[1][0] = 0.0;
  a[1][1] = 0.0;
  return *this;
}

inline 
void 
Matrix2d2C::Set(RealT b00, RealT b01,
                RealT b10, RealT b11)
//===========================================================
{
  a[0][0] = b00;
  a[0][1] = b01;

  a[1][0] = b10;
  a[1][1] = b11;
}

inline
Matrix2d2C::Matrix2d2C()
//======================
{ SetZero(); }

inline 
const Matrix2d2C &
Matrix2d2C::Matrix() const
//========================
{ return *this; }

inline 
Matrix2d2C & 
Matrix2d2C::Matrix()                     
//==================
{ return *this; }

inline 
const Matrix2d2C & 
Matrix2d2C::operator+=(const Matrix2d2C & mat)
//============================================
{
  a[0][0] += mat.a[0][0];  a[0][1] += mat.a[0][1];
  a[1][0] += mat.a[1][0];  a[1][1] += mat.a[1][1]; 
  return *this;
}

inline 
const Matrix2d2C & 
Matrix2d2C::operator-=(const Matrix2d2C & mat)
//============================================
{
  a[0][0] -= mat.a[0][0];  a[0][1] -= mat.a[0][1];
  a[1][0] -= mat.a[1][0];  a[1][1] -= mat.a[1][1];
  return *this;
}

inline 
const Matrix2d2C & 
Matrix2d2C::operator*=(RealT lambda)
//==================================
{
  a[0][0] *= lambda;  a[0][1] *= lambda;
  a[1][0] *= lambda;  a[1][1] *= lambda;
  return *this;
}

inline 
const Matrix2d2C & 
Matrix2d2C::operator/=(RealT lambda)
//==================================
{
  a[0][0] /= lambda;  a[0][1] /= lambda;
  a[1][0] /= lambda;  a[1][1] /= lambda;
  return *this;
}

inline
const SizeBufferAccessC<RealT> 
Matrix2d2C::operator[](IndexT r) const
//========================================
{
#ifdef AMMA_CHECK
    ErrOutOfRange(r, "Matrix2d2C::operator[](IndexT i) const");
#endif
  return SizeBufferAccessC<RealT>((RealT*)a[r.V()],CDim());
}

inline
SizeBufferAccessC<RealT> 
Matrix2d2C::operator[](IndexT r)
//================================
{
#ifdef AMMA_CHECK
    ErrOutOfRange(r, "Matrix2d2C::operator[](IndexT i) const");
#endif
  return SizeBufferAccessC<RealT>((RealT*)a[r.V()],CDim());
}

inline 
const RealT & 
Matrix2d2C::A00() const
//=====================
{ return a[0][0]; }

inline 
const RealT & 
Matrix2d2C::A01() const
//=====================
{ return a[0][1]; }

inline 
const RealT & 
Matrix2d2C::A10() const
//=====================
{ return a[1][0]; }

inline 
const RealT & 
Matrix2d2C::A11() const
//=====================
{ return a[1][1]; }

inline 
RealT &Matrix2d2C::A00()
{ return a[0][0]; }

inline 
RealT &Matrix2d2C::A01()
{ return a[0][1]; }

inline 
RealT & 
Matrix2d2C::A10()
//===============
{ return a[1][0]; }

inline 
RealT & 
Matrix2d2C::A11()
//===============
{ return a[1][1]; }

inline 
Matrix2d2C 
Matrix2d2C::Sqr() const
//=====================
{
  return Matrix2d2C(a[0][0] * a[0][0],  a[1][0] * a[1][0],
                    a[0][1] * a[0][1],  a[1][1] * a[1][1]);
}

inline 
RealT 
Matrix2d2C::Sum() const
//=====================
{ return a[0][0] + a[0][1] + a[1][0] + a[1][1]; }

inline 
RealT 
Matrix2d2C::Sum2() const
//======================
{ return ::Sqr(a[0][0]) + ::Sqr(a[0][1]) + ::Sqr(a[1][0]) + ::Sqr(a[1][1]); }


inline 
Matrix2d2C 
Matrix2d2C::T() const
//===================
{
  return Matrix2d2C(a[0][0], a[1][0],
                    a[0][1], a[1][1]);
}

inline 
RealT
Matrix2d2C::Det() const
//=====================
{ return a[0][0]*a[1][1] - a[0][1]*a[1][0]; }

inline 
RealT 
Matrix2d2C::Tr() const
//====================
{ return a[0][0] + a[1][1]; }

inline 
Matrix2d2C 
Matrix2d2C::EigenValues() const
//=============================
// Ref.: Radek Marik, Maths, p.29
{
  if (A01() == 0)
  { // The matrix is diagonal
    if (A00() > A11())
      return Matrix2d2C(A00(),   0.0,
                          0.0, A11());
    else
      return Matrix2d2C(A11(),   0.0,
                          0.0, A00());
  }
  const RealT d  = ::Sqr(A00()-A11()) + 4 * ::Sqr(A01());

  #ifdef AMMA_CHECK
    if (d < 0)
    {
      errAMMA << "Not expected complex eigenvalues: d = " << d;
      errAMMA.Function("Matrix2d2C::EigenValues() const").Exit();
    }
  #endif

  const RealT sd = ::Sqrt(d);
  RealT lambda1 = (A00()+A11() + sd)/2.0;
  RealT lambda2 = (A00()+A11() - sd)/2.0;
  if (::Abs(lambda1) < ::Abs(lambda2))
    AMMA_Swap(lambda1,lambda2);
  return Matrix2d2C(lambda1,       0,
                          0, lambda2);
}

inline 
Matrix2d2C 
Matrix2d2C::SingularValues() const
//================================
// Ref.: Radek Marik, Maths, p.29
{
  const RealT det = Det();
  const RealT s2  = Sum2();
  const RealT d1  = s2 - 2 * det;
  const RealT d2  = s2 + 2 * det;

  #ifdef AMMA_CHECK
    if (d1 < 0 || d2 < 0)
    {
      errAMMA << "Not expected complex singular values:\n"
              << "d1 = " << d1 << '\n'
              << "d2 = " << d2;
      errAMMA.Function("Matrix2d2C::SingularValues() const").Exit();
    }
  #endif

  const RealT sd1 = ::Sqrt(d1);
  const RealT sd2 = ::Sqrt(d2);
  RealT sigma1 = (sd1 - sd2)/2.0;
  RealT sigma2 = (sd1 + sd2)/2.0;
  cout << "det: " << det << '\n'
       << "s2:  " << s2  << '\n'
       << ((sd1 - sd2)/2.0) << ' ' << ((sd1 + sd2)/2.0) << ' ' 
       << ((- sd1 - sd2)/2.0) << ' ' << ((- sd1 + sd2)/2.0) << '\n'; 
  if (::Abs(sigma1) < ::Abs(sigma2))
    AMMA_Swap(sigma1,sigma2);
  return Matrix2d2C(sigma1,      0,
                         0, sigma2);
}

inline 
Matrix2d2C 
Matrix2d2C::EigenVectors(const Matrix2d2C & eigenVal) const
//=========================================================
{
  const RealT lambda = eigenVal.A00();
  Vector2dC u1( A01(), lambda - A00());
  u1 /= u1.Modulus();
  return Matrix2d2C(u1, u1.Perpendicular(), COLUMNS);
}

inline 
void 
Matrix2d2C::SVD(Matrix2d2C & u, Matrix2d2C & s, Matrix2d2C & v) const
//===================================================================
// Ref.: Radek Marik, Maths, pp.30-31
{
  s = SingularValues();
  Matrix2d2C ssT(::Sqr(s.A00()),        0      ,
                       0       , ::Sqr(s.A11()));
  const Matrix2d2C & a = *this;
  u = (a * a.T()).EigenVectors(ssT);
  v = (a.T() * a).EigenVectors(ssT);
}

#endif

// IAPS - Image analysis program system. 
// End of include file Matrix2d2.hh