#include <math.h>
#include <stdio.h>
#include <stdarg.h>
#include <stdlib.h>
#include "lsolve.h"

double getelm(double* matrix, const unsigned int i, const unsigned int j, const unsigned int m) {
    const unsigned n = m + 1;
    return *((matrix + i * n) + j);
}

void setelm(double* matrix, const unsigned i, const unsigned int j, const unsigned int m, double val) {
    const unsigned n = m + 1;
    *((matrix + i * n) + j) = val;
}

unsigned int argmax_abs(const unsigned int low, const unsigned int high, const unsigned int column, double* A) {
    unsigned int i_max = 0;
    double max_value = -1.0;  // computes the abs argmax, so always larger than -1
    for(unsigned int i = low; i < high; ++i) {
        double a = fabs(getelm(A, i, column, high));
        if(a > max_value) {
            max_value = a;
            i_max = i;
        }
    }
    return i_max;
}

void swrows(double* matrix, const unsigned int r1, const unsigned int r2, const unsigned int cols) {
    if(r1 == r2) { return; }
    double tmp;
    for(unsigned int i = 0; i < cols; ++i) {
        tmp = getelm(matrix, r2, i, cols - 1);
        setelm(matrix, r2, i, cols - 1, getelm(matrix, r1, i, cols - 1));
        setelm(matrix, r1, i, cols - 1, tmp);
    }
}

void lsolve(double* matrix, const unsigned int size) {
    const double eps = 1e-4;
    const unsigned int m = size;        // rows
    const unsigned int n = size + 1;    // columns
    unsigned int h = 0;                 // pivot row
    unsigned int k = 0;                 // pivot column

    while(h < m && k < n) {
        unsigned int i_max = argmax_abs(h, m, k, matrix);
        double elm = getelm(matrix, i_max, k, m);
        if(fabs(elm) <= eps) {
            // Go to the next column
            k += 1;
        } else {
            swrows(matrix, h, i_max, n);
            for(unsigned int i = k; i < n; ++i) {
                setelm(matrix, h, i, m, getelm(matrix, h, i, m) / elm);
            }
            for(unsigned int i = 0; i < m; ++i) {
                if(i == h) { continue; }
                double f = getelm(matrix, i, k, m) / getelm(matrix, h, k, m);
                setelm(matrix, i, k, m, 0.0);
                for(unsigned int j = k + 1; j < n; ++j) {
                    setelm(matrix, i, j, m, getelm(matrix, i, j, m) - getelm(matrix, h, j, m) * f);
                }
            }
            swrows(matrix, h, i_max, n);

//            printf("MAT %d, %d\n", i_max, k);
//            for(unsigned int i = 0; i < m; ++i) {
//                for(unsigned int j = 0; j < n; ++j) {
//                    printf("\t%7f", getelm(matrix, i, j, m));
//                }
//                printf("\n");
//            }

            h += 1;
            k += 1;
        }
    }
}

double det(double* matrix, const unsigned int size) {
    unsigned int height = size - 1;

    if(size == 2) {
        double a00 = getelm(matrix, 0, 0, size);
        double a01 = getelm(matrix, 0, 1, size);
        double a10 = getelm(matrix, 1, 0, size);
        double a11 = getelm(matrix, 1, 1, size);
        return (a00 * a11) - (a01 * a10);
    }

    double total = 0.0;
    for(unsigned int fc = 0; fc < size; ++fc) {
        double As[height][height];
        for(unsigned int r = 0; r < height; ++r) {
            for(unsigned int c = 0; c < fc; ++c) {
                As[r][c] = getelm(matrix, r + 1, c, height);
            }
            for(unsigned int c = fc; c < height; ++c) {
                As[r][c] = getelm(matrix, r + 1, c + 1, height);
            }
        }
        double sign = -1.0;
        if(fc % 2 == 1) {
            sign = 1.0;
        }
        double sub_det = det(&As, height);
        total += sign * getelm(matrix, 0, fc, height) * sub_det;
    }
    return total;
}

void agloop(double* A, const unsigned int size, ...) {
    double D = det(A, size);
    if(fabs(D) < 1e-5) {
        printf("ERROR: SINGULAR MATRIX!\n");
        exit(1);
    }
    lsolve(A, size);

    // Read the arguments
    va_list list;
    va_start(list, size);
    for(unsigned int j = 0; j < size; ++j) {
        *va_arg(list, double*) = getelm(A, j, size, size);
    }
    va_end(list);
}

//int main(int argc, char *args[]) {
//    double A[2][3] = {{-1.0, 6.0, 0.0}, {1.0, -1.0, -3.0}};
//    lsolve(&A, 2);
//
////    printf("END\n");
////    for(unsigned int i = 0; i < 2; ++i) {
////        for(unsigned int j = 0; j < 3; ++j) {
////            printf("\t%7f", A[i][j]);
////        }
////        printf("\n");
////    }
//}
