SparseMatrix.cpp

/* This file is part of the BIAS library (Basic ImageAlgorithmS).

   Copyright (C) 2003-2009    (see file CONTACT for details)
   Multimediale Systeme der Informationsverarbeitung
   Institut fuer Informatik
   Christian-Albrechts-Universitaet Kiel


   BIAS is free software; you can redistribute it and/or modify
   it under the terms of the GNU Lesser General Public License as published by
   the Free Software Foundation; either version 2.1 of the License, or
   (at your option) any later version.

   BIAS 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 Lesser General Public License for more details.

   You should have received a copy of the GNU Lesser General Public License
   along with BIAS; if not, write to the Free Software
   Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA*/



#include <Base/Common/W32Compat.hh>

#include "SparseMatrix.hh"
#include <MathAlgo/SVD.hh>

using namespace BIAS;
using namespace std;

#define ABS(x)   (((x) < 0) ? -(x) : (x))
#define MAX(x,y) ((x) > (y)) ? (x) : (y)

//#define GAUSS_JORDAN_DEBUG


SparseMatrix::SparseMatrix(BIAS::Matrix<double> const &A){
  numRow_ = A.GetRows();
  numCol_ = A.GetCols();

  maxRow_ = 0;
  maxCol_ = 0;
  
  double val;
  for (unsigned int i = 0; i < numRow_; i++) {
    std::list<std::pair<unsigned int, double> > row;
    for (unsigned int j = 0; j < numCol_; j++) {
      val = A[i][j];
      if (ABS(val) > SPARSE_MATRIX_EPSILON) {
        row.push_back(std::pair<unsigned int, double>(j, val));
        maxRow_ = MAX(maxRow_, i);
        maxCol_ = MAX(maxCol_, j);
      }
    }
    if (!row.empty()) rows_[i] = row;
  }
}


//////////////////////////////////////////////////////////////////////////////
//
// Assignment Operator
//
//////////////////////////////////////////////////////////////////////////////
SparseMatrix& SparseMatrix::operator=(const SparseMatrix& S)
{
  // copy sizes
  numRow_ = S.GetRowsNum();
  numCol_ = S.GetColsNum();
  
  maxRow_ = S.GetRows()-1;
  maxCol_ = S.GetCols()-1;
  

//  rows_.clear();
//  
//  // copy map
//  double val;
//  for (unsigned int i = 0; i <= maxRow_; i++) {
//    std::list<std::pair<unsigned int, double> > row;
//    for (unsigned int j = 0; j <= maxCol_; j++) {
//      val = GetElement(i, j);
//      if (ABS(val) > SPARSE_MATRIX_EPSILON) {
//        row.push_back(std::pair<unsigned int, double>(j, val));
//      }
//    }
//    if (!row.empty()) rows_[i] = row;
//  }

  // copy map the simple way
  rows_ = S.rows_;

  return (*this);
}

double SparseMatrix::
GetElement(unsigned int row, unsigned int col) const
{
  std::map<unsigned int, std::list<std::pair<unsigned int, double> > >::
    const_iterator rowsIter = rows_.find(row);
  if (rowsIter != rows_.end()) {
    std::list<std::pair<unsigned int, double> >::const_iterator colsIter;
    std::list<std::pair<unsigned int, double> >::const_iterator colsEnd
      = rowsIter->second.end();
    for (colsIter = rowsIter->second.begin();
         colsIter != colsEnd && colsIter->first < col; colsIter++);
    if (colsIter != colsEnd && colsIter->first == col) {
      return colsIter->second;
    }
  }
  return 0.0;
}

void SparseMatrix::
SetElement(unsigned int row, unsigned int col, double val)
{
  maxRow_ = MAX(maxRow_, row);
  maxCol_ = MAX(maxCol_, col);
  std::map<unsigned int, std::list<std::pair<unsigned int, double> > >::
    iterator rowsIter = rows_.find(row);
  if (rowsIter == rows_.end() && ABS(val) <= SPARSE_MATRIX_EPSILON) {
    return; // Row not found, no element to set
  }
  std::list<std::pair<unsigned int, double> >::iterator it;
  std::list<std::pair<unsigned int, double> >::iterator end = rows_[row].end();
  for (it = rows_[row].begin(); it != end && it->first < col; it++);
  if (it != end && it->first == col) {
    if (ABS(val) > SPARSE_MATRIX_EPSILON) {
      it->second = val;
    } else {
      rows_[row].erase(it);
      if (rows_[row].empty()) rows_.erase(row);
    }
  } else if (ABS(val) > SPARSE_MATRIX_EPSILON) {
    rows_[row].insert(it, std::pair<unsigned int,double>(col, val));
  }

  if (numRow_ < (row + 1) || numCol_ < (col + 1)){
    BIASWARNONCE("Element was set out of current matrix dimensions. "
                            << "Dimensions have been increased!")
    numRow_ = MAX(numRow_, (row + 1));
    numCol_ = MAX(numCol_, (col + 1));
  }

}

void SparseMatrix::
SetSubMatrix(unsigned int row, unsigned int col,
             BIAS::Matrix<double> const &A,
             unsigned int startRowInA, unsigned int startColInA,
             unsigned int row_count, unsigned int col_count)
{
  BIASASSERT(startRowInA+row_count <= (unsigned int)(A.GetRows()));
  BIASASSERT(startColInA+col_count <= (unsigned int)(A.GetCols()));

//  cout << "sparse matrix : set subMatrix " << A << endl;
//  cout << "to be taken " << startRowInA << " " << startColInA << " num " 
//       << row_count << " " << col_count << endl;
  
  maxRow_ = MAX(maxRow_, row+row_count-1);
  maxCol_ = MAX(maxCol_, col+col_count-1);
  for (unsigned int i = 0; i < row_count; i++) {
    std::list<std::pair<unsigned int, double> >::iterator it;
    std::list<std::pair<unsigned int, double> >::iterator end
      = rows_[row+i].end();
    for (it = rows_[row+i].begin(); it != end && it->first < col; it++);

    // Make sure that there is no element set within the submatrix range
    BIASASSERT(it == end || it->first >= col+col_count);

    for (unsigned int j = 0; j < col_count; j++) {
      if (ABS(A[startRowInA+i][startColInA+j]) > SPARSE_MATRIX_EPSILON) {
        rows_[row+i].insert(it, std::pair<unsigned int, double>
                            (col+j, A[startRowInA+i][startColInA+j]));
      }
    }
  }

  if (numRow_ < (row+row_count) || numCol_ < (col+col_count)){
    BIASWARNONCE("Sub matrix exceeds current matrix dimensions. "
                            << "Dimensions have been increased!");
    numRow_ = MAX(numRow_, (row+row_count));
    numCol_ = MAX(numCol_, (col+col_count));
  }

}

void SparseMatrix::Transpose(SparseMatrix &result)
{
  result.maxRow_ = maxCol_;
  result.maxCol_ = maxRow_;
  result.numRow_ = numCol_;
  result.numCol_ = numRow_;
  result.rows_.clear();
  std::map< unsigned int, std::list<std::pair<unsigned int, double> > >::
    iterator rowsIter;
  std::map< unsigned int, std::list<std::pair<unsigned int, double> > >::
    iterator rowsEnd = rows_.end();
  for (rowsIter = rows_.begin(); rowsIter != rowsEnd; rowsIter++) {
    std::list<std::pair<unsigned int, double> >::iterator colsIter;
    std::list<std::pair<unsigned int, double> >::iterator colsEnd
      = rowsIter->second.end();
    for (colsIter = rowsIter->second.begin();
         colsIter != colsEnd; colsIter++) {
      // @note More efficient than InsertElement_, since we go ordered
      // through rows/columns and fill them up!
      result.rows_[colsIter->first].push_back(
        std::pair<unsigned int, double>(rowsIter->first, colsIter->second));
    }
  }


}




void SparseMatrix::
InsertElement_(unsigned int row, unsigned int col, double value)
{
  std::list<std::pair<unsigned int, double> >::iterator it;
  std::list<std::pair<unsigned int, double> >::iterator end = rows_[row].end();
  for (it = rows_[row].begin(); it != end && it->first < col; it++);

  // Make sure that element is not already set
  BIASASSERT(it == end || it->first > col);

  rows_[row].insert(it, std::pair<unsigned int, double>(col, value));
}


void SparseMatrix::MultiplyIP(const double multiplier) 
{
  std::map< unsigned int, 
            std::list<std::pair<unsigned int, double> > >::iterator row;
  std::list< std::pair<unsigned int, double> >::iterator elem;
  for (row = rows_.begin(); row != rows_.end(); row++) {
    for (elem = row->second.begin(); elem != row->second.end(); elem++) {
      elem->second *= multiplier;
    }
  }
}


void SparseMatrix::Multiply(SparseMatrix &A, SparseMatrix &result)
{
    
    
  SparseMatrix AT(A.numCol_, A.numRow_);
    A.Transpose(AT);
  MultiplyWithTransposeOf(AT, result);

}

void SparseMatrix::
MultiplyWithTransposeOf(SparseMatrix &A, SparseMatrix &result)
{
  if(numCol_ != A.numCol_){
    BIASERR("does not work for some reason, returing with doing anything...");
    BIASABORT;
    return;
  }
  
  result.maxRow_ = maxRow_;
  result.maxCol_ = A.maxRow_;
  result.rows_.clear();
  std::map<unsigned int, std::list<std::pair<unsigned int, double> > >::
    iterator rowsIter1;
  std::map<unsigned int, std::list<std::pair<unsigned int, double> > >::
    iterator rowsEnd1 = rows_.end();
  for (rowsIter1 = rows_.begin(); rowsIter1 != rowsEnd1; rowsIter1++) {
    std::list<std::pair<unsigned int, double> > row;
    std::map<unsigned int, std::list<std::pair<unsigned int, double> > >::
      iterator rowsIter2;
    std::map<unsigned int, std::list<std::pair<unsigned int, double> > >::
      iterator rowsEnd2 = A.rows_.end();
    for (rowsIter2 = A.rows_.begin(); rowsIter2 != rowsEnd2; rowsIter2++) {
      double sum = 0;
      std::list<std::pair<unsigned int, double> >::iterator colsIter1 =
        rowsIter1->second.begin();
      std::list<std::pair<unsigned int, double> >::iterator colsIter2 =
        rowsIter2->second.begin();
      std::list<std::pair<unsigned int, double> >::iterator colsEnd1 =
        rowsIter1->second.end();
      std::list<std::pair<unsigned int, double> >::iterator colsEnd2 =
        rowsIter2->second.end();
      while (colsIter1 != colsEnd1 && colsIter2 != colsEnd2) {
        if (colsIter1->first < colsIter2->first) {
          colsIter1++;
        } else if (colsIter1->first > colsIter2->first) {
          colsIter2++;
        } else {
          sum += colsIter1->second * colsIter2->second;
          colsIter1++;
          colsIter2++;
        }
      }
      if (ABS(sum) > SPARSE_MATRIX_EPSILON) {
        // @note More efficient than InsertElement_, since we go ordered
        // through rows/columns and fill them up!
        row.push_back(std::pair<unsigned int, double>(rowsIter2->first, sum));
      }
    }
    if (!row.empty()) result.rows_[rowsIter1->first] = row;
  }
  
  result.numRow_ = numRow_;
  result.numCol_ = A.numRow_;

}

void SparseMatrix::GetAsDense(BIAS::Matrix<double> &M) const
{
  M.newsize(numRow_, numCol_);
  M.SetZero();
  std::map<unsigned int, std::list<std::pair<unsigned int, double> > >::
    const_iterator rowsIter;
  std::map<unsigned int, std::list<std::pair<unsigned int, double> > >::
    const_iterator rowsEnd = rows_.end();
  for (rowsIter = rows_.begin(); rowsIter != rowsEnd; rowsIter++) {
    BIASASSERT(rowsIter->first < M.GetRows());
    std::list<std::pair<unsigned int, double> >::const_iterator colsIter;
    std::list<std::pair<unsigned int, double> >::const_iterator colsEnd
      = rowsIter->second.end();
    for (colsIter = rowsIter->second.begin();
         colsIter != colsEnd; colsIter++) {
      BIASASSERT(colsIter->first < M.GetCols());
      M[rowsIter->first][colsIter->first] = colsIter->second;
    }
  }
}

double SparseMatrix::
GetMaxDiagonalElement(unsigned int &index) const
{
  index = 0;
  double maxValue = 0.0;
  std::map<unsigned int, std::list<std::pair<unsigned int, double> > >::
    const_iterator rowsIter;
  std::map<unsigned int, std::list<std::pair<unsigned int, double> > >::
    const_iterator rowsEnd = rows_.end();
  for (rowsIter = rows_.begin(); rowsIter != rowsEnd; rowsIter++) {
    std::list<std::pair<unsigned int, double> >::const_iterator colsIter;
    std::list<std::pair<unsigned int, double> >::const_iterator colsEnd
      = rowsIter->second.end();
    for (colsIter = rowsIter->second.begin();
         colsIter != colsEnd; colsIter++) {
      // Check if we run past diagonal
      if (colsIter->first > rowsIter->first) break;
      if (colsIter->first == rowsIter->first) {
        if (colsIter->second > maxValue) {
          maxValue = colsIter->second;
          index = colsIter->first;
        }
      }
    }
  }
  return maxValue;
}

double SparseMatrix::
GetMaxRowElement(unsigned int row, unsigned int &col) const
{
  col = 0;
  double maxValue = 0.0;
  map<unsigned int, std::list<std::pair<unsigned int, double> > >::
    const_iterator rowsIter = rows_.find(row);
  if (rowsIter != rows_.end()) {
    std::list<std::pair<unsigned int, double> >::const_iterator colsIter;
    std::list<std::pair<unsigned int, double> >::const_iterator colsEnd
      = rowsIter->second.end();
    for (colsIter = rowsIter->second.begin();
         colsIter != colsEnd; colsIter++) {
      if (colsIter->second > maxValue) {
        maxValue = colsIter->second;
        col = colsIter->first;
      }
    }
  }
  return maxValue;
}

/// TODO Not efficient because of row->column mapping...
double SparseMatrix::
GetMaxColumnElement(unsigned int col, unsigned int &row) const
{
  row = 0;
  double maxValue = 0.0;
  std::map<unsigned int, std::list<std::pair<unsigned int, double> > >::
    const_iterator rowsIter;
  std::map<unsigned int, std::list<std::pair<unsigned int, double> > >::
    const_iterator rowsEnd = rows_.end();
  for (rowsIter = rows_.begin(); rowsIter != rowsEnd; rowsIter++) {
    std::list<std::pair<unsigned int, double> >::const_iterator colsIter;
    std::list<std::pair<unsigned int, double> >::const_iterator colsEnd
      = rowsIter->second.end();
    for (colsIter = rowsIter->second.begin();
         colsIter != colsEnd; colsIter++) {
      // Check if we run past column
      if (colsIter->first > col) break;
      if (colsIter->first == col) {
        if (colsIter->second > maxValue) {
          maxValue = colsIter->second;
          row = rowsIter->first;
        }
      }
    }
  }
  return maxValue;
}

void SparseMatrix::MultiplyDiagonalBy(const double &value)
{
  std::map<unsigned int, std::list<std::pair<unsigned int, double> > >::
    iterator rowsIter;
  std::map<unsigned int, std::list<std::pair<unsigned int, double> > >::
    iterator rowsEnd = rows_.end();
  // Run over all existing rows
  for (rowsIter = rows_.begin(); rowsIter != rowsEnd; rowsIter++) {
    std::list<std::pair<unsigned int, double> >::iterator colsIter;
    std::list<std::pair<unsigned int, double> >::iterator colsEnd
      = rowsIter->second.end();
    // In each row, look for column (row;row)
    for (colsIter = rowsIter->second.begin();
         colsIter != colsEnd; colsIter++) {
      // Check if we are further than the diagonal
      if (colsIter->first > rowsIter->first) break;
      // Check if we are at the diagonal
      if (colsIter->first == rowsIter->first) {
        colsIter->second *= value;
        break;
      }
    }
  }

}

void SparseMatrix::AddToDiagonal(const double &value)
{
  // Works only for square matrices so far:
  // For rectangular, we need to check maxRow_ and maxCol_ always!
  BIASASSERT(maxRow_ == maxCol_);
  std::map<unsigned int, std::list<std::pair<unsigned int, double> > >::
    iterator rowsIter = rows_.begin();

  // Run over all rows and find diagonal element
  for (unsigned int rowId = 0; rowId <= maxRow_; rowId++) {
    // Insert rowId-th row if it does not exist yet
    if (rowsIter == rows_.end() || rowsIter->first != rowId) {
      std::list<pair<unsigned int, double> > newRow;
      newRow.push_back(std::pair<unsigned int, double>(rowId, value));
      rowsIter = rows_.insert(rowsIter,
        std::pair<unsigned int,
                  list<pair<unsigned int, double> > >(rowId, newRow));
    } else {
      // Here we can be sure that rowsIter is valid and points to
      // the rowId-th row! Now find the diagonal element.
      std::list<std::pair<unsigned int, double> >::iterator colsIter;
      std::list<std::pair<unsigned int, double> >::iterator colsEnd
        = rowsIter->second.end();
      bool missingCol = true;
      for (colsIter = rowsIter->second.begin();
           colsIter != colsEnd && missingCol; colsIter++) {
        // Check if we are further than the diagonal
        if (colsIter->first > rowId) break;
        // Check if we are at the diagonal
        if (colsIter->first == rowId) {
          colsIter->second += value;
          missingCol = false;
          break;
        }
      }
      // The diagonal element is missing (value == 0) in this row, add it!
      if (missingCol) {
        rowsIter->second.insert(colsIter,
          std::pair<unsigned int, double>(rowId, value));
      }
    }
    rowsIter++;
  }

 
  if (numRow_ < (maxRow_+1) || numCol_ < (maxCol_+1)){
    BIASWARNONCE("Sub matrix exceeds current matrix dimensions. "
               << "Dimensions have been increased!");
    numRow_ = MAX(numRow_, (maxRow_+1));
    numCol_ = MAX(numCol_, (maxCol_+1));
  }
  
}

void SparseMatrix::GetAsDense(unsigned int row, unsigned int col,
                              unsigned int numRows, unsigned int numCols,
                              BIAS::Matrix<double> &M) const
{
  M.newsize(numRows, numCols);
  M.SetZero();
  map<unsigned int, std::list<std::pair<unsigned int, double> > >::
    const_iterator rowsIter = rows_.find(row);
  map<unsigned int, std::list<std::pair<unsigned int, double> > >::
    const_iterator rowsEnd = rows_.end();
  // If the first row could not be found, try to find the next one.
  // Don't give up before we couldn't find the last interesting one!
  unsigned int rowOffset = 0;
  while (rowsIter == rowsEnd && rowOffset < numRows) {
    rowsIter = rows_.find(row + (++rowOffset));
  }

  // Now that we've found the first interesting row, start with iterating.

  // TODO Will the map-iterator deliver sorted numbers?! Then why are there no
  // maps through columns, it would save all the nasty iterator-insertions!!

  for (; rowsIter != rowsEnd; rowsIter++) {
    // Check if at 2nd or n-th iteration we are outside the submatrix.
    if (rowsIter->first >= numRows+row) break;
    std::list<std::pair<unsigned int, double> >::const_iterator colsIter;
    std::list<std::pair<unsigned int, double> >::const_iterator colsEnd
      = rowsIter->second.end();
    // Now run along current row and find first interesting column.
    for (colsIter = rowsIter->second.begin();
         colsIter != colsEnd; colsIter++) {
#ifdef BIAS_DEBUG
      if (colsIter == colsEnd) {
        BIASERR("Sparse matrix is not consistent (contains empty rows)!");
        break;
      }
#else
      if (colsIter == colsEnd) break;
#endif
      // Check whether we have to go further in the row
      if (colsIter->first >= col) break;
    }
    for (; colsIter != colsEnd; colsIter++) {
      if (colsIter->first >= numCols+col) break;
      BIASASSERT(rowsIter->first >= row && rowsIter->first < M.GetRows()+row);
      BIASASSERT(colsIter->first >= col && colsIter->first < M.GetCols()+col);
      M[rowsIter->first-row][colsIter->first-col] = colsIter->second;
    }
  }
}

void SparseMatrix::
Print(std::string const &name, std::ostream &stream) const
{
  stream << name << " m " << GetRowsNum() << "n  " << GetColsNum() << endl;
  std::map<unsigned int, std::list<std::pair<unsigned int, double> > >::
    const_iterator rowsIter;
  std::map<unsigned int, std::list<std::pair<unsigned int, double> > >::
    const_iterator rowsEnd = rows_.end();
  for (rowsIter = rows_.begin(); rowsIter != rowsEnd; rowsIter++) {
    std::list<std::pair<unsigned int, double> >::const_iterator colsIter;
    std::list<std::pair<unsigned int, double> >::const_iterator colsEnd
      = rowsIter->second.end();
    for (colsIter = rowsIter->second.begin();
         colsIter != colsEnd; colsIter++) {
      stream << name << "[" << rowsIter->first << "][" << colsIter->first
             << "] = " << colsIter->second << endl;
    }
  }
}


void SparseMatrix::
Multiply(BIAS::Vector<double> &x, BIAS::Vector<double> &result)
{
 // BIASASSERT(x.Size() == numCol_)
  result.newsize(numRow_);
  result.SetZero();
  std::map<unsigned int, std::list<std::pair<unsigned int, double> > >::
    iterator rowsIter;
  std::map<unsigned int, std::list<std::pair<unsigned int, double> > >::
    iterator rowsEnd = rows_.end();
  for (rowsIter = rows_.begin(); rowsIter != rowsEnd; rowsIter++) {
    double sum = 0;
    std::list<std::pair<unsigned int, double> >::iterator colsIter;
    std::list<std::pair<unsigned int, double> >::iterator colsEnd
      = rowsIter->second.end();
    for (colsIter = rowsIter->second.begin();
         colsIter != colsEnd; colsIter++) {
      sum += colsIter->second * x[colsIter->first];
    }
    result[rowsIter->first] = sum;
    
  }
}

void SparseMatrix::AppendMatrixBottom(SparseMatrix &H)
{
  std::map<unsigned int, std::list<std::pair<unsigned int, double> > >::
    iterator rowsIter;
  std::map<unsigned int, std::list<std::pair<unsigned int, double> > >::
    iterator rowsEnd = H.rows_.end();

  
  for (rowsIter = H.rows_.begin(); rowsIter != rowsEnd; rowsIter++) {
    rows_[numRow_+rowsIter->first] = rowsIter->second;
  }
  
   
  if (H.numRow_ > 0){
    maxRow_ = numRow_ + H.maxRow_;
    numRow_ += H.numRow_;
  }
   

  if (H.numCol_ > 0){
    maxCol_ = MAX(maxCol_, H.maxCol_);
  }
  
  
//  if (numRow_ < (maxRow_ + 1)){
//      BIASWARNONCE("Append matrix exceeds current matrix dimensions. "
//                          << "Dimensions have been increased!")
//      numRow_ = maxRow_ + 1;
//  }
  
//  if (numCol_ < (maxCol_ + 1)){
//    BIASWARNONCE("Append matrix exceeds current matrix dimensions. "
//              << "Dimensions have been increased!")
//    numCol_ = maxCol_ + 1;
//  }
  
  
}

void SparseMatrix::AppendMatrixRight(SparseMatrix &H)
{
  std::map<unsigned int, std::list<std::pair<unsigned int, double> > >::
    iterator rowsIter;
  std::map<unsigned int, std::list<std::pair<unsigned int, double> > >::
    iterator rowsEnd = H.rows_.end();
  for (rowsIter = H.rows_.begin(); rowsIter != rowsEnd; rowsIter++) {
    std::list<std::pair<unsigned int, double> > row = rows_[rowsIter->first];
    std::list<std::pair<unsigned int, double> >::iterator colsIter;
    std::list<std::pair<unsigned int, double> >::iterator colsEnd
      = rowsIter->second.end();
    for (colsIter = rowsIter->second.begin();
         colsIter != colsEnd; colsIter++) {
      row.push_back(std::pair<unsigned int,double>(maxCol_+colsIter->first +1,
                                                   colsIter->second));
    }
    rows_[rowsIter->first] = row;
  }

  if (H.numCol_ > 0){
    maxCol_ = numCol_ + H.maxCol_;
    numCol_ += H.numCol_;
  }
    
  if (H.numRow_ > 0)
    maxRow_ = MAX(maxRow_, H.maxRow_);
  
//  if (numCol_ < (maxCol_ + 1)){
//      BIASWARNONCE("Append matrix exceeds current matrix dimensions. "
//                          << "Dimensions have been increased!")
//      numCol_ = maxCol_ + 1;
//  }
//  if (numRow_ < (maxRow_ + 1)){
//    BIASWARNONCE("Append matrix exceeds current matrix dimensions. "
//              << "Dimensions have been increased!")
//    numRow_ = maxRow_ + 1;
//  }
  
  
}


int SparseMatrix::
InvertAndSolve(BIAS::Vector<double> &b, BIAS::Matrix<double> &inverse,
               BIAS::Vector<double> &result)
{
  BIASASSERT ((numRow_ == numCol_) && (b.Size() == numRow_));

  // Augment matrix
  for (unsigned int i = 0; i <= maxRow_; i++) {
    // Add identity to the right
    rows_[i].push_back(std::pair<unsigned int, double>(maxCol_+i+1, 1.0));
    if (ABS(b[i]) > SPARSE_MATRIX_EPSILON) {
      // Add vector b to the right
      rows_[i].push_back(std::pair<unsigned int, double>(2*maxCol_+2, b[i]));
    }
  }

  // Perform Gauss-Jordan algorithm
  int res = GaussJordan_();
  if (res != 0) return res;

  // Write back results
  inverse.newsize(maxRow_+1, maxCol_+1);
  inverse.SetZero();
  result.newsize(maxRow_+1);
  result.SetZero();
  std::map<unsigned int, std::list<std::pair<unsigned int, double> > >::
    iterator rowsIter;
  std::map<unsigned int, std::list<std::pair<unsigned int, double> > >::
    iterator rowsEnd = rows_.end();
  for (rowsIter = rows_.begin(); rowsIter != rowsEnd; rowsIter++) {
    std::list<std::pair<unsigned int, double> >::iterator colsIter;
    std::list<std::pair<unsigned int, double> >::iterator colsEnd
      = rowsIter->second.end();
    for (colsIter = rowsIter->second.begin();
         colsIter != colsEnd && colsIter->first <= maxCol_; colsIter++);
#ifdef BIAS_DEBUG
    if (colsIter == colsEnd) {
      BIASERR("Sparse matrix is not consistent (contains empty rows)!");
    }
#endif
    for (; colsIter != colsEnd; colsIter++) {
      if (colsIter->first < 2*maxCol_+2) {
        inverse[rowsIter->first][colsIter->first-maxCol_-1] = colsIter->second;
      } else {
        result[rowsIter->first] = colsIter->second;
      }
    }
  }

  // Update column number
  maxCol_ = 2*maxCol_+2;

  if ((numCol_ < maxCol_ + 1)){
    BIASWARNONCE("Call to 'InvertAndSolve' enlarged current matrix which now exceeds "
                          << "its dimensions. Dimensions have been increased!");
    numCol_ = maxCol_ + 1;
  }


  return res;
}


int SparseMatrix::
InvertAndSolve(BIAS::Vector<double> &b, SparseMatrix &inverse,
               BIAS::Vector<double> &result)
{
  BIASASSERT ((numRow_ == numCol_) && (b.Size() == numRow_));

  // Augment matrix
  for (unsigned int i = 0; i <= maxRow_; i++) {
    // Add identity to the right
    rows_[i].push_back(std::pair<unsigned int, double>(maxCol_+i+1, 1));
    if (ABS(b[i]) > SPARSE_MATRIX_EPSILON) {
      // Add vector b to the right
      rows_[i].push_back(std::pair<unsigned int,double>(2*maxCol_+2, b[i]));
    }
  }

  // Perform Gauss-Jordan algorithm
  int res = GaussJordan_();
  if (res != 0) return res;

  // Write back results
  result.newsize(maxRow_+1);
  result.SetZero();
  inverse.rows_.clear();
  inverse.maxRow_ = maxRow_;
  inverse.maxCol_ = maxCol_;
  std::map<unsigned int, std::list<std::pair<unsigned int, double> > >::
    iterator rowsIter;
  std::map<unsigned int, std::list<std::pair<unsigned int, double> > >::
    iterator rowsEnd = rows_.end();
  for (rowsIter = rows_.begin(); rowsIter != rowsEnd; rowsIter++) {
    std::list<std::pair<unsigned int, double> >::iterator colsIter;
    std::list<std::pair<unsigned int, double> >::iterator colsEnd
      = rowsIter->second.end();
    for (colsIter = rowsIter->second.begin();
         colsIter != colsEnd && colsIter->first <= maxCol_; colsIter++);
#ifdef BIAS_DEBUG
    if (colsIter == colsEnd) {
      BIASERR("Sparse matrix is not consistent (contains empty rows)!");
    }
#endif
    std::list<std::pair<unsigned int, double> > row;
    for (; colsIter != colsEnd; colsIter++) {
      if (colsIter->first < 2*maxCol_+2) {
        row.push_back(std::pair<unsigned int, double>(colsIter->first-maxCol_-1,
                                                      colsIter->second));
      } else {
        result[rowsIter->first] = colsIter->second;
      }
    }
    if (!row.empty()) inverse.rows_[rowsIter->first] = row;
  }

  // Update column number
  maxCol_ = maxCol_*2+2;


  if ((numCol_ < maxCol_ + 1) || (numRow_ < maxRow_ + 1)){
    BIASWARNONCE("Call to 'InvertAndSolve' enlarged current matrix which now exceeds "
                          << "its dimensions. Dimensions have been increased!");
    numCol_ = maxCol_ + 1;
    numRow_ = maxRow_ + 1;
  }

  return 0;
}


int SparseMatrix::
Solve(BIAS::Vector<double> &b, BIAS::Vector<double> &result)
{
//  cout << "numRow " << numRow_ << " numCol " << numCol_ << " b size " << b.Size() << endl;
  BIASASSERT ((numRow_ == numCol_) && (b.Size() == numRow_));
  
  
  // Augment matrix
  for (unsigned int i = 0; i <= maxRow_; i++) {
    if (ABS(b[i]) > SPARSE_MATRIX_EPSILON) {
      // Add vector b to the right
      rows_[i].push_back(std::pair<unsigned int, double>(maxCol_+1, b[i]));
    }
  }

  // Perform Gauss-Jordan algorithm
  int res = GaussJordan_();
  if (res != 0) return res;

  // Write back results
  result.newsize(maxRow_+1);
  result.SetZero();
  std::map<unsigned int, std::list<std::pair<unsigned int, double> > >::
    iterator rowsIter;
  std::map<unsigned int, std::list<std::pair<unsigned int, double> > >::
    iterator rowsEnd = rows_.end();
  for (rowsIter = rows_.begin(); rowsIter != rowsEnd; rowsIter++) {
    std::pair<unsigned int, double> back = rowsIter->second.back();
    if (back.first == maxCol_+1) {
      result[rowsIter->first] = back.second;
    }
  }

  // Update column number
  maxCol_++;


  if (((numCol_ < maxCol_ + 1) || (numRow_ < maxRow_ + 1))){
    BIASWARNONCE("Call to 'Invert' enlarged current matrix which now exceeds "
                      << "its dimensions. Dimensions have been increased!");
    numCol_ = maxCol_ + 1;
    numRow_ = maxRow_ + 1;
  }

  return 0;
}


int SparseMatrix::PseudoInverse(BIAS::Matrix<double>& inverse)
{  
  SparseMatrix pinv(numCol_, numRow_);
  PseudoInverse(pinv);
  pinv.GetAsDense(inverse);
  
  return 0;
}

    
void SparseMatrix::Reinit(unsigned rows, unsigned cols)
{
  //clear old matrix
  rows_.clear();
  numCol_ = cols;
  numRow_ = rows;
  maxCol_ = 0;
  maxRow_ = 0;
}

int SparseMatrix::PseudoInverse(BIAS::SparseMatrix& inverse){
  
  
  if(numRow_ > numCol_){
      SparseMatrix AT(numCol_, numRow_);
      Transpose(AT);
      SparseMatrix ATA(numCol_, numCol_);
      AT.Multiply(*this, ATA);
      SparseMatrix ATA_inv(numCol_, numCol_);
      ATA.Invert(ATA_inv);
      ATA_inv.MultiplyWithTransposeOf(*this, inverse);
   } else {
     SparseMatrix AAT(numRow_, numRow_);
     MultiplyWithTransposeOf(*this, AAT);
     SparseMatrix AAT_inv(numRow_, numRow_);
     AAT.Invert(AAT_inv);
     SparseMatrix AT(numCol_, numRow_);
     Transpose(AT);
     AT.Multiply(AAT_inv, inverse);
   }
  
  return 0;
}




int SparseMatrix::Invert(BIAS::Matrix<double> &inverse)
{
  BIASASSERT(numRow_ == numCol_); // Only allowed for square matrices!

  // Augment matrix
  for (unsigned int i = 0; i <= maxRow_; i++) {
    // Add identity to the right
    rows_[i].push_back(std::pair<unsigned int, double>(maxCol_+i+1, 1));
  }

  // Perform Gauss-Jordan algorithm
  int res = GaussJordan_();
  if (res != 0) return res;

  // Write back results
  inverse.newsize(numRow_, numCol_);
  inverse.SetZero();
  std::map<unsigned int, std::list<std::pair<unsigned int, double> > >::
    iterator rowsIter;
  std::map<unsigned int, std::list<std::pair<unsigned int, double> > >::
    iterator rowsEnd = rows_.end();
  for (rowsIter = rows_.begin(); rowsIter != rowsEnd; rowsIter++) {
    std::list<std::pair<unsigned int, double> >::iterator colsIter;
    std::list<std::pair<unsigned int, double> >::iterator colsEnd
      = rowsIter->second.end();
    for (colsIter = rowsIter->second.begin();
         colsIter != colsEnd && colsIter->first <= maxCol_; colsIter++);
#ifdef BIAS_DEBUG
    if (colsIter == colsEnd) {
      BIASERR("Sparse matrix is not consistent (contains empty rows)!");
    }
#endif
    for (; colsIter != colsEnd; colsIter++) {
      inverse[rowsIter->first][colsIter->first-maxCol_-1] = colsIter->second;
    }
  }

  // Update column number
  maxCol_ = 2*maxCol_+1;

  if ((numCol_ < maxCol_ + 1) || (numRow_ < maxRow_)){
    BIASWARNONCE("Call to 'Invert' enlarged current matrix which now exceeds "
                   << "its dimensions. Dimensions have been increased!");
    numCol_ = maxCol_ + 1;
    numRow_ = maxRow_ + 1;
  }


  return 0;
}


int SparseMatrix::Invert(SparseMatrix &inverse)
{
 
  cout << "numRow " << numRow_ << " numCol " << numCol_ << endl;
  BIASASSERT (numRow_ == numCol_); // Only allowed for square matrices!

  // Augment matrix
  for (unsigned int i = 0; i <= maxRow_; i++) {
    // Add identity to the right
    rows_[i].push_back(std::pair<unsigned int, double>(maxCol_+i+1, 1));
  }

  // Perform Gauss-Jordan algorithm
  int res = GaussJordan_();
  if (res != 0) return res;

  // Write back results
  inverse.rows_.clear();
  inverse.maxRow_ = maxRow_;
  inverse.maxCol_ = maxCol_;
  inverse.numRow_ = numRow_;
  inverse.numCol_ = numCol_;
  std::map<unsigned int, std::list<std::pair<unsigned int, double> > >::
    iterator rowsIter;
  std::map<unsigned int, std::list<std::pair<unsigned int, double> > >::
    iterator rowsEnd = rows_.end();
  for (rowsIter = rows_.begin(); rowsIter != rowsEnd; rowsIter++) {
    std::list<std::pair<unsigned int, double> >::iterator colsIter;
    std::list<std::pair<unsigned int, double> >::iterator colsEnd
      = rowsIter->second.end();
    for (colsIter = rowsIter->second.begin();
         colsIter != colsEnd && colsIter->first <= maxCol_; colsIter++);
#ifdef BIAS_DEBUG
    if (colsIter == colsEnd) {
      BIASERR("Sparse matrix is not consistent (contains empty rows)!");
    }
#endif
    std::list<std::pair<unsigned int, double> > row;
    for (; colsIter != colsEnd; colsIter++) {
      row.push_back(std::pair<unsigned int, double>(colsIter->first-maxCol_-1,
                                                    colsIter->second));
    }
    if (!row.empty()) inverse.rows_[rowsIter->first] = row;
  }

  // Update column number
  maxCol_ = 2*maxCol_+1;
  
  if ((numCol_ < maxCol_ + 1) || (numRow_ < maxRow_ + 1)){
    BIASWARNONCE("Call to 'Invert' enlarged current matrix which now exceeds "
                          << "its dimensions. Dimensions have been increased! maxCol " << maxCol_+1 << " numCol " << numCol_ << " maxRows " << maxRow_+1 << " numRows " << numRow_ << endl);
    numCol_ = maxCol_ + 1;
    numRow_ = maxRow_ + 1;
  }

  return 0;
}

int SparseMatrix::GaussJordan_()
{
#ifdef GAUSS_JORDAN_DEBUG
  cout << "-- SparseMatrix::GaussJordan_() : Debugging --" << endl << endl;
#endif

  std::map<unsigned int, std::list<std::pair<unsigned int, double> > >::
    iterator rowsIter1;
  for (rowsIter1 = rows_.begin(); rowsIter1 != rows_.end(); rowsIter1++) {
#ifdef GAUSS_JORDAN_DEBUG
    cout << "- Processing line " << rowsIter1->first << endl << flush;
#endif
    // Find pivot row
    double pivot_value = 0.0;
    std::map<unsigned int, std::list<std::pair<unsigned int, double> > >::
      iterator pivot_index;
    std::map<unsigned int, std::list<std::pair<unsigned int, double> > >::
      iterator rowsIter2;
    // Run over all rows below
    for (rowsIter2 = rowsIter1; rowsIter2 != rows_.end(); rowsIter2++) {
      // For each check if front element is at the current column (or behind)
      if (rowsIter2->second.empty()) continue;
#ifdef GAUSS_JORDAN_DEBUG
      cout << "  - Checking for row echelon form in line "
           << rowsIter2->first << endl << flush;
#endif
      std::pair<unsigned int, double> front = rowsIter2->second.front();
      // If we have a first element in a column smaller than row iter1
      // something went wrong, because the first columns should be zero!
      BIASASSERT(front.first >= rowsIter1->first);

      if (front.first == rowsIter1->first) {
        if (ABS(front.second) > ABS(pivot_value)) {
#ifdef GAUSS_JORDAN_DEBUG
          cout << "  - Changed pivot index to " << rowsIter2->first
               << endl << flush;
#endif
          pivot_value = front.second;
          pivot_index = rowsIter2;
        }
      }
    }
    if (pivot_value == 0.0) {
      BIASERR("Matrix is singular!");
      return -1;
    }
    // Exchange rows at iter1 and pivot row
    if (pivot_index != rowsIter1) {
      //std::list<std::pair<unsigned int, double> > temp = rowsIter1->second;
      //rowsIter1->second = pivot_index->second;
      //pivot_index->second = temp;
#ifdef GAUSS_JORDAN_DEBUG
      cout << "- Swapping lines " << rowsIter1->first << " and "
           << pivot_index->first << " with sizes " << rowsIter1->second.size()
           << " and " << pivot_index->second.size() << "... " << flush;
#endif
      swap(rowsIter1->second, pivot_index->second);
#ifdef GAUSS_JORDAN_DEBUG
      cout << "done!" << flush << endl;
#endif
    }
#ifdef GAUSS_JORDAN_DEBUG
    cout << "- Eliminating column downwards..." << flush << endl;
#endif
    // Eliminate column downwards
    rowsIter2 = rowsIter1;
    for (rowsIter2++; rowsIter2 != rows_.end(); rowsIter2++) {
      if (rowsIter2->second.empty()) continue;
      std::pair<unsigned int, double> front = rowsIter2->second.front();
      // Row iter2 has an entry in column iter1, which has to be eliminated for
      // row echolon form.
      if (front.first == rowsIter1->first) {
        rowsIter2->second.pop_front();
        double factor = front.second/pivot_value;

        // CORRECT ??????????????? Changed inequality!

        if (ABS(factor) < 1.0/(SPARSE_MATRIX_EPSILON*SPARSE_MATRIX_EPSILON)) {
          //BIASERR("Matrix is singular to working precision " << factor);
          //BIASABORT;
          //return -1;
        }
        std::list<std::pair<unsigned int, double> >::iterator colsIter1 =
          rowsIter1->second.begin();
        std::list<std::pair<unsigned int, double> >::iterator colsIter2 =
          rowsIter2->second.begin();
        colsIter1++;
        // Run over all elements in current row
        while (colsIter1 != rowsIter1->second.end()) {
          if (colsIter2 == rowsIter2->second.end()) {
            double value = -factor*colsIter1->second;
            if (ABS(value) > SPARSE_MATRIX_EPSILON) {
              rowsIter2->second.insert(colsIter2,
                std::pair<unsigned int, double>(colsIter1->first, value));
            }
            colsIter1++;
          } else if (colsIter1->first>colsIter2->first) {
            colsIter2++;
          } else if (colsIter1->first<colsIter2->first) {
            double value = -factor*colsIter1->second;
            if (ABS(value) > SPARSE_MATRIX_EPSILON) {
              rowsIter2->second.insert(colsIter2,
                std::pair<unsigned int, double>(colsIter1->first, value));
            }
            colsIter1++;
          } else {
            colsIter2->second -= factor*colsIter1->second;
            if (ABS(colsIter2->second) <= SPARSE_MATRIX_EPSILON) {
              colsIter2 = rowsIter2->second.erase(colsIter2);
            } else {
              colsIter2++;
            }
            colsIter1++;
          }
        }
      }
    }
#ifdef GAUSS_JORDAN_DEBUG
    cout << "- Eliminating column upwards..." << flush << endl;
#endif
    // Eliminate column upwards
    for (rowsIter2 = rows_.begin(); rowsIter2 != rowsIter1; rowsIter2++) {
      std::list<std::pair<unsigned int, double> >::iterator colsIter2 =
        rowsIter2->second.begin();
      colsIter2++;
      if (colsIter2 != rowsIter2->second.end()) {
        std::list<std::pair<unsigned int, double> >::iterator colsIter1 =
          rowsIter1->second.begin();
        if (colsIter2->first == colsIter1->first) {
          double factor = colsIter2->second/pivot_value;

          // CORRECT ??????????????? Changed inequality!

          if (ABS(factor) < 1.0/(SPARSE_MATRIX_EPSILON*SPARSE_MATRIX_EPSILON)) {
            //BIASERR("Matrix is singular to working precision " << factor);
            //BIASABORT;
            //return -1;
          }
          colsIter2 = rowsIter2->second.erase(colsIter2);
          colsIter1++;
          while (colsIter1 != rowsIter1->second.end()) {
            if (colsIter2 == rowsIter2->second.end()) {
              double value = -factor*colsIter1->second;
              if (ABS(value) > SPARSE_MATRIX_EPSILON) {
                rowsIter2->second.insert(colsIter2,
                  std::pair<unsigned int, double>(colsIter1->first, value));
              }
              colsIter1++;
            } else if (colsIter1->first>colsIter2->first) {
              colsIter2++;
            } else if (colsIter1->first<colsIter2->first) {
              double value = -factor*colsIter1->second;
              if (ABS(value) > SPARSE_MATRIX_EPSILON) {
                rowsIter2->second.insert(colsIter2,
                  std::pair<unsigned int, double>(colsIter1->first, value));
              }
              colsIter1++;
            } else {
              colsIter2->second -= factor*colsIter1->second;
              if (ABS(colsIter2->second) <= SPARSE_MATRIX_EPSILON) {
                colsIter2 = rowsIter2->second.erase(colsIter2);
              } else {
                colsIter2++;
              }
              colsIter1++;
            }
          }
        }
      }
    }
#ifdef GAUSS_JORDAN_DEBUG
    cout << "- Normalizing row..." << flush << endl;
#endif
    // Normalize row, i.e. set pivot element to 1
    std::list<std::pair<unsigned int, double> >::iterator colsIter1
      = rowsIter1->second.begin();
    while (colsIter1 != rowsIter1->second.end()) {
      colsIter1->second /= pivot_value;
      if (ABS(colsIter1->second) <= SPARSE_MATRIX_EPSILON) {
        colsIter1 = rowsIter1->second.erase(colsIter1);
      } else {
        colsIter1++;
      }
    }
  }

  // Remove empty row from resulting matrix
#ifdef GAUSS_JORDAN_DEBUG
  cout << "- Removing empty rows from resulting matrix..." << flush << endl;
#endif
  rowsIter1 = rows_.begin();
  while (rowsIter1 != rows_.end()) {
    if (rowsIter1->second.empty()) {
      rows_.erase(rowsIter1);
    } else {
      rowsIter1++;
    }
  }
  return 0;
}


bool SparseMatrix::CheckConsistency()
{
  unsigned int maxRow = 0, maxCol = 0;
  unsigned int lastRow = 0;
  std::map<unsigned int, std::list<std::pair<unsigned int, double> > >::
    const_iterator rowsIter;
  std::map<unsigned int, std::list<std::pair<unsigned int, double> > >::
    const_iterator rowsEnd = rows_.end();
  for (rowsIter = rows_.begin(); rowsIter != rowsEnd; rowsIter++) {
    if (rowsIter != rows_.begin()) {
      if (rowsIter->first <= lastRow) {
        BIASERR("Sparse matrix has rows in wrong order: Row "
                << rowsIter->first << " follows row " << lastRow << "!");
        //BIASABORT;
        return false;
      }
    }
    lastRow = rowsIter->first;
    maxRow = MAX(maxRow, rowsIter->first);

    if (rowsIter->second.empty()) {
      BIASERR("Sparse matrix contains empty row " << rowsIter->first<< "!");
      //BIASABORT;
      return false;
    }

    unsigned int lastCol = 0;
    std::list<std::pair<unsigned int, double> >::const_iterator colsIter;
    std::list<std::pair<unsigned int, double> >::const_iterator colsEnd
      = rowsIter->second.end();
    for (colsIter = rowsIter->second.begin();
         colsIter != colsEnd; colsIter++) {
      if (colsIter != rowsIter->second.begin()) {
        if (colsIter->first <= lastCol) {
          BIASERR("Sparse matrix has columns in wrong order: Column "
                  << colsIter->first << " follows column " << lastCol << "!");
          //BIASABORT;
          return false;
        }
      }
      lastCol = colsIter->first;
      maxCol = MAX(maxCol, colsIter->first);

      if (ABS(colsIter->second) <= SPARSE_MATRIX_EPSILON) {
        BIASERR("Sparse matrix contains to small value " << colsIter->second
                << " at [" << rowsIter->first << "][" << colsIter->first
                << "]!");
        //BIASABORT;
        return false;
      }

    }
  }

  if (maxRow > maxRow_ || maxCol > maxCol_) {
    BIASERR("Sparse matrix contains values out of bounds (max. row ="
            << maxRow_ << ", max. col = " << maxCol_ << ", value at ["
            << maxRow << "][" << maxCol << "] found!)");
    //BIASABORT;
    return false;
  }
  return true;
}

void SparseMatrix::
GetAllElements(std::vector<unsigned int> &rows,
               std::vector<unsigned int> &cols,
               std::vector<double> &values) const
{
  rows.clear();
  cols.clear();
  values.clear();
  unsigned int num = 0;
  std::map<unsigned int, std::list<std::pair<unsigned int, double> > >::
    const_iterator rowsIter;
  std::map<unsigned int, std::list<std::pair<unsigned int, double> > >::
    const_iterator rowsEnd = rows_.end();
  for (rowsIter = rows_.begin(); rowsIter != rowsEnd; rowsIter++) {
    std::list<std::pair<unsigned int, double> >::const_iterator colsIter;
    std::list<std::pair<unsigned int, double> >::const_iterator colsEnd
      = rowsIter->second.end();
    for (colsIter = rowsIter->second.begin(); colsIter != colsEnd; colsIter++) {
      rows.push_back(rowsIter->first);
      cols.push_back(colsIter->first);
      values.push_back(colsIter->second);
      num++;
    }
  }
  BIASASSERT(rows.size() == num && cols.size() == num && values.size() == num);
}