#include "DataSource.H"
#include "Utilities.H"
#include "StandAlone.H"
#include "Version.H"
#include <set>
#include <algorithm>
#include <cfloat>
#include <string>
#include <iomanip>

// Tolerance of what counts as zero for optimality checks and variable
// deactivation.
double gDeactivate = 1.0e-09;
double gInf = 9.9e20;
double gC,gTOL,gEPSILON;
bool gSETC,gCONVERGENCE,gPROMPT,gHEADER,gNORMALIZE,gPERTURB;
int gMAXITER;
char gTESTFILENAME[1000],gOUTPUTFILENAME[1000],gSOURCEFILENAME[1000];


double getLambda(Vector *u, Vector *uMedium, set<int> &b)
{
  if (u->cols != uMedium->cols)
    {	throw (const char *)"getLambda: bad vector sizes!";	}
	
  set<int>::iterator max = b.end();
  max--;
  if (*max > u->cols)
    {	throw (const char *)"getLambda: indices out of range!";	}
	
  DenseVector *uD, *uMediumD;
	
  if ((uD = dynamic_cast<DenseVector*>(u)))
    {
      if ((uMediumD = dynamic_cast<DenseVector*>(uMedium)))
	{
	  set<int>::iterator iter = b.begin();
	  int i;
	  double *uData = uD->dataMatrix, *uMediumData = uMediumD->dataMatrix,
	    smallest = (uData[*iter] / -uMediumData[*iter]), temp;
			
	  for (; iter != max; iter++)
	    {
	      i = *iter;
	      temp = uData[i] / -uMediumData[i];
	      if (temp < smallest)
		{	smallest = temp;	}
	    }
			
	  return(smallest);
	}
    }
	
  throw (const char *)"getLambda: bad vector types!";
}



template <class T>
void qTimesVec(Vector *vec, MatrixTemplate<T> *H, double C, Vector *ans)
{
  if ((vec->cols != 2*H->rows) || (vec->cols != ans->cols))
    {	throw (const char *)"qTimesVec: bad sizes!";	}
	
  DenseVector *dVec, *dAns;
  DenseMatrixType<T> *dH;
	
  if (dVec = dynamic_cast<DenseVector*>(vec))
    {
      if (dH = dynamic_cast<DenseMatrixType<T>*>(H))
	{
	  if (dAns = dynamic_cast<DenseVector*>(ans))
	    {
	      int m = H->rows, n = H->cols, i, j;
	      double *alpha = dVec->dataMatrix, *beta = dVec->dataMatrix + m,
		*alphaMinusBeta = new double[m],
		*HtAmB = new double[n],
		temp, *ansData = dAns->dataMatrix; // *MAmB = new double[m],
				
	      T **hData = dH->dataMatrix, *hRow;
				
				// Initialize the temps.
	      for (i = (n - 1); i >= 0; i--)
		{	HtAmB[i] = 0.0;	}
				
				
				
				// Get alpha - beta.
	      for (i = (m-1); i >= 0; i--)
		{	alphaMinusBeta[i] = alpha[i] - beta[i];	}
         	
				
				
				// Multiply alpha minus beta by H'.
	      for (i = (m-1); i >= 0; i--)	// rows of H, alphaMinusBeta
		{
		  hRow = hData[i];
					
		  for (j = (n-1); j >= 0; j--)	// Cols of H, rows of the answer.
		    {	HtAmB[j] += hRow[j] * alphaMinusBeta[i];	}
		}					
					
				
				// Multiply H' by alpha minus beta.
				
				
				
				// Multiply by H, add alpha minus beta over C, and reassemble.
				
	      for (i = (m-1); i >= 0; i--)	// Rows of H and the answer
		{
		  hRow = hData[i];
					
		  temp = alphaMinusBeta[i] / C;
					
		  for (j = (n-1); j >= 0; j--)	// Cols of H, rows of the factor.
		    {	temp += hRow[j] * HtAmB[j];	}
					
		  ansData[i] = temp;
		  ansData[i+m] = -temp;
		}
			
	      delete [] alphaMinusBeta;
	      delete [] HtAmB;
	      return;
	    }
	}  	
    }     	
	
  throw (const char *)"qTimesVec: bad types!";
}


	
template <class T>
void qTimesVec(Vector *vec, MatrixTemplate<T> *H, double C, set<int> &b, Vector *ans)
{
  if ((vec->cols != 2*H->rows) || (vec->cols != ans->cols))
    {	throw (const char *)"qTimesVec: bad sizes!";	}
	
  DenseVector *dVec, *dAns;
  DenseMatrixType<T> *dH;
	
  if ((dVec = dynamic_cast<DenseVector*>(vec)))
    {
      if ((dH = dynamic_cast<DenseMatrixType<T>*>(H)))
	{
	  if ((dAns = dynamic_cast<DenseVector*>(ans)))
	    {
	      int m = H->rows, n = H->cols, i, j;
	      double *alpha = dVec->dataMatrix, *beta = dVec->dataMatrix + m,
		*alphaMinusBeta = new double[m],
		*HtAmB = new double[n],
		temp, *ansData = dAns->dataMatrix; //, *MAmB = new double[m]
				
	      T **hData = dH->dataMatrix, *hRow;
				
				// Initialize the temps.
	      for (i = (n - 1); i >= 0; i--)
		{	HtAmB[i] = 0.0;	}
				
				
				
				// Get alpha - beta.
	      for (i = (m-1); i >= 0; i--)
		{	alphaMinusBeta[i] = alpha[i] - beta[i];	}
	      
	      
	      
				// Multiply alpha minus beta by H'.
	      for (i = (m-1); i >= 0; i--)	// rows of H, alphaMinusBeta
		{
		  hRow = hData[i];
					
		  for (j = (n-1); j >= 0; j--)	// Cols of H, rows of the answer.
		    {	HtAmB[j] += hRow[j] * alphaMinusBeta[i];	}
		}					
					
				
				// Multiply H' by alpha minus beta.
				
				
				
				// Multiply by H, add alpha minus beta over C, and reassemble.
				
	      for (i = (m-1); i >= 0; i--)	// Rows of H and the answer
		{
		  hRow = hData[i];
					
		  temp = alphaMinusBeta[i] / C;
					
		  for (j = (n-1); j >= 0; j--)	// Cols of H, rows of the factor.
		    {	temp += hRow[j] * HtAmB[j];	}
					
		  ansData[i] = temp;
		  ansData[i+m] = -temp;
		}
				
				
	      dAns->ZeroInactivesInPlace(b);
				
			
	      delete [] alphaMinusBeta;
	      delete [] HtAmB;
	      return;
	    }
	}  	
    }     	
	
  throw (const char *)"qTimesVec: bad types!";
}	

template<class aType>
void computeSolution(MatrixTemplate<aType> *A, Vector *y, double epsilon,
		     double C, double D,
                     Vector *w, double &gamma, double &objective,
					double &time,int itmax,double tol,int &itNum,
                     bool normalizeData = true, bool silent = false)
{

  CPUTime clock;
  clock.Reset();
  DenseMatrixType<double> *H;
  DenseVector *q, *u, *uIdeal, *uNew;
  set<int> basicSet;
	
  int m = A->rows;
	
  if (normalizeData)
    {
      cout << "Sorry, data normalization is not yet available.\n\n";
    }
	
  // Set up initial stuff.
	
  // H = [A e*(1/sqrt(D)];
  H = new DenseMatrixType<double>(A->rows, A->cols+1);
  AppendConstCol(A, 1/sqrt(D), H);	// Written in DataSource.cpp
	
	
  // M = (H*H') + (I/C);
  // Q = [M -M; -M M];
	
  // q = [-y ; y] + epsilon
  q = new DenseVector(2*y->cols);
  makeQ(y, epsilon, q);	// Written here

  // Set up the basic set so that it consists of all of the alpha-hats.
  for (int i = 0; i < m; i++)
    {	basicSet.insert(i);	}
		
  // Take a stab-in-the-dark at a solution.
  u = new DenseVector(2 * H->rows);
  getIdeal(H, y, basicSet, epsilon, C, u);
  double f = gInf;
	
  // Start the big loop.
  itNum = 0;
  bool stillIterating = true;
	
  double opt = -1.0, fOld;
  uIdeal = new DenseVector(2*A->rows);
  uNew = new DenseVector(2*A->rows);
  opt = findOptimality(H, C, q, u);
  char itType[500];

  PRINT("Iter         Optimality      Objective     Basic set    Description\n");
	
  while(stillIterating)
    {
      itNum++;
      strcpy(itType,"Unknown");
		
      if (opt < tol)
	{
	  if (itNum==1)
	    cout << "Initial heuristic starting value is optimal.\n";
	  else
	    cout << "Optimality condition within tolerance.\n";
	  stillIterating = false;
	}
      else if (itNum > itmax)
	{
	  cout << "Maximum number of iterations reached.\n";
	  stillIterating = false;
	}
      else
	{
	  fOld = f;
	  basicSet = u->findTolBasic(gDeactivate);

	  // Determine optimality on face
	  double faceCheck = findOptimality(H,C,q,u,basicSet);

	  if (faceCheck < tol) {
	    
	    // Free all variables and try the hard step again.
	    int max = 2*m;
	    for (int i = 0; i < max; i++)
	      {	basicSet.insert(i);	}
	    getHard(H, C, q, u, uNew);
	    madHatter(uNew);
	    f = getF(uNew, H, C, q, basicSet);
	    if (f >= fOld)
	      {
		cout << "Couldn't improve solution! Bailing out.\n";
		stillIterating = false;
	      }
	    else
	      {	
		  strcpy(itType,"Global grad proj");
	      }

	  } // Not at optimality on face
	  else {
			
	    getIdeal(H, y, basicSet, epsilon, C, uIdeal);
	    // Try an easy step.
	    uIdeal->Copy(uNew);
	    uNew->PlusFunctionInPlace();
	    f = getF(uNew, H, C, q, basicSet);
			
	    if (f >= fOld)
	    {
				// Try a medium step.
	      getMedium(u, uIdeal, basicSet, uNew);
	      f = getF(uNew, H, C, q, basicSet);
				
	      if (f >= fOld)
		{
		  // Try a hard step.
		  getHard(H, C, q, u, basicSet, uNew);
		  f = getF(uNew, H, C, q, basicSet);
					
		  if (f >= fOld)
		    {
		      // Free all variables and try the hard step again.
		      int max = 2*m;
		      for (int i = 0; i < max; i++)
			{	basicSet.insert(i);	}
		      getHard(H, C, q, u, uNew);
		      madHatter(uNew);
		      f = getF(uNew, H, C, q, basicSet);
		      
		      if (f >= fOld)
			{
			  strcpy(itType,"Couldn't improve solution. Bailing out.");
			  stillIterating = false;
			}
		      else
			{	
			  strcpy(itType,"Global Grad Proj");
			}
		    }
		  else
		    {
		      strcpy(itType,"Face Grad Proj");
		    }
		}
	      else
		{
		  strcpy(itType,"Line search");
		}
	    }
	    else
	      {
		strcpy(itType,"Normal iteration");
	      }
	  } // End of optimality on face test
			
			
	  uNew->Copy(u);
	}

      opt = findOptimality(H, C, q, u);
      if (!silent && stillIterating) {
	PRINT("%3d\t%15.6g\t%15.6g\t%10d\t%s\n",itNum,opt,f,basicSet.size(),itType);
	FLUSH;
      }
		
		
    }	// End of main while loop.
	
	
  DenseVector *alphas = new DenseVector(u->Size()/2);
  DenseVector *alphaHats = new DenseVector(u->Size()/2);
	
  getAlphasHats(u, alphas, alphaHats);	// Written here
	
  // w = data' * (alphaHat - alpha);
  DenseVector *hatsMinusAlphas = new DenseVector(alphaHats->cols);
  alphaHats->Copy(hatsMinusAlphas);
  hatsMinusAlphas->SubtractInPlace(alphas);
  MultAtB(A, hatsMinusAlphas, w);
	
  // gamma = sum(alphaHat - alpha) / D
  gamma = hatsMinusAlphas->Sum() / D;
	
  //xi = alpha ./ C; xiHat = alphaHat ./ C;
  alphas->MultiplyInPlace(1.0/C);
  alphaHats->MultiplyInPlace(1.0/C);
	
  // obj = ((w' * w) + C * ((xi' * xi) + (xiHat' * xiHat)) + D * gamma * gamma)/2;
  objective = (w->Square() + C * (alphas->Square() + alphaHats->Square()) + D * gamma * gamma)/2.0;
	
  // Clean up memory we allocated.
  delete(H);
  //delete(Q);
  delete(q);
  delete(u);
  delete(uIdeal);
  delete(uNew);
  delete(hatsMinusAlphas);
  delete(alphas);
  delete(alphaHats);

  time = clock.Check();
}

template<class hType>
void getIdeal(MatrixTemplate<hType> *H, Vector *y, set<int> &basicSet, double epsilon, double C, Vector *uIdeal)
{
  if (basicSet.size() <= 0)
    throw (const char *)"getIdeal: Basic set has size 0.";

  DenseVector *qTilde = new DenseVector(H->rows),
    *uTilde = new DenseVector(H->rows),
    *vectorN1 = new DenseVector(H->cols),
    *vectorN2 = new DenseVector(H->cols);

  set<int> active;
  DenseMatrixType<double> *tempMatrix = new DenseMatrixType<double>(H->cols, H->cols);
			
  getQTilde(y, epsilon, basicSet, qTilde, active);	// Written here

  int m = active.size();

  DenseVector *vectorM1 = new DenseVector(m),
    *uSmall = new DenseVector(m);

  // mInv = C * (eye(mTilde) - hTilde * inv(eye(n+1)/C + hTilde' * hTilde) * hTilde');
  // uTilde = -mInv * qTilde
  // uTilde = -C*(eye(mTilde)*qTilde - hTilde * inv(eye(n+1)/C + hTilde' * hTilde) * (hTilde' * qTilde))

  MultAtB(H, active, qTilde, active, vectorN1);
  MultAtB(H, active, H, active, tempMatrix);
  tempMatrix->DiagAddInPlace(1/C);
  tempMatrix->GaussianInverse();
  MultAB(tempMatrix,vectorN1,vectorN2);
  MultAB(H, active, vectorN2, vectorM1);
  qTilde->Strip(uSmall,active);
  uSmall->SubtractInPlace(vectorM1);
  uSmall->MultiplyInPlace(-C);

  // Update  locations of uTilde with uSmall
  set<int>::iterator iterI;
  int i;
  for (i=0, iterI=active.begin(); iterI!=active.end(); i++,iterI++)
    uTilde->Assign(*iterI,(*uSmall)[i]);

  uTildeToU(uTilde, basicSet, uIdeal);	// Written here

  // Clean up
  delete(qTilde);
  delete(uTilde);
  delete(vectorN1);
  delete(vectorN2);
  delete(vectorM1);
  delete(tempMatrix);
  delete(uSmall);
}



void getMedium(Vector *u, Vector *uIdeal, set<int> &basicSet, Vector *uMedium)
{
  double lambda = 1;
  //int minIndex;
  set<int> negIndices;
	
  // d = uIdeal - u;	
  SubAB(uIdeal, u, basicSet, uMedium);
	
  negIndices = uIdeal->findTolNegative(gDeactivate, basicSet);	// Written in DataSource.cpp
  if (negIndices.size() > 0)
    {
      lambda = getLambda(u, uMedium, negIndices);
    }
	
  // uMedium = u + lambda*d;
  uMedium->MultiplyThenAddInPlace(lambda, u, basicSet);
}

template <class qType>
void getHard(MatrixTemplate<qType> *Q, Vector *q, Vector *u, set<int> &basicSet, Vector *uHard)
{
  DenseVector *uBar = new DenseVector(u->cols),
    *temp = new DenseVector(u->cols);
  set<int> negIndices;
  double lambda;
	
  // uBar = killNeg(u-(Q*u+q));
  MultAB(Q, basicSet, u, uBar);
		
  uBar->AddInPlace(q);
  uBar->SubtractInPlaceFrom(u);
  uBar->PlusFunctionInPlace();	
	
  // d = uBar - u
  uBar->Copy(uHard);
  uHard->SubtractInPlace(u);
	
  // lambda = ((-1*q')*d-d'*Q*u)/(d'*Q*d); (uHard is d)
  MultAB(uHard, basicSet, Q, temp);
  lambda = ((q->ScalarProduct(uHard, basicSet) * -1.0) - temp->ScalarProduct(u, basicSet)) /
    temp->ScalarProduct(uHard, basicSet);
	
  // uTemp = u + lambda*d;
  uHard->Copy(temp);
  temp->MultiplyInPlace(lambda);
  temp->AddInPlace(u);
	
  negIndices = temp->findTolNegative(gDeactivate, basicSet);	// Written in DataSource.cpp

  if (negIndices.size() > 0)
    {
      DenseVector *tempVector = new DenseVector(uBar->cols);
      ElDivAB(u, uHard, negIndices, tempVector);	// Written in DataSource.cpp
      tempVector->MultiplyInPlace(-1.0);
		
      lambda = tempVector->findMin(negIndices);	// Written in DataSource.cpp
      delete tempVector;
    }
	
  // uHard = u + lambda*d;
  uHard->MultiplyInPlace(lambda);
  uHard->AddInPlace(u);
	
  // Clean up
  delete(uBar);
  delete(temp);
}

template <class T>
void getHard(MatrixTemplate<T> *H, double C, Vector *q, Vector *u, set<int> &basicSet, Vector *uHard)
{
  DenseVector *uBar = new DenseVector(u->cols),
    *Qu = new DenseVector(u->cols),
    *Qd = new DenseVector(u->cols),
    *uTemp = Qu;
	
  Vector *d = uHard;
	
  set<int> negIndices;
	
  double lambda;
	
  // uBar = killNeg(u-(Q*u+q));
  qTimesVec(u, H, C, basicSet, Qu);
  Qu->Copy(uBar);
  //qTimesVec(u, H, C, basicSet, uBar);
  uBar->AddInPlace(q);
  uBar->SubtractInPlaceFrom(u);
  uBar->PlusFunctionInPlace();
	
  uBar->ZeroInactivesInPlace(basicSet);
	
  // d = uBar - u;
  uBar->Copy(d);
  d->SubtractInPlace(u);
	
  d->ZeroInactivesInPlace(basicSet);
		
  // lambda = ((-1*q')*d-d'*Q*u)/(d'*Q*d);
  qTimesVec(d, H, C, basicSet, Qd);
  //qTimesVec(u, H, C, basicSet, Qu);
	
  lambda =  (-1.0 * (q->ScalarProduct(d, basicSet)) - d->ScalarProduct(Qu, basicSet)) /
    d->ScalarProduct(Qd, basicSet);

  // uTemp = u + lambda*d;
  d->Copy(uTemp);
  uTemp->MultiplyInPlace(lambda);
  uTemp->AddInPlace(u);
	
  negIndices = uTemp->findTolNegative(gDeactivate, basicSet);	// Written in DataSource.cpp
  if (negIndices.size() > 0)
    {	lambda = getLambda(u, d, negIndices);	}
	
  uHard->MultiplyThenAddInPlace(lambda, u, basicSet);
	
  // Clean up.
  delete(uBar);
  delete(Qu);
  delete(Qd);
}

template <class qType>
void getHard(MatrixTemplate<qType> *Q, Vector *q, Vector *u, Vector *uHard)
{
  DenseVector *uBar = new DenseVector(u->cols),
    //*d = new DenseVector(u->cols),
    *temp = new DenseVector(u->cols);
  set<int> negIndices;
  double lambda;
	
  // uBar = killNeg(u-(Q*u+q));
  MultAB(Q, u, uBar);
  uBar->AddInPlace(q);
  uBar->SubtractInPlaceFrom(u);
  uBar->PlusFunctionInPlace();
	
  // d = uBar - u
  uBar->Copy(uHard);
  uHard->SubtractInPlace(u);
	
  // lambda = ((-1*q')*d-d'*Q*u)/(d'*Q*d);
  MultAB(uHard, Q, temp);
	
  lambda = ((q->ScalarProduct(uHard) * -1.0) - temp->ScalarProduct(u)) /
    temp->ScalarProduct(uHard);

  // uTemp = u + lambda*d;
  uHard->Copy(temp);
  temp->MultiplyInPlace(lambda);
  temp->AddInPlace(u);
	
  negIndices = temp->findTolNegative(gDeactivate);	// Written in DataSource.cpp

  if (negIndices.size() > 0)
    {
		
      DenseVector *tempVector = new DenseVector(uBar->cols);
      ElDivAB(u, uHard, negIndices, tempVector);	// Written in DataSource.cpp
      tempVector->MultiplyInPlace(-1.0);
		
      lambda = tempVector->findMin(negIndices);	// Written in DataSource.cpp
		
      delete tempVector;
    }
	
  // uHard = u + lambda*d;
  uHard->MultiplyInPlace(lambda);
  uHard->AddInPlace(u);
	
  // Clean up
  delete(uBar);
  delete(temp);
}



template <class T>
void getHard(MatrixTemplate<T> *H, double C, Vector *q, Vector *u, Vector *uHard)
{
  DenseVector *uBar = new DenseVector(u->cols),
    *Qu = new DenseVector(u->cols),
    *Qd = new DenseVector(u->cols),
    *uTemp = Qu;
	
  Vector *d = uHard;
	
  set<int> negIndices;
	
  double lambda;
	
  // uBar = killNeg(u-(Q*u+q));
  qTimesVec(u, H, C, Qu);
  Qu->Copy(uBar);
  uBar->AddInPlace(q);
  uBar->SubtractInPlaceFrom(u);
  uBar->PlusFunctionInPlace();
	
  // d = uBar - u;
  uBar->Copy(d);
  d->SubtractInPlace(u);
	
  qTimesVec(d, H, C, Qd);
	
  lambda =  (-1.0 * (q->ScalarProduct(d)) - d->ScalarProduct(Qu)) /
    d->ScalarProduct(Qd);

  // uTemp = u + lambda*d;
  d->Copy(uTemp);
  uTemp->MultiplyInPlace(lambda);
  uTemp->AddInPlace(u);
	
  negIndices = uTemp->findTolNegative(gDeactivate);	// Written in DataSource.cpp
  if (negIndices.size() > 0)
    {	lambda = getLambda(u, d, negIndices);	}
	
  uHard->MultiplyThenAddInPlace(lambda, u);
	
  // Clean up.
  delete(uBar);
  delete(Qu);
  delete(Qd);
}



template <class qType>
double findOptimality(MatrixTemplate<qType> *Q, Vector *q, Vector *u)
{	
  DenseVector *tempVec = new DenseVector(q->cols);
  double returnMe;
	
  // returnMe = norm(u-killNeg(u-Q*u-q));
  u->Copy(tempVec);
  MultAB(Q, u, tempVec);
  tempVec->SubtractInPlaceFrom(u);
  tempVec->SubtractInPlace(q);
  tempVec->PlusFunctionInPlace();
  tempVec->SubtractInPlaceFrom(u);
  returnMe = sqrt(tempVec->Square());
	
  delete(tempVec);
  return(returnMe);
}



template <class T>
double findOptimality(MatrixTemplate<T> *H, double C, Vector *q, Vector *u)
{	
  DenseVector *tempVec = new DenseVector(q->cols);
  double returnMe;
	
  // returnMe = norm(u-killNeg(u-Q*u-q));
  qTimesVec(u, H, C, tempVec);
  tempVec->SubtractInPlaceFrom(u);
  tempVec->SubtractInPlace(q);
  tempVec->PlusFunctionInPlace();
  tempVec->SubtractInPlaceFrom(u);
  returnMe = sqrt(tempVec->Square());
	
  delete(tempVec);
  return(returnMe);

}

// findOptimality again, but this dime designed to check if
// a given face is at optimality
//    faceCheck = norm(ub-killNeg(ub-Qb*ub-qb));
template <class T>
double findOptimality(MatrixTemplate<T> *H, double C, Vector *q, Vector *u,
		      set<int> &basicSet)
{	
  DenseVector *tempVec = new DenseVector(q->cols);
  DenseVector *ub = new DenseVector(q->cols);
  DenseVector *qb = new DenseVector(q->cols);
  double returnMe;
	
  // returnMe = norm(u-killNeg(u-Q*u-q));
  u->Copy(ub);
  q->Copy(qb);
  ub->ZeroInactivesInPlace(basicSet);
  qb->ZeroInactivesInPlace(basicSet);

  //MultAB(Q, u, tempVec);
  qTimesVec(ub, H, C, basicSet,tempVec);
  tempVec->SubtractInPlaceFrom(ub);
  tempVec->SubtractInPlace(qb);
  tempVec->PlusFunctionInPlace();
  tempVec->SubtractInPlaceFrom(ub);
  returnMe = sqrt(tempVec->Square());
	
  delete(tempVec);
  delete(ub);
  delete(qb);
  return(returnMe);
}

// Q = [M -M; -M M];
template <class mType, class qType>
void QFromH(MatrixTemplate<mType> *H, double C, MatrixTemplate<qType> *Q)
{
  if (((2*H->rows) != Q->cols) || ((2*H->rows) != Q->rows))
    {	throw (const char *)"QFromH: Bad matrix sizes!";	}

  DenseMatrixType<mType> *dH;
  DenseMatrixType<qType> *dQ;
  unsigned int i,j;
	
  if (dH = dynamic_cast<DenseMatrixType<mType>*>(H))
    {
      if (dQ = dynamic_cast<DenseMatrixType<qType>*>(Q))
	{
	  DenseMatrixType<double> *M = new DenseMatrixType<double>(H->rows, H->rows);
	  double **mArray = M->dataMatrix, *mRow;
	  qType **qArray = dQ->dataMatrix, *qRowHi, *qRowLo;
	  unsigned int m = M->rows;
			
	  MultABt(H, H, M);
	  M->DiagAddInPlace(1.0/C);
			
			
	  for (i=0; i<m; i++)
	    {
	      mRow = mArray[i];
	      qRowLo = qArray[i];
	      qRowHi = qArray[i+m];
				
	      for (j=0; j<m; j++)
		{
		  qRowLo[j] = mRow[j];
		  qRowLo[j+m] = -1.0 * mRow[j];
		  qRowHi[j] = -1.0 * mRow[j];
		  qRowHi[j+m] = mRow[j];
		}
	    }
			
	  delete(M);
	}	// if dM = DenseMatrixType
    }	// if dQ = DenseMatrixType
	
  else
    {	throw (const char *)"QFromH: bad matrix types!";	}
}

// q = [-y ; y] + epsilon
void makeQ(Vector *y, double epsilon, Vector *q)
{
  if (2 * y->cols != q->cols)
    {	throw (const char *)"makeQ: bad matrix sizes!";	}
	
  DenseVector *dy, *dq;
	
  if ((dy = dynamic_cast<DenseVector*>(y)))
    {
      if ((dq = dynamic_cast<DenseVector*>(q)))
	{
	  double *yArray = dy->dataMatrix, *qArray = dq->dataMatrix;
	  unsigned int i, m = y->cols;
			
	  for (i=0; i<m; i++)
	    {
	      qArray[i] = epsilon - yArray[i];
	      qArray[i+m] = epsilon + yArray[i];
	    }
		
	  return;
	}	// if dy = DenseVector
    }
	
  throw (const char *)"makeQ: bad matrix types!";
}



// f = .5 * ((uNew' * Qb) * uNew) + (qb' * uNew);
template <class qType>
double getF(Vector *uNew, MatrixTemplate<qType> *Q, Vector *q, set<int> &basicSet)
{
  double ans;
  DenseVector *temp = new DenseVector(uNew->cols);
		
  MultAB(uNew, basicSet, Q, temp);
		
  ans = ((0.5 * temp->ScalarProduct(uNew, basicSet)) + q->ScalarProduct(uNew, basicSet));
	
  delete(temp);
  return(ans);
}


// f = .5 * (uNew' * (Qb * uNew)) + (qb' * uNew);
template <class T>
double getF(Vector *uNew, MatrixTemplate<T> *H, double C, Vector *q, set<int> &basicSet)
{
  double ans;
  DenseVector *temp = new DenseVector(uNew->cols);
		
  qTimesVec(uNew, H, C, temp);
	
  ans = (0.5 * uNew->ScalarProduct(temp, basicSet))+ q->ScalarProduct(uNew, basicSet);
	
  delete(temp);
  return(ans);
}
	
void madHatter	(Vector *vec)
{
  if (vec->cols%2 != 0)
    {	throw (const char *)"madHatter: input vector cannot have an odd number of elements!";	}
	
  DenseVector *dVec;
	
  if ((dVec = dynamic_cast<DenseVector *>(vec)))
    {
      double *data = dVec->dataMatrix;
      unsigned int m = dVec->cols;
		
      for (unsigned int i = 0, iPlus = (m/2); iPlus<m; i++, iPlus++)
	{
	  if (data[i] > data[iPlus])
	    {
	      data[i] = data[i] - data[iPlus];
	      data[iPlus] = 0.0;
	    }
	  else
	    {
	      data[iPlus] = data[iPlus] - data[i];
	      data[i] = 0.0;
	    }
			
	  //iPlus++;
	}
    }	// if dVec = DenseVector
  else
    {	throw (const char *)"madHatter: bad vector type!";	}
}

// qTilde = -y + (epsilon * hatsBigger) - (epsilon * alphasBigger);
void getQTilde(Vector *y, double epsilon, set<int> &basicSet, Vector *qTilde, set<int> &active)
{
  if (basicSet.size() <= 0)
    throw (const char *)"getQTilde: Basic set has size 0.";

  int m = y->cols, temp;
	
  if (m != qTilde->cols)
    {	throw (const char *)"getQTilde: bad vector sizes!";	}
	
  set<int>::iterator iTemp = basicSet.end();
  iTemp--;
  if (*iTemp >= (2*m))
    {	throw (const char *)"getQTilde: indices out of range!";	}
	
  DenseVector *yD, *qTildeD;
	
  if ((yD = dynamic_cast<DenseVector*>(y)))
    {
      if ((qTildeD = dynamic_cast<DenseVector*>(qTilde)))
	{
	  set<int>::iterator max = basicSet.end(), iter = basicSet.begin();
	  double *qData = qTildeD->dataMatrix, *yData = yD->dataMatrix;
	  active.clear();
			
	  for (; iter != max; iter++)
	    {
	      temp = *iter;
				
	      if (temp >= m)	// We have an alpha.
		{
		  temp = temp-m;
		  qData[temp] = (-1.0 * yData[temp]) - epsilon;
		  active.insert(temp);
		}
	      else	// We have an alpha-hat.
		{
		  qData[temp] = (-1.0 * yData[temp]) + epsilon;
		  active.insert(temp);
		}
	    }
			
	  return;
	}
    }
	
  throw (const char *)"getQTilde: bad vector types!";		
}
				
void uTildeToU(Vector *uTilde, set<int> &basicSet, Vector *u)
{
  if ((2 * uTilde->cols) != u->cols)
    {	throw (const char *)"uTildeToU: bad vector sizes!";	}
	
  DenseVector *uTildeD, *uD;	

  if ((uTildeD = dynamic_cast<DenseVector*>(uTilde)))
    {
      if ((uD = dynamic_cast<DenseVector*>(u)))
	{
	  double *uData = uTildeD->dataMatrix, *rData = uD->dataMatrix;
	  set<int>::iterator max = basicSet.end(), iter = basicSet.begin();
	  int temp, m = uTilde->cols;
			
	  for (; iter != max; iter++)
	    {
	      temp = *iter;
				
	      if (temp >= m)	// We have an alpha
		{	rData[temp] = -1.0 * uData[temp-m];	}
	      else	// We have an alpha-hat.
		{	rData[temp] = uData[temp];	}
	    }
			
	  u->ZeroInactivesInPlace(basicSet);
			
	  return;
	}
    }  	

  throw (const char *)"uTildeToU: bad vector type!";
}


void getAlphasHats(Vector *u, Vector *alphas, Vector *hats)
{
  DenseVector *uD, *alphaD, *hatD;
	
  if (u->cols%2 != 0)
    {	throw (const char *)"getAlhpasHats: vector u must have an even number of elements!";	}
	
  if ((uD = dynamic_cast<DenseVector*>(u)))
    {
      if ((alphaD = dynamic_cast<DenseVector*>(alphas)))
	{
	  if ((hatD = dynamic_cast<DenseVector*>(hats)))
	    {
	      int size = uD->cols/2;
	      for (int i=0; i < size; i++) {
		hatD->dataMatrix[i] = uD->dataMatrix[i];
		alphaD->dataMatrix[i] = uD->dataMatrix[i+size];
	      }
				
	      return;
	    }
	}
    }
	
  throw (const char *)"getAlphasHats: bad vector types!";
}
/*
void spoolTo(string stopPoint)
{
  string temp;
	
  while (cin >> temp)
    {
      if (temp == stopPoint)
	{	break;	}
    }
}

void spoolTo(ifstream &theFile, string stopPoint)
{
  string temp;
	
  while (theFile >> temp)
    {
      if (temp == stopPoint)
	{	break;	}
    }
}
*/

void getTrainingData(::ifstream &fileIn, DenseMatrixType<double>* &A,
		     DenseVector* &y)
{

  int rows, cols, dataCols;
  char comma;
	
  // Pull rows and columns from header if present, otherwise determine
  // dynamically
  if (gHEADER) {
    fileIn >> rows >> comma >>  dataCols;
  } else {

	  ::ifstream inFile(gSOURCEFILENAME,ios::in);
    char inBuf[MAXROWLENGTH];
    inFile.getline(inBuf,MAXROWLENGTH);
    // Determine number of cols by taking number of commas and adding 1
    cols = 1;
    for (unsigned int i=0; i < strlen(inBuf); i++)
      if (inBuf[i]==',') cols++;
    dataCols = cols-1;
    rows = 1; // have read one row
    // Scan rows and count how many there are
    while (inFile.getline(inBuf,MAXROWLENGTH))
      rows++;
    inFile.close();

  }
	
  A = new DenseMatrixType<double>(rows, dataCols);
  y = new DenseVector(rows);
  double **data = A->dataMatrix, *dataRow, *yRow = y->dataMatrix;
	
  for (int i = 0; i < rows; i++)
    {
      dataRow = data[i];
      for (int j = 0; j < dataCols; j++)
	{	fileIn >> dataRow[j] >> comma;	}
		
      fileIn >> yRow[i];
    }

}

// Parse the inputs off the command line.
void ParseInputs(int argc, char *argv[]) {
  int i;

  // Set default values 
  gC = 1;
  gSETC = FALSE;
  gCONVERGENCE = TRUE;
  gMAXITER = 100;
  gTOL = 1e-3;
  gPROMPT = TRUE;
  gEPSILON = 0.1;
  gNORMALIZE = FALSE;

  // Parse the inputs
  for (i=1; i < argc && (argv[i][0]=='-'); i++) {
    
    if (!strcmp(argv[i],"-C")) {
      i++;
      gC = atof(argv[i]);
      gSETC = TRUE;
    } else if (!strcmp(argv[i],"-noconverge")) {
      gCONVERGENCE = FALSE;
    } else if (!strcmp(argv[i],"-header")) {
      gHEADER = TRUE;
    } else if (!strcmp(argv[i],"-noprompt")) {
      gPROMPT = FALSE;
    } else if (!strcmp(argv[i],"-test")) {
      i++;
      strcpy(gTESTFILENAME,argv[i]);
    } else if (!strcmp(argv[i],"-maxiter")) {
      i++;
      gMAXITER = atoi(argv[i]);
    } else if (!strcmp(argv[i],"-normalize")) {
      gNORMALIZE = TRUE;
    } else if (!strcmp(argv[i],"-tol")) {
      i++;
      gTOL = atof(argv[i]);
    } else if (!strcmp(argv[i],"-epsilon")) {
      i++;
      gEPSILON = atof(argv[i]);
    } else
      ThrowIt("Invalid option.");

  } // end-for

  if (gC < 0)
    ThrowIt("Error: C must be positive.");

  if (i != argc-2) {
    PRINT("Active Support Vector Regression Software.\n");
    PRINT("Copyright (C) 2002 David R. Musicant and Alexander Feinberg.\n");
    PRINT(VERSION);
    PRINT("\n\n");
    PRINT("Syntax: asvr [options] training_file solution_file\n");
    PRINT("Possible choices are:\n");
    //PRINT("-noconverge          Do not enforce convergence conditions\n");
    PRINT("-maxiter x           Specify maximum number of iterations as x\n");
    PRINT("-C x                 Specify value x for C\n");
    //PRINT("-noprompt            Do not prompt to stop if ASVR seems stuck\n");
    PRINT("-normalize           Normalize the data\n");
    PRINT("-tol x               Specify convergence tolerance as x\n");
    exit(2);
  }

  strcpy(gSOURCEFILENAME,argv[i]);
  strcpy(gOUTPUTFILENAME,argv[i+1]);
}



void loadData(DenseMatrixType<double>* &A, DenseVector* &y,
              double &eps, double &C, double &D, bool &silent)
{
	::ifstream fileIn;
	
  eps = gEPSILON;
  C = gC;
  D = 1;
  silent = false;


  fileIn.open(gSOURCEFILENAME);
  
  getTrainingData(fileIn, A, y);

  fileIn.close();
}


void WriteWGamma(DenseVector *w,double gamma,double *mean, double *std,
                 char *outputFile) {
  int n = w->Size();
  FILE *out = fopen(outputFile,"w");
  if (!out)
    ThrowIt("Unable to write to output file.");
  fprintf(out,"%d\n%30.15g\n",n,gamma);
  int i;
  for (i=0; i < n; i++)
    fprintf(out,"%30.15g\n",(*w)[i]);
  for (i=0; i < n; i++)
    fprintf(out,"%30.15g\n",mean[i]);
  for (i=0; i < n; i++)
    fprintf(out,"%30.15f\n",std[i]);
  fclose(out);
}

int main(int argc, char *argv[])
{
  try
    {
		
      DenseMatrixType<double> *A;
      DenseVector *y,*w;
      double eps, C, D, gamma, objective;
      bool silent;
      ParseInputs(argc,argv);
      
      loadData(A, y, eps, C, D, silent);
		
      w = new DenseVector(A->cols);
      
      cout << "Active Set Support Vector Regression" << endl;
      cout << "(C) 2002 David R. Musicant and Alexander Feinberg" << endl;
      cout << VERSION << endl;
      cout << "Parameter settings:" << endl;
      cout << "  C = " << C << endl;
      cout << "  Epsilon = " << eps << endl;
      cout << "  Max iterations = " << gMAXITER << endl;
      double time;
      int iters;

      // Handle normalization, if selected
      double *mean = new double[A->cols];
      double *std = new double[A->cols];
      if (gNORMALIZE) {
	A->GetMeanStd(mean,std);
	A->Normalize(mean,std);
      } else
	for (int i=0; i < A->cols; i++) {
	  mean[i] = 0;
	  std[i] = 1;
	}
		
      computeSolution(A, y, eps, C, D, w, gamma, objective, 
		      time, gMAXITER, gTOL, iters, false, silent);

      cout << "Running time = " << time << " seconds." << endl;
  
      WriteWGamma(w,gamma,mean,std,gOUTPUTFILENAME);
		
      delete(A);
      delete(y);
      delete(w);	
      delete [] mean;
      delete [] std;
    }
  catch(const char *err)
    {
      cerr << err << endl;
      exit(0);
    }
}
