/* -------------------------------------------------------------
           --------------------
		  S V D
	   --------------------


  This is a tandem translation of a cdc 6600 fortran program to ibm 360
  fortran iv and then to 'C'.  The original routine used short precision
  arithmetic; thus, I have used single precision arithmetic.

  original cdc 6600 programmer=  R. C. Singleton
  360 version by=  J. G. Lewis
  'C' version by k.t. kilty   latest revision October 31, 1988
  -------------------------------------------------------------------

  additional routine needed:  rotate

  -------------------------------------------------------------------


  This subroutine computes the singular value decomposition
  of a real m*n matrix a, i.e. it computes matrices u,s,and v
  such that

             a = u * s * vt
  where
         u is an m*m matrix and ut*u = i, (ut=transpose
                                               of u),
         v is an n*n matrix and vt*v = i, (vt=transpose
                                               of v),
    and  s is an n*n diagonal matrix.

  description of parameters

  a= real array.  a contains the matrix to be decomposed.
     the original data are lost.  If withv=TRUE, then
     the matrix u is computed and stored in the array a.
     a is organized row major, and should work with 'C'
     two-D vectors.

  m,n = integer variables.  The number of rows and columns
        in the matrix stored in a. If m<n, then either
        transpose the matrix a or add rows of zeros to
        increase m to n.

  p = integer variable.  If p>0, then p columns n,...,
      n+p-1 of a are assumed to contain the columns of an m*p
      matrix b.  This matrix is multiplied by ut, and upon
      exit, a contains in these same columns the n*p matrix
      ut*b.  (p>=0)

  withu, withv = integers.  If withu=TRUE, then
     the matrix u is computed and stored in the array a.
     If withv=TRUE, then the matrix v is computed and
     stored in the array v.

  s = real array.  s[0], ..., s[n-1] contain the diagonal
      elements of the matrix s ordered so that s[i]>=s[i+1],
      i=0, ..., n-1.  The s' are the singular values of the
      matrix and satisfy the shifted eigenvalue equations

      a.v=su  a.u=sv  or the s-squared are eigen values of the
      matrix (at).a.

  v = real array.  v contains the matrix v.  If withu
      and withv are not both = TRUE, then the actual
      parameter corresponding to a and v may be the same.

  This original routine is a version of a fortran subroutine
  by Businger and Golub, algorithm 358= singular value
  decomposition of a complex matrix, comm. acm, v. 12,
  no. 10, pp. 564-565 (Oct. 1969).
  with revisions by RC Singleton, May 1972.
 -------------------------------------------------------------------
*/
#define  BEGIN   {
#define  END     }
#define  TRUE    1
#define  FALSE   0

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

void  rotate(float *x,int offset1,int offset2,float cosine,float sine,int count,int cols)
BEGIN
int j,bias1,bias2;
float xx;
      for(j=0; j<count; j++)
      BEGIN
	 bias1=offset1+j*cols;
	 bias2=offset2+j*cols;
         xx=x[bias1];
         x[bias1]=xx*cosine+x[bias2]*sine;
         x[bias2]=x[bias2]*cosine-xx*sine;
      END
END   /* rotate */

int   svd(float *a,float *s,float *v,int m,int n,int p,int withu,int withv)
BEGIN
float r,w,cosine,sine,f,x,epsilon=0.0,g=0.0,*t,y;
float h,q,z,temp;
float eta=1.0e-15,tolerance=1.0e-30;
int   i,j,k,l=0,l1,np,ii,jj,index;
/*
    eta is the machine epsilon (relative accuracy)_
    tolerance is the smallest representable real divided by eta.
*/
      np=n+p;
      t=(float *)calloc((unsigned)n,sizeof(float));
      if (t==NULL) return(-1);
/*
    householder reduction to bidiagonal form
*/
      do   /* while k<n-1 */
      BEGIN
         t[l]=g;
         k=l++;
/*
    elimination of a[i,k], i=k+1, ..., m
*/
         s[k]=0.0;
         z=0.0;
         for(i=k; i<m; i++) z+=a[i*np+k]*a[i*np+k];
         if (z>=tolerance)
	 BEGIN
            g=(float)sqrt((double)z);
            f=a[k*np+k];
            if(f>=0.0) g=-g;
            s[k]=g;
            h=g*(f-g);
            a[k*np+k]=f-g;
            if (k<(np-1))
	    BEGIN
               for(j=l; j<np; j++)
               BEGIN
                  f=0.0;
                  for(i=k; i<m; i++)  f+=a[i*np+k]*a[i*np+j];
                  f/=h;
                  for(i=k; i<m; i++) a[i*np+j]+=f*a[i*np+k];
               END /* loop over j */
	    END /* if k!=np */
	 END /* if z>=tolerance */
/*
     elimination of a[k,j], j=k+2, ..., n
*/
         temp=(float)(fabs((double)s[k])+fabs((double)t[k]));
         if (epsilon<temp) epsilon=temp;
         if(k<(n-1))
	 BEGIN
            z=g=0.0;
            for(j=l; j<n; j++) z+=a[k*np+j]*a[k*np+j];
            if(z>=tolerance)
	    BEGIN
               g=(float)sqrt((double)z);
               f=a[k*np+l];
               if(f>=0.0) g=-g;
               h=g*(f-g);
               a[k*np+l]=f-g;
               for(j=l; j<n; j++) t[j]=a[k*np+j]/h;
               for(i=l; i<m; i++)
               BEGIN
                  f=0.0;
                  for(j=l; j<n; j++) f+=a[k*np+j]*a[i*np+j];
                  for(j=l; j<n; j++) a[i*np+j]+=f*t[j];
               END  /* loop over i */
	    END /* if z>=tolerance */
	 END /* if k<n-1 */
      END while(k<(n-1));
/*
      tolerance for negligible elements
*/
      epsilon*=eta;
/*
      accumulation of transformations
*/
      if(withv==TRUE)
      BEGIN
         k=n-1;
         for(j=0; j<n; j++) v[k*n+j]=0.0;
         v[k*n+k]=1.0;
         l=k--;
         while (k>-1)
	 BEGIN
            if(t[l]!=0.0)
	    BEGIN
               h=a[k*np+l]*t[l];
               for(j=l; j<n; j++)
               BEGIN
                  q=0.0;
                  for(i=l; i<n; i++) q+=a[k*np+i]*v[i*n+j];
                  q/=h;
                  for(i=l; i<n; i++) v[i*n+j]+=q*a[k*np+i];
               END /* loop over j */
	    END /* if t[l]!=0.0 */
            for(j=0; j<n; j++) v[k*n+j]=0.0;
            v[k*n+k]=1.0;
            l=k--;
         END    /* while(k>-1) */
      END /* if withv==TRUE */

      if(withu==TRUE)
      BEGIN
	 k=n-1;
         g=s[k];
         if(g!=0.0) g=1.0/g;
         for(j=k; j<m; j++) a[j*np+k]*=g;
         a[k*np+k]+=1.0;
         l=k--;
         while(k>-1)
	 BEGIN
            for(j=l; j<n; j++) a[k*np+j]=0.0;
            g=s[k];
            if(g!=0.0)
	    BEGIN
               h=a[k*np+k]*g;
               for(j=l; j<n; j++)
               BEGIN
                  q=0.0;
                  for(i=l; i<m; i++) q+=a[i*np+k]*a[i*np+j];
                  q/=h;
                  for(i=k; i<m; i++) a[i*np+j]+=q*a[i*np+k];
               END /* loop over j */
               g=1.0/g;
	    END /* if g!=0.0 */
            for(j=k; j<m; j++) a[j*np+k]*=g;
            a[k*np+k]+=1.0;
            l=k--;
         END   /* while(k>-1) */
      END /* if withu==TRUE */
/*
     qr diagonalization
*/
      k=n-1;
/*
     test for split
*/
      do
      BEGIN  /* end while */
         l=k;
         while((float)fabs((double)t[l])>epsilon)
         BEGIN
	    l--;
            if ((float)fabs((double)s[l])<=epsilon) break;
	 END
/*
     cancellation
*/
         if((float)fabs((double)s[l])<=epsilon)
         BEGIN
            sine=1.0;
	    cosine=0.0;
            l1=l++;
            for(i=l; i<=k; i++)
            BEGIN
               f=sine*t[i];
               t[i]*=cosine;
               if((float)fabs((double)f)>epsilon)
	       BEGIN
                  h=s[i];
                  w=(float)sqrt((double)(f*f+h*h));
                  s[i]=w;
                  cosine=h/w;
                  sine=-f/w;
                  if(withu==TRUE) rotate(a,l1,i,cosine,sine,m,np);
                  if(np>n)
                  BEGIN
                     for(j=n; j<np; j++)
                     BEGIN
                        q=a[l1*np+j];
                        r=a[i*np+j];
                        a[l1*np+j]=q*cosine+r*sine;
                        a[i*np+j]=r*cosine-q*sine;
                     END /* loop over  j */
                  END /* if np!=n */
	       END /* if abs(f)>epsilon */
               else break;
	    END /* loop over i */
         END /* if abs(s[l])<epsilon  */
/*
     test for convergence
*/
         w=s[k];
         if(l==k)
/*
     convergence
*/
	 BEGIN
            if(w<0.0)
            BEGIN
               s[k]=-w;
               if(withv==TRUE) for(j=0; j<n; j++) v[j*n+k]=-v[j*n+k];
            END /* if w<0.0 */
            k--;
	 END /* if l==k */
         else /* if l!=k */
         BEGIN
/*
     origin shift
*/
            x=s[l];
            y=s[k-1];
            g=t[k-1];
            h=t[k];
            f=((y-w)*(y+w)+(g-h)*(g+h))/(2.0*h*y);
            g=(float)sqrt((double)(f*f+1.0));
            if(f<0.0) g=-g;
            f=((x-w)*(x+w)+h*((y/(f+g))-h))/x;
/*
     qr step
*/
            sine=cosine=1.0;
            l1=l+1;
            for(i=l1; i<=k; i++)
            BEGIN
               g=t[i];
               y=s[i];
               h=sine*g;
               g=cosine*g;
               w=(float)sqrt((double)(h*h+f*f));
               t[i-1]=w;
               cosine=f/w;
               sine=h/w;
               f=x*cosine+g*sine;
               g=g*cosine-x*sine;
               h=y*sine;
               y=y*cosine;
               if(withv==TRUE) rotate(v,i-1,i,cosine,sine,n,n);
               w=(float)sqrt((double)(h*h+f*f));
               s[i-1]=w;
               cosine=f/w;
               sine=h/w;
               f=cosine*g+sine*y;
               x=cosine*y-sine*g;
               if(withu==TRUE) rotate(a,i-1,i,cosine,sine,m,np);
               if(n!=np)
	       BEGIN
                  for(j=n; j<np; j++)
                  BEGIN
                     q=a[(i-1)*np+j];
                     r=a[i*np+j];
                     a[(i-1)*np+j]=q*cosine+r*sine;
                     a[i*np+j]=r*cosine-q*sine;
	          END /* loop over j */
	       END /* if n!=np */
            END /* loop over  i */
            t[l]=0.0;
            t[k]=f;
            s[k]=x;
         END /* if l!=k */
      END while(k>-1);
/*
     sort singular values
*/
      for(k=0; k<n; k++)
      BEGIN
         g=s[k]; j=k;
         for(i=k+1; i<n; i++) if(s[i]>=g) BEGIN g=s[i]; j=i; END
         if(j!=k)
	 BEGIN
	    g=s[j];
            s[j]=s[k];
            s[k]=g;
            if(withv==TRUE)
            BEGIN
               for(i=0; i<n; i++)
               BEGIN
                  q=v[i*n+j];
                  v[i*n+j]=v[i*n+k];
                  v[i*n+k]=q;
               END /* loop over i */
            END /* if withv==TRUE */
            if(withu==TRUE)
            BEGIN
               for(i=0; i<m; i++)
               BEGIN
                  q=a[i*np+j];
                  a[i*np+j]=a[i*np+k];
                  a[i*np+k]=q;
               END /* loop over i */
            END /* if withu==TRUE */
            if(np>n)
            BEGIN
               for(i=n; i<np; i++)
               BEGIN
                  q=a[j*np+i];
                  a[j*np+i]=a[k*np+i];
                  a[k*np+i]=q;
               END /* loop over i */
	    END /* if np>n */
	 END /* if j!=k */
      END /* loop over k */
      free(t);
      return(0);
END   /* svd */

main()
/*  To test svd routine */
BEGIN
FILE *fp;
float static x[10]=BEGIN 0,.5,1,1.5,2,2.5,3,3.5,4,4.5 END;
float static t[10]=BEGIN 95,64,50,36,30,0,0,0,0,0 END;
float static a[10*4];
float sum;
float s[4],v[26];
int   i,j,k,tindex,index,ROWS=5,COLS=4;
      fp = fopen("svdtest3.prn","w");
      for(j=0; j<ROWS; j++)
      BEGIN
	 fprintf(fp,"Row %2d elements: x=%9.1f T=%9.1f",j,x[j],t[j]);
	 for(i=0; i<COLS; i++)
	 BEGIN
	    index=j*COLS+i;
	    if (i==0) a[index]=1.0;
	    else if (i==1) a[index]=x[j];
	    else if (i==2) a[index]=x[j]*x[j];
	    else if (i==3) a[index]=x[j]*x[j]*x[j];
	    else a[index]=0.0;
	 END
	 fputs("\n",fp);
      END
      fputs("Matrix A elements:\n\n",fp);
      for(j=0; j<ROWS; j++)
      BEGIN
	 fprintf(fp,"Row %2d elements:",j);
	 for(i=0; i<COLS; i++)
	 BEGIN
	    index=j*COLS+i;
	    fprintf(fp," %9.4f",a[index]);
	 END
	 fputs("\n",fp);
      END
      svd(a,s,v,ROWS,COLS,0,TRUE,TRUE);
      fputs("Matrix U elements:\n\n",fp);
      for(j=0; j<ROWS; j++)
      BEGIN
	 fprintf(fp,"Row %2d elements:",j);
	 for(i=0; i<COLS; i++)
	 BEGIN
	    index=j*COLS+i;
	    fprintf(fp," %9.4f",a[index]);
	 END
	 fputs("\n",fp);
      END
      fputs("Matrix U*Ut elements:\n\n",fp);
      for(j=0; j<ROWS; j++)
      BEGIN
	 fprintf(fp,"Row %2d elements:",j);
	 for(i=0; i<ROWS; i++)
	 BEGIN
	    sum = 0.0;
	    for(k=0; k<COLS; k++) { index=j*COLS+k;
				    tindex=i*COLS+k;
				    sum+=a[index]*a[tindex];
				    }
	    fprintf(fp," %9.4f",sum);

	 END
	 fputs("\n",fp);
      END
      fputs("Matrix Ut*U elements:\n\n",fp);
      for(j=0; j<COLS; j++)
      BEGIN
	 fprintf(fp,"Row %2d elements:",j);
	 for(i=0; i<COLS; i++)
	 BEGIN
	    sum = 0.0;
	    for(k=0; k<ROWS; k++) { index=j+k*COLS;
				    tindex=k*COLS+i;
				    sum+=a[index]*a[tindex];
				    }
	    fprintf(fp," %9.4f",sum);

	 END
	 fputs("\n",fp);
      END
      fputs("Matrix V elements:\n\n",fp);
      for(j=0; j<COLS; j++)
      BEGIN
	 fprintf(fp,"Row %2d elements:",j);
	 for(i=0; i<COLS; i++)
	 BEGIN
	    index=j*COLS+i;
	    fprintf(fp," %9.4f",v[index]);
	 END
	 fputs("\n",fp);
      END
      fputs("Eigenvalues:\n\n",fp);
      for(i=0; i<COLS; i++)
	  fprintf(fp," %9.4f",s[i]);
      fputs("\n",fp);
END   /* svd test routine */