#include <iostream>
#include <iomanip>

// A procedure implementing a Gaussian elimination algorithm (with row pivoting) for
// solving the equation Ax=b, where:
// A - nonsingular square matrix (changed to upper-triangular in the procedure),
// b - a vector containing the RHS on entering and the solution on exiting
// n - the dimention of the vector b (and the sqare matrix A)
int gaussian_elimination(double** A, double *b, int n)
{
   int i,j,k, tmp;

   //STEP 0.5 - ouput the matrix A (included for debugging only)
   for (int i=0; i<n; i++)
   {
      for (int j=0; j<n; j++)
         std::cout << A[i][j] << " ";
      std::cout << std::endl;
   }

   //STEP 1 - converting A to an upper-triangular matrix.

   int *p = new int[n];
   for(i=0; i<n; i++)
      p[i] = i;
   
   // A cycle through the rows.
   for (i=0; i<n; i++)
   {
      // Find the maximal element in the column.
      int index = i;
      for(int j=i+1; j<n; j++)
      {
         if (fabs(A[p[j]][i]) > fabs(A[p[i]][i]))
             index = j;
      }

      // Interchange the i-th and index-th rows
      tmp = p[i]; p[i] = p[index]; p[index] = tmp;

      // Clear the entries below the diagonal in the i-th column
      for(int j=i+1; j<n; j++)
      {
         double factor = -A[p[j]][i] / A[p[i]][i];
         A[p[j]][i] = 0; //do the calculation ourselves to avoid entries of the type 2.3456e-21
         for (int k=i+1; k<n; k++)
         {
            A[p[j]][k] += A[p[i]][k]*factor;
         }

         b[p[j]] += b[p[i]]*factor;
      }
   }

   //STEP 1.5 - print out the matrix A (included for debugging only)
   for (int i=0; i<n; i++)
   {
      for (int j=0; j<n; j++)
         std::cout << std::setw(7) << A[i][j] << " ";
      std::cout << std::endl;
   }

   std::cout << "And with the proper ordering:" << std::endl;
   for (int i=0; i<n; i++)
   {
      for (int j=0; j<n; j++)
         std::cout << std::setw(7) << A[p[i]][j] << " ";
      std::cout << std::endl;
   }

   //STEP 2 - Do the backward solver (since the matrix is triangular)
   for (i=n-1; i>=0; i--)
   {
      for(k=n-1; k>i; k--)
         b[p[i]] -= b[p[k]]*A[p[i]][k];

      if(A[p[i]][i]==0)
         return -i;

      b[p[i]] = b[p[i]]/A[p[i]][i];
   }

   //STEP 3 - Reorder the solution
   double* temp = new double[n];
   for(i=0; i<n; i++)
      temp[i] = b[i];
   for(i=0; i<n; i++)
      b[i] = temp[p[i]];

   // Clean up the memory
   delete temp;
   delete p;
}

int main()
{
   int N;

   std::cout << "Enter the dimension of the problem: " << std::endl;
   std::cin >> N;
   std::cout << std::endl;
   
   double** A = new double*[N];
   double*  b = new double[N];

   for (int i=0; i<N; i++)
      A[i] = new double[5];

   for (int i=0; i<N; i++)
   {
      std::cout << "Enter row number "<< i <<": "<< std::endl;

      for (int k=0; k<N; k++)
      {
         std::cin >>A[i][k];
      }
   }

   std::cout << std::endl;
   std::cout << "Enter the right hand side: " << std::endl;
   for (int k=0; k<N; k++)
   {
      std::cin >>b[k];
   }

   gaussian_elimination(A,b,N);
   
   for (int i=0; i<N; i++)
      std::cout << "b[" << i << "]=" << b[i] << std::endl;

   for (int i=0; i<N; i++)
      delete A[i];
   delete A;
   delete b;
}