#include <iostream>
#include <fstream>
#include <iomanip>
#include <vector>
#include <cmath>
#include <algorithm>
#include <stdlib.h>

using namespace std;
bool WczytajPlik(double ** &A, double * &b, unsigned *matrix_size);
void WyswietlMenu();
void WyswietlMacierz(double ** A, double * b, unsigned matrix_size);
void KopiowanieTablicy(double ** &A, double * &b, double ** &C, double * &d, unsigned matrix_size);
void TranspozycjaMacierzy(double ** w, double ** wT, unsigned matrix_size);
void WyswietlMacierzBezWartosci(double ** A, unsigned matrix_size);
double CoefficientB(double ** &A, vector<double> &w, unsigned matrix_size, int step);
void WektorU(double ** &A, double * &b, unsigned matrix_size);
double Sign(double value);
vector<double> Aprim(double ** &A, vector<double> &w, unsigned matrix_size,int col, int step);
vector<double> Bprim(double * &b, vector<double> &w, unsigned matrix_size, int step);
double Alfa(double ** &A, vector<double> &w, unsigned matrix_size, int step, int col);
double CoefficientB(double ** &A, vector<double> &w, unsigned matrix_size, int step);

void NewB(double * &b, vector<double> &newB, unsigned matrix_size, int step);
void NewMatrix(double ** &A, vector<vector<double> > &MatrixPrim, unsigned matrix_size, int step);

int main()
{
    double ** A;
    double * b;
    double ** C;
    double * d;
    int wybor;
    unsigned matrix_size;
    bool czyWczytany = false;
    for(;;)
    {
        WyswietlMenu();
        cout<<"Wybor => "; cin>>wybor;
        system("clear");
        switch(wybor)
        {
            case 1:
                if(WczytajPlik(C, d, &matrix_size)) {
                    czyWczytany = true;
                    cout<<"Plik zostal wczytany.\n";
                    WyswietlMacierz(C, d, matrix_size);
                    KopiowanieTablicy(A, b, C, d, matrix_size);
                }
                else
                    cout<<"Nie udalo sie wczytac pliku\n";
            break;
            case 2:
                if(czyWczytany) {
                    WektorU(A, b, matrix_size);
                    }
                else
                    cout<<"Nalezy wczytac plik przed rozpoczeciem programu";
            break;
            case 3:
                return 0;
            break;
            default:
                cout<<"Nalezy wybrac opcje z zakresu [1-3]\n";
            break;
        }
    }

    return 0;
}

void WektorU(double ** &A, double * &b, unsigned matrix_size)
{
    vector<double> w;
    vector<double> newB;
    vector<vector<double> > MatrixPrim;
    vector<double> aprim;
    double sum = 0;
    int iteratorW = 0;
    cout<<"\n\n\n----POCZATEK PROGRAMU----\n\n\n";
    for(int step=0; step<matrix_size; step++)
    {
        iteratorW = 0;
        cout<<"\nKrok nr"<<step+1<<endl;
        for(int row=step; row<matrix_size; row++)
        {
                if(step == row) {
                    for(int el=step; el<matrix_size; el++)
                    {
                        sum+= pow(A[el][step], 2);
                        // cout<<"A[el][col]="<<A[el][col]<<endl;
                        // cout<<"Suma = "<<sum<<endl;
                    }
                    // cout<<"A[col][col] + Sign(A[col][col])*sqrt(sum))"<<A[step][step]<<"+"<<Sign(A[step][step])<<"*"<<sqrt(sum)<<endl;
                    w.push_back(A[step][step] + Sign(A[step][step])*sqrt(sum));
                    cout<<"\nW["<<iteratorW<<"] = "<<w[iteratorW]<<endl;
                    iteratorW++;
                }
                else
                {
                    w.push_back(A[row][step]);
                    cout<<"W["<<iteratorW<<"] = "<<w[iteratorW]<<endl;
                    iteratorW++;
                }
        }
        for(int col=step; col<matrix_size; col++)
        {
            cout<<"\n\n!-!-!------------------!-!-!\n";
            sum = 0;


            cout<<endl;
            aprim = Aprim(A, w, matrix_size, col, step);
            MatrixPrim.push_back(aprim);
            aprim.clear();
        }
        cout<<endl;
        cout<<endl;

        newB = Bprim(b, w, matrix_size, step);
        NewMatrix(A, MatrixPrim, matrix_size, step);
        MatrixPrim.clear();
        NewB(b, newB, matrix_size, step);
        newB.clear();
        WyswietlMacierz(A, b, matrix_size);
        w.clear();

    }
}

void NewMatrix(double ** &A, vector<vector<double> > &MatrixPrim, unsigned matrix_size, int step)
{
    const int newStep = step;
    for(int i=newStep, iV=0; i<matrix_size && iV<matrix_size; i++, iV++)
        for(int j=newStep, jV=0; j<matrix_size && jV<matrix_size; j++, jV++)
            A[i][j] = MatrixPrim[jV][iV];
}

void NewB(double * &b, vector<double> &newB, unsigned matrix_size, int step)
{
    const int newStep = step;
    for(int i=newStep, iV=0; i<matrix_size && iV<matrix_size; i++, iV++)
        b[i] = newB[iV];
}

vector<double> Aprim(double ** &A, vector<double> &w, unsigned matrix_size, int col, int step)
{
    double B = CoefficientB(A, w, matrix_size, step);
    int iterationW = 0;
    vector<double> newA;
    double alfa = Alfa(A, w, matrix_size, step, col);
    cout<<"alfa"<<col+1<<" = "<<alfa<<endl;
    cout<<"B = "<<B<<endl;
    for(int i=step; i<matrix_size; i++)
    {
        cout<<endl<<"------------------"<<endl;
        cout<<"A["<<i<<"]["<<col<<"] = "<<A[i][col]<<setw(10)<<" "<<"(2*alfa/ B)*w[iterationW]) = "<<(2*alfa/ B)*w[iterationW]<<endl;
        cout<<"------------------"<<endl;
        newA.push_back(A[i][col] - (2*alfa/B)*w[iterationW]);
        cout<<"A"<<i+1<<"' = "<<newA[i]<<setw(5)<<" ";
        iterationW++;
    }
    return newA;
}

vector<double> Bprim(double * &b, vector<double> &w, unsigned matrix_size, int step)
{
    vector<double> newB;
    double B = 24;
    double alfa = 0;
    int iterationW = 0;
    for(int i=step; i<matrix_size; i++) {
        alfa += b[i] * w[iterationW];
        iterationW++;

    }
    cout<<"alfa b = "<< alfa<<endl;
    iterationW = 0;
    for(int i=step; i<matrix_size; i++) {
        newB.push_back(b[i] - (2*alfa/B)*w[iterationW]);
        cout<<"\nb[i] - (2*alfa/B)*w[iterationW] == "<<b[i]<<" - "<<(2*alfa/B)<<" * "<<w[iterationW]<<endl;
        cout<<"b"<<i+1<<"' = "<<newB[i]<<setw(5)<<" ";
        iterationW++;
    }
    return newB;
}

double CoefficientB(double ** &A, vector<double> &w, unsigned matrix_size, int step)
{
    vector<double> alfa;
    vector<double> Aprim;
    double B=0;
    for(int i=0; i<matrix_size-step; i++)
        B += pow(w[i], 2);

    return B;
}

double Alfa(double ** &A, vector<double> &w, unsigned matrix_size, int step, int col)
{
    double sum = 0;
    int iteratorW = 0;
    for(int i=step; i<matrix_size; i++) {
        sum += A[i][col] * w[iteratorW];
        iteratorW++;
    }

    return sum;
}

double MultiplyVectors(vector<double> &w1, vector<double> &w2, unsigned matrix_size)
{
    double sum = 0;
    for(int i=0; i<matrix_size; i++)
            sum += w1[i] * w2[i];
    return sum;
}

double Sign(double value) {
    if(value < 0)
        return -1;
    else if(value > 0)
        return 1;
    else
        return 0;
}

void WyswietlMenu()
{
    cout<<"\n----MENU----\n";
    cout<<"1. Wczytanie danych z pliku.\n";
    cout<<"2. Householder.\n";
    cout<<"3. Wyjscie z programu.\n";
    cout<<"\n";
}

bool WczytajPlik(double ** &A, double * &b, unsigned *matrix_size)
{
    ifstream source_file("test.csv");
    if (!source_file.is_open())
    {
        cout <<"The file has not been open!"<<endl;
        return false;
    }
    source_file >> *matrix_size;
    A = new double*[*matrix_size];
    A[0] = new double[*matrix_size**matrix_size];
    for(unsigned i = 1; i< *matrix_size; i++)
        A[i] = A[i-1] + *matrix_size;
    b = new double[*matrix_size];
    char semicolumn;
    for (unsigned i = 0; i < *matrix_size+1; i++)
        source_file >> semicolumn;

    for (unsigned i = 0; i < *matrix_size; i++)
    {
        for (unsigned j = 0; j < *matrix_size; j++)
        {
            source_file >> A[i][j];
            source_file >> semicolumn;
        }
        source_file >> semicolumn;
        source_file >> b[i];
    }
    source_file.close();
    return true;
}

void WyswietlMacierz(double ** A, double * b, unsigned matrix_size)
{
    cout<<endl;
    for(int i=0; i<matrix_size; i++) {
        for(int j=0; j<matrix_size; j++) {
            cout<<setw(10)<<A[i][j]<<" ";
        }
        cout<<"|"<<setw(10)<<b[i];
        cout<<"\n";
    }
    cout<<endl;

}
void WyswietlMacierzBezWartosci(double ** A, unsigned matrix_size)
{
    cout<<endl;
    for(int i=0; i<matrix_size; i++) {
        for(int j=0; j<matrix_size; j++) {
            cout<<setw(10)<<A[i][j]<<" ";
        }
        cout<<"\n";
    }
    cout<<endl;

}

void KopiowanieTablicy(double ** &A, double * &b, double ** &C, double * &d, unsigned matrix_size)
{
    b = new double[matrix_size];
    A = new double*[matrix_size];
    for(int i=0; i<matrix_size; i++)
        A[i] = new double[matrix_size];
    for(int i=0; i<matrix_size; i++)
        for(int j=0; j<matrix_size; j++)
            A[i][j] = C[i][j];
    for(int i=0; i<matrix_size; i++)
        b[i] = d[i];
}

void TranspozycjaMacierzy(double ** w, double ** wT, unsigned matrix_size)
{
    for(int i=0; i<matrix_size; i++)
        for(int j=0; j<matrix_size; j++)
            wT[i][j] = w[j][i];

    WyswietlMacierzBezWartosci(wT, matrix_size);
}