lab3

using namespace std;

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

template <int N>
struct vec {
    double data[N];
    double &operator[](int i) { return data[i]; }
    const double &operator[](int i) const { return data[i]; }
};

#define defop(OUT, op, LIN, RIN, _lvalue, _rvalue)                                      \
template <int N> OUT operator op(LIN _1, RIN _2) {                                      \
    OUT _out; for (int i = 0; i < N; i++) _out[i] = _lvalue op _rvalue; return _out;    \
}

defop(vec<N>, +, const vec<N> &, const vec<N> &, _1[i], _2[i]);
defop(vec<N>, -, const vec<N> &, const vec<N> &, _1[i], _2[i]);
defop(vec<N>, *, const vec<N> &, const double &, _1[i], _2);
defop(vec<N>, /, const vec<N> &, const double &, _1[i], _2);

typedef vec<4> vec4;
typedef vec<5> vec5;
typedef vec<5> mat4x5[4];

void print(mat4x5 matrix) {
    for (int i = 0; i < 4; i++) {
        for (int j = 0; j <= 4; j++) {
            printf("%-8g", matrix[i][j]);
        }
        printf("\n");
    }
}

void CompletePivoting(mat4x5 matrix) {
    printf("Complete pivoting method\n");
    print(matrix);

    mat4x5 temp;
    for (int k = 0, p, q; k < 3; k++) {
        double a = 0;
        for (int i = 0; i < 4; i++) {
            for (int j = 0; j < 4; j++) {
                double t = fabs(matrix[i][j]);
                if (t > a) {
                    a = t;
                    p = i;
                    q = j;
                }
            }
        }

        temp[k] = matrix[p];
        for (int i = 0; i < 4; i++) {
            if (i == p) continue;

            matrix[i] = matrix[i] - matrix[p] * matrix[i][q] / a;
        }

        matrix[p] = {0};

        for (int i = 0; i < 4; i++) {
            matrix[i][q] = 0;
        }

    }

    for (int i = 0; i < 4; i++) {
        if (matrix[i][4] != 0) {
            temp[3] = matrix[i];
            break;
        }
    }

    vec4 x = {0};
    for (int i = 3; i >= 0; i--) {
        double R = temp[i][4];
        for (int j = 0; j < 4; j++) {
            if ((temp[i][j] != 0) && (x[j] != 0)) {
                R -= temp[i][j] * x[j];
            }
        }
        for (int j = 0; j < 4; j++) {
            if ((temp[i][j] != 0) && (x[j] == 0)) {
                x[j] = R / temp[i][j];
            }
        }
    }

    for (int i = 0; i < 4; i++) {
        printf("x%i = %f\n", i, x[i]);
    }
}

void SimpleIteration(mat4x5 matrix) {
    printf("Simple iteration method\n");
    print(matrix);

    for (int i = 0; i < 4; i++) {
        matrix[i] = matrix[i] / matrix[i][i];
    }

    double q = 0;
    for (int i = 0; i < 4; i++) {
        double s = 0;
        for (int j = 0; j < 4; j++) {
            if (i == j) continue;
            s += fabs(matrix[i][j]);
        }
        if (s > q) q = s;
    }

    vec4 x = {0};

    double error = 1e-8 * fabs(((1 - q) / q));
    while(true) {
        vec4 x0 = x;
        for (int i = 0; i < 4; i++) {
            x[i] = matrix[i][4];
            for (int j = 0; j < 4; j++) {
                if (i == j) continue;
                x[i] -= matrix[i][j] * x0[j];
            }
        }

        double err = 0;
        for (int i = 0; i < 4; i++) {
            double t = fabs(x0[i] - x[i]);
            if (t > err) err = t;
        }

        if (err < error) break;
    }

    for (int i = 0; i < 4; i++) {
        printf("x%i = %f\n", i, x[i]);
    }
}

int main() {
    mat4x5 matrix1 = {
        13, 14, 17, 14, 146,
        9,  26,  7,  9, 135,
        8,  4,  25, 11, 119,
        15, 11,  5, 16, 109
    };

    mat4x5 matrix2 = {
        11, -5,-10, 10,   1, // [0] + [3] - [2] - [1],
         9, 26,  7,  9, 135, // [1],
        13, 14, 17, 14, 146, // [0],
        15, 11,  5, 16, 109, // [3],
    };

    CompletePivoting(matrix1);
    printf("\n");
    SimpleIteration(matrix2);
    return 0;
}