#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "matrixlib.h"

#define N 3
double A[N][N] = { 1,2,1, -1,0,2, 2,-1,-2 };


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];
            for(int k=j+1;k<n;k++){
                A[i][k]-=alpha*A[j][k];
            }
            A[i][j]=alpha;
        }
    }
}
void LUsolve(int n,double LU[n][n],
    double x[n],double b[n]){
    double y[n];
    for(int i=0;i<n;i++){ // Solve Ly=b
        y[i]=b[i];
        for(int j=0;j<i;j++){
            y[i]-=LU[i][j]*y[j];
        }
    }
    for(int i=n-1;i>=0;i--){ // Solve Ux=y
        x[i]=y[i];
        for(int j=i+1;j<n;j++){
            x[i]-=LU[i][j]*x[j];
        }
        x[i]/=LU[i][i];
    }
}



// PLUsolve is not finished
void PLUsolve(int n,double LU[n][n],
    double x[n],double b[n]){
    double y[n];
    for(int i=0;i<n;i++){ // Solve Ly=b
        y[i]=b[i];
        for(int j=0;j<i;j++){
            y[i]-=LU[i][j]*y[j];
        }
    }
    for(int i=n-1;i>=0;i--){ // Solve Ux=y
        x[i]=y[i];
        for(int j=i+1;j<n;j++){
            x[i]-=LU[i][j]*x[j];
        }
        x[i]/=LU[i][i];
    }
}

// PLUfact is not finished
void PLUfact(int n,double A[n][n],double *P[n]){
    for(int i=0;i<n;i++){
        P[i]=&A[i][0];
    }
    for(int j=0;j<n-1;j++){
        for(int i=j+1;i<n;i++){
            double alpha=P[i][j]/P[j][j];
            for(int k=j+1;k<n;k++){
                P[i][k]-=alpha*P[j][k];
            }
            P[i][j]=alpha;
        }
    }
}
double B[3],X[3]={1,2,3},XX[3],*P[3];
int main(){
    printf("A=\n"); matprint(N,N,A);
    multAx(N,N,A,X,B);  // B=AX
    printf("B=\n"); vecprint(N,B);
    PLUfact(N,A,P);     // now A contains LU fact
    printf("LU=\n"); matprint(N,N,A);
    LUsolve(N,A,XX,B);
    printf("XX=\n"); vecprint(N,X);
    return 0;
}