//This source code was created by Kostas Theocharis (kostastheoSam@gmail.com) and Tasos Papadopoulos (tasospap1995@gmail.com) for the pusrposes of the module "Computational Techniques and Solution Algorithms" of the "Computational Mechanics" Postgraduate Program
//Ιn case you have questions or corrections regarding the code, contact one of the authors at the emails found above

#include <iostream>
#include <cmath>
#include <fstream>

using namespace std;

void msip5(double** A, double psi, int N, int jmax);
void msip9(double** LU, double** A,double psi, int N, int jmax);
void rhs_constructor(double* rhs, double** A, double* b, double* u, int N, int jmax, int stencil);

int main()
{

//-----------Variable declaration and user inputs----------
    int solver,imax,jmax,N,maxiter,stencil,i,j,k,write,iter,selection,extra_iter;
    double psi,tol,omega,dx,dy,res_init,res,err,err_init;
    cout<<"This program solves the Poisson Equation u_xx+u_yy=4 , 0<=x<=1 , 0<=y<=1, with Dirichlet boundary conditions on all boundaries, using the ILU decomposition algorithms MSIP-5,MSIP-9."<<endl;
    cout<<"The analytical solution is u(x,y) = x^2 + y^2."<<endl<<"The initial guess is 0."<<endl<<"Only the stencil diagonal bands of the matrix A are stored."<<endl;
    cout<<"This program creates by default a file named Convergence.txt with three columns. The number of iteration is stored in the first column. The relative residual and relative error norms are stored in the second and third column respectively."<<endl;
    cout<<"Three more files are created by dafault. These are the x-coordinates (X.txt) of each node, the y-coordinates (Y.txt) of each node and the respective value of the solution u of the system (U.txt)."<<endl;
    cout<<"The user can choose to save in .txt files the matrices A and LU. The matrix LU has the same number of columns as the matrix A, as the diagonal elements of the matrix U are all 1."<<endl;
    cout<<endl<<endl;
    cout<<"Enter the number of intervals in the x-direction: ";
    cin>>imax; 
    cout<<"Enter the number of intervals in the y-direction: ";
    cin>>jmax;
    cout<<"Select the solver:"<<endl<<"1. MSIP-5"<<endl<<"2. MSIP-9"<<endl<<"Your selection: ";
    cin>>solver;
    cout<<"Enter the damping coefficient (0<=psi<1): ";
    cin>>psi;
    if (psi<0)
        psi=0;
    if (psi>=1)
        psi = 0.9; 
    cout<<"Enter the relaxation factor: ";
    cin>>omega;
    if (omega<=0)
        omega = 1;
    if (omega>2)
        omega = 1;
    cout<<"Enter the desired relative residual tolerance in exponential form: ";
    cin>>tol;
    cout<<"Enter the maximum number of iterations: ";
    cin>>maxiter;
    cout<<"Do you want to write the matrices on a .txt file?"<<endl<<"0. No"<<endl<<"1. Yes"<<endl<<"Your selection: ";
    cin>>write;

    imax++;             //number of nodes in the x-direction = number of intervals in the x-direction +1
    jmax++;             //number of nodes in the y-direction = number of intervals in the y-direction +1
    N = imax*jmax;      //total number of nodes 
    dx = (double)1/(imax-1);    //length of the interval in the x-direction
    dy = (double)1/(jmax-1);    //length of the interval in the y-direction

    if (solver==1)
        stencil = 5;
    else
        stencil = 9;
        

//----------Dynamic memory allocation----------
    double **A;
    double **LU;
    double *b;
    double *du;
    double *u_exact;
    double *u;
    double *w;
    double *rhs;

    b = new double [N];
    du = new double [N];
    u_exact = new double [N];
    u = new double [N];
    w = new double [N];
    rhs = new double [N];
    A = new double* [N];
    for (i=0;i<N;i++)
        A[i] = new double [stencil];
    LU = new double* [N];
    for (i=0;i<N;i++)
        LU[i] = new double [stencil];    

//----------Construction of system matrices and vectors----------         
    //Initiation of matrices 
    for (i=0;i<N;i++)
    {
        for (j=0;j<stencil;j++)
        {
            A[i][j] = 0;
        }
        b[i] = 0;
        u_exact[i] = 0;    
    }
    
    //Construction of matrix A and vector b - CHANGE IT IF YOU WANT TO SOLVE A DIFFERENT PROBLEM
        switch (stencil)
        {
        case 5:
            //boundary conditions , west and east boundaries 
            for (i=0;i<imax;i++)
            {
                j=0;
                k=i*jmax+j;
                A[k][2] = 1.0;
                b[k] = pow(i*dx,2)+pow(j*dy,2);
                u_exact[k] = pow(i*dx,2)+pow(j*dy,2);       //exact solution
                j=jmax-1;
                k=i*jmax+j;
                A[k][2] = 1.0;
                b[k] = pow(i*dx,2)+pow(j*dy,2);
                u_exact[k] = pow(i*dx,2)+pow(j*dy,2);
            }
            //boundary conditions , north and south boundaries
            for (j=1;j<jmax-1;j++)
            {
                i=0;
                k=i*jmax+j;
                A[k][2] = 1.0;
                b[k] = pow(i*dx,2)+pow(j*dy,2);
                u_exact[k] = pow(i*dx,2)+pow(j*dy,2);
                i=imax-1;
                k=i*jmax+j;
                A[k][2] = 1.0;
                b[k] = pow(i*dx,2)+pow(j*dy,2);
                u_exact[k] = pow(i*dx,2)+pow(j*dy,2);
            }
            //internal nodes
            for (i=1;i<imax-1;i++)
            {
                for (j=1;j<jmax-1;j++)
                {
                    k = i*jmax+j;
                    A[k][0] = 1/pow(dx,2);
                    A[k][1] = 1/pow(dy,2);
                    A[k][2] = -2/pow(dx,2)-2/pow(dy,2);
                    A[k][3] = 1/pow(dy,2);
                    A[k][4] = 1/pow(dx,2);
                    b[k] = 4;
                    u_exact[k] = pow(i*dx,2)+pow(j*dy,2);
                }
            }
            break;
        
        default:
            //boundary conditions , west and east boundaries 
            for (i=0;i<imax;i++)
            {
                j=0;
                k=i*jmax+j;
                A[k][4] = 1.0;
                b[k] = pow(i*dx,2)+pow(j*dy,2);
                u_exact[k] = pow(i*dx,2)+pow(j*dy,2);       //exact solution
                j=jmax-1;
                k=i*jmax+j;
                A[k][4] = 1.0;
                b[k] = pow(i*dx,2)+pow(j*dy,2);
                u_exact[k] = pow(i*dx,2)+pow(j*dy,2);
            }
            //boundary conditions , north and south boundaries
            for (j=1;j<jmax-1;j++)
            {
                i=0;
                k=i*jmax+j;
                A[k][4] = 1.0;
                b[k] = pow(i*dx,2)+pow(j*dy,2);
                u_exact[k] = pow(i*dx,2)+pow(j*dy,2);
                i=imax-1;
                k=i*jmax+j;
                A[k][4] = 1.0;
                b[k] = pow(i*dx,2)+pow(j*dy,2);
                u_exact[k] = pow(i*dx,2)+pow(j*dy,2);
            }
            //internal nodes
            for (i=1;i<imax-1;i++)
            {
                for (j=1;j<jmax-1;j++)
                {
                    k = i*jmax+j;
                    A[k][1] = 1/pow(dx,2);
                    A[k][3] = 1/pow(dy,2);
                    A[k][4] = -2/pow(dx,2)-2/pow(dy,2);
                    A[k][5] = 1/pow(dy,2);
                    A[k][7] = 1/pow(dx,2);
                    b[k] = 4;
                    u_exact[k] = pow(i*dx,2)+pow(j*dy,2);
                }
            }
            break;
        }

    //Incomplete LU decomposition
    switch (stencil)
    {
    case 5:
        for (i=0;i<N;i++)
        {
            for (j=0;j<stencil;j++)
            {
            LU[i][j] = A[i][j];
            } 
        }
        msip5(LU,psi,N,jmax); 
        break;
    
    default:
    for (i=0;i<N;i++)
        {
            for (j=0;j<stencil;j++)
            {
            LU[i][j] = 0;
            } 
        }
    msip9(LU,A,psi,N,jmax);
        break;
    }   
    
    //Initial solution 
    for (i=0;i<N;i++)
    {
        u[i] = 0;
        du[i] = 0;
        w[i] = 0;
        rhs[i] =0;
    }
     

//----------Matrices and vectors storage---------- 
if (write==1)
{
    //save of matrix A
    ofstream write_A;
    write_A.open("A.txt");
    for (i=0;i<N-1;i++)
    {
        for (j=0;j<stencil-1;j++)
            write_A<<A[i][j]<<"\t";
        write_A<<A[i][j];
        write_A<<"\n";    
    }
    for (j=0;j<stencil-1;j++)
        write_A<<A[i][j]<<"\t";
    write_A<<A[i][j];
    write_A.close();
    //save vector b 
    ofstream write_b;
    write_b.open("b.txt");
    for (i=0;i<N-1;i++)
        write_b<<b[i]<<"\n";
    write_b<<b[i];
    write_b.close(); 
}
    //save matrix LU
    ofstream LU_;
    LU_.open("LU.txt");
    for (i=0;i<N-1;i++)
    {
        for (j=0;j<stencil-1;j++)
            LU_<<LU[i][j]<<"\t";
        LU_<<LU[i][j];
        LU_<<"\n";    
    }
    for (j=0;j<stencil-1;j++)
        LU_<<LU[i][j]<<"\t";
    LU_<<LU[i][j];
    LU_.close();

    //----------Solution of the system using the Delta Formulation----------
    rhs_constructor(rhs,A,b,u,N,jmax,stencil);
    
    res_init = 0;
    err_init = 0;
    for (i=0;i<N;i++)
    {
        res_init += pow(rhs[i],2);
        err_init += pow(u_exact[i]-u[i],2);
    }
    res_init = sqrt(res_init);      //initial (absolute) residual norm
    err_init = sqrt(err_init);      //initial (absolute) error norm
    iter = 0;                       //iteration counter

    ofstream cp;                    //file to save the residual and error norm for each iteration (1st column: number of iter , 2nd column : relative residual)
    cp.open("Convergence.txt");

    switch (stencil)
    {
    case 5:
        while(1)
        {   
            //calculation of the new rhs and the relative residual
            rhs_constructor(rhs,A,b,u,N,jmax,stencil);
            res = 0;
            err = 0;
            for (i=0;i<N;i++)
            {
                res += pow(rhs[i],2);
                err += pow(u_exact[i]-u[i],2);
            }
            res = sqrt(res)/res_init;       //relative residual norm
            err = sqrt(err)/err_init;       //relative error norm
            //save iteration , residual and error 
            cp<<iter<<"\t"<<res<<"\t"<<err<<"\n";

            //check convergence 
            if (res<tol)
                break;

            if(iter==maxiter)
            {
                cout<<"Maximum number of iterations reached"<<endl<<"Current relative residual norm: "<<res<<endl;
                cout<<"Select one of the following: "<<endl<<"1. More iterations"<<endl<<"2. End"<<endl<<"Your selection: ";
                cin>>selection;
                if (selection == 1)
                {
                    cout<<"Select the number of extra iterations: ";
                    cin>>extra_iter;
                    maxiter += extra_iter;
                }
                else 
                    break;
            }    

            //count the next iteration
            iter ++;
            
            //solution of the forward system L*w=rhs
            k=0;
            w[k] = rhs[k]/LU[k][2];
            for (k=1;k<jmax;k++)
                w[k] = (rhs[k]-LU[k][1]*w[k-1])/LU[k][2];
            for (k=jmax;k<N;k++)
                w[k] = (rhs[k]-LU[k][0]*w[k-jmax]-LU[k][1]*w[k-1])/LU[k][2];   
            //solution of the backward system U*du=w
            k=N-1;
            du[k] = w[k];
            for (k=N-2;k>=N-jmax-2;k--)
                du[k] = w[k] - LU[k][3]*du[k+1];
            for (k=N-jmax-1;k>=0;k--)
                du[k] = w[k] - LU[k][3]*du[k+1] - LU[k][4]*du[k+jmax];

            //new u 
            for (k=1;k<N;k++)
                u[k] = u[k] + omega*du[k];    
    

        }
        break;
    
    default:
        while(1)
        {   
            //calculation of the new rhs and the relative residual
            rhs_constructor(rhs,A,b,u,N,jmax,stencil);
            res = 0;
            err = 0;
            for (i=0;i<N;i++)
            {
                res += pow(rhs[i],2);
                err += pow(u_exact[i]-u[i],2);
            }
            res = sqrt(res)/res_init;       //relative residual norm
            err = sqrt(err)/err_init;       //relative error norm
            //save iteration , residual and error 
            cp<<iter<<"\t"<<res<<"\t"<<err<<"\n";
            //check convergence 
            if (res<tol)
                break;

            if(iter==maxiter)
            {
                cout<<"Maximum number of iterations reached"<<endl<<"Current relative residual norm: "<<res<<endl;
                cout<<"Select one of the following: "<<endl<<"1. More iterations"<<endl<<"2. End"<<endl<<"Your selection: ";
                cin>>selection;
                if (selection == 1)
                {
                    cout<<"Select the number of extra iterations: ";
                    cin>>extra_iter;
                    maxiter += extra_iter;
                }
                else 
                    break;
            }    

            //count the next iteration
            iter ++;
            
            //solution of the forward system L*w=rhs
            k=0;
            w[k] = rhs[k]/LU[k][4];
            for (k=1;k<jmax-1;k++)
                w[k] = (rhs[k] - LU[k][3]*w[k-1])/LU[k][4];
            w[k] = (rhs[k] - LU[k][2]*w[k-jmax+1] - LU[k][3]*w[k-1])/LU[k][4];    
            k = jmax;
            w[k] = (rhs[k] - LU[k][1]*w[k-jmax] - LU[k][2]*w[k-jmax+1] - LU[k][3]*w[k-1])/LU[k][4];   
            for (k=jmax+1;k<N;k++)
                w[k] = (rhs[k] - LU[k][0]*w[k-jmax-1] - LU[k][1]*w[k-jmax] - LU[k][2]*w[k-jmax+1] - LU[k][3]*w[k-1])/LU[k][4];
            //solution of the backward system U*du=w
            k=N-1;
            du[k] = w[k];
            for (k=N-2;k>=N-jmax+1;k--)
                du[k] = w[k] - LU[k][5]*du[k+1];
            k = N-jmax;
            du[k] = w[k] - LU[k][5]*du[k+1] - LU[k][6]*du[k+jmax-1];
            k=N-jmax-1;
            du[k] = w[k] - LU[k][5]*du[k+1] - LU[k][6]*du[k+jmax-1] - LU[k][7]*du[k+jmax];
            for (k=N-jmax-2;k>=0;k--)
                du[k] = w[k] - LU[k][5]*du[k+1] - LU[k][6]*du[k+jmax-1] - LU[k][7]*du[k+jmax] - LU[k][8]*du[k+jmax+1];

            //new u 
            for (k=1;k<N;k++)
                u[k] = u[k] + omega*du[k];    

        }
    
        break;
    }
    cp.close();


    //----------Results--------------
    cout<<endl<<endl;
    cout<<"System solved"<<endl<<"Final relative residual norm: "<<res<<endl<<"Final relative error norm: "<<err<<endl<<"Number of iterations: "<<iter<<endl;

    //----------Output-----------
    ofstream X;
    X.open("X.txt");
    for (j=0;j<jmax;j++)
    {
        for (i=0;i<imax;i++)
            X<<i*dx<<"\t";
        X<<"\n";    
    }
    X.close();

    ofstream Y;
    Y.open("Y.txt");
    for (j=jmax-1;j>=0;j--)
    {
    	for (i=0;i<imax;i++)
    		Y<<j*dy<<"\t";
    	Y<<"\n";	
	}
	Y.close();

    ofstream U;
    U.open("U.txt");
    for (j=jmax-1;j>=0;j--)
    {
        for (i=0;i<imax;i++)
        {
            k = i*jmax+j;
            U<<u[k]<<"\t";
        }
        U<<"\n";
    }

    //----------Freeing dynamically allocated memory----------
    delete [] b;
    delete [] du;
    delete [] u_exact;
    delete [] u;
    delete [] w;
    delete [] rhs;
    for (i=0;i<N;i++)
        delete [] A[i];
    delete [] A;
    for (i=0;i<N;i++)
        delete [] LU[i];
    delete [] LU;

    return 0;
}




void msip5(double** A, double psi, int N, int jmax)
{
    int k=0;
    A[k][3] = A[k][3]/A[k][2];
    A[k][4] = A[k][4]/A[k][2];
    for (k=1;k<jmax;k++)
    {
        A[k][1] = A[k][1]/(1+psi*A[k-1][4]);
        A[k][2] = A[k][2] - A[k][1]*A[k-1][3] + psi*A[k][1]*A[k-1][4];
        A[k][3] = A[k][3]/A[k][2];
        A[k][4] = (A[k][4] - psi*A[k][1]*A[k-1][4])/A[k][2];           
    }
    for (k=jmax;k<N-jmax;k++)
    {
        A[k][0] = A[k][0]/(1+psi*A[k-jmax][3]);
        A[k][1] = A[k][1]/(1+psi*A[k-1][4]);
        A[k][2] = A[k][2] - A[k][1]*A[k-1][3] - A[k][0]*A[k-jmax][4] + psi*(A[k][1]*A[k-1][4] + A[k][0]*A[k-jmax][3]);
        A[k][3] = (A[k][3] - psi*A[k][0]*A[k-jmax][3])/A[k][2];
        A[k][4] = (A[k][4] - psi*A[k][1]*A[k-1][4])/A[k][2];
    }    
    for (k=N-jmax;k<N-1;k++)
    {
        A[k][0] = A[k][0]/(1+psi*A[k-jmax][3]);
        A[k][1] = A[k][1]/(1+psi*A[k-1][4]);
        A[k][2] = A[k][2] - A[k][1]*A[k-1][3] - A[k][0]*A[k-jmax][4] + psi*(A[k][1]*A[k-1][4] + A[k][0]*A[k-jmax][3]);
        A[k][3] = (A[k][3] - psi*A[k][0]*A[k-jmax][3])/A[k][2];    
    }
    A[k][0] = A[k][0]/(1+psi*A[k-jmax][3]);
    A[k][1] = A[k][1]/(1+psi*A[k-1][4]);
    A[k][2] = A[k][2] - A[k][1]*A[k-1][3] - A[k][0]*A[k-jmax][4] + psi*(A[k][1]*A[k-1][4] + A[k][0]*A[k-jmax][3]);

}

void msip9(double** LU, double** A,double psi, int N, int jmax)
{
    int k=0;
    LU[k][4] = A[k][4];
    LU[k][5] = A[k][5]/LU[k][4];
    LU[k][6] = A[k][6]/LU[k][4];
    LU[k][7] = A[k][7]/LU[k][4];
    LU[k][8] = A[k][8]/LU[k][4];
    for (k=1;k<jmax-1;k++)
    {
        LU[k][3] = A[k][3]/(1+2*psi*LU[k-1][6]);
        LU[k][4] = A[k][4] - LU[k][3]*LU[k-1][5] + 2*psi*LU[k][3]*LU[k-1][6];
        LU[k][5] = A[k][5]/LU[k][4];
        LU[k][6] = (A[k][6] - LU[k][3]*LU[k-1][7])/LU[k][4];
        LU[k][7] = (A[k][7] - psi*LU[k][3]*LU[k-1][6] - LU[k][3]*LU[k-1][8])/LU[k][4];
        LU[k][8] = A[k][8]/LU[k][4];
    }
    LU[k][2] = A[k][2];
    LU[k][3] = A[k][3]/(1+2*psi*LU[k-1][6]);
    LU[k][4] = A[k][4] - LU[k][3]*LU[k-1][5] - LU[k][2]*LU[k-jmax+1][6] + 2*psi*(LU[k][2]*LU[k-jmax+1][5] + LU[k][3]*LU[k-1][6]) + psi*LU[k][2]*LU[k-jmax+1][8];
    LU[k][5] = (A[k][5] - 2*psi*LU[k][2]*(LU[k-jmax+1][5] + LU[k-jmax+1][8]) - LU[k][2]*LU[k-jmax+1][7])/LU[k][4];
    LU[k][6] = (A[k][6] - LU[k][3]*LU[k-1][7])/LU[k][4];
    LU[k][7] = (A[k][7] - psi*LU[k][3]*LU[k-1][6] - LU[k][3]*LU[k-1][8])/LU[k][4];
    LU[k][8] = A[k][8]/LU[k][4];
    k = jmax;
    LU[k][1] = (A[k][1] - psi*LU[k-jmax+1][5]*A[k][2])/(1-psi*LU[k-jmax][5]*LU[k-jmax+1][5]);
    LU[k][2] = A[k][2] - LU[k][1]*LU[k-jmax][5]; 
    LU[k][3] = (A[k][3] - LU[k][1]*LU[k-jmax][6])/(1+2*psi*LU[k-1][6]);
    LU[k][4] = A[k][4] - LU[k][2]*LU[k-jmax+1][6] - LU[k][3]*LU[k-1][5] - LU[k][1]*LU[k-jmax][7] + 2*psi*(LU[k][2]*LU[k-jmax+1][5] + LU[k][3]*LU[k-1][6]) + psi*LU[k][2]*LU[k-jmax+1][8];
    LU[k][5] = (A[k][5] - 2*psi*LU[k][2]*(LU[k-jmax+1][5] + LU[k-jmax+1][8]) - LU[k][2]*LU[k-jmax+1][7] - LU[k][1]*LU[k-jmax][8])/LU[k][4];
    LU[k][6] = (A[k][6] - LU[k][3]*LU[k-1][7])/LU[k][4]; 
    LU[k][7] = (A[k][7] - psi*LU[k][3]*LU[k-1][6] - LU[k][3]*LU[k-1][8])/LU[k][4];
    LU[k][8] = A[k][8]/LU[k][4];
    for (k=jmax+1;k<N-jmax-1;k++)
    {
        LU[k][0] = A[k][0];
        LU[k][1] = (A[k][1] - psi*LU[k-jmax+1][5]*A[k][2] - LU[k][0]*LU[k-jmax-1][5])/(1-psi*LU[k-jmax][5]*LU[k-jmax+1][5]);
        LU[k][2] = A[k][2] - LU[k][1]*LU[k-jmax][5];
        LU[k][3] = (A[k][3] - 2*psi*LU[k][0]*LU[k-jmax-1][6] - LU[k][0]*LU[k-jmax-1][7] - LU[k][1]*LU[k-jmax][6])/(1+2*psi*LU[k-1][6]);
        LU[k][4] = A[k][4] - LU[k][0]*LU[k-jmax-1][8] - LU[k][1]*LU[k-jmax][7] - LU[k][2]*LU[k-jmax+1][6] - LU[k][3]*LU[k-1][5] + 2*psi*(LU[k][2]*LU[k-jmax+1][5] + LU[k][3]*LU[k-1][6]) + psi*(LU[k][0]*LU[k-jmax-1][6] + LU[k][2]*LU[k-jmax+1][8]);
        LU[k][5] = (A[k][5] - 2*psi*LU[k][2]*(LU[k-jmax+1][5] + LU[k-jmax+1][8]) - LU[k][2]*LU[k-jmax+1][7] - LU[k][1]*LU[k-jmax][8])/LU[k][4];
        LU[k][6] = (A[k][6] - LU[k][3]*LU[k-1][7])/LU[k][4]; 
        LU[k][7] = (A[k][7] - psi*LU[k][3]*LU[k-1][6] - LU[k][3]*LU[k-1][8])/LU[k][4];
        LU[k][8] = A[k][8]/LU[k][4]; 
    }
    LU[k][0] = A[k][0];
    LU[k][1] = (A[k][1] - psi*LU[k-jmax+1][5]*A[k][2] - LU[k][0]*LU[k-jmax-1][5])/(1-psi*LU[k-jmax][5]*LU[k-jmax+1][5]);
    LU[k][2] = A[k][2] - LU[k][1]*LU[k-jmax][5];
    LU[k][3] = (A[k][3] - 2*psi*LU[k][0]*LU[k-jmax-1][6] - LU[k][0]*LU[k-jmax-1][7] - LU[k][1]*LU[k-jmax][6])/(1+2*psi*LU[k-1][6]);
    LU[k][4] = A[k][4] - LU[k][0]*LU[k-jmax-1][8] - LU[k][1]*LU[k-jmax][7] - LU[k][2]*LU[k-jmax+1][6] - LU[k][3]*LU[k-1][5] + 2*psi*(LU[k][2]*LU[k-jmax+1][5] + LU[k][3]*LU[k-1][6]) + psi*(LU[k][0]*LU[k-jmax-1][6] + LU[k][2]*LU[k-jmax+1][8]);
    LU[k][5] = (A[k][5] - 2*psi*LU[k][2]*(LU[k-jmax+1][5] + LU[k-jmax+1][8]) - LU[k][2]*LU[k-jmax+1][7] - LU[k][1]*LU[k-jmax][8])/LU[k][4];
    LU[k][6] = (A[k][6] - LU[k][3]*LU[k-1][7])/LU[k][4]; 
    LU[k][7] = (A[k][7] - psi*LU[k][3]*LU[k-1][6] - LU[k][3]*LU[k-1][8])/LU[k][4];
    k = N-jmax;
    LU[k][0] = A[k][0];
    LU[k][1] = (A[k][1] - psi*LU[k-jmax+1][5]*A[k][2] - LU[k][0]*LU[k-jmax-1][5])/(1-psi*LU[k-jmax][5]*LU[k-jmax+1][5]);
    LU[k][2] = A[k][2] - LU[k][1]*LU[k-jmax][5];
    LU[k][3] = (A[k][3] - 2*psi*LU[k][0]*LU[k-jmax-1][6] - LU[k][0]*LU[k-jmax-1][7] - LU[k][1]*LU[k-jmax][6])/(1+2*psi*LU[k-1][6]);
    LU[k][4] = A[k][4] - LU[k][0]*LU[k-jmax-1][8] - LU[k][1]*LU[k-jmax][7] - LU[k][2]*LU[k-jmax+1][6] - LU[k][3]*LU[k-1][5] + 2*psi*(LU[k][2]*LU[k-jmax+1][5] + LU[k][3]*LU[k-1][6]) + psi*(LU[k][0]*LU[k-jmax-1][6] + LU[k][2]*LU[k-jmax+1][8]);
    LU[k][5] = (A[k][5] - 2*psi*LU[k][2]*(LU[k-jmax+1][5] + LU[k-jmax+1][8]) - LU[k][2]*LU[k-jmax+1][7] - LU[k][1]*LU[k-jmax][8])/LU[k][4];
    LU[k][6] = (A[k][6] - LU[k][3]*LU[k-1][7])/LU[k][4]; 
    for (k=N-jmax+1;k<N-1;k++)
    {
        LU[k][0] = A[k][0];
        LU[k][1] = (A[k][1] - psi*LU[k-jmax+1][5]*A[k][2] - LU[k][0]*LU[k-jmax-1][5])/(1-psi*LU[k-jmax][5]*LU[k-jmax+1][5]);
        LU[k][2] = A[k][2] - LU[k][1]*LU[k-jmax][5];
        LU[k][3] = (A[k][3] - 2*psi*LU[k][0]*LU[k-jmax-1][6] - LU[k][0]*LU[k-jmax-1][7] - LU[k][1]*LU[k-jmax][6])/(1+2*psi*LU[k-1][6]);
        LU[k][4] = A[k][4] - LU[k][0]*LU[k-jmax-1][8] - LU[k][1]*LU[k-jmax][7] - LU[k][2]*LU[k-jmax+1][6] - LU[k][3]*LU[k-1][5] + 2*psi*(LU[k][2]*LU[k-jmax+1][5] + LU[k][3]*LU[k-1][6]) + psi*(LU[k][0]*LU[k-jmax-1][6] + LU[k][2]*LU[k-jmax+1][8]);
        LU[k][5] = (A[k][5] - 2*psi*LU[k][2]*(LU[k-jmax+1][5] + LU[k-jmax+1][8]) - LU[k][2]*LU[k-jmax+1][7] - LU[k][1]*LU[k-jmax][8])/LU[k][4];
    }
    LU[k][0] = A[k][0];
    LU[k][1] = (A[k][1] - psi*LU[k-jmax+1][5]*A[k][2] - LU[k][0]*LU[k-jmax-1][5])/(1-psi*LU[k-jmax][5]*LU[k-jmax+1][5]);
    LU[k][2] = A[k][2] - LU[k][1]*LU[k-jmax][5];
    LU[k][3] = (A[k][3] - 2*psi*LU[k][0]*LU[k-jmax-1][6] - LU[k][0]*LU[k-jmax-1][7] - LU[k][1]*LU[k-jmax][6])/(1+2*psi*LU[k-1][6]);
    LU[k][4] = A[k][4] - LU[k][0]*LU[k-jmax-1][8] - LU[k][1]*LU[k-jmax][7] - LU[k][2]*LU[k-jmax+1][6] - LU[k][3]*LU[k-1][5] + 2*psi*(LU[k][2]*LU[k-jmax+1][5] + LU[k][3]*LU[k-1][6]) + psi*(LU[k][0]*LU[k-jmax-1][6] + LU[k][2]*LU[k-jmax+1][8]);
}

void rhs_constructor(double* rhs, double** A, double* b, double* u, int N, int jmax, int stencil)
{

    int k=0;
    switch (stencil)
    {
    case 5:
        rhs[k] = b[k] - A[k][2]*u[k] - A[k][3]*u[k+1] - A[k][4]*u[k+jmax];
        for (k=1;k<jmax;k++)
            rhs[k] = b[k] - A[k][1]*u[k-1] - A[k][2]*u[k] - A[k][3]*u[k+1] - A[k][4]*u[k+jmax];
        for (k=jmax;k<N-jmax;k++)
            rhs[k] = b[k] - A[k][0]*u[k-jmax] - A[k][1]*u[k-1] - A[k][2]*u[k] - A[k][3]*u[k+1] - A[k][4]*u[k+jmax];        
        for (k=N-jmax;k<N-1;k++)
            rhs[k] = b[k] - A[k][0]*u[k-jmax] - A[k][1]*u[k-1] - A[k][2]*u[k] - A[k][3]*u[k+1];
        rhs[k] = b[k] - A[k][0]*u[k-jmax] - A[k][1]*u[k-1] - A[k][2]*u[k];
        break;
    
    default:
        rhs[k] = b[k] - A[k][4]*u[k] - A[k][5]*u[k+1] - A[k][6]*u[k+jmax-1] - A[k][7]*u[k+jmax] - A[k][8]*u[k+jmax+1];
        for (k=1;k<jmax-1;k++)
            rhs[k] = b[k] - A[k][3]*u[k-1] - A[k][4]*u[k] - A[k][5]*u[k+1] - A[k][6]*u[k+jmax-1] - A[k][7]*u[k+jmax] - A[k][8]*u[k+jmax+1]; 
        rhs[k] = b[k] - A[k][2]*u[k-jmax+1] - A[k][3]*u[k-1] - A[k][4]*u[k] - A[k][5]*u[k+1] - A[k][6]*u[k+jmax-1] - A[k][7]*u[k+jmax] - A[k][8]*u[k+jmax+1];    
        k=jmax;
        rhs[k] = b[k] - A[k][1]*u[k-jmax] - A[k][2]*u[k-jmax+1] - A[k][3]*u[k-1] - A[k][4]*u[k] - A[k][5]*u[k+1] - A[k][6]*u[k+jmax-1] - A[k][7]*u[k+jmax] - A[k][8]*u[k+jmax+1];
        for (k=jmax+1;k<N-jmax-1;k++)
            rhs[k] = b[k] - A[k][0]*u[k-jmax-1] - A[k][1]*u[k-jmax] - A[k][2]*u[k-jmax+1] - A[k][3]*u[k-1] - A[k][4]*u[k] - A[k][5]*u[k+1] - A[k][6]*u[k+jmax-1] - A[k][7]*u[k+jmax] - A[k][8]*u[k+jmax+1];
        rhs[k] = b[k] - A[k][0]*u[k-jmax-1] - A[k][1]*u[k-jmax] - A[k][2]*u[k-jmax+1] - A[k][3]*u[k-1] - A[k][4]*u[k] - A[k][5]*u[k+1] - A[k][6]*u[k+jmax-1] - A[k][7]*u[k+jmax];
        k=N-jmax;
        rhs[k] = b[k] - A[k][0]*u[k-jmax-1] - A[k][1]*u[k-jmax] - A[k][2]*u[k-jmax+1] - A[k][3]*u[k-1] - A[k][4]*u[k] - A[k][5]*u[k+1] - A[k][6]*u[k+jmax-1];
        for (k=N-jmax+1;k<N-1;k++)
            rhs[k] = b[k] - A[k][0]*u[k-jmax-1] - A[k][1]*u[k-jmax] - A[k][2]*u[k-jmax+1] - A[k][3]*u[k-1] - A[k][4]*u[k] - A[k][5]*u[k+1];
        rhs[k] = b[k] - A[k][0]*u[k-jmax-1] - A[k][1]*u[k-jmax] - A[k][2]*u[k-jmax+1] - A[k][3]*u[k-1] - A[k][4]*u[k];    
        break;
    }




}