/* -*- C++ -*-
 
  This file is part of ViPEC
  Copyright (C) 1991-2000 Johan Rossouw (jrossouw@alcatel.altech.co.za)
 
  This program is free software; you can redistribute it and/or modify
  it under the terms of the GNU Library General Public License as
  published by
  the Free Software Foundation; either version 2 of the License, or
  (at your option) any later version.
 
  This program is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  GNU General Public License for more details.
 
  You should have received a copy of the GNU Library General Public License
  along with this program; if not, write to the Free Software
  Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
 
*/

#include <Matrix.h>
#include <Setup.h>
#include <Logger.h>
#include <qglobal.h>


using namespace std;

#include <iostream>

const TReal ZERO = 1E-1000;

uint Matrix::counter_ = 0;

//---------------------------------------------------------------------------
Matrix::Matrix()
    : rows_(0),
    cols_(0),
    matrix_(0)
{
  counter_++;
}

//---------------------------------------------------------------------------
Matrix::Matrix(uint size)
    : rows_(size),
    cols_(size),
    matrix_(0)
{
  allocateMemory();
  zero();
  counter_++;
}

//---------------------------------------------------------------------------
Matrix::Matrix(uint rows, uint cols)
    : rows_(rows),
    cols_(cols),
    matrix_(0)
{
  allocateMemory();
  zero();
  counter_++;
}

//---------------------------------------------------------------------------
Matrix::Matrix(const Matrix& matrix)
    : rows_(0),
    cols_(0),
    matrix_(0)
{
  rows_ = matrix.rows_;
  cols_ = matrix.cols_;
  allocateMemory();
  //Copy data
  for (uint r = 0; r < rows_; r++)
    {
      for (uint c = 0; c < cols_; c++)
        {
          matrix_[r][c] = matrix.matrix_[r][c];
        }
    }
  counter_++;
}

//---------------------------------------------------------------------------
Matrix::~Matrix()
{
  clear();
  counter_--;
}

//---------------------------------------------------------------------------
Matrix& Matrix::operator=(const Matrix& matrix)
{
  if ( ( rows_ != matrix.rows_ ) || ( cols_ != matrix.cols_ ) )
    {
      resize( matrix.rows_, matrix.cols_ );
    }

  for (uint r = 0; r < rows_; r++)
    {
      for (uint c = 0; c < cols_; c++)
        {
          matrix_[r][c] = matrix.matrix_[r][c];
        }
    }
  return *this;
}

//---------------------------------------------------------------------------
TComplex& Matrix::operator()(uint row, uint col)
{
  ASSERT((row <= rows_) && (col <= cols_));
  return matrix_[row][col];
}

//---------------------------------------------------------------------------
Matrix Matrix::operator+(const Matrix& matrix)
{
  if ( (rows_ != matrix.rows_) && (cols_ != matrix.cols_) )
    {
      Logger::error("Matrix::operator+() - throwing ExceptionMatrixSizeMismatch() exception");
      throw ExceptionMatrixSizeMismatch();
    }

  Matrix temp( cols_, rows_ );

  for (uint r = 0; r < rows_; r++)
    {
      for (uint c = 0; c < cols_; c++)
        {
          temp.matrix_[r][c] = matrix_[r][c] + matrix.matrix_[r][c];
        }
    }
  return temp;
}

//---------------------------------------------------------------------------
Matrix Matrix::operator-(const Matrix& matrix)
{
  if ( (rows_ != matrix.rows_) && (cols_ != matrix.cols_) )
    {
      Logger::error("Matrix::operator-() - throwing ExceptionMatrixSizeMismatch() exception");
      throw ExceptionMatrixSizeMismatch();
    }

  Matrix temp( cols_, rows_ );

  for (uint r = 0; r < rows_; r++)
    {
      for (uint c = 0; c < cols_; c++)
        {
          temp.matrix_[r][c] = matrix_[r][c] - matrix.matrix_[r][c];
        }
    }
  return temp;
}

//---------------------------------------------------------------------------
Matrix Matrix::operator*(const Matrix& matrix)
{
  unsigned long i, j, k;
  TComplex t;

  if ( this->cols_ != matrix.rows_ )
    {
      Logger::error("Matrix::operator*() - throwing ExceptionMatrixSizeMismatch() exception" );
      throw ExceptionMatrixSizeMismatch();
    }

  Matrix temp(rows_, matrix.cols_);

  for (i = 0; i < rows_; i++)
    for (j = 0; j < matrix.cols_; j++)
      {
        t = 0.0;
        for (k = 0; k < cols_; k++)
          {
            t += matrix_[i][k] * matrix.matrix_[k][j];
          }
        temp.matrix_[i][j] = t;
      }

  return temp;
}

//---------------------------------------------------------------------------
Matrix& Matrix::operator~()
{
  uint* indxc = new uint [cols_]; //indxc[i] : the column of the i'th pivot
  uint* indxr = new uint [rows_]; //indyc[i] : the (orig) row of the i'th pivot
  uint* ipiv  = new uint [cols_]; //bookkeeping of pivot element
  uint  irow  = 0;                    //stores the current pivot row
  uint  icol  = 0;                    //...and pivot column

  for (uint init=0; init<cols_; init++)
    {
      indxc[init] = 0;
      ipiv[init] = 0;
    }

  for (uint init2=0; init2<rows_; init2++)
    {
      indxr[init2] = 0;
    }

  for (uint i=0; i<cols_; i++)
    {
      TComplex big(0,0);

      //pivot search
      for (uint j=0; j<rows_; j++)
        {
          if (ipiv[j] != 1)
            {
              for (uint k=0; k<cols_; k++)
                {
                  if (ipiv[k] == 0)
                    {
                      if ( abs(matrix_[j][k]) >= abs(big) )
                        {
                          big = abs(matrix_[j][k]);
                          irow=j;
                          icol=k;
                        }
                    }
                  else if (ipiv[k] > 1)
                    {
                      delete [] indxc;
                      delete [] indxr;
                      delete [] ipiv;
                      Logger::error("Matrix::operator~() - throwing ExceptionMatrixSingular() exception");
                      throw ExceptionMatrixSingular();
                    }



                }
            }
        }

      ++(ipiv[icol]);

      if (irow != icol)
        {
          swapRows(irow, icol);
        }

      indxr[i] = irow;
      indxc[i] = icol;

      if ( abs(matrix_[icol][icol]) <= ZERO )
        {
          delete [] indxc;
          delete [] indxr;
          delete [] ipiv;
          Logger::error("Matrix::operator~() - throwing ExceptionMatrixSingular() exception");
          throw ExceptionMatrixSingular();
        }

      TComplex pivinv = 1.0 / matrix_[icol][icol];

      matrix_[icol][icol] = TComplex(1.0,0.0);

      for (uint c=0; c<cols_; c++)
        {
          matrix_[icol][c] *= pivinv;
        }

      for (uint r=0; r<rows_; r++)
        {
          if (r != icol)
            {
              TComplex dum = matrix_[r][icol];
              matrix_[r][icol] = TComplex(0,0);
              for (uint c=0; c<cols_; c++)
                {
                  matrix_[r][c] -= matrix_[icol][c]*dum;
                }
            }
        }

    } //main loop over columns

  //now unscrable columns of solution in reverse order
  for (uint cols=cols_; cols>0; cols--)
    {
      uint c = cols-1;
      if (indxr[c] != indxc[c])
        {
          swapColumns(indxr[c], indxc[c]);
        }
    }

  delete [] indxc;
  delete [] indxr;
  delete [] ipiv;

  return *this;
}

//---------------------------------------------------------------------------
Matrix Matrix::subMatrix(uint startRow, uint startCol,
                         uint noRows, uint noCols)
{
  if ( ((startRow+noRows)>rows_) || ((startCol+noCols)>cols_) )
    {
      Logger::error("Matrix::subMatrix() - throwing ExceptionMatrixBoundsOverrun() exception");
      throw ExceptionMatrixBoundsOverrun();
    }
  Matrix temp( noRows, noCols );
  for (uint r=0; r<noRows; r++)
    {
      for (uint c=0; c<noCols; c++)
        {
          temp.matrix_[r][c] = matrix_[startRow+r][startCol+c];
        }
    }
  return temp;
}

//---------------------------------------------------------------------------
void Matrix::allocateMemory()
{
  ASSERT( matrix_ == 0 );
  matrix_ = new TComplex* [rows_];         // STEP 1: SET UP THE ROWS.
  for (uint j = 0; j < rows_; j++)
    {
      matrix_[j] = new TComplex [cols_];  // STEP 2: SET UP THE COLUMNS
    }
}

//---------------------------------------------------------------------------
void Matrix::clear()
{
  //Free allocated memory
  if ( matrix_ )
    {
      for (uint i = 0; i < cols_;  i++)
        {
          delete[] matrix_[i];                 // STEP 1: DELETE THE COLUMNS
        }
      delete[] matrix_;                        // STEP 2: DELETE THE ROWS
    }
  rows_ = 0;
  cols_ = 0;
  matrix_ = 0;
}

//---------------------------------------------------------------------------
void Matrix::resize( uint size )
{
  resize( size, size );
}

//---------------------------------------------------------------------------
void Matrix::resize( uint rows, uint cols )
{
  if ( (rows_ != rows) && (cols_ != cols) )
    {
      clear();
      cols_ = cols;
      rows_ = rows;
      allocateMemory();
    }
}

//---------------------------------------------------------------------------
void Matrix::dump()
{
  cout.setf(ios::scientific, ios::floatfield);
  cout.precision(8);
  for (uint r = 0; r < rows_; r++)
    {
      for (uint c = 0; c < cols_; c++)
        {
          cout << matrix_[r][c] << ",";
        }
      cout << endl;
    }
}

//---------------------------------------------------------------------------
uint Matrix::rows() const
  {
    return rows_;
  }

//---------------------------------------------------------------------------
uint Matrix::columns() const
  {
    return cols_;
  }

//---------------------------------------------------------------------------
void Matrix::zero()
{
  for (uint r = 0; r < rows_; r++)
    {
      for (uint c = 0; c < cols_; c++)
        {
          matrix_[r][c] = TComplex(0,0);
        }
    }
}

//---------------------------------------------------------------------------
void Matrix::swapRows(uint a, uint b)
{
  if (a == b)
    {
      return;
    }
  for (uint c=0; c<cols_; c++)
    {
      TComplex temp = matrix_[a][c];
      matrix_[a][c] = matrix_[b][c];
      matrix_[b][c] = temp;
    }
}

//---------------------------------------------------------------------------
void Matrix::swapColumns(uint a, uint b)
{
  if (a == b)
    {
      return;
    }
  for (uint r=0; r<rows_; r++)
    {
      TComplex temp = matrix_[r][a];
      matrix_[r][a] = matrix_[r][b];
      matrix_[r][b] = temp;
    }
}

//---------------------------------------------------------------------------
void Matrix::delRow(uint a)
{
  for (uint r=a; r<rows_-1; r++)
    {
      for (uint c=0; c<cols_; c++)
        {
          matrix_[r][c] = matrix_[r+1][c];
        }
    }
  rows_--;
}

//---------------------------------------------------------------------------
void Matrix::delColumn(uint a)
{
  for (uint c=a; c<cols_-1; c++)
    for (uint r=1; r<rows_; r++)
      matrix_[r-1][c-1] = matrix_[r-1][c];
  cols_--;
}

//---------------------------------------------------------------------------
void Matrix::conj()
{
  ASSERT( "Must still implement!" == 0 );
}

//---------------------------------------------------------------------------
void Matrix::coFactor(uint r, uint c)
{
  delRow(r);
  delColumn(c);
  uint t = (r+c) % 2;
  if (t == 1)
    {
      negate();
    }
}

//---------------------------------------------------------------------------
void Matrix::negate()
{
  for (uint r=0; r<rows_; r++)
    {
      for (uint c=0; c<cols_; c++)
        {
          matrix_[r][c] = TComplex(-1,0)*matrix_[r][c];
        }
    }
}

//---------------------------------------------------------------------------
void Matrix::diagonal(TComplex n)
{
  ASSERT(rows_ == cols_);
  zero();
  for (uint i=0; i<rows_; i++)
    {
      matrix_[i][i] = n;
    }
}

//---------------------------------------------------------------------------
void Matrix::reduce( uint nrPorts )
{
  ASSERT( nrPorts == 2 );
  for (uint k=cols_; k>=4; k--)
    {

      TComplex piv;
      piv = TComplex(0,0);
      {//For resolving namespace difference in for scoping
        for (uint j=1; j<k; j++)
          {
            piv = piv + matrix_[k-1][j-1];
          }
      }

      for (uint i=1; i<k; i++)
        {
          for (uint j=1; j<k; j++)
            {
              if (i != j)
                {
                  matrix_[i-1][j-1] = matrix_[i-1][j-1]+matrix_[i-1][k-1]*
                                      (matrix_[k-1][j-1]/piv);
                }
            }
        }
    }

  matrix_[0][0] = TComplex(-1,0)*matrix_[0][1] - matrix_[0][2];
  matrix_[1][1] = TComplex(-1,0)*matrix_[1][0] - matrix_[1][2];

  rows_ = nrPorts;
  cols_ = nrPorts;

}

//---------------------------------------------------------------------------
void Matrix::insertComponent(TComplex& y, uint node[4])
{
  (*this)(node[0],node[0]) += y;
  (*this)(node[0],node[1]) -= y;
  (*this)(node[1],node[0]) -= y;
  (*this)(node[1],node[1]) += y;
}

//---------------------------------------------------------------------------
void Matrix::insertComponent(TComplex& y, uint node0, uint node1)
{
  uint n[4];
  n[0] = node0;
  n[1] = node1;
  insertComponent(y, n);
}

//---------------------------------------------------------------------------
void Matrix::insertGm(TComplex &y, uint node[4])
{
  (*this)(node[0],node[2]) += y;
  (*this)(node[1],node[3]) += y;
  (*this)(node[0],node[3]) -= y;
  (*this)(node[1],node[2]) -= y;
}

//---------------------------------------------------------------------------
void Matrix::insertGm(TComplex &y, uint node0, uint node1,
                      uint node2, uint node3)
{
  uint n[4];
  n[0] = node0;
  n[1] = node1;
  n[2] = node2;
  n[3] = node3;

  insertGm(y, n);
}

//---------------------------------------------------------------------------
void Matrix::insertTwoPort(Matrix& y, uint node[4])
{
  ASSERT( (y.cols_ == 2) && (y.rows_ == 2) );
  (*this)(node[0],node[0]) += y(0,0);
  (*this)(node[0],node[1]) -= y(0,0);
  (*this)(node[0],node[2]) += y(0,1);
  (*this)(node[0],node[3]) -= y(0,1);

  (*this)(node[1],node[0]) -= y(0,0);
  (*this)(node[1],node[1]) += y(0,0);
  (*this)(node[1],node[2]) -= y(0,1);
  (*this)(node[1],node[3]) += y(0,1);

  (*this)(node[2],node[0]) += y(1,0);
  (*this)(node[2],node[1]) -= y(1,0);
  (*this)(node[2],node[2]) += y(1,1);
  (*this)(node[2],node[3]) -= y(1,1);

  (*this)(node[3],node[0]) -= y(1,0);
  (*this)(node[3],node[1]) += y(1,0);
  (*this)(node[3],node[2]) -= y(1,1);
  (*this)(node[3],node[3]) += y(1,1);

}

//---------------------------------------------------------------------------
void Matrix::insertCoupledLine(Matrix& y, uint node[4])
{
  ASSERT( (y.cols_ == 2) && (y.rows_ == 2) );
  insertComponent(y(0,0),node[0],0);
  insertGm(y(0,1),node[0],0,node[1],0);
  insertGm(y(1,0),node[0],0,node[2],0);
  insertGm(y(1,1),node[0],0,node[3],0);

  insertComponent(y(0,0),node[1],0);
  insertGm(y(0,1),node[1],0,node[0],0);
  insertGm(y(1,1),node[1],0,node[2],0);
  insertGm(y(1,0),node[1],0,node[3],0);

  insertComponent(y(0,0),node[2],0);
  insertGm(y(1,0),node[2],0,node[0],0);
  insertGm(y(1,1),node[2],0,node[1],0);
  insertGm(y(0,1),node[2],0,node[3],0);

  insertComponent(y(0,0),node[3],0);
  insertGm(y(1,1),node[3],0,node[0],0);
  insertGm(y(1,0),node[3],0,node[1],0);
  insertGm(y(0,1),node[3],0,node[2],0);

}//end of insertCoupledLine()