/**
 * \file svd.c
 * \brief This file contains SVD implementation.
 *
 * \remark I have used an existing solution with unclear license. So far let it be so. In fact this solution is a reworked solution from
 * "Numerical Recipies in C", so it it will be easy to re-implement it to make it absolutely clean.
 *
 *
 *  \ingroup corefiles
 *  \date Jun 16, 2009
 *  \author Alexander Pimenov based on code form "Numerical Recipies in C" and looking over Tina code
 */
#include "global.h"

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "algebra.h"
#include "svd.h"



#define Sign(u,v)               ( (v)>=0.0 ? Abs(u) : -Abs(u) )

#define Max(u,v)                ( (u)>(v)?  (u) : (v) )

#define Abs(x)                  ( (x)>0.0?  (x) : (-(x)) )



/**
 * \brief This function does the singular vector decomposition.
 *
 *
 *
 *
 * For details see: http://en.wikipedia.org/wiki/Singular_value_decomposition
 *
 *
 *
 *
 * \param[in,out] A   The input matrix to be decomposed
 * \param[out] W
 * \param[out] V
 *
 */
void singularVectorDecompose(Matrix *A, Matrix *W, Matrix *V)
{
#ifdef ASSERTS
	 if (A == NULL || W == NULL || V == NULL)
		 DOASSERT("NULL input Matrix\n");

	 if (W->m != 1 || W->n != A->n )
		 DOASSERT("Matrix W has wrong size\n");

	 if (A->n != V->m || A->n != V->n)
		 DOASSERT("Matrix V has wrong size\n");
	 // TODO: Add asserts on matrix sizes
#endif


	 int n = A->n;
	 int m = A->m;
     /* BUG `nm' may be used uninitialized in this function */
     int     flag, i, its, j, jj, k, l, nm, nm1 = n - 1, mm1 = m - 1;
     double  c, f, h, s, x, y, z;
     double  anorm = 0.0, g = 0.0, scale = 0.0;
     double *rv1;

     if (m < n)
         printf("SVDCMP: You must augment A with extra zero rows");
     rv1 = (double *) dv_malloc(n * sizeof(double));

     /* Householder reduction to bidigonal form */
     for (i = 0; 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(ELEM(A,k,i));
             if (scale)
             {
                 for (k = i; k < m; k++)
                 {
                	 ELEM(A,k,i) /= scale;
                     s += ELEM(A,k,i) * ELEM(A,k,i);
                 }
                 f = ELEM(A,i,i);
                 g = -Sign(sqrt(s), f);
                 h = f * g - s;
                 ELEM(A,i,i) = f - g;
                 if (i != nm1)
                 {
                     for (j = l; j < n; j++)
                     {
                         for (s = 0.0, k = i; k < m; k++)
                             s += ELEM(A,k,i) * ELEM(A,k,j);
                         f = s / h;
                         for (k = i; k < m; k++)
                        	 ELEM(A,k,j) += f * ELEM(A,k,i);
                     }
                 }
                 for (k = i; k < m; k++)
                	 ELEM(A,k,i) *= scale;
             }
         }
         ELEM(W,0,i) = scale * g;
         g = s = scale = 0.0;
         if (i < m && i != nm1)
         {
             for (k = l; k < n; k++)
                 scale += fabs(ELEM(A,i,k));
             if (scale)
             {
                 for (k = l; k < n; k++)
                 {
                	 ELEM(A,i,k) /= scale;
                     s += ELEM(A,i,k) * ELEM(A,i,k);
                 }
                 f = ELEM(A,i,l);
                 g = -Sign(sqrt(s), f);
                 h = f * g - s;
                 ELEM(A,i,l) = f - g;
                 for (k = l; k < n; k++)
                     rv1[k] = ELEM(A,i,k) / h;
                 if (i != mm1)
                 {
                     for (j = l; j < m; j++)
                     {
                         for (s = 0.0, k = l; k < n; k++)
                             s += ELEM(A,j,k) * ELEM(A,i,k);
                         for (k = l; k < n; k++)
                        	 ELEM(A,j,k) += s * rv1[k];
                     }
                 }
                 for (k = l; k < n; k++)
                	 ELEM(A,i,k) *= scale;
             }
         }
         anorm = Max(anorm, (fabs(ELEM(W, 0, i)) + fabs(rv1[i])));
     }

     /* Accumulation of right-hand transformations */
     for (i = n - 1; i >= 0; i--)
     {
         if (i < nm1)
         {
             if (g)
             {
                 /* double division to avoid possible underflow */
                 for (j = l; j < n; j++)
                     ELEM(V, j, i) = (ELEM(A, i, j) / ELEM(A, i, l)) / g;
                 for (j = l; j < n; j++)
                 {
                     for (s = 0.0, k = l; k < n; k++)
                         s += ELEM(A, i, k) * ELEM(V, k, j);
                     for (k = l; k < n; k++)
                         ELEM(V, k, j) += s * ELEM(V, k, i);
                 }
             }
             for (j = l; j < n; j++)
                 ELEM(V, i, j) = ELEM(V, j, i) = 0.0;
         }
         ELEM(V, i, i) = 1.0;
         g = rv1[i];
         l = i;
     }
     /* Accumulation of left-hand transformations */
     for (i = n - 1; i >= 0; i--)
     {
         l = i + 1;
         g = ELEM(W, 0, i);
         if (i < nm1)
             for (j = l; j < n; j++)
                 ELEM(A, i, j) = 0.0;
         if (g)
         {
             g = 1.0 / g;
             if (i != nm1)
             {
                 for (j = l; j < n; j++)
                 {
                     for (s = 0.0, k = l; k < m; k++)
                         s += ELEM(A, k, i) * ELEM(A, k, j);
                     f = (s / ELEM(A, i, i)) * g;
                     for (k = i; k < m; k++)
                         ELEM(A, k, j) += f * ELEM(A, k, i);
                 }
             }
             for (j = i; j < m; j++)
                 ELEM(A, j, i) *= g;
         } else
             for (j = i; j < m; j++)
                 ELEM(A, j, i) = 0.0;
         ++ELEM(A, i, i);
     }
     /* diagonalization of the bidigonal form */
     for (k = n - 1; k >= 0; k--)
     {                           /* loop over singlar values */
         for (its = 0; its < 30; its++)
         {                       /* loop over allowed iterations */
             flag = 1;
             for (l = k; l >= 0; l--)
             {                   /* test for splitting */
                 nm = l - 1;     /* note that rv1[l] is always zero */
                 if (Abs(rv1[l]) + anorm == anorm)
                 {
                     flag = 0;
                     break;
                 }
                 if (fabs(ELEM(W,0,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];
                     if (Abs(f) + anorm != anorm)
                     {
                         g = ELEM(W, 0, i);
                         h = getLengthStable(f, g);
                         ELEM(W, 0, i) = h;
                         h = 1.0 / h;
                         c = g * h;
                         s = (-f * h);
                         for (j = 0; j < m; j++)
                         {
                             y = ELEM(A, j, nm);
                             z = ELEM(A, j, i);
                             ELEM(A, j, nm) = y * c + z * s;
                             ELEM(A, j, i) = z * c - y * s;
                         }
                     }
                 }
             }
             z = ELEM(W, 0, k);
             if (l == k)
             {                   /* convergence */
                 if (z < 0.0)
                 {
                	 ELEM(W, 0, k) = -z;
                     for (j = 0; j < n; j++)
                         ELEM(V, j, k) = (-ELEM(V, j, k));
                 }
                 break;
             }
             if (its == 30)
                 printf("No convergence in 30 SVDCMP iterations");
             x = ELEM(W, 0, l);           /* shift from bottom 2-by-2 minor */
             nm = k - 1;
             y = ELEM(W, 0, nm);
             g = rv1[nm];
             h = rv1[k];
             f = ((y - z) * (y + z) + (g - h) * (g + h)) / (2.0 * h * y);
             g = getLengthStable(f, 1.0);
             /* next QR transformation */
             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 = ELEM(W, 0, i);
                 h = s * g;
                 g = c * g;
                 z = getLengthStable(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 = y * c;
                 for (jj = 0; jj < n; jj++)
                 {
                     x = ELEM(V, jj, j);
                     z = ELEM(V, jj, i);
                     ELEM(V, jj, j) = x * c + z * s;
                     ELEM(V, jj, i) = z * c - x * s;
                 }
                 z = getLengthStable(f, h);
                 ELEM(W, 0, j) = z;       /* rotation can be arbitrary id 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 = 0; jj < m; jj++)
                 {
                     y = ELEM(A, jj, j);
                     z = ELEM(A, jj, i);
                     ELEM(A, jj, j) = y * c + z * s;
                     ELEM(A, jj, i) = z * c - y * s;
                 }
             }
             rv1[l] = 0.0;
             rv1[k] = f;
             ELEM(W, 0, k) = x;
         }
     }
     dv_free(rv1);
 }
