#include "toonz/ikjacobian.h"

#include <stdlib.h></stdlib.h>

#include <math.h></math.h>

#include <assert.h></assert.h>

#include <iostream></iostream>

#include "tstopwatch.h"

using namespace std;

inline bool NearZero(double x, double tolerance)

{

	return (fabs(x) <= tolerance);

}

/*

#ifdef _DYNAMIC

const double BASEMAXDIST = 0.02;

#else

const double MAXDIST = 0.08;	

#endif

const double DELTA = 0.4;

const long double LAMBDA = 2.0;		// solo per DLS. ottimale : 0.24

const double NEARZERO = 0.0000000001;

*/

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

// Class VectorRn

VectorRn VectorRn::WorkVector;

double VectorRn::MaxAbs() const

{

	double result = 0.0;

	double *t = x;

	for (long i = length; i > 0; i--) {

		if ((*t) > result) {

			result = *t;

		} else if (-(*t) > result) {

			result = -(*t);

		}

		t++;

	}

	return result;

}

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

// MatrixRmn

MatrixRmn MatrixRmn::WorkMatrix; // Temporary work matrix

// Fill the diagonal entries with the value d.  The rest of the matrix is unchanged.

void MatrixRmn::SetDiagonalEntries(double d)

{

	long diagLen = std::min(NumRows, NumCols);

	double *dPtr = x;

	for (; diagLen > 0; diagLen--) {

		*dPtr = d;

		dPtr += NumRows + 1;

	}

}

// Fill the diagonal entries with values in vector d.  The rest of the matrix is unchanged.

void MatrixRmn::SetDiagonalEntries(const VectorRn &d)

{

	long diagLen = std::min(NumRows, NumCols);

	assert(d.length == diagLen);

	double *dPtr = x;

	double *from = d.x;

	for (; diagLen > 0; diagLen--) {

		*dPtr = *(from++);

		dPtr += NumRows + 1;

	}

}

// Fill the superdiagonal entries with the value d.  The rest of the matrix is unchanged.

void MatrixRmn::SetSuperDiagonalEntries(double d)

{

	long sDiagLen = std::min(NumRows, (long)(NumCols - 1));

	double *to = x + NumRows;

	for (; sDiagLen > 0; sDiagLen--) {

		*to = d;

		to += NumRows + 1;

	}

}

// Fill the superdiagonal entries with values in vector d.  The rest of the matrix is unchanged.

void MatrixRmn::SetSuperDiagonalEntries(const VectorRn &d)

{

	long sDiagLen = std::min((long)(NumRows - 1), NumCols);

	assert(sDiagLen == d.length);

	double *to = x + NumRows;

	double *from = d.x;

	for (; sDiagLen > 0; sDiagLen--) {

		*to = *(from++);

		to += NumRows + 1;

	}

}

// Fill the subdiagonal entries with the value d.  The rest of the matrix is unchanged.

void MatrixRmn::SetSubDiagonalEntries(double d)

{

	long sDiagLen = std::min(NumRows, NumCols) - 1;

	double *to = x + 1;

	for (; sDiagLen > 0; sDiagLen--) {

		*to = d;

		to += NumRows + 1;

	}

}

// Fill the subdiagonal entries with values in vector d.  The rest of the matrix is unchanged.

void MatrixRmn::SetSubDiagonalEntries(const VectorRn &d)

{

	long sDiagLen = std::min(NumRows, NumCols) - 1;

	assert(sDiagLen == d.length);

	double *to = x + 1;

	double *from = d.x;

	for (; sDiagLen > 0; sDiagLen--) {

		*to = *(from++);

		to += NumRows + 1;

	}

}

// Set the i-th column equal to d.

void MatrixRmn::SetColumn(long i, const VectorRn &d)

{

	assert(NumRows == d.GetLength());

	double *to = x + i * NumRows;

	const double *from = d.x;

	for (i = NumRows; i > 0; i--) {

		*(to++) = *(from++);

	}

}

// Set the i-th column equal to d.

void MatrixRmn::SetRow(long i, const VectorRn &d)

{

	assert(NumCols == d.GetLength());

	double *to = x + i;

	const double *from = d.x;

	for (i = NumRows; i > 0; i--) {

		*to = *(from++);

		to += NumRows;

	}

}

// Sets a "linear" portion of the array with the values from a vector d

// The first row and column position are given by startRow, startCol.

// Successive positions are found by using the deltaRow, deltaCol values

//	to increment the row and column indices.  There is no wrapping around.

void MatrixRmn::SetSequence(const VectorRn &d, long startRow, long startCol, long deltaRow, long deltaCol)

{

	long length = d.length;

	assert(startRow >= 0 && startRow < NumRows && startCol >= 0 && startCol < NumCols);

	assert(startRow + (length - 1) * deltaRow >= 0 && startRow + (length - 1) * deltaRow < NumRows);

	assert(startCol + (length - 1) * deltaCol >= 0 && startCol + (length - 1) * deltaCol < NumCols);

	double *to = x + startRow + NumRows * startCol;

	double *from = d.x;

	long stride = deltaRow + NumRows * deltaCol;

	for (; length > 0; length--) {

		*to = *(from++);

		to += stride;

	}

}

// The matrix A is loaded, in into "this" matrix, based at (0,0).

//  The size of "this" matrix must be large enough to accomodate A.

//	The rest of "this" matrix is left unchanged.  It is not filled with zeroes!

void MatrixRmn::LoadAsSubmatrix(const MatrixRmn &A)

{

	assert(A.NumRows <= NumRows && A.NumCols <= NumCols);

	int extraColStep = NumRows - A.NumRows;

	double *to = x;

	double *from = A.x;

	for (long i = A.NumCols; i > 0; i--) {	 // Copy columns of A, one per time thru loop

		for (long j = A.NumRows; j > 0; j--) { // Copy all elements of this column of A

			*(to++) = *(from++);

		}

		to += extraColStep;

	}

}

// The matrix A is loaded, in transposed order into "this" matrix, based at (0,0).

//  The size of "this" matrix must be large enough to accomodate A.

//	The rest of "this" matrix is left unchanged.  It is not filled with zeroes!

void MatrixRmn::LoadAsSubmatrixTranspose(const MatrixRmn &A)

{

	assert(A.NumRows <= NumCols && A.NumCols <= NumRows);

	double *rowPtr = x;

	double *from = A.x;

	for (long i = A.NumCols; i > 0; i--) { // Copy columns of A, once per loop

		double *to = rowPtr;

		for (long j = A.NumRows; j > 0; j--) { // Loop copying values from the column of A

			*to = *(from++);

			to += NumRows;

		}

		rowPtr++;

	}

}

// Calculate the Frobenius Norm (square root of sum of squares of entries of the matrix)

double MatrixRmn::FrobeniusNorm() const

{

	return sqrt(FrobeniusNormSq());

}

// Multiply this matrix by column vector v.

// Result is column vector "result"

void MatrixRmn::Multiply(const VectorRn &v, VectorRn &result) const

{

	assert(v.GetLength() == NumCols && result.GetLength() == NumRows);

	double *out = result.GetPtr(); // Points to entry in result vector

	const double *rowPtr = x;	  // Points to beginning of next row in matrix

	for (long j = NumRows; j > 0; j--) {

		const double *in = v.GetPtr();

		const double *m = rowPtr++;

		*out = 0.0f;

		for (long i = NumCols; i > 0; i--) {

			*out += (*(in++)) * (*m);

			m += NumRows;

		}

		out++;

	}

}

// Multiply transpose of this matrix by column vector v.

//    Result is column vector "result"

// Equivalent to mult by row vector on left

void MatrixRmn::MultiplyTranspose(const VectorRn &v, VectorRn &result) const

{

	assert(v.GetLength() == NumRows && result.GetLength() == NumCols);

	double *out = result.GetPtr(); // Points to entry in result vector

	const double *colPtr = x;	  // Points to beginning of next column in matrix

	for (long i = NumCols; i > 0; i--) {

		const double *in = v.GetPtr();

		*out = 0.0f;

		for (long j = NumRows; j > 0; j--) {

			*out += (*(in++)) * (*(colPtr++));

		}

		out++;

	}

}

// Form the dot product of a vector v with the i-th column of the array

double MatrixRmn::DotProductColumn(const VectorRn &v, long colNum) const

{

	assert(v.GetLength() == NumRows);

	double *ptrC = x + colNum * NumRows;

	double *ptrV = v.x;

	double ret = 0.0;

	for (long i = NumRows; i > 0; i--) {

		ret += (*(ptrC++)) * (*(ptrV++));

	}

	return ret;

}

// Add a constant to each entry on the diagonal

MatrixRmn &MatrixRmn::AddToDiagonal(double d) // Adds d to each diagonal entry

{

	long diagLen = std::min(NumRows, NumCols);

	double *dPtr = x;

	for (; diagLen > 0; diagLen--) {

		*dPtr += d;

		dPtr += NumRows + 1;

	}

	return *this;

}

// Aggiunge i temini del vettore alla diagonale

MatrixRmn &MatrixRmn::AddToDiagonal(const VectorRn &v) // Adds d to each diagonal entry

{

	long diagLen = std::min(NumRows, NumCols);

	double *dPtr = x;

	const double *dv = v.x;

	for (; diagLen > 0; diagLen--) {

		*dPtr += *(dv++);

		dPtr += NumRows + 1;

	}

	return *this;

}

MatrixRmn &MatrixRmn::MultiplyScalar(const MatrixRmn &A, double k, MatrixRmn &dst)

{

	long length = A.NumCols;

	double *dPtr = dst.x;

	for (long i = dst.NumCols; i > 0; i--) {

		double *aPtr = A.x; // Points to beginning of row in A

		for (long j = dst.NumRows; j > 0; j--) {

			*dPtr = *aPtr * k;

			dPtr++;

			aPtr++;

		}

		aPtr += A.NumRows;

	}

	return dst;

}

// Multiply two MatrixRmn's

MatrixRmn &MatrixRmn::Multiply(const MatrixRmn &A, const MatrixRmn &B, MatrixRmn &dst)

{

	assert(A.NumCols == B.NumRows && A.NumRows == dst.NumRows && B.NumCols == dst.NumCols);

	long length = A.NumCols;

	double *bPtr = B.x; // Points to beginning of column in B

	double *dPtr = dst.x;

	for (long i = dst.NumCols; i > 0; i--) {

		double *aPtr = A.x; // Points to beginning of row in A

		for (long j = dst.NumRows; j > 0; j--) {

			*dPtr = DotArray(length, aPtr, A.NumRows, bPtr, 1);

			dPtr++;

			aPtr++;

		}

		bPtr += B.NumRows;

	}

	return dst;

}

// Multiply two MatrixRmn's,  Transpose the first matrix before multiplying

MatrixRmn &MatrixRmn::TransposeMultiply(const MatrixRmn &A, const MatrixRmn &B, MatrixRmn &dst)

{

	assert(A.NumRows == B.NumRows && A.NumCols == dst.NumRows && B.NumCols == dst.NumCols);

	long length = A.NumRows;

	double *bPtr = B.x; // bPtr Points to beginning of column in B

	double *dPtr = dst.x;

	for (long i = dst.NumCols; i > 0; i--) {	 // Loop over all columns of dst

		double *aPtr = A.x;						 // aPtr Points to beginning of column in A

		for (long j = dst.NumRows; j > 0; j--) { // Loop over all rows of dst

			*dPtr = DotArray(length, aPtr, 1, bPtr, 1);

			dPtr++;

			aPtr += A.NumRows;

		}

		bPtr += B.NumRows;

	}

	return dst;

}

// Multiply two MatrixRmn's.  Transpose the second matrix before multiplying

MatrixRmn &MatrixRmn::MultiplyTranspose(const MatrixRmn &A, const MatrixRmn &B, MatrixRmn &dst)

{

	assert(A.NumCols == B.NumCols && A.NumRows == dst.NumRows && B.NumRows == dst.NumCols);

	long length = A.NumCols;

	double *bPtr = B.x; // Points to beginning of row in B

	double *dPtr = dst.x;

	for (long i = dst.NumCols; i > 0; i--) {

		double *aPtr = A.x; // Points to beginning of row in A

		for (long j = dst.NumRows; j > 0; j--) {

			*dPtr = DotArray(length, aPtr, A.NumRows, bPtr, B.NumRows);

			dPtr++;

			aPtr++;

		}

		bPtr++;

	}

	return dst;

}

// Solves the equation   (*this)*xVec = b;

// Uses row operations.  Assumes *this is square and invertible.

// No error checking for divide by zero or instability (except with asserts)

void MatrixRmn::Solve(const VectorRn &b, VectorRn *xVec) const

{

	assert(NumRows == NumCols && NumCols == xVec->GetLength() && NumRows == b.GetLength());

	// Copy this matrix and b into an Augmented Matrix

	MatrixRmn &AugMat = GetWorkMatrix(NumRows, NumCols + 1);

	AugMat.LoadAsSubmatrix(*this);

	AugMat.SetColumn(NumRows, b);

	// Put into row echelon form with row operations

	AugMat.ConvertToRefNoFree();

	// Solve for x vector values using back substitution

	double *xLast = xVec->x + NumRows - 1;			   // Last entry in xVec

	double *endRow = AugMat.x + NumRows * NumCols - 1; // Last entry in the current row of the coefficient part of Augmented Matrix

	double *bPtr = endRow + NumRows;				   // Last entry in augmented matrix (end of last column, in augmented part)

	for (long i = NumRows; i > 0; i--) {

		double accum = *(bPtr--);

		// Next loop computes back substitution terms

		double *rowPtr = endRow; // Points to entries of the current row for back substitution.

		double *xPtr = xLast;	// Points to entries in the x vector (also for back substitution)

		for (long j = NumRows - i; j > 0; j--) {

			accum -= (*rowPtr) * (*(xPtr--));

			rowPtr -= NumCols; // Previous entry in the row

		}

		assert(*rowPtr != 0.0); // Are not supposed to be any free variables in this matrix

		*xPtr = accum / (*rowPtr);

		endRow--;

	}

}

// ConvertToRefNoFree

// Converts the matrix (in place) to row echelon form

// For us, row echelon form allows any non-zero values, not just 1's, in the

//		position for a lead variable.

// The "NoFree" version operates on the assumption that no free variable will be found.

// Algorithm uses row operations and row pivoting (only).

// Augmented matrix is correctly accomodated.  Only the first square part participates

//		in the main work of row operations.

void MatrixRmn::ConvertToRefNoFree()

{

	// Loop over all columns (variables)

	// Find row with most non-zero entry.

	// Swap to the highest active row

	// Subtract appropriately from all the lower rows (row op of type 3)

	long numIters = std::min(NumRows, NumCols);

	double *rowPtr1 = x;

	const long diagStep = NumRows + 1;

	long lenRowLeft = NumCols;

	for (; numIters > 1; numIters--) {

		// Find row with most non-zero entry.

		double *rowPtr2 = rowPtr1;

		double maxAbs = fabs(*rowPtr1);

		double *rowPivot = rowPtr1;

		long i;

		for (i = numIters - 1; i > 0; i--) {

			const double &newMax = *(++rowPivot);

			if (newMax > maxAbs) {

				maxAbs = *rowPivot;

				rowPtr2 = rowPivot;

			} else if (-newMax > maxAbs) {

				maxAbs = -newMax;

				rowPtr2 = rowPivot;

			}

		}

		// Pivot step: Swap the row with highest entry to the current row

		if (rowPtr1 != rowPtr2) {

			double *to = rowPtr1;

			for (long i = lenRowLeft; i > 0; i--) {

				double temp = *to;

				*to = *rowPtr2;

				*rowPtr2 = temp;

				to += NumRows;

				rowPtr2 += NumRows;

			}

		}

		// Subtract this row appropriately from all the lower rows (row operation of type 3)

		rowPtr2 = rowPtr1;

		for (i = numIters - 1; i > 0; i--) {

			rowPtr2++;

			double *to = rowPtr2;

			double *from = rowPtr1;

			assert(*from != 0.0);

			double alpha = (*to) / (*from);

			*to = 0.0;

			for (long j = lenRowLeft - 1; j > 0; j--) {

				to += NumRows;

				from += NumRows;

				*to -= (*from) * alpha;

			}

		}

		// Update for next iteration of loop

		rowPtr1 += diagStep;

		lenRowLeft--;

	}

}

// Calculate the c=cosine and s=sine values for a Givens transformation.

// The matrix M = ( (c, -s), (s, c) ) in row order transforms the

//   column vector (a, b)^T to have y-coordinate zero.

void MatrixRmn::CalcGivensValues(double a, double b, double *c, double *s)

{

	double denomInv = sqrt(a * a + b * b);

	if (denomInv == 0.0) {

		*c = 1.0;

		*s = 0.0;

	} else {

		denomInv = 1.0 / denomInv;

		*c = a * denomInv;

		*s = -b * denomInv;

	}

}

// Applies Givens transform to columns i and i+1.

// Equivalent to postmultiplying by the matrix

//      ( c  -s )

//		( s   c )

// with non-zero entries in rows i and i+1 and columns i and i+1

void MatrixRmn::PostApplyGivens(double c, double s, long idx)

{

	assert(0 <= idx && idx < NumCols);

	double *colA = x + idx * NumRows;

	double *colB = colA + NumRows;

	for (long i = NumRows; i > 0; i--) {

		double temp = *colA;

		*colA = (*colA) * c + (*colB) * s;

		*colB = (*colB) * c - temp * s;

		colA++;

		colB++;

	}

}

// Applies Givens transform to columns idx1 and idx2.

// Equivalent to postmultiplying by the matrix

//      ( c  -s )

//		( s   c )

// with non-zero entries in rows idx1 and idx2 and columns idx1 and idx2

void MatrixRmn::PostApplyGivens(double c, double s, long idx1, long idx2)

{

	assert(idx1 != idx2 && 0 <= idx1 && idx1 < NumCols && 0 <= idx2 && idx2 < NumCols);

	double *colA = x + idx1 * NumRows;

	double *colB = x + idx2 * NumRows;

	for (long i = NumRows; i > 0; i--) {

		double temp = *colA;

		*colA = (*colA) * c + (*colB) * s;

		*colB = (*colB) * c - temp * s;

		colA++;

		colB++;

	}

}

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

// Singular value decomposition.

// Return othogonal matrices U and V and diagonal matrix with diagonal w such that

//     (this) = U * Diag(w) * V^T     (V^T is V-transpose.)

// Diagonal entries have all non-zero entries before all zero entries, but are not

//		necessarily sorted.  (Someday, I will write ComputedSortedSVD that handles

//		sorting the eigenvalues by magnitude.)

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

void MatrixRmn::ComputeSVD(MatrixRmn &U, VectorRn &w, MatrixRmn &V) const

{

	assert(U.NumRows == NumRows && V.NumCols == NumCols && U.NumRows == U.NumCols && V.NumRows == V.NumCols && w.GetLength() == std::min(NumRows, NumCols));

	double temp = 0.0;

	VectorRn &superDiag = VectorRn::GetWorkVector(w.GetLength() - 1); // Some extra work space.  Will get passed around.

	// Choose larger of U, V to hold intermediate results

	// If U is larger than V, use U to store intermediate results

	// Otherwise use V.  In the latter case, we form the SVD of A transpose,

	//		(which is essentially identical to the SVD of A).

	MatrixRmn *leftMatrix;

	MatrixRmn *rightMatrix;

	if (NumRows >= NumCols) {

		U.LoadAsSubmatrix(*this); // Copy A into U

		leftMatrix = &U;

		rightMatrix = &V;

	} else {

		V.LoadAsSubmatrixTranspose(*this); // Copy A-transpose into V

		leftMatrix = &V;

		rightMatrix = &U;

	}

	// Do the actual work to calculate the SVD

	// Now matrix has at least as many rows as columns

	CalcBidiagonal(*leftMatrix, *rightMatrix, w, superDiag);

	ConvertBidiagToDiagonal(*leftMatrix, *rightMatrix, w, superDiag);

}

// ************************************************ CalcBidiagonal **************************

// Helper routine for SVD computation

// U is a matrix to be bidiagonalized.

// On return, U and V are orthonormal and w holds the new diagonal

//	  elements and superDiag holds the super diagonal elements.

void MatrixRmn::CalcBidiagonal(MatrixRmn &U, MatrixRmn &V, VectorRn &w, VectorRn &superDiag)

{

	assert(U.NumRows >= V.NumRows);

	// The diagonal and superdiagonal entries of the bidiagonalized

	//	  version of the U matrix

	//	  are stored in the vectors w and superDiag (temporarily).

	// Apply Householder transformations to U.

	// Householder transformations come in pairs.

	//   First, on the left, we map a portion of a column to zeros

	//   Second, on the right, we map a portion of a row to zeros

	const long rowStep = U.NumCols;

	const long diagStep = U.NumCols + 1;

	double *diagPtr = U.x;

	double *wPtr = w.x;

	double *superDiagPtr = superDiag.x;

	long colLengthLeft = U.NumRows;

	long rowLengthLeft = V.NumCols;

	while (true) {

		// Apply a Householder xform on left to zero part of a column

		SvdHouseholder(diagPtr, colLengthLeft, rowLengthLeft, 1, rowStep, wPtr);

		if (rowLengthLeft == 2) {

			*superDiagPtr = *(diagPtr + rowStep);

			break;

		}

		// Apply a Householder xform on the right to zero part of a row

		SvdHouseholder(diagPtr + rowStep, rowLengthLeft - 1, colLengthLeft, rowStep, 1, superDiagPtr);

		rowLengthLeft--;

		colLengthLeft--;

		diagPtr += diagStep;

		wPtr++;

		superDiagPtr++;

	}

	int extra = 0;

	diagPtr += diagStep;

	wPtr++;

	if (colLengthLeft > 2) {

		extra = 1;

		// Do one last Householder transformation when the matrix is not square

		colLengthLeft--;

		SvdHouseholder(diagPtr, colLengthLeft, 1, 1, 0, wPtr);

	} else {

		*wPtr = *diagPtr;

	}

	// Form U and V from the Householder transformations

	V.ExpandHouseholders(V.NumCols - 2, 1, U.x + U.NumRows, U.NumRows, 1);

	U.ExpandHouseholders(V.NumCols - 1 + extra, 0, U.x, 1, U.NumRows);

	// Done with bidiagonalization

	return;

}

// Helper routine for CalcBidiagonal

// Performs a series of Householder transformations on a matrix

// Stores results compactly into the matrix:   The Householder vector u (normalized)

//   is stored into the first row/column being transformed.

// The leading term of that row (= plus/minus its magnitude is returned

//	 separately into "retFirstEntry"

void MatrixRmn::SvdHouseholder(double *basePt,

							   long colLength, long numCols, long colStride, long rowStride,

							   double *retFirstEntry)

{

	// Calc norm of vector u

	double *cPtr = basePt;

	double norm = 0.0;

	long i;

	double aa0 = *cPtr;

	double aa1 = *basePt;

	double aa2 = *retFirstEntry;

	for (i = colLength; i > 0; i--) {

		norm += Square(*cPtr);

		cPtr += colStride;

	}

	norm = sqrt(norm); // Norm of vector to reflect to axis  e_1

	// Handle sign issues

	double imageVal; // Choose sign to maximize distance

	if ((*basePt) < 0.0) {

		imageVal = norm;

		norm = 2.0 * norm * (norm - (*basePt));

	} else {

		imageVal = -norm;

		norm = 2.0 * norm * (norm + (*basePt));

	}

	norm = sqrt(norm); // Norm is norm of reflection vector

	if (norm == 0.0) { // If the vector being transformed is equal to zero

		// Force to zero in case of roundoff errors

		cPtr = basePt;

		for (i = colLength; i > 0; i--) {

			*cPtr = 0.0;

			cPtr += colStride;

		}

		*retFirstEntry = 0.0;

		return;

	}

	*retFirstEntry = imageVal;

	// Set up the normalized Householder vector

	*basePt -= imageVal; // First component changes. Rest stay the same.

	// Normalize the vector

	norm = 1.0 / norm; // Now it is the inverse norm

	cPtr = basePt;

	for (i = colLength; i > 0; i--) {

		*cPtr *= norm;

		cPtr += colStride;

	}

	// Transform the rest of the U matrix with the Householder transformation

	double *rPtr = basePt;

	for (long j = numCols - 1; j > 0; j--) {

		rPtr += rowStride;

		// Calc dot product with Householder transformation vector

		double dotP = DotArray(colLength, basePt, colStride, rPtr, colStride);

		// Transform with I - 2*dotP*(Householder vector)

		AddArrayScale(colLength, basePt, colStride, rPtr, colStride, -2.0 * dotP);

	}

}

// ********************************* ExpandHouseholders ********************************************

// The matrix will be square.

//   numXforms = number of Householder transformations to concatenate

//		Each Householder transformation is represented by a unit vector

//		Each successive Householder transformation starts one position later

//			and has one more implied leading zero

//	 basePt = beginning of the first Householder transform

//	 colStride, rowStride: Householder xforms are stored in "columns"

//   numZerosSkipped is the number of implicit zeros on the front each

//			Householder transformation vector (only values supported are 0 and 1).

void MatrixRmn::ExpandHouseholders(long numXforms, int numZerosSkipped, const double *basePt, long colStride, long rowStride)

{

	// Number of applications of the last Householder transform

	//     (That are not trivial!)

	long numToTransform = NumCols - numXforms + 1 - numZerosSkipped;

	assert(numToTransform > 0);

	if (numXforms == 0) {

		SetIdentity();

		return;

	}

	// Handle the first one separately as a special case,

	// "this" matrix will be treated to simulate being preloaded with the identity

	long hDiagStride = rowStride + colStride;

	const double *hBase = basePt + hDiagStride * (numXforms - 1);	  // Pointer to the last Householder vector

	const double *hDiagPtr = hBase + colStride * (numToTransform - 1); // Pointer to last entry in that vector

	long i;

	double *diagPtr = x + NumCols * NumRows - 1;	 // Last entry in matrix (points to diagonal entry)

	double *colPtr = diagPtr - (numToTransform - 1); // Pointer to column in matrix

	for (i = numToTransform; i > 0; i--) {

		CopyArrayScale(numToTransform, hBase, colStride, colPtr, 1, -2.0 * (*hDiagPtr));

		*diagPtr += 1.0;		  // Add back in 1 to the diagonal entry (since xforming the identity)

		diagPtr -= (NumRows + 1); // Next diagonal entry in this matrix

		colPtr -= NumRows;		  // Next column in this matrix

		hDiagPtr -= colStride;

	}

	// Now handle the general case

	// A row of zeros must be in effect added to the top of each old column (in each loop)

	double *colLastPtr = x + NumRows * NumCols - numToTransform - 1;

	for (i = numXforms - 1; i > 0; i--) {

		numToTransform++;	 // Number of non-trivial applications of this Householder transformation

		hBase -= hDiagStride; // Pointer to the beginning of the Householder transformation

		colPtr = colLastPtr;

		for (long j = numToTransform - 1; j > 0; j--) {

			// Get dot product

			double dotProd2N = -2.0 * DotArray(numToTransform - 1, hBase + colStride, colStride, colPtr + 1, 1);

			*colPtr = dotProd2N * (*hBase); // Adding onto zero at initial point

			AddArrayScale(numToTransform - 1, hBase + colStride, colStride, colPtr + 1, 1, dotProd2N);

			colPtr -= NumRows;

		}

		// Do last one as a special case (may overwrite the Householder vector)

		CopyArrayScale(numToTransform, hBase, colStride, colPtr, 1, -2.0 * (*hBase));

		*colPtr += 1.0; // Add back one one as identity

		// Done with this Householder transformation

		colLastPtr--;

	}

	if (numZerosSkipped != 0) {

		assert(numZerosSkipped == 1);

		// Fill first row and column with identity (More generally: first numZerosSkipped many rows and columns)

		double *d = x;

		*d = 1;

		double *d2 = d;

		for (i = NumRows - 1; i > 0; i--) {

			*(++d) = 0;

			*(d2 += NumRows) = 0;

		}

	}

}

// **************** ConvertBidiagToDiagonal ***********************************************

// Do the iterative transformation from bidiagonal form to diagonal form using

//		Givens transformation.  (Golub-Reinsch)

// U and V are square.  Size of U less than or equal to that of U.

void MatrixRmn::ConvertBidiagToDiagonal(MatrixRmn &U, MatrixRmn &V, VectorRn &w, VectorRn &superDiag) const

{

	// These two index into the last bidiagonal block  (last in the matrix, it will be

	//	first one handled.

	long lastBidiagIdx = V.NumRows - 1;

	long firstBidiagIdx = 0;

	//togliere

	double aa = w.MaxAbs();

	double bb = superDiag.MaxAbs();

	double eps = 1.0e-15 * std::max(w.MaxAbs(), superDiag.MaxAbs());

	while (true) {

		bool workLeft = UpdateBidiagIndices(&firstBidiagIdx, &lastBidiagIdx, w, superDiag, eps);

		if (!workLeft) {

			break;

		}

		// Get ready for first Givens rotation

		// Push non-zero to M[2,1] with Givens transformation

		double *wPtr = w.x + firstBidiagIdx;

		double *sdPtr = superDiag.x + firstBidiagIdx;

		double extraOffDiag = 0.0;

		if ((*wPtr) == 0.0) {

			ClearRowWithDiagonalZero(firstBidiagIdx, lastBidiagIdx, U, wPtr, sdPtr, eps);

			if (firstBidiagIdx > 0) {

				if (NearZero(*(--sdPtr), eps)) {

					*sdPtr = 0.0;

				} else {

					ClearColumnWithDiagonalZero(firstBidiagIdx, V, wPtr, sdPtr, eps);

				}

			}

			continue;

		}

		// Estimate an eigenvalue from bottom four entries of M

		// This gives a lambda value which will shift the Givens rotations

		// Last four entries of M^T * M are  ( ( A, B ), ( B, C ) ).

		double A;

		A = (firstBidiagIdx < lastBidiagIdx - 1) ? Square(superDiag[lastBidiagIdx - 2]) : 0.0;

		double BSq = Square(w[lastBidiagIdx - 1]);

		A += BSq; // The "A" entry of M^T * M

		double C = Square(superDiag[lastBidiagIdx - 1]);

		BSq *= C;									// The squared "B" entry

		C += Square(w[lastBidiagIdx]);				// The "C" entry

		double lambda;								// lambda will hold the estimated eigenvalue

		lambda = sqrt(Square((A - C) * 0.5) + BSq); // Use the lambda value that is closest to C.

		if (A > C) {

			lambda = -lambda;

		}

		lambda += (A + C) * 0.5; // Now lambda equals the estimate for the last eigenvalue

		double t11 = Square(w[firstBidiagIdx]);

		double t12 = w[firstBidiagIdx] * superDiag[firstBidiagIdx];

		double c, s;

		CalcGivensValues(t11 - lambda, t12, &c, &s);

		ApplyGivensCBTD(c, s, wPtr, sdPtr, &extraOffDiag, wPtr + 1);

		V.PostApplyGivens(c, -s, firstBidiagIdx);

		long i;

		for (i = firstBidiagIdx; i < lastBidiagIdx - 1; i++) {

			// Push non-zero from M[i+1,i] to M[i,i+2]

			CalcGivensValues(*wPtr, extraOffDiag, &c, &s);

			ApplyGivensCBTD(c, s, wPtr, sdPtr, &extraOffDiag, extraOffDiag, wPtr + 1, sdPtr + 1);

			U.PostApplyGivens(c, -s, i);

			// Push non-zero from M[i,i+2] to M[1+2,i+1]

			CalcGivensValues(*sdPtr, extraOffDiag, &c, &s);

			ApplyGivensCBTD(c, s, sdPtr, wPtr + 1, &extraOffDiag, extraOffDiag, sdPtr + 1, wPtr + 2);

			V.PostApplyGivens(c, -s, i + 1);

			wPtr++;

			sdPtr++;

		}

		// Push non-zero value from M[i+1,i] to M[i,i+1] for i==lastBidiagIdx-1

		CalcGivensValues(*wPtr, extraOffDiag, &c, &s);

		ApplyGivensCBTD(c, s, wPtr, &extraOffDiag, sdPtr, wPtr + 1);

		U.PostApplyGivens(c, -s, i);

		// DEBUG

		// DebugCalcBidiagCheck( V, w, superDiag, U );

	}

}

// This is called when there is a zero diagonal entry, with a non-zero superdiagonal entry on the same row.

// We use Givens rotations to "chase" the non-zero entry across the row; when it reaches the last

//	column, it is finally zeroed away.

// wPtr points to the zero entry on the diagonal.  sdPtr points to the non-zero superdiagonal entry on the same row.

void MatrixRmn::ClearRowWithDiagonalZero(long firstBidiagIdx, long lastBidiagIdx, MatrixRmn &U, double *wPtr, double *sdPtr, double eps)

{

	double curSd = *sdPtr; // Value being chased across the row

	*sdPtr = 0.0;

	long i = firstBidiagIdx + 1;

	while (true) {

		// Rotate row i and row firstBidiagIdx (Givens rotation)

		double c, s;

		CalcGivensValues(*(++wPtr), curSd, &c, &s);

		U.PostApplyGivens(c, -s, i, firstBidiagIdx);

		*wPtr = c * (*wPtr) - s * curSd;

		if (i == lastBidiagIdx) {

			break;

		}

		curSd = s * (*(++sdPtr)); // New value pops up one column over to the right

		*sdPtr = c * (*sdPtr);

		i++;

	}

}

// This is called when there is a zero diagonal entry, with a non-zero superdiagonal entry in the same column.

// We use Givens rotations to "chase" the non-zero entry up the column; when it reaches the last

//	column, it is finally zeroed away.

// wPtr points to the zero entry on the diagonal.  sdPtr points to the non-zero superdiagonal entry in the same column.