/*  Solve the system of N linear equations

        Ax = b

    by using Gaussian elimination with partial pivoting to factor

        A = PLU

    where P is a permutation matrix, L is a lower-triagular matrix
    with 1's on the diagonal and U is an upper triangular matrix. 
    Then forward and back substituting to find x. */

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

#define N 5

double A[N][N] = {
    {  -3,  1, -6,  5, -5 },
    {   0, -5, -6,  3,  9 },
    {  -6, -6,  6,  8, -6 },
    {   9, -3, -5,  2, -4 },
    {  -6,  8, -7, -9, -8 }};

double b[N] = { 1, 2, 3, 4, 5 };

double y[N] = { -0.2235509997469, -1.1348946469248,
    -0.42091400911162, -0.67584946848899, -0.46360098709188 };

void LUfact(double *P[N],double A[N][N]){
    for(int i=0;i<N;i++) P[i]=&A[i][0];
    for(int k=0;k<N-1;k++){
        double pmax=fabs(P[k][k]); int imax=k;
        for(int i=k+1;i<N;i++){
            if(pmax<fabs(P[i][k])){
                pmax=fabs(P[i][k]); imax=i;
            }
        }
        double *t=P[k]; P[k]=P[imax]; P[imax]=t;
        for(int i=k+1;i<N;i++){
            double alpha=P[i][k]/P[k][k];
            for(int j=k;j<N;j++){
                P[i][j]-=alpha*P[k][j];
            }
            P[i][k]=alpha;
        }
    }
}

void LUsolve(double x[N],double *P[N],
        double LU[N][N],double b[N]){
    double *pmin=&LU[0][0];
    // Permute the vector y = P^t b
    double y[N];
    for(int i=0;i<N;i++){
        y[i]=b[(P[i]-pmin)/N];
    }
    // Forward solve L z = y
    double z[N];
    for(int i=0;i<N;i++){
        double r=0;
        for(int j=0;j<i;j++) r+=P[i][j]*z[j];
        z[i]=y[i]-r;
    }
    // Backward solve U x = z
    for(int i=N-1;i>=0;i--){
        double r=0;
        for(int j=i+1;j<N;j++) r+=P[i][j]*x[j];
        x[i]=(z[i]-r)/P[i][i];
    }
}

void vecprint(double x[N]){
    for(int i=0;i<N;i++) printf("%g ",x[i]);
    printf("\n");
}
double vecdist(double x[N],double y[N]){
    double r=0;
    for(int i=0;i<N;i++){
        double t=x[i]-y[i];
        r+=t*t;
    }
    return sqrt(r);
}

int main(){
    printf("This is problem 2 on exam part 2.\n");
    double *P[N], x[N];

    /* Please fill in the missing code here with suitable calls
       to the functions LUfact and LUsolve */

    printf("x = "); vecprint(x);
    if(vecdist(x,y)<1e-8){
        printf("Congratulations!  Your answer is correct!\n");
    } else {
        printf("Please try again.  Your answer is incorrect.\n");
    }
    return 0;
}