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

#define N 2000

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];
    }
}

void PLUsolve(int n,double LU[n][n],double *P[n],
    double x[n],double b[n]){
    double y[n];
    for(int i=0;i<n;i++){ // Solve Ly=b
        y[i]=b[(P[i]-&LU[0][0])/n];
        for(int j=0;j<i;j++){
            y[i]-=P[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]-=P[i][j]*x[j];
        }
        x[i]/=P[i][i];
    }
}

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++){
            if(fabs(P[j][j])<fabs(P[i][j])){
                double *t=P[j]; P[j]=P[i]; P[i]=t;
//                printf("Swapped row %d with row %d\n",j,i);
            }
        }
        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 A[N][N];
double B[N],X[N],XX[N],*P[N];
int main(){
//    printf("A=\n"); matprint(N,N,A);
    for(int i=0;i<N;i++){
        for(int j=0;j<N;j++){
            A[i][j]=2.0*random()/RAND_MAX-1.0;
        }
        X[i]=1;
    }
    multAx(N,N,A,X,B);  // B=AX
//    printf("B=\n"); vecprint(N,B);
    tic();
    PLUfact(N,A,P);     // now A contains LU fact
//    printf("LU=\n"); matprint(N,N,A);
    PLUsolve(N,A,P,XX,B);
    double t=toc();
    printf("The elapsed time was %g seconds.\n",t);
//    printf("XX=\n"); vecprint(N,XX);
    double r=0;
    for(int i=0;i<N;i++){
        double rd=XX[i]-1;
        r+=rd*rd;
    }
    r=sqrt(r);
    printf("The error is %g.\n",r);
    return 0;
}