//####################################################################
//Using Gaussian elimination to solve linear equations.
//####################################################################
#include <stdio.h>

//To disable PIVOTING, comment the next line
#define PIVOTING


//==================================================================
// prints a vector of doubles, one per line.
//==================================================================
void print_dvector(double v[], int w)
{
    int i;
    for(i=0; i<w; i++) {
	printf("%8.3f\n", v[i]);
    }
}

//==================================================================
// prints a matrix of double numbers.
//==================================================================
void print_dmatrix(double m[][4], int h, int w)
{
    int i, j;
    for(i=0; i<h; i++) {
        for(j=0; j<w; j++)
            printf("%8.3f ", m[i][j]);
        printf("\n");
    }
    printf("\n");
}

//==================================================================
// Gauss elimination with pivoting
//==================================================================
void genp(double m[][4], double v[], int h, int w)
{
    int row, next_row, col;
    double factor;

    #ifdef PIVOTING
    int max_row;
    double tmp;
    #endif
    
    for(row = 0; row < (h - 1); ++row) {
        printf("### row = %d\n", row);

        #ifdef PIVOTING
        //find out which row has the max number in the row-th column
        max_row = row;
        for(next_row = row + 1; next_row < h; ++next_row) {
            if(m[next_row][row] > m[max_row][row])
                max_row = next_row;
        }
        printf("  max_row = %d\n", max_row);

        //swap row with max_row
        if(max_row != row) {
            for(col = 0; col < w; ++col) {
                tmp = m[row][col];
                m[row][col] = m[max_row][col];
                m[max_row][col] = tmp;
            }
            printf("  After swapping row with max_row:\n");
            print_dmatrix(m, h, w);
            // also swap the v vector
            tmp = v[row];
            v[row] = v[max_row];
            v[max_row] = tmp;
	    print_dvector(v, w);
        }
        #endif //PIVOTING

        // do the elimination
        for(next_row = row + 1; next_row < h; ++next_row) {
            factor = m[next_row][row] / m[row][row];

            for(col = 0; col < w; ++col)
                m[next_row][col] -= factor * m[row][col];

            v[next_row] -= factor * v[row];
        }
        printf("  After elimination round %d:\n", row);
        print_dmatrix(m, h, w);
	print_dvector(v, w);
    }
}

void back_substitute(double m[][4], double v[],
                     int h, int w){
  int row, next_row;

  for(row = h - 1; row >= 0; --row) {
    v[row] /= m[row][row];
    m[row][row] = 1;
    for(next_row = row - 1; next_row >= 0; --next_row) {
      v[next_row] -= v[row] * m[next_row][row];
      m[next_row][row] = 0;
    }
    printf("*** row= %d\n", row);
    print_dmatrix(m, h, w);
    print_dvector(v, w);
  }
}


int main()
{
    double a[4][4]= {{2,-3,2,5},
                     {1,-1,1,2},
                     {3,2,2,1},
                     {1,1,3,-1}};
    double v[4]= {3,1,0,0};

    printf("Initially:\n");
    print_dmatrix(a, 4, 4);
    print_dvector(v, 4);
    genp(a, v, 4, 4);
    printf("\n\nBack substitution:\n");
    back_substitute(a, v, 4, 4);
}