#include <iostream>
#include <math.h>
#include <cmath>
#include <stdlib.h>
#include <fstream>
using namespace std;

//Нормирует вектор y размерности N
void normalize(double* y, int N)
{
    int i;
    double norm, result = 0;
    for (i = 0; i < N; i++)
        result += y[i] * y[i];
    norm = sqrt(result);

    for (i = 0; i < N; i++)
        y[i] = y[i] / norm;
}

//Записывает в res результат умножения матрицы А на столбец х
void Multiply_Ax(double** A, double* x, double* res, int N)
{
    int i, j;

    for (i = 0; i < N; i++) {
        res[i] = 0;
        for (j = 0; j < N; j++) {
            res[i] += A[i][j] * x[j];
        }
    }
}

void Gauss_Seidel_method(double** A, double* B, double* x, int N, double deviation)
{
    int i, j, k = 0;
    double tmp, sum = 0;

    //Перестановка строк в случае, если есть нулевой диаг элемент
    for (i = 0; i < N; i++) {
        if (A[i][i] == 0) {
            for (j = i + 1; j < N; j++) {
                if (A[j][i] != 0) {
                    for (k = 0; k < N; k++) {
                        tmp = A[j][k];
                        A[j][k] = A[i][k];
                        A[i][k] = tmp;
                    }
                    tmp = x[i];
                    x[i] = x[j];
                    x[j] = tmp;
                    tmp = B[i];
                    B[i] = B[j];
                    B[j] = tmp;
                }
            }
        }
    }

    //Реализация алгоритма
    k = 0;
    int flag = 1;
    while (flag > 0 && k < 300) {
        flag = 0;
        for (i = 0; i < N; i++) {
            for (j = 0; j < N; j++) {
                if (j != i)
                    sum += A[i][j] * x[j];
            }
            tmp = B[i];
            x[i] = (tmp - sum) / A[i][i];
            sum = 0;
        }
        k++;

        //Проверка невязки (B - A*x')
        for (i = 0; i < N; i++) {
            tmp = B[i];
            for (j = 0; j < N; j++)
                tmp -= A[i][j] * x[j];

            if (abs(tmp) > deviation) {
                flag = 1;
            }
        }
    }
}

void Gradient_descent_method(double** A, double* B, double* x, int N, double deviation)
{
    int i, j, k, flag = 1;
    double tau = 0, sum = 0;
    double denominator = 0, numerator = 0, tmp;
    double* r = new double[N];
    double* res = new double[N];

    k = 0;
    while (flag > 0 && k < 300) {
        flag = 0;

        for (i = 0; i < N; i++) {
            sum = 0;
            for (j = 0; j < N; j++)
                sum += A[i][j] * x[j];
            r[i] = sum - B[i];
        }

        Multiply_Ax(A, r, res, N);
        for (j = 0; j < N; j++) {
            denominator += res[j] * r[j];
            numerator += r[j] * r[j];
        }
        tau = numerator / denominator;
        denominator = 0;
        numerator = 0;

        for (i = 0; i < N; i++) {
            x[i] -= tau * r[i];
        }
        //Проверка невязки (B - A*x' = 0)
        for (i = 0; i < N; i++) {
            tmp = B[i];
            for (j = 0; j < N; j++)
                tmp -= A[i][j] * x[j];
            if (abs(tmp) > deviation)
                flag = 1;
            break;
        }
        k++;
    }
}

//Степенной метод нахождения макс собств числа
double Power_iteration(double** A, int N, double deviation)
{
    int flag = 1, i;
    double lambda = 0;

    double* x = new double[N];
    double* y = new double[N];
    for (i = 0; i < N; i++)
        x[i] = 1.0;

    while (flag > 0) {
        flag = 0;

        Multiply_Ax(A, x, y, N);

        for (i = 0; i < N; i++)
            if (abs(y[i] - lambda * x[i]) > deviation)
                flag = 1;

        lambda = 0;
        for (i = 0; i < N; i++)
            lambda += x[i] * y[i];

        normalize(y, N);
        for (i = 0; i < N; i++)
            x[i] = y[i];
    }
    return lambda;
}

void Gaussian_elimination(double** A, double* B, double* x, int n)
{

    double* tmp_mas;
    tmp_mas = new double[n];

    int i, j, k, index;
    double tmp, max, r;

    for (j = 0; j <= n - 2; j++) {

        max = abs(A[j][j]);
        index = j;

        for (i = index + 1; i <= n - 1; i++) {
            if (abs(A[i][index]) > max) {
                max = abs(A[i][index]);
                index = i;
            }
        }

        if (index != j) {
            for (i = 0; i <= n - 1; i++) {
                tmp_mas[i] = A[j][i];
                A[j][i] = A[index][i];
                A[index][i] = tmp_mas[i];
            }
            tmp = B[j];
            B[j] = B[index];
            B[index] = tmp;
        }

        for (i = j + 1; i <= n - 1; i++) {
            r = -A[i][j] / A[j][j];

            for (k = j; k <= n - 1; k++) {
                A[i][k] = A[i][k] + r * A[j][k];
            }
            B[i] = B[i] + r * B[j];
        }
    }

    x[n - 1] = B[n - 1] / A[n - 1][n - 1];
    for (i = n - 2; i >= 0; i--) {
        tmp = 0.0;
        for (int j = i + 1; j <= n - 1; j++)
            tmp += A[i][j] * x[j];
        x[i] = (B[i] - tmp) / A[i][i];
    }
}

double scalar(double* x, double* y, int N)
{
    double sum = 0;
    for (int i = 0; i < N; i++)
        sum += x[i] * y[i];
    return sum;
}

double Inverse_Power_iteration(double** A, int N, double deviation)
{
    int flag, i, j;
    double lambda;

    double** A_copy;
    A_copy = new double*[N];
    for (i = 0; i < N; i++)
        A_copy[i] = new double[N];
    double* x_copy = new double[N];

    double* x = new double[N];
    double* y = new double[N];
    double* res = new double[N];

    for (i = 0; i < N; i++)
        x[i] = 1.0;

    do {
        flag = 0;

        for (i = 0; i < N; i++) {
            x_copy[i] = x[i];
            for (j = 0; j < N; j++)
                A_copy[i][j] = A[i][j];
        }

        Gaussian_elimination(A_copy, x_copy, y, N); //Ay=x

        lambda = scalar(x, y, N) / scalar(y, y, N);

        Multiply_Ax(A, x, res, N);
        for (i = 0; i < N; i++)
            if (abs(res[i] - lambda * x[i]) > deviation)
                flag = 1;

        normalize(y, N);

        for (i = 0; i < N; i++)
            x[i] = y[i];

    } while (flag > 0);

    return lambda;
}

void Richardson_method(double** A, double* B, double* x, int N, double deviation)
{
    int i, j;
    double max_lambda, min_lambda, tau;
    double* res = new double[N];

    max_lambda = Power_iteration(A, N, deviation);
    min_lambda = Inverse_Power_iteration(A, N, deviation);
    tau = 2 / (max_lambda + min_lambda);

    //Реализация алгоритма
    int flag;
    do {
        flag = 0;
        Multiply_Ax(A, x, res, N);
        for (i = 0; i < N; i++) {
            res[i] = tau * (res[i] - B[i]);
            x[i] -= res[i];
        }

        //Проверка невязки (B - A*x' = 0)
        for (i = 0; i < N; i++) {
            double tmp = B[i];
            for (j = 0; j < N; j++)
                tmp -= A[i][j] * x[j];

            if (abs(tmp) > deviation)
                flag = 1;
        }

    } while (flag > 0);
}

int main()
{
    setlocale(LC_ALL, "RUS");

    int i, j;
    int N;
    double deviation = 1e-5;
    double max_lambda, min_lambda;

    //Читаем из файла матрицу
    ifstream file("D:\\test3.txt");
    file >> N;
    double* B = new double[N];
    double* x = new double[N];
    double** A;
    A = new double*[N];

    for (i = 0; i < N; i++) {
        A[i] = new double[N];
        x[i] = 1; //Первое приближение
        for (j = 0; j < N; j++)
            file >> A[i][j];
    }

    for (i = 0; i < N; i++)
        file >> B[i];

    file.close();

    //Вывод до изменения
    for (i = 0; i < N; i++) {
        for (j = 0; j < N; j++)
            cout << A[i][j] << "\t";
        cout << " = " << B[i];
        cout << "; x[" << i << "] = " << x[i] << endl;
    }
    cout << endl
         << endl;

    //Gauss_Seidel_method(A, B, x, N, deviation);
    //Gradient_descent_method(A, B, x, N, deviation);
    Richardson_method(A, B, x, N, deviation);

    //Вывод после
    cout << endl
         << endl;
    for (i = 0; i < N; i++) {
        for (j = 0; j < N; j++)
            cout << A[i][j] << "\t";
        cout << " = " << B[i];
        cout << "; x[" << i << "] = " << x[i] << endl;
    }

    cout << "N = " << N << endl;
    cout << "max_lambda = " << Power_iteration(A, N, deviation) << endl;
    cout << "min_lambda = " << Inverse_Power_iteration(A, N, deviation) << endl;

    cout << endl;
    system("pause");
    return 0;
}