#include <stdio.h>
#include <math.h>

#define N 4
#define EPS 1e-8
#define MAX_ITER 100

double A[N][N] = {
    {1.0, 2.0, 3.0, 5.0},
    {3.0, -5.0, 1.0, 4.0},
    {5.0, 9.0, 2.0, -6.0},
    {1.0, 7.0, 4.0, 1.0}
};

/* LU decomposition */
void lu_decomposition(double B[N][N],
                      double L[N][N],
                      double U[N][N])
{
    for(int i=0;i<N;i++){
        for(int j=0;j<N;j++){
            L[i][j]=(i==j)?1.0:0.0;
            U[i][j]=0.0;
        }
    }

    for(int i=0;i<N;i++){

        for(int j=i;j<N;j++){
            double sum = 0.0;

            for(int k=0;k<i;k++)
                sum += L[i][k]*U[k][j];

            U[i][j] = B[i][j] - sum;
        }

        for(int j=i+1;j<N;j++){
            double sum = 0.0;

            for(int k=0;k<i;k++)
                sum += L[j][k]*U[k][i];

            L[j][i] = (B[j][i]-sum)/U[i][i];
        }
    }
}

/* Forward substitution */
void forward_substitution(double L[N][N],
                          double x[N],
                          double y[N])
{
    for(int i=0;i<N;i++){

        double sum = 0.0;

        for(int j=0;j<i;j++)
            sum += L[i][j]*y[j];

        y[i] = x[i] - sum;
    }
}

/* Backward substitution */
void backward_substitution(double U[N][N],
                           double y[N],
                           double x[N])
{
    for(int i=N-1;i>=0;i--){

        double sum = 0.0;

        for(int j=i+1;j<N;j++)
            sum += U[i][j]*x[j];

        x[i] = (y[i]-sum)/U[i][i];
    }
}

void inverse_iteration(double lambda_hat)
{
    double B[N][N];
    double L[N][N];
    double U[N][N];

    double x[N] = {1.0,1.0,1.0,1.0};

    double y[N];
    double x_next[N];

    for(int i=0;i<N;i++){
        for(int j=0;j<N;j++){

            B[i][j] = A[i][j];

            if(i==j)
                B[i][j] -= lambda_hat;
        }
    }

    lu_decomposition(B,L,U);

    printf("\n=====================================\n");
    printf("shift = %.6f\n", lambda_hat);
    printf("=====================================\n");

    int iter=0;
    double diff;
    double lambda=0.0;

    do{

        forward_substitution(L,x,y);
        backward_substitution(U,y,x_next);

        double max_mag = 0.0;
        int idx = 0;

        for(int i=0;i<N;i++){
            if(fabs(x_next[i]) > max_mag){
                max_mag = fabs(x_next[i]);
                idx = i;
            }
        }

        double scale = x_next[idx];

        for(int i=0;i<N;i++)
            x_next[i] /= scale;

        diff = 0.0;

        for(int i=0;i<N;i++)
            diff += fabs(x_next[i]-x[i]);

        for(int i=0;i<N;i++)
            x[i]=x_next[i];

        lambda = lambda_hat + 1.0/scale;

        printf("Iter %2d : lambda = %.10f\n",
               iter+1, lambda);

        iter++;

    }while(diff>EPS && iter<MAX_ITER);

    printf("\nFinal eigenvalue:\n");
    printf("%.10f\n", lambda);

    printf("\nEigenvector:\n");
    for(int i=0;i<N;i++)
        printf("%.10f\n", x[i]);
}

int main()
{
    inverse_iteration(-5.0);
    inverse_iteration(-2.0);
    inverse_iteration(-3.0);

    return 0;
}