/* -------------------------------------------------------------

This file is a component of SDPA
Copyright (C) 2004-2013 SDPA Project

This program is free software; you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation; either version 2 of the License, or
(at your option) any later version.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with this program; if not, write to the Free Software
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307 USA

------------------------------------------------------------- */

#include "sdpa_parts.h"
#include "sdpa_linear.h"
#include "sdpa_newton.h"  

namespace sdpa {

ComputeTime::ComputeTime()
{
  Predictor = 0.0;
  Corrector = 0.0;
  
  StepPredictor = 0.0;
  StepCorrector = 0.0;
  
  xMatTime = 0.0;
  zMatTime = 0.0;
  xMatzMatTime = 0.0;
  invzMatTime = 0.0;
  
  EigxMatTime = 0.0;
  EigzMatTime = 0.0;
  EigxMatzMatTime = 0.0;

  makebMat = 0.0;
  B_DIAG   = 0.0;
  B_F1     = 0.0;
  B_F2     = 0.0;
  B_F3     = 0.0;
  B_PRE    = 0.0;

  makebMatgVec = 0.0;
  makerMat = 0.0;
  choleskybMat = 0.0;
  
  solve    = 0.0;
  sumDz    = 0.0;
  makedX    = 0.0;
  symmetriseDx = 0.0;
  makedXdZ = 0.0;
  updateRes = 0.0;

  MainLoop  = 0.0;
  FileRead  = 0.0;
  FileCheck = 0.0;
  FileChange= 0.0;
  TotalTime = 0.0;
}

ComputeTime::~ComputeTime()
{
  // Nothing needs.
}


void ComputeTime::display(FILE* fpout)
{
  if (fpout == NULL) {
    return;
  }
  fprintf(fpout,"\n");
  #if 0
  if (TotalTime <= 0.0) {
    return;
  }
  #endif
  fprintf(fpout, "                         Time(sec) ");
  fprintf(fpout," Ratio(%% : MainLoop) \n");
  fprintf(fpout, " Predictor time  =       %f,  %f\n",
	  Predictor, Predictor/MainLoop*100.0);
  fprintf(fpout, " Corrector time  =       %f,  %f\n",
	  Corrector, Corrector/MainLoop*100.0);
  fprintf(fpout, " Make bMat time  =       %f,  %f\n",
	  makebMat, makebMat/MainLoop*100.0);
  fprintf(fpout, " Make bDia time  =       %f,  %f\n",
	  B_DIAG,B_DIAG/MainLoop*100.0);
  fprintf(fpout, " Make bF1  time  =       %f,  %f\n",
	  B_F1,B_F1/MainLoop*100.0);
  fprintf(fpout, " Make bF2  time  =       %f,  %f\n",
	  B_F2,B_F2/MainLoop*100.0);
  fprintf(fpout, " Make bF3  time  =       %f,  %f\n",
	  B_F3,B_F3/MainLoop*100.0);
  fprintf(fpout, " Make bPRE time  =       %f,  %f\n",
	  B_PRE,B_PRE/MainLoop*100.0);
  fprintf(fpout, " Make rMat time  =       %f,  %f\n",
	  makerMat, makerMat/MainLoop*100.0);
  fprintf(fpout, " Make bMatgVec   =       %f,  %f\n",
	  makebMatgVec, makebMatgVec/MainLoop*100.0);
  fprintf(fpout, " Cholesky bMat   =       %f,  %f\n",
	  choleskybMat, choleskybMat/MainLoop*100.0);
  fprintf(fpout, " Ste Pre time    =       %f,  %f\n",
	  StepPredictor, StepPredictor/MainLoop*100.0);
  fprintf(fpout, " Ste Cor time    =       %f,  %f\n",
	  StepCorrector, StepCorrector/MainLoop*100.0);
  fprintf(fpout, " solve           =       %f,  %f\n",
	  solve, solve/MainLoop*100.0);
  fprintf(fpout, " sumDz           =       %f,  %f\n",
  	  sumDz, sumDz/MainLoop*100.0);
  fprintf(fpout, " makedX          =       %f,  %f\n",
  	  makedX, makedX/MainLoop*100.0);
  fprintf(fpout, " symmetriseDx    =       %f,  %f\n",
  	  symmetriseDx, symmetriseDx/MainLoop*100.0);
  fprintf(fpout, " makedXdZ        =       %f,  %f\n",
	  makedXdZ, makedXdZ/MainLoop*100.0);
  fprintf(fpout, " xMatTime        =       %f,  %f\n",
	  xMatTime, xMatTime/MainLoop*100.0);
  fprintf(fpout, " zMatTime        =       %f,  %f\n",
	  zMatTime, zMatTime/MainLoop*100.0);
  fprintf(fpout, " invzMatTime     =       %f,  %f\n",
  	  invzMatTime, invzMatTime/MainLoop*100.0);
  fprintf(fpout, " xMatzMatTime    =       %f,  %f\n",
	  xMatzMatTime, xMatzMatTime/MainLoop*100.0);
  fprintf(fpout, " EigxMatTime     =       %f,  %f\n",
	  EigxMatTime, EigxMatTime/MainLoop*100.0);
  fprintf(fpout, " EigzMatTime     =       %f,  %f\n",
	  EigzMatTime, EigzMatTime/MainLoop*100.0);
  fprintf(fpout, " EigxMatzMatTime =       %f,  %f\n",
	  EigxMatzMatTime, EigxMatzMatTime/MainLoop*100.0);
  fprintf(fpout, " updateRes       =       %f,  %f\n",
	  updateRes, updateRes/MainLoop*100.0);
  double total_eigen = EigxMatTime + EigzMatTime + EigxMatzMatTime;
  fprintf(fpout, " EigTime         =       %f,  %f\n",
	  total_eigen, total_eigen/MainLoop*100.0);
  double sub_total_bMat = MainLoop - makebMat;
  fprintf(fpout, " sub_total_bMat  =       %f,  %f\n",
	  sub_total_bMat, sub_total_bMat/MainLoop*100.0);
  fprintf(fpout, " Main Loop       =       %f,  %f\n",
	  MainLoop, MainLoop/MainLoop*100.0);
  fprintf(fpout, " File Check      =       %f,  %f\n",
	  FileCheck, FileCheck/MainLoop*100.0);
  fprintf(fpout, " File Change     =       %f,  %f\n",
	  FileChange, FileChange/MainLoop*100.0);
  fprintf(fpout, " File Read       =       %f,  %f\n",
	  FileRead, FileRead/MainLoop*100.0);
  fprintf(fpout, " Total           =       %f,  %f\n",
	  TotalTime, TotalTime/MainLoop*100.0);
  fprintf(fpout, "\n");

  return;
}

//-------------------------------------------------------------

Parameter::Parameter()
{
  // setDefaultParameter();
}
Parameter::Parameter(FILE* parameterFile)
{
  readFile(parameterFile);
}

Parameter::~Parameter()
{
  // Nothings needs.
}

void Parameter::setDefaultParameter(Parameter::parameterType type)
{
  if (type == PARAMETER_STABLE_BUT_SLOW) {
    maxIteration =  1000;
    epsilonStar  =  1.0e-7;
    lambdaStar   =  1.0e+4;
    omegaStar    =  2.0;
    lowerBound   = -1.0e+5;
    upperBound   =  1.0e+5;
    betaStar     =  0.1;
    betaBar      =  0.5;
    gammaStar    =  0.8;
    epsilonDash  =  1.0e-7;
  }
  else if (type == PARAMETER_UNSTABLE_BUT_FAST) {
    maxIteration =  100;
    epsilonStar  =  1.0e-7;
    lambdaStar   =  1.0e+2;
    omegaStar    =  2.0;
    lowerBound   = -1.0e+5;
    upperBound   =  1.0e+5;
    betaStar     =  0.01;
    betaBar      =  0.02;
    gammaStar    =  0.95;
    epsilonDash  =  1.0e-7;
  }
  else {
    maxIteration =  100;
    epsilonStar  =  1.0e-7;
    lambdaStar   =  1.0e+2;
    omegaStar    =  2.0;
    lowerBound   = -1.0e+5;
    upperBound   =  1.0e+5;
    betaStar     =  0.1;
    betaBar      =  0.3;
    gammaStar    =  0.9;
    epsilonDash  =  1.0e-7;
  }    
  strcpy(xPrint,xPRINT_DEFAULT);
  strcpy(XPrint,XPRINT_DEFAULT);
  strcpy(YPrint,YPRINT_DEFAULT);
  strcpy(infPrint,infPRINT_DEFAULT);
}

char Parameter::xPRINT_DEFAULT[PRINT_DEFAULT_LENGTH] = "%+8.3e";
char Parameter::XPRINT_DEFAULT[PRINT_DEFAULT_LENGTH] = "%+8.3e";
char Parameter::YPRINT_DEFAULT[PRINT_DEFAULT_LENGTH] = "%+8.3e";
char Parameter::infPRINT_DEFAULT[PRINT_DEFAULT_LENGTH] = "%+10.16e";

void Parameter::readFile(FILE* parameterFile)
{
  fscanf(parameterFile,"%d%*[^\n]",&maxIteration);
  fscanf(parameterFile,"%lf%*[^\n]",&epsilonStar);
  fscanf(parameterFile,"%lf%*[^\n]",&lambdaStar);
  fscanf(parameterFile,"%lf%*[^\n]",&omegaStar);
  fscanf(parameterFile,"%lf%*[^\n]",&lowerBound);
  fscanf(parameterFile,"%lf%*[^\n]",&upperBound);
  fscanf(parameterFile,"%lf%*[^\n]",&betaStar);
  fscanf(parameterFile,"%lf%*[^\n]",&betaBar);
  fscanf(parameterFile,"%lf%*[^\n]",&gammaStar);
  fscanf(parameterFile,"%lf%*[^\n]",&epsilonDash);
  fscanf(parameterFile,"%s %*[^\n]",xPrint);
  fscanf(parameterFile,"%s %*[^\n]",XPrint);
  fscanf(parameterFile,"%s %*[^\n]",YPrint);
  fscanf(parameterFile,"%s %*[^\n]",infPrint);
  if (strcmp(xPrint,NO_P_FORMAT)!=0 && xPrint[0]!='%') {
    rMessage("Strange xPrint[" << xPrint << "]"
	     " migh cause trouble when printing x");
  }
  if (strcmp(XPrint,NO_P_FORMAT)!=0 && XPrint[0]!='%') {
    rMessage("Strange XPrint[" << XPrint << "]"
	     " migh cause trouble when printing X.");
  }
  if (strcmp(YPrint,NO_P_FORMAT)!=0 && YPrint[0]!='%') {
    rMessage("Strange YPrint[" << YPrint << "]"
	     " migh cause trouble when printing Y.");
  }
  if (strcmp(infPrint,NO_P_FORMAT)!=0 && infPrint[0]!='%') {
    rMessage("Strange infPrint[" << infPrint << "]"
	     " migh cause trouble when printing information.");
  }
}


void Parameter::display(FILE* fpout, char* printFormat)
{
  if (fpout == NULL) {
    return;
  }
  if (strcmp(printFormat,NO_P_FORMAT) == 0) {
    fprintf(fpout,"%s\n",NO_P_FORMAT);
    return;
  }
  fprintf(fpout, "** Parameters **\n");
  fprintf(fpout, "maxIteration = %d\n",maxIteration);
  fprintf(fpout, "epsilonStar  = ");
  fprintf(fpout, printFormat, epsilonStar );
  fprintf(fpout, "\n");
  fprintf(fpout, "lambdaStar   = ");
  fprintf(fpout, printFormat, lambdaStar );
  fprintf(fpout, "\n");
  fprintf(fpout, "omegaStar    = ");
  fprintf(fpout, printFormat, omegaStar  );
  fprintf(fpout, "\n");
  fprintf(fpout, "lowerBound   = ");
  fprintf(fpout, printFormat, lowerBound);
  fprintf(fpout, "\n");
  fprintf(fpout, "upperBound   = ");
  fprintf(fpout, printFormat, upperBound);
  fprintf(fpout, "\n");
  fprintf(fpout, "betaStar     = ");
  fprintf(fpout, printFormat, betaStar   );
  fprintf(fpout, "\n");
  fprintf(fpout, "betaBar      = ");
  fprintf(fpout, printFormat, betaBar    );
  fprintf(fpout, "\n");
  fprintf(fpout, "gammaStar    = ");
  fprintf(fpout, printFormat, gammaStar  );
  fprintf(fpout, "\n");
  fprintf(fpout, "epsilonDash  = ");
  fprintf(fpout, printFormat, epsilonDash );
  fprintf(fpout, "\n");
  #if 1
  fprintf(fpout, "xPrint       = %s \n", xPrint );
  fprintf(fpout, "XPrint       = %s \n", XPrint );
  fprintf(fpout, "YPrint       = %s \n", YPrint );
  fprintf(fpout, "infPrint     = %s \n", infPrint );
  #endif
  return;
}

//----------------------------------------------------------

StepLength::StepLength()
{
  primal = 0.0;
  dual   = 0.0;

}

StepLength::~StepLength()
{
  finalize();
}


void StepLength::initialize(double alphaP, double alphaD)
{
  primal = alphaP;
  dual   = alphaD;
}

void StepLength::finalize()
{
  // Nothing needs.
}

double StepLength::minBlockVector(BlockVector& aVec)
{
  int nBlock = aVec.nBlock;
  double ret = aVec.ele[0].ele[0];
  double tmp;
  int size = aVec.ele[0].nDim;
  for (int j=1; j<size; ++j) {
    tmp = aVec.ele[0].ele[j];
    if (tmp < ret) {
      ret = tmp;
    }
  }
  for (int k=1; k<nBlock; ++k) {
    size = aVec.ele[k].nDim;
    for (int j=0; j<size; ++j) {
      tmp = aVec.ele[k].ele[j];
      if (tmp < ret) {
	ret = tmp;
      }
    }
  }
  return ret;
}

void StepLength::computeStepLength(Solutions& currentPt,
				   ComputeTime& com)
{
  double alphaBD = 100.0;
  CholmodSpace& cholmodSpace = currentPt.cholmodSpace;
  TimeStart(START1);
  // rMessage("invCholeskyX=");
  double minValue = 1.0e+50;
  for (int l=0; l<cholmodSpace.LP_nBlock; ++l) {
    double* LP_dX   = cholmodSpace.LP_dX;
    double* LP_invX = cholmodSpace.LP_invX;
    double value = LP_dX[l]*LP_invX[l];
    if (value < minValue) {
      minValue = value;
    }
  } // end of 'for (int l)'
  for (int l=0; l<cholmodSpace.SDP_nBlock; ++l) {
    CliqueMatrix&  clique_invCholeskyX
      = cholmodSpace.SDP_block[l].clique_invCholeskyX;
    CliqueMatrix&  clique_dX   = cholmodSpace.SDP_block[l].clique_dX;
    for (int l2=0; l2<clique_dX.nBlock; ++l2) {
      DenseMatrix& invCholeskyX  = clique_invCholeskyX.ele[l2];
      DenseMatrix& DxMat         = clique_dX.ele[l2];
      const int size = DxMat.nRow;
      double value = 0.0; // dummy initialize
      DenseMatrix DLS1;
      DLS1.initialize(size,size);
      Vector Two_BV1(3*size);
      Vector BV1(size);
      if (size > 64) { // Lanczos method
	Vector BV2(size);
	Vector BV3(size);
	Vector BV4(size);
	Vector BV5(size);
	Vector BV6(size);
	Vector BV7(size);
	Vector BV8(size);
	Vector BV9(size);
	// In SDPA,   DLS1 = invCholeskyX*Dx*invCholeskyX'
	// In SDPA-C, DLS1 = invCholeskyX'*Dx*invCholeskyX
	// The last 'T' means transpose of invCholeskyX
	value = Lal::getMinEigen(invCholeskyX, DxMat, DLS1,
				 BV1, BV2, BV3, BV4, BV5, BV6,
				 BV7, BV8, BV9, Two_BV1,'T');
      }
      else { // QR method
	// In SDPA,   DLS1 = invCholeskyX*Dx*invCholeskyX'
	// In SDPA-C, DLS1 = invCholeskyX'*Dx*invCholeskyX
	DenseMatrix DLS2;
	DLS2.initialize(size,size);
	Lal::let(DLS2,'=',DxMat,'*',invCholeskyX);
	Lal::let(DLS1,'=',invCholeskyX,'t',DLS2);
	value = Lal::getMinEigenValue(DLS1, BV1, Two_BV1);
      }
      if (value < minValue) {
	minValue = value;
      }
      #if 0
      rMessage("value for " << l2 << " th clique of "
	       << l << " th block = " << value);
      #endif
    }  // end of 'for (int l2=0; l2<clique_xMat.nBlock; ++l2) '
  }
  double minxInvDxEigenValue = minValue;
  // rMessage("minxInvDxEigenValue = " << minxInvDxEigenValue);
  if (-minxInvDxEigenValue > 1.0 /alphaBD) {
    primal = - 1.0/minxInvDxEigenValue;
    // the limint of primal steplength
  } else {
    primal = alphaBD;
  }
  TimeEnd(END1);
  com.EigxMatTime += TimeCal(START1,END1);

  // calculate  eigenvalues of Z^{-1} dZ
  TimeStart(START2);
  // rMessage("invCholeskyZ=");
  // currentPt.invCholeskyZ.display();
  // rMessage("Dz=");
  // newton.DzMat.display();

  minValue = 1.0e+50;
  for (int l=0; l<cholmodSpace.LP_nBlock; ++l) {
    double* LP_dZ   = cholmodSpace.LP_dZ;
    double* LP_invZ = cholmodSpace.LP_invZ;
    double value = LP_dZ[l]*LP_invZ[l];
    if (value < minValue) {
      minValue = value;
    }
  } // end of 'for (int l)'
  for (int l=0; l<cholmodSpace.SDP_nBlock; ++l) {
    double value = Lal::getMinEigenValue(cholmodSpace.SDP_block[l]);
    if (value < minValue) {
      minValue = value;
    }
  }
  double minzInvDzEigenValue = minValue;
  // rMessage("minzInvDzEigenValue = " << minzInvDzEigenValue);
  if (-minzInvDzEigenValue > 1.0 /alphaBD) {
    dual = - 1.0/minzInvDzEigenValue;
    // the limint of dual steplength
  } else {
    dual = alphaBD;
  }
  TimeEnd(END2);
  com.EigzMatTime += TimeCal(START2,END2);
  #if 0
  rMessage("minxInvDxEigenValue = " << minxInvDxEigenValue);
  rMessage("minzInvDzEigenValue = " << minzInvDzEigenValue);
  #endif
}


void StepLength::MehrotraPredictor(InputData& inputData,
				   Solutions& currentPt,
				   Phase& phase,
				   Switch& reduction,
				   AverageComplementarity& mu,
				   RatioInitResCurrentRes& theta,
				   Parameter& param, 
				   ComputeTime& com)
{
  computeStepLength(currentPt, com);

  // adjust steplength with param.gammaStar
  // param.gammaStar = 0.9;
  primal = param.gammaStar * primal;
  dual   = param.gammaStar * dual;

  CholmodSpace& cholmodSpace = currentPt.cholmodSpace;
  OrderingSpace& order = currentPt.order;

  if (phase.value==SolveInfo::noINFO
      || phase.value==SolveInfo::dFEAS) {
    // primal is infeasible
    if (primal>1.0) {
      primal = 1.0;
    }
  } else {
    // when primal is feasible,
    // check stepP1 is effective or not.
    double incPrimalObj = 0.0;
    // Lal::let(incPrimalObj,'=',C,'.',newton.DxMat);
    CompSpace& C = inputData.C;
    for (int l_index = 0; l_index< C.LP_sp_nBlock; ++l_index) {
      const double Cvalue = C.LP_sp_block[l_index];
      const int         l = C.LP_sp_index[l_index];
      const double dXvalue = cholmodSpace.LP_dX[l];
      incPrimalObj += Cvalue*dXvalue;
    }

    for (int l_index = 0; l_index< C.SDP_sp_nBlock; ++l_index) {
      double tmpret = 0.0;
      const int l = C.SDP_sp_index[l_index];
      Lal::getInnerProduct(tmpret, C.SDP_sp_block[l_index],
			   cholmodSpace.SDP_block[l].clique_dX,
			   order.SDP_block[l]);
      incPrimalObj += tmpret;
    }
    
    if (incPrimalObj>0.0) {
      #if 1
      if (primal>dual) {
	primal = dual;
      }
      #endif
      if (primal>1.0) {
	primal = 1.0;
      }
    }
  }
  if (phase.value==SolveInfo::noINFO
      || phase.value==SolveInfo::pFEAS) {
    // dual is infeasible
    if (dual>1.0) {
      dual = 1.0;
    }
  } else {
    // when dual is feasible
    // check stepD1 is effective or not.
    double incDualObj;
    Lal::let(incDualObj,'=',inputData.b,'.',cholmodSpace.dyVec);
    if(incDualObj<0.0) {
      #if 1
      if (dual>primal) {
	dual = primal;
      }
      #endif
      if (dual>1.0) {
	dual = 1.0;
      }
    }
  }

#if 1
  // attain feasibility before mu reduction
  if (reduction.switchType==Switch::CENTERING
      && (phase.value == SolveInfo::noINFO
	  || phase.value == SolveInfo::pFEAS
	  || phase.value == SolveInfo::dFEAS) ) {
    double xMatvMat = 0.0;
    // rAl::let(xMatvMat,'=',currentPt.xMat,'.',newton.DzMat);
    for (int l = 0; l < cholmodSpace.LP_nBlock; ++l) {
      xMatvMat += cholmodSpace.LP_X[l]*cholmodSpace.LP_dZ[l];
    }
    for (int l=0; l<cholmodSpace.SDP_nBlock; ++l) {
      CliqueMatrix&   X  = cholmodSpace.SDP_block[l].clique_xMat;
      cholmod_sparse* dZ = cholmodSpace.SDP_block[l].dZ;
      double tmpret = 0.0; // dummy initialize
      Lal::getInnerProduct(tmpret, dZ, X, currentPt.order.SDP_block[l],
			   cholmodSpace.SDP_block[l].Z_blockNumber,
			   cholmodSpace.SDP_block[l].Z_blockIndex);
      xMatvMat += tmpret;
    }

    double uMatzMat = 0.0;
    // rAl::let(uMatzMat,'=',newton.DxMat,'.',currentPt.zMat);
    for (int l = 0; l < cholmodSpace.LP_nBlock; ++l) {
      uMatzMat += cholmodSpace.LP_dX[l]*cholmodSpace.LP_Z[l];
    }
    for (int l=0; l<cholmodSpace.SDP_nBlock; ++l) {
      CliqueMatrix&   dX = cholmodSpace.SDP_block[l].clique_dX;
      cholmod_sparse*  Z = cholmodSpace.SDP_block[l].Z;
      double tmpret = 0.0; // dummy initialize
      Lal::getInnerProduct(tmpret, Z, dX, currentPt.order.SDP_block[l],
			   cholmodSpace.SDP_block[l].Z_blockNumber,
			   cholmodSpace.SDP_block[l].Z_blockIndex);
      uMatzMat += tmpret;
    }

    double uMatvMat = 0.0;
    // rAl::let(uMatvMat,'=',newton.DxMat,'.',newton.DzMat);
    for (int l = 0; l < cholmodSpace.LP_nBlock; ++l) {
      uMatvMat += cholmodSpace.LP_dX[l]*cholmodSpace.LP_dZ[l];
    }
    for (int l=0; l<cholmodSpace.SDP_nBlock; ++l) {
      CliqueMatrix&   dX = cholmodSpace.SDP_block[l].clique_dX;
      cholmod_sparse* dZ = cholmodSpace.SDP_block[l].dZ;
      double tmpret = 0.0; // dummy initialize
      Lal::getInnerProduct(tmpret, dZ, dX, currentPt.order.SDP_block[l],
			   cholmodSpace.SDP_block[l].Z_blockNumber,
			   cholmodSpace.SDP_block[l].Z_blockIndex);
      uMatvMat += tmpret;
    }

    int nDim = cholmodSpace.LP_nBlock;
    for (int l=0; l<cholmodSpace.SDP_nBlock; ++l) {
      nDim += cholmodSpace.SDP_block[l].nDim;
    }
    

    double thetaMax = max((1.0-primal)*theta.primal,
			  (1.0-dual  )*theta.dual);
    double muNew = mu.current
      + (primal*uMatzMat + dual*xMatvMat
	 + primal*dual*uMatvMat) / nDim;
    const double xi = 3.0; // this is one of implicit parameters
    while (thetaMax*mu.initial > xi*muNew) {
      double alphaMax = 0.95 * max(primal,dual);
      primal = min(primal,alphaMax);
      dual   = min(dual  ,alphaMax);
      thetaMax = max((1.0-primal)*theta.primal,
		     (1.0-dual  )*theta.dual);
      muNew = mu.current + (primal*uMatzMat + dual*xMatvMat
			    + primal*dual*uMatvMat) / nDim;
      // if "too short step", then break down the algorithm.
      if (primal < 1.0e-6 && dual < 1.0e-6) {
	break;
      }
    }
  }
#endif
}

void StepLength::Centering(Solutions& currentPt,
			   Parameter& param, 
			   ComputeTime& com)
{
  computeStepLength(currentPt,  com);

  // adjust steplength with param.gammaStar
  // param.gammaStar = 0.5;
  primal = param.gammaStar * primal;
  dual   = param.gammaStar * dual;

  if (primal>1.0) {
    primal = 1.0;
  }
  if (dual>1.0) {
    dual = 1.0;
  }
  

}


void StepLength::display(FILE* fpout)
{
  if (fpout == NULL) {
    return;
  }

  fprintf(fpout,"alpha.primal = %8.3e\n",primal);
  fprintf(fpout,"alpha.dual   = %8.3e\n",dual);
}

//-------------------------------------------------
DirectionParameter::DirectionParameter(double betaStar)
{
  initialize(betaStar);
}

DirectionParameter::~DirectionParameter()
{
  // Nothing needs.
}

void DirectionParameter::initialize(double betaStar)
{
  value = betaStar;
}

void DirectionParameter::Predictor(Phase& phase,
				   Switch& reduction,
				   Parameter& param)
{
  const double nu = 2.0;
  if (phase.value == SolveInfo::pdFEAS) {
    value = param.betaStar;
  } else {
    value = param.betaBar;
    if (reduction.switchType==Switch::AFFINE) {
      value = nu;
    }
  }
}

void DirectionParameter::Centering()
{
  value = 1.0;
}

void DirectionParameter::
MehrotraCorrector(Phase& phase, StepLength& alpha,
		  Solutions& currentPt, 
		  AverageComplementarity& mu, Parameter& param)
{
  int nDim = currentPt.nDim;
  CholmodSpace& cholmodSpace = currentPt.cholmodSpace;
  
  // Lal::let(uMatzMat,'=',newton.DxMat,'.',currentPt.zMat);
  // Lal::let(xMatvMat,'=',currentPt.xMat,'.',newton.DzMat);
  // Lal::let(uMatvMat,'=',newton.DxMat,'.',newton.DzMat);
  double uMatzMat = 0.0;
  double xMatvMat = 0.0;
  double uMatvMat = 0.0;
  for (int l=0; l<cholmodSpace.LP_nBlock; ++l) {
    uMatzMat += cholmodSpace.LP_dX[l] * cholmodSpace.LP_Z [l];
    xMatvMat += cholmodSpace.LP_X [l] * cholmodSpace.LP_dZ[l];
    uMatvMat += cholmodSpace.LP_dX[l] * cholmodSpace.LP_dZ[l];
  }
  for (int l=0; l<cholmodSpace.SDP_nBlock; ++l) {
    OrderingMatrix&        order = currentPt.order.SDP_block[l];
    CholmodMatrix& cholmodMatrix = cholmodSpace.SDP_block[l];
    CliqueMatrix&    clique_xMat = cholmodMatrix.clique_xMat;
    CliqueMatrix&    clique_dX   = cholmodMatrix.clique_dX;
    cholmod_sparse*            Z = cholmodMatrix.Z;
    cholmod_sparse*           dZ = cholmodMatrix.dZ;
    int*           Z_blockNumber = cholmodMatrix.Z_blockNumber;
    int*           Z_blockIndex  = cholmodMatrix.Z_blockIndex;
    double ip = 0.0; // dummy initialize
    Lal::getInnerProduct(ip, Z, clique_dX, order, Z_blockNumber, Z_blockIndex);
    uMatzMat += ip;
    Lal::getInnerProduct(ip, dZ, clique_xMat, order,
			 Z_blockNumber, Z_blockIndex);
    xMatvMat += ip;
    Lal::getInnerProduct(ip, dZ, clique_dX, order, Z_blockNumber, Z_blockIndex);
    uMatvMat += ip;
  }

  double primal = alpha.primal;
  double   dual = alpha.dual;

  // primal = max(alpha.primal,1.0);
  // dual   = max(alpha.dual,  1.0);

  double muTarget = mu.current
    + (primal*uMatzMat + dual*xMatvMat + primal*dual*uMatvMat) / nDim;
  // rMessage("muTarget  : " << muTarget);
  // rMessage("muCurrent : " << mu.current);
  value = muTarget/mu.current;
  // rMessage("muValue : " << value);
  if (value < 1.0) {
    value = value*value;
  }
  if (phase.value==SolveInfo::pdFEAS) {
    // rMessage("MehrotraCorrector : pdFEAS" << value);
    if (value < param.betaStar) {
      value = param.betaStar;
    }
    if (value > 1.0) {
      value = 1.0;
    }
  } else {
    if (value < param.betaBar) {
      value = param.betaBar;
    }
  }
  // rMessage("MehrotraCorrector : " << value);
}

void DirectionParameter::display(FILE* fpout)
{
  if (fpout == NULL) {
    return;
  }
  fprintf(fpout,"beta.value = %8.3e\n",value);
}

//---------------------------------------------------

Switch::Switch(SwitchType switchType)
{
  initialize(switchType);
}

Switch::~Switch()
{
  // Nothing needs.
}

void Switch::initialize(SwitchType switchType)
{
  this->switchType = switchType;
}

void Switch::MehrotraPredictor(Phase& phase)
{
  if (phase.value==SolveInfo::noINFO
      || phase.value==SolveInfo::pFEAS
      || phase.value==SolveInfo::dFEAS) {
    // At least one of primal or dual is infeasible.
    switchType = CENTERING;
  } else {
    switchType = AFFINE;
  }
}

void Switch::display(FILE* fpout)
{
  if (fpout == NULL) {
    return;
  }

  if (switchType == CENTERING) {
    fprintf(fpout,"reduction.switchType == CENTERING\n");
  } else {
    fprintf(fpout,"reduction.switchType == AFFINE\n");
  }
}

// ----------------------------------------

AverageComplementarity::AverageComplementarity(double lambdaStar)
{
  initialize(lambdaStar);
}

AverageComplementarity::~AverageComplementarity()
{
  // Nothing needs.
}

void AverageComplementarity::initialize(double lambdaStar)
{
  initial = lambdaStar*lambdaStar;
  current = initial;
  // rMessage("initial average = " << initial);
}

void AverageComplementarity::update(Solutions& currentPt)
{
  CholmodSpace& cholmodSpace = currentPt.cholmodSpace;
  int nDim = cholmodSpace.LP_nBlock;
  current = 0.0;
  for (int l=0; l<cholmodSpace.LP_nBlock; ++l) {
    current += cholmodSpace.LP_Z[l]*cholmodSpace.LP_X[l];
  }
  for (int l=0; l<cholmodSpace.SDP_nBlock; ++l) {
    nDim += cholmodSpace.SDP_block[l].nDim;
    cholmod_sparse* Z = cholmodSpace.SDP_block[l].Z;
    CliqueMatrix&   X = cholmodSpace.SDP_block[l].clique_xMat;
    double tmpret = 0.0; // dummy initialize
    Lal::getInnerProduct(tmpret, Z, X, currentPt.order.SDP_block[l],
			 cholmodSpace.SDP_block[l].Z_blockNumber,
			 cholmodSpace.SDP_block[l].Z_blockIndex);
    current += tmpret;
  }
  current /= nDim;
}

void AverageComplementarity::display(FILE* fpout)
{
  if (fpout == NULL) {
    return;
  }

  fprintf(fpout,"mu0 = %8.3e\n",initial);
  fprintf(fpout,"mu  = %8.3e\n",current);
}

//--------------------------------------------------


RatioInitResCurrentRes::RatioInitResCurrentRes()
{
  primal = 0.0;
  dual   = 0.0;
}

RatioInitResCurrentRes::~RatioInitResCurrentRes()
{
  // Nothing needs.
}

void RatioInitResCurrentRes::initialize(Parameter& param,
					Residuals& currentRes)
{
  double accuracy = param.epsilonDash;
  if (currentRes.normPrimal < accuracy) {
    primal = 0.0;
  } else {
    primal = 1.0;
  }
  if (currentRes.normDual < accuracy) {
    dual = 0.0;
  } else {
    dual = 1.0;
  }
}

void RatioInitResCurrentRes::update(Switch& reduction,
				    StepLength& alpha)
{
  if (reduction.switchType==Switch::CENTERING) {
    // At least one of primal or dual is infeasible
    primal = fabs((1.0-alpha.primal)*primal);
    dual   = fabs((1.0-alpha.dual  )*dual  );
  }
}

void RatioInitResCurrentRes::update_exact(Residuals& currentRes,
					  Parameter& param)
{
  if (currentRes.initNormPrimal
      > param.epsilonDash * 1.0e-2) {
    primal = currentRes.normPrimal / currentRes.initNormPrimal;
  }
  else {
    primal = 0.0;
  }
  if (currentRes.initNormDual
      > param.epsilonDash * 1.0e-2) {
    dual = currentRes.normDual / currentRes.initNormDual;
  }
  else {
    dual = 0.0;
  }
}

void RatioInitResCurrentRes::display(FILE* fpout)
{
  if (fpout == NULL) {
    return;
  }

  fprintf(fpout,"theta.primal = %8.3e\n",primal);
  fprintf(fpout,"theta.dual   = %8.3e\n",dual);
}

//---------------------------------------------------

SolveInfo::SolveInfo()
{
  rho = 0.0;
  etaPrimal = 0.0;
  etaDual   = 0.0;
  objValPrimal = 0.0;
  objValDual   = 0.0;
}

SolveInfo::SolveInfo(InputData& inputData, Solutions& currentPt, 
		     double mu0, double omegaStar)
{
  initialize(inputData,currentPt,mu0,omegaStar);
}

SolveInfo::~SolveInfo()
{
  // Nothing needs.
}

void SolveInfo::initialize(InputData& inputData, Solutions& currentPt, 
			   double mu0, double omegaStar)
{
  int nDim = currentPt.nDim;
  Vector& b = inputData.b;
  CompSpace& C = inputData.C;
  CholmodSpace& cholmodSpace = currentPt.cholmodSpace;

  rho = 1.0;
  etaPrimal = omegaStar * nDim * mu0;
  etaDual   = omegaStar * nDim * mu0;
  cholmodSpace.getInnerProductAX(objValPrimal, C, currentPt.order);
  #if 0
  C.display();
  cholmodSpace.display();
  rMessage("objValPrimal = " << objValPrimal);
  #endif
  Lal::let(objValDual,'=',b,'.',cholmodSpace.yVec);
}

void SolveInfo::update(InputData& inputData,
		       Solutions& currentPt,
		       Residuals& currentRes,
		       AverageComplementarity& mu,
		       RatioInitResCurrentRes& theta,
		       Parameter& param)
{
  CholmodSpace& cholmodSpace = currentPt.cholmodSpace;

  // Lal::let(objValPrimal,'=',C,'.',currentPt.xMat);
  objValPrimal = 0.0;
  cholmodSpace.getInnerProductAX(objValPrimal, inputData.C, currentPt.order);
  Lal::let(objValDual,'=',inputData.b,'.',cholmodSpace.yVec);

  int nDim = cholmodSpace.LP_nBlock;
  for (int l=0; l<cholmodSpace.SDP_nBlock; ++l) {
    nDim += cholmodSpace.SDP_block[l].nDim;
  }

  double primal = theta.primal;
  double dual   = theta.dual;
  double omega  = param.omegaStar;
  double lambda = param.lambdaStar;
  rho = 0.0;
  double x0z0     = nDim*mu.initial;
  double xMatzMat = nDim*mu.current;
  
  // Lal::let(x0zMat,'=',initPt_xMat,'.',currentPt.zMat);
  double x0zMat   = 0.0;
  for (int l=0; l<cholmodSpace.LP_nBlock; ++l) {
    x0zMat += cholmodSpace.LP_Z[l];
  }
  for (int l=0; l<cholmodSpace.SDP_nBlock; ++l) {
    cholmod_sparse* Z = cholmodSpace.SDP_block[l].Z;
    const int ncol = (int) Z->ncol;
    for (int j=0; j < ncol; ++j) {
      const int diag_row = ((int*)Z->p)[j];
      x0zMat += ((double*)Z->x)[diag_row];
    }
  }
  x0zMat *= lambda;
  
  // Lal::let(xMatz0,'=',currentPt.xMat,'.',initPt_zMat);
  double xMatz0   = 0.0;
  for (int l=0; l<cholmodSpace.LP_nBlock; ++l) {
    xMatz0 += cholmodSpace.LP_X[l];
  }
  for (int l=0; l<cholmodSpace.SDP_nBlock; ++l) {
    CliqueMatrix& clique_xMat = cholmodSpace.SDP_block[l].clique_xMat;
    OrderingMatrix& order = currentPt.order.SDP_block[l];
    int size = cholmodSpace.SDP_block[l].nDim;
    for (int j=0; j<size; ++j) {
      for (int index1 = 0; index1 < order.dXtNonzeros[j]; ++index1) {
	int i = order.dXtIndex[j][index1];
	if (i==j) {
	  int block = order.dXtClique[j][index1];
	  int position = order.dXtBlock[j][index1];
	  xMatz0 += clique_xMat.ele[block].de_ele[position];
	  break;
	}
      }
    }
  }
  xMatz0 *= lambda;

  double accuracy = param.epsilonDash;

  if (currentRes.normPrimal <= accuracy) {
    // rMessage("primal accuracy");
    if (xMatz0 < etaPrimal) {
      etaPrimal = xMatz0;
    }
  }
  if (currentRes.normDual <= accuracy) {
    // rMessage("dual accuracy");
    if (x0zMat < etaDual) {
      etaDual = x0zMat;
    }
  }

  // primal is infeasible and dual is feasible
  if (currentRes.normPrimal > accuracy
      && currentRes.normDual <= accuracy) {
    rho = primal*x0zMat
      / ((primal+(1.0-primal)*omega)*etaDual + xMatzMat);
  }

  // primal is feasible and dual is infeasible
  if (currentRes.normPrimal <= accuracy
      && currentRes.normDual > accuracy) {
    rho = dual*xMatz0
      / ((dual+(1.0-dual)*omega)* etaPrimal + xMatzMat);
  }
  
  // primal and dual are infeasible
  if (currentRes.normPrimal > accuracy
      && currentRes.normDual > accuracy) {
    rho = (dual*xMatz0+primal*x0zMat)
      / ((primal*dual
	  + omega *(primal*(1.0-dual) + (1.0-primal)*dual))* x0z0
	 + xMatzMat);
  }
  // rMessage("eta Primal = " << etaPrimal);
  // rMessage("eta Dual = " << etaDual);
}

// 2007/09/13 kazuhide nakata  
// print information of ObjVal, residual, gap, complementarity
//   b^T y       + R \bullet X =  value,    norm(r),   norm(Z)
//   C \bullet X + r^T y       =  value,    norm(R),   norm(X)
//     gap                         gap,     mu * nDim    
void SolveInfo::check(InputData& inputData,
					  Solutions& currentPt,
					  Residuals& currentRes,
					  AverageComplementarity& mu,
					  RatioInitResCurrentRes& theta,
					  Parameter& param)
{
  rMessage("This function is not implemented in SDPA-C");
  #if 0
  double tmp,tmp1p,tmp1d,tmp2p,tmp2d,tmp3p,tmp3d,tmp4,tmp5p,tmp5d;

  Lal::let(tmp,'=',inputData.b,'.',currentPt.yVec);
  tmp1p = - tmp;
  printf("Primal: %9.1e",tmp1p);
  Lal::let(tmp,'=',currentRes.dualMat,'.',currentPt.xMat);
  tmp2p = -tmp;
  printf(" + %9.1e",tmp2p);
  tmp3p = tmp1p + tmp2p;
  printf(" = %9.1e",tmp3p);
  printf(",   residual:%-9.1e",currentRes.normDualMat);
  tmp5p = currentRes.computeMaxNorm(currentPt.zMat);
  printf(" norm:%-9.1e\n",tmp5p);

  Lal::let(tmp,'=',inputData.C,'.',currentPt.xMat);
  tmp1d = - tmp;
  printf("Dual:   %9.1e",tmp1d);
  Lal::let(tmp,'=',currentRes.primalVec,'.',currentPt.yVec);
  tmp2d = -tmp;
  printf(" + %9.1e",tmp2d);
  tmp3d = tmp1d + tmp2d;
  printf(" = %9.1e",tmp3d);
  printf(",   residual:%-9.1e", currentRes.normPrimalVec);
  tmp5d = currentRes.computeMaxNorm(currentPt.xMat);
  printf(" norm:%-9.1e\n",tmp5d);

  tmp4 = tmp1p - tmp1d;
  printf("P-D:    %9.1e",tmp4);
  tmp4 = tmp3p - tmp3d;
  printf("               %9.1e",tmp4);
  tmp4 = mu.current * currentPt.nDim;
  printf(",    mu * n:%-9.1e\n",tmp4);

  #endif
}


void SolveInfo::display(FILE* fpout)
{
  if (fpout == NULL) {
    return;
  }

  fprintf(fpout,"rSolveInfo.rho          = %8.3e\n",rho);
  fprintf(fpout,"rSolveInfo.etaPrimal    = %8.3e\n",etaPrimal);
  fprintf(fpout,"rSolveInfo.etaDual      = %8.3e\n",etaDual);
  fprintf(fpout,"rSolveInfo.objValPrimal = %8.3e\n",objValPrimal);
  fprintf(fpout,"rSolveInfo.objValDual   = %8.3e\n",objValDual);
}

// ----------------------------------------------------

Phase::Phase()
{
  nDim = 0;
  value = SolveInfo::noINFO;
}

Phase::~Phase()
{
  // Nothing needs.
}

bool Phase::initialize(Residuals& currentRes,
		       SolveInfo& solveInfo,
		       Parameter& param, int nDim)
{
  this->nDim = nDim;
  return updateCheck(currentRes, solveInfo, param);
}

bool Phase::updateCheck(Residuals& currentRes,
			SolveInfo& solveInfo,
			Parameter& param)
{
  const double NONZERO = 1.0e-6;
  double accuracy = param.epsilonDash;
  value = SolveInfo::noINFO;

  if (currentRes.normPrimal <= accuracy) {
    if (currentRes.normDual <= accuracy) {
      value = SolveInfo::pdFEAS;
    } else {
      value = SolveInfo::pFEAS;
    }
  }
  if (value==SolveInfo::noINFO
      && currentRes.normDual <= accuracy) {
    value = SolveInfo::dFEAS;
  }
  if (value==SolveInfo::pdFEAS) {
    double mean = (fabs(solveInfo.objValPrimal)+
		   fabs(solveInfo.objValDual)) / 2.0;
    double PDgap = fabs(solveInfo.objValPrimal - solveInfo.objValDual);

    double dominator;
    if (mean < 1.0) {
      dominator = 1.0;
    } else {
      dominator = mean;
    }
    #if 0
    rMessage("PDgap = " << PDgap);
    rMessage("dominator = " << dominator);
    rMessage("PDgap/dominator = " << PDgap/dominator);
    #endif
    if (PDgap/dominator <= param.epsilonStar) {
      value = SolveInfo::pdOPT;
      return false;
    }
  }
  if (value == SolveInfo::noINFO
      && solveInfo.rho > 1.0+NONZERO) {
    rMessage("pdINF criteria");
    value = SolveInfo::pdINF;
    return false;
  }
  if (value == SolveInfo::pFEAS) {
    #if REVERSE_PRIMAL_DUAL
    if (solveInfo.objValPrimal<=-param.upperBound) {
      rMessage("pUNBD criteria");
      value = SolveInfo::pUNBD;
      return false;
    }
    #else
    if (solveInfo.objValPrimal<=param.lowerBound) {
      rMessage("pdINF criteria");
      value = SolveInfo::pUNBD;
      return false;
    }
    #endif
    if (solveInfo.rho > 1.0+NONZERO) {
      rMessage("pFEAS_dINF criteria");
      value = SolveInfo::pFEAS_dINF;
      return false;
    }
  }

  if (value == SolveInfo::dFEAS) {
    #if REVERSE_PRIMAL_DUAL
    if (solveInfo.objValDual>=-param.lowerBound) {
      rMessage("dUNBD criteria");
      value = SolveInfo::dUNBD;
      return false;
    }
    #else
    if (solveInfo.objValDual>=param.upperBound) {
      rMessage("dUNBD criteria");
      value = SolveInfo::dUNBD;
      return false;
    }
    #endif
    if (solveInfo.rho > 1.0+NONZERO) {
      rMessage("pINF_dFEAD criteria");
      value = SolveInfo::pINF_dFEAS;
      return false;
    }
  }
  #if 0
  rMessage("phase =");
  display();
  #endif
  return true;
}

void Phase::reverse()
{
  #if REVERSE_PRIMAL_DUAL
  switch (value) {
  case SolveInfo::noINFO    :                              ; break;
  case SolveInfo::pFEAS     : value = SolveInfo::dFEAS     ; break;
  case SolveInfo::dFEAS     : value = SolveInfo::pFEAS     ; break;
  case SolveInfo::pdFEAS    :                              ; break;
  case SolveInfo::pdINF     :                              ; break;
  case SolveInfo::pFEAS_dINF: value = SolveInfo::pINF_dFEAS; break;
  case SolveInfo::pINF_dFEAS: value = SolveInfo::pFEAS_dINF; break;
  case SolveInfo::pdOPT     :                              ; break;
  case SolveInfo::pUNBD     : value = SolveInfo::dUNBD     ; break;
  case SolveInfo::dUNBD     : value = SolveInfo::pUNBD     ; break;
  default: break;
  }
  #else
  // do nothing
  #endif
}


void Phase::display(FILE* fpout)
{
  if (fpout == NULL) {
    return;
  }
  char* str;
  switch (value) {
  case SolveInfo::noINFO    : str = (char *)"noINFO    "; break;
  case SolveInfo::pFEAS     : str = (char *)"pFEAS     "; break;
  case SolveInfo::dFEAS     : str = (char *)"dFEAS     "; break;
  case SolveInfo::pdFEAS    : str = (char *)"pdFEAS    "; break;
  case SolveInfo::pdINF     : str = (char *)"pdINF     "; break;
  case SolveInfo::pFEAS_dINF: str = (char *)"pFEAS_dINF"; break;
  case SolveInfo::pINF_dFEAS: str = (char *)"pINF_dFEAS"; break;
  case SolveInfo::pdOPT     : str = (char *)"pdOPT     "; break;
  case SolveInfo::pUNBD     : str = (char *)"pUNBD     "; break;
  case SolveInfo::dUNBD     : str = (char *)"dUNBD     "; break;
  default:
    str = (char *)"phase error";
    rMessage("rPhase:: phase error");
    break;
  }
  fprintf(fpout,"phase.value  = %s\n",str);
}

} // end of namespace 'sdpa'