#include <iostream>
#include <iomanip>
#include <vector>
#include <cmath>
#include <algorithm>

using namespace std;

#define EPSILON 10e-6

typedef vector<double> Vec;
typedef vector< Vec > VecMatrix;

double alpha(const VecMatrix &A, const Vec &N, const int j, const int n)
{
    double sum = 0;
    for (int i = 0; i < n; i++)
    {
        sum += A[i][j] * N[i];
    }
    return sum;
}

double beta(const Vec &N, const int n)
{
    double sum = 0;
    for (int i = 0; i < n; i++)
    {
        sum += N[i] * N[i];
    }
    sum *= 0.5;
    return sum;
}

int sign(double arg)
{
    if (arg > 0) return 1;
    else if (arg < 0) return -1;
    else return 0;
}

void printMatrix(const VecMatrix &mat, const int N)
{
    cout << endl;
    for (int i = 0; i < N; i++)
    {
        for (int j = 0; j < N; j++)
        {
            cout.width(8);
            cout << mat[i][j];
        }
        cout << "   |";
        cout.width(6);
        cout << mat[i][N] << endl;
    }
    cout << endl;
}


void householder(VecMatrix &mat, const int n)
{
    for (int k = 0; k < n - 1; k++)
    {
        Vec N(n);

        for (int i = 0; i < n; i++)
            N[i] = mat[i][k];


        double A = 0;
        for (int i = 0; i < n; i++)
            A += mat[i][k] * mat[i][k];
        A = sqrt(A);

        N[k] += sign(mat[k][k]) * A;

        double b = beta(N, n);

        cout << "A: " << A << endl;
        cout << "Beta: " << b << endl;

        for (int j = k; j < n + 1; j++)
        {
            double a = alpha(mat, N, j, n);
            double ab = a / b;

            if (fabs(ab) < EPSILON)
                ab = 0;

            cout << "Alpha" << j << ": " << a << endl;

            for (int i = k; i < n; i++)
            {
                mat[i][j] -= ab * N[i];

                if (fabs(mat[i][j]) < EPSILON)
                    mat[i][j] = 0;
            }
        }

        printMatrix(mat, n);
    }
}

bool processSolutions(const VecMatrix &mat, Vec &X, const int N)
{
    for (int i = N - 1; i >= 0; i--)
    {
        if (mat[i][i] == 0)
            return false;

        X[i] = mat[i][N];

        for (int j = i + 1; j < N; j++)
            X[i] -= mat[i][j] * X[j];

        X[i] /= mat[i][i];

        if (fabs(X[i]) < EPSILON)
            X[i] = 0;
    }

    return true;
}

int main()
{
    cout << setprecision(3);

    const int N = 4;
    VecMatrix matrix(N, Vec(N + 1));
    Vec solutions(N);


    matrix[0][0] = 1;
    matrix[0][1] = 2;
    matrix[0][2] = -1;
    matrix[0][3] = 2;
    matrix[0][4] = 3;

    matrix[1][0] = 2;
    matrix[1][1] = -1;
    matrix[1][2] = 3;
    matrix[1][3] = -2;
    matrix[1][4] = 0;

    matrix[2][0] = 0;
    matrix[2][1] = 1;
    matrix[2][2] = 2;
    matrix[2][3] = 1;
    matrix[2][4] = 1;

    matrix[3][0] = -2;
    matrix[3][1] = 3;
    matrix[3][2] = 1;
    matrix[3][3] = 0;
    matrix[3][4] = 2;

    householder(matrix, N);
    processSolutions(matrix, solutions, N);

    cout << endl;
    for (int i = 0; i < N; i++)
    {
        cout << "x" << i + 1 << " = " << solutions[i] << endl;
    }

    cin.get();

    return 0;
}