gauss-seidel, jacobi?

#include <iostream>
#include <vector>
#include <stdlib.h>
#include <time.h>
using namespace std;

double norma(vector<double> a, vector<double> b)
{
    double joachim=0;
    for(int i=0; i<a.size(); i++)
    {
        if(joachim<abs(a[i]-b[i]))
            joachim = abs(a[i]-b[i]);
    }
    return joachim;
}

int main()
{
    int t = 5;
    srand(time(NULL));
    int N = 1000;
    double epsilon = 1;
    int iter = 100000;
    double e=1e-20;
    cout.precision(26);
    cout <<"\n";
    for(int i= 0; i<9; i++)
    {
        cout << epsilon << " ";
        epsilon = (double)(epsilon/10.0);
    }
    epsilon = 1;

    vector<vector<double> > macierzA;
    vector<double> b;
    vector<double> x;
    for(int iterator=0; iterator<t; iterator++)
    {
        //cin >> N;

        vector<vector<double> > macierzM;
        vector<vector<double> > macierzL;
        vector<vector<double> > macierzU;
        vector<vector<double> > macierzD;
        vector<vector<double> > macierzN;
        ////macierz L - od poczatku bez przekatnej
        ////macierz D - przekatna
        ////macierz U - od przekatnej do konca


        vector<double> wczesniej;

        vector<double> x1, x2, b_gs;

        for(int i=0; i<N; i++)
        {
            if(iterator==0)
            {
                x.push_back((double)(rand() / RAND_MAX));
                x[i]+=((double)(rand()%9999));
                //x[i]-=((double)(rand()%999));
            }
            //to przyblizenie poczatkowe moze byc inne
            //podobno zwykle daje sie wektor zer.
            //przyblizenie.push_back(0);
            if(iterator==0)
            {
                x1.push_back(0);
            }
            else if(iterator==1)
            {
                x1.push_back((double)x[i]+0.999);
            }
            else if(iterator==2)
            {
                x1.push_back(double(x[i]+49));
            }
            else if(iterator==3)
            {
                x1.push_back(double(x[i]+9999));
            }
            else if(iterator==4)
            {
                x1.push_back(double(x[i]+iter));
            }
            x2.push_back(0);
            wczesniej.push_back(0);
            b_gs.push_back(0);
        }

        for(int i=0; i<N; i++)
        {
            vector<double> usagi;
            vector<double> marchewka;
            double btemp=0;
            for(int j=0; j<N; j++)
            {
                //chyba nie ma szans, zeby wyskoczyly tutaj jakies zera na glownej przekatnej.
                //dodam coś, derp. nigdy nie wiadomo z tymi randomami.
                double sailor_moon = (rand()%19) - (rand()/RAND_MAX);
                if(i==j)
                {
                    sailor_moon+= (rand()%99900)*N;
                    //sailor_moon+=(rand()%79);
                    //diagonalnie dominujaca
                }
                usagi.push_back(sailor_moon);
                marchewka.push_back(0);
                btemp = btemp+sailor_moon * x[j];
            }


            if(iterator==0)
            {
                b.push_back(btemp);
                macierzA.push_back(usagi);
            }
            macierzM.push_back(marchewka);
            macierzD.push_back(marchewka);
            macierzU.push_back(marchewka);
            macierzL.push_back(marchewka);
            macierzN.push_back(marchewka);
        }

        //a moze dane z klawiatury? a może frytki do tego?
        //for(int i=0; i<N; i++)
        //{
        //  vector<double> usagi;
        //  for(int j=0; j<N+1; j++)
        //  {
        //      double sailor_moon;
        //      cin >> sailor_moon;
        //      usagi.push_back(sailor_moon);
        //  }
        //  macierz.push_back(usagi);
        //}

        //jacobi
        ////macierz L - od poczatku bez przekatnej
        ////macierz D - przekatna
        ////macierz U - od przekatnej do konca
        for(int i=0; i<N; i++)
        {
            for(int j=0; j<i; j++)
                macierzL[i][j]=macierzA[i][j];
        }

        for(int i=0; i<N; i++)
            macierzD[i][i]=macierzA[i][i];

        for(int i=0; i<N; i++)
        {
            for(int j=i+1; j<N; j++)
                macierzU[i][j]=macierzA[i][j];
        }

        // N = D^(-1)
        for(int i=0; i<N; i++)
            macierzN[i][i] = (1/macierzD[i][i]);

        for (int i=0; i<N; i++)
        {
            for (int j=0; j<N; j++)
            {
                if (i == j)
                    macierzM[i][j] = 0;
                else
                    macierzM[i][j] = - (macierzA[i][j] * macierzN[i][i]);
            }
        }
        //printf("Wynik jacobi\n");
        cout << "\njacobi\n";
        //tu wlasciwe obliczanie
        double blad=0;
        int jacobi=0; //k to iteracje ktorych szukamy, albo droidy


        for (jacobi=0; jacobi<iter; jacobi++) {

            for(int i=0; i<N; i++)
                wczesniej[i]=x1[i];

            for (int i=0; i<N; i++)
            {
                x2[i] = macierzN[i][i]*b[i];
                for (int j=0; j<N; j++)
                    x2[i] += macierzM[i][j]*x1[j];
            }
            for (int i=0; i<N; i++)
                x1[i] = x2[i];

            blad=norma(x1, wczesniej);
            //cout << "\n" << jacobi << "\n";
            if(blad<epsilon)
            {
            //cout << blad << "\n";
                while(blad<epsilon)
                {
                    //cout <<"e= " << epsilon << " iteracje = " << jacobi << "\n";
                    cout << jacobi << " ";
                    epsilon=epsilon/10.0;
                    if(epsilon*10 < e)
                        break;
                }

            if(epsilon*10 < e)
                break;
            }
        }

        //for(int i = 0; i < N; i++)
        //{
        //  //printf("x[%d] = %f\n", (i + 1), x1[i]) ;
        //  //printf("blad wzgledny x[%d] = %f\n", (i + 1), x2[i]) ;
        //  cout << "blad wzgledny x[" << i+1 << "] = " << x2[i] << "\n";
        //}
        //cout << "blad sredni wzgledny = " << blad_sredni_j << "\n";
        //cout << "iteracje = " << jacobi << "\n";

        //to samo ale gauss-seidel czy jak tam.
        // Calculate D^-1 * b
        for (int i=0; i<N; i++)
            b_gs[i] = b[i] * macierzN[i][i];

        //Calculate D^-1 * L
        for (int i=0; i<N; i++)
            for (int j=0; j<i; j++)
                macierzL[i][j] *= macierzN[i][i];

        //Calculate D^-1 * U
        for (int i=0; i<N; i++)
            for (int j=i+1; j<N; j++)
                macierzU[i][j] *= macierzN[i][i];
        //tu tracimy rozwiazania z jacobiego
        //for(int i=0; i<N; i++)
        //  x1[i]=0;
        for(int i=0; i<N; i++)
        {
        if(iterator==0)
            {
                x1[i]=(0);
            }
            else if(iterator==1)
            {
                x1[i]=((double)x[i]+0.999);
            }
            else if(iterator==2)
            {
                x1[i]=(double(x[i]+49));
            }
            else if(iterator==3)
            {
                x1[i]=(double(x[i]+9999));
            }
            else if(iterator==4)
            {
                x1[i]=(double(x[i]+iter));
            }
        }
        int gauss_s =0;
        //printf("Wynik gs\n");
        cout << "\nG-S\n";
        epsilon=1.0;
        for (gauss_s=0;gauss_s<iter; gauss_s++)
        {
            for(int i=0; i<N; i++)
                wczesniej[i]=x1[i];

            for (int i=0; i<N; i++)
            {
                x1[i] = b_gs[i];                       // x = D^-1*b -
                for (int j=0; j<i; j++)
                    x1[i] -= macierzL[i][j]*x1[j];    // D^-1*L * x -
                for (int j=i+1; j<N; j++)
                    x1[i] -= macierzU[i][j]*x1[j];    // D^-1*U * x
            }

            blad=norma(x1, wczesniej);
            if(blad<epsilon)
            {
                //cout << blad << "\n";

                while(blad<epsilon)
                {
                    //cout <<"e= " << epsilon << " iteracje = " << gauss_s << "\n";
                    cout << gauss_s << " ";
                    epsilon=epsilon/10.0;
                    if(epsilon*10 < e)
                        break;
                }

                if(epsilon*10 < e)
                    break;
            }
        }

        epsilon=1;
        cout << "\n\n";
    }

    return 0;
}