#include <stdio.h>
#include <math.h>
#include <stdlib.h>

void swap_vec(float* x, float* y, unsigned n) {
    unsigned i; float tmp;
    for (i=0; i<n; ++i) { tmp=x[i]; x[i]=y[i]; y[i]=tmp; }
}

void swap(float* x, float* y) {
    float tmp; tmp=*x; *x=*y; *y=tmp;
}

unsigned max_col(float* A, unsigned col, unsigned n) {
    unsigned i, maxi; float max; max = A[n*col+col]; maxi = col;
    for (i = col+1; i < n; ++i) {
        if(fabsf(A[i*n+col]) > fabsf(max)) { max = A[i*n+col]; maxi=i; }
    }
    return maxi;
}

int solve_gauss(float* A, float* x, float* b, unsigned n) {
    /*
     * Gaussian elimination based equation system solver
     * solves equation Ax = b for x, where A is a
     * square nxn matrix, x and b are n-dimensional vectors
     * Puts solution in x (if possible), modifies A, x, b
     * output: 0 - ok, 1 - A is singular (unsolvable system or
     * has more than 1 solutions)
     */
    unsigned i, j, k, l;
    float scaler;
    // stage I
    for (j = 0; j < n; ++j) {
        // select new pivot and swap
        l = max_col(A, j, n);
        swap_vec(&A[j*n], &A[l*n], n);
        swap(&b[j], &b[l]);
        if (fabsf(A[j*n+j])<1e-15) return 1;
        // ^ zero on diagonal -> singularity -> unsolvable
        // eliminate A(j,j) from rows below
        for (i = (j+1); i < n; ++i) {
            scaler = A[i*n+j]/A[j*n+j];
            b[i] -= b[j]*scaler;
            for (k = 0; k < n; ++k) {
                A[i*n+k] -= A[j*n+k]*scaler;
            }
        }
    }
    // stage II
    for (i = n; i > 0; --i) {
        --i; x[i] = b[i];
        for(j = n-1; j > i; --j ) x[i] -= x[j]*A[i*n+j];
        x[i] /= A[i*n+i]; ++i;
    }
    return 0;
}
void print_vec(float* v, unsigned n) {
    int i;
    printf("[ ");
    for (i = 0; i < n; ++i) printf("%5.3f ", v[i]);
    printf("]");
}

void print_mat(float* A, unsigned n) {
    int i;
    for (i = 0; i < n; ++i) {
        print_vec(&A[i*n], n);
        printf("\n");
    }
}

#define NUM 3

int main(void)
{
    int sing;
    float A[NUM][NUM] = {{-6, 5, 4}, {-3, 7, 2}, {-2, 0, 2}};
    float b[NUM] = { 1, 2, 3 };
    float x[NUM];
    printf("A =\n");
    print_mat((float*) A, NUM);
    printf("b =\n");
    print_vec(b, NUM);
    printf("\nEliminating...\n");
    sing  = solve_gauss((float*) A, x, b, NUM);
    printf("A =\n");
    print_mat((float*) A, NUM);
    printf("b =\n");
    print_vec(b, NUM);
    if (sing) printf("\nMatrix A is singular!\n");
    else {
    printf("\nx =\n");
    print_vec(x, NUM);
    printf("\n");
    }
    return 0;
}