ADMMQPModifiedRuizEquilibrium

/*
 * Licensed to the Hipparchus project under one or more
 * contributor license agreements.  See the NOTICE file distributed with
 * this work for additional information regarding copyright ownership.
 * The Hipparchus project licenses this file to You under the Apache License, Version 2.0
 * (the "License"); you may not use this file except in compliance with
 * the License.  You may obtain a copy of the License at
 *
 *      https://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
package org.hipparchus.optim.nonlinear.vector.constrained;

import org.hipparchus.linear.ArrayRealVector;
import org.hipparchus.linear.MatrixUtils;
import org.hipparchus.linear.RealMatrix;
import org.hipparchus.linear.RealVector;
import org.hipparchus.util.FastMath;

/** TBD.
 * @since 3.1
 */
public class ADMMQPModifiedRuizEquilibrium {

    /** Minimum scaling value. */
    private static final double MIN_SCALING = 1.0e-6;

    /** Maximum scaling value. */
    private static final double MAX_SCALING = 1.0e+6;

    /** Square matrix of weights for quadratic terms. */
    private final RealMatrix H;

    /** Vector of weights for linear terms. */
    private final RealVector q;

    /** Constraints coefficients matrix. */
    private final RealMatrix A;

    /** TBC. */
    private RealMatrix D;

    /** TBC. */
    private RealMatrix E;

    /** TBC. */
    private double c;

    /** Inverse of D. */
    private RealMatrix Dinv;

    /** Inverse of E. */
    private RealMatrix Einv;

    /** Inverse of c. */
    private double cinv;

    /** Simple constructor
     * @param H square matrix of weights for quadratic terms
     * @param A constraints coefficients matrix
     * @param q vector of weights for linear terms
     */
    public ADMMQPModifiedRuizEquilibrium(RealMatrix H, RealMatrix A, RealVector q) {
        this.H  = H;
        this.A  = A;
        this.q  = q;
    }

    /** Normalize matrices.
     * @param epsilon TBD
     * @param maxIteration TBD
     */
    public void normalize(double epsilon, int maxIteration) {

        int iteration = 0;
        this.c = 1.0;
        RealVector gammaD = new ArrayRealVector(H.getRowDimension());
        RealVector gammaE = new ArrayRealVector(A.getRowDimension());

        double lambda;
        RealVector q1 = q.copy();
        RealMatrix H1 = H.copy();
        RealMatrix A1 = A.copy();
        RealMatrix diagD;
        RealMatrix diagE;
        RealMatrix SD = MatrixUtils.createRealIdentityMatrix(H.getRowDimension());
        RealMatrix SE = MatrixUtils.createRealIdentityMatrix(A.getRowDimension());
        RealVector H1norm = new ArrayRealVector(H1.getColumnDimension());
        while (iteration < maxIteration) {
       // while (1.0 - gamma.getLInfNorm() > epsilon && iteration < maxIteration) {

            for (int i = 0; i < H1.getColumnDimension(); i++) {
                double norma = (new ArrayRealVector(H1.getColumn(i), A1.getColumn(i))).getLInfNorm();
                gammaD.setEntry(i, norma);
            }

            for (int i = 0; i < A1.getRowDimension(); i++) {
                double norma = A1.getRowVector(i).getLInfNorm();
                gammaE.setEntry(i, norma);
            }

            gammaD = limitScaling(gammaD);
            gammaE = limitScaling(gammaE);

            for (int i = 0; i < gammaD.getDimension(); i++) {
                gammaD.setEntry(i, 1.0 / FastMath.sqrt(gammaD.getEntry(i)));
            }

            for (int i = 0; i < gammaE.getDimension(); i++) {
                gammaE.setEntry(i, 1.0 / FastMath.sqrt(gammaE.getEntry(i)));
            }

            diagD = MatrixUtils.createRealDiagonalMatrix(gammaD.toArray());
            diagE = MatrixUtils.createRealDiagonalMatrix(gammaE.toArray());

            H1 = diagD.multiply(H1.copy()).multiply(diagD.copy());
            q1 = diagD.operate(q1.copy());
            A1 = diagE.multiply(A1.copy()).multiply(diagD.copy());

            for (int i = 0; i < H1.getRowDimension(); i++) {
                double norma = (new ArrayRealVector(H1.getRow(i))).getLInfNorm();
                H1norm.setEntry(i, norma);
            }
            double qnorm = q1.getLInfNorm();
            if (qnorm == 0) {
                qnorm = 1.0;
            }
            qnorm = limitScaling(new ArrayRealVector(1, qnorm)).getEntry(0);
            double mean = 0;
            for (int i = 0; i < H1norm.getDimension(); i++) {
                mean+=H1norm.getEntry(i) / H1norm.getDimension();
            }

            lambda = 1.0 / limitScaling(new ArrayRealVector(1, FastMath.max(mean, qnorm))).getEntry(0);

            H1 = H1.copy().scalarMultiply(lambda);
            q1 = q1.copy().mapMultiply(lambda);
            c *= lambda;
            SD = diagD.multiply(SD.copy());
            SE = diagE.multiply(SE.copy());
            iteration += 1;
        }
        this.E    = SE.copy();
        this.D    = SD.copy();
        this.Einv = MatrixUtils.inverse(SE);
        this.Dinv = MatrixUtils.inverse(SD);
        this.cinv = 1.0 / c;

    }

    /** Get scaled square matrix of weights for quadratic terms.
     * @return scaled square matrix of weights for quadratic terms
     */
    public RealMatrix getScaledH() {
        return D.multiply(H).multiply(D).scalarMultiply(c);
    }

    /** Get scaled constraints coefficients matrix.
     * @return scaled constraints coefficients matrix
     */
    public RealMatrix getScaledA() {
        return E.multiply(A).multiply(D);
    }

    /** Get scaled vector of weights for linear terms.
     * @return scaled vector of weights for linear terms
     */
    public RealVector getScaledQ() {
        return D.operate(q.mapMultiply(c));
    }

    /** Get scaled upper bound
     * @param lb1 unscaled lower bound
     * @return scaled lower bound
     */
    public RealVector getScaledLUb(RealVector lb1) {
        RealVector scaledLUb = new ArrayRealVector(lb1.getDimension());
        for (int i = 0; i < lb1.getDimension(); i++) {
            if (lb1.getEntry(i) != Double.POSITIVE_INFINITY) {
                scaledLUb.setEntry(i, E.getEntry(i, i) * lb1.getEntry(i));
            } else {
                scaledLUb.setEntry(i, Double.POSITIVE_INFINITY);
            }
        }
        return scaledLUb;
    }

    /** Unscale solution vector.
     * @param x scaled solution vector
     * @return unscaled solution vector
     */
    public RealVector unscaleX(RealVector x) {
        return D.operate(x);
    }

    /** Unscale Y vector.
     * @param y scaled Y vector
     * @return unscaled Y vector
     */
    public RealVector unscaleY(RealVector y) {
        return E.operate(y).mapMultiply(cinv);
    }

    /** Unscale Z vector.
     * @param z scaled Z vector
     * @return unscaled Z vector
     */
    public RealVector unscaleZ(RealVector z) {
        return Einv.operate(z);
    }

    /** Scale solution vector.
     * @param x unscaled solution vector
     * @return scaled solution vector
     */
    RealVector scaleX(RealVector x) {
        return Dinv.operate(x);
    }

    /** Limit scaling.
     * @param v vector to limit
     * @return rescaled vector, taking scaling limits into account
     */
    private RealVector limitScaling(RealVector v) {

        RealVector result = new ArrayRealVector(v.getDimension());
        for (int i = 0; i < v.getDimension(); i++) {
            result.setEntry(i,  v.getEntry(i) < MIN_SCALING ? 1.0 : v.getEntry(i));
            result.setEntry(i,  v.getEntry(i) > MAX_SCALING ? MAX_SCALING : v.getEntry(i));
        }

        return result;

    }

}