/*----------------------------------------*/
/* ヤコビ法による固有値問題の解法         */
/*----------------------------------------*/

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

#define n      5        /*　固有ベクトルの次元　*/ 
#define max_it 10000    /*　最大計算回数　　　　*/
#define eps    1e-10   /*　誤差の閾値          */

int main(void)
{
  int i,j,k,jj,kk,num;
  int jmax,kmax;
  double error, aa, theta, diag;
  double matrix_a[n][n]={{2.,-1.,0.,0.,0.},{-1.,2.,-1.,0.,0.},{0.,-1.,2.,-1.,0.},{0.,0.,-1.,2.,-1.},{0.,0.,0.,-1.,2.}};
  double matrix_a_new[n][n];
  double r[n][n], r_inv[n][n], r_stack[n][n], r_stack_new[n][n];
  
  num=0; /* 繰り返し回数初期化 */

  for( j = 0; j < n; j++){
    for( k = 0; k < n; k++){
      if( j != k){
	r[j][k]=0.;  r_inv[j][k]=0.; r_stack[j][k]=0.;}
      else{
	r[j][k]=1.;  r_inv[j][k]=1.; r_stack[j][k]=1.;}
    }
  }

  for(i=0; i < max_it; i++){    /* i < max_it　である限りループ */

    num+=1; error=0;  /* numを1増やし、errorを初期化 */

    /* search for max component */
    aa=0.; jmax=1; kmax=0;
    for(j=0; j < n; j++){
      for(k=0; k < n; k++){
	if( k != j) {
	  if(aa < fabs(matrix_a[j][k])){ jmax=j; kmax=k; aa = fabs(matrix_a[j][k]);}
	}
      }
    }

    printf("jmax= %d   kmax= %d \n", jmax,kmax);

    /* sum of the diagonal components */
    diag=0.;
    for(j=0; j < n; j++){
      diag += fabs(matrix_a[j][j]);
    }
    
    if( aa/diag < eps ) break; /* errorが十分小さければループを出る */

    printf("error= %.16e \n", aa / diag);


    /* set up kk,kl,lk,ll components of r */
    if( fabs( matrix_a[kmax][kmax]- matrix_a[jmax][jmax] ) < 1e-16 ){
      theta = atan(1.); }
    else{
      theta = 0.5 * atan( 2.* matrix_a[jmax][kmax]/(matrix_a[kmax][kmax]- matrix_a[jmax][jmax]));
    }

    for( j = 0; j < n; j++){
      for( k = 0; k < n; k++){
	if( j != k){
	  r[j][k]=0.;  r_inv[j][k]=0.;}
	else{
	  r[j][k]=1.;  r_inv[j][k]=1.;}
      }
    }

    r[jmax][jmax] =  cos(theta);
    r[jmax][kmax] =  sin(theta);
    r[kmax][jmax] = -sin(theta);
    r[kmax][kmax] =  cos(theta);

    r_inv[jmax][jmax] =  cos(theta);
    r_inv[jmax][kmax] = -sin(theta);
    r_inv[kmax][jmax] =  sin(theta);
    r_inv[kmax][kmax] =  cos(theta);



    /* calculate matrix_a_new */
    for(j=0; j < n; j++){
      for(k=0; k < n; k++){

	matrix_a_new[j][k]=0.;

	for(jj=0; jj < n; jj++){
	  for(kk=0; kk < n; kk++){
	    matrix_a_new[j][k] += r_inv[j][jj] * matrix_a[jj][kk] * r[kk][k] ;  
	  }
	}

      }
    }

    /* copy to matrix_a */
    for(j=0; j < n; j++){
      for(k=0; k < n; k++){
	    matrix_a[j][k] = matrix_a_new[j][k];  
      }
    }

   
    /* calculate r_stack_new */
    for(j=0; j < n; j++){
      for(k=0; k < n; k++){
	r_stack_new[j][k]=0.;
	for(jj=0; jj < n; jj++){
	  r_stack_new[j][k] += r_stack[j][jj] * r[jj][k];
	}
      }
    }
   
    
    /* copy to r_stack */
    for(j=0; j < n; j++){
       for(k=0; k < n; k++){
	 r_stack[j][k] = r_stack_new[j][k];
      }
    }

  }

  printf("number of iteration = %d\n",num);



  for(i=0; i < n ; i++){
    printf("eigen value lambda%d =%f\n",i, matrix_a[i][i]);
    printf("eigen vector v%d\n", i);
    for(j=0; j < n ; j++){    
    printf("  %d    %f \n",j, r_stack[j][i]);
    }
    printf("\n");
  }

  return (0);
}