#include "matrix.h"
namespace math_tools {
  
   Matrix & Matrix::round() {
      char buffer[1024];

      for(int i = 0; i < _rows; i++) {
         for(int j = 0; j < _cols; j++) {
            sprintf(buffer,"%g", _mat[i][j]);
            _mat[i][j] = strtod(buffer, NULL);
         }
      }

      return *this;
   }

   double ** Matrix::alloc(int rows, int cols) {
      double ** ret = new double*[rows];
      for(int v = 0; v < rows; v++)
         ret[v] = new double[cols];
      return ret;
   }

   Matrix::Matrix() {
      _mat = new double *[1];
      _mat[0] = new double[1];
      _mat[0][0] = 0.0;
      _rows = _cols = 1;
   }

   void Matrix::freeMatrix2() {
      if(_mat) {
         for(int i = 0; i < _rows; i++) {
            delete [] _mat[i];
         }
         delete [] _mat;
         _mat = 0;
      }
   }

   void Matrix::freeMatrix() {
      if(_mat) {
         for(int i = 0; i < _rows; i++) {
            delete [] _mat[i];
         }
         delete [] _mat;
         _mat = 0;
         _rows = 0;
         _cols = 0;
      }
   }

    Matrix::Matrix(const Matrix & cp) {
      _mat = alloc(cp._rows,cp._cols);
      _rows = cp._rows;
      _cols = cp._cols;
      for(int i = 0; i < _rows; i++) {
         for(int j = 0; j < _cols; j++) {
            set(i,j,cp.get(i,j));
         }
      }
   }

   /**
    * Construct a matrix from a two dimensional array
    * of double.
    * @param array the array of numbers.
    * @param rows the number of rows of the array.
    * @param cols the number of columns of the array.
    */
    Matrix::Matrix(double ** array, int rows, int cols) {
      _mat = array;
      _rows = rows;
      _cols = cols;
   }

    Matrix & Matrix::operator=(const Matrix & mat) {
      freeMatrix();
      _mat = alloc(mat._rows, mat._cols);
      _rows = mat._rows;
      _cols = mat._cols;
      for(int i = 0; i < _rows; i++) {
         for(int j = 0; j < _cols; j++) {
            _mat[i][j] = mat._mat[i][j];
         }
      }
      return *this;
   }

   /**
    * Construct a new matrix.
    * @param rows the number of rows in the new matrix.
    * @param cols the number of columns in the new matrix.
    * @param startValue the starting value for each entry in
    *                   the new matrix.
    */
    Matrix::Matrix(int rows, int cols, double startValue) {
      _rows = rows;
      _cols = cols;
      _mat = alloc(rows, cols);
      int i,j;
      for(i = 0; i < _rows; i++)
         for(j = 0; j < _cols; j++)
            _mat[i][j] = startValue;
   }

   /**
    * Construct a new matrix.
    * @param rows the number of rows in the new matrix.
    * @param cols the number of colums in the new matrix.
    */
   Matrix::Matrix(int rows, int cols)
   {
      _rows = rows;
      _cols = cols;
      _mat = alloc(rows, cols);
      int i,j;
      for(i = 0; i < _rows; i++)
         for(j = 0; j < _cols; j++)
            _mat[i][j] = 0.0;
   }

   Matrix::~Matrix() {
      freeMatrix2();
   }
      
   /**
    * Return the number of rows.
    * @return the number of rows.
    */
    int Matrix::rows() { return _rows; }
   
   /**
    * Return the number of colums.
    * @return the number of colums.
    */
    int Matrix::cols() { return _cols; }
   
   /**
    * Set the value of an entry in the matrix.
    * @param i which row.
    * @param j which colum.
    * @param value the value to set.
    */
    void Matrix::set(int i, int j, double value) {
      if(i >= _rows || i < 0)
         throw Exception("bad row index");
      if(j >= _cols || j < 0)
         throw Exception("bad col index");
      _mat[i][j] = value;
   }
   
   /**
    * Return the value at an i,j entry in the matrix.
    * @param i the row.
    * @param j the column.
    * @return the value at that i,j entry.
    */
    double Matrix::get(int i, int j) const {
      if(i >= _rows || i < 0)
         throw Exception("bad row index");
      if(j >= _cols || j < 0)
         throw Exception("bad col index");
      return _mat[i][j];
   }
   
   /**
    * doubleest to see if another matrix is equal 
    * to this matrix.
    * @param A the other matrix.
    * @return true if they are equal. False, otherwise.
    */
    boolean Matrix::equal(Matrix & A) {
      int i, j;

      if(_cols != A._cols || _rows != A._rows)
         return false;

      //char str1[1024];
      //char str2[1024];

      //round();
      //A.round();
      
      for(i = 0; i < _rows; i++) {
         for(j = 0; j < _cols; j++) {

            //sprintf(str1, "%g", A.get(i,j));
            //sprintf(str2, "%g", get(i,j));

cout << get(i,j) << " -> " << A.get(i,j) << endl << flush;
            //if(_mat[i][j] != (A._mat)[i][j]) {
            if(get(i,j) != A.get(i,j)) {
            //if(strcmp(str1, str2) != 0) {
cout << "not equal...\n" << endl << flush;
               return false;
            }
         }
      }
      
      return true;
   }
   
   /**
    * Multiply this matrix by another and return the product 
    * as a new matrix.
    * @param A a matrix.
    * @return 0 if the dimensions are wrong.  Otherwise, a 
    *              new matrix that is the product of this 
    *              matrix and A is returned.
    */

   Matrix Matrix::mul(const Matrix & A) {
      if(_cols != A._rows)
         throw Exception("bad matrix multiply n != m.");
      Matrix ret = Matrix(_cols, A._cols, 0.0);
      double prod;
      int i,j,k;
      for(i = 0; i < _cols; i++) {
         prod = 0.0;
         for(j = 0; j < A._rows; j++) {
            prod = 0.0;
            for(k = 0; k < A._rows; k++)
               prod += get(i,k) * A.get(k,j);
               //prod += __muldf3(get(i,k), A.get(k,j));
            ret.set(i, j, prod);
         }
      }
      return ret;
   }
   
   /**
    * Add a matrix to this matrix and return the result.
    * @param A the matrix to add with.
    * @return 0 if the two matrixes can not be added 
    *         together.  Otherwise, the result of the addition
    *         operation is returned.
    */
    Matrix Matrix::add(const Matrix & A) {
      if(_rows != A._rows || _cols != A._cols)
         throw Exception("rows and cols don't match in matrix add.");
      int i, j;
      Matrix ret = Matrix(_rows, _cols, 0.0);
      for(i = 0; i < _rows; i++)
         for(j = 0; j < _cols; j++)
            ret.set(i, j, _mat[i][j] + A._mat[i][j]);
      return ret;
   }

   /**
    * Subtract a matrix to this matrix and return the result.
    * @param A the matrix to subtract.
    * @return 0 if the two matrixes can not be subtracted.
    *         Otherwise, the result of the subtraction
    *         operation is returned.
    */
    Matrix Matrix::sub(const Matrix & A) {
      if(_rows != A._rows || _cols != A._cols)
         throw Exception("rows and cols don't match in matrix subtract.");
      int i, j;
      Matrix ret = Matrix(_rows, _cols, 0.0);
      for(i = 0; i < _rows; i++)
         for(j = 0; j < _cols; j++)
            ret.set(i, j, _mat[i][j] + A._mat[i][j]);
      return ret;
   }
   
   /**
    * Scaler multipy.
    * @param k a scaler value to multiply the matrix by.
    * @return return the result matrix of the scaler 
    *         multiplication.
    */
    Matrix Matrix::scalerMul(double k) {
      Matrix ret = Matrix(_rows, _cols, 0.0);
      int i,j;
      for(i = 0; i < _rows; i++)
         for(j = 0; j < _cols; j++)
            ret._mat[i][j] = k * _mat[i][j];
      return ret;
   }
   
   /**
    * Compute the trace of a matrix.
    * @return the value of this matrixes trace.
    * @exception an Exception is raised if the 
    *            matrix is not square.
    */
    double Matrix::trace() {
      if(_cols != _rows)
         throw Exception("Matrix must be square to compute the trace!");
      double total = 0.0;
      int i,j;
      for(i = 0; i < _rows; i++)
         for(j = 0; j < _cols; j++) {
            if(i == j)
               total += _mat[i][j];
         }
      return total;
   }
   
   /**
    * Compute the transpose of this matrix.
    * @return the transpose of this matrix.
    */
    Matrix Matrix::transpose() {
      Matrix ret = Matrix(_cols, _rows, 0.0);
      int i,j;
      for(i = 0; i < _rows; i++) 
         for(j = 0; j < _cols; j++)
            ret.set(j,i,get(i,j));
      return ret;
   }

    double Matrix::_det2x2() {
      return ((_mat[0][0] * _mat[1][1]) - (_mat[0][1] * _mat[1][0]));
   }
   
   
   /**
    * Remove a row from the matrix.
    * @param k the index of a row to remove.
    */
    void Matrix::removeRow(int k) {
      double ** mat = alloc(_rows-1,_cols);
      int i,j,ii=0;
      for(i = 0; i < _rows; i++) {
         if(i != k) {
            for(j = 0; j < _cols; j++) {
               mat[ii][j] =_mat[i][j];
            }
            ii++;
         }
      }
      freeMatrix2();
      _mat = mat;
      _rows--;
   }
   
   /**
    * Remove a column from the matrix.
    * @param k the index of a column to remove.
    */
    void Matrix::removeCol(int k) {
      double ** mat = alloc(_rows,_cols-1);
      int i,j,jj;
      for(i = 0; i < _rows; i++) {
         mat[i] = new double[_cols-1];
         jj = 0;
         for(j = 0; j < _cols; j++) {
            if(j != k) {
                mat[i][jj] =_mat[i][j];
                jj++;
            }
         }
      }
      freeMatrix2();
      _mat = mat;
      _cols--;
   }
   
   /**
    * Compute the determanent of this matrix.
    * @return the determanent of this matirx.
    */
   double Matrix::det()
   {
      if(_rows != _cols)
         throw Exception("Matrix must be square to compute the determanent!");
      if(_rows == 2 && _cols == 2)
         return _det2x2();
      else if(_rows == 1 && _cols == 1)
         return _mat[0][0];
      else {
         double total = 0.0;
         Matrix temp;
         //int i2,j2;
         for(int ii = 0; ii < _cols; ii++) {
            temp = Matrix(*this);
            temp.removeRow(0);
            temp.removeCol(ii);
            total += (_mat[0][ii] * pow(-1.0, (ii+1)+1) * temp.det());
         }
         return total;
      }
   }

   /**
    * Compute the adjoint of this matrix.
    * @return the adjoint of this matrix.
    */
    Matrix Matrix::adj() {
      Matrix temp, ret = Matrix(_rows, _cols, 0.0);
      int i,j;
      for(i = 0; i < _rows; i++) {
         for(j = 0; j < _cols; j++) {
            temp = Matrix(*this);
            temp.removeRow(i);
            temp.removeCol(j);
            ret.set(i, j, pow(-1.0, (i+1)+(j+1)) * temp.det());
         }
      }
      return ret.transpose();
   }
   
   /**
    * Compute the inverse of a matrix.
    * @return the inverse of this matrix or 0 if there is no inverse
    *         for this matrix.
    */
    Matrix Matrix::inverse() {
      double d = det();
      if(d == 0.0)
         throw Exception("bad det()");
      return adj().scalerMul(1.0 / d);
   }
   
   /**
    * Swap two rows of this matrix.
    * @param i1 the first row.
    * @param i2 the second row.
    */
    void Matrix::swapRows(int i1, int i2) {
      double temp;
      for(int j = 0; j < _cols; j++) {
         temp = _mat[i1][j];
         _mat[i1][j] = _mat[i2][j];
         _mat[i2][j] = temp;
      }
   }
   
   /**
    * Multiply one row by a scaler and add it to another row.
    * @param srcRow the row to multiply by the scaler.
    * @param destRow the row to add the multipl too.
    */
    void Matrix::addMulOfRowdoubleoRow(int srcRow, int destRow, double scaler) {
      double * row = new double[_cols];
      for(int i = 0; i < _cols; i++)
         row[i] = _mat[srcRow][i] * scaler;
      for(int i = 0; i < _cols; i++)
         _mat[destRow][i] += row[i];
   }
   
   /**
    * Multiply a row by a scaler.
    * @param i the row index.
    * @param s the scaler.
    */
    void Matrix::mulRowByScaler(int i, double s) {
      for(int j = 0; j < _cols; j++)
         _mat[i][j] *= s;
   }
   
   /**
    * Row echelon reduce a matrix.
    * @return the row reduced matrix for this matrix.
    */
    Matrix Matrix::rref() {
      Matrix ret = Matrix(*this);
      int i,j,k;
      //double temp;
      boolean found;
      for(i = 0, j = 0; i < _rows && j < _cols; i++, j++) {
         if(ret._mat[i][j] == 1.0) {
            for(k = 0; k < _rows; k++) {
               if(k != i && ret._mat[k][j] != 0.0) {
                  ret.addMulOfRowdoubleoRow(i, k, -ret._mat[k][j]);
               }
            }
         }
         else if(ret._mat[i][j] == 0.0) {
            found = false;
            for(k = i; k < _rows; k++) {
               if(ret._mat[k][j] != 0.0 && i != k) {
                  found = true;
                  break;
               }
            }
            if(found) {
               ret.swapRows(i, k);
               i--;
               j--;
            }
            else {
               i--;
            }
         }
         else {
            ret.mulRowByScaler(i, 1.0 / ret._mat[i][j]);
            for(k = 0; k < _rows; k++) {
               if(k != i && ret._mat[k][j] != 0.0) {
                  ret.addMulOfRowdoubleoRow(i, k, -ret._mat[k][j]);
               }
            }
         }
      }
      return ret;
   }
      
   /**
    * Return the identity matrix for a given dimension.
    * @param dim the dimension for identity matrix.
    */
    Matrix Matrix::identity(int dim) {
      Matrix ret = Matrix(dim, dim);
      for(int i = 0, j = 0; j < dim; j++, i++)
         ret._mat[i][j] = 1.0;
      return ret;
   }

}
