/* This is a sample program to show how to use a function called jacobi to 
   find the eigen values and eigen vectors of a n by n symmetric matrix.
   You enter the dimension n and a symmetric matrix will be generated and
   its eigen values and vectors will be found. To verify the result, it is
   further shown in the main program that the eigen vector matrix, a
   orthogonal matrix, indeed converts the original matrix to a diagonal
   matrix (with all eigen values on the main diagonal). You can enter some 
   value less than 10 as an example to see this diagonalization.

   You need the Jacobi function included in this program to do KLT for feature
   election needed in project 7. When n=256, it takes less than 10 minutes to 
   do 9 iterations to generate results with more than enough accuracy. Note
   that in order to run the function Jacobi properly, you need to use "matrix"
   and "vector" functions (available in this program) to allocate memery space
   for all matrices and vectors that are used in the Jacobi function.
*/
   
#include <math.h>
#include <stdio.h>

prtvtr(v,n)
/* print an n by 1 vector */
float v[];
int n;
{ int i;
  for (i=0; i<n; i++)
    printf("%7.2f ",v[i]);
}

prtmtx(a,m,n)
/* print an n by n matrix */
float **a;
int m,n;
{ int i,j;
  for (i=0; i<m; i++){
    for (j=0; j<n; j++){
      printf("%7.2f ",a[i][j]);
    }
    printf("\n");
  }
}

main()
{
  float **a,**b,*d,**v,**f,**g,x,y;
  float **matrix(),*vector();
  int i,j,k,l,m,n,nrot;
  void jacobi();

  printf("n=");
  scanf("%d",&n);
  printf("\nn=%d\n",n);

  d=vector(0,n-1);
  a=matrix(0,n-1,0,n-1);
  b=matrix(0,n-1,0,n-1);
  v=matrix(0,n-1,0,n-1); 
  f=matrix(0,n-1,0,n-1);
  g=matrix(0,n-1,0,n-1);

  for (i=0; i<n; i++)
    for (j=0; j<n; j++)
      {	x=i; y=j;
	a[i][j]=10.0/(2.0+abs(i-j));
	b[i][j]=a[i][j];
      }
/*
  prtmtx(b,n,n); 
  */

  jacobi(a,n,d,v);

  printf("\nThe eigen values...\n");
  prtvtr(d,n);
/*
  printf("\nThe eigen vectors...\n");
  prtmtx(v,n,n); 
  */
  printf("\n\n");

  for (i=0; i<n; i++)
    for (j=0; j<n; j++){
      f[i][j]=0.0;
      for (k=0; k<n; k++)
	f[i][j]+=b[i][k]*v[k][j];
    }

  for (i=0; i<n; i++)
    for (j=0; j<n; j++){
      g[i][j]=0.0;
      for (k=0; k<n; k++)
	g[i][j]+=v[k][i]*f[k][j];
    }
  prtmtx(g,n,n);
}

void jacobi(x,n,d,vec)
float **x, d[], **vec;
int  n;
{
  int i,j,k,l,m,ic,it,iz;
  float *a,*b,*z,*vector();
  float c,s,t,tr,ta,g,g1,h,sm;

  a=vector(0,n*(n+1)/2);
  b=vector(0,n-1);
  z=vector(0,n-1);

  m=0;
  for (i=0; i<n; i++) 
    for (j=i; j<n; j++) {
      m+=1; 
      a[m]=x[i][j]; 
    }

  for (i=0; i<n; i++) {
    z[i]=0;
    for (j=0; j<n; j++) {
      vec[i][j]=0.0;
      if (i == j) vec[i][j]=1.0;
    }
  }

  iz=1;
  for (i=1; i<=n; i++){
    b[i-1]=d[i-1]=a[iz];          /* b and d hold the diagonal elements */
    iz+=n-i+1;
  }

  for (l=1; l<=9; l++){           /* the main iteration */
    sm=0.0; 
    iz=1;
    for (i=1; i<=n-1; i++){
      for (j=1; j<n-i; j++)
	sm+=fabs(a[iz+j]);        /* sm: sum of all elements above diagonal */
      iz+=n-i+1;
    }
    printf("l=%d, sm=%f\n",l,sm);
    if (sm < 10.e-8) goto l0;

    if (l < 4) tr=0.2*sm/(n*n);
    else tr=0.0;
    iz=1;
    for (i=1; i<=n-1; i++) {
      for (j=i+1; j<=n; j++) {
	g1=a[iz+j-i];
	g=100*g1;
	if (l > 4 && g == 0.0) goto l1;
	if (fabs(g1) <= tr) goto l2;
	h=d[i-1]-d[j-1];
	if (2*fabs(g1) < fabs(h)){
	  t=2*g1/h;
	  c=1.0/sqrt(1+t*t);
	  s=t*c;
	}
	else {
	  t=0.5*h/g1;
	  c=fabs(t)/sqrt(1+t*t);
	  if (t == 0) s=1.0;
	  else s=c/t;
	}
	c=sqrt((1.0+c)/2);
	s=0.5*s/c;
	h=g1*s/c;
	ta=s/(1+c);
	z[i-1]=z[i-1]+h;
	z[j-1]=z[j-1]-h;
	d[i-1]=d[i-1]+h;
	d[j-1]=d[j-1]-h;
	ic=i;
	it=j;
	for (k=1; k<=n; k++) {
	  g=a[ic];
	  h=a[it];
	  a[ic]=g+s*(h-g*ta);
	  a[it]=h-s*(g+h*ta);
	  if (k < i) ic+=n-k;
	  else   ic+=1;
	  if (k < j) it+=n-k;
	  else it+=1;
	}
	for (k=0; k<n; k++) {
	  g=vec[k][i-1];
	  h=vec[k][j-1];
	  vec[k][i-1]=g+s*(h-g*ta);
	  vec[k][j-1]=h-s*(g+h*ta);
	}
	l1:
	a[iz+j-i]=0.0;
	l2:
	g=g;
      }       /* close for loop for (j=i+1, n-1) */
      iz+=n-i+1;
    }       /* close for loop for (i=0, n-2) */

    for (i=0; i<n; i++) {
      b[i]+=z[i];
      d[i]=b[i];
      z[i]=0.0;
    }
  }         /* close for loop for l=1,10 */

  l0:
  printf("rearranging eigen values and vectors in descending order...\n");
  for (i=0; i<n-1; i++)           
    for (j=i+1; j<n; j++) 
      if ( d[i] < d[j] ) {
	g=d[i];
	d[i]=d[j];
	d[j]=g;
	for (k=0; k<n; k++) {
	  g=vec[k][i];
	  vec[k][i]=vec[k][j];
	  vec[k][j]=g;
	}
      }
  return;
}  


float *vector(nl,nh)
int nl,nh;
{
  float *v;

  v=(float *)malloc((unsigned) (nh-nl+1)*sizeof(float));
  if (!v) printf("allocation failure in vector()");
  return v-nl;
}


void free_vector(v,nl,nh)
float *v;
int nl,nh;
{
  free((char*) (v+nl));
}

float **matrix(nrl,nrh,ncl,nch)
int nrl,nrh,ncl,nch;
{ 
  int i;
  float **m;
  m=(float **)malloc((unsigned) (nch-ncl+1)*sizeof(float*));
  if (!m) printf("allocation failure 1 in matrix()");
  m -= nrl;

  for (i=nrl; i<=nrh; i++) {
    m[i]=(float *)malloc((unsigned) (nch-ncl+1)*sizeof(float));
    if (!m[i]) printf("allocation failure 2 in matrix()");
    m[i] -= ncl;
  }
  return m;
}