#include <cmath>
#include <iostream>

using namespace std;

int CheckIfDiagonalMatricesAreSameTask9(double **oldMatrix, double **newMatrix, int dimension, double epsilon)
{
    int result = 1;
    for (int row = 0; row < dimension; row++)
    {
        double diff = fabs(oldMatrix[row][row] - newMatrix[row][row]);
        if (diff > epsilon)
        {
            result = 0;
        }
    }
    return result;
}
int CheckIfLowerIsZeroTask9(double **matrix, int dimension, double epsilon)
{
    int result = 1;
    for (int i = 0; i < dimension; i++)
    {
        for (int j = 0; j < i; j++)
        {
            double value = matrix[i][j];
            if (value > epsilon)
            {
                return 0;
            }
        }
    }
    return result;
}
double norma(double * wektor, int rozmiar) {
    int i;
    double A, suma = 0;


    for (i = 0; i < rozmiar; i++) {
        suma += wektor[i] * wektor[i];
    }


    return sqrt(suma);
}
void OdejmowanieSkalaraOdWektora(double * wektor, double R, int rozmiar, double * y) {
    int i;


    for (i = 0; i < rozmiar; i++) {
        y[i] -= R * wektor[i];
    }

}

void DzieleniePrzezSkalar(double * wektor, double R, int rozmiar, double * y) {
    int i;



    for (i = 0; i < rozmiar; i++) {
        y[i] = wektor[i] / R;
    }

}

double dot_product(double * wektor, double * y, int rozmiar) {
    int i;
    double suma = 0;


    for (i = 0; i < rozmiar; i++) {
        suma += wektor[i] * y[i];
    }


    return suma;
}

void MnozenieMacierzy(double **matrix1, double **matrix2, double **wynik, int n)
{
    for (int wiersz = 0; wiersz < n; wiersz++)
    {
        for (int kolumna = 0; kolumna < n; kolumna++)
        {
            double value = 0;
            for (int i = 0; i < n; i++)
            {
                value += matrix1[wiersz][i] * matrix2[i][kolumna];
            }
            wynik[wiersz][kolumna] = value;
        }
    }
}

void WypelnianieZerami(double **matrix, int rozmiar)
{
    for (int i = 0; i < rozmiar; i++)
    {
        for (int j = 0; j < rozmiar; j++)
        {
            matrix[i][j] = 0;
        }
    }
}

void MetodaGramaSchmidta(double ** A, double ** R, int n) {
    int i, j;
    double anorm, tol = 10e-7;

    for (i = 0; i < n; i++) {
        R[i][i] = norma(A[i], n);

        if (R[i][i] > tol) {
            DzieleniePrzezSkalar(A[i], R[i][i], n, A[i]);
        }
        else if (i == 0) {
            A[i][0] = 1;
            for (j = 1; j < n; j++) {
                A[i][j] = 0;
            }
        }
        else {
            for (j = 0; j < n; j++) {
                A[i][j] = -A[0][i] * A[0][j];
            }
            A[i][i] += 1;

            for (j = 1; j < i; j++) {
                OdejmowanieSkalaraOdWektora(A[j], A[j][i], n, A[i]);
            }

            anorm = norma(A[i], n);
            DzieleniePrzezSkalar(A[i], anorm, n, A[i]);
        }

        for (j = i + 1; j < n; j++) {
            R[j][i] = dot_product(A[i], A[j], n);
            OdejmowanieSkalaraOdWektora(A[i], R[j][i], n, A[j]);
        }
    }


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

        for (j = 1; j < i; j++) {
            OdejmowanieSkalaraOdWektora(A[j], A[j][i], n, A[i]);
        }

        anorm = norma(A[i], n);
        DzieleniePrzezSkalar(A[i], anorm, n, A[i]);
    }

}


int main() {
    int i, j, n;
    double wektor;
    int rozmiar = 3;
    n = rozmiar;

    double ** A = new double*[n];
    double ** Aold = new double*[n];
    double ** R = new double*[n];
    double ** Q = new double*[n];
    double ** Qtransposed = new double*[n];
    double ** wynik = new double*[n];
    for (i = 0; i < n; i++) {
        A[i] = new double[n];
        Aold[i]= new double[n];
        R[i] = new double[n];
        Q[i] = new double[n];
        Qtransposed[i] = new double[n];
        wynik[i] = new double[n];
    }
    WypelnianieZerami(Q, n);
    WypelnianieZerami(Qtransposed, n);
    WypelnianieZerami(R, n);
    WypelnianieZerami(wynik, n);

    //A[0][0] = 1;
    //A[0][1] = 2;
    //A[0][2] = 3;
    //A[0][3] = 5;
    //A[1][0] = 2;
    //A[1][1] = 3;
    //A[1][2] = 4;
    //A[1][3] = 5;
    //A[2][0] = 3;
    //A[2][1] = 4;
    //A[2][2] = 5;
    //A[2][3] = 6;
    //A[3][0] = 5;
    //A[3][1] = 5;
    //A[3][2] = 6;
    //A[3][3] = 8;

    A[0][0] = 8;
    A[0][1] = -2;
    A[0][2] = -2;

    A[1][0] = -2;
    A[1][1] = 4;
    A[1][2] = -2;

    A[2][0] = -2;
    A[2][1] = -2;
    A[2][2] = 13;


    cout << "A = " << endl;
    for (i = 0; i < n; i++) {
        for (j = 0; j < n; j++) {
            cout << A[j][i] << "\t";
        }
        cout << endl;
    }
    cout << endl;
    //////tu mam problem z tą pętlą jak ją zbudować
    do{
        cout << "A" << endl;
        for (i = 0; i < n; i++) {
            for (j = 0; j < n; j++) {
                cout << A[i][j] << "\t";
            }
            cout << endl;
        }
        cout << endl;
        for (i = 0; i < n; i++) {
            for (j = 0; j < n; j++) {
                Aold[i][j] = A[i][j];
            }
        }

        MetodaGramaSchmidta(A, R, n);


        for (i = 0; i < n; i++) {
            for (j = 0; j < n; j++) {
                Q[i][j] = A[i][j];
            }
        }
        for (i = 0; i < n; i++) {
            for (j = 0; j < n; j++) {
                Qtransposed[i][j] = A[j][i];
            }
        }
        MnozenieMacierzy(A, R, Qtransposed, n);
        MnozenieMacierzy(R, Qtransposed, A, n);

    cout << "Q = " << endl;
    for (i = 0; i < n; i++) {
        for (j = 0; j < n; j++) {
            cout << Qtransposed[i][j] << "\t";
        }
        cout << endl;
    }
    cout << endl;


    cout << "R = " << endl;
    for (i = 0; i < n; i++) {
        for (j = 0; j < n; j++) {
            cout << R[j][i] << "\t";
        }
        cout << endl;
    }
    cout << endl;

    cout << "RQ" << endl;
    for (i = 0; i < n; i++) {
        for (j = 0; j < n; j++) {
            cout << A[i][j] << "\t";
        }
        cout << endl;
    }
    cout << endl;

    } while (!(CheckIfDiagonalMatricesAreSameTask9(Aold, A, n, DBL_EPSILON) == 1 &&
            CheckIfLowerIsZeroTask9(A, n, DBL_EPSILON)));


    cout << "Q = " << endl;
    for (i = 0; i < n; i++) {
        for (j = 0; j < n; j++) {
            cout << Q[i][j] << "\t";
        }
        cout << endl;
    }
    cout << endl;


    cout << "R = " << endl;
    for (i = 0; i < n; i++) {
        for (j = 0; j < n; j++) {
            cout << R[j][i] << "\t";
        }
        cout << endl;
    }
    cout << endl;

    cout << "A =  QR" << endl;
    for (i = 0; i < n; i++) {
        for (j = 0; j < n; j++) {
            cout << wynik[i][j] << "\t";
        }
        cout << endl;
    }
    cout << endl;
    for (i = 0; i < n; i++) {
        delete[] A[i];
        delete[] R[i];
        delete[] Q[i];
        delete[] wynik[i];
    }

    delete[] A;
    delete[] R;
    delete[] Q;
    delete[] wynik;
    //system("pause");
    cin.get();
    return 0;
}