//=============================================================================
#include "numericalMath.h"

#include "math.h"

// Foir now, these are just interfaces over top of the Numerical Recipes in C
// code.  In particular, we deal with the fact that these routines take array
// index in the Fortan style of 1 as the first element.
extern "C" int svdcmp( float **a, int m, int n, float *w, float **v );
extern "C" void svbksb( float **u, float *w, float **v, int m, int n,
			float *b, float *x );

bool
numericalMath::singularValueDecomp( matrixMxN &A, vectorN &W, matrixMxN &V )
{
  int i;
  int m=A.dimM();
  int n=A.dimN();
  
  float **a = (float**)malloc( (m+1)*sizeof(float*) );
  for ( i=0; i<m; i++ )
    a[i+1] = A[i]-1;
  
  float **v = (float**)malloc( (n+1)*sizeof(float*) );
  for ( i=0; i<n; i++ )
    v[i+1] = V[i]-1;

  bool ret = !svdcmp( a, m, n, &W[0]-1, v );

  free(a);
  free(v);
  return ret;
}

void 
numericalMath::singularValueBacksub( const matrixMxN &A, const vectorN &W,
				     const matrixMxN &V, const vectorN &B,
				     vectorN &Res )
{
  // Note: I cast away constness on most of the const input parameters because
  // we know that the Numerical Recipes code WON'T modify them
  int i;
  int m=A.dimM();
  int n=A.dimN();
  
  float **a = (float**)malloc( (m+1)*sizeof(float*) );
  for ( i=0; i<m; i++ )
    a[i+1] = (float*)A[i]-1;
  
  float **v = (float**)malloc( (n+1)*sizeof(float*) );
  for ( i=0; i<n; i++ )
    v[i+1] = (float*)V[i]-1;

  svbksb( a, (float*)&W[0]-1, v, m, n, (float*)&B[0]-1, &Res[0]-1 );
  
  free(a);
  free(v);
}