#include <../source/TAL/objbase.h>

/* Retuns memAlloc()'ed block */
matrix_p V_GLOBAL
matAlloc(u32_t rows,u32_t cols)
{
	matrix_t *m;
	u32_t i,size;
	if(!rows || !cols)
	{
#ifdef	CHECKED_BUILD
		conlPrintf(LOG_ERROR,"matAlloc(%d,%d): incorrect parameter(s).\n",
			rows,cols);
#endif	/* CHECKED_BUILD */
		return NULL;
	}
	size=
		sizeof(matrix_t)+
		rows*sizeof(mvec_t*)+
		rows*cols*sizeof(mvec_t);
	m=(matrix_t*)memAlloc(size);
	m->rows=rows;
	m->cols=cols;
	m->cell=(mvec_t**)(m+1);
	m->data=(mvec_t*)(m->cell+rows);
	m->cell[0]=m->data;
	for(i=1;i<rows;i++)
		m->cell[i]=m->cell[i-1]+cols;
	return m;
}

void V_GLOBAL
matCopy(matrix_t *dest,matrix_t *src)
{
	u32_t i;
	if(!dest || !src)
	{
#ifdef	CHECKED_BUILD
		conlPrintf(LOG_ERROR,"matCopy(%X,%X): incorrect parameter(s).\n",
			dest,src);
#endif	/* CHECKED_BUILD */
		return;
	}
	V_ASSERT(dest->rows==src->rows && dest->cols==src->cols);
	if(src->rows!=dest->rows || src->cols!=dest->cols)
	{
#ifdef	CHECKED_BUILD
		conlPrintf(LOG_ERROR,
			"matCopy(): matrices have different size (%ux%u and %ux%u).\n",
			dest->rows,dest->cols,src->rows,src->cols);
#endif	/* CHECKED_BUILD */
		return;
	}
	dest->cell[0]=dest->data;
	for(i=1;i<dest->rows;i++)
		dest->cell[i]=dest->cell[i-1]+dest->cols;
	for(i=0;i<src->rows;i++)
		memCopy32(dest->cell[i],src->cell[i],(src->cols*sizeof(mvec_t))>>2);
}

/* Retuns memAlloc()'ed block */
matrix_p V_GLOBAL
matDuplicate(matrix_t *ref)
{
	u32_t i;
	matrix_t *d;
	if(!ref)
	{
#ifdef	CHECKED_BUILD
		conlPuts(LOG_ERROR,"matDuplicate(NULL): incorrect parameter.\n");
#endif	/* CHECKED_BUILD */
		return NULL;
	}
	d=matAlloc(ref->rows,ref->cols);
	/* We can't copy all block because pointers in ref may be swapped */
	for(i=0;i<ref->rows;i++)
		memCopy32(d->cell[i],ref->cell[i],(ref->cols*sizeof(mvec_t))>>2);
	return d;
}

void	V_GLOBAL
matCopyRow(vector_t *dst,matrix_t *m,u32_t row)
{
	if(!dst || !m || row==0 || row>m->rows ||  dst->size!=m->cols)
	{
#ifdef	CHECKED_BUILD
		conlPrintf(LOG_ERROR,"matCopyRow(%X,%X,%d): incorrect parameter(s).\n",
			dst,m,row);
#endif	/* CHECKED_BUILD */
		return;
	}
	memCopy32(dst->elements,m->cell[row],m->cols*(sizeof(mvec_t)>>2));
}

void	V_GLOBAL
matCopyCol(vector_t *dst,matrix_t *m,u32_t col)
{
	u32_t r;
	if(!dst || !m || col==0 || col>m->cols ||  dst->size!=m->rows)
	{
#ifdef	CHECKED_BUILD
		conlPrintf(LOG_ERROR,"matCopyCol(%X,%X,%d): incorrect parameter(s).\n",
			dst,m,col);
#endif	/* CHECKED_BUILD */
		return;
	}
	for(r=0;r<m->rows;r++)
		dst->elements[r]=m->cell[r][col];
}

vector_p	V_GLOBAL
matDuplicateRow(matrix_t *m,u32_t row)
{
	vector_p dst;
	dst=vecAlloc(m->cols);
	matCopyRow(dst,m,row);
	return dst;
}

vector_p	V_GLOBAL
matDuplicateCol(matrix_t *m,u32_t col)
{
	vector_t *dst;
	dst=vecAlloc(m->rows);
	matCopyCol(dst,m,col);
	return dst;
}

void	V_GLOBAL
matUnit(matrix_t *m)
{
	u32_t i,min;
	if(!m)
	{
#ifdef	CHECKED_BUILD
		conlPuts(LOG_ERROR,"matUnit(NULL): incorrect parameter.\n");
#endif	/* CHECKED_BUILD */
		return;
	}
	matZero(m);
	min=(m->rows > m->cols) ? m->cols : m->rows;
	for(i=0;i<min;i++)
		m->cell[i][i]=1.0;
}

void	V_GLOBAL
matAdd(matrix_t *dest,matrix_t *m1,matrix_t *m2)
{
	u32_t i,j;
	if(!dest || !m1 || !m2)
	{
#ifdef	CHECKED_BUILD
		conlPrintf(LOG_ERROR,"matAdd(%X,%X,%X): incorrect parameter(s).\n",
			dest,m1,m2);
#endif	/* CHECKED_BUILD */
		return;
	}
	if(m1->rows!=m2->rows || m1->cols!=m2->cols ||
		dest->rows!=m1->rows || dest->cols!=m1->cols)
	{
#ifdef	CHECKED_BUILD
		conlPrintf(LOG_ERROR,"matAdd(%X,%X,%X): matrices with different sizes.\n",
			dest,m1,m2);
#endif	/* CHECKED_BUILD */
		return;
	}
	for(i=0;i<dest->rows;i++)
		for(j=0;j<dest->cols;j++)
			dest->cell[i][j]=m1->cell[i][j]+m2->cell[i][j];
}

void	V_GLOBAL
matSub(matrix_t *dest,matrix_t *m1,matrix_t *m2)
{
	u32_t i,j;
	if(!dest || !m1 || !m2)
	{
#ifdef	CHECKED_BUILD
		conlPrintf(LOG_ERROR,"matSub(%X,%X,%X): incorrect parameter(s).\n",
			dest,m1,m2);
#endif	/* CHECKED_BUILD */
		return;
	}
	if(m1->rows!=m2->rows || m1->cols!=m2->cols ||
		dest->rows!=m1->rows || dest->cols!=m1->cols)
	{
#ifdef	CHECKED_BUILD
		conlPrintf(LOG_ERROR,"matSub(%X,%X,%X): matrices with different sizes.\n",
			dest,m1,m2);
#endif	/* CHECKED_BUILD */
		return;
	}
	for(i=0;i<dest->rows;i++)
		for(j=0;j<dest->cols;j++)
			dest->cell[i][j]=m1->cell[i][j]-m2->cell[i][j];
}

void	V_GLOBAL
matNegate(matrix_t *dest,matrix_t *m)
{
	u32_t i,j;
	if(!dest || !m)
	{
#ifdef	CHECKED_BUILD
		conlPrintf(LOG_ERROR,"matNegate(%X,%X): incorrect parameter(s).\n",
			dest,m);
#endif	/* CHECKED_BUILD */
		return;
	}
	if(dest->rows!=m->rows || dest->cols!=m->cols)
	{
#ifdef	CHECKED_BUILD
		conlPrintf(LOG_ERROR,"matNegate(%X,%X): matrices with different sizes.\n",
			dest,m);
#endif	/* CHECKED_BUILD */
		return;
	}
	for(i=0;i<dest->rows;i++)
		for(j=0;j<dest->cols;j++)
			dest->cell[i][j]=-m->cell[i][j];
}

vboolean	V_GLOBAL
matCompare(matrix_t *m1,matrix_t *m2)
{
	u32_t i,j;
	if(!m1 || !m2)
	{
#ifdef	CHECKED_BUILD
		conlPrintf(LOG_ERROR,"matCompare(%X,%X): incorrect parameter(s).\n",
			m1,m2);
#endif	/* CHECKED_BUILD */
		return false;
	}
	if(m1->rows!=m2->rows || m1->cols!=m2->cols)
	{
#ifdef	CHECKED_BUILD
		conlPrintf(LOG_ERROR,"matCompare(%X,%X): matrices with different sizes.\n",
			m1,m2);
#endif	/* CHECKED_BUILD */
		return false;
	}
	for(i=0;i<m1->rows;i++)
		for(j=0;j<m1->cols;j++)
		{
			mvec_t diff = m1->cell[i][j] - m2->cell[i][j];
			if(diff < -FLOAT_ZERO_EPSILON || diff > FLOAT_ZERO_EPSILON)
				return false;
		}
	return true;
}

void	V_GLOBAL
matMin(matrix_t *m,u32_t *row,u32_t *col)
{
	u32_t i,j,r,c;
	if(!m)
	{
#ifdef	CHECKED_BUILD
		conlPuts(LOG_ERROR,"matMin(NULL): incorrect parameter.\n");
#endif	/* CHECKED_BUILD */
		return;
	}
	r=c=0;
	for(i=0;i<m->rows;i++)
		for(j=0;j<m->cols;j++)
			if(m->cell[i][j]<m->cell[r][c]) 
			{
				r=i;
				c=j;
			}
	if(row)
		*row=r;
	if(col)
		*col=c;
}

void	V_GLOBAL
matMax(matrix_t *m,u32_t *row,u32_t *col)
{
	u32_t i,j,r,c;
	if(!m)
	{
#ifdef	CHECKED_BUILD
		conlPuts(LOG_ERROR,"matMax(NULL): incorrect parameter.\n");
#endif	/* CHECKED_BUILD */
		return;
	}
	r=c=0;
	for(i=0;i<m->rows;i++)
		for(j=0;j<m->cols;j++)
			if(m->cell[i][j]>m->cell[r][c]) 
			{
				r=i;
				c=j;
			}
	if(row)
		*row=r;
	if(col)
		*col=c;
}

void	V_GLOBAL
matScale(matrix_t *dest,matrix_t *m,mvec_t x)
{
	u32_t r,c;
	if(!dest || !m)
	{
#ifdef	CHECKED_BUILD
		conlPrintf(LOG_ERROR,"matScale(%X,%X,%g): incorrect parameter(s).\n",
			dest,m,x);
#endif	/* CHECKED_BUILD */
		return;
	}
	if(dest->rows!=m->rows || dest->cols!=m->cols)
	{
#ifdef	CHECKED_BUILD
		conlPrintf(LOG_ERROR,"matScale(%X,%X,%g): matrices with different sizes.\n",
			dest,m,x);
#endif	/* CHECKED_BUILD */
		return;
	}
	for(r=0;r<dest->rows;r++)
		for(c=0;c<dest->cols;c++)
			dest->cell[r][c]=m->cell[r][c]*x;
}

void	V_GLOBAL
matVecMul(vector_t *dest,matrix_t *m,vector_t *v)
{
	u32_t i,j;
	if(!dest || !m || !v)
	{
#ifdef	CHECKED_BUILD
		conlPrintf(LOG_ERROR,"matVecMul(%X,%X,%X): incorrect parameter(s).\n",
			dest,m,v);
#endif	/* CHECKED_BUILD */
		return;
	}
	if(dest->size!=m->rows || m->cols!=v->size)
	{
#ifdef	CHECKED_BUILD
		conlPuts(LOG_ERROR,"matVecMul(): wrong dimensions of operands.\n");
#endif	/* CHECKED_BUILD */
		return;
	}
	for(i=0;i<m->rows;i++)
	{
		dest->elements[i]=0.0;
		for(j=0;j<m->cols;j++)
			dest->elements[i]+=v->elements[j] * m->cell[i][j];
	}
}

void	V_GLOBAL
matMul(matrix_t *dest,matrix_t *ml,matrix_t *mr)
{
	u32_t M,N,P;
	u32_t i,j,k;
	mvec_t s;
	if(!dest || !ml || !mr)
	{
#ifdef	CHECKED_BUILD
		conlPrintf(LOG_ERROR,"matMul(%X,%X,%X): incorrect parameter(s).\n",
			dest,ml,mr);
#endif	/* CHECKED_BUILD */
		return;
	}
	if(ml->cols!=mr->rows || dest->rows!=ml->rows || dest->cols!=mr->cols)
	{
#ifdef	CHECKED_BUILD
		conlPuts(LOG_ERROR,"matMul(): wrong dimensions of operands.\n");
#endif	/* CHECKED_BUILD */
		return;
	}
	M=ml->rows;
	N=ml->cols;
	P=mr->cols;
	for(i=0;i<M;i++)
	{
		for(j=0;j<P;j++)
		{
			s=0;
			for(k=0;k<N;k++)
				s+=ml->cell[i][k]*mr->cell[k][j];
			dest->cell[i][j]=s;
		}
	}
}

void	V_GLOBAL
matTranspose(matrix_t *dest,matrix_t *m)
{
	u32_t r,c;
	if(!dest || !m)
	{
#ifdef	CHECKED_BUILD
		conlPrintf(LOG_ERROR,"matTranspose(%X,%X): incorrect parameter(s).\n",
			dest,m);
#endif	/* CHECKED_BUILD */
		return;
	}
	if(dest->rows!=m->cols || dest->cols!=m->rows)
	{
#ifdef	CHECKED_BUILD
		conlPuts(LOG_ERROR,"matTranspose(): wrong dimensions of operands.\n");
#endif	/* CHECKED_BUILD */
		return;
	}
	for(r=0;r<dest->rows;r++)
		for(c=0;c<dest->cols;c++)
			dest->cell[r][c]=m->cell[c][r];
}

mvec_t	V_GLOBAL
matDeterm(matrix_t *m,vboolean keepon)
{
	u32_t i,j,k,ind,N;
	mvec_t lead, elem, max, res;
	mvec_t *row;
	mvec_t **Rows;
	matrix_t *m1;
	if(!m)
	{
#ifdef	CHECKED_BUILD
		conlPuts(LOG_ERROR,"matDeterm(NULL): incorrect parameter.\n");
#endif	/* CHECKED_BUILD */
		return 0;
	}
	if(m->rows!=m->cols)
	{
#ifdef	CHECKED_BUILD
		conlPuts(LOG_ERROR,"matDeterm(): operand must be square matrix.\n");
#endif	/* CHECKED_BUILD */
		return 0;
	}
	if(keepon)
		m1=matDuplicate(m);
	else
		m1=m;
	Rows=m1->cell;
	N=m1->rows;
	res=1;
	for(k=0;k<N;k++)
	{
		/* find pivot element under diagonal in column #k */
		ind=k;
		max=(mvec_t)fabs(Rows[k][k]);
		for(i=k+1;i<N;i++)
		{
			elem=(mvec_t)fabs(Rows[i][k]);
			if(max<elem)
			{
				max=elem;
				ind=i;
			}
		}
		if(max < -FLOAT_ZERO_EPSILON || max > FLOAT_ZERO_EPSILON) 
		{
			memFree(m1);
			return (mvec_t)0;
		}
		/* swap rows */
		if(k!=ind)
		{
			row=Rows[ind];
			Rows[ind]=Rows[k];
			Rows[k]=row;
			res=-res;
		}
		/* next iteration of Haussian elimination method */
		lead=Rows[k][k];
		res*=lead;
		for(j=k+1;j<N;j++)
			Rows[k][j]/=lead;
		for(i=k+1;i<N;i++)
			for(j=k+1;j<N;j++)
				Rows[i][j]-=Rows[i][k]*Rows[k][j];
	}
	memFree(m1);
	return res;
}

vector_p	V_GLOBAL
matSolve(matrix_t *a,vector_t *_b,vboolean keepon)
{
	u32_t i,j,k,ind, N;
	mvec_t elem,max,b;
	mvec_t *row, *B;
	mvec_t **Rows;
	matrix_p a1;
	vector_p b1;
	if(!a || !_b)
	{
#ifdef	CHECKED_BUILD
		conlPrintf(LOG_ERROR,"matSolve(%X,%X,%d): incorrect parameter(s).\n",
			a,_b,keepon);
#endif	/* CHECKED_BUILD */
		return NULL;
	}
	if(a->rows!=a->cols || a->rows!=_b->size)
	{
#ifdef	CHECKED_BUILD
		conlPuts(LOG_ERROR,"matSolve(): wrong dimensions of operands.\n");
#endif	/* CHECKED_BUILD */
		return NULL;
	}
	if(keepon)
	{
		a1=matDuplicate(a);
		b1=vecDuplicate(_b);
	}
	else
	{
		a1=a;
		b1=_b;
	}
	Rows=a1->cell;
	B=b1->elements;
	N=a1->rows;
	/* Цикл по столбцам матрицы системы */
	for(k=0;k<N;k++)
	{
		/*	За разрешающий элемент берем наибольший по модулю из непреобра-
			зованной части текущего столбца, затем переставляем k-ю строку
			со строкой, в которой находится разрешающий элемент. */
		ind=k;
		max=(mvec_t)fabs(Rows[ind][k]);
		for(i=ind+1;i<N;i++)
		{
			elem=(mvec_t)fabs(Rows[i][k]);
			if(elem>max)
			{
				ind=i;
				max=elem;
			}
		}
		if(max < -FLOAT_ZERO_EPSILON || max > FLOAT_ZERO_EPSILON) 
		{
#ifdef		CHECKED_BUILD
			conlPrintf(LOG_ERROR,"matSolve(%d by %d): singular matrix, "
				"couldn't find pivot for column %d.\n",a->rows,a->rows,k);
#endif		/* CHECKED_BUILD */
			memFree(a1);
			memFree(b1);
			return NULL;
		}
		/* Перестановка строк матрицы A и элементов столбца B */
		if(ind!=k)
		{
			row=Rows[ind];
			Rows[ind]=Rows[k];
			Rows[k]=row;
			b=B[ind];
			B[ind]=B[k];
			B[k]=b;
		}
		/* Собственно решение. */
		elem = Rows[k][k];
		for(j=k+1;j<N;j++)
			Rows[k][j]/=elem;
		B[k]/=elem;
		/*	Непреобразованная часть матрицы состоит из двух подматриц:
			- верхней: (0,k+1) - (k-1,N-1)
			- нижней:  (k+1,k+1) - (N-1,N-1)
			Элементы подматриц пересчитываются по правилу прямоугольника.
		 */
		for(i=0;i<k;i++)
		{
			for(j=k+1;j<N;j++)
				Rows[i][j]-=Rows[i][k]*Rows[k][j];
			B[i]-=Rows[i][k]*B[k];
		}
		for(i=k+1;i<N;i++)
		{
			for(j=k+1;j<N;j++)
				Rows[i][j]-=Rows[i][k]*Rows[k][j];
			B[i]-=Rows[i][k]*B[k];
		}
	}
	memFree(a1);
	return b1;
}

matrix_p V_GLOBAL
matInverse(matrix_t *m,vboolean keepon)
{
	u32_t i,j,k,ind, N;
	mvec_t elem,max;
	mvec_t *row;
	mvec_t **RowsS, **RowsD;
	matrix_t *temp;
	matrix_t *duple;
	if(!m)
	{
#ifdef	CHECKED_BUILD
		conlPrintf(LOG_ERROR,"matInverse(NULL,%d): incorrect parameter.\n",
			keepon);
#endif	/* CHECKED_BUILD */
		return NULL;
	}
	if(m->rows!=m->cols)
	{
#ifdef	CHECKED_BUILD
		conlPuts(LOG_ERROR,"matInverse(): operand must be square matrix.\n");
#endif	/* CHECKED_BUILD */
		return NULL;
	}
	temp=matAlloc(m->rows,m->cols);
	matUnit(temp);
	if(keepon)
		duple=matDuplicate(m);
	else
		duple=m;
	RowsS=duple->cell;
	RowsD=temp->cell;
	N=duple->rows;
	/* Цикл по столбцам */
	for(k=0;k<N;k++)
	{
		/* Типовая методика выбора ведущего элемента по столбцу. */
		ind=k;
		max=(mvec_t)fabs(RowsS[ind][k]);
		for(i=ind+1;i<N;i++)
		{
			elem=(mvec_t)fabs(RowsS[i][k]);
			if(elem>max)
			{
				ind=i;
				max=elem;
			}
		}
		if(max < -FLOAT_ZERO_EPSILON || max > FLOAT_ZERO_EPSILON)
		{
#ifdef		CHECKED_BUILD
			conlPrintf(LOG_ERROR,"matInverse(%d by %d): singular matrix, "
				"couldn't find pivot for column %d.\n",m->rows,m->rows,k);
#endif		/* CHECKED_BUILD */
			memFree(duple);
			memFree(temp);
			return NULL;
		}
		/* Перестановка строк матриц */
		if(ind!=k)
		{
			row=RowsS[ind];
			RowsS[ind]=RowsS[k];
			RowsS[k]=row;
			row=RowsD[ind];
			RowsD[ind]=RowsD[k];
			RowsD[k]=row;
		}
		/* Собственно решение. */
		elem = RowsS[k][k];
		for(j=k+1;j<N;j++)
			RowsS[k][j]/=elem;
		for(j=0;j<N;j++)
			RowsD[k][j]/=elem;
		/*	Непреобразованная часть матрицы состоит из двух подматриц:
		 *		- верхней: (0,k+1) - (k-1,N-1)
		 *		- нижней:  (k+1,k+1) - (N-1,N-1)
		 *	Элементы подматриц пересчитываются по правилу прямоугольника.
		 */
		for(i=0;i<k;i++)
		{
			for(j=k+1;j<N;j++)
				RowsS[i][j]-=RowsS[i][k]*RowsS[k][j];
			for(j=0;j<N;j++)
				RowsD[i][j]-=RowsS[i][k]*RowsD[k][j];
		}
		for(i=k+1;i<N;i++)
		{
			for(j=k+1;j<N;j++)
				RowsS[i][j]-=RowsS[i][k]*RowsS[k][j];
			for(j=0;j<N;j++)
				RowsD[i][j]-=RowsS[i][k]*RowsD[k][j];
		}
	}
	memFree(duple);
	duple=matDuplicate(temp);
	memFree(temp);
	return duple;
}

#ifdef	MATRIX_NO_MACRO
void	V_GLOBAL
matZero(matrix_t *m)
{
	V_ASSERT(m);
	memZero32(m->data,m->rows*m->cols*(sizeof(mvec_t)>>2));
}  

u32_t		V_GLOBAL
matRowCount(matrix_t *m)
{
	V_ASSERT(m);
	return m->rows;
}

u32_t		V_GLOBAL
matColCount(matrix_t *m)
{
	V_ASSERT(m);
	return m->cols;
}
#endif	/* MATRIX_NO_MACRO */

/*
 *	SV decomp: port from svdcmp() from "Numerical Recipies in C"
 */

/*
PYTHAG computes sqrt(a^{2} + b^{2}) without destructive overflow or underflow.
*/
#define	PYTHAG(a, b) \
	((at = m_abs((mvec_t)(a))) > (bt = m_abs((mvec_t)(b))) ? \
	(ct = bt/at, at*(mvec_t)sqrt((mvec_t)1.0+ct*ct)) : \
    (bt ? (ct = at/bt, bt*(mvec_t)sqrt(1.0+ct*ct)) : (mvec_t)0.0))

#define MAX(a, b)	\
	(maxarg1 = (a), maxarg2 = (b), (maxarg1) > (maxarg2) ? \
	(maxarg1) : (maxarg2))

#define SIGN(a, b)	((b) < (mvec_t)0.0 ? -m_abs(a): m_abs(a))

/*
Given a matrix a[m][n], this routine computes its singular value
decomposition, A = U*W*V^{T}.  The matrix U replaces a on output.
The diagonal matrix of singular values W is output as a vector w[n].
The matrix V (not the transpose V^{T}) is output as v[n][n].
m must be greater or equal to n;  if it is smaller, then a should be
filled up to square with zero rows.
*/
vboolean	V_GLOBAL
matSVD(matrix_p src,matrix_p *U_ret,vector_p *W_ret,matrix_p *V_ret)
{
	mvec_t at, bt, ct;
	mvec_t maxarg1, maxarg2;
    int m = src->rows;
    int n = src->cols;
    vector_p W=NULL;
    mvec_t *w;
    matrix_p U=NULL,V=NULL;
    int flag, i, its, j, jj, k, l = 0, nm = 0;
    mvec_t c, f, h, s, x, y, z;
    mvec_t anorm = 0.0, g = 0.0, scale = 0.0;
	mvec_t *rv1, **a, **v;
    if(m < n)
    {
#ifdef	CHECKED_BUILD
		conlPuts(LOG_ERROR,"matSVD(): source must be augmented "
			"with extra rows of zeros.\n");
#endif	/* CHECKED_BUILD */
		goto out_fail;
	}
    W = vecAlloc(n);
    w = W->elements;
    U = matAlloc(m,n);
    matCopy(U,src);
    V = matAlloc(n,n);
    a = U->cell;
    v = V->cell;
    rv1 = (mvec_t*)memAlloc(n*sizeof(mvec_t));
    /* Householder reduction to bidiagonal 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 += m_abs(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((mvec_t)sqrt(s), f);
	        h = f*g - s;
	        a[i][i] = f - g;
	        if (i != n - 1)
	          {
		    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 - 1)
          {
	    for (k = l; k < n; k++)
	      scale += m_abs(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((mvec_t)sqrt(s), f);
	        h = f*g - s;
	        a[i][l] = f - g;
	        for (k = l; k < n; k++)
	          rv1[k] = a[i][k]/h;
	        if (i != m - 1)
	          {
		    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 = MAX(anorm, (m_abs(w[i]) + m_abs(rv1[i])));
      };
    /* Accumulation of right-hand transformations.			*/
    for (i = n - 1; 0 <= i; i--)
      {
	if (i < n - 1)
	  {
	    if (g)
	      {
		for (j = l; j < n; j++)
		  v[j][i] = (a[i][j]/a[i][l])/g;
		  /* Double division to avoid possible underflow:	*/
		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;
      };
    /* Accumulation of left-hand transformations.			*/
    for (i = n - 1; 0 <= i; i--)
      {
	l = i + 1;
	g = w[i];
	if (i < n - 1)
	  for (j = l; j < n; j++)
	    a[i][j] = 0.0;
	if (g)
	  {
	    g = (mvec_t)1.0/g;
	    if (i != n - 1)
	      {
		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];
      };
    /* Diagonalization of the bidiagonal form.				*/
    for (k = n - 1; 0 <= k; k--)	/* Loop over singular values.	*/
      {
	for (its = 0; its < 30; its++)	/* Loop over allowed iterations.*/
	  {
	    flag = 1;
	    for (l = k; 0 <= l; l--)	/* Test for splitting:		*/
	      {
		nm = l - 1;		/* Note that rv1[0] is always zero.*/
		if (fabs(rv1[l]) + anorm == anorm)
		  {
		    flag = 0;
		    break;
		  };
		if (fabs(w[nm]) + anorm == anorm)
		  break;
	      };
	    if (flag)
	      {
		c = 0.0;		/* Cancellation of rv1[l], if l>0:*/
		s = 1.0;
		for (i = l; i <= k; i++) {
		    f = s*rv1[i];
		    if (m_abs(f) + anorm != anorm)
		      {
			g = w[i];
			h = PYTHAG(f, g);
			w[i] = h;
			h = (mvec_t)1.0/h;
			c = g*h;
			s = (-f*h);
			for (j = 0; 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)		/* Convergence.				*/
	      {
		if (z < (mvec_t)0.0)	/* Singular value is made non-negative.	*/
		  {
		    w[k] = -z;
		    for (j = 0; j < n; j++)
		      v[j][k] = (-v[j][k]);
		  };
		break;
	      };
	    if (its == 29)
        {
#ifdef		CHECKED_BUILD
			conlPuts(LOG_WARNING,"matSVD(): no convergence in 30 iterations.\n");
#endif		/* CHECKED_BUILD */
        }
	    x = w[l];		/* Shift from bottom 2-by-2 minor.	*/
	    nm = k - 1;
	    y = w[nm];
	    g = rv1[nm];
	    h = rv1[k];
	    f = ((y - z)*(y + z) + (g - h)*(g + h))/((mvec_t)2.0*h*y);
	    g = PYTHAG(f, 1.0);
	    f = ((x - z)*(x + z) + h*((y/(f + SIGN(g, f))) - h))/x;
	    /* Next QR transformation:					*/
	    c = s = (mvec_t)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 = y*c;
		for (jj = 0; 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;	/* Rotation can be arbitrary if z = 0.	*/
		if (z)
		  {
		    z = (mvec_t)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 = a[jj][j];
		    z = a[jj][i];
		    a[jj][j] = y*c + z*s;
		    a[jj][i] = z*c - y*s;
		  };
	      };
	    rv1[l] = (mvec_t)0.0;
	    rv1[k] = f;
	    w[k] = x;
	  };
      };

    memFree(rv1);
	if(U_ret != NULL)
       	*U_ret=U;
	else
    	memFree(U);
	if(W_ret != NULL)
       	*W_ret=W;
	else
    	memFree(W);
	if(V_ret != NULL)
       	*V_ret=V;
	else
    	memFree(V);
    return true;
out_fail:;
	if(U != NULL)
    	memFree(U);
	if(W != NULL)
    	memFree(W);
	if(V != NULL)
    	memFree(V);
	if(U_ret != NULL)
       	*U_ret=NULL;
	if(W_ret != NULL)
       	*W_ret=NULL;
	if(V_ret != NULL)
       	*V_ret=NULL;
	return false;
}
