#include "matutil.h"

#define FLOAT_FORMAT "%10.7f "

void print_vector_float(int size, float *v) {
    int i;
    for (i = 1; i <= size; i++) {
        printf(FLOAT_FORMAT, v[i]);
    }
    printf("\n");
}

void print_vector_int(int size, int *v) {
    int i;
    for (i = 1; i <= size; i++) {
        printf("%d ", v[i]);
    }
    printf("\n");
}

float **new_matrix(int m, int n) {
    float **mat = matrix(1, m, 1, n);
    int i, j;
    for (i = 1; i <= m; i++) {
        for (j = 1; j <= n; j++) {
            mat[i][j] = 0;
        }
    }
    return mat;
}

float **new_diagonal(int n, float *v) {
    float **mat = new_matrix(n, n);
    int i;
    for (i = 1; i <= n; i++) {
        mat[i][i] = v[i];
    }

    return mat;
}

void simple_free_matrix(int m, int n, float **mat) {
    free_matrix(mat, 1, m, 1, n);
}

void print_matrix(int m, int n, float **mat) {
    int i, j;
    for (i = 1; i <= m; i++) {
        for (j = 1; j <= n; j++) {
            printf(FLOAT_FORMAT, mat[i][j]);
        }
        printf("\n");
    }
}

void copy_matrix(int m, int n, float **src, float **dst) {
    int i, j;
    for (i = 1; i <= m; i++) {
        for (j = 1; j <= n; j++) {
            dst[i][j] = src[i][j];
        }
    }
}

float **matmul(int m, int n, int p, float **a, float **b) {
    float **c = new_matrix(m, p);
    int i, j, k;

    for (i = 1; i <= m; i++) {

        for (j = 1; j <= p; j++) {
            c[i][j] = 0;

            for (k = 1; k <= n; k++) {

                c[i][j] += a[i][k] * b[k][j];
            }
        }
    }

    return c;
}

float **mattranspose(int m, int n, float **mat) {
    float **t = new_matrix(n, m);
    int i, j;
    for (i = 1; i <= m; i++) {
        for (j = 1; j <= n; j++) {
            t[j][i] = mat[i][j];
        }
    }
    return t;
}

float **matinv(int m, float **mat) {
    float **inv = new_matrix(m, m);
    int i, j;
    for (i = 1; i <= m; i++) {
        for (j = 1; j <= m; j++) {
            if (i == j) {
                inv[i][j] = 1;
            } else {
                inv[i][j] = 0;
            }
        }
    }
    gaussj(mat, m, inv, m);
    return inv;
}

void create_submatrix(float **matrix, float **sub_matrix, int n, int exclude_col) {
    int sub_i = 1;
    int i, j;
    for (i = 2; i <= n; i++) {
        int sub_j = 1;
        for (j = 1; j <= n; j++) {
            if (j == exclude_col) {
                continue;
            }
            sub_matrix[sub_i][sub_j] = matrix[i][j];
            sub_j++;
        }
        sub_i++;
    }
}

float matdet(int m, float **mat) {
    if (m == 1) {
        return mat[1][1];
    } else if (m == 2) {
        return mat[1][1] * mat[2][2] - mat[1][2] * mat[2][1];
    }

    float det = .0f;

    float **submat = new_matrix(m - 1, m - 1);
    int i, j;
    for(i = 1; i <= m; i ++) {
        create_submatrix(mat, submat, m, i);
        float sign = (i % 2 == 0) ? -1 : 1;
        
        float el = mat[1][i];

        det += el * sign * matdet(m-1, submat);
    }

    simple_free_matrix(m - 1, m - 1, submat);
    return det;
}

/* for JMatrixData */

void copy_jmatdata(JMatrixData *src, JMatrixData *dst) {
    dst->m = src->m;
    dst->n = src->n;
    copy_matrix(src->m, src->n, src->a, dst->a);
    copy_matrix(src->m, 1, src->b, dst->b);
}

void free_jmatdata(JMatrixData *data) {
    simple_free_matrix(data->m, data->n, data->a);
    simple_free_matrix(data->m, 1, data->b);
}

JMatrixData *new_jmatdata(int m, int n) {
    JMatrixData *data = malloc(sizeof(JMatrixData));
    data->m = m;
    data->n = n;
    data->a = new_matrix(data->m, data->n);
    data->b = new_matrix(data->m, 1);
    return data;
}