/* C program to perform singular value decomposition on a
   space-time array with eigenvectors and PCs rotated according
   to the varimax criterion.  Varimax subroutine originally from
   EOF/PC code used at NASA GSFC.

   **NOTE: DO NOT use singular values to compute explained variance
   of EOFs if using varimax rotation!  Compute explained variance of the
   rotated mode by calculating the variance of the raw PCs and dividing 
   by the sum of the variance of all raw PCs.

   Input file is the space-time array in binary format (AT array)

   Output files are:

   Rotated V array (spacedim x spacedim): Normalized vectors (var=1)
   Rotated U array (timedim x timedim): Normalized vectors (var=1)
   Original singular values (sigma) (maxdim)
   Raw PCs (timedim x timedim): Unnormalized Rotated PCs

*/ 


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

#define NR_END 1
#define FREE_ARG char*
#define SIGN(a,b) ((b) >= 0.0 ? fabs(a) : -fabs(a))
static double dmaxarg1,dmaxarg2;
#define DMAX(a,b) (dmaxarg1=(a),dmaxarg2=(b),(dmaxarg1) > (dmaxarg2) ?\
(dmaxarg1) : (dmaxarg2))
static int iminarg1,iminarg2;
#define IMIN(a,b) (iminarg1=(a),iminarg2=(b),(iminarg1) < (iminarg2) ?\
(iminarg1) : (iminarg2))

/* Specify space and time dimensions + number of eigenvector rotations 

MAXDIM is the smaller of TIMEDIM or SPACEDIM
MINDIM is the larger of TIMEDIM or SPACEDIM  
ROTATIONS is the number of eigenvector rotations */

#define TIMEDIM 60
#define SPACEDIM 1000
#define MINDIM 60
#define MAXDIM 1000
#define ROTATIONS 10

void svdcmp(double**, int, int, double[MAXDIM], double**);

void varimax(double **, double **, int, int, int, int); 


double **dmatrix(int,int,int,int);

main(){

/* Specify variables and arrays 

   Integer variables
   m = TIMEDIM
   n = SPACEDIM
   rot = ROTATIONS
   q = smaller of TIMEDIM or SPACEDIM (MINDIM)
   i,j,l,pass = counters

   Arrays

   at = original data in format space-time
   a = original data in format time-space
   v = V array (**Note it's not VT)
   w = Sigma array: Singular values
   rawPC = Unnormalized PCs in format time-time

   temp arrays used for data sorting

*/



  int m,n,q,i,j,l,pass;
  int rot;
  double **at;
  double **a,**v,**rawPC;
  double w[MAXDIM];
  double temp1,temp2[TIMEDIM],temp3[SPACEDIM];
  float temp;
  double hold,sumPC;

  FILE *inPtr, *outPtr1, *outPtr2, *outPtr3, *outPtr4;


  /* Input and output files */ 

  inPtr=fopen("Atarray.bin", "rb");
 
  outPtr1=fopen("Uarray_rot_c.bin", "wb");

  outPtr2=fopen("Varray_rot_c.bin", "wb");

  outPtr3=fopen("Sigmavalues_c.asci", "w");

  outPtr4=fopen("PCsraw__rot_c.bin", "wb");
  

  

  a=dmatrix(1,TIMEDIM,1,SPACEDIM);
  v=dmatrix(1,SPACEDIM,1,SPACEDIM);
  at=dmatrix(1,SPACEDIM,1,TIMEDIM);
  rawPC=dmatrix(1,TIMEDIM,1,TIMEDIM);


  /* Read in data (AT array) */


  m=TIMEDIM;
  n=SPACEDIM;

  for(i=1;i<=TIMEDIM;i++){
    for(j=1;j<=SPACEDIM;j++){
      fread(&temp,sizeof(float),1,inPtr);
      at[j][i]=(double)temp; 
    }
  }

  

  /* Transpose data so it is in the form time-space */

  for(j=1;j<=SPACEDIM;j++){
    for(i=1;i<=TIMEDIM;i++){
      hold=at[j][i];
      a[i][j]=hold;
    }
  } 


  /* Call SVDcmp subroutine */

   printf("To svdcmp\n");   

   svdcmp(a,m,n,w,v); 

   printf("Exit svdcmp\n");


   /* Sort the data from highest to lowest eigenvalue 
      Null space excluded */

   q=MINDIM;


   for(pass=1;pass<q;pass++){
     for(j=1;j<=(q-pass);j++){
       if(w[j]<w[j+1]){
         temp1=w[j];
         for(i=1;i<=m;i++){
           temp2[i]=a[i][j];
	 }
         for(i=1;i<=n;i++){
           temp3[i]=v[i][j];
	 }
         w[j]=w[j+1];
         for(i=1;i<=m;i++){
	   a[i][j]=a[i][j+1];
	 }
         for(i=1;i<=n;i++){
           v[i][j]=v[i][j+1];
	 }
         w[j+1]=temp1;
         for(i=1;i<=m;i++){
           a[i][j+1]=temp2[i];
	 }
         for(i=1;i<=n;i++){
           v[i][j+1]=temp3[i];
	 }
       }
     }
   }


   /*  Call to do varimax rotation on eigenvectors  */

   printf("To varimax rotation\n");

   m=TIMEDIM;
   n=SPACEDIM;
   rot=ROTATIONS;

   varimax(v,a,n,m,rot,rot); 

   printf("Completed rotation on first %d eigenvectors\n", rot);

   /* Project original data onto EOFs to get unnormalized PCs
      Need these to compute updated explained variance for REOFs */


   for(l=1;l<=TIMEDIM;l++){
     for(j=1;j<=TIMEDIM;j++){
       sumPC=0;
       for(i=1;i<=SPACEDIM;i++){
         sumPC=v[i][l]*at[i][j]+sumPC;
       }
       rawPC[j][l]=sumPC;
     }
   }


   /* Output U and V arrays, Singular values, Raw PCs */


   for(j=1;j<=TIMEDIM;j++){
     for(i=1;i<=TIMEDIM;i++){
       temp=(float)a[i][j];
       fwrite(&temp,sizeof(float),1,outPtr1);
     }
   }

   for(j=1;j<=SPACEDIM;j++){
     for(i=1;i<=SPACEDIM;i++){
       temp=(float)v[i][j];
       fwrite(&temp,sizeof(float),1,outPtr2);
     }
   }

   for(i=1;i<=MAXDIM;i++){
     temp=(float)w[i];
     fprintf(outPtr3, "%.4f\n", w[i]);
   } 

   
   for(l=1;l<=TIMEDIM;l++){
     for(j=1;j<=TIMEDIM;j++){
       temp=(float)rawPC[j][l];
       fwrite(&temp,sizeof(float),1,outPtr4);
     }
   }



   printf("SVD Analysis with rotated eigenvectors complete\n");
   

  return 0;

}


double **dmatrix(int nrl, int nrh, int ncl, int nch)
/* allocate a double matrix with subscript range m[nrl..nrh][ncl..nch] */
{
	int i,nrow=nrh-nrl+1,ncol=nch-ncl+1;
	double **m;
	/* allocate pointers to rows */
	m=(double **) malloc((size_t)((nrow+NR_END)*sizeof(double*)));
	m += NR_END;
	m -= nrl;
	/* allocate rows and set pointers to them */
	m[nrl]=(double *) malloc((size_t)((nrow*ncol+NR_END)*sizeof(double)));
	m[nrl] += NR_END;
	m[nrl] -= ncl;
	for(i=nrl+1;i<=nrh;i++) m[i]=m[i-1]+ncol;
	/* return pointer to array of pointers to rows */
	return m;
}

double *dvector(int nl, int nh)
/* allocate a double vector with subscript range v[nl..nh] */
{
	double *v;
	v=(double *)malloc((size_t) ((nh-nl+1+NR_END)*sizeof(double)));
	return v-nl+NR_END;
}

void free_dvector(double *v, int nl, int nh)
/* free a double vector allocated with dvector() */
{
	free((FREE_ARG) (v+nl-NR_END));
}

double pythag(double a, double b)
/* compute (a2 + b2)^1/2 without destructive underflow or overflow */
{
	double absa,absb;
	absa=fabs(a);
	absb=fabs(b);
	if (absa > absb) return absa*sqrt(1.0+(absb/absa)*(absb/absa));
	else return (absb == 0.0 ? 0.0 : absb*sqrt(1.0+(absa/absb)*(absa/absb)));
}

/******************************************************************************/


void svdcmp(double **a, int m, int n, double w[], double **v)  
/*******************************************************************************
Given a matrix a[1..m][1..n], this routine computes its singular value
decomposition, A = U.W.VT.  The matrix U replaces a on output.  The diagonal
matrix of singular values W is output as a vector w[1..n].  The matrix V (not
the transpose VT) is output as v[1..n][1..n].
*******************************************************************************/
{
	int flag,i,its,j,jj,k,l,nm;
	double anorm,c,f,g,h,s,scale,x,y,z,*rv1;

	rv1=dvector(1,n);
	g=scale=anorm=0.0; /* Householder reduction to bidiagonal form */
	for (i=1;i<=n;i++) {
		l=i+1;
		rv1[i]=scale*g;
		g=s=scale=0.0;
		if (i <= m) {
			for (k=i;k<=m;k++) scale += fabs(a[k][i]);
			if (scale) {
				for (k=i;k<=m;k++) {
					a[k][i] /= scale;
					s += a[k][i]*a[k][i];
				}
				f=a[i][i];
				g = -SIGN(sqrt(s),f);
				h=f*g-s;
				a[i][i]=f-g;
				for (j=l;j<=n;j++) {
					for (s=0.0,k=i;k<=m;k++) s += a[k][i]*a[k][j];
					f=s/h;
					for (k=i;k<=m;k++) a[k][j] += f*a[k][i];
				}
				for (k=i;k<=m;k++) a[k][i] *= scale;
			}
		}
		w[i]=scale *g;
		g=s=scale=0.0;
		if (i <= m && i != n) {
			for (k=l;k<=n;k++) scale += fabs(a[i][k]);
			if (scale) {
				for (k=l;k<=n;k++) {
					a[i][k] /= scale;
					s += a[i][k]*a[i][k];
				}
				f=a[i][l];
				g = -SIGN(sqrt(s),f);
				h=f*g-s;
				a[i][l]=f-g;
				for (k=l;k<=n;k++) rv1[k]=a[i][k]/h;
				for (j=l;j<=m;j++) {
					for (s=0.0,k=l;k<=n;k++) s += a[j][k]*a[i][k];
					for (k=l;k<=n;k++) a[j][k] += s*rv1[k];
				}
				for (k=l;k<=n;k++) a[i][k] *= scale;
			}
		}
		anorm = DMAX(anorm,(fabs(w[i])+fabs(rv1[i])));
	}
	for (i=n;i>=1;i--) { /* Accumulation of right-hand transformations. */
		if (i < n) {
			if (g) {
				for (j=l;j<=n;j++) /* Double division to avoid possible underflow. */
					v[j][i]=(a[i][j]/a[i][l])/g;
				for (j=l;j<=n;j++) {
					for (s=0.0,k=l;k<=n;k++) s += a[i][k]*v[k][j];
					for (k=l;k<=n;k++) v[k][j] += s*v[k][i];
				}
			}
			for (j=l;j<=n;j++) v[i][j]=v[j][i]=0.0;
		}
		v[i][i]=1.0;
		g=rv1[i];
		l=i;
	}
	for (i=IMIN(m,n);i>=1;i--) { /* Accumulation of left-hand transformations. */
		l=i+1;
		g=w[i];
		for (j=l;j<=n;j++) a[i][j]=0.0;
		if (g) {
			g=1.0/g;
			for (j=l;j<=n;j++) {
				for (s=0.0,k=l;k<=m;k++) s += a[k][i]*a[k][j];
				f=(s/a[i][i])*g;
				for (k=i;k<=m;k++) a[k][j] += f*a[k][i];
			}
			for (j=i;j<=m;j++) a[j][i] *= g;
		} else for (j=i;j<=m;j++) a[j][i]=0.0;
		++a[i][i];
	}
	for (k=n;k>=1;k--) { /* Diagonalization of the bidiagonal form. */
		for (its=1;its<=30;its++) {
			flag=1;
			for (l=k;l>=1;l--) { /* Test for splitting. */
				nm=l-1; /* Note that rv1[1] is always zero. */
				if ((double)(fabs(rv1[l])+anorm) == anorm) {
					flag=0;
					break;
				}
				if ((double)(fabs(w[nm])+anorm) == anorm) break;
			}
			if (flag) {
				c=0.0; /* Cancellation of rv1[l], if l > 1. */
				s=1.0;
				for (i=l;i<=k;i++) {
					f=s*rv1[i];
					rv1[i]=c*rv1[i];
					if ((double)(fabs(f)+anorm) == anorm) break;
					g=w[i];
					h=pythag(f,g);
					w[i]=h;
					h=1.0/h;
					c=g*h;
					s = -f*h;
					for (j=1;j<=m;j++) {
						y=a[j][nm];
						z=a[j][i];
						a[j][nm]=y*c+z*s;
						a[j][i]=z*c-y*s;
					}
				}
			}
			z=w[k];
			if (l == k) { /* Convergence. */
				if (z < 0.0) { /* Singular value is made nonnegative. */
					w[k] = -z;
					for (j=1;j<=n;j++) v[j][k] = -v[j][k];
				}
				break;
			}
			if (its == 30){
			  printf("no convergence in 30 svdcmp iterations");
			}
			x=w[l]; /* Shift from bottom 2-by-2 minor. */
			nm=k-1;
			y=w[nm];
			g=rv1[nm];
			h=rv1[k];
			f=((y-z)*(y+z)+(g-h)*(g+h))/(2.0*h*y);
			g=pythag(f,1.0);
			f=((x-z)*(x+z)+h*((y/(f+SIGN(g,f)))-h))/x;
			c=s=1.0; /* Next QR transformation: */
			for (j=l;j<=nm;j++) {
				i=j+1;
				g=rv1[i];
				y=w[i];
				h=s*g;
				g=c*g;
				z=pythag(f,h);
				rv1[j]=z;
				c=f/z;
				s=h/z;
				f=x*c+g*s;
				g = g*c-x*s;
				h=y*s;
				y *= c;
				for (jj=1;jj<=n;jj++) {
					x=v[jj][j];
					z=v[jj][i];
					v[jj][j]=x*c+z*s;
					v[jj][i]=z*c-x*s;
				}
				z=pythag(f,h);
				w[j]=z; /* Rotation can be arbitrary if z = 0. */
				if (z) {
					z=1.0/z;
					c=f*z;
					s=h*z;
				}
				f=c*g+s*y;
				x=c*y-s*g;
				for (jj=1;jj<=m;jj++) {
					y=a[jj][j];
					z=a[jj][i];
					a[jj][j]=y*c+z*s;
					a[jj][i]=z*c-y*s;
				}
			}
			rv1[l]=0.0;
			rv1[k]=f;
			w[k]=x;
		}
	}
	free_dvector(rv1,1,n);
}


void varimax(double **X, double **Y, int M, int K, int N, int NM) 

/* THIS SUBROUTINE ROTATES THE VECTORS X USING THE VARIMAX CRITERION
   ON INPUT, X CONTAINS THE N EIGENVECTORS, EACH OF LENGTH M, TO BE ROTATED
   ON OUTPUT, X CONTAINS THE ROTATED SYSTEM */

{

  double H[M],U[M],V[M];
  double T[K];
  double A,B,C,D,VVOLD,VVNEW;
  double PHI,COSPHI,SINPHI;
  double SUMTERM,SUMTERM2,SUMTERM3;
  int P,Q,L,I,MAXITER;
  int exitflag;

  /* Normalize components */


  for(P=1;P<=M;P++){
    SUMTERM=0;
      for(Q=1;Q<=N;Q++){
        SUMTERM=pow(X[P][Q],2)+SUMTERM;
      }
    H[P]=sqrt(SUMTERM);
  }



  for(Q=1;Q<=N;Q++){
    for(P=1;P<=M;P++){
      if(H[P]!=0){
        X[P][Q]=X[P][Q]/H[P];
      }
    }
  }


  /* Varimax iteration for all pairs of vectors */

  
  SUMTERM=0;
  
  for(P=1;P<=M;P++){
    for(Q=1;Q<=N;Q++){
      SUMTERM=pow(X[P][Q],4)+SUMTERM;
    }
  }

  SUMTERM2=0;
  SUMTERM3=0;
 
  for(Q=1;Q<=N;Q++){
    for(P=1;P<=M;P++){
      SUMTERM2=pow(X[P][Q],2)+SUMTERM2;
    }
    SUMTERM3=pow(SUMTERM2,2)+SUMTERM3;
    SUMTERM2=0;
  }

  MAXITER=40;

  VVOLD=M*SUMTERM-SUMTERM3;

  exitflag=0;
  L=1;

  do{

    for(P=N;P>=(N-NM+2);P--){
      for(Q=P-1;Q>=(N-NM+1);Q--){
        
	for(I=1;I<=M;I++){
          U[I]=pow(X[I][P],2)-pow(X[I][Q],2);
          V[I]=2*X[I][P]*X[I][Q];
	}

        SUMTERM=0;
        SUMTERM2=0;
  
        for(I=1;I<=M;I++){
          A=U[I]+SUMTERM;
          B=V[I]+SUMTERM2;
	}

        SUMTERM=0;
        SUMTERM2=0;
 
        for(I=1;I<=M;I++){
          SUMTERM=U[I]*U[I]-V[I]*V[I]+SUMTERM;
          SUMTERM2=U[I]*V[I]+SUMTERM2;
	}

        C=SUMTERM-(A*A-B*B)/(double)M;
        D=2*(SUMTERM2-A*B/(double)M);

        PHI=.25*(double)((atan2(D,C)));
        COSPHI=(double)(cos(PHI));
        SINPHI=(double)(sin(PHI));
 
        for(I=1;I<=M;I++){
          U[I]=X[I][P]*COSPHI+X[I][Q]*SINPHI;
          X[I][Q]=-X[I][P]*SINPHI+X[I][Q]*COSPHI;
	  X[I][P]=U[I];
	}

        for(I=1;I<=K;I++){
          T[I]=Y[I][P]*COSPHI+Y[I][Q]*SINPHI;
	  Y[I][Q]=-Y[I][P]*SINPHI+Y[I][Q]*COSPHI;
          Y[I][P]=T[I];
	}
      }
    }

    SUMTERM=0;

    for(P=1;P<=M;P++){
      for(Q=1;Q<=N;Q++){
	SUMTERM=pow(X[P][Q],4)+SUMTERM;
      }
    }

    SUMTERM2=0;
    SUMTERM3=0;

    for(Q=1;Q<=N;Q++){
      for(P=1;P<=M;P++){
	SUMTERM2=pow(X[P][Q],2)+SUMTERM2;
      }
      SUMTERM3=pow(SUMTERM2,2)+SUMTERM3;
      SUMTERM2=0;
    }

    VVNEW=M*SUMTERM-SUMTERM3;

    if((VVNEW-VVOLD)/VVNEW<(0.0001)){
      exitflag=1;
    }
    else{
      exitflag=0;
    }

    if(L==MAXITER){
      printf("VARIMAX rotation iteration did not coverge\n");
      exitflag=1;
    }

    VVOLD=VVNEW;
    L=L+1;

  }while(L<=MAXITER && exitflag==0);

  
  for(Q=1;Q<=N;Q++){
    for(P=1;P<=M;P++){
      X[P][Q]=X[P][Q]*H[P];
    }
  }

}