# include <ohana.h>

/*
  matrix[0..N-1][0..N-1] is a square matrix to be inverted,
  vector[0..N-1][0..M-1] are several vectors to find solns' 
*/

void svd (double ** matrix, int N, double **vector, int M) {

  int i, j, k;
  double **u, *b, *w, **v, *x;
  double wmax, wmin, term;

  ALLOCATE (u, double *, N + 1);
  /* press routine wants u[1..N][1..N] */
  for (i = 0; i < N; i++) {
    ALLOCATE (u[i+1], double, N + 1);
  }
  for (i = 0; i < N; i++) {
    for (j = 0; j < N; j++) {
      u[i+1][j+1] = matrix[i][j];
    }
  }
  ALLOCATE (v, double *, N + 1);
  for (i = 0; i < N; i++) {
    ALLOCATE (v[i+1], double, N + 1);
    bzero (v[i+1], sizeof(double)*(N+1));
  }
  ALLOCATE (w, double, N + 1);
  bzero (w, sizeof(double)*(N+1));
  ALLOCATE (b, double, N + 1);
  bzero (b, sizeof(double)*(N+1));
  ALLOCATE (x, double, N + 1);
  bzero (x, sizeof(double)*(N+1));

  fprintf (stderr, "sending matrix to svdcmp\n");
  svdcmp (u, N, N, w, v);
  fprintf (stderr, "done with svdcmp\n");
  wmax = 0.0;
  for (j = 1; j <= N; j++) if (w[j] > wmax) wmax = w[j];
  for (j = 1; j <= N; j++) fprintf (stderr, "%10.3e ", w[j]);
  fprintf (stderr, "\n");
  wmin = wmax * 1e-22;
  for (j = 1; j <= N; j++) if (w[j] < wmin) w[j] = 0.0;
  for (j = 1; j <= N; j++) fprintf (stderr, "%10.3e ", w[j]);
  fprintf (stderr, "\n");

  for (i = 0; i < M; i++) { 
    fprintf (stderr, "preping backsub\n");
    for (j = 0; j < N; j++) {
      b[j+1] = vector[j][i];
    }

    fprintf (stderr, "running backsub\n");
    for (j = 1; j <= N; j++) {
      fprintf (stderr, "%10.3e ", b[j]);
    }
    fprintf (stderr, "\n");
    svbksb (u, w, v, N, N, b, x);
    fprintf (stderr, "done with backsub\n");
    for (j = 0; j < N; j++) {
      vector[j][i] = x[j+1];
    }
  }

  /* replace matrix with inverse matrix */
  /* transpose u, invert w */
  for (i = 1; i <= N; i++) {
    for (j = i+1; j <= N; j++) {
      SWAP (u[i][j], u[j][i]);
    }
    if (w[i] != 0) {
      w[i] = 1.0/w[i];
    }
  }
      
  for (i = 1; i <= N; i++) {
    for (j = 1; j <= N; j++) {
      term = 0;
      for (k = 1; k <= N; k++) {
	term += matrix[i-1][j-1] * v[i][k]*w[k]*u[k][j];
      }
      fprintf (stderr, "%6.4f ", term);
    }
    fprintf (stderr, "\n");
  }
  

}

