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

void scale(int n,double v[],double alpha){
    for(int i=0;i<n;i++){
        v[i]*=alpha;
    }
}
void eliminate(int n,double v1[],
    double v2[],double alpha){
    for(int i=0;i<n;i++){
        v1[i]+=alpha*v2[i];
    }
}
void swap(int n,double v1[],double v2[]){
    for(int i=0;i<n;i++){
        double t=v1[i]; v1[i]=v2[i]; v2[i]=t;
    }
}
void matprint(int n,double A[n][n]){
    for(int i=0;i<n;i++){
        for(int j=0;j<n;j++){
            printf("%g ",A[i][j]);
        }
        printf("\n");
    }
}
//  Factor A into LU where L is lower triangular with 1's on
//  the diagonal and U is upper triangular.  Return the result
//  in A where the upper triangular part of A is now U and the
//  part of A below the diagonal holds L.
void lufact(int n,double A[n][n]){
    for(int j=0;j<n-1;j++){
        for(int i=j+1;i<n;i++){
            double alpha=-A[i][j]/A[j][j];
            printf("r%d <- r%d + %g r%d\n",
                i,i,alpha,j);
            eliminate(n-j-1,A[i]+j+1,A[j]+j+1,alpha);
            A[i][j]=-alpha;
        }
    }
}
//  Solve LUx=b for x where LU is the matrix returned by lufact.
void lusolve(int n,double LU[n][n],double x[n],double b[n]){
    double y[n];
    //  Forward substitute to solve Ly=b
    for(int i=0;i<n;i++){
        y[i]=b[i];
        for(int j=0;j<i;j++){
            y[i]-=LU[i][j]*y[j];
        }
        printf("y[%d]=%g\n",i,y[i]);
    }
    //  Backward substitute to solve Ux=y
    for(int i=n-1;i>=0;i--){
        x[i]=y[i];
        for(int j=i+1;j<n;j++){
            x[i]-=LU[i][j]*x[j];
        }
        x[i]/=LU[i][i];
        printf("x[%d]=%g\n",i,x[i]);
    }
}
//  Compute b where b=Ax.
void multAb(int n,double b[n],double A[n][n],double x[n]){
    for(int i=0;i<n;i++){
        b[i]=0;
        for(int j=0;j<n;j++){
            b[i]+=A[i][j]*x[j];
        }
    }
}
//  Compute the norm |x-y|.
double dist(int n,double x[n],double y[n]){
    double s=0.0;
    for(int i=0;i<n;i++){
        double t=x[i]-y[i];
        s+=t*t;
    }
    return sqrt(s);
}
double A[3][3] = {{3,1,8},{2,4,7},{1,2,3}};
double b[3] = {4,5,6};
double x[3],LU[3][3],b2[3];

int main(){
    matprint(3,A);
    memcpy(LU,A,9*sizeof(double));
    lufact(3,LU);
    matprint(3,LU);
    lusolve(3,LU,x,b);
    multAb(3,b2,A,x);
    for(int i=0;i<3;i++){
        printf("b2[%d]=%g\n",i,b2[i]);
    }
    printf("error=%.15e\n",dist(3,b,b2));
    return 0;
}