# Copyright 2010-2026 M. S. Andersen & L. Vandenberghe
#
# This file is part of SMCP.
#
# SMCP 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 3 of the License, or
# (at your option) any later version.
#
# SMCP 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 SMCP.  If not, see <http://www.gnu.org/licenses/>.

from copy import copy as __ccopy


# DEFAULT OPTIONS
options = {
    "debug": False,
    "maxiters": 100,
    "abstol": 1e-6,
    "reltol": 1e-6,
    "feastol": 1e-8,
    "refinement": 2,
    "cholmod": False,
    "order": "AMD",
    "tnzcols": 0.1,
    "show_progress": True,
    "dimacs": True,
    "eta": None,
    "delta": 0.9,
    "alpha": 1e-1,
    "beta": 0.7,
    "minstep": 1e-8,
    "lifting": True,
    "t0": 1e-1,
    "equalsteps": True,
    "prediction": True,
    "step": 0.98,
}

__options = __ccopy(options)


def chordalsolver_feas(A, b, primalstart=None, dualstart=None,
                       scaling="primal", kktsolver="chol", p=None):
    """
    Chordal SDP solver:

    sol = chordalsolver_feas(A,b[,primalstart[,dualstart[,scaling[,kktsolver]]]])

    PURPOSE
    Solves a pair of primal and dual cone programs

         minimize   c'*x        maximize   b'*y
         subject to A*x = b     subject to A'*y + s = c
                    x in C                 s in K.

    Here C is the cone of symmetric matrices with sparsity
    pattern V that have a positive semidefinite completion,
    and K is the cone of positive semidefinite matrices with
    sparsity pattern V.

    ARGUMENTS
    A         CVXOPT spmatrix with doubles
    b         CVXOPT matrix with doubles

    RETURNS
    sol       dictionary

    """

    from cvxopt import matrix, spmatrix, sqrt, log, blas, lapack, base, cholmod

    # from chompack import sparse, project, tril, symmetrize, hessian, \
    #    perm, embed, maxcardsearch, llt, dot, axpy, copy, scal, \
    #    completion, cholesky, solve, partial_inv, logdet
    from chompack import (
        cspmatrix,
        symbolic,
        tril,
        perm,
        symmetrize,
        dot,
        peo,
        hessian,
        completion,
        cholesky,
        projected_inverse,
        llt,
        trsm,
        maxcardsearch,
    )
    from smcp._version import __version__
    from smcp import misc
    from time import perf_counter, process_time

    # Start time
    T0wall = perf_counter()
    T0 = process_time()

    status = "unknown"
    # Number of constraints
    m = A.size[1] - 1
    # Matrix order
    n = int(sqrt(A.size[0]))

    DEBUG = options["debug"]
    if not isinstance(DEBUG, bool):
        raise TypeError("options['debug'] must be a bool")

    ALPHA = options["alpha"]
    if type(ALPHA) is not float or (ALPHA >= 0.5 or ALPHA <= 0.0):
        raise TypeError("options['alpha'] must be a float" "in the interal (0.0,0.5)")

    BETA = options["beta"]
    if type(BETA) is not float or (BETA >= 1.0 or BETA <= 0.0):
        raise TypeError("options['beta'] must be a float" "in the interal (0.0,1.0)")

    MINSTEP = options["minstep"]
    if type(MINSTEP) is not float or (MINSTEP < 0.0):
        raise TypeError("options['minstep'] must be a nonnegative float")

    DELTA = options["delta"]
    if type(DELTA) is not float or (DELTA <= 0.0 or DELTA >= 1.0):
        raise TypeError("options['delta'] must be a float" "in the interal (0.0,1.0)")

    ETA = options["eta"]
    if ETA is not None:
        if type(ETA) is not float or ETA <= 0.0:
            raise TypeError("options['eta'] must be a positive float")
        ETATOL = 0.10 * ETA

    t = options["t0"]
    if type(t) is not float:
        raise TypeError("options['t0'] must be a positive float")
    elif t <= 0.0:
        raise ValueError("options['t0'] must be a positive float")

    LIFTING = options["lifting"]
    if type(LIFTING) is not bool:
        raise TypeError("options['lifting'] must be True or False")

    EQUALSTEPS = options["equalsteps"]
    if type(EQUALSTEPS) is not bool:
        raise TypeError("options['equalsteps'] must be True or False")

    PREDICTION = options["prediction"]
    if type(PREDICTION) is not bool:
        raise TypeError("options['prediction'] must be True or False")

    STEP = options["step"]
    if type(STEP) is not float:
        raise TypeError("options['step'] must be a float")
    elif STEP > 1 or STEP <= 0:
        raise ValueError("options['step'] must be between 0 and 1.")

    MAXITERS = options["maxiters"]
    if type(MAXITERS) is not int:
        raise TypeError("options['maxiters'] must be a positive " "integer")
    elif MAXITERS < 1:
        raise ValueError("options['maxiters'] must be positive")

    ABSTOL = options["abstol"]
    if type(ABSTOL) is not float and type(ABSTOL) is not int:
        raise TypeError("options['abstol'] must be a scalar")

    RELTOL = options["reltol"]
    if type(RELTOL) is not float and type(RELTOL) is not int:
        raise TypeError("options['reltol'] must be a scalar")

    if RELTOL <= 0.0 and ABSTOL <= 0.0:
        raise ValueError(
            "at least one of options['reltol'] and "
            "options['abstol'] must be positive"
        )

    FEASTOL = options["feastol"]
    if type(FEASTOL) is not float and type(FEASTOL) is not int:
        raise TypeError("options['feastol'] must be a positive " "scalar")
    elif FEASTOL <= 0.0:
        raise ValueError("options['feastol'] must be positive")

    CHOLMOD = options["cholmod"]
    if type(CHOLMOD) is not bool:
        raise TypeError("options['cholmod'] must be bool")

    ORDER = options["order"]
    if ORDER == "AMD":
        from cvxopt.amd import order
    elif ORDER == "METIS":
        from cvxopt.metis import order
    else:
        raise ValueError("options['order'] must be 'AMD' or 'METIS'")

    show_progress = options["show_progress"]
    if type(show_progress) is not bool:
        raise TypeError("options['show_progress'] must be a bool")

    REFINEMENT = options["refinement"]
    if (type(REFINEMENT)) is not int:
        raise TypeError("options['refinement'] must be a nonnegative " "integer")
    elif REFINEMENT < 0:
        raise ValueError("options['refinement'] must be nonnegative ")

    TNZCOLS = options["tnzcols"]
    if type(TNZCOLS) is not float:
        raise TypeError("tnzcols must be a float between 0.0 and 1.0")
    elif TNZCOLS > 1 or TNZCOLS < 0:
        raise ValueError("tnzcols must be between 0.0 and 1.0")
    else:
        TNZCOLS = int(n * TNZCOLS)

    DIMACS = options["dimacs"]
    if type(DIMACS) is not bool:
        raise TypeError("dimacs must be a bool")

    if not (scaling == "primal" or scaling == "dual"):
        raise ValueError("scaling must be 'primal' or 'dual'")

    def iperm(p):
        """
        Returns inverse permutation vector.
        ip = iperm(p)
        """
        ip = matrix(0, (len(p), 1))
        ip[p] = matrix(list(range(len(p))))
        return ip

    # solver does not handle sparse b yet:
    b = matrix(b)

    # aggregate sparsity pattern
    LIa = matrix(list(set(A.I)))
    Ia, Ja = misc.ind2sub(n, LIa)
    Va = tril(spmatrix(0.0, Ia, Ja, (n, n)))
    del Ia, Ja, LIa

    if kktsolver == "chol":
        # Find permutation
        #
        Nz = misc.nzcolumns(A)
        pm, Ns = misc.matperm(Nz, TNZCOLS)
        # Permute first part of pm (Decorate-Sort-Undecorate)
        if Ns < m:
            L = list(
                zip(
                    A.CCS[0][pm[: m - Ns] + 2] - A.CCS[0][pm[: m - Ns] + 1],
                    pm[: m - Ns],
                )
            )
            L.sort(reverse=True)
            pm[0:m-Ns] = matrix([l for _, l in L])
        # Permute second part of pm (Decorate-Sort-Undecorate)
        if Ns > 0:
            L = list(
                zip(
                    A.CCS[0][pm[m-Ns:m] + 2] - A.CCS[0][pm[m-Ns:m] + 1],
                    pm[m-Ns:m],
                )
            )
            # L = [(A.CCS[0][pm[i]+2]-A.CCS[0][pm[i]+1] + Nz[pm[i]],pm[i]) for i in xrange(m-Ns,m)]
            L.sort(reverse=True)
            pm[m-Ns:m] = matrix([l for _, l in L])
        del Nz
        # Permute b
        b = +b[pm]
    else:
        Ns = 0
        pm = matrix(range(m))

    bmax, ii = max(list(zip(abs(b), list(range(len(b))))))

    if p is None:
        p = order(Va)

    # make embeddings
    if CHOLMOD:  # hack for using cholmod symbolic
        ORDER += "+CHOLMOD"
        Ve = Va + spmatrix(
            [float(i + 1) for i in range(n)], list(range(n)), list(range(n)), (n, n)
        )
        F = cholmod.symbolic(Ve, p=p)
        cholmod.numeric(Ve, F)
        f = cholmod.getfactor(F)
        fd = [(v, i) for i, v in enumerate(f[:n**2:n+1])]
        fd.sort()
        ip = matrix([i for _, i in fd])
        p = iperm(ip)
        Vp = +f
        symb = symbolic(Vp)
        Vp.V = matrix([float(v) for v in range(len(Vp))])
        Ve = tril(perm(symmetrize(Vp), ip))
        lp = iperm([int(v) for v in Ve.V])
        CHORDAL = False
    else:
        pmcs = maxcardsearch(Va)
        if peo(Va, pmcs):
            CHORDAL = True
            p = pmcs
            symb = symbolic(Va, pmcs)
        else:
            CHORDAL = False
            symb = symbolic(Va, p)

        Ve = symb.sparsity_pattern(reordered=False, symmetric=False)
        Ve.V = matrix([float(v) for v in range(len(Ve))])
        Vp = tril(perm(symmetrize(Ve), p))
        lp = matrix([int(v) for v in Vp.V])
        symb = symbolic(Vp)
        ip = iperm(p)

    # extract vector space index permutation and Vp subscripts
    LI = misc.sub2ind((n, n), Ve.I, Ve.J)
    Ip, Jp = Vp.I, Vp.J

    # Extract Av and c
    # c = A[LI[lp],0] ## <-- unpacks A[:,0]
    c = A[:, 0][LI[lp]]  # <-- does not unpack A[:,0]
    # Av = A[LI[lp],pm+1]
    if len(A) < 0.005 * len(Va) * m or n > 15000:  # do not unpack A
        Ve.V = matrix([float(v) for v in iperm(lp)])
        IJV = []
        tmp = 0
        for j in pm:
            j1, j2 = A.CCS[0][j + 1], A.CCS[0][j + 2]
            # Ltmp = [(int(Ve[A.CCS[1][k]]),tmp,A.CCS[2][k]) for k in xrange(j1,j2)]
            Ltmp = list(
                zip(
                    [int(k) for k in Ve[A.CCS[1][j1:j2]]],
                    [tmp for i in range(j2 - j1)],
                    [k for k in A.CCS[2][j1:j2]],
                )
            )
            Ltmp.sort()
            IJV += Ltmp
            tmp += 1
        It, Jt, Vt = list(zip(*IJV))
        Av = spmatrix(Vt, It, Jt, (len(Vp), m))
        del It, Jt, Vt, IJV
    else:  # unpack A
        Av = A[LI[lp], pm + 1]

    # Indices of diagonal elements in Av
    Id = matrix([i for i in range(len(Vp)) if Ip[i] == Jp[i]])

    # Check that number of constraints is leq. number of nonzeros
    if m > len(Vp):
        raise ValueError("more constraints than nonzeros")

    # if Cholesky solver, extract nonzero column indices
    if kktsolver == "chol":
        Kl = []
        for j in range(m - Ns, m):
            k = []
            for ll in Av.CCS[1][Av.CCS[0][j]:Av.CCS[0][j+1]]:
                k.append(Ip[ll])
                k.append(Jp[ll])
            Kl.append(list(set(k)))

    # Convert A0 to chompack
    Vp.V = matrix(c)
    C = cspmatrix(symb) + Vp

    def Amap(X, i=None):
        Xp = X.spmatrix(reordered=False, symmetric=False)
        misc.scal_diag(Xp, Id, 0.5)
        v = matrix(0.0, (m, 1))
        if i is None:
            base.gemv(Av, Xp.V, v, trans="T", alpha=2.0)
            return v
        else:
            base.gemv(Av[:, i], Xp.V, v, trans="T", alpha=2.0)
            return v[0]

    def Aadj(y):
        v = matrix(0.0, (len(Vp), 1))
        base.gemv(Av, y, v, trans="N")
        Vp.V = v
        return cspmatrix(symb) + Vp

    def Omega(Lsh, Ls, gap):
        """
        Computes

        Omega(X,S) = phip(X) + phid(S) + n log(<X,S>/n) + n

        where Ls is the Cholesky factor of S and Lsh is the Cholesky
        factor of Shat = X^-1.
        """
        return 2.0 * sum(log(Lsh.diag())) - 2.0 * sum(log(Ls.diag())) + n * log(gap / n)

    # Norm of b and C
    resy0 = max(1, blas.nrm2(b))
    resx0 = max(1, sqrt(dot(C, C)))

    def kkt_res(L, Y, x, y, bx, by):
        r = x.copy()
        # hessian(L,Y,r,inv=True,adj=True)
        # hessian(L,Y,r,inv=True)
        hessian(L, Y, r, inv=True, adj=None)
        if scaling == "primal":
            blas.scal(-1.0 / t, r.blkval)
        else:
            blas.scal(-t, r.blkval)
        r += Aadj(y) - bx
        return r, Amap(x) - by

    def kkt_qr(L, Y):
        Ac_ = [Aj.copy() for Aj in Ac]
        hessian(L, Y, Ac_)
        for j in range(len(Ac_)):
            Aj = Ac_[j].spmatrix(reordered=False, symmetric=False)
            At[:, j] = Aj.V
        # scale diagonal elements
        At[Id, :] /= sqrt(2.0)

        # compute QR factorization of At
        try:
            lapack.geqrf(At, tq)
        except ArithmeticError:
            print("*** Factorization failed")
            return {status: "unknown"}

        def solve_(bx, by, tt=None):
            """
            Solves the system:

              [  -kk*H  A' ][ x ] = [ bx ]
              [   A     0  ][ y ]   [ by ]

            """
            if tt is None:
                tt = t
            if scaling == "primal":
                kk = 1.0 / tt
            else:
                kk = tt

            # compute x,y
            r1 = bx.copy()
            hessian(L, Y, [r1])
            r1 = r1.spmatrix(reordered=False, symmetric=False).V
            r1[Id] /= sqrt(2.0)
            x = +r1

            lapack.ormqr(At, tq, x, trans="T")
            x[m:] = 0.0

            r2 = +by
            lapack.trtrs(At[:m, :], r2, uplo="U", trans="T")
            x[:m] += 0.5 * kk * r2
            y = +x[:m]
            lapack.trtrs(At[:m, :], y, uplo="U")

            lapack.ormqr(At, tq, x)
            Vp.V = x - r1
            misc.scal_diag(Vp, Id, sqrt(2.0))
            x = cspmatrix(symb) + Vp
            hessian(L, Y, [x], adj=True)
            blas.scal(1.0 / kk, x.blkval)

            if DEBUG:
                r, rr = kkt_res(L, Y, x, y, bx, by)
                r = sqrt(dot(r, r)) / max(1, sqrt(dot(bx, bx)))
                rr = blas.nrm2(rr) / max(1, blas.nrm2(by))
                print("\033[1;33m KKTsolver: %.2e  %.2e \033[0m" % (r, rr))

            return x, y

        return solve_

    def kkt_chol(L, Y):
        # Compute H (lower triangle)
        for j in range(0, m - Ns):
            At = misc.Av_to_spmatrix(Av, Ip, Jp, j, n)
            U = cspmatrix(symb) + At
            # hessian(L,Y,[U]); hessian(L,Y,[U],adj=True)
            hessian(L, Y, U, inv=False, adj=None)
            At = U.spmatrix(reordered=False, symmetric=False)
            misc.scal_diag(At, Id, 0.5)
            base.gemv(Av[:, j:m], At.V, tmp, trans="T", alpha=2.0)
            H[j:m, j] = tmp[: m - j]

        for j in range(0, Ns):
            V = matrix(spmatrix(1.0, Kl[j], range(len(Kl[j])), (n, len(Kl[j]))))
            trsm(L, V)
            trsm(L, V, trans="T")
            # compute column j of H (lower triangle only)
            kkl = matrix(0, (n, 1))
            for k1, k2 in enumerate(Kl[j]):
                kkl[k2] = k1
            misc.SCMcolumn2(H, Av, V, Ip, Jp, kkl, m - Ns + j)

        # Cholesky factorization: H = L*L'
        try:
            lapack.potrf(H)
        except ArithmeticError:
            print("*** Factorization failed")
            return {status: "unknown"}

        def solve_(bx, by, tt=None):
            """
            Solves the system:

              [ -kk*H  A' ][ x ] = [ bx ]
              [   A    0  ][ y ]   [ by ]

            """
            if tt is None:
                tt = t
            if scaling == "primal":
                kk = 1.0 / tt
            else:
                kk = tt

            r1 = bx.copy()
            # hessian(L,Y,[r1])
            # hessian(L,Y,[r1],adj=True)
            hessian(L, Y, r1, inv=False, adj=None)
            y = kk * by + Amap(r1)
            lapack.potrs(H, y)

            x = Aadj(y) - bx
            # hessian(L,Y,[x])
            # hessian(L,Y,[x],adj=True)
            hessian(L, Y, [x], inv=False, adj=None)
            blas.scal(1.0 / kk, x.blkval)

            if DEBUG:
                r, rr = kkt_res(L, Y, x, y, bx, by)
                r1 = sqrt(dot(r, r)) / max(1, sqrt(dot(bx, bx)))
                r2 = blas.nrm2(rr) / max(1, blas.nrm2(by))
                print("\033[1;33m KKTsolver: %.2e  %.2e \033[0m" % (r1, r2))
            return x, y

        return solve_

    if kktsolver == "chol":
        factor = kkt_chol
        SolvStr = "Chol."
        # Allocate storage for SCM
        H = matrix(0.0, (m, m))
        # Allocate storage for tmp
        if Ns < m:
            tmp = matrix(0.0, (m, 1))
    elif kktsolver == "qr":
        factor = kkt_qr
        SolvStr = "QR"
        # Convert A_1,...,A_m to chompack
        Ac = [cspmatrix(symb) + misc.Av_to_spmatrix(Av, Ip, Jp, j, n) for j in range(m)]
        # Allocate storage for At
        At = matrix(0.0, (len(Vp), m))
        # Allocate storage for tq
        tq = matrix(0.0, (m, 1))
    else:
        raise ValueError("Unknown 'kktsolver'.")

    if LIFTING:
        SolvStr += ",lifting"
    if PREDICTION:
        SolvStr += ",prediction"

    def kktsolver(L, Y):
        return factor(L, Y)

    def print_head():
        print("%-20s Barrier method, %s scaling (%s)" % (__version__, scaling, SolvStr))
        print(
            "-------------------------------------------------------------------------------"
        )
        print("SDP var. size:       %i " % (n))
        print("Constraints:         %i (%i|%i)" % (m, m - Ns, Ns))
        if CHORDAL:
            ChStr = "Chordal"
        else:
            ChStr = "Nonchordal"
        print("Aggregate sparsity:  %-14s NNZ(tril(V)) = %7i" % (ChStr, len(Va)))
        if not CHORDAL:
            print("Embedding:           %-14s       NNZ(L) = %7i" % (ORDER, len(Vp)))
        print(
            "-------------------------------------------------------------------------------"
        )
        print(
            " it  pcost       dcost      gap     pres    dres    ntdecr  Omega   pstep dstep"
        )

    def print_exit_info():
        if status == "optimal" or status == "unknown":
            if pcost is not None:
                print("   Primal objective:                % .8e" % (pcost))
            if dcost is not None:
                print("   Dual objective:                  % .8e" % (dcost))
        if gap is not None:
            print("   Gap:                             % .8e" % (gap))
        if relgap is not None:
            print("   Relative gap:                    % .8e" % (relgap))
        if pres is not None:
            print("   Primal infeasibility:            % .8e" % (pres))
        if dres is not None:
            print("   Dual infeasibility:              % .8e" % (dres))
        if not iter == 0:
            print("   Iterations:                       %i" % (iter))
            print("   CPU time:                         %.2f" % (Tcpu))
            print("   CPU time per iteration:           %.2f" % (Tcpu / iter))
            print("   Real time:                        %.2f" % (Twall))
            print("   Real time per iteration:          %.2f\n" % (Twall / iter))
        if DIMACS:
            print("   DIMACS:  %.2e %.2e %.2e %.2e %.2e %.2e\n" % tuple(DIMACS_err))

    def linesearch(X, dx, S, ds, a=1.0):
        N = 8
        # PRIMAL STEP
        gMIN = MINSTEP
        gMAX = 1.0
        for i in range(N):
            gam = (gMIN + gMAX) / 2.0
            Xt = X.copy() + gam * dx
            try:
                Lt = Xt.copy()
                completion(Lt)
                gMIN = gam
                gam_ = gam
            except ArithmeticError:
                gMAX = gam
                gam_ = gMIN
        pstep = gam_
        # DUAL STEP
        gMIN = MINSTEP
        gMAX = 1.0
        for i in range(N):
            gam = (gMIN + gMAX) / 2.0
            St = S.copy() + gam * ds
            try:
                Lt = St.copy()
                cholesky(Lt)
                gMIN = gam
                gam_ = gam
            except ArithmeticError:
                gMAX = gam
                gam_ = gMIN
        dstep = gam_
        return a * pstep, a * dstep

    def linesearch_backtrack(X, dx, S, ds, gstart=1.0):
        gam = gstart
        while gam > MINSTEP:
            Xt = X.copy() + gam * dx
            St = S.copy() + gam * ds
            try:
                cholesky(St)
                completion(Xt)
                break
            except ArithmeticError:
                gam *= BETA
        return gam, gam

    def linesearch_Omega(X, dx, S, ds, eta):
        gMIN = MINSTEP
        gMAX = 1.0
        for i in range(8):
            gam = (gMIN + gMAX) / 2.0
            Xt = X.copy() + gam * dx
            St = S.copy() + gam * ds
            try:
                Lt = Xt.copy()
                completion(Lt)
                Lst = St.copy()
                cholesky(Lst)
                gapt = dot(St, Xt)
                gam_ = gam
                Ot = Omega(Lt, Lst, gapt)
                if Ot - eta > ETATOL:
                    gMAX = gam
                elif Ot - eta < -ETATOL:
                    gMIN = gam
                else:
                    break
            except ArithmeticError:
                gMAX = gam
                gam_ = None
        if gam_:
            return gam_
        else:
            return gMIN

    # Starting point
    if primalstart is not None:
        X = cspmatrix(symb) + tril(perm(symmetrize(tril(primalstart["x"])), p))
        if blas.nrm2(b - Amap(X)) / resy0 > 1e-8:
            raise ValueError("infeasible primal starting point")
        try:
            L = X.copy()
            completion(L)
        except ArithmeticError:
            raise ValueError("infeasible primal starting point")
    else:
        X = None
    if dualstart is not None:
        if "y" in dualstart and "s" in dualstart:
            y = dualstart["y"][pm]
            S = cspmatrix(tril(perm(symmetrize(tril(dualstart["s"])), p)))
            resx = Aadj(-y) + C - S
            if sqrt(dot(resx, resx)) / resx0 > 1e-8:
                raise ValueError("infeasible dual starting point")
        elif "y" in dualstart and "s" not in dualstart:
            y = dualstart["y"][pm]
            S = Aadj(-y) + C
        try:
            L = S.copy()
            cholesky(L)
        except ArithmeticError:
            raise ValueError("infeasible dual starting point")
    else:
        y = None
        S = None

    if primalstart is None and dualstart is None:
        # heuristics for finding feasible starting point

        # look for strictly feasible primal point
        Xt = cspmatrix(symb) + spmatrix(1.0, list(range(n)), list(range(n)))
        Lt = Xt.copy()
        completion(Lt)
        fI = kktsolver(Lt, Xt)
        X0, nu = fI(cspmatrix(symb), b)
        try:
            L = X0.copy()
            completion(L)
            X = X0
            print("Primal least-norm solution is feasible.")
        except ArithmeticError:
            trA = matrix(0.0, (m, 1))
            base.gemv(Av[Id, :], matrix(1.0, (n, 1)), trA, trans="T")
            Xb, nu = fI(cspmatrix(symb), trA)
            Xb -= Xt
            try:
                L = Xb.copy()
                completion(L)
                # Xb is in cone!
                gam = 2.0
                while True:
                    X = X0.copy() + gam * Xb
                    try:
                        Lt = X.copy()
                        cholesky(Lt)
                        break
                    except ArithmeticError:
                        gam *= 2
                print("Feasible primal solution found.")
            except ArithmeticError:
                try:
                    blas.scal(-1.0, Xb.blkval)
                    L = Xb.copy()
                    completion(L)
                    gam = 2.0
                    while True:
                        X = X0.copy() + gam * Xb
                        try:
                            L = X.copy()
                            cholesky(L)
                            break
                        except ArithmeticError:
                            gam *= 2
                    print("Feasible primal solution found.")
                except ArithmeticError:
                    X = None

        # look for strictly feasible dual point
        nu, y = fI(C, matrix(0.0, (m, 1)))
        S = Aadj(-y) + C
        try:
            L = S.copy()
            cholesky(L)
            print("Dual least-squares solution is feasible.")
        except ArithmeticError:
            nu, y = fI(
                cspmatrix(symb) + spmatrix(-1.0, list(range(n)), list(range(n))),
                matrix(0.0, (m, 1)),
            )
            Sh = Aadj(-y)
            try:
                L = Sh.copy()
                cholesky(L)
                # Sh is in cone!
                gam = 1.4
                while True:
                    S = Aadj(-y) + C
                    try:
                        Lt = S.copy()
                        cholesky(Lt)
                        break
                    except ArithmeticError:
                        y *= gam
                print("Feasible dual solution found.")
            except ArithmeticError:
                S = None
                y = None

    if y is None and X is None:
        raise ValueError(
            "could not find a feasible starting point"
            " (solve Phase I problem instead)"
        )
    elif X is None and scaling == "primal":
        print("Switching to dual scaling.")
        scaling = "dual"
    elif S is None and scaling == "dual":
        print("Switching to primal scaling.")
        scaling = "primal"

    if show_progress:
        print_head()

    # initial gap:
    gap = n / t

    if scaling == "primal":
        dres = None
        dcost = None
    else:  # scaling == 'dual'
        pres = None
        pcost = None
    CENTER = True

    for iter in range(1, MAXITERS + 2):
        # RESIDUALS AND CONVERGENCE STATISTICS
        #
        if scaling == "primal":
            resy = b - Amap(X)
            pres = blas.nrm2(resy) / resy0
            pcost = dot(C, X)
        else:  # scaling == 'dual'
            resx = Aadj(-y) + C - S
            dres = sqrt(dot(resx, resx)) / resx0
            dcost = blas.dot(b, y)

        # compute relgap
        if pcost is not None and pcost < 0.0:
            relgap = gap / -pcost
        elif dcost is not None and dcost > 0.0:
            relgap = gap / dcost
        else:
            relgap = None

        # Stopping criteria
        #
        if iter == MAXITERS + 1:
            # Max. number of iterations reached
            if show_progress:
                print("Terminated (maximum number of iterations reached).")
            status = "unknown"
            break
        elif dres is not None and pres is not None:
            if pres < FEASTOL and dres < FEASTOL and (gap < ABSTOL):
                if show_progress:
                    print("Optimal solution found.")
                status = "optimal"
                break
            elif relgap is not None:
                if pres < FEASTOL and dres < FEASTOL and (relgap < RELTOL):
                    if show_progress:
                        print("Optimal solution found.")
                    status = "optimal"
                    break

        if scaling == "primal":
            try:
                L = X.copy()
                completion(L)
            except ArithmeticError:
                if show_progress:
                    print("*** Completion failed.")
                status = "unknown"
                break
            Y = X.copy()
        else:  # scaling == 'dual'
            try:
                L = S.copy()
                cholesky(L)
            except ArithmeticError:
                if show_progress:
                    print("*** Factorization of S failed.")
                status = "unknown"
                break
            Y = L.copy()
            projected_inverse(Y)

        f = kktsolver(L, Y)
        if f is None:
            break

        if CENTER:
            if scaling == "primal":
                # Centering:
                # [ -1/t Hphip(x)   A^T ][ dx  ] = [ C + 1/t Gphip(x) ]
                # [   A             0   ][ lam ] = [      b-Ax        ]
                #
                Shat = L.copy()
                llt(Shat)
                bx = C.copy() - (1.0 / t) * Shat
                by = b - Amap(X)
                dx, lam = f(bx, by)
                # iterative refinement
                for ii in range(REFINEMENT):
                    r1, r2 = kkt_res(L, Y, dx, lam, bx, by)
                    ddx, dlam = f(r1, r2)
                    dx -= ddx
                    lam -= dlam

                # compute Newton decr.
                du = dx.copy()
                hessian(L, Y, [du], inv=True, adj=True)
                ntdecr = sqrt(dot(du, du))
                del du

                if ntdecr > DELTA:
                    if ntdecr >= 1.0:
                        # centering step: backtracking line search
                        gam = 1.0
                        logdetL = sum(log(L.diag()))
                        tdcdx = t * dot(C, dx)
                        # val = t*dot(C,dx) + ALPHA*ntdecr**2
                        while gam > MINSTEP:
                            Xt = X.copy() + gam * dx
                            val = tdcdx + gam * ALPHA * ntdecr**2
                            try:
                                Lt = Xt.copy()
                                completion(Lt)
                                if gam * val < 2 * (logdetL - sum(log(Lt.diag()))):
                                    break
                                else:
                                    gam *= BETA
                            except ArithmeticError:
                                gam *= BETA

                        # update X
                        X = Xt
                    else:
                        # update X (full step)
                        X += dx
                        gam = 1.0

                    stype = "c"

                else:
                    # Lifting
                    if LIFTING:
                        X -= dx
                        dxL = dx
                    else:
                        X += dx
                    y = lam
                    S = Aadj(-y) + C
                    CENTER = False

            else:  # scaling == 'dual'
                # Centering:
                # [ -(1/t Hphid(s))^-1   A^T ][ nu ] = [-s ]
                # [        A              0  ][ dy ] = [ b ]
                bx = S.copy()
                blas.scal(-1.0, bx.blkval)
                by = b
                nu, dy = f(bx, by)
                # iterative refinement
                for ii in range(REFINEMENT):
                    r1, r2 = kkt_res(L, Y, nu, dy, bx, by)
                    dnu, ddy = f(r1, r2)
                    nu -= dnu
                    dy -= ddy

                # compute Newton decr.
                du = Aadj(dy)
                hessian(L, Y, [du])
                ntdecr = sqrt(dot(du, du))
                del du

                if ntdecr > DELTA:
                    if ntdecr >= 1.0:
                        # centering step: backtracking line search
                        gam = 1.0
                        logdetL = sum(log(L.diag()))
                        ddyb = -t * blas.dot(dy, b)
                        # val = ALPHA*ntdecr**2 - t*blas.dot(dy,b)
                        while gam > MINSTEP:
                            yt = y + gam * dy
                            St = Aadj(-yt) + C
                            val = ddyb + gam * ALPHA * ntdecr**2
                            try:
                                Lt = St.copy()
                                cholesky(Lt)
                                if gam * val < 2 * (sum(log(Lt.diag())) - logdetL):
                                    break
                                else:
                                    gam *= BETA
                            except ArithmeticError:
                                gam *= BETA

                        # update S,y
                        S = St
                        y = yt
                    else:
                        # update S,y (full step)
                        y += dy
                        S = Aadj(-y) + C
                        gam = 1.0

                    stype = "c"

                else:
                    # Lifting
                    if LIFTING:
                        y -= dy
                        S = Aadj(-y) + C
                        dyL = dy
                    else:
                        y += dy
                        S = Aadj(-y) + C
                    X = nu
                    CENTER = False

        if not CENTER:

            # Approximate tangent step (primal scaling):
            # [ -1/t Hphip(w)   A^T ][ dx  ] = [  s   ]
            # [   A             0   ][ dy  ] = [ b-Ax ]
            #
            # Approximate tangent step (dual scaling):
            # [ -(1/t Hphid(w))^{-1}   A^T ][ dx  ] = [  s   ]
            # [        A               0   ][ dy  ] = [ b-Ax ]
            bx = S.copy()
            by = b - Amap(X)
            dx, dy = f(bx, by)
            # iterative refinement
            for ii in range(REFINEMENT):
                r1, r2 = kkt_res(L, Y, dx, dy, bx, by)
                ddx, ddy = f(r1, r2)
                dx -= ddx
                dy -= ddy
            ds = Aadj(-dy)

            if ETA is not None:  # STAY IN OMEGA-NEIGHBORHOOD
                # line search (bisection)
                gam = linesearch_Omega(X, dx, S, ds, ETA)
                # update X,y,S
                X += gam * dx
                y += gam * dy
                S = Aadj(-y) + C
                pstep = gam
                dstep = gam  # <-- for iter. info

            else:
                # "EXACT" LINE SEARCH ALONG APPROX. TANGENT DIRECTION
                pstep, dstep = linesearch(X, dx, S, ds, STEP)
                if EQUALSTEPS:
                    pstep = min(pstep, dstep)
                    dstep = pstep

                Xt = X.copy() + pstep * dx
                yt = y + dstep * dy
                St = Aadj(-yt) + C

                if not PREDICTION:
                    # Update X,y,S
                    X = Xt
                    S = St
                    y = yt

                else:  # PREDICTION

                    # Compute predicted gap
                    gapt = dot(Xt, St)

                    if LIFTING:
                        # undo lifting
                        if scaling == "primal":
                            X += dxL
                        else:  # scaling == 'dual'
                            blas.axpy(dyL, y)
                            S = Aadj(-y) + C

                    if DEBUG:
                        print(" -- predicted gap: %.2e" % (gapt))
                    # update t
                    t = n / gapt

                    if scaling == "primal":
                        # step:
                        # [ -1/t Hphip(x)   A^T ][ dx  ] = [ s - (1/t)*shat ]
                        # [    A            0   ][ dy  ] = [ b-Ax           ]
                        bx = S.copy() - (1.0 / t) * Shat
                        by = b - Amap(X)
                    else:  # scaling == 'dual'
                        # step:
                        # [ -t Hphid(s)^{-1}   A^T ][ dx  ] = [ t*Hphid(s)^{-1}*x - s ]
                        # [    A               0   ][ dy  ] = [ b-Ax                  ]
                        bx = X.copy()
                        # hessian(L,Y,[bx],inv=True,adj=True)
                        # hessian(L,Y,[bx],inv=True)
                        hessian(L, Y, bx, inv=True, adj=None)
                        blas.scal(t, bx.blkval)
                        bx -= S
                        by = b - Amap(X)

                    dx, dy = f(bx, by, t)
                    # iterative refinement
                    for ii in range(REFINEMENT):
                        r1, r2 = kkt_res(L, Y, dx, dy, bx, by)
                        ddx, ddy = f(r1, r2, t)
                        dx -= ddx
                        dy -= ddy
                    ds = Aadj(-dy)

                    # NTDECR
                    if scaling == "primal":
                        du = dx.copy()
                        hessian(L, Y, [du], inv=True, adj=True)
                        ntdecr = sqrt(dot(du, du))
                        del du
                    else:  # scaling == 'dual'
                        du = Aadj(dy)
                        hessian(L, Y, [du])
                        ntdecr = sqrt(dot(du, du))
                        del du

                    # LINE SEARCH
                    if scaling == "primal":
                        if ntdecr >= 1.0:
                            # centering step: backtracking line search
                            gam = 1.0
                            logdetL = sum(log(L.diag()))
                            tdcdx = t * dot(C, dx)
                            # val = t*dot(C,dx) + ALPHA*ntdecr**2
                            while gam > MINSTEP:
                                Xt = X.copy() + gam * dx
                                val = tdcdx + gam * ALPHA * ntdecr**2
                                try:
                                    Lt = Xt.copy()
                                    completion(Lt)
                                    if gam * val < 2 * (logdetL - sum(log(Lt.diag()))):
                                        break
                                    else:
                                        gam *= BETA
                                except ArithmeticError:
                                    gam *= BETA

                            # update X
                            pstep = gam
                            X = Xt
                        else:
                            # (full step)
                            pstep = 1.0
                            X += dx

                        gam = 1.0
                        while True:
                            yt = y + gam * dy
                            St = Aadj(-yt) + C
                            try:
                                Lt = St.copy()
                                cholesky(Lt)
                                break
                            except ArithmeticError:
                                gam *= BETA
                        dstep = gam
                        y = yt
                        S = St
                    else:
                        if ntdecr >= 1.0:
                            # centering step: backtracking line search
                            gam = 1.0
                            logdetL = sum(log(L.diag()))
                            ddyb = -t * blas.dot(dy, b)
                            # val = ALPHA*ntdecr**2 - t*blas.dot(dy,b)
                            while gam > MINSTEP:
                                yt = y + gam * dy
                                St = Aadj(-yt) + C
                                val = ddyb + gam * ALPHA * ntdecr**2
                                try:
                                    Lt = St.copy()
                                    cholesky(Lt)
                                    if gam * val < 2 * (sum(log(Lt.diag())) - logdetL):
                                        break
                                    else:
                                        gam *= BETA
                                except ArithmeticError:
                                    gam *= BETA

                            # update S,y
                            S = St
                            y = yt
                            dstep = gam
                        else:
                            # update S,y (full step)
                            y += dy
                            S = Aadj(-y) + C
                            dstep = 1.0
                        gam = 1.0
                        while True:
                            Xt = X.copy() + gam * dx
                            try:
                                Lt = Xt.copy()
                                completion(Lt)
                                break
                            except ArithmeticError:
                                gam *= BETA
                        pstep = gam
                        X = Xt

            # Compute new gap and Omega(x,s)
            Lt = X.copy()
            completion(Lt)
            Lst = S.copy()
            cholesky(Lst)
            gapt = dot(X, S)
            Ot = Omega(Lt, Lst, gapt)
            if DEBUG:
                print(" -- actual gap:    %.2e" % (gapt))

            # update gap, t
            gap = min(n / t, gapt)  # Alternatively, set: gap = gapt [XXX]
            t = n / gap

            stype = "a"

            # update residuals
            pres = blas.nrm2(Amap(X) - b) / resy0

            dres = Aadj(y) + S - C
            dres = sqrt(dot(dres, dres)) / resx0

            pcost = dot(C, X)
            dcost = blas.dot(b, y)

            CENTER = True

        # print iter. info
        if gap is None:
            ggap = n / t
        else:
            ggap = gap
        if stype == "c":
            if dcost is None:
                print(
                    "%3i % .4e %-11s %.1e %.1e %7s %.1e %7s %4.2f"
                    % (iter, pcost, " ", ggap, pres, " ", ntdecr, " ", gam)
                )
            elif pcost is None:
                print(
                    "%3i %-11s % .4e %.1e %7s %.1e %.1e %13s %4.2f"
                    % (iter, " ", dcost, ggap, " ", dres, ntdecr, " ", gam)
                )
            elif scaling == "primal":
                print(
                    "%3i % .4e % .4e %.1e %.1e %.1e %.1e %7s %4.2f"
                    % (iter, pcost, dcost, ggap, pres, dres, ntdecr, " ", gam)
                )
            else:
                print(
                    "%3i % .4e % .4e %.1e %.1e %.1e %.1e %13s %4.2f"
                    % (iter, pcost, dcost, ggap, pres, dres, ntdecr, " ", gam)
                )
        elif stype == "a":
            print(
                "%3i % .4e % .4e %.1e %.1e %.1e %.1e %.1e %4.2f  %4.2f"
                % (iter, pcost, dcost, ggap, pres, dres, ntdecr, Ot, pstep, dstep)
            )

    # END OF ITERATION LOOP
    #

    # Get cputime and realtime
    Tcpu = process_time() - T0
    Twall = perf_counter() - T0wall

    if DIMACS and (X is not None) and (y is not None) and (S is not None):
        DIMACS_err = matrix(0.0, (6, 1))
        DIMACS_err[0] = blas.nrm2(Amap(X) - b) / (1.0 + max(abs(b)))
        DIMACS_err[1] = 0.0
        R = Aadj(y) + S - C
        c_abs = abs(c)
        if len(c_abs) == 0:
            c_abs = [0.0]
        DIMACS_err[2] = sqrt(dot(R, R)) / (1.0 + max(c_abs))
        DIMACS_err[3] = 0.0
        DIMACS_err[4] = (pcost - dcost) / (1 + abs(pcost) + abs(dcost))
        DIMACS_err[5] = gap / (1 + abs(pcost) + abs(dcost))
    else:
        DIMACS = False
        DIMACS_err = None

    # Convert to CVXOPT variables
    try:
        X = X.spmatrix(reordered=False, symmetric=False)
        X = perm(symmetrize(X), ip)
    except ArithmeticError:
        X = None
    try:
        y = y[iperm(pm)]
        S = S.spmatrix(reordered=False, symmetric=False)
        S = perm(symmetrize(S), ip)
    except ArithmeticError:
        y = None
        S = None

    if show_progress:
        print_exit_info()

    return {
        "status": status,
        "x": X,
        "y": y,
        "s": S,
        "primal objective": pcost,
        "dual objective": dcost,
        "gap": gap,
        "relative gap": relgap,
        "primal infeasibility": pres,
        "dual infeasibility": dres,
        "iterations": iter,
        "cputime": Tcpu,
        "time": Twall,
    }


def chordalsolver_esd(
    A, b, primalstart=None, dualstart=None, scaling="primal", kktsolver="chol", p=None
):
    """
    Chordal SDP solver:

    sol = chordalsolver_esd(A,b[,primalstart[,dualstart[,scaling[,kktsolver]]]])

    PURPOSE
    Solves a pair of primal and dual cone programs

         minimize   c'*x        maximize   b'*y
         subject to A*x = b     subject to A'*y + s = c
                    x in C                 s in K.

    Here C is the cone of symmetric matrices with sparsity
    pattern V that have a positive semidefinite completion,
    and K is the cone of positive semidefinite matrices with
    sparsity pattern V.

    ARGUMENTS
    A         CVXOPT spmatrix with doubles
    b         CVXOPT matrix with doubles

    RETURNS
    sol       dictionary

    """

    from cvxopt import matrix, spmatrix, sqrt, blas, lapack, base, cholmod

    # from chompack import sparse, project, tril, symmetrize, hessian, \
    #    perm, embed, maxcardsearch, llt, dot, axpy, copy, scal, \
    #    completion, cholesky, solve, partial_inv
    from chompack import (
        cspmatrix,
        symbolic,
        tril,
        perm,
        symmetrize,
        dot,
        peo,
        hessian,
        completion,
        cholesky,
        projected_inverse,
        llt,
        trsm,
        maxcardsearch,
    )
    from smcp._version import __version__
    from smcp import misc
    from time import perf_counter, process_time

    BETA = 0.7
    EXPON = 3.0
    STEP = 0.99
    MINSTEP = 1e-12

    # Start time
    T0wall = perf_counter()
    T0 = process_time()

    status = "unknown"
    # Number of constraints
    m = A.size[1] - 1
    # Matrix order
    n = int(sqrt(A.size[0]))

    DEBUG = options["debug"]
    if not isinstance(DEBUG, bool):
        raise TypeError("options['debug'] must be a bool")

    MAXITERS = options["maxiters"]
    if type(MAXITERS) is not int:
        raise TypeError("options['maxiters'] must be a positive " "integer")
    elif MAXITERS < 1:
        raise ValueError("options['maxiters'] must be positive")

    ABSTOL = options["abstol"]
    if type(ABSTOL) is not float and type(ABSTOL) is not int:
        raise TypeError("options['abstol'] must be a scalar")

    RELTOL = options["reltol"]
    if type(RELTOL) is not float and type(RELTOL) is not int:
        raise TypeError("options['reltol'] must be a scalar")

    if RELTOL <= 0.0 and ABSTOL <= 0.0:
        raise ValueError(
            "at least one of options['reltol'] and "
            "options['abstol'] must be positive"
        )

    FEASTOL = options["feastol"]
    if type(FEASTOL) is not float and type(FEASTOL) is not int:
        raise TypeError("options['feastol'] must be a positive " "scalar")
    elif FEASTOL <= 0.0:
        raise ValueError("options['feastol'] must be positive")

    CHOLMOD = options["cholmod"]
    if type(CHOLMOD) is not bool:
        raise TypeError("options['cholmod'] must be bool")

    ORDER = options["order"]
    if ORDER == "AMD":
        from cvxopt.amd import order
    elif ORDER == "METIS":
        from cvxopt.metis import order
    else:
        raise ValueError("options['order'] must be 'AMD' or 'METIS'")

    show_progress = options["show_progress"]
    if type(show_progress) is not bool:
        raise TypeError("options['show_progress'] must be a bool")

    REFINEMENT = options["refinement"]
    if (type(REFINEMENT)) is not int:
        raise TypeError("options['refinement'] must be a nonnegative " "integer")
    elif REFINEMENT < 0:
        raise ValueError("options['refinement'] must be nonnegative ")

    TNZCOLS = options["tnzcols"]
    if type(TNZCOLS) is not float:
        raise TypeError("tnzcols must be a float between 0.0 and 1.0")
    elif TNZCOLS > 1 or TNZCOLS < 0:
        raise ValueError("tnzcols must be between 0.0 and 1.0")
    else:
        TNZCOLS = int(n * TNZCOLS)

    DIMACS = options["dimacs"]
    if type(DIMACS) is not bool:
        raise TypeError("dimacs must be a bool")

    if not (scaling == "primal" or scaling == "dual"):
        raise ValueError("scaling must be 'primal' or 'dual'")

    def iperm(p):
        """
        Returns inverse permutation vector.
        ip = iperm(p)
        """
        ip = matrix(0, (len(p), 1))
        ip[p] = matrix(list(range(len(p))))
        return ip

    # solver does not handle sparse b yet:
    b = matrix(b)

    # aggregate sparsity pattern
    LIa = matrix(list(set(A.I)))
    Ia, Ja = misc.ind2sub(n, LIa)
    Va = tril(spmatrix(0.0, Ia, Ja, (n, n)))
    del Ia, Ja, LIa

    if kktsolver == "chol":
        # Find permutation
        #
        Nz = misc.nzcolumns(A)
        pm, Ns = misc.matperm(Nz, TNZCOLS)
        # Permute first part of pm (Decorate-Sort-Undecorate)
        if Ns < m:
            L = list(
                zip(
                    A.CCS[0][pm[: m - Ns] + 2] - A.CCS[0][pm[: m - Ns] + 1],
                    pm[: m - Ns],
                )
            )
            L.sort(reverse=True)
            pm[0:m-Ns] = matrix([l for _, l in L])
        # Permute second part of pm (Decorate-Sort-Undecorate)
        if Ns > 0:
            L = list(
                zip(
                    A.CCS[0][pm[m-Ns:m]+2] - A.CCS[0][pm[m-Ns:m]+1],
                    pm[m-Ns:m],
                )
            )
            # L = [(A.CCS[0][pm[i]+2]-A.CCS[0][pm[i]+1] + Nz[pm[i]],pm[i]) for i in xrange(m-Ns,m)]
            L.sort(reverse=True)
            pm[m-Ns:m] = matrix([l for _, l in L])
        del Nz
        # Permute b
        b = +b[pm]
    else:
        Ns = 0
        pm = matrix(range(m))

    bmax, ii = max(list(zip(abs(b), list(range(len(b))))))

    if p is None:
        p = order(Va)

    # make embeddings
    if CHOLMOD:  # hack for using cholmod symbolic
        ORDER += "+CHOLMOD"
        Ve = Va + spmatrix(
            [float(i + 1) for i in range(n)], list(range(n)), list(range(n)), (n, n)
        )
        F = cholmod.symbolic(Ve, p=p)
        cholmod.numeric(Ve, F)
        f = cholmod.getfactor(F)
        fd = [(j, i) for i, j in enumerate(f[:n**2:n+1])]
        fd.sort()
        Vp = +f
        symb = symbolic(Vp)
        Vp.V = matrix([float(v) for v in range(len(Vp))])
        ip = matrix([j for _, j in fd])
        p = iperm(ip)
        Ve = tril(perm(symmetrize(Vp), ip))
        lp = iperm([int(v) for v in Ve.V])
        CHORDAL = False
    else:
        pmcs = maxcardsearch(Va)
        if peo(Va, pmcs):
            CHORDAL = True
            p = pmcs
            symb = symbolic(Va, pmcs)
        else:
            CHORDAL = False
            symb = symbolic(Va, p)

        Ve = symb.sparsity_pattern(reordered=False, symmetric=False)
        Ve.V = matrix([float(v) for v in range(len(Ve))])
        Vp = tril(perm(symmetrize(Ve), p))
        lp = matrix([int(v) for v in Vp.V])
        symb = symbolic(Vp)
        ip = iperm(p)

    # extract vector space index permutation and Vp subscripts
    LI = misc.sub2ind((n, n), Ve.I, Ve.J)
    Ip, Jp = Vp.I, Vp.J

    # Extract Av and c
    # c = A[LI[lp],0] ## <-- unpacks A[:,0]
    c = A[:, 0][LI[lp]]  # <-- does not unpack A[:,0]
    # Av = A[LI[lp],pm+1]
    if len(A) < 0.005 * len(Va) * m or n > 15000:  # do not unpack A
        Ve.V = matrix([float(v) for v in iperm(lp)])
        IJV = []
        tmp = 0
        for j in pm:
            j1, j2 = A.CCS[0][j + 1], A.CCS[0][j + 2]
            # Ltmp = [(int(Ve[A.CCS[1][k]]),tmp,A.CCS[2][k]) for k in xrange(j1,j2)]
            Ltmp = list(
                zip(
                    [int(k) for k in Ve[A.CCS[1][j1:j2]]],
                    [tmp for i in range(j2 - j1)],
                    [k for k in A.CCS[2][j1:j2]],
                )
            )
            Ltmp.sort()
            IJV += Ltmp
            tmp += 1
        It, Jt, Vt = list(zip(*IJV))
        Av = spmatrix(Vt, It, Jt, (len(Vp), m))
        del It, Jt, Vt, IJV
    else:  # unpack A
        Av = A[LI[lp], pm + 1]

    # Indices of diagonal elements in Av
    Id = matrix([i for i in range(len(Vp)) if Ip[i] == Jp[i]])

    # Check that number of constraints is leq. number of nonzeros
    if m > len(Vp):
        raise ValueError("more constraints than nonzeros")

    # if Cholesky solver, extract nonzero column indices
    if kktsolver == "chol":
        Kl = []
        for j in range(m - Ns, m):
            k = []
            for ll in Av.CCS[1][Av.CCS[0][j]:Av.CCS[0][j+1]]:
                k.append(Ip[ll])
                k.append(Jp[ll])
            Kl.append(list(set(k)))

    # Convert A0 to chompack
    Vp.V = matrix(c)
    C = cspmatrix(symb) + Vp

    def Amap(X, i=None):
        Xp = X.spmatrix(reordered=False, symmetric=False)
        misc.scal_diag(Xp, Id, 0.5)
        v = matrix(0.0, (m, 1))
        if i is None:
            base.gemv(Av, Xp.V, v, trans="T", alpha=2.0)
            return v
        else:
            base.gemv(Av[:, i], Xp.V, v, trans="T", alpha=2.0)
            return v[0]

    def Aadj(y):
        v = matrix(0.0, (len(Vp), 1))
        base.gemv(Av, y, v, trans="N")
        Vp.V = v
        return cspmatrix(symb) + Vp

    # Starting point
    if primalstart is not None:
        X = cspmatrix(symb) + tril(perm(symmetrize(tril(primalstart["x"])), p))
    else:
        X = cspmatrix(symb) + spmatrix(1.0, range(n), range(n))
    if dualstart is not None:
        y = dualstart["y"][pm]
        S = cspmatrix(symb) + tril(perm(symmetrize(tril(dualstart["s"])), p))
    else:
        S = cspmatrix(symb) + spmatrix(1.0, range(n), range(n))
        y = matrix(0.0, (m, 1))

    # Alternative starting point
    if False:
        # * Find X that solves
        #
        #     minimize    ||X||_F
        #     subject to  <A_i,X> = b_i, for all i
        #
        #   by solving the system
        #
        #     [ 0   A ][ y ] = [ b ]
        #     [ A' -I ][ X ]   [ 0 ]
        #
        # * Find S=C-sum_i A_i*y_i  that solves
        #
        #     minimize ||S||_F
        #
        #   by solving
        #
        #     [ 0   A ][ y ] = [ 0 ]
        #     [ A' -I ][-S ]   [ C ]

        if primalstart is None or dualstart is None:
            Avs = +Av
            Avs[Id] /= sqrt(2.0)
            L = spmatrix([], [], [], (m, m))
            base.syrk(Avs, L, trans="T")
            F = cholmod.symbolic(L)
            cholmod.numeric(L, F)
            Ic = cspmatrix(symb) + spmatrix(1.0, list(range(n)), list(range(n)))
        if primalstart is None:
            u = +b
            cholmod.solve(F, u)
            X = Aadj(u)
            X = cspmatrix(symb) + Vp
            u = 1.0
            while True:
                try:
                    Lt = X.copy()
                    completion(Lt)
                    break
                except ArithmeticError:
                    X += u * Ic
                    u *= 2.0

        if dualstart is None:
            y = Amap(C)
            cholmod.solve(F, y)
            # compute s
            S = Aadj(-y) + C
            u = 1.0
            while True:
                St = S.copy()
                try:
                    Lt = St.copy()
                    cholesky(Lt)
                    break
                except ArithmeticError:
                    S += u * Ic
                    u *= 2.0

    tau = 1.0
    kappa = 1.0

    # Initial gap
    gap = dot(X, S) / tau**2
    t = (n + 1.0) / (gap * tau**2 + tau * kappa)

    # Norm of b and C
    resy0 = max(1, blas.nrm2(b))
    resx0 = max(1, sqrt(dot(C, C)))

    if scaling == "primal":

        def bres(sigma, dz=None):
            rby = (1 - sigma) * ry

            rbx = rx.copy()
            blas.scal(1 - sigma, rbx.blkval)

            rbt = (1 - sigma) * rt

            rbs = L.copy()
            llt(rbs)
            blas.scal(sigma / t, rbs.blkval)
            rbs -= S

            rbk = -kappa + sigma / (t * tau)

            if dz:
                rby += b * dz[2] - Amap(dz[1])  # uddate rby
                rbx += Aadj(dz[0]) + dz[3] - dz[2] * C  # update rbx
                rbt += dot(C, dz[1]) - blas.dot(b, dz[0]) + dz[4]  # update rbt
                rbs -= dz[3]  # update rbs
                u = dz[1].copy()
                # hessian(L,Y,[u],inv=True,adj=True)
                # hessian(L,Y,[u],inv=True)
                hessian(L, Y, [u], inv=True, adj=None)
                rbs -= (1.0 / t) * u
                rbk -= dz[4] + 1.0 / (t * tau**2) * dz[2]  # update rbk

            return (rby, rbx, rbt, rbs, rbk)

    else:  # dual scaling

        def bres(sigma, dz=None):

            rby = (1 - sigma) * ry

            rbx = rx.copy()
            blas.scal(1 - sigma, rbx.blkval)

            rbt = (1 - sigma) * rt

            rbs = Y.copy()
            blas.scal(sigma / t, rbs.blkval)
            rbs -= X

            rbk = -tau + sigma / (t * kappa)

            if dz:
                rby += b * dz[2] - Amap(dz[1])  # uddate rby
                rbx += -dz[2] * C + dz[3] + Aadj(dz[0])  # update rbx
                rbt += dot(C, dz[1]) - blas.dot(b, dz[0]) + dz[4]  # update rbt
                rbs -= dz[1]  # update rbs
                u = dz[3].copy()
                # hessian(L,Y,[u])
                # hessian(L,Y,[u],adj=True)
                hessian(L, Y, [u], inv=False, adj=None)
                rbs -= (1.0 / t) * u
                rbk -= dz[2] + 1.0 / (t * kappa**2) * dz[4]  # update rbk

            return (rby, rbx, rbt, rbs, rbk)

    if scaling == "primal":

        def tres(rbz):
            a = tau**2 * t * (rbz[2] + rbz[4])

            rtx = C.copy()
            blas.scal(a, rtx.blkval)
            rtx -= rbz[1] + rbz[3]
            rty = rbz[0] + a * b

            return rtx, rty

    else:

        def tres(rbz):
            a = rbz[4] + 1.0 / (t * kappa**2) * rbz[2]

            rtx = rbz[3].copy()
            # hessian(L,Y,[rtx],inv=True,adj=True)
            # hessian(L,Y,[rtx],inv=True)
            hessian(L, Y, rtx, inv=True, adj=None)
            blas.scal(-t, rtx.blkval)
            rtx += a * C - rbz[1]
            rty = rbz[0] + a * b

            return rtx, rty

    if DEBUG:

        def kkt_res(L, Y, x, y, bx, by):
            r = x.copy()
            # hessian(L,Y,[r],inv=True,adj=True)
            # hessian(L,Y,[r],inv=True)
            hessian(L, Y, [r], inv=True, adj=None)
            if scaling == "primal":
                blas.scal(-1.0 / t, r.blkval)
            else:
                blas.scal(-t, r.blkval)
            r += Aadj(y) - bx
            return r, Amap(x) - by

        def newton_res(sigma):
            rbz = bres(sigma)
            r1 = blas.nrm2(Amap(dX) - dtau * b - rbz[0])
            r2 = Aadj(-dy) + dtau * C - dS - rbz[1]
            r2 = sqrt(dot(r2, r2))
            r3 = abs(blas.dot(b, dy) - dot(C, dX) - dkappa - rbz[2])

            if scaling == "primal":
                r4 = dX.copy()
                # hessian(L,Y,[r4],adj=True,inv=True)
                # hessian(L,Y,[r4],adj=False,inv=True)
                hessian(L, Y, r4, inv=True, adj=None)
                blas.scal(1.0 / t, r4.blkval)
                r4 += dS - rbz[3]
                r4 = sqrt(dot(r4, r4))
                r5 = abs(dtau / (t * tau**2) + dkappa - rbz[4])
            else:
                r4 = dS.copy()
                # hessian(L,Y,[r4])
                # hessian(L,Y,[r4],adj=True)
                hessian(L, Y, r4, inv=False, adj=None)
                blas.scal(1.0 / t, r4.blkval)
                r4 += dX - rbz[3]
                r4 = sqrt(dot(r4, r4))
                r5 = abs(dkappa / (t * kappa**2) + dtau - rbz[4])
            print(
                "\033[1;32m Newton:   % .2e % .2e % .2e % .2e % .2e \033[0m"
                % (r1, r2, r3, r4, r5)
            )

    def kkt_qr(L, Y):
        Ac_ = [Aj.copy() for Aj in Ac]
        hessian(L, Y, Ac_)
        for j in range(len(Ac_)):
            Aj = Ac_[j].spmatrix(reordered=False, symmetric=False)
            At[:, j] = Aj.V
        # scale diagonal elements
        At[Id, :] /= sqrt(2.0)

        # compute QR factorization of At
        try:
            lapack.geqrf(At, tq)
        except ArithmeticError:
            print("\033[1;31m*** Factorization failed\033[0m")
            return {status: "unknown"}

        if scaling == "primal":
            kk = 1.0 / t
        else:
            kk = t

        def solve_(bx, by):
            """
            Solves the system:

              [  -kk*H  A' ][ x ] = [ bx ]
              [   A     0  ][ y ]   [ by ]

            """
            # compute x,y
            r1 = bx.copy()
            hessian(L, Y, [r1])
            r1 = r1.spmatrix(reordered=False, symmetric=False)
            r1 = r1.V
            r1[Id] /= sqrt(2.0)
            x = +r1

            lapack.ormqr(At, tq, x, trans="T")
            x[m:] = 0.0

            r2 = +by
            lapack.trtrs(At[:m, :], r2, uplo="U", trans="T")
            x[:m] += 0.5 * kk * r2
            y = +x[:m]
            lapack.trtrs(At[:m, :], y, uplo="U")

            lapack.ormqr(At, tq, x)
            Vp.V = x - r1
            misc.scal_diag(Vp, Id, sqrt(2.0))
            x = cspmatrix(symb) + Vp
            hessian(L, Y, [x], adj=True)
            blas.scal(1.0 / kk, x.blkval)

            if DEBUG:
                r, rr = kkt_res(L, Y, x, y, bx, by)
                r = sqrt(dot(r, r)) / sqrt(dot(bx, bx))
                rr = blas.nrm2(rr) / blas.nrm2(by)
                print("\033[1;33m KKTsolver: %.2e  %.2e \033[0m" % (r, rr))

            return x, y

        return solve_

    def kkt_chol(L, Y):
        # Compute H (lower triangle)
        for j in range(0, m - Ns):
            At = misc.Av_to_spmatrix(Av, Ip, Jp, j, n)
            U = cspmatrix(symb) + At
            # hessian(L,Y,[U]);
            # hessian(L,Y,[U],adj=True)
            hessian(L, Y, U, inv=False, adj=None)
            At = U.spmatrix(reordered=False, symmetric=False)
            misc.scal_diag(At, Id, 0.5)
            base.gemv(Av[:, j:m], At.V, tmp, trans="T", alpha=2.0)
            H[j:m, j] = tmp[: m - j]

        for j in range(0, Ns):
            V = matrix(spmatrix(1.0, Kl[j], range(len(Kl[j])), (n, len(Kl[j]))))
            trsm(L, V)
            trsm(L, V, trans="T")
            # compute column j of H (lower triangle only)
            kkl = matrix(0, (n, 1))
            for k1, k2 in enumerate(Kl[j]):
                kkl[k2] = k1
            misc.SCMcolumn2(H, Av, V, Ip, Jp, kkl, m - Ns + j)

        # Cholesky factorization: H = L*L'
        try:
            lapack.potrf(H)
        except ArithmeticError:
            print("\033[1;31m*** Factorization failed\033[0m")
            return {status: "unknown"}

        if scaling == "primal":
            kk = 1.0 / t
        else:
            kk = t

        def solve_(bx, by):
            """
            Solves the system:

              [ -kk*H  A' ][ x ] = [ bx ]
              [   A    0  ][ y ]   [ by ]

            """
            r1 = bx.copy()
            # hessian(L,Y,[r1])
            # hessian(L,Y,[r1],adj=True)
            hessian(L, Y, r1, inv=False, adj=None)
            y = kk * by + Amap(r1)
            lapack.potrs(H, y)

            x = Aadj(y) - bx
            # hessian(L,Y,[x])
            # hessian(L,Y,[x],adj=True)
            hessian(L, Y, [x], inv=False, adj=None)
            blas.scal(1.0 / kk, x.blkval)

            if DEBUG:
                r, rr = kkt_res(L, Y, x, y, bx, by)
                r1 = sqrt(dot(r, r)) / sqrt(dot(bx, bx))
                r2 = blas.nrm2(rr) / blas.nrm2(by)
                print("\033[1;33m KKTsolver: %.2e  %.2e \033[0m" % (r1, r2))
            return x, y

        return solve_

    if kktsolver == "chol":
        factor = kkt_chol
        SolvStr = "Cholesky"
        # Allocate storage for SCM
        H = matrix(0.0, (m, m))
        # Allocate storage for tmp
        if Ns < m:
            tmp = matrix(0.0, (m, 1))
    elif kktsolver == "qr":
        factor = kkt_qr
        SolvStr = "QR"
        # Convert A_1,...,A_m to chompack
        Ac = [cspmatrix(symb) + misc.Av_to_spmatrix(Av, Ip, Jp, j, n) for j in range(m)]
        # Allocate storage for At
        At = matrix(0.0, (len(Vp), m))
        # Allocate storage for tq
        tq = matrix(0.0, (m, 1))
    else:
        raise ValueError("Unknown 'kktsolver'.")

    def kktsolver(L, Y):
        return factor(L, Y)

    if scaling == "primal":

        def newton(sigma):
            rbz = bres(sigma)
            rtx, rty = tres(rbz)
            u1, u2 = f(rtx, rty)

            gamma = (-blas.dot(b, u2) + dot(C, u1)) / (
                1.0 / (t * tau**2) + blas.dot(b, v2) - dot(C, v1)
            )

            dy = u2 + gamma * v2
            dX = u1.copy() + gamma * v1

            dkappa = -rbz[2] + blas.dot(b, dy) - dot(C, dX)
            if bmax > 1e-5:
                dtau = (Amap(dX, ii) - rbz[0][ii]) / b[ii]  # OPTIMIZE!!
            else:
                dtau = (rbz[4] - dkappa) * t * tau**2

            # dS = Aadj(-dy)
            # axpy(C,dS,dtau)
            # axpy(rbz[1],dS,-1)
            dS = dX.copy()
            blas.scal(-1.0 / t, dS.blkval)
            # hessian(L,Y,[dS],inv=True,adj=True)
            # hessian(L,Y,[dS],inv=True)
            hessian(L, Y, dS, inv=True, adj=None)
            dS += rbz[3]

            for i in range(REFINEMENT):
                rbz = bres(sigma, dz=(dy, dX, dtau, dS, dkappa))
                rtx, rty = tres(rbz)
                u1, u2 = f(rtx, rty)

                gamma = (-blas.dot(b, u2) + dot(C, u1)) / (
                    1.0 / (t * tau**2) + blas.dot(b, v2) - dot(C, v1)
                )

                ddy = u2 + gamma * v2
                ddX = u1.copy() + gamma * v1

                ddkappa = -rbz[2] + blas.dot(b, ddy) - dot(C, ddX)
                if bmax > 1e-5:
                    ddtau = (Amap(ddX, ii) - rbz[0][ii]) / b[ii]  # OPTIMIZE!!
                else:
                    ddtau = (rbz[4] - ddkappa) * t * tau**2

                # ddS = Aadj(-ddy)
                # axpy(C,dS,ddtau)
                # axpy(rbz[1],ddS,-1.0)
                ddS = ddX.copy()
                blas.scal(-1.0 / t, ddS.blkval)
                # hessian(L,Y,[ddS],inv=True,adj=True)
                # hessian(L,Y,[ddS],inv=True)
                hessian(L, Y, [ddS], inv=True, adj=None)
                ddS += rbz[3]

                dy += ddy
                dX += ddX
                dtau += ddtau
                dS += ddS
                dkappa += ddkappa
            return (dy, dX, dtau, dS, dkappa)

    else:

        def newton(sigma):
            rbz = bres(sigma)
            rtx, rty = tres(rbz)
            u1, u2 = f(rtx, rty)

            gamma = (-blas.dot(b, u2) + dot(C, u1)) / (
                t * kappa**2 + blas.dot(b, v2) - dot(C, v1)
            )

            dy = u2 + gamma * v2
            dX = u1.copy() + gamma * v1

            dkappa = -rbz[2] + blas.dot(b, dy) - dot(C, dX)
            if bmax > 1e-5:
                dtau = (Amap(dX, ii) - rbz[0][ii]) / b[ii]  # OPTIMIZE!!
            else:
                dtau = rbz[4] - dkappa / (t * kappa**2)

            # dS = Aadj(-dy)
            # axpy(C,dS,dtau)
            # axpy(rbz[1],dS,-1.0)
            dS = rbz[3].copy() - dX
            blas.scal(t, dS.blkval)
            # hessian(L,Y,[dS],inv=True,adj=True)
            # hessian(L,Y,[dS],inv=True)
            hessian(L, Y, [dS], inv=True, adj=None)

            for i in range(REFINEMENT):
                rbz = bres(sigma, dz=(dy, dX, dtau, dS, dkappa))
                rtx, rty = tres(rbz)
                u1, u2 = f(rtx, rty)

                gamma = (-blas.dot(b, u2) + dot(C, u1)) / (
                    t * kappa**2 + blas.dot(b, v2) - dot(C, v1)
                )

                ddy = u2 + gamma * v2
                ddX = u1.copy() + gamma * v1

                ddkappa = -rbz[2] + blas.dot(b, ddy) - dot(C, ddX)
                if bmax > 1e-5:
                    ddtau = (Amap(ddX, ii) - rbz[0][ii]) / b[ii]  # OPTIMIZE!!
                else:
                    ddtau = rbz[4] - ddkappa / (t * kappa**2)

                # ddS = Aadj(-ddy)
                # axpy(C,ddS,ddtau)
                # axpy(rbz[1],ddS,-1.0)
                ddS = rbz[3].copy() - ddX
                blas.scal(t, ddS.blkval)
                # hessian(L,Y,[ddS],inv=True,adj=True)
                # hessian(L,Y,[ddS],inv=True)
                hessian(L, Y, [ddS], inv=True, adj=None)

                dy += ddy
                dX += ddX
                dtau += ddtau
                dS += ddS
                dkappa += ddkappa
            return (dy, dX, dtau, dS, dkappa)

    def print_head():
        print(
            "%-20s Extended self-dual embedding, %s scaling (%s)"
            % (__version__, scaling, SolvStr)
        )
        print(
            "----------------------------------------------------------------------------"
        )
        print("SDP var. size:       %i " % (n))
        print("Constraints:         %i (%i|%i)" % (m, m - Ns, Ns))
        if CHORDAL:
            ChStr = "Chordal"
        else:
            ChStr = "Nonchordal"
        print("Aggregate sparsity:  %-14s NNZ(tril(V)) = %7i" % (ChStr, len(Va)))
        if not CHORDAL:
            print("Embedding:           %-14s       NNZ(L) = %7i" % (ORDER, len(Vp)))
        print(
            "----------------------------------------------------------------------------"
        )
        print(
            " it  pcost       dcost      gap     pres    dres    k/t     step    cputime"
        )

    def print_exit_info():
        if status == "optimal" or status == "unknown":
            if pcost is not None:
                print("   Primal objective:                % .8e" % (pcost))
            if dcost is not None:
                print("   Dual objective:                  % .8e" % (dcost))
        if gap is not None:
            print("   Gap:                             % .8e" % (gap))
        if relgap is not None:
            print("   Relative gap:                    % .8e" % (relgap))
        if pres is not None:
            print("   Primal infeasibility:            % .8e" % (pres))
        if dres is not None:
            print("   Dual infeasibility:              % .8e" % (dres))
        if not iter == 0:
            print("   Iterations:                       %i" % (iter))
            print("   CPU time:                         %.2f" % (Tcpu))
            print("   CPU time per iteration:           %.2f" % (Tcpu / iter))
            print("   Real time:                        %.2f" % (Twall))
            print("   Real time per iteration:          %.2f\n" % (Twall / iter))
        if DIMACS:
            print(
                "   DIMACS:  %.2e %.2e %.2e %.2e %.2e %.2e"
                % tuple([v for v in DIMACS_err])
            )

    def linesearch(dX, dS, dtau, dkappa):
        """
        Backtracking linesearch:

        Computes step length t such that

           kappa + t*dkappa > 0, tau + t*dtau > 0,
           S + t*dS > 0, X + t*dX > 0
        """
        t = 1.0
        while (kappa + t * dkappa <= 0) or (tau + t * dtau <= 0):
            t *= BETA
            if t < MINSTEP:
                return None

        while True:
            Xt = X.copy() + t * dX
            try:
                Lt = Xt.copy()
                completion(Lt)
                break
            except ArithmeticError:
                t *= BETA
                if t < MINSTEP:
                    return None

        while True:
            St = S.copy() + t * dS
            try:
                Lt = St.copy()
                cholesky(Lt)
                break
            except ArithmeticError:
                t *= BETA
                if t < MINSTEP:
                    return None

        return t

    if show_progress:
        print_head()

    for iter in range(MAXITERS + 1):
        # RESIDUALS AND CONVERGENCE STATISTICS
        #

        # hry = A*x
        hry = Amap(X)
        # ry = b*tau - A*x
        ry = b * tau - hry

        # hrx = A'*y + s
        hrx = Aadj(y) + S
        # rx = A'*y + s - tau*C
        rx = hrx.copy() - tau * C

        # rt = kappa - b'*y + c'*x
        rt = kappa - blas.dot(b, y) + dot(C, X)

        resy = blas.nrm2(ry) / tau
        resx = sqrt(dot(rx, rx)) / tau
        pres = resy / resy0
        dres = resx / resx0

        cx = dot(C, X)
        pcost = cx / tau
        by = blas.dot(b, y)  # TODO: handle sparse b
        dcost = by / tau

        # compute gap
        gap = dot(X, S) / tau**2
        # compute relgap
        if pcost < 0.0:
            relgap = gap / -pcost
        elif dcost > 0.0:
            relgap = gap / dcost
        else:
            relgap = None

        # compute pinfres
        if -by < 0.0:
            hresx = sqrt(dot(hrx, hrx))
            pinfres = hresx / resx0 / by
        else:
            pinfres = None

        # compute dinfres
        if cx < 0.0:
            hresy = blas.nrm2(hry)
            dinfres = hresy / resy0 / (-cx)
        else:
            dinfres = None

        if show_progress:
            if not iter == 0:
                print(
                    "%3d % .4e % .4e %.1e %.1e %.1e %.1e %.1e %7.1f"
                    % (
                        iter,
                        pcost,
                        dcost,
                        gap,
                        pres,
                        dres,
                        kappa / tau,
                        step,
                        process_time() - T0,
                    )
                )
            else:
                print(
                    "%3d % .4e % .4e %.1e %.1e %.1e %.1e         %7.1f"
                    % (iter, pcost, dcost, gap, pres, dres, kappa / tau, process_time() - T0)
                )

        # Stopping criteria
        #
        if (
            (dres <= FEASTOL)
            and (pres <= FEASTOL)
            and (gap <= ABSTOL or (relgap is not None and relgap <= RELTOL))
        ):
            # Optimal
            if show_progress:
                print("Optimal solution found.")
            status = "optimal"
            break
        elif pinfres is not None and pinfres <= FEASTOL:
            # Primal infeasible
            if show_progress:
                print("Certificate of primal infeasibility found.")
            status = "primal infeasibility"
            X = None
            pcost = None
            dcost = 1
            gap = None
            relgap = None
            pres = None
            dres = None
            dinfres = None
            break
        elif dinfres is not None and dinfres <= FEASTOL:
            # Dual infeasible
            if show_progress:
                print("Certificate of dual infeasibility found.")
            status = "dual infeasibility"
            y = None
            S = None
            pcost = -1
            dcost = None
            gap = None
            relgap = None
            pres = None
            dres = None
            pinfres = None
            break
        elif iter == MAXITERS:
            # Max. number of iterations reached
            if show_progress:
                print(
                    "\033[1;31mTerminated "
                    "(maximum number of iterations reached).\033[0m"
                )
            status = "unknown"
            break

        # compute new t
        t = (n + 1) / (gap * tau**2 + kappa * tau)

        if scaling == "primal":
            try:
                L = X.copy()
                completion(L)
            except ArithmeticError:
                if show_progress:
                    print("\033[1;31m*** Completion failed\033[0m")
                status = "unknown"
                break
            Y = X
        else:
            try:
                L = S.copy()
                cholesky(L)
            except ArithmeticError:
                if show_progress:
                    print("\033[1;31m*** Factorization of S failed\033[0m")
                status = "unknown"
                break
            Y = L.copy()
            projected_inverse(Y)

        f = kktsolver(L, Y)
        if f is None:
            break
        v1, v2 = f(C, b)

        # Solve for aff. scaling direction
        dy, dX, dtau, dS, dkappa = newton(0.0)
        if DEBUG:
            newton_res(0.0)
        # Compute step length
        step = linesearch(dX, dS, dtau, dkappa)
        if not step:
            if show_progress:
                print("\033[1;31mTerminated " "(small step size detected).\033[0m")
            status = "unknown"
            break

        # Compute sigma
        St = S.copy() + step * dS
        Xt = X.copy() + step * dX
        taut = tau + step * dtau
        kappat = kappa + step * dkappa
        sigma = ((dot(Xt, St) + taut * kappat) / (gap * tau**2 + kappa * tau)) ** EXPON

        # compute newton direction
        dy, dX, dtau, dS, dkappa = newton(sigma)
        if DEBUG:
            newton_res(sigma)
        # Compute step length
        step = linesearch(dX, dS, dtau, dkappa)
        if not step:
            if show_progress:
                print("\033[1;31mTerminated " "(small step size detected).\033[0m")
            break

        # Update (X,y,S)
        X += STEP * step * dX
        y += STEP * step * dy
        S += STEP * step * dS
        tau += STEP * step * dtau
        kappa += STEP * step * dkappa
        if DEBUG:
            print(
                "\033[1;31m mu=%.2e  tau=%.2e  kappa=%.2e  sigma=%.2e \033[0m"
                % (1.0 / t, tau, kappa, sigma)
            )

    # END OF ITERATION LOOP
    #

    # Get cputime and realtime
    Tcpu = process_time() - T0
    Twall = perf_counter() - T0wall

    if DIMACS and (X is not None) and (y is not None) and (S is not None):
        DIMACS_err = matrix(0.0, (6, 1))
        DIMACS_err[0] = blas.nrm2(Amap(X) / tau - b) / (1 + max(abs(b)))
        DIMACS_err[1] = 0.0
        R = Aadj(y) + S
        blas.scal(1.0 / tau, R.blkval)
        R -= C
        c_abs = abs(c)
        if len(c_abs) == 0:
            c_abs = [0.0]
        DIMACS_err[2] = sqrt(dot(R, R)) / (1 + max(c_abs))
        DIMACS_err[3] = 0.0
        DIMACS_err[4] = (pcost - dcost) / (1 + abs(pcost) + abs(dcost))
        DIMACS_err[5] = gap / (1 + abs(pcost) + abs(dcost))
    else:
        DIMACS = False
        DIMACS_err = None

    # Scale and convert to CVXOPT
    if X is not None:
        blas.scal(1.0 / tau, X.blkval)
        X = X.spmatrix(reordered=False, symmetric=False)
        X = perm(symmetrize(X), ip)
    if y is not None:
        y = y[iperm(pm)]
        y /= tau
    if S is not None:
        blas.scal(1.0 / tau, S.blkval)
        S = S.spmatrix(reordered=False, symmetric=False)
        S = perm(symmetrize(S), ip)

    if show_progress:
        print_exit_info()

    return {
        "status": status,
        "x": X,
        "y": y,
        "s": S,
        "primal objective": pcost,
        "dual objective": dcost,
        "gap": gap,
        "relative gap": relgap,
        "primal infeasibility": pres,
        "dual infeasibility": dres,
        "residual as primal infeasibility certificate": pinfres,
        "residual as dual infeasibility certificate": dinfres,
        "iterations": iter,
        "cputime": Tcpu,
        "time": Twall,
    }


def conelp(c, G, h, dims=None, kktsolver="chol"):
    from smcp import SDP, misc
    from cvxopt import matrix, sparse, spdiag, spmatrix
    import chompack

    Nl = dims["l"]
    Nq = dims["q"]
    Ns = dims["s"]
    if not Nl:
        Nl = 0

    P = SDP()

    P._n = Nl + sum(Nq) + sum(Ns)
    P._m = G.size[1]

    P._A = spmatrix([], [], [], (P._n**2, P._m + 1))
    P._b = -c
    P._blockstruct = []
    if Nl:
        P._blockstruct.append(-Nl)
    for i in Nq:
        P._blockstruct.append(i)
    for i in Ns:
        P._blockstruct.append(i)

    for k in range(P._m + 1):
        if not k == 0:
            v = sparse(G[:, k - 1])
        else:
            v = sparse(h)
        B = []

        ptr = 0
        # lin. constraints
        if Nl:
            u = v[:Nl]
            Il = u.I
            B.append(spmatrix(u.V, Il, Il, (Nl, Nl)))
            ptr += Nl

        # SOC constraints
        for i in range(len(Nq)):
            nq = Nq[i]
            u0 = v[ptr]
            u1 = v[ptr+1:ptr+nq]
            tmp = spmatrix(u1.V, [nq - 1 for j in range(len(u1))], u1.I, (nq, nq))
            if not u0 == 0.0:
                tmp += spmatrix(u0, range(nq), range(nq), (nq, nq))
            B.append(tmp)
            ptr += Nq[i]

        # SDP constraints
        for i in range(len(Ns)):
            ns = Ns[i]
            u = v[ptr:ptr+ns**2]
            I, J = misc.ind2sub(ns, u.I)
            tmp = chompack.tril(spmatrix(u.V, I, J, (ns, ns)))
            B.append(tmp)
            ptr += ns**2

        Ai = spdiag(B)
        P._A[:, k] = Ai[:]

    P._agg_sparsity()
    sol = P.solve_esd(kktsolver=kktsolver)

    x = sol["x"]
    s = sol["s"]
    n = P.n

    # recover primal and dual variables
    N = 0
    if Nl:
        xl = x[:n*Nl+1:n+1]
        N += Nl
    else:
        xl = matrix([])

    xq = []
    if Nq:
        for i in Nq:
            xq.append(
                matrix(
                    [
                        sum(x[(n+1)*N:(n+1)*(N+i):n+1]),
                        2 * x[N+i-1, N:N+i-1].T,
                    ]
                )
            )
            N += i
    else:
        xq = matrix([])

    xs = []
    if Ns:
        for i in Ns:
            xs.append(x[N:N+i, N:N+i])
            N += i
    else:
        xs = [matrix([])]

    N = 0
    if Nl:
        sl = s[:n*Nl+1:n+1]
        N += Nl
    else:
        sl = matrix([])

    sq = []
    if Nq:
        for i in Nq:
            sq.append(matrix([s[N + i - 1, N + i - 1], s[N + i - 1, N:N + i - 1].T]))
            N += i
    else:
        sq = matrix([])

    ss = []
    if Ns:
        for i in Ns:
            ss.append(s[N:N+i, N:N+i])
            N += i
    else:
        ss = [matrix([])]

    sol["x"] = sol.pop("y")
    sol["z"] = sparse([xl, sparse(xq), sparse([z[:] for z in xs])])
    sol["s"] = sparse([sl, sparse(sq), sparse([z[:] for z in ss])])

    return sol


def lp(c, G, h, kktsolver="chol"):
    return conelp(c, G, h, dims={"l": G.size[0], "q": [], "s": []}, kktsolver=kktsolver)


def socp(c, Gl=None, hl=None, Gq=None, hq=None, kktsolver="chol"):
    from cvxopt import sparse

    dims = {"l": 0, "q": [], "s": []}
    G = []
    h = []
    if Gl is not None and hl is not None:
        dims["l"].append(Gl.size[0])
        G.append(Gl)
        h.append(hl)
    if Gq is not None and hq is not None:
        for j in range(len(Gq)):
            dims["q"].append(Gq[j].size[0])
            G.append(Gq[j])
            h.append(hq[j])
    else:
        raise ValueError("'Gq' and 'hq' cannot be zero")

    sol = conelp(c, sparse(G), sparse(h), dims=dims, kktsolver=kktsolver)

    Nl = dims["l"]
    N = 0
    if Nl:
        sol["zl"] = sol["z"][N:Nl]
        sol["sl"] = sol["s"][N:Nl]
        N += Nl
    else:
        sol["zl"] = None
        sol["sl"] = None
    zq = []
    sq = []

    for ns in dims["q"]:
        zq += [sol["z"][N:N+ns]]
        sq += [sol["s"][N:N+ns]]
        N += ns

    sol["zq"] = zq
    sol["sq"] = sq
    sol.pop("s")
    sol.pop("z")

    return sol


def sdp(c, Gl=None, hl=None, Gs=None, hs=None, kktsolver="chol"):
    from cvxopt import sparse, spmatrix, sqrt
    from smcp import misc

    dims = {"l": 0, "q": [], "s": []}
    G = []
    h = []
    if Gl is not None and hl is not None:
        dims["l"] = Gl.size[0]
        G.append(Gl)
        h.append(hl)
    if Gs is not None and hs is not None:
        for j in range(len(Gs)):
            dims["s"].append(int(sqrt(Gs[j].size[0])))
            G.append(Gs[j])
            h.append(hs[j][:])
    else:
        raise ValueError("'Gs' and 'hs' cannot be zero")

    sol = conelp(c, sparse(G), sparse(h), dims=dims, kktsolver=kktsolver)

    Nl = dims["l"]
    N = 0
    if Nl:
        sol["zl"] = sol["z"][N:Nl]
        sol["sl"] = sol["s"][N:Nl]
        N += Nl
    else:
        sol["zl"] = None
        sol["sl"] = None
    zs = []
    ss = []

    for ns in dims["s"]:
        u = sol["z"][N:N+ns**2]
        I, J = misc.ind2sub(ns, u.I)
        zs += [spmatrix(u.V, I, J, (ns, ns))]

        u = sol["s"][N:N+ns**2]
        I, J = misc.ind2sub(ns, u.I)
        ss += [spmatrix(u.V, I, J, (ns, ns))]
        N += ns**2

    sol["zs"] = zs
    sol["ss"] = ss
    sol.pop("s")
    sol.pop("z")

    return sol


__all__ = [
    "chordalsolver_feas",
    "chordalsolver_esd",
    "lp",
    "socp",
    "sdp",
    "conelp",
    "options",
]
