/** rPri := Ax - b */
 @Override
 protected DoubleMatrix1D rPri(DoubleMatrix1D X) {
   if (getMeq() == 0) {
     return F1.make(0);
   }
   return ColtUtils.zMult(getA(), X, getB(), -1);
 }
 /** @see "Convex Optimization, p. 610" */
 protected DoubleMatrix1D rDual(DoubleMatrix1D gradF0X, DoubleMatrix1D L, DoubleMatrix1D V) {
   // m1 = GradFiX[T].L + gradF0X
   DoubleMatrix1D m1 = F1.make(getDim());
   for (int i = 0; i < getDim(); i++) {
     double m = 0;
     m += -L.getQuick(i);
     m += L.getQuick(getDim() + i);
     m1.setQuick(i, m + gradF0X.get(i));
   }
   if (getMeq() == 0) {
     return m1;
   }
   return ColtUtils.zMultTranspose(getA(), V, m1, 1.);
 }
  /**
   * Solves a presolved standard form LP problem in the form of min(c) s.t. A.x = b lb <= x <= ub
   */
  protected int optimizePresolvedStandardLP() throws Exception {
    log.info("optimizePresolvedStandardLP");

    long tStart = System.currentTimeMillis();

    LPOptimizationRequest lpRequest = getLPOptimizationRequest();
    if (log.isDebugEnabled() && lpRequest.isDumpProblem()) {
      log.debug("LP problem: " + lpRequest.toString());
    }

    if (this.dim <= -1) {
      if (getLb().size() != getUb().size()) {
        log.error("Lower and upper bounds must have the same dimension");
        throw new IllegalArgumentException("Lower and upper bounds must have the same dimension");
      }
      this.dim = getLb().size();
      double minDeltaBoundsValue = Double.MAX_VALUE;
      int minDeltaBoundsIndex = -1;
      for (int i = 0; i < getDim(); i++) {
        double deltai = getUb().getQuick(i) - getLb().getQuick(i);
        if (deltai < minDeltaBoundsValue) {
          minDeltaBoundsValue = deltai;
          minDeltaBoundsIndex = i;
        }
      }
      log.info("min delta bounds index: " + minDeltaBoundsIndex);
      log.info("min delta bounds value: " + minDeltaBoundsValue);
    }
    // this.boundedLb = new boolean[getDim()];
    // this.boundedUb = new boolean[getDim()];
    //		for(int i=0; i<getDim(); i++){
    //			if(!isLbUnbounded(getLb().getQuick(i))){
    //				boundedLb[i] = true;
    //				nOfBoundedLb++;
    //			}
    //			if(!isUbUnbounded(getUb().getQuick(i))){
    //				boundedUb[i] = true;
    //				nOfBoundedUb++;
    //			}
    //		}
    this.meq = (this.meq > -1) ? this.meq : ((getA() != null) ? getA().rows() : 0);
    // this.mieq = (this.mieq>-1)? this.mieq : (nOfBoundedLb+nOfBoundedUb);
    this.mieq = (this.mieq > -1) ? this.mieq : (2 * getDim());

    if (log.isDebugEnabled()) {
      log.debug("dim : " + getDim());
      log.debug("meq : " + getMeq());
      log.debug("mieq: " + getMieq());
    }

    LPOptimizationResponse lpResponse = new LPOptimizationResponse();

    DoubleMatrix1D X0 = getInitialPoint();
    if (X0 == null) {
      DoubleMatrix1D X0NF = getNotFeasibleInitialPoint();
      if (X0NF != null) {
        double rPriX0NFNorm = Math.sqrt(ALG.norm2(rPri(X0NF)));
        DoubleMatrix1D fiX0NF = getFi(X0NF);
        int maxIndex = Utils.getMaxIndex(fiX0NF);
        double maxValue = fiX0NF.get(maxIndex);
        if (log.isDebugEnabled()) {
          log.debug("rPriX0NFNorm :  " + rPriX0NFNorm);
          log.debug("X0NF         :  " + ArrayUtils.toString(X0NF.toArray()));
          log.debug("fiX0NF       :  " + ArrayUtils.toString(fiX0NF.toArray()));
        }
        if (maxValue < 0 && rPriX0NFNorm <= getToleranceFeas()) {
          // the provided not-feasible starting point is already feasible
          log.debug("the provided initial point is already feasible");
          X0 = X0NF;
        }
      }
      if (X0 == null) {
        BasicPhaseILPPDM bf1 = new BasicPhaseILPPDM(this);
        X0 = bf1.findFeasibleInitialPoint();
      }
    }

    // check X0 feasibility
    DoubleMatrix1D fiX0 = getFi(X0);
    int maxIndex = Utils.getMaxIndex(fiX0);
    double maxValue = fiX0.get(maxIndex);
    double rPriX0Norm = Math.sqrt(ALG.norm2(rPri(X0)));
    if (maxValue >= 0. || rPriX0Norm > getToleranceFeas()) { // must be fi STRICTLY < 0
      log.warn("rPriX0Norm  : " + rPriX0Norm);
      log.warn("ineqX0      : " + ArrayUtils.toString(fiX0.toArray()));
      log.warn("max ineq index: " + maxIndex);
      log.warn("max ineq value: " + maxValue);
      // the point must be INTERNAL, fi are used as denominators
      throw new Exception("initial point must be strictly feasible");
    }

    DoubleMatrix1D V0 = F1.make(getMeq());
    if (getYlb() != null && getYub() != null) {
      // NB: the Lagrangian multipliers for eq. constraints used in this interior point method (v)
      // are the opposite of the Lagrangian  multipliers for eq. constraints used in the presolver
      // (y)
      // and so Ylb<=y<=Yub becomes -Yub<=v<=-Ylb
      for (int i = 0; i < getMeq(); i++) {
        double v0i = 0;
        if (!isLbUnbounded(getYlb().getQuick(i))) {
          if (!isUbUnbounded(getYub().getQuick(i))) {
            v0i = -(getYub().getQuick(i) + getYlb().getQuick(i)) / 2;
          } else {
            v0i = -getYlb().getQuick(i);
          }
        } else {
          if (!isUbUnbounded(getYub().getQuick(i))) {
            v0i = -getYub().getQuick(i);
          } else {
            v0i = 0;
          }
        }
        V0.setQuick(i, v0i);
      }
    }

    DoubleMatrix1D L0 = getInitialLagrangian();
    if (L0 != null) {
      for (int j = 0; j < L0.size(); j++) {
        // must be >0
        if (L0.get(j) <= 0) {
          throw new IllegalArgumentException("initial lagrangian must be strictly > 0");
        }
      }
    } else {
      L0 = F1.make(getMieq(), 1.); // must be >0 strictly
      if (getZlb() != null && getZub() != null) {
        // Zlb<= L <=Zub, meaning that:
        // zlb[i] and zub[i] are the bounds on the Lagrangian of the constraint associated with
        // lb[i]<x[i]<ub[i]
        // note that zlb.size = zub.size = lb.size = ub.size (and = n of variables of the problem (=
        // getDim())
        // and that L.size = nOfBoundedLb + nOfBoundedUb (and in general < 2*getDim())
        int cntLB = 0;
        int cntUB = 0;
        for (int i = 0; i < getDim(); i++) {
          double zlbi =
              (isLbUnbounded(getZlb().getQuick(i))) ? 0 : getZlb().getQuick(i); // L must be > 0
          double zubi = (isUbUnbounded(getZub().getQuick(i))) ? 1 : getZub().getQuick(i);
          L0.setQuick(cntLB, (zubi - zlbi) / 2);
          cntLB++;
          L0.setQuick(getDim() + cntUB, (zubi - zlbi) / 2);
          cntUB++;
        }
      } else {
        // inequalities comes from the pairs lower bounds-upper bounds
        // in the calculation of the H matrix fro the KKT system, each pairs gives terms of the
        // form:
        // t = tl + tu
        // tl = -L[i] / fi[i] for the lower bound
        // tu =  L[dim+i] / fi[dim+i] for the upper bound
        // we want t = 1, and hence
        // L[i] > -cc * fi[i]
        // L[dim+i] = (1 + L[i] / fi[i]) * fi[dim+i]
        //				double cc = 10;
        //				int nOfLB = getMieq()/2;
        //				for (int i = 0; i < nOfLB; i++) {
        //					L0.setQuick(i, -cc * fiX0.getQuick(i));
        //					L0.setQuick(nOfLB + i, (1 - 10)	* fiX0.getQuick(nOfLB + i));
        //					double sum = -L0.getQuick(i)/fiX0.getQuick(i)+L0.getQuick(nOfLB +
        // i)/fiX0.getQuick(nOfLB + i);
        //					log.debug("sum["+i+"]:  " + sum);
        //				}
      }
    }
    if (log.isDebugEnabled()) {
      log.debug("X0:  " + ArrayUtils.toString(X0.toArray()));
      log.debug("V0:  " + ArrayUtils.toString(V0.toArray()));
      log.debug("L0:  " + ArrayUtils.toString(L0.toArray()));
    }
    if (log.isInfoEnabled()) {
      log.info("toleranceFeas:  " + getToleranceFeas());
      log.info("tolerance    :  " + getTolerance());
    }

    DoubleMatrix1D X = X0;
    DoubleMatrix1D V = V0;
    DoubleMatrix1D L = L0;
    double previousF0X = Double.NaN;
    double previousRPriXNorm = Double.NaN;
    double previousRDualXLVNorm = Double.NaN;
    double previousSurrDG = Double.NaN;
    double t;
    int iteration = 0;
    // List<DoubleMatrix1D> XList = new ArrayList<DoubleMatrix1D>();
    // List<double[]> SList = new ArrayList<double[]>();
    while (true) {

      iteration++;
      // iteration limit condition
      if (iteration == getMaxIteration() + 1) {
        lpResponse.setReturnCode(OptimizationResponse.FAILED);
        log.error("Max iterations limit reached");
        throw new Exception("Max iterations limit reached");
      }

      // XList.add(XList.size(), X);
      double F0X = getF0(X);
      if (log.isInfoEnabled()) {
        log.info("iteration: " + iteration);
        log.info("f0(X)=" + F0X);
      }
      if (log.isDebugEnabled()) {
        log.debug("X=" + ArrayUtils.toString(X.toArray()));
        log.debug("L=" + ArrayUtils.toString(L.toArray()));
        log.debug("V=" + ArrayUtils.toString(V.toArray()));
      }

      // determine functions evaluations
      DoubleMatrix1D gradF0X = getGradF0(X);
      DoubleMatrix1D fiX = getFi(X);
      log.debug("fiX=" + ArrayUtils.toString(fiX.toArray()));
      // DoubleMatrix2D GradFiX = getGradFi(X);
      // DoubleMatrix2D GradFiXOLD = getGradFiOLD(X);

      // determine t
      double surrDG = getSurrogateDualityGap(fiX, L);
      t = getMu() * getMieq() / surrDG;
      log.debug("t:  " + t);

      // determine residuals
      DoubleMatrix1D rPriX = rPri(X);
      DoubleMatrix1D rCentXLt = rCent(fiX, L, t);
      DoubleMatrix1D rDualXLV = rDual(gradF0X, L, V);
      // DoubleMatrix1D rDualXLVOLD = rDualOLD(GradFiXOLD, gradF0X, L, V);
      // log.debug("delta: " + ALG.normInfinity(rDualXLVOLD.assign(rDualXLV, Functions.minus)));
      double rPriXNorm = Math.sqrt(ALG.norm2(rPriX));
      double rCentXLtNorm = Math.sqrt(ALG.norm2(rCentXLt));
      double rDualXLVNorm = Math.sqrt(ALG.norm2(rDualXLV));
      double normRXLVt =
          Math.sqrt(Math.pow(rPriXNorm, 2) + Math.pow(rCentXLtNorm, 2) + Math.pow(rDualXLVNorm, 2));
      // @TODO: set log.debug not log.info
      log.info("rPri  norm: " + rPriXNorm);
      log.info("rCent norm: " + rCentXLtNorm);
      log.info("rDual norm: " + rDualXLVNorm);
      log.info("surrDG    : " + surrDG);

      // custom exit condition
      if (checkCustomExitConditions(X)) {
        lpResponse.setReturnCode(OptimizationResponse.SUCCESS);
        break;
      }

      // exit condition
      if (rPriXNorm <= getToleranceFeas()
          && rDualXLVNorm <= getToleranceFeas()
          && surrDG <= getTolerance()) {
        lpResponse.setReturnCode(OptimizationResponse.SUCCESS);
        break;
      }

      // progress conditions
      if (isCheckProgressConditions()) {
        if (!Double.isNaN(previousRPriXNorm)
            && !Double.isNaN(previousRDualXLVNorm)
            && !Double.isNaN(previousSurrDG)) {
          if ((previousRPriXNorm <= rPriXNorm && rPriXNorm >= getToleranceFeas())
              || (previousRDualXLVNorm <= rDualXLVNorm && rDualXLVNorm >= getToleranceFeas())) {
            log.error("No progress achieved, exit iterations loop without desired accuracy");
            lpResponse.setReturnCode(OptimizationResponse.FAILED);
            throw new Exception(
                "No progress achieved, exit iterations loop without desired accuracy");
          }
        }
        previousRPriXNorm = rPriXNorm;
        previousRDualXLVNorm = rDualXLVNorm;
        previousSurrDG = surrDG;
      }

      // compute primal-dual search direction
      // a) prepare 11.55 system
      DoubleMatrix2D Hpd = GradLSum(L, fiX);
      // DoubleMatrix2D HpdOLD = GradLSumOLD(GradFiXOLD, L, fiX);
      // log.debug("delta: " + ALG.normInfinity(HpdOLD.assign(Hpd, Functions.minus)));
      DoubleMatrix1D gradSum = gradSum(t, fiX);
      DoubleMatrix1D g = null;
      // if(getAT()==null){
      if (getA() == null) {
        g = ColtUtils.add(gradF0X, gradSum);
      } else {
        // g = ColtUtils.add(ColtUtils.add(gradF0X, gradSum), ALG.mult(getAT(), V));
        g =
            ColtUtils.add(
                ColtUtils.add(gradF0X, gradSum),
                ColtUtils.zMultTranspose(getA(), V, F1.make(getDim()), 0));
      }

      // b) solving 11.55 system
      if (this.kktSolver == null) {
        this.kktSolver = new UpperDiagonalHKKTSolver(getDim(), lpRequest.isRescalingDisabled());
        // this.kktSolver = new DiagonalHKKTSolver(getDim(), lpRequest.isRescalingDisabled());
      }
      if (isCheckKKTSolutionAccuracy()) {
        kktSolver.setCheckKKTSolutionAccuracy(true);
        kktSolver.setToleranceKKT(getToleranceKKT());
      }
      kktSolver.setHMatrix(Hpd);
      kktSolver.setGVector(g);
      if (getA() != null) {
        kktSolver.setAMatrix(getA());
        // kktSolver.setATMatrix(getAT());
        kktSolver.setHVector(rPriX);
        //				if(rPriXNorm > getToleranceFeas()){
        //					kktSolver.setHVector(rPriX);
        //				}
      }
      DoubleMatrix1D[] sol = kktSolver.solve();
      DoubleMatrix1D stepX = sol[0];
      // double[] signa = new double[stepX.size()];
      //			for(int p=0; p<stepX.size(); p++){
      //				signa[p] =	Math.signum(stepX.getQuick(p));
      //			}
      // SList.add(SList.size(), signa);
      DoubleMatrix1D stepV = (sol[1] != null) ? sol[1] : F1.make(0);
      if (log.isDebugEnabled()) {
        log.debug("stepX: " + ArrayUtils.toString(stepX.toArray()));
        log.debug("stepV: " + ArrayUtils.toString(stepV.toArray()));
      }

      // c) solving for L
      DoubleMatrix1D stepL = F1.make(getMieq());
      DoubleMatrix1D gradFiStepX = gradFiStepX(stepX);
      for (int i = 0; i < getMieq(); i++) {
        stepL.setQuick(
            i, (-L.getQuick(i) * gradFiStepX.getQuick(i) + rCentXLt.getQuick(i)) / fiX.getQuick(i));
      }
      if (log.isDebugEnabled()) {
        log.debug("stepL: " + ArrayUtils.toString(stepL.toArray()));
      }

      // line search and update
      // a) sMax computation
      double sMax = Double.MAX_VALUE;
      for (int j = 0; j < getMieq(); j++) {
        if (stepL.get(j) < 0) {
          sMax = Math.min(-L.get(j) / stepL.get(j), sMax);
        }
      }
      sMax = Math.min(1, sMax);
      double s = 0.99 * sMax;
      // b) backtracking with f
      DoubleMatrix1D X1 = F1.make(X.size());
      DoubleMatrix1D L1 = F1.make(L.size());
      DoubleMatrix1D V1 = F1.make(V.size());
      DoubleMatrix1D fiX1 = null;
      DoubleMatrix1D gradF0X1 = null;
      // DoubleMatrix2D GradFiX1 = null;
      // DoubleMatrix2D GradFiX1 = null;
      DoubleMatrix1D rPriX1 = null;
      DoubleMatrix1D rCentX1L1t = null;
      DoubleMatrix1D rDualX1L1V1 = null;
      int cnt = 0;
      boolean areAllNegative = true;
      while (cnt < 500) {
        cnt++;
        // X1 = X + s*stepX
        X1 = stepX.copy().assign(Mult.mult(s)).assign(X, Functions.plus);
        DoubleMatrix1D ineqValueX1 = getFi(X1);
        areAllNegative = true;
        for (int j = 0; areAllNegative && j < getMieq(); j++) {
          areAllNegative = (Double.compare(ineqValueX1.get(j), 0.) < 0);
        }
        if (areAllNegative) {
          break;
        }
        s = getBeta() * s;
      }

      if (!areAllNegative) {
        // exited from the feasible region
        throw new Exception("Optimization failed: impossible to remain within the faesible region");
      }

      log.debug("s: " + s);
      // c) backtracking with norm
      double previousNormRX1L1V1t = Double.NaN;
      cnt = 0;
      while (cnt < 500) {
        cnt++;
        X1 = ColtUtils.add(X, stepX, s);
        L1 = ColtUtils.add(L, stepL, s);
        V1 = ColtUtils.add(V, stepV, s);

        if (isInDomainF0(X1)) {
          fiX1 = getFi(X1);
          gradF0X1 = getGradF0(X1);
          // GradFiX1 = getGradFi(X1);

          rPriX1 = rPri(X1);
          rCentX1L1t = rCent(fiX1, L1, t);
          rDualX1L1V1 = rDual(gradF0X1, L1, V1);
          double normRX1L1V1t =
              Math.sqrt(ALG.norm2(rPriX1) + ALG.norm2(rCentX1L1t) + ALG.norm2(rDualX1L1V1));
          // log.debug("normRX1L1V1t: "+normRX1L1V1t);
          if (normRX1L1V1t <= (1 - getAlpha() * s) * normRXLVt) {
            break;
          }

          if (!Double.isNaN(previousNormRX1L1V1t)) {
            if (previousNormRX1L1V1t <= normRX1L1V1t) {
              log.warn("No progress achieved in backtracking with norm");
              break;
            }
          }
          previousNormRX1L1V1t = normRX1L1V1t;
        }

        s = getBeta() * s;
        // log.debug("s: " + s);
      }

      // update
      X = X1;
      V = V1;
      L = L1;
    }

    //		if(lpRequest.isCheckOptimalDualityConditions()){
    //			//check duality conditions:
    //			if(!checkDualityConditions(X, L, V)){
    //				log.error("duality conditions not satisfied");
    //				lpResponse.setReturnCode(OptimizationResponse.FAILED);
    //				throw new Exception("duality conditions not satisfied");
    //			}
    //		}

    if (lpRequest.isCheckOptimalLagrangianBounds()) {
      // check equality constraints Lagrangian bounds
      //			if(!checkEqConstraintsLagrangianBounds(V)){
      //				log.error("equality constraints Lagrangian multipliers bounds not satisfied");
      //				lpResponse.setReturnCode(OptimizationResponse.FAILED);
      //				throw new Exception("equality constraints Lagrangian multipliers bounds not satisfied");
      //			}

      // check inequality constraints Lagrangian bounds
      //			if(!checkIneqConstraintsLagrangianBounds(X, L)){
      //				log.error("inequality constraints Lagrangian multipliers bounds not satisfied");
      //				lpResponse.setReturnCode(OptimizationResponse.FAILED);
      //				throw new Exception("inequality constraints Lagrangian multipliers bounds not
      // satisfied");
      //			}
    }

    long tStop = System.currentTimeMillis();
    log.debug("time: " + (tStop - tStart));
    log.debug("sol : " + ArrayUtils.toString(X.toArray()));
    log.debug("ret code: " + lpResponse.getReturnCode());
    //		log.debug("XList : " + ArrayUtils.toString(XList));
    //		for(int s=0; s<SList.size(); s++){
    //			log.debug("SList : " + ArrayUtils.toString(SList.get(s)));
    //		}
    lpResponse.setSolution(X.toArray());
    setLPOptimizationResponse(lpResponse);
    return lpResponse.getReturnCode();
  }
  /** Solves an LP in the form of: min(c) s.t. A.x = b G.x < h lb <= x <= ub */
  @Override
  public int optimize() throws Exception {
    log.info("optimize");

    LPOptimizationRequest lpRequest = getLPOptimizationRequest();
    if (log.isDebugEnabled() && lpRequest.isDumpProblem()) {
      log.debug("LP problem: " + lpRequest.toString());
    }

    // standard form conversion
    LPStandardConverter lpConverter =
        new LPStandardConverter(); // the slack variables will have default unboundedUBValue

    lpConverter.toStandardForm(getC(), getG(), getH(), getA(), getB(), getLb(), getUb());
    int nOfSlackVariables = lpConverter.getStandardS();
    log.debug("nOfSlackVariables: " + nOfSlackVariables);
    DoubleMatrix1D standardC = lpConverter.getStandardC();
    DoubleMatrix2D standardA = lpConverter.getStandardA();
    DoubleMatrix1D standardB = lpConverter.getStandardB();
    DoubleMatrix1D standardLb = lpConverter.getStandardLB();
    DoubleMatrix1D standardUb = lpConverter.getStandardUB();

    // solve the standard form problem
    LPOptimizationRequest standardLPRequest = lpRequest.cloneMe();
    standardLPRequest.setC(standardC);
    standardLPRequest.setA(standardA);
    standardLPRequest.setB(standardB);
    standardLPRequest.setLb(
        ColtUtils.replaceValues(
            standardLb,
            lpConverter.getUnboundedLBValue(),
            minLBValue)); // substitute not-double numbers
    standardLPRequest.setUb(
        ColtUtils.replaceValues(
            standardUb,
            lpConverter.getUnboundedUBValue(),
            maxUBValue)); // substitute not-double numbers
    if (getInitialPoint() != null) {
      standardLPRequest.setInitialPoint(
          lpConverter.getStandardComponents(getInitialPoint().toArray()));
    }
    if (getNotFeasibleInitialPoint() != null) {
      standardLPRequest.setNotFeasibleInitialPoint(
          lpConverter.getStandardComponents(getNotFeasibleInitialPoint().toArray()));
    }

    // optimization
    LPPrimalDualMethod opt = new LPPrimalDualMethod(minLBValue, maxUBValue);
    opt.setLPOptimizationRequest(standardLPRequest);
    if (opt.optimizeStandardLP(nOfSlackVariables) == OptimizationResponse.FAILED) {
      return OptimizationResponse.FAILED;
    }

    // back to original form
    LPOptimizationResponse lpResponse = opt.getLPOptimizationResponse();
    double[] standardSolution = lpResponse.getSolution();
    double[] originalSol = lpConverter.postConvert(standardSolution);
    lpResponse.setSolution(originalSol);
    setLPOptimizationResponse(lpResponse);
    return lpResponse.getReturnCode();
  }