#include <basis.h>
#include <vmblock.h>

/************************************************************
* This subroutine computes all eigenvalues and eigenvectors *
* of a real symmetric square matrix A(N,N). On output, ele- *
* ments of A above the diagonal are destroyed. D(N) returns *
* the eigenvalues of matrix A. V(N,N) contains, on output,  *
* the eigenvectors of A by columns. THe normalization to    *
* unity is made by main program before printing results.    *
* NROT returns the number of Jacobi matrix rotations which  *
* were required.                                            *
* --------------------------------------------------------- *
* Ref.:"NUMERICAL RECIPES IN FORTRAN, Cambridge University  *
*       Press, 1986, chap. 11, pages 346-348" [BIBLI 08].   *
*                                                           *
*                         C++ version by J-P Moreau, Paris. *
*                                (www.jpmoreau.fr)          *
************************************************************/
void Jacobi(REAL **A,int N,REAL *D, REAL **V, int *NROT) {
REAL  *B, *Z;
double  c,g,h,s,sm,t,tau,theta,tresh;
int     i,j,ip,iq;

void *vmblock1 = NULL;

  //allocate vectors B, Z
  vmblock1 = vminit();
  B = (REAL *) vmalloc(vmblock1, VEKTOR,  100, 0);  
  Z = (REAL *) vmalloc(vmblock1, VEKTOR,  100, 0);

  for (ip=1; ip<=N; ip++) {  //initialize V to identity matrix
    for (iq=1; iq<=N; iq++)  V[ip][iq]=0; 
    V[ip][ip]=1;
  }  
  for (ip=1; ip<=N; ip++) {
    B[ip]=A[ip][ip];
    D[ip]=B[ip];
    Z[ip]=0;    
  }
  *NROT=0;
  for (i=1; i<=50; i++) {
    sm=0;
    for (ip=1; ip<N; ip++)    //sum off-diagonal elements
      for (iq=ip+1; iq<=N; iq++)
	    sm=sm+fabs(A[ip][iq]);
	if (sm==0) {
	  vmfree(vmblock1);
      return;       //normal return
    }
    if (i < 4)
      tresh=0.2*sm*sm;
    else
      tresh=0;
    for (ip=1; ip<N; ip++) {
	  for (iq=ip+1; iq<=N; iq++) {
        g=100*fabs(A[ip][iq]);
// after 4 sweeps, skip the rotation if the off-diagonal element is small
        if ((i > 4) && (fabs(D[ip])+g == fabs(D[ip])) && (fabs(D[iq])+g == fabs(D[iq])))
		  A[ip][iq]=0;
		else if (fabs(A[ip][iq]) > tresh) {
	      h=D[iq]-D[ip];
	      if (fabs(h)+g == fabs(h))
	        t=A[ip][iq]/h;
		  else {
	        theta=0.5*h/A[ip][iq];  
            t=1/(fabs(theta)+sqrt(1.0+theta*theta));
	        if (theta < 0)  t=-t;
		  }
	      c=1.0/sqrt(1.0+t*t);
	      s=t*c;
          tau=s/(1.0+c);
	      h=t*A[ip][iq];
	      Z[ip] -= h;
	      Z[iq] += h;
	      D[ip] -= h;
	      D[iq] += h;
	      A[ip][iq]=0;
		  for (j=1; j<ip; j++) {
	        g=A[j][ip];
	        h=A[j][iq];
	        A[j][ip] = g-s*(h+g*tau);
	        A[j][iq] = h+s*(g-h*tau);
		  }
		  for (j=ip+1; j<iq; j++) {
	        g=A[ip][j];
	        h=A[j][iq];
	        A[ip][j] = g-s*(h+g*tau);
	        A[j][iq] = h+s*(g-h*tau);
		  }		      
		  for (j=iq+1; j<=N; j++) {
	        g=A[ip][j];
	        h=A[iq][j];
	        A[ip][j] = g-s*(h+g*tau);
		    A[iq][j] = h+s*(g-h*tau);
		  }		  
		  for (j=1; j<=N; j++) {
	        g=V[j][ip];
	        h=V[j][iq];
	        V[j][ip] = g-s*(h+g*tau);
	        V[j][iq] = h+s*(g-h*tau);
		  }		  
          *NROT=*NROT+1;
		} //end ((i.gt.4)...else if
      } // main iq loop
    } // main ip loop
    for (ip=1; ip<=N; ip++) {
      B[ip] += Z[ip];
      D[ip]=B[ip];
      Z[ip]=0;
    }
  } //end of main i loop
  printf("\n 50 iterations !\n");
  vmfree(vmblock1);
  return;  //too many iterations
}    

// end of file ujacobi.cpp