#include "Matrix.hpp"
#include <iomanip>

// Handle errors
void MatrixError(const string& msg)
{
   cout << "Error in " << msg << "\n";
   exit(1);
}



// Returns true if r,c is a valid element in the matrix
bool Matrix::hasElement(int r, int c) const
{
   return (r>=0 && r<rows && c>=0 && c<columns);
}


// Returns linear index of r,c element in vector
int Matrix::index(int r, int c) const
{
   if (r<0 || r>=rows)    MatrixError("index: Error in row");
   if (c<0 || c>=columns) MatrixError("index: Error in column");
         
   return c*rows+r;
}


// Returns value of the element at position r,c
const double& Matrix::Element(int r,int c) const
{
   if (r<0 || r>=rows)    MatrixError("Element: Error in row");
   if (c<0 || c>=columns) MatrixError("Element: Error in column");
          
  return elements[index(r,c)];
}


// Returns reference to the element at position r,c
double& Matrix::Element(int r,int c)
{
   if (r<0 || r>=rows)    MatrixError("Element: Error in row");
   if (c<0 || c>=columns) MatrixError("Element: Error in column");
          
  return elements[index(r,c)];
}


// Sets the value of the element at position r,c to e.
void Matrix::SetElement(int r,int c, double e)
{
   if (r<0 || r>=rows)    MatrixError("SetElement: Error in row");
   if (c<0 || c>=columns) MatrixError("SetElement: Error in column");

   Element(r,c)=e;
}


// operator() returns reference, thus we can do A(r,c)=value
double& Matrix::operator()(int r,int c)
{
   return Element(r,c);
}


// operator() const  returns the value of that element, but not as reference
const double& Matrix::operator()(int r,int c) const
{
   return elements[index(r,c)];
}


// Make this matrix into the identity matrix
void Matrix::Identity()
{
   if (Rows() != Columns())       MatrixError("Identity: Error not symmetric matric");
   
   for (int r=0; r<rows; r++)
      for (int c=0; c<columns; c++)
          if (r==c) Element(r,c)=1.0;
               else Element(r,c)=0.0;
   
}





// A=B   => A.operator=(B)
Matrix& Matrix::operator=(const Matrix &B)
{
   // We are A, and receive B as an argument.

   if (this == &B)       // What if the call is B=B?
      return *this;      // Yes, so skip assignment, and just return *this.

   // Copy B into ourself, discarding whatever is there before.
   rows=B.Rows();
   columns=B.Columns();
   elements.resize(rows*columns);
   
   for (int r=0; r<rows; r++)
      for (int c=0; c<columns; c++)
           Element(r,c)=B(r,c);

   // We should return a reference to ourself. this is always a pointer to ourself.
   return *this;
}


// A+=B  => A.operator+=(B)
Matrix& Matrix::operator+=(const Matrix &B)
{
   // We are again A, and receive B as an argument.
    
   // First check + makes sense for these two matrices
   if (Rows() != B.Rows())       MatrixError("operator+=: Error mismatching rows");
   if (Columns() != B.Columns()) MatrixError("operator+=: Error mismatching Columns");
   
   for (int r=0; r<rows; r++)
      for (int c=0; c<columns; c++)
           Element(r,c)+=B(r,c);

    return *this;
}


// A-=B  => A.operator-=(B)
Matrix& Matrix::operator-=(const Matrix &B)
{
   // We are again A, and receive B as an argument.
    
   // First check + makes sense for these two matrices
   if (Rows() != B.Rows())       MatrixError("operator+=: Error mismatching rows");
   if (Columns() != B.Columns()) MatrixError("operator+=: Error mismatching Columns");
   
   for (int r=0; r<rows; r++)
      for (int c=0; c<columns; c++)
           Element(r,c)-=B(r,c);

    return *this;
}


// A*=B  => A.operator*=(B)
Matrix& Matrix::operator*=(const Matrix &B)
{
   // We are again A, and receive B as an argument.
   // We have to turn A into A*B and return ourself.
    
   // First check * makes sense for these two matrices   A.columns == B.rows
   if (Columns() != B.Rows())       MatrixError("operator*=: Error mismatching rows and columns");
   int N=Columns();
   
   // After the operation M = A*B the resulting matrix has
   // A.rows and B.columns, we need to store the product matrix
   // while calculating its values.
   
   // Shape of product matrix
   int prodrows    = Rows();
   int prodcolumns = B.Columns();
   vector<double> prod(prodrows*prodcolumns, 0.0);
   
   for (int r=0; r<prodrows; r++)
      for (int c=0; c<prodcolumns; c++)
         {
            int pindex = c*prodrows+r;
            for (int i=0;i<N;i++) prod[pindex] += Element(r,i)*B(i,c);     
         }

   // Now overwrite internal data of A with those of the product matrix
   rows=prodrows;
   columns=prodcolumns;
   elements=prod;

   // Return ourself
   return *this;
}


// A*=x  => A.operator*=(x)
Matrix& Matrix::operator*=(const double &x)
{
   // We are again A, and receive x as an argument.

   for (int r=0; r<rows; r++)
      for (int c=0; c<columns; c++)
           Element(r,c)*=x;

    return *this;
}



// Binary operators, we define them reusing operator= that we wrote above.
// No need to replicate code.

// C=A+B =>   C.operator=(   A.operator+(B)   )
const Matrix Matrix::operator+(const Matrix &B) const
{
   Matrix sum(*this);
   sum+=B;
   return sum;   
}

// C=A-B =>   C.operator=(   A.operator-(B)   )
const Matrix Matrix::operator-(const Matrix &B) const
{
   Matrix diff(*this);
   diff-=B;
   return diff;   
}

// C=A*B =>   C.operator*(   A.operator*(B)   )
const Matrix Matrix::operator*(const Matrix &B) const
{
   Matrix prod(*this);
   prod*=B;
   return prod;   
}


// A*x  => A.operator*(double x)
const Matrix Matrix::operator*(double x) const
{
   Matrix M(*this);
   M*=x;
   return M;
}


cout << A << B << C;


// Define how a matrix should be printed
ostream& operator<<(ostream& out, Matrix& m)
{
   for (int r=0; r<m.Rows(); r++)
     {
        for (int c=0; c<m.Columns(); c++)
           {
              out << setw(8) << m.Element(r,c);
           }
        out << endl;
     }
     
  return out;
}


// Add scale row r2 scaled by c to row r1
void Matrix::AddScaledRow(int r1, int r2,double scale)
{
   if (r1<0 || r1>=Rows()) MatrixError("AddScaledRow: Error target row outside range");
   if (r2<0 || r2>=Rows()) MatrixError("AddScaledRow: Error source row outside range");
   
   for (int c=0; c<columns; c++) Element(r1,c)+=scale*Element(r2,c);
}

// Multiply row r by scale
void Matrix::ScaleRow(int r, double scale)
{
   if (r<0 || r>=Rows()) MatrixError("ScaleRow: Error target row outside range");
   
   for (int c=0; c<columns; c++) Element(r,c)*=scale;
}

// Swap two rows
void Matrix::SwapRows(int r1,int r2)
{
   if (r1<0 || r1>=Rows()) MatrixError("SwapRows: Error target row outside range");
   if (r2<0 || r2>=Rows()) MatrixError("SwapRows: Error source row outside range");
   
   if (r1=!r2)
     for (int c=0; c<columns; c++)
        {
          double tmp=Element(r1,c);
          Element(r1,c)=Element(r2,c);
          Element(r2,c)=tmp;
        }
}



// Apply Gauss Jordan turning the matrix into the identity, and return its inverse.
void Matrix::GaussJordan(Matrix& Inverse)
{
   if (Rows() != Columns()) MatrixError("GaussJordan: Error rows != columns");
   
   Inverse.SetSize(Rows(), Columns());
   Inverse.Identity();
   
   // Apply Gauss Jordan transformation to the present matrix.
   // apply transformations to Inverse, such that it becomes the inverse of the present matrix.
   // At the end the present matrix will be the identity matrix.


/*
   for s 0,..,N
     {
      // Create non-zero element in element s,s     
      if M[s,s]==0
          {
             find r (s+1,..,N) så M[r,s]!=0
             SwapRows(s,r)
          }
      
      // Normalize     
      ScaleRow(s, 1.0/M[s,s])
      
      // Create zeros above diagonal
      for r   s+1,..,N
           AddScaledRow(r,s,-M[r,s])
     }
     
   
   for s = N,..,1
      for r = 0,..,s-1
         AddScaledRow(r,s,-M[r,s])
      
   // All Row operation also performed for Inverse.
          
*/

}