MSG

#include <iostream>
#include "Matrix.h"
using namespace std;

/*void Matrix_vector(Matrix *Matrix, Vector vector, Vector vector_out);
void TransposeMatrix_vector(Matrix *Matrix, Vector vector, Vector vector_out);
void InverseIL0_vector(Matrix *ILU0, Vector vector, Vector vector_out);
void InverseTransposeIU0_vector(Matrix *ILU0, Vector vector, Vector vector_out);
void InverseIU0_vector(Matrix *ILU0, Vector vector, Vector vector_out);
void InverseTransposeIL0_vector(Matrix *ILU0, Vector vector, Vector vector_out);*/

void Matrix_vector(Matrix *Matrix, Vector vector, Vector vector_out)
{
    int i, j, j0,
        N = Matrix->N,
        *ig = Matrix->ig,
        *jg = Matrix->jg;
    double *di = Matrix->di,
        *ggl = Matrix->ggl,
        *ggu = Matrix->ggu;

    for (i = 0; i < N; i++)
        vector_out[i] = di[i] * vector[i]; // += ? =

    for (i = 1; i < N; i++)
        for (j0 = ig[i]; j0 < ig[i+1]; j0++)
        {
            j = jg[j0];
            vector_out[i] += ggl[j0] * vector[j];
            vector_out[j] += ggu[j0] * vector[i];
        }
}

void TransposeMatrix_vector(Matrix *Matrix, Vector vector, Vector vector_out)
{
    int i, j, j0,
        N = Matrix->N,
        *ig = Matrix->ig,
        *jg = Matrix->jg;
    double *di = Matrix->di,
        *ggl = Matrix->ggl,
        *ggu = Matrix->ggu;

    for (i = 0; i < N; i++)
        vector_out[i] = di[i] * vector[i]; // += ? =

    for (i = 1; i < N; i++)
        for (j0 = ig[i]; j0 < ig[i+1]; j0++)
        {
            j = jg[j0];
            vector_out[i] += ggu[j0] * vector[j];
            vector_out[j] += ggl[j0] * vector[i];
        }
}

void InverseIL0_vector(Matrix *ILU0, Vector vector, Vector vector_out)
{
    int i, j, j0,
        N = ILU0->N,
        *ig = ILU0->ig,
        *jg = ILU0->jg;
    double sum = 0,
        *di = ILU0->di,
        *ggl = ILU0->ggl;

    for (i = 0; i < N; i++)             //
        vector_out[i] = vector[i];      //

    for (i = 0; i < N; i++)
    {
        for (j0 = ig[i]; j0 < ig[i+1]; j0++)
        {
            j = jg[j0];
            vector_out[i] -= ggl[j0] * vector_out[j];
        }
        vector_out[i] /= di[i];
    }
}

void InverseTransposeIU0_vector(Matrix *ILU0, Vector vector, Vector vector_out)
{
    int i, j, j0,
        N = ILU0->N,
        *ig = ILU0->ig,
        *jg = ILU0->jg;
    double sum = 0,
        *ggu = ILU0->ggu;

    for (i = 0; i < N; i++)             //
        vector_out[i] = vector[i];      //

    for (i = 0; i < N; i++)
        for (j0 = ig[i]; j0 < ig[i+1]; j0++)
        {
            j = jg[j0];
            vector_out[i] -= ggu[j0] * vector_out[j];
        }
}

void InverseIU0_vector(Matrix *ILU0, Vector vector, Vector vector_out)
{
    int i, j, j0,
        N = ILU0->N,
        *ig = ILU0->ig,
        *jg = ILU0->jg;
    double sum = 0,
        *ggu = ILU0->ggu;

    for (i = 0; i < N; i++)             //
        vector_out[i] = vector[i];      //

    for (i = N-1; i >= 0; i--)
        for (j0 = ig[i]; j0 < ig[i+1]; j0++)
        {
            j = jg[j0];
            vector_out[j] -= ggu[j0] * vector_out[i];
        }
}

void InverseTransposeIL0_vector(Matrix *ILU0, Vector vector, Vector vector_out)
{
    int i, j, j0,
        N = ILU0->N,
        *ig = ILU0->ig,
        *jg = ILU0->jg;
    double sum = 0,
        *di = ILU0->di,
        *ggl = ILU0->ggl;

    for (i = 0; i < N; i++)             //
        vector_out[i] = vector[i];      //

    for (i = N-1; i >= 0; i--)
    {
        vector_out[i] /= di[i];
        for (j0 = ig[i]; j0 < ig[i+1]; j0++)
        {
            j = jg[j0];
            vector_out[j] -= ggl[j0] * vector_out[i];
        }
    }
}

double Norm(Vector v, int n)
{
    double norm = 0;
    for (int i = 0; i < n; i++)
        norm += v[i] * v[i];
    return sqrt(norm);
}

void Sum(Vector a, Vector b, int n, Vector v_out)
{
    for (int i = 0; i < n; i++)
        v_out[i] = a[i] + b[i];
}

void Diff(Vector a, Vector b, int n, Vector v_out)
{
    for (int i = 0; i < n; i++)
        v_out[i] = a[i] - b[i];
}

void MultToConst(Vector v, double c, int n, Vector v_out)
{
    for (int i = 0; i < n; i++)
        v_out[i] = v[i] * c;
}

double Scal(Vector a, Vector b, int n)
{
    double scal = 0;
    for (int i = 0; i < n; i++)
        scal += a[i] * b[i];
    return scal;
}



Vector MSG(Matrix *A, Vector f, Vector x0, double Eps, int MaxIter)
{
    int N = A->N;
    double alpha, beta,
        normF = Norm(f, N);
    Vector x, p,
        r = new double[N],
        buf = new double[N];

    // x = x0;  r(0) = b - A*x(0);  p(0) = r(0)
    x = (Vector)memcpy(new double[N], x0, N*sizeof(double));
    Matrix_vector(A, x, buf);
    Diff(f, buf, N, r);
    p = (Vector)memcpy(new double[N], r, N*sizeof(double));

    for (int j = 0; j < MaxIter; j++)
    {
        Matrix_vector(A, p, buf);                   // alpha(k) = <r(k-1),r(k-1)> / <A*p(k-1), p(k-1)>
        alpha = Scal(r, r, N) / Scal(buf, p, N);
        MultToConst(p, alpha, N, buf);              // x(k) = x(k-1) + alpha(k)*p(k-1)
        Sum(x, buf, N, x);
        Matrix_vector(A, p, buf);                   // r(k) = r(k-1) - alpha(k)*A*p(k-1)
        MultToConst(buf, alpha, N, buf);
        Diff(r, buf, N, buf);
        swap(r, buf);
        if (Norm(r, N) / normF < Eps) break;        // ||r(k)|| / ||f|| < Eps ?
        beta = Scal(r, r, N) / Scal(buf, buf, N);   // beta(k) = <r(k), r(k)> / <r(k-1), r(k-1)>
        MultToConst(p, beta, N, buf);               // p(k) = r(k) + beta(k)*p(k-1)
        Sum(r, buf, N, p);
    }
    return x;
}

/*
    for (int i = 0; i < N; i++)
        cout << r[i] << " ";
    return 0;
    */

/*cout << "r = {";
        for (int i = 0; i < N; i++)
            cout << r[i] << ", ";
        cout << endl;
        cout << "p = {";
        for (int i = 0; i < N; i++)
            cout << p[i] << ", ";
        cout << endl;
        cout << "alpha = " << alpha << endl;
        cout << "beta = " << beta << endl;*/


int main()
{
    Matrix M;
    M.ReadFromFile("N.txt", "ig.txt", "jg.txt", "di.txt", "ggl.txt", "ggu.txt");

    Vector f = new double[5];
    Vector x0 = new double[5];
    for (int i = 0; i < 5; i++)
        x0[i] = 0;
    f[0] = 9;
    f[1] = 48;
    f[2] = 20;
    f[3] = 37;
    f[4] = -9;
    Vector v_out = new double[5];

    v_out = MSG(&M, f, x0, 0.001, 1000);

    //Matrix_vector(&M, f, v_out);                  // A * v
    //TransposeMatrix_vector(&M, v, v_out);         // A^T * v
    //InverseIL0_vector(&M, v, v_out);              // L^(-1) * v
    //InverseTransposeIU0_vector(&M, v, v_out);     // U^(-T) * v
    //InverseIU0_vector(&M, v, v_out);              // U^(-1) * v
    //InverseTransposeIL0_vector(&M, v, v_out);     // L^(-T) * v

    for (int i = 0; i < 5; i++)
        cout << v_out[i] << " ";

    getchar();
    return 0;
}