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

#define EPS 1e-100
#define ISZERO(e) (abs(e) < EPS)

/*** GLS ***/
typedef struct {
    float ** data;
    float * rs;
    int size;
} GLS;

GLS glsCreate(int size) {
    float** data = malloc(sizeof(float*) * size);
    for(unsigned int row = 0; row < size; row++) data[row] = malloc(sizeof(float)*size);
    float* rs = malloc(sizeof(float) * size);
    return (GLS){data,rs,size};
}

void glsSet(GLS m, size_t row, size_t col, float v) {
    assert(row < m.size);
    assert(col < m.size);
    m.data[row][col] = v;
}

float glsGet(GLS m, size_t row, size_t col) {
    assert(row < m.size);
    assert(col < m.size);
    return m.data[row][col];
}

float glsGetRS(GLS m, size_t row) {
    assert(row < m.size);
    return m.rs[row];
}

void glsSetRS(GLS m, size_t row, float value) {
    assert(row < m.size);
    m.rs[row] = value;
}

void glsSwapRows(GLS m, size_t row1, size_t row2) {
    assert(row1 < m.size);
    assert(row2 < m.size);
    float* tmp = m.data[row1];
    m.data[row1] = m.data[row2];
    m.data[row2] = tmp;
    float tmp2 = m.rs[row1];
    m.rs[row1] = m.rs[row2];
    m.rs[row2] = tmp2;
}

void glsTransform(GLS m, size_t row1, float fac1, size_t row2, float fac2) {
    assert(row1 < m.size);
    assert(row2 < m.size);

    for(unsigned int col = 0; col < m.size; col++) {
        glsSet(m,row1,col, fac1 * glsGet(m,row1,col) + fac2 * glsGet(m,row2,col) );
    }

    glsSetRS(m, row1, fac1 * glsGetRS(m, row1) + fac2 * glsGetRS(m,row2));
}

void glsPrint(GLS m) {
    for(unsigned int row = 0; row < m.size; row++) {
        for(unsigned int col = 0; col < m.size; col++) {
            printf("%3.1f ", glsGet(m,row,col));
        }
        printf("| %3.1f\n", m.rs[row]);
    }
}

void glsFree(GLS m) {
    for(unsigned int row = 0; row < m.size; row++) free(m.data[row]);
    free(m.data);
    free(m.rs);
}

/*** End GLS ***/

/*** Gauss ***/
int reorder_gls(GLS m, int rowCol) {
    if(rowCol >= m.size) return 1;
    if(!ISZERO(glsGet(m, rowCol, rowCol))) return reorder_gls(m,rowCol+1);

    for(int candidate = rowCol+1; candidate < m.size; candidate++) {
        if(!ISZERO(glsGet(m,candidate, rowCol))) {
            glsSwapRows(m, rowCol, candidate);
            if(reorder_gls(m, rowCol+1)) return 1;
            glsSwapRows(m, rowCol, candidate);
        }
    }

    return 0;
}

int solve_gauss_rec(GLS m, int rowCol) {
    if(rowCol >= m.size) return 1;
    if( ISZERO(glsGet(m,rowCol,rowCol)) ) return 0;

    for(size_t row = rowCol+1; row < m.size; row++) {
        if(!ISZERO(glsGet(m,row, rowCol)))
            glsTransform(m, row, glsGet(m,rowCol,rowCol), rowCol, - glsGet(m,row,rowCol) );
        glsSet(m,row,rowCol, 0.0f);
    }

    if(!solve_gauss_rec(m, rowCol+1)) return 0;

    for(size_t row = 0; row < rowCol; row++) {
        if(!ISZERO(glsGet(m,row,rowCol)))
            glsTransform(m, row, glsGet(m,rowCol,rowCol), rowCol, - glsGet(m,row,rowCol) );
        glsSet(m,row,rowCol, 0.0f);
    }

    glsTransform(m, rowCol, 1.0f / glsGet(m,rowCol,rowCol), rowCol, 0.0f );
    glsSet(m, rowCol, rowCol, 1.0f);

    return 1;
}

int solve(GLS m) {
    if(!reorder_gls(m,0)) return 0;
    if(!solve_gauss_rec(m,0)) return 0;

    return 1;
}

int main( int argc, char *argv[] ) {

    GLS m = glsCreate(3);
    glsSet(m,0,0,3); glsSet(m,0,1,-1); glsSet(m,0,2,-1); glsSetRS(m,0, 0);
    glsSet(m,1,0,1); glsSet(m,1,1,4);  glsSet(m,1,2,2);  glsSetRS(m,1, 4);
    glsSet(m,2,0,0); glsSet(m,2,1,2);  glsSet(m,2,2,1);  glsSetRS(m,2, 1);

    printf("Input: \n");
    glsPrint(m);
    if(solve(m)) {
        printf("Output: \n");
    } else {
        printf("Failed to solve. :(\n");
    }
    glsPrint(m);

	return 0;
}