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

#define data_t double
#define MODIFIER_A "%4.01lf"
#define MODIFIER_B "%9.05lf"


void swap(data_t* x, data_t* y) {
    data_t buffer = *x;
    *x = *y;
    *y = buffer;
}

// Добавить к одной строке другую, применяется в случае нуля на диагонали
void swap_rows_inplace(data_t* A, size_t matrix_size, size_t row_first_index, size_t row_second_index) {
    data_t* row_first = A + matrix_size * row_first_index;
    data_t* row_second = A + matrix_size * row_second_index;

    // cache friendly optimization for sse2 set of instructions
    for (size_t index = 0; index < matrix_size; ++index) {
        swap(row_first + index, row_second + index);
    }
}


void gauss_forward(data_t *A, data_t *B, size_t matrix_size) {
    for (size_t row_index = 0; row_index < matrix_size; ++row_index) {
        if (A[matrix_size * row_index + row_index] == 0) {

            for (size_t next_row_index = row_index + 1; next_row_index < matrix_size; ++next_row_index) {
                if (A[matrix_size * next_row_index + row_index] != 0) {
                    swap_rows_inplace(A, matrix_size, row_index, next_row_index);
                    swap(B + row_index, B + next_row_index);
                    break;
                }
            }

            if (A[matrix_size * row_index + row_index] == 0) {
                printf("Oops, det A = 0\n");
                exit(EXIT_FAILURE);
            }

        }

        for (size_t next_row_index = row_index + 1; next_row_index < matrix_size; ++next_row_index) {
            data_t multiplier = A[next_row_index * matrix_size + row_index] / A[row_index * matrix_size + row_index];

            for (size_t k = row_index; k < matrix_size; ++k) {
                A[next_row_index * matrix_size + k] -= A[row_index * matrix_size + k] * multiplier;
            }

            B[next_row_index] -= multiplier * B[row_index];
        }

    }
}

void gauss_backward(data_t* A, data_t* B, size_t matrix_size) {
    for (int row_index = (int)matrix_size - 1; row_index >= 0; --row_index) {
        data_t diag_elem = A[row_index * matrix_size + row_index];

        A[row_index * matrix_size + row_index] /= diag_elem;
        B[row_index] /= diag_elem;

        for (int prev_row_index = row_index - 1; prev_row_index >= 0; --prev_row_index) {
            data_t local_mul =  A[prev_row_index * matrix_size + row_index];
            A[prev_row_index * matrix_size + row_index] = 0;
            B[prev_row_index] -= B[row_index] * local_mul;
        }
    }
}

void print_system(data_t* A, data_t* B, size_t matrix_size) {
    for (size_t i = 0; i < matrix_size; ++i) {
        for (size_t j = 0; j < matrix_size; ++j) {
            printf(MODIFIER_A " ", A[matrix_size * i + j]);
        }
        printf(" |" MODIFIER_B "\n", B[i]);
    }
    printf("\n");
}

int main(void) {
    size_t matrix_size = 20;
    data_t* A = (data_t*) malloc (sizeof(data_t) * matrix_size * matrix_size);
    data_t* B = (data_t*) malloc (sizeof(data_t) * matrix_size);

    if (A == NULL || B == NULL) {
        printf("Not enough memory\n");
        free(A);
        free(B);
        return EXIT_FAILURE;
    }

    // Заполняем А
    A[0 * matrix_size + 0] = 1;
    for (size_t i = 1; i + 1 < matrix_size; ++i) {
        A[i * matrix_size + i] = -2; // Главная диагональ
        A[i * matrix_size + i + 1] = 1; // Над главной диагональю
        A[i * matrix_size + i - 1] = 1; // под главной диагональю
    }

    for (size_t i = 0; i < matrix_size; ++i) {
        A[(matrix_size - 1) * matrix_size + i] = 1;
    }

    // Заполняем В
    B[0] = 1;
    for (size_t i = 1; i + 1 < matrix_size; ++i) {
        B[i] = 2.0 / (i + 1) / (i + 1);
    }
    B[matrix_size - 1] = -19.0 / 3;

    print_system(A, B, matrix_size);
    gauss_forward(A, B, matrix_size);

    // print_system(A, B, matrix_size); // диагональная после прямого хода гаусса
    gauss_backward(A, B, matrix_size);

    print_system(A, B, matrix_size);

    // В содержит решение методом гаусса


}