#include "GL_Matrix.h"
#include <assert.h>

// this must be stl one day !!! 
// matrix type and operations 



// constructor
GL_Matrix::GL_Matrix (int row, int col)
{
   Row = row;
   Col = col;
   Val.resize(row);
   for (int i=0; i < row; i++)
      Val[i].resize(col);
}

// copy constructor
GL_Matrix::GL_Matrix (const GL_Matrix& m)
{
  Row = m.Row;
  Col = m.Col;
   
  Val = m.Val;
}

GL_Matrix::GL_Matrix( double a1, double a2,  double a3,  double a4,
		      double a5, double a6,  double a7,  double a8,
		      double a9, double a10, double a11, double a12){
  Row = 4;
  Col = 4;

  Val.resize(Row);
  for (int i=0; i < Row; i++)
    Val[i].resize(Col);

  Val[0][0] = a1;
  Val[0][1] = a2;
  Val[0][2] = a3;
  Val[0][3] = a4;
  Val[1][0] = a5;
  Val[1][1] = a6;
  Val[1][2] = a7;
  Val[1][3] = a8;
  Val[2][0] = a9;
  Val[2][1] = a10;
  Val[2][2] = a11;
  Val[2][3] = a12;
  Val[3][0] = 0;
  Val[3][1] = 0;
  Val[3][2] = 0;
  Val[3][3] = 1;
  
}


// destructor
GL_Matrix::~GL_Matrix (void)
{
  ;
}


// public method for resizing matrix
void
GL_Matrix::SetSize (int row, int col) 
{
  Row = row;
  Col = col;
  Val.resize(row);
  for(int i = 0; i < row; i++)
    Val[i].resize(col);
   
   return;
}

// subscript operator to get/set individual elements
double&
GL_Matrix::operator () (int row, int col) 
{
  
   assert(row <= Row && col <= Col);
   
   return Val[row][col];
}

// input stream function
istream&
operator >> (istream& istrm, GL_Matrix& m)
{
   for (int i = 0; i < m.Row; i++)
     for (int j=0; j < m.Col; j++)
       istrm >> m.Val[i][j];
   return istrm;
}

// output stream function
ostream&
operator << (ostream &ostrm, GL_Matrix& m)
{
   for (int i=0; i < m.Row; i++)
   {
       for (int j=0; j < m.Col; j++)
          cout << m.Val[i][j] << '\t';
       cout << endl;
   }
   return ostrm;
}

// assignment operator
GL_Matrix&
GL_Matrix::operator = (const GL_Matrix& m) 
{
  Row = m.Row; 
  Col = m.Col;
  Val = m.Val;

  return *this;
}


// logical equal-to operator
bool
operator == (const GL_Matrix& m1, const GL_Matrix& m2) 
{
   bool retVal = false;

   if (m1.Row != m2.Row || m1.Col != m2.Col)
      return retVal;

   for (int i=0; i < m1.Row; i++)
      for (int j=0; j < m1.Col; i++)
         if (m1.Val[i][j] != m2.Val[i][j])
            return retVal;

   return true;
}

// logical no-equal-to operator
bool 
operator != (const GL_Matrix& m1, const GL_Matrix& m2) 
{
    return (m1 == m2) ? false : true;
}


// combined addition and assignment operator
GL_Matrix&
GL_Matrix::operator += (const GL_Matrix& m) 
{
   assert(Row == m.Row && Col == m.Col);
      
   for (int i=0; i < m.Row; i++)
      for (int j=0; j < m.Col; j++)
         Val[i][j] += m.Val[i][j];
   return *this;
}


// combined subtraction and assignment operator
GL_Matrix&
GL_Matrix::operator -= (const GL_Matrix& m)
{
   assert(Row == m.Row && Col == m.Col);
      
   for (int i=0; i < m.Row; i++)
      for (int j=0; j < m.Col; j++)
         Val[i][j] -= m.Val[i][j];
   return *this;
}

// combined scalar multiplication and assignment operator
GL_Matrix&
GL_Matrix::operator *= (const double& c) 
{
   for (int i=0; i < Row; i++)
      for (int j=0; j < Col; j++)
         Val[i][j] *= c;
   return *this;
}


// combined matrix multiplication and assignment operator
GL_Matrix&
GL_Matrix::operator *= (const GL_Matrix& m)
{
  assert(Col == m.Row);
    
  *this = *this * m;
  return *this;
}


// combined scalar division and assignment operator
GL_Matrix&
GL_Matrix::operator /= (const double& c) 
{
   for (int i=0; i < Row; i++)
      for (int j=0; j < Col; j++)
         Val[i][j] /= c;

   return *this;
}

// combined power and assignment operator
GL_Matrix&
GL_Matrix::operator ^= (const int& pow)
{
   for (int i=2; i <= pow; i++)
      *this = *this * *this;

   return *this;
}

// unary negation operator
GL_Matrix
GL_Matrix::operator - (void) 
{
   GL_Matrix temp(Row,Col);

   for (int i=0; i < Row; i++)
      for (int j=0; j < Col; j++)
         temp.Val[i][j] = - Val[i][j];

   return temp;
}

// binary addition operator
GL_Matrix
operator + (const GL_Matrix& m1, const GL_Matrix& m2)
{
   assert(m1.Row == m2.Row && m1.Col == m2.Col);
      
   GL_Matrix temp(m1.Row,m1.Col);

   for (int i=0; i < m1.Row; i++)
      for (int j=0; j < m1.Col; j++)
         temp.Val[i][j] = m1.Val[i][j] + m2.Val[i][j];

   return temp;
}

// binary subtraction operator
GL_Matrix
operator - (const GL_Matrix& m1, const GL_Matrix& m2)
{
   assert(m1.Row == m2.Row && m1.Col == m2.Col);
     

   GL_Matrix temp(m1.Row,m1.Col);

   for (int i=0; i < m1.Row; i++)
      for (int j=0; j < m1.Col; j++)
         temp.Val[i][j] = m1.Val[i][j] - m2.Val[i][j];

   return temp;
}


// binary scalar multiplication operator
GL_Matrix
operator * (const GL_Matrix& m, const double& no) 
{
   GL_Matrix temp(m.Row,m.Col);

   for (int i=0; i < m.Row; i++)
      for (int j=0; j < m.Col; j++)
         temp.Val[i][j] = no * m.Val[i][j];

   return temp;
}


// binary matrix multiplication operator
GL_Matrix
operator * (const GL_Matrix& m1, const GL_Matrix& m2)
{
   assert(m1.Col == m2.Row);
     
   GL_Matrix temp(m1.Row,m2.Col);

   for (int i=0; i < m1.Row; i++)
      for (int j=0; j < m2.Col; j++)
      {
         temp.Val[i][j] = 0.0;
         for (int k=0; k < m1.Col; k++)
            temp.Val[i][j] += m1.Val[i][k] * m2.Val[k][j];
      }
   return temp;
}

// binary power operator
GL_Matrix
operator ^ (const GL_Matrix& m, const int& pow) 
{
   GL_Matrix temp(m);

   for (int i=2; i <= pow; i++)
     ;//temp = temp * temp;

   return temp;
}


// unary transpose operator
GL_Matrix
operator  ~ (const GL_Matrix& m) 
{
   GL_Matrix temp(m.Col,m.Row);

   for (int i=0; i < m.Row; i++)
      for (int j=0; j < m.Col; j++)
	 temp.Val[j][i] = m.Val[i][j];

   return temp;
}


// unary inversion operator
GL_Matrix
operator ! (GL_Matrix m) 
{
   int i,j,k;
   double a1,a2;
   vector<double> rowptr;

 
   assert(m.Row == m.Col);

   GL_Matrix temp(m.Row,m.Col);

   temp.Unit();
   for (k=0; k < m.Row; k++)
   {
      int indx = m.pivot(k);
      assert(indx != -1);
         
      if (indx != 0)
      {
         rowptr = temp.Val[k];
         temp.Val[k] = temp.Val[indx];
         temp.Val[indx] = rowptr;
      }
      a1 = m.Val[k][k];
      for (j=0; j < m.Row; j++)
      {
         m.Val[k][j] /= a1;
         temp.Val[k][j] /= a1;
      }
      for (i=0; i < m.Row; i++)
         if (i != k)
         {
            a2 = m.Val[i][k];
            for (j=0; j < m.Row; j++)
            {
               m.Val[i][j] -= a2 * m.Val[k][j];
               temp.Val[i][j] -= a2 * temp.Val[k][j];
            }
         }
   }
   return temp;
}


// solve simultaneous equation
GL_Matrix
GL_Matrix::Solve (const GL_Matrix& v) const
{
   int i,j,k;
   double a1;

   assert((Row == Col && Col == v.Row));
      

   GL_Matrix temp(Row,Col+v.Col);
   for (i=0; i < Row; i++)
   {
      for (j=0; j < Col; j++)
         temp.Val[i][j] = Val[i][j];
      for (k=0; k < v.Col; k++)
         temp.Val[i][Col+k] = v.Val[i][k];
   }
   for (k=0; k < Row; k++)
   {
      int indx = temp.pivot(k);
      assert(indx != -1);

      a1 = temp.Val[k][k];
      for (j=k; j < temp.Col; j++)
         temp.Val[k][j] /= a1;

      for (i=k+1; i < Row; i++)
      {
         a1 = temp.Val[i][k];
         for (j=k; j < temp.Col; j++)
            temp.Val[i][j] -= a1 * temp.Val[k][j];
      }
   }
   GL_Matrix s(v.Row,v.Col);
   for (k=0; k < v.Col; k++)
      for (int m=int(Row)-1; m >= 0; m--)
      {
         s.Val[m][k] = temp.Val[m][Col+k];
         for (j=m+1; j < Col; j++)
            s.Val[m][k] -= temp.Val[m][j] * s.Val[j][k];
      }
   return s;
}
   

// set zero to all elements of this matrix
void
GL_Matrix::Null() 
{
   for (int i=0; i < Row; i++)
      for (int j=0; j < Col; j++)
 	 Val[i][j] = 0;
   return;
}

// set this matrix to unity
void
GL_Matrix::Unit (const int& row) 
{
   for (int i=0; i < Row; i++)
      for (int j=0; j < Col; j++)
         Val[i][j] = i == j ? 1 : 0;
  return;
}

// set this matrix to unity
void
GL_Matrix::Unit () 
{
    // if we could have everywhere ansi c++ ......
    int row = Row < Col ? Row : Col;
    Row = Col = row;

    for (int i=0; i < Row; i++)
	for (int j=0; j < Col; j++)
	    Val[i][j] = i == j ? 1 : 0;
    return;
}


// private partial pivoting method
int
GL_Matrix::pivot (int row)
{
  int k = int(row);
  double amax,temp;

  amax = -1;
  for (int i=row; i < Row; i++)
     if ( (temp = fabs( Val[i][row])) > amax && temp != 0.0)
     {
       amax = temp;
       k = i;
     }
  if (Val[k][row] == 0)
     return -1;
  if (k != row)
  {
     vector<double> rowptr = Val[k];
     Val[k] = Val[row];
     Val[row] = rowptr;
     return k;
  }
  return 0;
}


// calculate the determinant of a matrix
double
GL_Matrix::Det ()
{
   int i,j,k;
   double piv,detVal = 1 ;

   assert(Row == Col);

   GL_Matrix temp(*this);

   for (k=0; k < Row; k++)
   {
      int indx = temp.pivot(k);
      if (indx == -1)
         return 0;
      if (indx != 0)
         detVal = - detVal;
      detVal = detVal * temp.Val[k][k];
      for (i=k+1; i < Row; i++)
      {
         piv = temp.Val[i][k] / temp.Val[k][k];
         for (j=k+1; j < Row; j++)
            temp.Val[i][j] -= piv * temp.Val[k][j];
      }
   }
   return detVal;
}

// calculate the norm of a matrix
double
GL_Matrix::Norm () 
{
   double retVal = 0;

   for (int i=0; i < Row; i++)
      for (int j=0; j < Col; j++)
         retVal += Val[i][j] * Val[i][j];
   retVal = sqrt( retVal);

   return retVal;
}


// calculate the condition number of a matrix
double
GL_Matrix::Cond () 
{
   GL_Matrix inv(Row,Col);

   inv = ! (*this);
   double retVal = Norm() * inv.Norm();

   return retVal;
}


// calculate the cofactor of a matrix for a given element
double
GL_Matrix::Cofact (int row, int col)
{
   int i,i1,j,j1;

   assert(Row == Col);
      

   assert(row < Row && col < Col);
      

   GL_Matrix temp (Row-1,Col-1);

   for (i=i1=0; i < Row; i++)
   {
      if (i == row)
        continue;
      for (j=j1=0; j < Col; j++)
      {
      	 if (j == col)
            continue;
      	 temp.Val[i1][j1] = Val[i][j];
         j1++;
      }
      i1++;
   }
   double  cof = temp.Det();
   if ((row+col)%2 == 1)
      cof = -cof;

   return cof;
}


// calculate adjoin of a matrix
GL_Matrix
GL_Matrix::Adj ()
{
   assert(Row == Col);
      

   GL_Matrix temp(Row,Col);

   for (int i=0; i < Row; i++)
      for (int j=0; j < Col; j++)
         temp.Val[i][j] = Cofact(i,j);

   temp = ~temp;
   return temp;
}

// Determine if the matrix is singular
bool
GL_Matrix::IsSingular () 
{
   if (Row != Col)
      return false;
   return (Det() == 0 );
}

// Determine if the matrix is diagonal
bool
GL_Matrix::IsDiagonal () 
{
   if (Row != Col)
      return false;
   for (int i=0; i < Row; i++)
     for (int j=0; j < Col; j++)
        if (i != j && Val[i][j] != 0)
          return false;
   return true;
}

// Determine if the matrix is scalar
bool
GL_Matrix::Isscalar () 
{
   if (!IsDiagonal())
     return false;
   double v = Val[0][0];
   for (int i=1; i < Row; i++)
     if (Val[i][i] != v)
        return false;
   return true;
}

// Determine if the matrix is a unit matrix
bool
GL_Matrix::IsUnit () 
{
   if (Isscalar() && Val[0][0] == 1)
     return true;
   return false;
}

// Determine if this is a null matrix
bool
GL_Matrix::IsNull () 
{
   for (int i=0; i < Row; i++)
      for (int j=0; j < Col; j++)
         if (Val[i][j] != 0)
            return false;
   return true;
}

// Determine if the matrix is symmetric
bool
GL_Matrix::IsSymmetric () 
{
   if (Row != Col)
      return false;
   for (int i=0; i < Row; i++)
      for (int j=0; j < Col; j++)
         if (Val[i][j] != Val[j][i])
            return false;
   return true;
}

// Determine if the matrix is skew-symmetric
bool
GL_Matrix::IsSkewSymmetric () 
{
   if (Row != Col)
      return false;
   for (int i=0; i < Row; i++)
      for (int j=0; j < Col; j++)
         if (Val[i][j] != -Val[j][i])
            return false;
   return true;
}

// Determine if the matrix is upper triangular
bool
GL_Matrix::IsUpperTiangular () 
{
   if (Row != Col)
      return false;
   for (int i=1; i < Row; i++)
      for (int j=0; j < i-1; j++)
         if (Val[i][j] != 0)
            return false;
   return true;
}

// Determine if the matrix is lower triangular
bool
GL_Matrix::IsLowerTiangular () 
{
   if (Row != Col)
      return false;

   for (int j=1; j < Col; j++)
      for (int i=0; i < j-1; i++)
         if (Val[i][j] != 0)
            return false;

   return true;
}
