/* ３種の反復法の計算時間の比較 */
/* 公平な比較のため, 三つとも帯状行列用に変えてある */
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <sys/time.h>
#ifndef max
#define max(x,y) ((x)>=(y) ? (x):(y))
#endif
#ifndef min
#define min(x,y) ((x)<=(y) ? (x):(y))
#endif
/* 第６回のプログラム例から借用したサブルーチン類 */
void inputvector(double *b,int n)
{
    int j;
    char buf[256], *p, *q;
    rewind(stdin);
    do {                         /* Linux では rewind が効かないので */
        fgets(buf,256,stdin);
    } while (*buf =='\n');
    p=buf;
    for (j=0;j<n;j++){
        q=strchr(p,',');    /* コンマまでが現在の要素を表す文字列 */
        if (q!=NULL) *q=0;  /* 最後の要素は文字列末が LF */
        b[j]=atof(p);
        p=q+1;
    }
}
void inputbandmat(double **A,int n,int bw){
    int i;
    printf("対称帯状行列の入力ルーチン：\n");fflush(stdout);
    for (i=0;i<=bw;i++){
        printf("第 %d 対角成分：\n",i);fflush(stdout);
        inputvector(A[i],n-i);
	}
}
void outputvector(double *b,int n)
{
    int i;
    printf("(");
    for (i=0;i<n-1;i++){
        printf("%lf,",b[i]);
    }
    printf("%lf)\n",b[n-1]);
}
/* 帯状行列とベクトルの積 */
void mulvector(double **A, double *x, double *y,int n,int bw)
{
    int i,j;
    for (i=0;i<n;i++){
        y[i]=0;
        for (j=max(0,i-bw);j<=min(n-1,i+bw);j++){
            if (j<=i) y[i]=y[i]+A[i-j][j]*x[j];
            else y[i]=y[i]+A[j-i][i]*x[j];
        }
    }
}
/* 帯状行列の出力 */
void outbandmat(double **A, int n, int bw)
{
    int i,j;
    double zero=0;
    for (i=0;i<n;i++){
        for (j=0;j<n;j++){
            if (j<i-bw) printf("%7.5lf,",zero);
            else if (j<=i) printf("%7.5lf,",A[i-j][j]);
            else if (j<=i+bw) printf("%7.5lf,",A[j-i][i]);
            else  printf("%7.5lf,",zero);
        }
        printf("\n");
    }
}
double innerprod(double *x,double *y,int n){
    double s;
    int i;
    s=(double)0;
    for (i=0;i<n;i++) s=s+x[i]*y[i];
    return s;
}
/* 帯状行列のコピー */
void copymat(double **a,double **c,int n,int bw){
    int i,j;
    for (i=0;i<=bw;i++){
        for (j=0;j<n;j++){
             c[i][j]=a[i][j];
        }
    }
}
/* モデル帯状行列設定のための微分方程式のポテンシャル函数 */
double q(double x){
    return (double)2;
}
/* モデルベクトル設定のための微分方程式の右辺の函数 */
double g(double x){
    return (double)1;
}
/* 微分方程式の離散化から帯状行列を作る */
/* 成分を具体的に定義しているので, M は使っていない. */
int setmat(double **a,int n){
    int i,j;
    double xmin,xmax,x,h;
    xmin=(double)0; xmax=(double)1;
    h=(xmax-xmin)/(n+1);
    x=xmin+h;
    for (i=0;i<n;i++){
        a[0][i]=2/h/h+q(x);
        x=x+h;
    }
    for (i=0;i<n-1;i++){
        a[1][i]=-1/h/h;
    }
    return 1;  /* bw */
}
/* 右辺の既知函数から離散化ベクトルを作る */
void setvec(double *f,int n){
    int i;
    double xmin,xmax,x,h;
    xmin=(double)0; xmax=(double)1;
    h=(xmax-xmin)/(n+1);
    x=xmin+h;
    for (i=0;i<n;i++){
        f[i]=g(x);
        x=x+h;
    }
}
/* Jacobi 法 */
int jacobi(double **a,double *b,double *x,int n,int bw,double eps){
    int i,j,ino;
    double anorm, *y=(double *)malloc(sizeof(double)*n);
/* とりあえず初期ベクトルを 0 にとると */
    for (i=0;i<n;i++){
        x[i]=(double)0;
    }
    ino=0;
/* 反復の開始  Y=D^{-1}(B-(L+R)X) */
    while (1){
        anorm=(double)0;
        for (i=0;i<n;i++){
            y[i]=b[i];
            for (j=max(0,i-bw);j<i;j++){
                y[i]=y[i]-a[i-j][j]*x[j];
            }
            for (j=i+1;j<=min(n-1,i+bw);j++){
                y[i]=y[i]-a[j-i][i]*x[j];
            }
            y[i]=y[i]/a[0][i];
            anorm=anorm+(x[i]-y[i])*(x[i]-y[i]);
        }
        ino=ino+1;
/* 途中経過をみたいときは次のコメントをはずせ */
/*      outputvector(y,n); */
        if (anorm>=eps){
	        for (i=0;i<n;i++){
                x[i]=y[i];
            }
        } else {
            break;
        }
    } 
    free(y);
    return ino;
}
/* Gauss-Seidel 法 */
int gaussseidel(double **a,double *b,double *x,int n,int bw,double eps){
    int i,j,ino;
    double y,anorm;
/* とりあえず初期ベクトルを 0 にとると */
    for (i=0;i<n;i++){
        x[i]=(double)0;
    }
    ino=0;
/* 反復の開始  Y=D^{-1}(B-(L+R)X) */
    do {
        anorm=(double)0;
        for (i=0;i<n;i++){
            y=b[i];
            for (j=max(0,i-bw);j<i;j++){
                y=y-a[i-j][j]*x[j];
            }
            for (j=i+1;j<=min(n-1,i+bw);j++){
                y=y-a[j-i][i]*x[j];
            }
            y=y/a[0][i];
            anorm=anorm+(y-x[i])*(y-x[i]);
            x[i]=y;
        }
        ino=ino+1;
/* 途中経過をみたいときは次のコメントをはずせ */
/*      outputvector(y,n); */
    } while(anorm>=eps);
    return ino;
}
/* Calculation of x = A^{-1}b by Conjugate Gradient Method */
int conjgrad(double **A,double *b,double *x,int n,int bw,double epsilon)
{
    double *w0=(double *)malloc(sizeof(double)*n);
    double *w1=(double *)malloc(sizeof(double)*n);
    double *wr=(double *)malloc(sizeof(double)*n);
    double rip,ripc,alpha,beta;
    int i,j,k;
    for (i=0;i<n;i++){
        wr[i]=b[i]; w0[i]=b[i];
        x[i]=0;
    }
    ripc=innerprod(wr,wr,n);
    for (k=0;k<2*n-1;k++){
        for (i=0;i<n;i++){
            w1[i]=(double)0;
            for (j=max(0,i-bw);j<=i;j++){
                w1[i]=w1[i]+A[i-j][j]*w0[j];
            }
            for (j=i+1;j<=min(n-1,i+bw);j++){
                w1[i]=w1[i]+A[j-i][i]*w0[j];
            }
        }
        alpha=ripc/innerprod(w0,w1,n);
        for (i=0;i<n;i++){
            x[i]=x[i]+alpha*w0[i];
            wr[i]=wr[i]-alpha*w1[i];
        }
        rip=innerprod(wr,wr,n);
/* Convergence test */
        if (rip<=epsilon) {
            free(wr);
            free(w1);
            free(w0);
            return k+1;
        }
        beta=rip/ripc;
        ripc=rip;
        for (i=0;i<n;i++){
            w0[i]=wr[i]+beta*w0[i];
        }
    }
    return k;
}
int main(void){
    int i,n,bw,ic,ind;
    double **A, **C, *b, *x, *y;
    double epsilon=1.0e-14;
    unsigned long t0,t1;
    struct timeval *tv=(struct timeval *)malloc(sizeof(struct timeval));
    struct timezone *tz=(struct timezone *)malloc(sizeof(struct timezone));
    printf("Menu: 1...input the matrix and the vector manually.\n");
    printf("      2...set the matrix and the vector from differential equation.\n");
    printf("Which? : ");
    scanf("%d",&ind);
    printf("Size n = ");
    scanf("%d",&n);
    switch(ind){
    case 1:printf("Bandwidth bw = ");
           scanf("%d",&bw);
           break;
    case 2:bw=1;
           break;
    }
    A=(double **)malloc(sizeof(double **));
    for (i=0;i<=bw;i++){
        A[i]=(double *)malloc(sizeof(double)*(n-i));
    }
    switch(ind){
    case 1:printf("Input the matrix A in band-limited form:\n");fflush(stdout);
           inputbandmat(A,n,bw);
           printf("A:\n");fflush(stdout);
           outbandmat(A,n,bw);
           b=(double *)malloc(sizeof(double)*n);
           printf("Vector in the right-hand side:\n");
           inputvector(b,n);
           break;
    case 2:setmat(A,n);
           setvec(b,n);
    }
    x=(double *)malloc(sizeof(double)*n);
    y=(double *)malloc(sizeof(double)*n);
/* 以下は Gauss の消去法を追加するときのためのもの
    C=(double **)malloc(sizeof(double **));
    for (i=0;i<=bw;i++){
        C[i]=(double *)malloc(sizeof(double)*(n-i));
    }
    copymat(A,C,n,bw); 
*/
/* Gauss の消去法は未装備 */
/* Jacobi 法は A, b を破壊しない */ 
    gettimeofday(tv,tz);
    t0=(tv->tv_sec)*1000000+(tv->tv_usec);
    ic=jacobi(A,b,x,n,bw,epsilon);
    gettimeofday(tv,tz);
    t1=(tv->tv_sec)*1000000+(tv->tv_usec);
    printf("%d microsecond passed\n",t1-t0);
    printf("after %d iterations\n",ic);
    printf("Solution:\n");
    outputvector(x,n);
    printf("Verification:\n");
    mulvector(A,x,y,n,bw);
    outputvector(y,n);
/* Gauss-Seidel 法は A, b を破壊しない */ 
    gettimeofday(tv,tz);
    t0=(tv->tv_sec)*1000000+(tv->tv_usec);
    ic=gaussseidel(A,b,x,n,bw,epsilon);
    gettimeofday(tv,tz);
    t1=(tv->tv_sec)*1000000+(tv->tv_usec);
    printf("%d microsecond passed\n",t1-t0);
    printf("after %d iterations\n",ic);
    printf("Solution:\n");
    outputvector(x,n);
    printf("Verification:\n");
    mulvector(A,x,y,n,bw);
    outputvector(y,n);
/* 共役勾配法は A も b も壊さない */
    gettimeofday(tv,tz);
    t0=(tv->tv_sec)*1000000+(tv->tv_usec);
    ic=conjgrad(A,b,x,n,bw,epsilon);
    gettimeofday(tv,tz);
    t1=(tv->tv_sec)*1000000+(tv->tv_usec);
    printf("%d microsecond passed\n",t1-t0);
    printf("after %d iterations\n",ic);
    printf("Solution:\n");
    outputvector(x,n);
    printf("Verification:\n");
    mulvector(A,x,y,n,bw);
    outputvector(y,n);
/* Gauss の消去法を実装した場合
    for (i=0;i<n;i++){
        free(C[i]);
    }
    free(C);
    for (i=0;i<n;i++){
        free(A[i]);
    }
*/
    free(A);
    free(y);
    free(x);
    free(b);
    free(tz);
    free(tv);
}