StepLength.java

package org.mklab.sdpj.algorithm;

import java.io.File;
import java.io.FileNotFoundException;
import java.io.FileOutputStream;
import java.io.PrintStream;

import org.mklab.cga.round.FPURoundModeSelector;
import org.mklab.gap.Guarantor;
import org.mklab.gap.eigen.MPFloatRumpMethod;
import org.mklab.mpf.ap.MPFloatMatrix;
import org.mklab.mpf.ap.MPFloatRoundModeSelector;
import org.mklab.nfc.matrix.ComplexNumericalMatrix;
import org.mklab.nfc.matrix.RealNumericalMatrix;
import org.mklab.nfc.scalar.ComplexNumericalScalar;
import org.mklab.nfc.scalar.RealNumericalScalar;
import org.mklab.nfc.util.RoundModeManager;
import org.mklab.sdpj.algebra.Algebra;
import org.mklab.sdpj.datastructure.BlockSparseMatrix;
import org.mklab.sdpj.datastructure.BlockVector;
import org.mklab.sdpj.datastructure.Vector;
import org.mklab.sdpj.tool.Tools;


/**
 * class <code>StepLength</code> compute the step length of moving next point based on interior point method.<br>
 * more detail of implementing code of algorithm, please refer to codes of solver <code>SDPA</code>. About information 
 * of <code>SDPA</code>, please refer to web site:
 *  <a href = "http://sdpa.sourceforge.net/" style=text-decoration:none>
 * <tt>http://sdpa.sourceforge.net/</tt></a>.<br>  
 * 
 * @author koga
 * @version $Revision$, 2009/04/24
 * @param <RS> type of real scalar
 * @param <RM> type of real matrix
 * @param <CS> type of complex scalar
 * @param <CM> type of complex Matrix
 */
public class StepLength<RS extends RealNumericalScalar<RS, RM, CS, CM>, RM extends RealNumericalMatrix<RS, RM, CS, CM>, CS extends ComplexNumericalScalar<RS, RM, CS, CM>, CM extends ComplexNumericalMatrix<RS, RM, CS, CM>> {

  /**unit based on a generic data type */
  private RS unit;
  /**primal solution */
  private RS primal;
  /**dual solution */
  private RS dual;

  /**
   * @return a dual value
   */
  public RS getDual() {
    return this.dual;
  }

  /**
   * @return a primal value
   */
  public RS getPrimal() {
    return this.primal;
  }

  /**
   * 新しく生成された<code>StepLength</code>オブジェクトを初期化します。
   * 
   * @param alphaP initial primal value of class Alpha
   * @param alphaD initial dual value of class Alpha
   * @param nBlock the number of block
   * @param blockStruct array, which saving the size of block
   */
  public StepLength(RS alphaP, RS alphaD, int nBlock, int[] blockStruct) {
    this.unit = alphaP.create(1);

    this.primal = alphaP;
    this.dual = alphaD;

    final int[] workBlockStruct = new int[nBlock];
    for (int i = 0; i < nBlock; ++i) {
      if (blockStruct[i] > 0) {
        workBlockStruct[i] = 3 * blockStruct[i] - 1;
      } else {
        workBlockStruct[i] = -3 * blockStruct[i] - 1;
      }
    }
  }

  /**
   * get a minimum block-vector vector.
   * 
   * @param a block-vector vector
   * @return min minimum block of block-vector vector
   */
  public RS minBlockVector(BlockVector<RS,RM,CS,CM> a) {
    final Vector<RS,RM,CS,CM> vector0 = a.getBlock(0);
    RS ans = vector0.getElement(0);
    final int size0 = vector0.getDimension();
    for (int i = 1; i < size0; ++i) {
      final RS element = vector0.getElement(i);
      if (element.isLessThan(ans)) {
        ans = element;
      }
    }

    final int blockSize = a.getBlockSize();
    for (int i = 1; i < blockSize; ++i) {
      final Vector<RS,RM,CS,CM> vector = a.getBlock(i);
      final int size = vector.getDimension();
      for (int j = 0; j < size; ++j) {
        final RS element = vector.getElement(j);
        if (element.isLessThan(ans)) {
          ans = element;
        }
      }
    }
    return ans;
  }

  /**
   * The method <code>MehrotraCorrector</code> compute the predictor based Mehrotra method solving SDP problem.<br>
   * more detail of algorithm of implementing code, please refer below.<br>
   * Refer to rsdpa_parts.cpp:SDPA 6.2.0 Line No.: 1646 Mehrotraの予測子を計算する関数
   * 
   * @param b vector
   * @param C block-matrices matrix
   * @param phase information of phase
   * @param newton information of newton method
   */
  public void MehrotraPredictor(Vector<RS,RM,CS,CM> b, BlockSparseMatrix<RS,RM,CS,CM> C, Phase<RS,RM,CS,CM> phase, Newton<RS,RM,CS,CM> newton) {
    this.primal = this.unit.create(9).divide(10);
    this.dual = this.unit.create(9).divide(10);

    if (phase.getValue() == PhaseType.noINFO || phase.getValue() == PhaseType.dFEAS) {
      if (this.primal.isGreaterThan(1)) {// primal is infeasible
        this.primal = createUnit();
      }
    } else {// when primal is feasible,check stepP1 is effective or not.
      RS incPrimalObj = Algebra.run(C, '.', newton.getDxMat());
      if (incPrimalObj.isGreaterThan(0)) {
        if (this.primal.isGreaterThan(this.dual)) {
          this.primal = this.dual;
        }
        if (this.primal.isGreaterThan(1)) {
          this.primal = createUnit();
        }
      }
    }
    if (phase.getValue() == PhaseType.noINFO || phase.getValue() == PhaseType.pFEAS) {
      if (this.dual.isGreaterThan(1)) {
        // dual is infeasible
        this.dual = createUnit();
      }
    } else {
      // when dual is feasible,check stepD1 is effective or not.
      RS incDualObj = Algebra.run(b, '.', newton.getDyVec());
      if (incDualObj.isLessThan(0)) {
        if (this.dual.isGreaterThan(this.primal)) {
          this.dual = this.primal;
        }
        if (this.dual.isGreaterThan(1)) {
          this.dual = createUnit();
        }
      }
    }
  }

  /**
   * The method <code>MehrotraCorrector</code> compute the corrector based Mehrotra method solving SDP problem.<br>
   * more detail of algorithm of implementing code, please refer below.<br>
   * rsdpa_parts.cpp:SDPA 6.2.0 Line No.: 1770 Mehrotraの修正子を計算する関数
   * 
   * @param nDim size of block-matrices matrix
   * @param b vector
   * @param C block-matrices matrix <code>C</code>
   * @param currentPt current primal-dual solution
   * @param phase information of phase
   * @param reduction type of switch
   * @param newton information of newton
   * @param mu value of <code>mu</code> := <code>X・Y</code>
   * @param theta value of theta
   * @param lanczos information of lanczos
   * @param param setting parameter of computing SDP problem
   */
  public void MehrotraCorrector(int nDim, Vector<RS,RM,CS,CM> b, BlockSparseMatrix<RS,RM,CS,CM> C, Solution<RS,RM,CS,CM> currentPt, Phase<RS,RM,CS,CM> phase, Switch<RS,RM,CS,CM> reduction, Newton<RS,RM,CS,CM> newton, AverageComplementarity<RS,RM,CS,CM> mu,
      RatioInitResCurrentRes<RS,RM,CS,CM> theta, Lanczos<RS,RM,CS,CM> lanczos, Parameter<RS,RM,CS,CM> param) {
    final boolean originalMethod = true;

    if (originalMethod) {
      RS alphaBD = create(100);

      RS minxInvDxEigenValue = lanczos.getMinEigen(currentPt.invCholeskyX, newton.getDxMat());
      if ((minxInvDxEigenValue.unaryMinus()).isGreaterThan(createUnit().divide(alphaBD))) {
        this.primal = createUnit().divide(minxInvDxEigenValue).unaryMinus();
      } else {
        this.primal = alphaBD;
      }

      RS minzInvDzEigenValue = lanczos.getMinEigen(currentPt.invCholeskyZ, newton.getDzMat());
      if ((minzInvDzEigenValue.unaryMinus()).isGreaterThan(createUnit().divide(alphaBD))) {
        this.dual = createUnit().unaryMinus().divide(minzInvDzEigenValue);
      } else {
        this.dual = alphaBD;
      }

    } else {
      getAlphaOfPrimalBasedOnNFC(currentPt, newton);
      getAlphaOfDualBasedOnNFC(currentPt, newton);
    }

    this.primal = param.gammaStar.multiply(this.primal);// TODO
    this.dual = param.gammaStar.multiply(this.dual);

    if (phase.getValue() == PhaseType.noINFO || phase.getValue() == PhaseType.dFEAS) {
      if (this.primal.isGreaterThan(1)) {
        // primal is infeasible.
        this.primal = createUnit();
      }
    } else {
      RS incPrimalObj = Algebra.run(C, '.', newton.getDxMat());
      if (incPrimalObj.isGreaterThan(0)) {
        // when primal is feasible
        if (this.primal.isGreaterThan(this.dual)) {
          // check stepD1 is effective or not.
          this.primal = this.dual;
        }
        if (this.primal.isGreaterThan(1)) {
          this.primal = createUnit();
        }
      }
    }
    if (phase.getValue() == PhaseType.noINFO || phase.getValue() == PhaseType.pFEAS) {
      if (this.dual.isGreaterThan(1)) {
        // dual is infeasible
        this.dual = createUnit();
      }
    } else {
      // when dual is feasible,check stepD1 is effective or not.
      RS incDualObj = Algebra.run(b, '.', newton.getDyVec());
      if (incDualObj.isLessThan(0)) {
        if (this.dual.isGreaterThan(this.primal)) {
          // change because noneffective
          this.dual = this.primal;
        }
        if (this.dual.isGreaterThan(1)) {
          this.dual = createUnit();
        }
      }
    }

    // attain feasibility before mu reduction
    if (reduction.getSwitchType() == SwitchType.ON && (phase.getValue() == PhaseType.noINFO || phase.getValue() == PhaseType.pFEAS || phase.getValue() == PhaseType.dFEAS)) {
      RS xMatvMat = Algebra.run(currentPt.getXmat(), '.', newton.getDzMat());
      RS uMatzMat = Algebra.run(newton.getDxMat(), '.', currentPt.getZmat());
      RS uMatvMat = Algebra.run(newton.getDxMat(), '.', newton.getDzMat());

      RS value1 = (createUnit().subtract(this.primal)).multiply(theta.getPrimal());
      RS value2 = (createUnit().subtract(this.dual)).multiply(theta.getDual());
      RS thetaMax =value1.max(value2);
      RS muNew = mu.getValue().add(((this.primal.multiply(uMatzMat)).add(this.dual.multiply(xMatvMat)).add(this.primal.multiply(this.dual).multiply(uMatvMat))).divide(nDim));

      RS xi = create(3);
      while ((thetaMax.multiply(thetaMax).multiply(mu.getInitialValue())).isGreaterThan(xi.multiply(muNew))) {
        RS alphaMax =this.primal.max(this.dual).multiply(create(95).divide(100));
        this.primal =this.primal.min(alphaMax);
        this.dual =this.dual.min(alphaMax);
        value1 = (createUnit().subtract(this.primal)).multiply(theta.getPrimal());
        value2 = (createUnit().subtract(this.dual)).multiply(theta.getDual());
        thetaMax =value1.max(value2);
        muNew = mu.getValue().add(((this.primal.multiply(uMatzMat)).add(this.dual.multiply(xMatvMat)).add(this.primal.multiply(this.dual).multiply(uMatvMat))).divide(nDim));
        RS d1eM6 = create(10).power(-6);// if "too short step", then break down the algorithm.
        if (this.primal.isLessThan(d1eM6) && this.dual.isLessThan(d1eM6)) {
          break;
        }
      }
    }

    if (phase.getValue() == PhaseType.pdFEAS) {
      // keep primal objective value >= dual objective value

      RS objValDual = Algebra.run(b, '.', currentPt.getYvec());
      RS objValPrimal = Algebra.run(C, '.', currentPt.getXmat());
      RS incDualObj = Algebra.run(b, '.', newton.getDyVec());
      incDualObj = incDualObj.multiply(this.dual);
      RS incPrimalObj = Algebra.run(C, '.', newton.getDxMat());
      incPrimalObj = incPrimalObj.multiply(this.primal);
      RS maxRatio = (objValDual.subtract(objValPrimal)).divide(incPrimalObj.subtract(incDualObj));
      if ((maxRatio.isGreaterThan(0)) && (maxRatio.isLessThan(1))) {
        this.primal = this.primal.multiply(maxRatio);
        this.dual = this.dual.multiply(maxRatio);
      }
    }

  }

  /**
   * Compute dual alpha based on NFC
   * 
   * @param currentPt information of current point
   * @param newton information of class newton
   */
  private void getAlphaOfDualBasedOnNFC(Solution<RS,RM,CS,CM> currentPt, Newton<RS,RM,CS,CM> newton) {
    final boolean guarantee = true;

    //    RoundModeManager manager = RoundModeManager.getManager();
    //    manager.add(new FPURoundModeSelector());
    //    manager.add(new MPFloatRoundModeSelector());

    RS[][] matrixA = currentPt.getZmat().getArrayOfElements();
    RS[][] matrixdA = newton.getDzMat().getArrayOfElements();

   //NumericalMatrix<RS> A = new BaseNumericalMatrix<>(matrixA);    
    RM A = matrixA[0][0].createGrid(matrixA);
    //NumericalMatrix<RS> dA = new BaseNumericalMatrix<>(matrixdA);
    RM dA = matrixdA[0][0].createGrid(matrixdA);

    double alpha = 0.1;
    for (int i = 1; i <= 10; i++) {
      alpha *= i;
      dA =dA.multiply(alpha);
      A =A.add(dA);

      RS minimumEigenValue;
      if (guarantee) {
        MPFloatRumpMethod verifier = new MPFloatRumpMethod((MPFloatMatrix)A.multiply(alpha));
        Guarantor<MPFloatRumpMethod> guarantor = new Guarantor<>(verifier);
        guarantor.solveWithEffectiveDigits(26);
        MPFloatRumpMethod guranteedVerifier = guarantor.getGuranteedVerifier();
        minimumEigenValue  = (RS)guranteedVerifier.getEigenValue().getInfimum().getRealPart().min();
      } else {
        CM eigenValue = A.eigenValue();
        minimumEigenValue =eigenValue.getRealPart().min();
      }
      //   System.err.println("min = " + minimumEigenValue);
      if (minimumEigenValue.isLessThan(0)) {
        break;
      }
      this.dual = this.unit.create(alpha);
      alpha = 0.1;
    }
  }

  /**
   * Compute primal alpha based on NFC.
   * 
   * @param currentPt information of current point
   * @param newton information of class newton
   */
  private void getAlphaOfPrimalBasedOnNFC(Solution<RS,RM,CS,CM> currentPt, Newton<RS,RM,CS,CM> newton) {
    final boolean guarantee = true;

    RoundModeManager manager = RoundModeManager.getManager();
    manager.add(new FPURoundModeSelector());
    manager.add(new MPFloatRoundModeSelector());

    RS[][] matrixA = currentPt.getXmat().getArrayOfElements();
    RS[][] matrixdA = newton.getDxMat().getArrayOfElements();

    //NumericalMatrix<RS> A = new BaseNumericalMatrix<>(matrixA);
    RM A = matrixA[0][0].createGrid(matrixA);
    //NumericalMatrix<RS> dA = new BaseNumericalMatrix<>(matrixdA);
    RM dA = matrixdA[0][0].createGrid(matrixdA);

    double alpha = 0.1;
    for (int i = 1; i <= 10; i++) {
      alpha *= i;
      dA =dA.multiply(alpha);
      A =A.add(dA);

      RS minimumEigenValue;
      if (guarantee) {
        MPFloatRumpMethod verifier = new MPFloatRumpMethod((MPFloatMatrix)A.multiply(alpha));
        Guarantor<MPFloatRumpMethod> guarantor = new Guarantor<>(verifier);
        guarantor.solveWithEffectiveDigits(26);
        MPFloatRumpMethod guranteedVerifier = guarantor.getGuranteedVerifier();
        minimumEigenValue = (RS)guranteedVerifier.getEigenValue().getInfimum().getRealPart().min();
      } else {
        CM eigenValue = (A).eigenValue();
        minimumEigenValue =eigenValue.getRealPart().min();
      }
      if (minimumEigenValue.isLessThan(0)) {
        break;
      }
      this.primal = this.unit.create(alpha);
      alpha = 0.1;
    }
  }

  /**
   * show a message
   */
  public void display() {
    Tools.message(this.toString());
  }

  /**
   * @param fpout ファイル
   */
  public void display(File fpout) {
    if (fpout == null) {
      return;
    }

    try (PrintStream ps = new PrintStream(new FileOutputStream(fpout))) {
      Tools.message(this.toString(), ps);
      ps.close();
    } catch (FileNotFoundException e) {
      throw new RuntimeException(e);
    }

  }

  /**
   * @see java.lang.Object#toString()
   */
  @Override
  public String toString() {
    return "alpha.primal\t= " + this.primal.toString() + "\nalpha.dual\t= " + this.dual.toString() + "\n"; //$NON-NLS-1$ //$NON-NLS-2$ //$NON-NLS-3$
  }

  /**
   * 単位数を返します。
   * 
   * @return 単位数
   */
  private RS createUnit() {
    return this.unit.createUnit();
  }

  /**
   * int型に対応する値を返します。
   * 
   * @param value int型の値
   * @return E型の値
   */
  private RS create(int value) {
    return this.unit.create(value);
  }
}