#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include "tictoc.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 PLU 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],int P[n]){
    for(int j=0;j<n;j++){
        P[j]=j;
    }
    for(int j=0;j<n-1;j++){
        //  Seach for the largest pivot in the column
        int i0=j;
        for(int i=j+1;i<n;i++){
            if(fabs(A[i][j])>fabs(A[i0][j])){
                i0=i;
            }
        }
 //       printf("swapping row %d with row %d\n",j,i0);
        swap(n,A[j],A[i0]);
        int t=P[j]; P[j]=P[i0]; P[i0]=t;
        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 PLUx=b for x where LU is the matrix returned by lufact.
void lusolve(int n,int P[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[P[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);
}

#define N 1000
double A[N][N];
double b[N];
double x[N],LU[N][N],b2[N];
int P[N];

int main(){
    for(int i=0;i<N;i++){
        for(int j=0;j<N;j++){
            A[i][j]=2.0*random()/RAND_MAX-1.0;
        }
        b[i]=2.0*random()/RAND_MAX-1.0;
    }
 //   matprint(N,A);
    memcpy(LU,A,N*N*sizeof(double));
    tic();
    lufact(N,LU,P);
    double tfact=toc();
 //   matprint(N,LU);
 //   for(int i=0;i<N;i++){
 //       printf("P[%d]=%d\n",i,P[i]);
 //   }
    tic();
    lusolve(N,P,LU,x,b);
    double tsolve=toc();
    multAb(N,b2,A,x);
//    for(int i=0;i<N;i++){
//        printf("b2[%d]=%g\n",i,b2[i]);
//    }
    printf("N=%d\n",N);
    printf("error=%.15e\n",dist(N,b,b2));
    printf("tfact=%g sec\n",tfact);
    printf("tsolve=%g sec\n",tsolve);
    printf("total time=%g sec\n",tfact+tsolve);
    printf("FLOPS=%g\n",2.0*(N/3.0+1.0)*N*N/(tfact+tsolve));
    return 0;
}