#include "moss.h"
#include "nrutil.h"

#define NRANSI
int 
_mossNRsvdcmp(float **a, int m, int n, float w[], float **v) {
  char me[]="_mossNRsvdcmp", err[128];
  float pythag(float a, float b);
  int flag,i,its,j,jj,k,l,nm;
  float anorm,c,f,g,h,s,scale,x,y,z,*rv1;
  
  rv1=vector(1,n);
  g=scale=anorm=0.0;
  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=FMAX(anorm,(fabs(w[i])+fabs(rv1[i])));
  }
  for (i=n;i>=1;i--) {
    if (i < n) {
      if (g) {
	for (j=l;j<=n;j++)
	  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--) {
    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--) {
    for (its=1;its<=30;its++) {
      flag=1;
      for (l=k;l>=1;l--) {
	nm=l-1;
	if ((float)(fabs(rv1[l])+anorm) == anorm) {
	  flag=0;
	  break;
	}
	if ((float)(fabs(w[nm])+anorm) == anorm) break;
      }
      if (flag) {
	c=0.0;
	s=1.0;
	for (i=l;i<=k;i++) {
	  f=s*rv1[i];
	  rv1[i]=c*rv1[i];
	  if ((float)(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) {
	if (z < 0.0) {
	  w[k] = -z;
	  for (j=1;j<=n;j++) v[j][k] = -v[j][k];
	}
	break;
      }
      if (its == 30) {
	sprintf(err, "%s: no convergence in 30 svdcmp iterations", me);
	biffAdd(MOSS, err);
	return 1;
      }
      x=w[l];
      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;
      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;
	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_vector(rv1,1,n);
  return 0;
}
#undef NRANSI

int
mossSVD(float *U, float *W, float *V, float *matx, int M, int N) {
  char me[]="mossSVD", err[128];
  float **nrU, *nrW, **nrV;
  int problem, i;

  /* <rant> The stupid tricks we play in order to interface with
     incredibly annoying Numerical Recipes code.  When I was a Cornell
     undergrad, I was interested in a programming job in the Astronomy
     department.  The first thing that Saul Teukolsky (one of the
     Numerical Recipes authors) asked was "What's your GPA?".  I was
     confused.  If programming ability is the real question at hand,
     this was about as sensible as asking "How much do you weigh?"  As
     soon as I admitted to getting a B+ in a previous previous physics
     class, he waved me away.  His loss.  This alone is sufficient
     evidence that he is a narrow-minded pissant in matters of
     progamming style.  This tired claptrap (which the Numerical
     Recipes authors spill) about how arrays make more sense when they
     are one-offset instead of zero-offset is an infuriatingly thin
     veil for the fact that these people are dinosaurs and FORTRAN
     apologists at heart.  For them to produce human-readable bug-free
     idiomatic C code would be as easy as performing self-trepanation
     after drinking two pots of coffee. </rant> */

  /* allocate arrays for the Numerical Recipes code to write into */
  nrU = (float **)malloc((M+1)*sizeof(float*));
  nrW = (float *)malloc((N+1)*sizeof(float));
  nrV = (float **)malloc((N+1)*sizeof(float*));
  problem = !(nrU && nrW && nrV);
  if (!problem) {
    problem = 0;
    for (i=1; i<=M; i++) {
      nrU[i] = (float *)malloc((N+1)*sizeof(float));
      problem |= !nrU[i];
    }
    for(i=1; i<=N; i++) {
      nrV[i] = (float *)malloc((N+1)*sizeof(float));
      problem |= !nrV[i];
    }
  }
  if (problem) {
    sprintf(err, "%s: couldn't allocate arrays", me);
    biffAdd(MOSS, err); return 1;
  }

  /* copy from given matx into nrU */
  for (i=1; i<=M; i++) {
    memcpy(&(nrU[i][1]), matx + N*(i-1), N*sizeof(float));
  }
  
  /*
  printf("%s: given matx:\n", me);
  for (i=1; i<=M; i++) {
    printf("%s:", me);
    for (j=1; j<=N; j++) {
      printf(" %g", nrU[i][j]);
    }
    printf("\n");
  }
  printf("%s:\n", me);
  */

  /* HERE IT IS: do SVD */
  if (_mossNRsvdcmp(nrU, M, N, nrW, nrV)) {
    sprintf(err, "%s: trouble in core SVD calculation", me);
    biffAdd(MOSS, err); return 1;
  }
  /*
  printf("%s: svdcmp returned U:\n", me);
  for (i=1; i<=M; i++) {
    for (j=1; j<=N; j++) {
      printf(" %g", -nrU[i][j]);
    }
    printf("\n");
  }
  printf("%s:\n", me);
  printf("%s: svdcmp returned W:\n", me);
  for (i=1; i<=N; i++) {
    printf(" %g", nrW[i]);
  }
  printf("\n");
  printf("%s:\n", me);
  printf("%s: svdcmp returned V:\n", me);
  for (i=1; i<=N; i++) {
    for (j=1; j<=N; j++) {
      printf(" %g", -nrV[i][j]);
    }
    printf("\n");
  }
  printf("%s:\n", me);
  */
  
  /* copy results into caller's arrays */
  for (i=1; i<=M; i++) {
    memcpy(U + N*(i-1), &(nrU[i][1]), N*sizeof(float));
  }
  memcpy(W, &(nrW[1]), N*sizeof(float));
  for (i=1; i<=N; i++) {
    memcpy(V + N*(i-1), &(nrV[i][1]), N*sizeof(float));
  }

  /*
  printf("%s: we will return U:\n", me);
  for (i=0; i<=M-1; i++) {
    for (j=0; j<=N-1; j++) {
      printf(" %g", U[j+N*i]);
    }
    printf("\n");
  }
  printf("%s:\n", me);
  printf("%s: we will return W:\n", me);
  for (i=0; i<=N-1; i++) {
    printf(" %g", W[i]);
  }
  printf("\n");
  printf("%s:\n", me);
  printf("%s: we will return V:\n", me);
  for (i=0; i<=N-1; i++) {
    for (j=0; j<=N-1; j++) {
      printf(" %g", V[j+N*i]);
    }
    printf("\n");
  }
  printf("%s:\n", me);
  */
  
  /* free Numerical Recipes arrays */
  for (i=1; i<=M; i++)
    free(nrU[i]);
  free(nrU);
  free(nrW);
  for (i=1; i<=N; i++)
    free(nrV[i]);
  free(nrV);
  
  return 0;
}

int
mossPseudoInverse(float *inv, float *matx, int M, int N) {
  char me[]="_mossPseudoInverse", err[128];
  float *U, *W, *V, ans;
  int problem, i, j, k;

  /*
  printf("%s: given M=%d, N=%d, matx=:\n", me, M, N);
  for (i=0; i<=M-1; i++) {
    printf("%s:", me);
    for (j=0; j<=N-1; j++) {
      printf(" %g", matx[j + N*i]);
    }
    printf("\n");
  }
  printf("%s:\n", me);
  */
  U = (float *)malloc(M*N*sizeof(float));
  W = (float *)malloc(N*sizeof(float));
  V = (float *)malloc(N*N*sizeof(float));
  if (!(U && W && V)) {
    sprintf(err, "%s: couldn't alloc matrices", me);
    biffAdd(MOSS, err); return 1;
  }
  if (mossSVD(U, W, V, matx, M, N)) {
    sprintf(err, "%s: trouble in SVD computation", me);
    biffAdd(MOSS, err); return 1;
  }
  problem = 0;
  for (i=0; i<=N-1; i++) {
    if (fabs(W[i]) < MOSS_TINYVAL) {
      sprintf(err, "%s: abs(W[%d]) = %g < %g = tiny", 
	      me, i, fabs(W[i]), MOSS_TINYVAL);
      biffAdd(MOSS, err);
      problem = 1;
    }
  }
  if (problem) {
    sprintf(err, "%s: no pseudo-inverse due to small singular values", me);
    biffAdd(MOSS, err);
    return 1;
  }
  for (i=0; i<=N-1; i++) {
    for (j=0; j<=M-1; j++) {
      ans = 0;
      for (k=0; k<=N-1; k++) {
	/* in V: row fixed at i, k goes through columns */
	/* in U^T: column fixed at j, k goes through rows ==>
	   in U: row fixed at j, k goes through columns */
	ans += V[k + N*i]*U[k + N*j]/W[k];
      }
      inv[j + M*i] = ans;
    }
  }
  free(U);
  free(W);
  free(V);
  /*
  printf("%s: returning inv:\n", me);
  for (i=0; i<=N-1; i++) {
    for (j=0; j<=M-1; j++) {
      printf(" %g", inv[j + M*i]);
    }
    printf("\n");
  }
  printf("%s:\n", me);
  */
  return 0;
}