bool InvertMatrix(A2D<ID_T>& vData)
{
	ID_T d;
	A2D<ID_T> vTmp(vData.Rows(),vData.Cols());
	int i,j;
	for(i=0;i<vData.Rows();i++)
		for(j=0;j<vData.Cols();j++)
			vTmp[i][j]=vData[i][j];
	
	int N = vData.Rows();
	vector<int> indx(N);
	vector<ID_T> col(vData.Cols());
	if(!NR::ludcmp(vTmp,indx,d))//Decompose the matrix just once.
	{	Write2Log("Couldn't invert matrix!");
		return false;		
	}
	for(j=0;j<N;j++) 
	{ //Find inverse by columns.
		for(i=0;i<N;i++)
			col[i]=0.0;
		col[j]=1.0;
		NR::lubksb(vTmp,indx,&col[0]);
		for(i=0;i<N;i++) 
			vData[i][j]=col[i];
	}

	return true;
}
//gets the inverse of the covariance matrix for a cluster
//input vFloat has each row as a data vector, each column as a dimension, to get data vector i do vFloat[i*iCols]
//bool Clust2CovarMatInv(vector<vector< ID_T > >& vCovarMat,vector<ID_T>& vMean, vector<int>& vClustIDs,int iClustID,vector<double>& vFloat,int iRows,int iCols,int& iClustSz)
bool Clust2CovarMatInv(A2D< ID_T >& vCovarMat,vector<ID_T>& vMean, vector<int>& vClustIDs,int iClustID,vector<double>& vFloat,int iRows,int iCols,int& iClustSz)
{
	int i = 0, j = 0;
		iClustSz = count(vClustIDs.begin(),vClustIDs.end(),iClustID);
	if(!iClustSz)
		return false;
	A2D<double> vClustData(iClustSz,iCols);
	vClustData.Fill(0.0);
	for(i=0;i<iRows;i++)
		if(vClustIDs[i]==iClustID)
			copy(&vFloat[i*iCols],&vFloat[i*iCols+iCols],vClustData[j++]);

	Write2LogPlain("clust %d sz=%d\n",iClustID,iClustSz);
	
	CovarMat(vClustData,iClustSz,iCols,vCovarMat,vMean);
		
	if(false){
	Write2LogPlain("clust %d covar mat %dX%d\n",iClustID,vCovarMat.Rows(),vCovarMat.Cols());
	for(i=0;i<vCovarMat.Rows();i++){
		for(j=0;j<vCovarMat.Cols();j++){
			Write2LogPlain("%g ",vCovarMat[i][j]);
		}
		Write2LogPlain("\n");
	}
	}

#ifdef _DEBUG
	A2D<ID_T> mtmp(vCovarMat);
	
	bool b = InvertMatrix(vCovarMat);

	Write2LogPlain("inv covar mat");
	for(i=0;i<vCovarMat.Rows();i++){
		for(j=0;j<vCovarMat.Cols();j++){
			Write2LogPlain("%g ",vCovarMat[i][j]);
		}
		Write2LogPlain("\n");
	}
	
	A2D<ID_T> vident;
	MatMult(mtmp,vCovarMat,vident);
	
	Write2LogPlain("is this identity matrix?");
	for(i=0;i<vident.Rows();i++){
		for(j=0;j<vident.Cols();j++){
			Write2LogPlain("%g ",vident[i][j]);
		}
		Write2LogPlain("\n");
	}
	
	return b;
#else
	return InvertMatrix(vCovarMat);
#endif
}
void MatMult(A2D<ID_T>& a, A2D<ID_T>& b,A2D<ID_T>& out)
{	int iRowsA = 0, iColsA = 0, iRowsB = 0 , iColsB = 0;
	try
	{
		iRowsA = a.Rows(); if(!iRowsA) return; iColsA = a.Cols();
		iRowsB = b.Rows(); if(!iRowsB) return; iColsB = b.Cols();
		if(iColsA!=iRowsB)
			return;
		out.Init(iRowsA,iColsB); out.Fill(0.0);
		int i,j,k;
		for(i=0;i<iRowsA;i++)
			for(j=0;j<iColsB;j++)
				for(k=0;k<iColsA;k++)
					out[i][j] += a[i][k] * b[k][j];
	}
	catch(...)
	{
		Write2Log("Exception in MatMult iRowsA=%d iColsA=%d iRowsB=%d iColsB=%d",iRowsA,iColsA,iRowsB,iColsB);
	}
}