///////////////////////////////////////////////////////////////////////////
// ASVR: Active Support Vector Regression                                //
// Copyright 2002 David R. Musicant and Alexander Feinberg.              //
// This software is free for academic use only.                          //
// For commercial use, contact dmusican@carleton.edu.                    //
///////////////////////////////////////////////////////////////////////////
// DataSource.cpp: Code for the objects dealing with data storage.       //
///////////////////////////////////////////////////////////////////////////

#include "DataSource.H"
#include "Utilities.H"
#include <typeinfo>

#include <iostream>

//////////////////////////////////////////////////////////////////////
// MatrixType methods
//////////////////////////////////////////////////////////////////////

// Default constructor - this one does not make extra room
MatrixType::MatrixType(int rows, int cols) {
  this->rows = rows;
  this->cols = cols;
}

//////////////////////////////////////////////////////////////////////
// DenseMatrixType methods
//////////////////////////////////////////////////////////////////////

// Default constructor
template<class T>DenseMatrixType<T>::DenseMatrixType(int rows, int cols) 
  : MatrixTemplate<T>(rows,cols) {
  int i,j;
  dataMatrix = new T*[rows];
  assert(dataMatrix);
  for (i=0; i < rows; i++) {

    dataMatrix[i] = new T[cols];
    assert(dataMatrix[i]);
    for (j=0; j < cols; j++)
      dataMatrix[i][j] = 0;
  }
}

// Destructor
template<class T>DenseMatrixType<T>::~DenseMatrixType()
{
  int i;
  for (i=0; i < rows; i++)
    delete [] dataMatrix[i];
  delete [] dataMatrix;
}

// Overloaded operator ()
template<class T>double DenseMatrixType<T>::Show(int row, int col) {
  return (double)dataMatrix[row][col];
}

// Used for debugging purposes
template<class T>void DenseMatrixType<T>::Display()
{
  int i,j;
  for (i=0; i < rows; i++) {
    for (j=0; j < cols; j++)
      PRINT("%g ",(double)dataMatrix[i][j]);
    PRINT("\n");
  }
}

// Add a term onto the diagonal of the given matrix, in place
template<class T>void DenseMatrixType<T>::DiagAddInPlace(double addOn) {
  T addOnT = (T)addOn;
  for (int i=0; i < rows; i++)
    dataMatrix[i][i] += addOnT;
}

// Multiply a constant term times an entire matrix
template<class T>void DenseMatrixType<T>::MultiplyInPlace(double factor) {
  for (int i=0; i < rows; i++) {
    T *spot = dataMatrix[i];
    for (int j=0; j < cols; j++)
      spot[j] = (T)(factor * spot[j]);
  }
}

// Compute the inverse using LU decomposition
// Overwrites original matrix, uses CLAPACK tools
template<class T>void DenseMatrixType<T>::GaussianInverse() {

  // Represent matrix in columnwise format for CLAPACK
  double *A = new double[rows*cols];

  //cout << '0' << flush;

  int i, j, k;
  k = 0;
  for (j=0; j < cols; j++)
    for (i=0; i < rows; i++)
      {A[k++] = dataMatrix[i][j]; /*cout << k << endl << flush;*/}

  // Get other information for routines
  int m = rows;
  int n = cols;
  int lda = rows;
  int *ipiv = new int[rows];
  int info;

  dgetrf_(&m, &n, A, &lda, ipiv, &info);

  if (info)
    throw (const char *)"GaussianInverse: Nonzero info on factorization.";
  int lwork = 1000;
  double *work = new double[lwork];
  dgetri_(&n, A, &lda, ipiv, work, &lwork, &info);

  if (info)
    throw (const char *)"GaussianInverse: Nonzero info on inverse.";

  // Dump inverse back into regular matrix. Since in the lower
  // triangular part, need to duplicate.
  for (j=0; j < cols; j++)
    for (i=0; i < rows; i++)
      dataMatrix[i][j] = (T)(A[j*rows+i]);

  delete [] A;
  delete [] ipiv;
  delete [] work;
}

// Add a matrix onto this one
template<class T>void DenseMatrixType<T>::AddInPlace(MatrixType *m) {
  if (rows!=m->rows || cols!=m->cols)
    throw (const char *)"AddInPlace: Bad matrix sizes.";

  int i,j;
  DenseMatrixType<T> *dMat;
  if ((dMat=dynamic_cast<DenseMatrixType<T>*>(m)))
    for (i=0; i < rows; i++)
      for (j=0; j < cols; j++)
	dataMatrix[i][j] += (T)(dMat->dataMatrix[i][j]);
  else
    throw (const char *)"AddInPlace: Undefined matrix type";
}

// Predict outputs of this matrix given w and gamma
template<class T>void DenseMatrixType<T>::Predict(Vector *w, double gamma,
						     DenseVector *results) {
	int i;
  if (w->Size() < cols -1 || w->Size() > cols || results->Size() != rows)
    throw (const char *)"Predict: Wrong matrix sizes";

  // Handle case where we should ignore last column (actual regression values)
  if (w->Size() == cols) {
    
    MultAB(this,w,results);
    
    for (i=0; i < rows; i++)
      results->Assign(i,results->dataMatrix[i]+gamma);
  }
  else if (w->Size() == cols-1) {
    DenseVector *bigW = new DenseVector(cols);
    for (int i=0; i < cols-1; i++)
      bigW->Assign(i,(*w)[i]);
    bigW->Assign(cols-1,0);

    MultAB(this,bigW,results);
    for (i=0; i < rows; i++)
      results->Assign(i,results->dataMatrix[i]+gamma);

    delete bigW;
  }
}


// Compute training (or testing) accuracy of this matrix given w and gamma
template<class T>double DenseMatrixType<T>::Accuracy(Vector *w, double gamma,
						     Vector *y) {
	int i;
  if (w->Size() != cols)
    throw (const char *)"Accuracy: Wrong matrix sizes";

  DenseVector *predict = new DenseVector(rows);
  MultAB(this,w,predict);

  for (i=0; i < rows; i++)
    predict->Assign(i,predict->dataMatrix[i]+gamma);

  DenseVector *diff = new DenseVector(rows);
  predict->Copy(diff);
  diff->SubtractInPlace(y);
  for (i=0;i<rows;i++) PRINT("%g %g\n",predict->dataMatrix[i],diff->dataMatrix[i]);

  double numerator = 0;
  double denominator = 0;

  // Error rate is norm(y-predictions)/norm(y)
  for (i=0; i < rows; i++) {
    numerator = numerator + pow((*diff)[i],2);
    denominator = denominator +  pow((*y)[i],2);
  }
  numerator = sqrt(numerator);
  denominator = sqrt(denominator);

  delete predict;
  delete diff;

  return numerator/denominator;

}

// Normalize the data matrix, and return the mean and std for use with
// the testing set.
template<class T>
void DenseMatrixType<T>::GetMeanStd(double *mean, double *std) {
  int i,j;
  for (j=0; j < cols; j++) {
    mean[j] = 0;
    std[j] = 0;
  }
  // Use variance = avg(x^2)-(avg(x))^2
  for (i=0; i < rows; i++)
    for (j=0; j < cols; j++) {
      mean[j]+=dataMatrix[i][j]; // correct sign
      std[j]+=dataMatrix[i][j]*dataMatrix[i][j];
    }
  for (j=0; j < cols; j++) {
    mean[j] = mean[j] / rows;
    // Sample standard deviation
    std[j] = sqrt(std[j]/(rows-1) - mean[j]*mean[j]*rows/(rows-1));
  }
}

// Given a mean and a standard deviation, normalize the matrix.
template<class T>
void DenseMatrixType<T>::Normalize(double *mean, double *std) {
  int i,j;
  for (i=0; i < rows; i++) {
    for (j=0; j < cols; j++)
      if (std[j])
	dataMatrix[i][j] = (T)((dataMatrix[i][j]-mean[j])/std[j]);
      else
	// Must be constant, make it zero effectively
	dataMatrix[i][j] = (T)(dataMatrix[i][j]-mean[j]);
  }
}

// Sum all the elements in the matrix
template<class T>double DenseMatrixType<T>::Sum() {
  double sum = 0;
  for (int i=0; i < rows; i++)
    for (int j=0; j < cols; j++)
      sum += dataMatrix[i][j];
  return sum;
}

// Sum the square of all elements in the matrix
template<class T>double DenseMatrixType<T>::Square() {
  double sum = 0;
  for (int i=0; i < rows; i++)
    for (int j=0; j < cols; j++)
      sum += pow(dataMatrix[i][j],2);
  return sum;
}

// Compute the 1-norm for the matrix
template<class T>double DenseMatrixType<T>::Norm1() {
  // Compute the sum for each column
  T *sum = new T[cols];
  int i,j;
  for (j=0; j < cols; j++)
    sum[j] = 0;
  for (i=0; i < rows; i++)
    for (j=0; j < cols; j++)
      if (dataMatrix[i][j] >= 0)
	sum[j] += dataMatrix[i][j];
      else
	sum[j] -= dataMatrix[i][j];
  
  // Find the maximum sum and return it
  double norm1 = 0;
  for (j=0; j < cols; j++)
    if (norm1 < sum[j])
      norm1 = sum[j];
  return norm1;

  delete [] sum;
}

// Compute the 1-norm of the transpose of the matrix
template<class T>double DenseMatrixType<T>::NormT1() {
  // Compute the sum for each row
  T *sum = new T[rows];
  int i,j;
  for (i=0; i < rows; i++)
    sum[i] = 0;
  for (i=0; i < rows; i++)
    for (j=0; j < cols; j++)
      if (dataMatrix[i][j] >= 0)
	sum[i] += dataMatrix[i][j];
      else
	sum[i] -= dataMatrix[i][j];
  
  // Find the maximum sum and return it
  double normT1 = 0;
  for (i=0; i < rows; i++)
    if (normT1 < sum[i])
      normT1 = sum[i];
  return normT1;

  delete [] sum;
}


//////////////////////////////////////////////////////////////////////
// Vector methods
//////////////////////////////////////////////////////////////////////

// Abstract constructor
Vector::Vector(int cols) {
  this->cols = cols;
}

//////////////////////////////////////////////////////////////////////
// Dense Vector methods
//////////////////////////////////////////////////////////////////////

// Main constructor
DenseVector::DenseVector(int cols) : Vector(cols) {
  int j;
  dataMatrix = new double[cols];
  assert(dataMatrix);
  for (j=0; j < cols; j++)
    dataMatrix[j] = 0;
}

// Destructor
DenseVector::~DenseVector() {
  delete [] dataMatrix;
}

// Overloaded operator []
double DenseVector::operator[](int col) {
  return dataMatrix[col];
}

// Overloaded operator >=
bool DenseVector::operator>=(double value) {
  for (int i=0; i < Size(); i++)
    if (dataMatrix[i] < value)
      return false;
  return true;
}

// Overloaded operator <=
bool DenseVector::operator<=(double value) {
  for (int i=0; i < Size(); i++)
    if (dataMatrix[i] > value)
      return false;
  return true;
}

// Assign a value to a location in the vector
void DenseVector::Assign(int col, double value) {
  dataMatrix[col] = value;
}

// Return the number of columnsi n the vector
int DenseVector::Size() {
  return cols;
}

// Display the vector, used for debugging purposes
void DenseVector::Display()
{
  int i;
  for (i=0; i < Size(); i++)
    PRINT("%g\n",dataMatrix[i]);
  PRINT("\n");
}

// Display part of the vector, used for debugging purposes
void DenseVector::Display(int m)
{
  if (m > Size())
    throw (const char *)"Display: m too big.";
  int i;
  for (i=0; i < m; i++)
    PRINT("%g ",dataMatrix[i]);
  PRINT("\n");
}

// Return a set of indices indicating which values are nonbasic,
// subject to a tolerance
set<int> DenseVector::findTolNonBasic(double tol) {
  set<int> nonbasic;
  for (int i=0; i < cols; i++)
    if (dataMatrix[i] < tol)
      nonbasic.insert(i);
  return nonbasic;
}

// Return a set of indices indicating which values are basic,
// subject to a tolerance
set<int> DenseVector::findTolBasic(double tol) {
  set<int> basic;
  for (int i=0; i < cols; i++)
    if (dataMatrix[i] >= tol)
      basic.insert(i);
  return basic;
}

// Subtract two vectors, place answer in the first one
void DenseVector::SubtractInPlace(Vector *m2) {
  if (Size() != m2->Size())
    throw (const char *)"SubtractInPlace: Bad vector sizes.";
  // Dense subtraction
  DenseVector *dVec;
  if ((dVec=dynamic_cast<DenseVector*>(m2))) {
    for (int i=0; i < Size(); i++)
      dataMatrix[i] -= dVec->dataMatrix[i];
  } else
    throw (const char *)"SubtractInPlace: Undefined vector type";
}

// Add two vectors, place answer in the first one
void DenseVector::AddInPlace(Vector *m2) {
  if (Size() != m2->Size())
    throw (const char *)"SubtractInPlace: Bad vector sizes.";
  // Dense subtraction
  DenseVector *dVec;
  if ((dVec=dynamic_cast<DenseVector*>(m2))) {
    for (int i=0; i < Size(); i++)
      dataMatrix[i] += dVec->dataMatrix[i];
  } else
    throw (const char *)"SubtractInPlace: Undefined vector type";
}

// Add constant value to vector
void DenseVector::AddInPlace(double value) {
  for (int i=0; i < Size(); i++)
    dataMatrix[i] += value;
}

// Multiply by a scalar
void DenseVector::MultiplyInPlace(double factor) {
  for (int i=0; i < cols; i++)
    dataMatrix[i] *= factor;
}

// Compute the plus function for the given vector, in place
void DenseVector::PlusFunctionInPlace() {
  for (int i=0; i < cols; i++)
    if (dataMatrix[i] < 0)
      dataMatrix[i] = 0;
}

// Copy one vector into another
void DenseVector::Copy(Vector *m) {
  if (cols > m->Size())
    throw (const char *)"Copy: Bad vector sizes.";
  // Dense
  DenseVector *dVec;
  if ((dVec=dynamic_cast<DenseVector*>(m))) {
    for (int i = 0; i < cols; i++)
      dVec->dataMatrix[i] = dataMatrix[i];
  } else
    throw (const char *)"Copy: Undefined vector type";
}

// Subtract every element in this vector from a given scalar value
void DenseVector::SubtractInPlaceFrom(double value) {
  for (int i=0; i < cols; i++)
    dataMatrix[i] = value - dataMatrix[i];
}

// Assign a subset of this vector's indices to be equal to another
void DenseVector::Update(Vector *m, set<int> indices) {
  set<int>::iterator iterI;
  iterI = indices.end();
  iterI--;
  if (*(iterI) > cols)
    throw (const char *)"Update: Bad set of indices.";

  int i;
  DenseVector *dVec;
  if ((dVec=dynamic_cast<DenseVector*>(m)))
    for (i=0, iterI=indices.begin(); iterI!=indices.end(); i++, iterI++)
      dataMatrix[*iterI] = dVec->dataMatrix[i];
  else
    throw (const char *)"Update: Undefined vector type";
}

// Set to zero the locations in the vector corresponding to the given
// indices.
void DenseVector::Zero(set<int> indices) {
  set<int>::iterator iterI;
  iterI = indices.end();
  iterI--;
  if (*(iterI) > cols)
    throw (const char *)"Update: Bad set of indices.";

  int i;
  for (i=0, iterI=indices.begin(); iterI!=indices.end(); i++, iterI++)
    dataMatrix[*iterI] = 0;
}

// Strip a vector to produce a smaller vector, with the given indices
void DenseVector::Strip(Vector *m, set<int> indices) {
  set<int>::iterator iterI;
  iterI = indices.end();
  iterI--;
  if (*(iterI) > cols)
    throw (const char *)"Update: Bad set of indices.";
  if (m->Size() != (int)indices.size())
    throw (const char *)"Update: Bad size for stripped vector.";

  int i;
  DenseVector *dVec;
  if ((dVec=dynamic_cast<DenseVector*>(m)))
    for (i=0, iterI=indices.begin(); iterI!=indices.end(); i++, iterI++)
      dVec->dataMatrix[i] = dataMatrix[*iterI];
  else
    throw (const char *)"Update: Undefined vector type";
}

// Return the sum of the squares of every element in the vector
double DenseVector::Square() {
  double norm=0, value;
  for (int i=0; i < cols; i++) {
    value = dataMatrix[i];
    norm += value*value;
  }
  return norm;
}

// Return the sum over all elements in the vector
double DenseVector::Sum() {
  double sum=0;
  for (int i=0; i < cols; i++)
    sum += dataMatrix[i];
  return sum;
}

// Compute the scalar product between this vector and another one
double DenseVector::ScalarProduct(Vector *m2) {

  int i;
  double product = 0;
  DenseVector *dVec;
  if ((dVec=dynamic_cast<DenseVector*>(m2))) {
    for (i=0; i < Size(); i++)
      product += dataMatrix[i]*dVec->dataMatrix[i];
    return product;
  }
  else
    throw (const char *)"Update: Undefined vector type";

}

//////////////////////////////////////////////////////////////////////
// General methods
//////////////////////////////////////////////////////////////////////

// Compute the matrix product between two matrices
template<class T1, class T2, class T3>
void MultAB(MatrixTemplate<T1> *m1, MatrixTemplate<T2> *m2,
	    MatrixTemplate<T3> *output) {

  if (m1->cols!=m2->rows || m1->rows!=output->rows || m2->cols!=output->cols)
    throw (const char *)"Multiply: Bad matrix sizes.";

  int i,j,k;
  DenseMatrixType<T1> *d1;
  DenseMatrixType<T2> *d2;
  DenseMatrixType<T3> *dout;
  if ((d1=dynamic_cast<DenseMatrixType<T1>*>(m1))) {
    if ((d2=dynamic_cast<DenseMatrixType<T2>*>(m2))) {
      if ((dout=dynamic_cast<DenseMatrixType<T3>*>(output))) {
	T1 **matrix1 = d1->dataMatrix;
	T2 **matrix2 = d2->dataMatrix;
	T3 **matrixOut = dout->dataMatrix;
	int rows1 = d1->rows;
        int cols1 = d1->cols;
	int cols2 = d2->cols;
	for (i=0; i < rows1; i++) {
	  T3 *matrixOutI = matrixOut[i];
	  for (j=0; j < cols2; j++)
	    matrixOutI[j]=0;
	}

	for (k=0; k < cols1; k++) {
	  T2 *matrix2K = matrix2[k];
	  for (i=0; i < rows1; i++) {
	    T3 matrix1IK = matrix1[i][k];
	    if (matrix1IK) {
	      T3 *matrixOutI = matrixOut[i];
	      for (j=0; j < cols2; j++) {
		//dout->dataMatrix[i][j] += 
		//d1->dataMatrix[i][k]*d2->dataMatrix[k][j];
		matrixOutI[j] += matrix1IK * (T3)matrix2K[j];
	      } // for j
	    } // if-matrix1IK
	  } // for i
	} // for k
      } //if-dout
    } // if-d2
  } //if-d1
  else
    throw (const char *)"MultAB: Undefined combination";
}

// Compute the matrix product between a subset of rows of one matrix
// and another full matrix
template<class T1, class T2, class T3>
void MultAB(MatrixTemplate<T1> *m1, set<int> set1,
	    MatrixTemplate<T2> *m2, MatrixTemplate<T3> *output) {

  if (m1->cols!=m2->rows || m2->cols!=output->cols)
    throw (const char *)"Multiply: Bad matrix sizes.";
  if (!set1.size()) {
    ThrowIt("Multiply: Empty set.");
  }
  set<int>::iterator iterI;
  iterI = set1.end();
  iterI--;
  if (*iterI > m1->rows)
    throw (const char *)"Multiply: Bad set indices.";

  DenseMatrixType<T1> *d1;
  DenseMatrixType<T2> *d2;
  DenseMatrixType<T3> *dout;
  int i,j,k;
  if ((d1=dynamic_cast<DenseMatrixType<T1>*>(m1))) {
    if ((d2=dynamic_cast<DenseMatrixType<T2>*>(m2))) {
      if ((dout=dynamic_cast<DenseMatrixType<T3>*>(output))) {
	T1 **matrix1 = d1->dataMatrix;
	T2 **matrix2 = d2->dataMatrix;
	T3 **matrixOut = dout->dataMatrix;
	int cols1 = d1->cols;
	int cols2 = d2->cols;
	for (i=0, iterI=set1.begin(); iterI!=set1.end(); i++, iterI++)
	  for (j=0; j < m2->cols; j++)
	    matrixOut[i][j]=0;
	for (k=0; k < cols1; k++)
	  for (i=0, iterI=set1.begin(); iterI!=set1.end(); i++, iterI++) {
	    T3 matrix1IterIK = matrix1[*iterI][k];
	    if (matrix1IterIK)
	      for (j=0; j < cols2; j++) {
		matrixOut[i][j] += matrix1IterIK * (T3)matrix2[k][j];
	      } // for-j
	  }
      } //if-dout
    } // if-d2
  } //if-d1
  else
    throw (const char *)"MultAB: Undefined combination";
}

// Compute the matrix product between a subset of rows of one matrix
// and a full vector
template<class T1>
void MultAB(MatrixTemplate<T1> *m1, set<int> set1, Vector *m2, 
	    Vector *output) {

  if (m1->cols!=m2->Size())
    throw (const char *)"Multiply: Bad matrix sizes.";
  set<int>::iterator iterI;
  iterI = set1.end();
  iterI--;
  if (*iterI > m1->rows)
    throw (const char *)"Multiply: Bad set indices.";

  DenseMatrixType<T1> *d1;
  DenseVector *d2, *dout;
  int i,k;
  if ((d1=dynamic_cast<DenseMatrixType<T1>*>(m1))) {
    if ((d2=dynamic_cast<DenseVector*>(m2))) {
      if ((dout=dynamic_cast<DenseVector*>(output))) {
	for (i=0, iterI=set1.begin(); iterI!=set1.end(); i++, iterI++) {
	  dout->dataMatrix[i]=0;
	  for (k=0; k < m1->cols; k++)
	    dout->dataMatrix[i] += 
	      (double)(d1->dataMatrix[*iterI][k]) * d2->dataMatrix[k];
	} // for-i
      } //if-dout
    } // if-d2
  } //if-d1
  else
    throw (const char *)"MultAB: Undefined combination";
}

// Compute the matrix product between a matrix and a vector
template<class T1>
void MultAB(MatrixTemplate<T1> *m1, Vector *m2, Vector *output) {

  if (m1->cols!=m2->Size() || m1->rows!=output->Size())
    throw (const char *)"Multiply: Bad matrix sizes.";

  DenseMatrixType<T1> *d1;
  DenseVector *d2, *dout;
  int i,k;
  if ((d1=dynamic_cast<DenseMatrixType<T1>*>(m1))) {
    if ((d2=dynamic_cast<DenseVector*>(m2))) {
      if ((dout=dynamic_cast<DenseVector*>(output))) {
	for (i=0; i < d1->rows; i++) {
	  dout->dataMatrix[i]=0;
	  for (k=0; k < d1->cols; k++)
	    dout->dataMatrix[i] +=
	      (double)(d1->dataMatrix[i][k]) * d2->dataMatrix[k];
	} // for
      } // if-output
    } // if-m2
  } // if-m1
  else
    throw (const char *)"MultAB: Undefined combination";

}

// Compute the matrix product between the transpose of one matrix and
// another
template<class T1, class T2, class T3>
void MultAtB(MatrixTemplate<T1> *m1, MatrixTemplate<T2> *m2,
	     MatrixTemplate<T3> *output) {

  if (m1->rows!=m2->rows || m1->cols!=output->rows || m2->cols!=output->cols)
    throw (const char *)"Multiply: Bad matrix sizes.";
  
  DenseMatrixType<T1> *d1;
  DenseMatrixType<T2> *d2;
  DenseMatrixType<T3> *dout;
  int i,j,k;
  if ((d1=dynamic_cast<DenseMatrixType<T1>*>(m1))) {
    if ((d2=dynamic_cast<DenseMatrixType<T2>*>(m2))) {
      if ((dout=dynamic_cast<DenseMatrixType<T3>*>(output))) {
	T1 **matrix1 = d1->dataMatrix;
	T2 **matrix2 = d2->dataMatrix;
	T3 **matrixOut = dout->dataMatrix;
	int cols1=m1->cols;
	int cols2=m2->cols;
	int rows1=m1->rows;
	for (i=0; i < cols1; i++)
	  for (j=0; j < cols2; j++)
	    matrixOut[i][j]=0;
	for (k=0; k < rows1; k++) {
	  T2 *spot2 = matrix2[k];
	  T1 *spot1k = matrix1[k];
	  for (i=0; i < cols1; i++) {
	    T3 *spotOut = matrixOut[i];
	    T3 matrix1ki = spot1k[i]; // cast up to final answer-faster
	    if (matrix1ki)
	      for (j=0; j < cols2; j++) {
		//  matrixOut[i][j] += matrix1[k][i]*matrix2[k][j];
		T3 matrix2kj = spot2[j];
		if (matrix2kj)
		  spotOut[j] += matrix1ki * matrix2kj;
	      } // for-j
	  } // for-i
	} // for-k
      } // if-output
    } // if-m2
  } // if-m1
  else
    throw (const char *)"MultAtB: Undefined combination";
}

// Compute the matrix product between the transpose of one matrix and
// a vector
template<class T1>
void MultAtB(MatrixTemplate<T1> *m1, Vector *m2, Vector *output) {

  if (m1->rows!=m2->Size() || m1->cols!=output->Size())
    throw (const char *)"Multiply: Bad matrix sizes.";

  int i,k;
  DenseMatrixType<T1> *d1;
  DenseVector *d2, *dout;
  if ((d1=dynamic_cast<DenseMatrixType<T1>*>(m1))) {
    if ((d2=dynamic_cast<DenseVector*>(m2))) {
      if ((dout=dynamic_cast<DenseVector*>(output))) {
	int rows = d1->rows;
	int cols = d1->cols;
	T1 **matrix1 = d1->dataMatrix;
	double *matrix2 = d2->dataMatrix;
	double *matrixOut = dout->dataMatrix;
	for (i=0; i < cols; i++)
	  matrixOut[i]=0;
	for (k=0; k < rows; k++) {
	  double matrix2k = matrix2[k];
	  T1 *spot1k = matrix1[k];
	  if (matrix2k)
	    for (i=0; i < cols; i++) {
	      //matrixOut[i] += matrix1[k][i] * matrix2[k];
	      double spot1ki = spot1k[i];
	      if (spot1ki)
		matrixOut[i] += spot1ki * matrix2k;
	    } // for-i
	} // for-k
      } // if-output
    } // if-d2
  } // if-d1
  else
    throw (const char *)"MultAtB: Undefined combination";

}

// Compute the matrix product between the transpose of one matrix and
// another. Do the multiplications with only those rows indicated in
// the sets.
template<class T1, class T2, class T3>
void MultAtB(MatrixTemplate<T1> *m1, set<int> set1,
	     MatrixTemplate<T2> *m2, set<int> set2,
	     MatrixTemplate<T3> *output) {

  if (m1->rows!=m2->rows || m1->cols!=output->rows || m2->cols!=output->cols)
    throw (const char *)"Multiply: Bad matrix sizes.";
  set<int>::iterator set1last, set2last;
  set1last=set1.end();  set2last = set2.end();
  set1last--;  set2last--;
  if (*(set1last) > m1->rows || *(set2last) > m2->rows)
    throw (const char *)"Multiply: Bad set indices.";

  DenseMatrixType<T1> *d1;
  DenseMatrixType<T2> *d2;
  DenseMatrixType<T3> *dout;
  set<int>::iterator iterI,iterJ;  
  int i,j,k1,k2;
  if ((d1=dynamic_cast<DenseMatrixType<T1>*>(m1))) {
    if ((d2=dynamic_cast<DenseMatrixType<T2>*>(m2))) {
      if ((dout=dynamic_cast<DenseMatrixType<T3>*>(output))) {
	T1 **matrix1 = d1->dataMatrix;
	T2 **matrix2 = d2->dataMatrix;
	T3 **matrixOut = dout->dataMatrix;
	int cols1=d1->cols;
	int cols2=d2->cols;
	for (i=0; i < d1->cols; i++)
	  for (j=0; j < d2->cols; j++)
	    dout->dataMatrix[i][j] = 0;
	for (iterI=set1.begin(), iterJ=set2.begin();
	     iterI!=set1.end(); iterI++,iterJ++) {
	  k1 = *iterI; k2 = *iterJ;
	  T1 *spot1 = matrix1[k1];
	  T2 *spot2 = matrix2[k2];
	  for (i=0; i < cols1; i++) {
	    T3 *spotOut = matrixOut[i];
	    T3 matrix1k1i = spot1[i];
	    if (matrix1k1i)
	      for (j=0; j < cols2; j++) {
		// matrixOut[i][j] += matrix1[k1][i] * matrix2[k2][j];
		T3 matrix2k2j = spot2[j];
		if (matrix2k2j)
		  spotOut[j] += matrix1k1i * matrix2k2j;
	      } // for j
	  } // for i
	} // for iterI-iterJ
      } // if-output
    } // if-m2
  } // if-m1
  else
    throw (const char *)"MultAtB: Undefined combination";
}

// Compute the matrix product between the transpose of a matrix and a
// vector. Do the multiplications with only those rows indicated in
// the sets.
template<class T1>
void MultAtB(MatrixTemplate<T1> *m1, set<int> set1,
	     Vector *m2, set<int> set2, Vector *output) {

  if (m1->rows!=m2->Size() || m1->cols!=output->Size())
    throw (const char *)"Multiply: Bad matrix sizes.";
  set<int>::iterator set1last, set2last;
  set1last=set1.end();  set2last = set2.end();
  set1last--;  set2last--;
  if (*(set1last) > m1->rows || *(set2last) > m2->Size())
    throw (const char *)"Multiply: Bad set indices.";

  DenseMatrixType<T1> *d1;
  DenseVector *d2, *dout;
  set<int>::iterator iterI,iterJ;  
  int i,k1,k2;
  if ((d1=dynamic_cast<DenseMatrixType<T1>*>(m1))) {
    if ((d2=dynamic_cast<DenseVector*>(m2))) {
      if ((dout=dynamic_cast<DenseVector*>(output))) {
	double *matrixOut = dout->dataMatrix;
	T1 **matrix1 = d1->dataMatrix;
	double *matrix2 = d2->dataMatrix;
	int cols1 = d1->cols;
	for (i=0; i < cols1; i++)
	  matrixOut[i]=0;
	for (iterI=set1.begin(), iterJ=set2.begin();
	     iterI!=set1.end(); iterI++,iterJ++) {
	  k1 = *iterI; k2 = *iterJ;
	  T1 *spotk1 = matrix1[k1];
	  double matrixk2 = matrix2[k2];
	  if (matrixk2)
	    for (i=0; i < cols1; i++) {
	      matrixOut[i] += (double)(spotk1[i]) * matrixk2;
	    } // for iterI-iterJ
	} // for-i
      } // if-output
    } // if-m2
  } // if-m1
  else
    throw (const char *)"MultAtB: Undefined combination";
}

// Compute the matrix product between the transpose of a matrix and a
// vector. Do the multiplications with only those rows indicated in
// the sets.
template<class T1>
void MultAtB(MatrixTemplate<T1> *m1, set<int> set1,
	     Vector *m2, Vector *output) {

  if ((int)set1.size()!=m2->Size() || m1->cols!=output->Size())
    throw (const char *)"Multiply: Bad matrix sizes.";
  set<int>::iterator set1last;
  set1last=set1.end();
  set1last--;
  if (*(set1last) > m1->rows)
    throw (const char *)"Multiply: Bad set indices.";

  DenseMatrixType<T1> *d1;
  DenseVector *d2, *dout;
  set<int>::iterator iterI;
  int i,j,k1,k2;
  if ((d1=dynamic_cast<DenseMatrixType<T1>*>(m1))) {
    if ((d2=dynamic_cast<DenseVector*>(m2))) {
      if ((dout=dynamic_cast<DenseVector*>(output))) {
	double *matrixOut = dout->dataMatrix;
	T1 **matrix1 = d1->dataMatrix;
	double *matrix2 = d2->dataMatrix;
	int cols1 = d1->cols;
	for (i=0; i < cols1; i++)
	  matrixOut[i]=0;
	for (iterI=set1.begin(), j=0;
	     iterI!=set1.end(); iterI++,j++) {
	  k1 = *iterI; k2 = j;
	  T1 *spotk1 = matrix1[k1];
	  double matrixk2 = matrix2[k2];
	  if (matrixk2)
	    for (i=0; i < cols1; i++) {
	      matrixOut[i] += (double)(spotk1[i]) * matrixk2;
	    } // for iterI-iterJ
	} // for-i
      } // if-output
    } // if-m2
  } // if-m1
  else
    throw (const char *)"MultAtB: Undefined combination";
}

// Compute the matrix product between the transpose of one matrix and
// another. Do the multiplications with only those rows indicated in
// the sets.
template<class T1, class T2, class T3>
void MultAtB(MatrixTemplate<T1> *m1, set<int> set1,
	     MatrixTemplate<T2> *m2, MatrixTemplate<T3> *output) {

  if (m1->cols!=output->rows || m2->cols!=output->cols)
    throw (const char *)"Multiply: Bad matrix sizes.";

  set<int>::iterator iterI;
  iterI = set1.end();
  iterI--;
  if (*iterI > m1->rows)
    throw (const char *)"Multiply: Bad set indices.";

  set<int>::iterator iterK;
  DenseMatrixType<T1> *d1;
  DenseMatrixType<T2> *d2;
  DenseMatrixType<T3> *dout;
  int i,j,k;
  if ( ((d1=dynamic_cast<DenseMatrixType<T1>*>(m1))) ) {
    if ( (d2=dynamic_cast<DenseMatrixType<T2>*>(m2)) ) {
      if ( (dout=dynamic_cast<DenseMatrixType<T3>*>(output)) ) {
	T1 **matrix1 = d1->dataMatrix;
	T2 **matrix2 = d2->dataMatrix;
	T3 **matrixOut = dout->dataMatrix;
	int cols1 = d1->cols;
	int cols2 = d2->cols;
	for (i=0; i < cols1; i++) {
	  double *matrixi = matrixOut[i];
	  for (j=0; j < cols2; j++)
	    matrixi[j]=0;
	}
	for (k=0, iterK=set1.begin(); iterK!=set1.end(); k++, iterK++) {
	  T1 *matrixIterK = matrix1[*iterK];
	  T2 *matrixK = matrix2[k];
	  for (i=0; i < cols1; i++) {
	    T3 matrixIterKI = matrixIterK[i];
	    T3 *matrixI = matrixOut[i];
	    if (matrixIterKI)
	      for (j=0; j < cols2; j++) {
		//dout->dataMatrix[i][j] += d1->dataMatrix[*iterK][i] *
		//d2->dataMatrix[k][j];
		matrixI[j] += matrixIterKI * (T3)(matrixK[j]);
	      } // for j
	  } // for i
	} // for k
      } // if-output
    } // if-m2
  } // if-m1
  else
    throw (const char *)"MultAtB: Undefined combination";
}

// Compute the matrix product between one matrix and the transpose of
// another. Do the multiplications with only those rows indicated in
// the sets.
template<class T1, class T2, class T3>
void MultABt(MatrixTemplate<T1> *m1, MatrixTemplate<T2> *m2, 
	     set<int> set2, MatrixTemplate<T3> *output) {

  if (m1->cols!=m2->cols || m1->rows!=output->rows)
    throw (const char *)"Multiply: Bad matrix sizes.";
  set<int>::iterator iterJ;
  iterJ = set2.end();
  iterJ--;
  if (*iterJ > m2->rows)
    throw (const char *)"Multiply: Bad set indices.";

  DenseMatrixType<T1> *d1;
  DenseMatrixType<T2> *d2;
  DenseMatrixType<T3> *dout;
  unsigned int i,j,k;
  if ( (d1=dynamic_cast<DenseMatrixType<T1>*>(m1)) ) {
    if ( (d2=dynamic_cast<DenseMatrixType<T2>*>(m2)) ) {
      if ( (dout=dynamic_cast<DenseMatrixType<T3>*>(output)) ) {
	T1 **matrix1 = d1->dataMatrix;
	T2 **matrix2 = d2->dataMatrix;
	T3 **matrixOut = dout->dataMatrix;
	unsigned int rows1 = d1->rows;
	unsigned int cols1 = d1->cols;
	for (i=0; i < rows1; i++) {
	  T3 *matrixOutI = matrixOut[i];
	  for (j=0; j < set2.size(); j++)
	    matrixOutI[j]=0;
	}
	for (k=0; k < cols1; k++) {
	  for (i=0; i < rows1; i++) {
	    T3 *matrixOutI = matrixOut[i];
	    T3 matrix1IK = matrix1[i][k];
	    if (matrix1IK)
	      for (j=0, iterJ=set2.begin(); iterJ!=set2.end(); j++, iterJ++) {
		//dout->dataMatrix[i][j] +=
		//d1->dataMatrix[i][k] * d2->dataMatrix[*iterJ][k];
		matrixOutI[j] += matrix1IK * (T3)(matrix2[*iterJ][k]);
	      }
	  } // for-j
	} // for-i
      } // if-dout
    } //if-d2
  } // if-d1
  else
    throw (const char *)"MultABt: Undefined combination";
}


//	Stuff added for the regression version.			
				
				
				
template <class T1, class T2>
void AppendConstCol(MatrixTemplate<T1> *in, double val, MatrixTemplate<T2> *out)
{
  if ((in->cols+1)!=out->cols || in->rows!=out->rows)
    throw (const char *)"AppendConstCol: Bad matrix sizes!";

  DenseMatrixType<T1> *din;
  DenseMatrixType<T2> *dout;
  int i,j;
	
  if (din = dynamic_cast<DenseMatrixType<T1>*>(in))
    {
      if (dout = dynamic_cast<DenseMatrixType<T2>*>(out))
	{
	  T1 **arrayIn = din->dataMatrix, *arrayInRow;
	  T2 **arrayOut = dout->dataMatrix, *arrayOutRow;
	  unsigned int m = in->rows, n = in->cols;
			
	  for (i=(m-1); i>=0; i--)
	    {
	      arrayInRow = arrayIn[i];
	      arrayOutRow = arrayOut[i];
				
	      for (j = (n-1); j >= 0; j--)
		{	arrayOutRow[j] = arrayInRow[j];	}
				
	      arrayOutRow[n] = val;
	    }
	  return;
			
	}	// if dout = DenseMatrixType
    }	// if din = DenseMatrixType
	
  // Could this be made faster by casting a void-pointer to the destination and the
  // source and copying the whole thing byte-by-byte? I guess I'd have to re-
  // template it to make sure both are the same type.
	
  throw (const char *)"AppendConstCol: bad matrix types!";
}



set<int> DenseVector::findTolNonBasic(double tol, set<int> indices)
{
  set<int>::iterator fin = indices.end();
  set<int> returnable;
  int index;
	
  for(set<int>::iterator iter = indices.begin(); iter != fin; iter++)
    {
      index = *iter;
		
      if(dataMatrix[index] < tol)
	{	returnable.insert(index);	}
    }
	
  return(returnable);
}



set<int> DenseVector::findTolNegative(double tol, set<int> &indices)
{
  set<int>::iterator fin = indices.end();
  set<int> returnable;
  int index;
	
  tol = 0.0 - tol;
	
  for(set<int>::iterator iter = indices.begin(); iter != fin; iter++)
    {
      index = *iter;
		
      if(dataMatrix[index] < tol)
	{	returnable.insert(index);	}
    }
	
  return(returnable);
}



set<int> DenseVector::findTolNegative(double tol)
{
  int index;
  set<int> returnable;
	
  tol = 0.0 - tol;
	
  for(index = (cols - 1); index >= 0; index--)
    {
      if(dataMatrix[index] < tol)
	{	returnable.insert(index);	}
    }
	
  return(returnable);
}


// Divide each item of one vector by each item of another.
void ElDivAB(Vector *vIn1, Vector *vIn2, set<int> indices, Vector *vOut)
{
  DenseVector *dIn1, *dIn2, *dOut;
	
  if ((vIn1->cols != vIn2->cols) || (vIn2->cols != vOut->cols))
    {	throw (const char *)"ElDivAB: bad matrix sizes!";	}
	
  set<int>::iterator test = indices.end();
  test--;
  if (*test >= vIn1->cols)
    {	throw (const char *)"ElDivAB: set of indices contains out-of-range elements!";	}
	
  if ((dIn1 = dynamic_cast<DenseVector*>(vIn1)))
    {
      if ((dIn2 = dynamic_cast<DenseVector*>(vIn2)))
	{
	  if ((dOut = dynamic_cast<DenseVector*>(vOut)))
	    {
	      double *inArray1 = dIn1->dataMatrix, *inArray2 = dIn2->dataMatrix,
		*outArray = dOut->dataMatrix;
				
	      set<int>::iterator fin = indices.end();
	      int index;
				
	      for (set<int>::iterator iter = indices.begin(); iter != fin; iter++)
		{
		  index = *iter;
		  outArray[index] = inArray1[index] / inArray2[index];
		}
				
	      return;
	    }	// if vOut = denseVector
	}	// if vIn2 = denseVector
    }	// if vIn1 = denseVector
	
  throw (const char *)"ElDivAB: bad matrix types!";
}
		


double DenseVector::findMin(set<int> &indices)
{
  set<int>::iterator fin = indices.end();
  fin--;
	
  if (*fin >= cols)
    {	throw (const char *)"min: set of indices contains out-of-range elements!";	}
  fin++;
	
  // Get an initial value (stupid fence-posting!)
  set<int>::iterator iter = indices.begin();
  int index = *iter;
  double smallVal = dataMatrix[index];
	
  for (iter++; iter != fin; iter++)
    {
      index = *iter;
      if (dataMatrix[index] < smallVal)
	{	smallVal = dataMatrix[index];	}
    }
	
  return smallVal;
}
		

	
template <class T1>
void MultAtB(Vector *in1, MatrixTemplate<T1> *in2, Vector *out)
{
  DenseVector *dIn1, *dOut;
  DenseMatrixType<T1> *dIn2;
	
  if ((in1->cols != in2->rows) || (in2->cols != out->cols))
    {	throw (const char *)"MultAtB: bad matrix and/or vector sizes!";	}
	
  if (dIn1 = dynamic_cast<DenseVector*>(in1))
    {
      if (dIn2 = dynamic_cast<DenseMatrixType<T1>*>(in2))
	{
	  if (dOut = dynamic_cast<DenseVector*>(out))
	    {
	      double *inArray1 = dIn1->dataMatrix, **inArray2 = dIn2->dataMatrix,
		*inArray2Row, *outArray = dOut->dataMatrix;
				
	      int i, j, m = in2->rows;
				//double temp;
				
	      for (i = (m-1); i >= 0; i--)
		{	outArray[i] = 0.0;	}
				
	      for (i = (m - 1); i >= 0; i--)
		{
		  inArray2Row = inArray2[i];
					
		  for (j = (m-1); j >= 0; j--)
		    {	outArray[j] += inArray1[j] * inArray2Row[j];	}
		}
				
	      return;
	    }	// dOut = DenseVector
	}	// dIn2 = DenseMatrixType
    }	// dIn1 = DenseVector
	
  throw (const char *)"MultAtB: bad matrix and/or vector types!";
}


template <class T1>
void MultAB(Vector *in1, set<int> &b, MatrixTemplate<T1> *in2, Vector *out)
{
  DenseVector *dIn1, *dOut;
  DenseMatrixType<T1> *dIn2;
  set<int>::iterator test;
	
  if ((in1->cols != in2->rows) || (in2->cols != out->cols))
    {	throw (const char *)"MultAB: bad matrix and/or vector sizes!";	}
	
  test = b.end();
  test--;
  if (*test >= in2->rows)
    {	throw (const char *)"MultAB: set contains out-of-range indices!";	}
	
  if (dIn1 = dynamic_cast<DenseVector*>(in1))
    {
      if (dIn2 = dynamic_cast<DenseMatrixType<T1>*>(in2))
	{
	  if (dOut = dynamic_cast<DenseVector*>(out))
	    {
	      double *inArray1 = dIn1->dataMatrix, **inArray2 = dIn2->dataMatrix,
		*inArray2Row, *outArray = dOut->dataMatrix;
				
	      set<int>::iterator max = b.end(), rIter = b.begin(),
		cIter = b.begin();
	      int i, j;

				
	      for (i = (dOut->cols - 1); i >= 0; i--)
		{	outArray[i] = 0.0;	}
				
				
				// matrix rows/vector cols
	      for (; rIter != max; rIter++)
		{
		  i = *rIter;
		  inArray2Row = inArray2[i];
					
		  // matrix cols/output rows
		  for (cIter = b.begin(); cIter != max; cIter++)
		    {
		      j = *cIter;
		      outArray[j] += inArray1[i] * inArray2Row[j];
		    }
		}	
				
	      return;
				
	    }	// dOut = DenseVector
	}	// dIn2 = DenseMatrixType
    }	// dIn1 = DenseVector
	
  throw (const char *)"MultAB: bad matrix and/or vector types!";
}




template <class T1>
void MultAB(Vector *in1, MatrixTemplate<T1> *in2, Vector *out)
{
  DenseVector *dIn1, *dOut;
  DenseMatrixType<T1> *dIn2;
	
  if ((in1->cols != in2->rows) || (in2->cols != out->cols))
    {	throw (const char *)"MultAtB: bad matrix and/or vector sizes!";	}
	
  if (dIn1 = dynamic_cast<DenseVector*>(in1))
    {
      if (dIn2 = dynamic_cast<DenseMatrixType<T1>*>(in2))
	{
	  if (dOut = dynamic_cast<DenseVector*>(out))
	    {
	      double *inArray1 = dIn1->dataMatrix, **inArray2 = dIn2->dataMatrix,
		*inArray2Row, *outArray = dOut->dataMatrix;
				
	      int i, j, m = in2->rows, n = in2->cols;
				
	      for (i = (out->cols - 1); i < 0; i--)
		{	outArray[i] = 0.0;	}
				
	      for (j = (m-1); j >= 0; j--)
		{
		  inArray2Row = inArray2[j];
					
		  for (i = (n - 1); i >= 0; i--)
		    {	outArray[i] += inArray1[j] * inArray2Row[i];	}
		}	
				
	      return;
				
	    }	// dOut = DenseVector
	}	// dIn2 = DenseMatrixType
    }	// dIn1 = DenseVector
	
  throw (const char *)"MultAtB: bad matrix and/or vector types!";
}


// Compute the scalar product between this vector and another one
double DenseVector::ScalarProduct(Vector *m2, set<int> &b)
{

  set<int>::iterator max = b.end(), iter = b.begin();
  int i;
  double product = 0.0, *data;
  DenseVector *dVec;
	
  max--;
  if (*max > cols)
    {	throw (const char *)"ScalarProduct: set indices out of range!";	}
  max++;
	
  if (cols != m2->cols)
    {	throw (const char *)"ScalarProduct: bad vector sizes!";	}
	
  if ((dVec=dynamic_cast<DenseVector*>(m2)))
    {
      data = dVec->dataMatrix;
		
      for (; iter != max; iter++)
	{
	  i = *iter;
	  product += dataMatrix[i] * data[i];
	}
		
      return(product);
    }
  else
    {	throw (const char *)"ScalarProduct: bad vector type!"; 	}

}



template<class T1, class T2, class T3>
void MultABt(MatrixTemplate<T1> *A, MatrixTemplate<T2> *B, MatrixTemplate<T3> *output)
{
  if ((A->rows != output->rows) ||
      (A->cols != B->cols) ||
      (B->rows != output->cols))
    {	throw (const char *)"MultABt: bad matrix sizes!";	}
	
  DenseMatrixType<T1> *AD;
  DenseMatrixType<T2> *BD;
  DenseMatrixType<T3> *outD;
	
  if (AD = dynamic_cast<DenseMatrixType<T1>*>(A))
    {
      if (BD = dynamic_cast<DenseMatrixType<T2>*>(B))
	{
	  if (outD = dynamic_cast<DenseMatrixType<T3>*>(output))
	    {
				
	      int i, j, k,
		m = A->rows, n = A->cols, q = B->rows;
				
	      double **dataA = AD->dataMatrix,
		**dataB = BD->dataMatrix,
		**dataOut = outD->dataMatrix,
		*dataARow, *dataBRow, *dataOutRow, temp;
				
	      for (i=(m-1); i>=0; i--)
		{
		  dataARow = dataA[i];
		  dataOutRow = dataOut[i];
					
		  for (j=(q-1); j>=0; j--)
		    {
		      dataBRow = dataB[j];
						
		      temp = 0.0;
						
		      for (k=(n-1); k>=0; k--)
			{	temp += dataARow[k] * dataBRow[k];	}
						
		      dataOutRow[j] = temp;
		    }
		}
				
				
				
				//i = a's rows, out's rows
				//j = b's rows, out's cols
				//k = a's cols, b's cols
				
				/*
				  for (i = 0; i < A->rows; i++)
				  {
				  for (j = 0; j < B->rows; j++)
				  {
				  dataOut[i][j] = 0.0;
						
				  for (k = 0; k < A->cols; k++)
				  {	dataOut[i][j] += dataA[i][k] * dataB[j][k];	}
				  }
				  }
				*/
							
				
	      return;
	    }
	}
    }
	
  throw (const char *)"MultABt: bad matrix types!";

}



void DenseVector::SubtractInPlaceFrom(Vector *vec)
{
  if (vec->cols != cols)
    {	throw (const char *)"SubtractInPlaceFrom: bad vector sizes!";	}
	
  DenseVector *vecD;
	
  if ((vecD = dynamic_cast<DenseVector*>(vec)))
    {
      double *data = vecD->dataMatrix;
		
      for (int i = cols-1; i >= 0; i--)
	{	dataMatrix[i] = data[i] - dataMatrix[i];	}
		
      return;
    }
	
  throw (const char *)"SubtractInPlaceFrom: bad vector types!";
}



void DenseVector::ZeroInactivesInPlace(set<int> &b)
{
  bool *killMe = new bool[cols];
  int i;
  set<int>::iterator iter = b.begin(), max = b.end();
	
  for(i=(cols - 1); i>=0; i--)
    {	killMe[i] = true;	}
	
  for(; iter != max; iter++)
    {	killMe[*iter] = false;	}
	
  for (i=(cols-1); i>=0; i--)
    {
      if (killMe[i])
	{	dataMatrix[i] = 0.0;	}
    }
	
  delete [] killMe;
}


void SubAB(Vector *A, Vector *B, set<int> &b, Vector *C)
{
  if ((A->cols != B->cols) || (B->cols != C->cols))
    {	throw (const char *)"SubAB: bad matrix sizes!";	}
	
  int m = A->cols, i;
	
  set<int>::iterator max = b.end();
  max--;
  if (*max > m)
    {	throw (const char *)"SubAB: indices out of range!";	}
  max++;
	
  DenseVector *dA, *dB, *dC;
	
  if ((dA = dynamic_cast<DenseVector*>(A)))
    {
      if ((dB = dynamic_cast<DenseVector*>(B)))
	{
	  if ((dC = dynamic_cast<DenseVector*>(C)))
	    {
	      double *aData = dA->dataMatrix,
		*bData = dB->dataMatrix,
		*cData = dC->dataMatrix;
				
	      for (set<int>::iterator iter = b.begin(); iter != max; iter++)
		{
		  i = *iter;
		  cData[i] = aData[i] - bData[i];
		}
				
	      return;
	    }
	}
    }
	
  throw (const char *)"SubAB: bad vector types!";
}



void DenseVector::MultiplyThenAddInPlace(double factor, Vector *addMe)
{
  if (addMe->cols != cols)
    {	throw (const char *)"MultiplyThenAddInPlace: bad vector sizes!";	}
	
  DenseVector *vec;
	
  if ((vec = dynamic_cast<DenseVector*>(addMe)))
    {
      double *data = vec->dataMatrix;
      int i;
		
      for (i = cols - 1; i >= 0; i--)
	{	dataMatrix[i] = (dataMatrix[i] * factor) + data[i];	}
		
      return;
    }
	
  throw (const char *)"MultiplyThenAddInPlace: bad vector type!";
}

void DenseVector::MultiplyThenAddInPlace(double factor, Vector *addMe, set<int> &b)
{
  if (addMe->cols != cols)
    {	throw (const char *)"MultiplyThenAddInPlace: bad vector sizes!";	}
	
  set<int>::iterator max = b.end();
  max--;
  if (*max > cols)
    {	throw (const char *)"MultiplyThenAddInPlace:	indices out of range!";	}
  max++;
	
  DenseVector *vec;
	
  if ((vec = dynamic_cast<DenseVector*>(addMe)))
    {
      double *data = vec->dataMatrix;
      int i;
		
      for (set<int>::iterator iter = b.begin(); iter != max; iter++)
	{
	  i = *iter;
	  dataMatrix[i] = (dataMatrix[i] * factor) + data[i];
	}
		
      return;
    }
	
  throw (const char *)"MultiplyThenAddInPlace: bad vector type!";
}

// In order to get the linking to work, some compilers require
// explicit declarations of all template instantiations of the methods

// double declarations
template class MatrixTemplate<double>;
template class DenseMatrixType<double>;
template void MultAB(MatrixTemplate<double> *m1,
		     MatrixTemplate<double> *m2, 
		     MatrixTemplate<double> *output);
template void MultAB(MatrixTemplate<double> *m1, set<int> i1,
		     MatrixTemplate<double> *m2,
		     MatrixTemplate<double> *output);
template void MultAB(MatrixTemplate<double> *m1, set<int> i1,
		     Vector *m2, Vector *output);
template void MultAB(MatrixTemplate<double> *m1, Vector *m2,
		     Vector *output);
template void MultAtB(MatrixTemplate<double> *m1,
		      MatrixTemplate<double> *m2,
		      MatrixTemplate<double> *output);
template void MultAtB(MatrixTemplate<double> *m1, Vector *m2,
		      Vector *output);
template void MultAtB(MatrixTemplate<double> *m1, set<int> i1,
		      MatrixTemplate<double> *m2, set<int> i2,
		      MatrixTemplate<double> *output);
template void MultAtB(MatrixTemplate<double>*m1, set<int> i1,
		      Vector *m2, set<int> i2, Vector *output);
template void MultAtB(MatrixTemplate<double>*m1, set<int> i1,
		      Vector *m2, Vector *output);
template void MultAtB(MatrixTemplate<double> *m1, set<int> i1,
		      MatrixTemplate<double> *m2,
		      MatrixTemplate<double> *output);
template void MultABt(MatrixTemplate<double> *m1,
		      MatrixTemplate<double> *m2,
		      set<int> i2, MatrixTemplate<double> *output);
				


template void MultABt(MatrixTemplate<double> *A,
		      MatrixTemplate<double> *B,
		      MatrixTemplate<double> *output);

template void MultAtB(Vector *in1, MatrixTemplate<double> *in2, Vector *out);


template void MultAB(Vector *in1, set<int> &b, MatrixTemplate<double> *in2,
                     Vector *out);

template void MultAB(Vector *in1, MatrixTemplate<double> *in2, Vector *out);

template void AppendConstCol(MatrixTemplate<double> *in, double val,
                             MatrixTemplate<double> *out);



// int declarations
template class MatrixTemplate<int>;
template class DenseMatrixType<int>;
template void MultAB(MatrixTemplate<int> *m1,
		     MatrixTemplate<double> *m2, 
		     MatrixTemplate<double> *output);
template void MultAB(MatrixTemplate<int> *m1, set<int> i1,
		     MatrixTemplate<double> *m2,
		     MatrixTemplate<double> *output);
template void MultAB(MatrixTemplate<int> *m1, set<int> i1,
		     Vector *m2, Vector *output);
template void MultAB(MatrixTemplate<int> *m1, Vector *m2,
		     Vector *output);
template void MultAtB(MatrixTemplate<int> *m1,
		      MatrixTemplate<int> *m2,
		      MatrixTemplate<double> *output);
template void MultAtB(MatrixTemplate<int> *m1, Vector *m2,
		      Vector *output);
template void MultAtB(MatrixTemplate<int> *m1, set<int> i1,
		      MatrixTemplate<int> *m2, set<int> i2,
		      MatrixTemplate<double> *output);
template void MultAtB(MatrixTemplate<int>*m1, set<int> i1,
		      Vector *m2, set<int> i2, Vector *output);
template void MultAtB(MatrixTemplate<int>*m1, set<int> i1,
		      Vector *m2, Vector *output);
template void MultAtB(MatrixTemplate<int> *m1, set<int> i1,
		      MatrixTemplate<double> *m2,
		      MatrixTemplate<double> *output);
template void MultABt(MatrixTemplate<double> *m1,
		      MatrixTemplate<int> *m2,
		      set<int> i2, MatrixTemplate<double> *output);

// short declarations
template class MatrixTemplate<short>;
template class DenseMatrixType<short>;
template void MultAB(MatrixTemplate<short> *m1,
		     MatrixTemplate<double> *m2, 
		     MatrixTemplate<double> *output);
template void MultAB(MatrixTemplate<short> *m1, set<int> i1,
		     MatrixTemplate<double> *m2,
		     MatrixTemplate<double> *output);
template void MultAB(MatrixTemplate<short> *m1, set<int> i1,
		     Vector *m2, Vector *output);
template void MultAB(MatrixTemplate<short> *m1, Vector *m2,
		     Vector *output);
template void MultAtB(MatrixTemplate<short> *m1,
		      MatrixTemplate<short> *m2,
		      MatrixTemplate<double> *output);
template void MultAtB(MatrixTemplate<short> *m1, Vector *m2,
		      Vector *output);
template void MultAtB(MatrixTemplate<short> *m1, set<int> i1,
		      MatrixTemplate<short> *m2, set<int> i2,
		      MatrixTemplate<double> *output);
template void MultAtB(MatrixTemplate<short>*m1, set<int> i1,
		      Vector *m2, set<int> i2, Vector *output);
template void MultAtB(MatrixTemplate<short>*m1, set<int> i1,
		      Vector *m2, Vector *output);
template void MultAtB(MatrixTemplate<short> *m1, set<int> i1,
		      MatrixTemplate<double> *m2,
		      MatrixTemplate<double> *output);				
template void MultABt(MatrixTemplate<double> *m1,
		      MatrixTemplate<short> *m2,
		      set<int> i2, MatrixTemplate<double> *output);
				
