/**
 * @file    svdcmp.cpp
 * @brief   SVD decomposition adapted from NRC
 * @author  Alireza Fathi
 * @author  Frank Dellaert
 */
#include <stdexcept>
#include <math.h>    /* for 'fabs' */
#include <iostream>
#include <vector>
#include "Matrix.h"

using namespace std;

#define SIGN(a,b) ((b) >= 0.0 ? fabs(a) : -fabs(a))
static double sqrarg;
#define SQR(a) ((sqrarg=(a)) == 0.0 ? 0.0 : sqrarg*sqrarg)
static double maxarg1, maxarg2;
#define FMAX(a,b) (maxarg1=(a),maxarg2=(b),(maxarg1) > (maxarg2) ?\
        (maxarg1) : (maxarg2))
static int iminarg1, iminarg2;
#define IMIN(a,b) (iminarg1=(a),iminarg2=(b),(iminarg1) < (iminarg2) ?\
        (iminarg1) : (iminarg2))

/* ************************************************************************* */
/*
 double pythag(double a, double b)
 {
 double absa = 0.0, absb = 0.0;
 absa=fabs(a);
 absb=fabs(b);
 if (absa > absb) return absa*sqrt(1.0+SQR(absb/absa));
 else return (absb == 0.0 ? 0.0 : absb*sqrt(1.0+SQR(absa/absb)));
 }
 */

/* ************************************************************************* */
double pythag(double a, double b) {
	double absa = 0.0, absb = 0.0;
	absa = fabs(a);
	absb = fabs(b);
	if (absa > absb)
		return absa * sqrt(1.0 + SQR(absb/absa));
	else {
		if (absb == 0.0)
			return 0.0;
		else
			return (absb * sqrt(1.0 + SQR(absa/absb)));
	}
}

/* ************************************************************************* */
void svdcmp(double **a, int m, int n, double w[], double **v, bool sort) {
	int flag, i, its, j, jj, k, l, nm;
	double anorm, c, f, g, h, s, scale, x, y, z;

	//vector sizes:
	// w[n] - q-1 passed in
	// a[m] - u-1 passed in
	// v[n] - v-1 passed in

	//Current progress on verifying array bounds:
	// rv1 references have been fixed

	double *rv1 = new double[n];

	g = 0.0;
	scale = 0.0;
	anorm = 0.0;
	for (i = 1; i <= n; i++) {
		l = i + 1;
		rv1[i - 1] = 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 - 1] = 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 - 1];
					}
				}
				for (k = l; k <= n; k++)
					a[i][k] *= scale;
			}
		}
		anorm = FMAX(anorm,(fabs(w[i])+fabs(rv1[i-1])));

	}
	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 - 1];
		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 ((double) (fabs(rv1[l - 1]) + anorm) == anorm) {
					flag = 0;
					break;
				}
				if ((double) (fabs(w[nm]) + anorm) == anorm)
					break;
			}
			if (flag) {
				c = 0.0;
				s = 1.0;
				for (i = l; i <= k; i++) {
					f = s * rv1[i - 1];
					rv1[i - 1] = c * rv1[i - 1];
					if ((double) (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)
				throw(std::domain_error(
						"no convergence in 30 svdcmp iterations"));
			x = w[l];
			nm = k - 1;
			y = w[nm];
			g = rv1[nm - 1];
			h = rv1[k - 1];
			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 - 1];
				y = w[i];
				h = s * g;
				g = c * g;
				z = pythag(f, h);
				rv1[j - 1] = 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 - 1] = 0.0;
			rv1[k - 1] = f;
			w[k] = x;
		}
	}

	if (sort) {
		int *indices1 = new int[n];
		int *indices2 = new int[n];
		for (int i1 = 1; i1 <= n; i1++) {
			indices1[i1] = i1;
			indices2[i1] = i1;
		}

		for (int i1 = 1; i1 <= n; i1++) {
			for (int i2 = i1; i2 <= n; i2++) {
				if (w[i1] < w[i2]) {
					double temp = w[i1];
					w[i1] = w[i2];
					w[i2] = temp;
					int ind = indices2[i1];
					indices2[i1] = indices2[i2];
					indices2[i2] = ind;
				}
			}
		}

		for (int i1 = 1; i1 <= n; i1++) {
			if (indices1[i1] != indices2[i1]) {
				int src = indices1[i1], dst = indices2[i1];
				for (int j = 1; j <= n; j++) {
					double temp = v[j][src];
					v[j][src] = v[j][dst];
					v[j][dst] = temp;
				}
				indices1[dst] = src;
			}
		}
		delete[] indices1;
		delete[] indices2;
	}

	delete[] rv1;

}

/* ************************************************************************* */