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

/*
 * Helper macro for computing 2D array indices
 */
#define REINITIALIZE_MATRIX_INDEX(i,mat) \
	(mat)->cell[0]=(mat)->data; \
	for((i)=1;(i)<(mat)->rows;(i)++) \
		(mat)->cell[(i)]=(mat)->cell[(i)-1]+(mat)->cols;

/*
 * 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(vec_t*)+
		rows*cols*sizeof(vec_t);
	m=(matrix_t*)memAlloc(size);
	m->rows=rows;
	m->cols=cols;
	m->cell=(vec_t**)(m+1);
	m->data=(vec_t*)(m->cell+rows);
	REINITIALIZE_MATRIX_INDEX(i,m);
	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;
	}
	if(src->rows!=dest->rows || src->cols!=dest->cols)
	{
#ifdef	CHECKED_BUILD
		conlPrintf(LOG_ERROR,
			"matCopy(): matrices with different size (%ux%u and %ux%u).\n",
			dest->rows,dest->cols,src->rows,src->cols);
#endif	/* CHECKED_BUILD */
		return;
	}
	REINITIALIZE_MATRIX_INDEX(i,dest);
	for(i=0;i<src->rows;i++)
		memCopy32(dest->cell[i],src->cell[i],(src->cols*sizeof(vec_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(vec_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(vec_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++)
		{
			vec_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,vec_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;
	vec_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];
}

void	V_GLOBAL
matTransposeIP(matrix_p m)
{
	vec_t temp;
	u32_t r, c, rlimit, climit;
	if(m->rows != m->cols)
	{
#ifdef	CHECKED_BUILD
		conlPrintf(LOG_ERROR,"matTransposeIP(%ux%u): "
			"must be square matrix.\n",m->rows,m->cols);
#endif	/* CHECKED_BUILD */
		return;
	}
	/* go through upper triangle */
	rlimit = m->rows-1;
	climit = m->cols;
	for(r = 0;r < rlimit;r++)
		for(c = r+1;c < climit;c++)
		{
			temp = m->cell[r][c];
			m->cell[r][c] = m->cell[c][r];
			m->cell[c][r] = temp;
		}
}

V_LOCAL vboolean
mat_nonempty_rect(u32_t left,u32_t top,u32_t *right,u32_t *bottom,
	u32_t max_right,u32_t max_bottom)
{
	if(*right > max_right)
		*right = max_right;
	if(*bottom > max_bottom)
		*bottom = max_bottom;
	if(left > *right)
		return false;
	if(top > *bottom)
		return false;
	return true;
}

V_LOCAL void
mat_copy_elements_pwr2(matrix_p dest,matrix_p source,
	s32_t dest_col,s32_t dest_row,
	u32_t src_left,u32_t src_top,u32_t src_right,u32_t src_bottom)
{
	u32_t sr, sc, dc;
	u32_t dest_row_mask = dest->rows - 1;
	u32_t dest_col_mask = dest->cols - 1;
	vec_p dst, src;
	if(!mat_nonempty_rect(src_left,src_top,&src_right,&src_bottom,
			source->cols-1,source->rows-1))
		return;
	for(sr = src_top;sr <= src_bottom;sr++)
	{
		dc = dest_col;
		dst = dest->cell[dest_row & dest_row_mask];
		src = source->cell[sr];
		for(sc = src_left;sc <= src_right;sc++)
			dst[dc++ & dest_col_mask] = src[sc];
		dest_row++;
	}
}

V_LOCAL	void
mat_copy_elements_generic(matrix_p dest,matrix_p source,
	s32_t dest_col,s32_t dest_row,
	u32_t src_left,u32_t src_top,u32_t src_right,u32_t src_bottom)
{
	u32_t sr, sc, dc;
	u32_t dest_rows = dest->rows;
	u32_t dest_cols = dest->cols;
	vec_p dst, src;
	if(!mat_nonempty_rect(src_left,src_top,&src_right,&src_bottom,
			source->cols-1,source->rows-1))
		return;
	for(sr = src_top;sr <= src_bottom;sr++)
	{
		dc = dest_col;
		dst = dest->cell[dest_row % dest_rows];
		src = source->cell[sr];
		for(sc = src_left;sc <= src_right;sc++)
			dst[dc++ % dest_cols] = src[sc];
		dest_row++;
	}
}

void	V_GLOBAL
matCopyElements(matrix_p dest,matrix_p source,
	s32_t dest_col,s32_t dest_row,
	u32_t src_left,u32_t src_top,u32_t src_right,u32_t src_bottom)
{
	CALLER_NAME(matCopyElements)
	DEBUG_MESSAGE_INCORRECT()
	if(!dest || !source || !dest->cols || !dest->rows)
	{
		ERROR_GENERIC(incorrect_parameters);
		return;
	}
	if(IS_POWER_TWO(dest->cols) && IS_POWER_TWO(dest->rows))
		mat_copy_elements_pwr2(dest,source,dest_col,dest_row,
			src_left,src_top,src_right,src_bottom);
	else
		mat_copy_elements_generic(dest,source,dest_col,dest_row,
			src_left,src_top,src_right,src_bottom);
}

vec_t	V_GLOBAL
matDeterm(matrix_t *m,vboolean keepon)
{
	u32_t i,j,k,ind,N;
	vec_t lead, elem, max, res;
	vec_t *row;
	vec_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 = v_abs(Rows[k][k]);
		for(i=k+1;i<N;i++)
		{
			elem = v_abs(Rows[i][k]);
			if(max<elem)
			{
				max=elem;
				ind=i;
			}
		}
		if(IsZero(max))
		{
#ifdef		CHECKED_BUILD
			conlPrintf(LOG_ERROR,"matDeterm(%d by %d): (near) singular matrix, "
				"couldn't find pivot for column %d.\n",m->rows,m->rows,k);
#endif		/* CHECKED_BUILD */
			memFree(m1);
			return (vec_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;
	vec_t elem,max,b;
	vec_t *row, *B;
	vec_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 = v_abs(Rows[ind][k]);
		for(i=ind+1;i<N;i++)
		{
			elem = v_abs(Rows[i][k]);
			if(elem>max)
			{
				ind=i;
				max=elem;
			}
		}
		if(IsZero(max))
		{
#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;
	vec_t elem,max;
	vec_t *row;
	vec_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 = v_abs(RowsS[ind][k]);
		for(i=ind+1;i<N;i++)
		{
			elem = v_abs(RowsS[i][k]);
			if(elem>max)
			{
				ind=i;
				max=elem;
			}
		}
		if(IsZero(max))
		{
#ifdef		CHECKED_BUILD
			conlPrintf(LOG_ERROR,"matInverse(%d by %d): (near) 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(vec_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 */

#if 0

/*
 *	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 = v_abs((vec_t)(a))) > (bt = v_abs((vec_t)(b))) ? \
	(ct = bt/at, at*(vec_t)sqrt((vec_t)1.0+ct*ct)) : \
    (bt ? (ct = at/bt, bt*(vec_t)sqrt(1.0+ct*ct)) : (vec_t)0.0))

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

#define MAX_SVD_ITER	30
#define SIGN(a, b)	((b) < (vec_t)0.0 ? -v_abs(a): v_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)
{
	vec_t at, bt, ct;
	vec_t maxarg1, maxarg2;
	int m = src->rows;
	int n = src->cols;
	vector_p W = NULL;
	vec_t *w;
	matrix_p U = NULL, V = NULL;
	int flag, i, its, j, jj, k, l = 0, nm = 0;
	vec_t c, f, h, s, x, y, z;
	vec_t anorm = 0.0, g = 0.0, scale = 0.0;
	vec_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 = (vec_t *) memAlloc (n * sizeof (vec_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 += v_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 ((vec_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 += v_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 ((vec_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, (v_abs (w[i]) + v_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 = (vec_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 (v_abs(rv1[l]) + anorm == anorm)
				{
					flag = 0;
					break;
				}
				if (v_abs(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 (v_abs (f) + anorm != anorm)
					{
						g = w[i];
						h = PYTHAG (f, g);
						w[i] = h;
						h = (vec_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 < 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 == MAX_SVD_ITER)
			{
#ifdef	CHECKED_BUILD
				conlPrintf(LOG_WARNING,
					"matSVD(): no convergence in %d iterations.\n",its);
#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)) / ((vec_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 = (vec_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 = (vec_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] = (vec_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;
}

#endif
