// Helper routine for SVD conversion from bidiagonal to diagonal
bool MatrixRmn::UpdateBidiagIndices(long *firstBidiagIdx, long *lastBidiagIdx, VectorRn &w, VectorRn &superDiag, double eps)
{
	long lastIdx = *lastBidiagIdx;
	double *sdPtr = superDiag.GetPtr(lastIdx - 1); // Entry above the last diagonal entry
	while (NearZero(*sdPtr, eps)) {
		*(sdPtr--) = 0.0;
		lastIdx--;
		if (lastIdx == 0) {
			return false;
		}
	}
	*lastBidiagIdx = lastIdx;
	long firstIdx = lastIdx - 1;
	double *wPtr = w.GetPtr(firstIdx);
	while (firstIdx > 0) {
		if (NearZero(*wPtr, eps)) { // If this diagonal entry (near) zero
			*wPtr = 0.0;
			break;
		}
		if (NearZero(*(--sdPtr), eps)) { // If the entry above the diagonal entry is (near) zero
			*sdPtr = 0.0;
			break;
		}
		wPtr--;
		firstIdx--;
	}
	*firstBidiagIdx = firstIdx;
	return true;
}

// ********************************************************************************************
// 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()==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 );

}

// 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++;
	}
}

bool MatrixRmn::DebugCheckSVD(const MatrixRmn &U, const VectorRn &w, const MatrixRmn &V) const
{
	// Special SVD test code

	MatrixRmn IV(V.getNumRows(), V.getNumColumns());
	IV.SetIdentity();
	MatrixRmn VTV(V.getNumRows(), V.getNumColumns());
	MatrixRmn::TransposeMultiply(V, V, VTV);
	IV -= VTV;
	double error = IV.FrobeniusNorm();

	MatrixRmn IU(U.getNumRows(), U.getNumColumns());
	IU.SetIdentity();
	MatrixRmn UTU(U.getNumRows(), U.getNumColumns());
	MatrixRmn::TransposeMultiply(U, U, UTU);
	IU -= UTU;
	error += IU.FrobeniusNorm();

	MatrixRmn Diag(U.getNumRows(), V.getNumRows());
	Diag.SetZero();
	Diag.SetDiagonalEntries(w);
	MatrixRmn B(U.getNumRows(), V.getNumRows());
	MatrixRmn C(U.getNumRows(), V.getNumRows());
	MatrixRmn::Multiply(U, Diag, B);
	MatrixRmn::MultiplyTranspose(B, V, C);
	C -= *this;
	error += C.FrobeniusNorm();

	bool ret = (fabs(error) <= 1.0e-13 * w.MaxAbs());
	assert(ret);
	return ret;
}

// 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--;
	}
}

// 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++;
	}
}

// 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;
	}
}

// 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;
}

bool MatrixRmn::DebugCalcBidiagCheck( const MatrixRmn& U, const VectorRn& w, const VectorRn& superDiag, const MatrixRmn& V ) const
{
	// Special SVD test code

	MatrixRmn IV( V.GetNumRows(), V.GetNumColumns() );
	IV.SetIdentity();
	MatrixRmn VTV( V.GetNumRows(), V.GetNumColumns() );
	MatrixRmn::TransposeMultiply( V, V, VTV );
	IV -= VTV;
	double error = IV.FrobeniusNorm();

	MatrixRmn IU( U.GetNumRows(), U.GetNumColumns() );
	IU.SetIdentity();
	MatrixRmn UTU( U.GetNumRows(), U.GetNumColumns() );
	MatrixRmn::TransposeMultiply( U, U, UTU );
	IU -= UTU;
	error += IU.FrobeniusNorm();

	MatrixRmn DiagAndSuper( U.GetNumRows(), V.GetNumRows() );
	DiagAndSuper.SetZero();
	DiagAndSuper.SetDiagonalEntries( w );
	if ( this->GetNumRows()>=this->GetNumColumns() ) {
		DiagAndSuper.SetSequence( superDiag, 0, 1, 1, 1 );
	}
	else {
		DiagAndSuper.SetSequence( superDiag, 1, 0, 1, 1 );
	}
	MatrixRmn B(U.GetNumRows(), V.GetNumRows() );
	MatrixRmn C(U.GetNumRows(), V.GetNumRows() );
	MatrixRmn::Multiply( U, DiagAndSuper, B );
	MatrixRmn::MultiplyTranspose( B, V, C );
	C -= *this;
	error += C.FrobeniusNorm();

	bool ret = ( fabs(error)<1.0e-13*Max(w.MaxAbs(),superDiag.MaxAbs()) );
	assert ( ret );
	return ret;
}

// **************** 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 );
	}
}

void Jacobian::CalcDeltaThetasPseudoinverse()
{
	MatrixRmn &J = const_cast<MatrixRmn &>(Jend);

	// costruisco matrice J1
	MatrixRmn J1;
	J1.SetSize(2, J.getNumColumns());

	for (int i = 0; i < 2; i++)
		for (int j = 0; j < J.getNumColumns(); j++)
			J1.Set(i, j, J.Get(i, j));

	// COSTRUISCO VETTORI ds1 e ds2
	VectorRn dS1(2);

	for (int i = 0; i < 2; i++)
		dS1.Set(i, dS.Get(i));

	// calcolo dtheta1
	MatrixRmn U, V;
	VectorRn w;

	U.SetSize(J1.getNumRows(), J1.getNumRows());
	w.SetLength(min(J1.getNumRows(), J1.getNumColumns()));
	V.SetSize(J1.getNumColumns(), J1.getNumColumns());

	J1.ComputeSVD(U, w, V);

	// Next line for debugging only
	assert(J1.DebugCheckSVD(U, w, V));

	// Calculate response vector dTheta that is the DLS solution.
	//	Delta target values are the dS values
	//  We multiply by Moore-Penrose pseudo-inverse of the J matrix

	double pseudoInverseThreshold = PseudoInverseThresholdFactor * w.MaxAbs();

	long diagLength = w.GetLength();
	double *wPtr = w.GetPtr();
	dTheta.SetZero();
	for (long i = 0; i < diagLength; i++) {
		double dotProdCol = U.DotProductColumn(dS1, i); // Dot product with i-th column of U
		double alpha = *(wPtr++);
		if (fabs(alpha) > pseudoInverseThreshold) {
			alpha = 1.0 / alpha;
			MatrixRmn::AddArrayScale(V.getNumRows(), V.GetColumnPtr(i), 1, dTheta.GetPtr(), 1, dotProdCol * alpha);
		}
	}

	MatrixRmn JcurrentPinv(V.getNumRows(), J1.getNumRows());		   // pseudoinversa di J1
	MatrixRmn JProjPre(JcurrentPinv.getNumRows(), J1.getNumColumns()); // Proiezione di J1
	if (skeleton->getNumEffector() > 1) {
		// calcolo la pseudoinversa di J1
		MatrixRmn VD(V.getNumRows(), J1.getNumRows()); // matrice del prodotto V*w

		double *wPtr = w.GetPtr();
		pseudoInverseThreshold = PseudoInverseThresholdFactor * w.MaxAbs();
		for (int j = 0; j < VD.getNumColumns(); j++) {
			double *VPtr = V.GetColumnPtr(j);
			double alpha = *(wPtr++); // elemento matrice diagonale
			for (int i = 0; i < V.getNumRows(); i++) {
				if (fabs(alpha) > pseudoInverseThreshold) {
					double entry = *(VPtr++);
					VD.Set(i, j, entry * (1.0 / alpha));
				}
			}
		}
		MatrixRmn::MultiplyTranspose(VD, U, JcurrentPinv);

		// calcolo la proiezione J1
		MatrixRmn::Multiply(JcurrentPinv, J1, JProjPre);

		for (int j = 0; j < JProjPre.getNumColumns(); j++)
			for (int i = 0; i < JProjPre.getNumRows(); i++) {
				double temp = JProjPre.Get(i, j);
				JProjPre.Set(i, j, -1.0 * temp);
			}
		JProjPre.AddToDiagonal(diagMatIdentity);
	}

	//task priority strategy
	for (int i = 1; i < skeleton->getNumEffector(); i++) {
		// costruisco matrice Jcurrent (Ji)
		MatrixRmn Jcurrent(2, J.getNumColumns());
		for (int j = 0; j < J.getNumColumns(); j++)
			for (int k = 0; k < 2; k++)
				Jcurrent.Set(k, j, J.Get(k + 2 * i, j));

		// costruisco il vettore dScurrent
		VectorRn dScurrent(2);
		for (int k = 0; k < 2; k++)
			dScurrent.Set(k, dS.Get(k + 2 * i));

		// Moltiplico Jcurrent per la proiezione di J(i-1)
		MatrixRmn Jdst(Jcurrent.getNumRows(), JProjPre.getNumColumns());
		MatrixRmn::Multiply(Jcurrent, JProjPre, Jdst);

		// Calcolo la pseudoinversa di Jdst
		MatrixRmn UU(Jdst.getNumRows(), Jdst.getNumRows()), VV(Jdst.getNumColumns(), Jdst.getNumColumns());
		VectorRn ww(min(Jdst.getNumRows(), Jdst.getNumColumns()));
//.........这里部分代码省略.........

 IKTrajectoryHelperInternalData()
 {
     m_endEffectorTargetPosition.SetZero();
     m_nullSpaceVelocity.SetZero();
     m_dampingCoeff.SetZero();
 }