import numpy as np
import matplotlib.pyplot as plt
import CBFdata
import scipy.sparse as sp
import scipy.optimize as scopt
import cd_solver  # can be downloaded from https://bitbucket.org/ofercoq/cd_solver/src/master/
import cvxpy
import copy
import time
from numba import njit
import os
import pdb

def norm(M):
    if type(M) == np.ndarray or type(M) == np.matrix:
        return np.linalg.norm(M, 2)
    else:
        return sp.linalg.norm(M, 2)


def F_star_M(z, beta, F, prox_F, M, n, hessian=None):
    if type(beta) == list:
        beta = np.array(beta)
    if type(beta) != np.ndarray:
        beta = beta * np.ones(2)
    beta_ = np.zeros(z.shape)
    beta_[:n] = beta[0]
    beta_[n:] = beta[1]
    zbeta_z = prox_F(z + 1./beta_ * (M @ z), 1/beta, n)
    val = - F(zbeta_z) + zbeta_z @ M @ z - (beta_/2 * (z-zbeta_z)) @ (z - zbeta_z)
    grad = -M @ zbeta_z + beta_ * (zbeta_z - z)
    if hessian is None:
        return val, grad
    else:
        H = -M  # approximation when hessian = 0
        # beta_diags = sp.csc_matrix(sp.diags(beta_))
        # dprox_dz = sp.linalg.spsolve(beta_diags + hessian(zbeta_z), beta_diags)
        # H = (-M+beta_diags) @ dprox_dz - beta_diags
        return val, grad, H

def grad_cbeta(z, beta, prox_F, M, n):
    beta_ = np.zeros(z.shape)
    beta_[:n] = beta[0]
    beta_[n:] = beta[1]
    zbeta_z = prox_F(z_star + 1/beta_ * (M @ z), 1/beta)
    return -M @ zbeta_z

def G(z, beta, F, prox_F, M, n):
    if type(beta) == list:
        beta = np.array(beta)
    beta_ = np.zeros(z.shape)
    beta_[:n] = beta[0]
    beta_[n:] = beta[1]
    zbeta_z = prox_F(z + 1./beta_ * (M @ z), 1/beta, n)
    val = F(z) - F(zbeta_z) + zbeta_z @ M @ z - (beta_/2 * (z-zbeta_z)) @ (z - zbeta_z)
    return val

def G_star(z, beta, F, prox_F, M, n):
    if type(beta) == list:
        beta = np.array(beta)
    beta_ = np.zeros(z.shape)
    beta_[:n] = beta[0]
    beta_[n:] = beta[1]
    zbeta_z = prox_F(z_star + 1/beta_ * (M @ z), 1/beta, n)
    return F(z) - F(zbeta_z) + zbeta_z @ M @ z - (beta_/2 * (z_star-zbeta_z)) @ (z - zbeta_z)

def Fbeta(z, beta, zdot, F_star, prox_F, M, n):
    # F_beta(z, zdot) where zdot = -grad_Fsbeta(z0)
    if type(beta) == list:
        beta = np.array(beta)
    beta_ = np.zeros(z.shape)
    beta_[:n] = beta[0]
    beta_[n:] = beta[1]
    # zbeta = prox_Fstar(zdot + z/beta_, beta)
    # prox_Fstar(x, gamma) = x - gamma prox_F(x/gamma, 1/gamma)
    zbeta = zdot + z/beta_ - (1./beta_) * prox_F(zdot * beta_ + z, beta, n)
    val = - F_star(zbeta, n) + z @ zbeta - ((zbeta - zdot) * beta_) @ (zbeta - zdot) / 2
    grad = zbeta
    return val, grad

beta_print = [0.00001, 0.00001]

def PDHG_1(z, tau, F, prox_F, M, normA, n):
    sigma = 1./normA**2 / tau
    z_ = prox_F(z + tau * M @ z, np.array([tau, sigma]), n)
    z_extra = z.copy()
    z_extra[:n] = 2 * z_[:n] - z[:n]
    z__ = prox_F(z + sigma * M @ z_extra, np.array([tau, sigma]), n)
    z__[:n] = z_[:n]
    return z__

def pdhg(z0, max_iter, F, prox_F, M, n, verbose=1.):
    GG_cp = np.zeros(max_iter)
    z_cp = z0.copy()
    normA = norm(M)
    tau = 1./normA
    sigma = 1./normA**2 / tau
    last_print_time = time.time()
    for k in range(max_iter):
        z_cp = PDHG_1(z_cp, tau, F, prox_F, M, normA, n)
        GG_cp[k] = G(z_cp, beta_print, F, prox_F, M, n)
        if GG_cp[k] < -1:
            break
        if verbose:
            time_now = time.time()
            if time_now > last_print_time + verbose: 
                print('iter', k, 'SDG', GG_cp[k])
                last_print_time = time_now
    return z_cp, GG_cp

def rapdhg(z0, max_iter, F, prox_F, M, n, verbose=1.):
    GG_cp = np.zeros(max_iter)
    z_cp = z0.copy()
    z_a = z0.copy()
    normA = norm(M)
    tau = 1/normA
    sigma = 1./normA**2 / tau
    k0 = 0
    last_print_time = time.time()
    for k in range(max_iter):
        z_cp = PDHG_1(z_cp, tau, F, prox_F, M, normA, n)
        z_a = 1./(k-k0+1)*z_cp + (k-k0)/(k-k0+1) * z_a
        GG_cp[k] = min([G(z_cp,beta_print, F, prox_F, M, n), G(z_a, beta_print, F, prox_F, M, n)])
        if GG_cp[k] < 0.5 * GG_cp[k0]:
            if G(z_cp,beta_print, F, prox_F, M, n) > G(z_a, beta_print, F, prox_F, M, n):
                z_cp = z_a.copy()
                print('restart at z_a at iteration', k0)
            else:
                print('restart at z_cp at iteration', k0)
            k0 = k
        if verbose:
            time_now = time.time()
            if time_now > last_print_time + verbose: 
                print('iter', k, 'SDG', GG_cp[k])
                last_print_time = time_now
    return z_cp, GG_cp

def asgard(z0, max_iter, F, prox_F, M, n, verbose=1.):
    # we run in parallel the primal and dual asgard in order to guarantee a convergence of the primal and dual sequences
    LM = norm(M)**2
    beta = 0.5 * np.sqrt(LM) * np.ones(2)
    beta_ = beta[0] * np.ones(len(z0))
    tau = 1
    zhat = z0.copy()
    zbar = zhat.copy()
    zdot = 0*z0
    GG = np.zeros(max_iter)
    last_print_time = time.time()
    for k in range(max_iter):
        tau_ = tau
        pol = np.polynomial.Polynomial([-tau**2, tau**2, 1, 1])
        r = pol.roots()
        r = np.real(r[np.abs(np.imag(r)) < 1e-10])
        tau = np.max(r)
        zs = prox_F(zdot + 1/beta_ * (M @ zhat), 1/beta, n)
        zbar_ = zbar.copy()
        zbar = prox_F(zhat + beta_/LM * (M @ zs), beta/LM, n)
        zhat = zbar + tau * (1-tau_) / tau_ * (zbar - zbar_)
        beta = beta / (1+tau)
        beta_[:] = beta[0]
        GG[k] = G(zbar, beta_print, F, prox_F, M, n)
        if verbose:
            time_now = time.time()
            if time_now > last_print_time + verbose: 
                print('iter', k, 'ASGARD', GG[k], 'tau', tau, 'beta', beta[0])
                last_print_time = time_now
    return zbar, GG
    

def proximal_gradient_sg(z0, max_iter, F, prox_F, M, verbose=1.):
    normM = norm(M)
    GG = np.zeros(max_iter)
    z = z0.copy()
    delta_p = 1/2/np.log(max_iter)
    p = 0.5 + delta_p
    b = 1 / ((3/2)**p - 1)
    last_print_time = time.time()
    for k in range(max_iter):
        beta = normM * np.sqrt(2*delta_p/(b+2*delta_p))/(k/b+1)**p * np.ones(2)
        gamma = beta / (normM**2 + 2 * beta**2)
        gamma_ = gamma[0] * np.ones(z.shape)
        z = prox_F(z - gamma_ * F_star_M(z, beta, F, prox_F, M, n)[1], gamma, n)
        GG[k] = G(z, beta_print, F, prox_F, M, n)
        if verbose:
            time_now = time.time()
            if time_now > last_print_time + verbose: 
                print('iter', k, 'SDG', GG[k])
                last_print_time = time_now
    return z, GG

def acc_prox_grad_sg(z0, max_iter, F, F_star, prox_F, M, n, beta0, verbose=1., lbfgs_addon=0, z_star=None):
    ITERS = np.zeros(max_iter, dtype=int)
    GG = np.zeros(max_iter)
    GG_algo = np.zeros(max_iter)
    GG_algo_beta = np.zeros(max_iter)
    LL = np.zeros((max_iter, 4))
    normM = norm(M)
    z = z0.copy()
    z_hat = z0.copy()
    z_bar = z0.copy()
    t = 2
    b = t
    k0 = 0
    s = 0.
    if beta0[0] == 0:
        cbar = 0.25
        beta0 = normM * np.sqrt(cbar) / b * np.ones(2)
    gamma = np.zeros(2)
    gamma_ = np.zeros(z.shape)

    num_iter_inner = 10
    last_print_time = time.time()
    for k in range(max_iter):
        z_ = z.copy()
        beta = beta0 / ((k-k0)/b+1)
        theta = t / (k+t-k0)
        gamma[0] = beta[1] / (normM**2 + 2 * beta[0]*beta[1])
        gamma[1] = beta[0] / (normM**2 + 2 * beta[0]*beta[1])
        gamma_[:n] = gamma[0]
        gamma_[n:] = gamma[1]
        z_hat = (1-theta) * z + theta * z_bar
        z_bar = prox_F(z_bar - gamma_ / theta * F_star_M(z_hat, beta, F, prox_F, M, n)[1], gamma / theta, n)
        z = (1-theta) * z + theta * z_bar
        GG[k] = G(z, beta_print, F, prox_F, M, n)
        GG_algo[k] = G(z, beta0, F, prox_F, M, n)
        GG_algo_beta[k] = G(z, beta, F, prox_F, M, n)
        if k > 0:
            ITERS[k] = ITERS[k-1] + 2
            if z_star is not None and k > k0:
                beta_m1 = beta0 / ((k-k0-1)/b+1)
                beta_m1_ = gamma_.copy()
                beta_m1_[:n] = beta_m1[0]
                beta_m1_[n:] = beta_m1[1]
                theta_ = t / (k+t-k0-1)
                gamma_m1 = gamma.copy()
                gamma_m1_ = gamma_.copy()
                gamma_m1[0] = beta_m1[1] / (normM**2 + 2 * beta_m1[0]*beta_m1[1])
                gamma_m1[1] = beta_m1[0] / (normM**2 + 2 * beta_m1[0]*beta_m1[1])
                gamma_m1_[:n] = gamma_m1[0]
                gamma_m1_[n:] = gamma_m1[1]
                
                LL[k, 0] = (k-k0+b-2)/(k-k0+b-1) * G(z, beta, F, prox_F, M, n) + 0.5 * (beta_m1_ * (z - z_star)) @ (z-z_star) + 0.5 * ((theta_**2 / gamma_m1_ - beta_m1_ * theta_) * (z_bar - z_star)) @ (z-z_star)
                LL[k, 1] = (k-k0+b-2)/(k-k0+b-1) * G(z, beta, F, prox_F, M, n)
                LL[k, 2] = 0.5 * (beta_m1_ * (z - z_star)) @ (z-z_star)
                LL[k, 3] = 0.5 * ((theta_**2 / gamma_m1_ - beta_m1_ * theta_) * (z_bar - z_star)) @ (z_bar-z_star)
        if GG_algo[k] < 0.5**(s+1) * GG_algo[0]:
            num_iter_inner = max([10, k - k0])
            k0 = k+1
            s = s+1
            z_bar = z.copy()
        if GG_algo[k] < 1e-13 and k - k0 > 10:
            # In the end, GG_algo is not informative any more:
            #   we make it larger by decreasing beta0
            print('beta0', beta0)
            beta0 /= 2
            num_iter_inner = max([10, k - k0])
            k0 = k+1
            s = s+1
            z_bar = z.copy()
        if verbose:
            time_now = time.time()
            if time_now > last_print_time + verbose: 
                print('iter', k, 'SDG', GG[k], 'primal_res', norm(z[:n]-z_[:n])/gamma[0] , 'dual_res', norm(z[n:]-z_[n:])/gamma[1], 'SDG_algo', GG_algo[k], '>', 0.5**(s+1) * GG_algo[0])
                last_print_time = time_now
        if lbfgs_addon > 0 and ((k0 == k+1) or (k-k0 == 2 * num_iter_inner)):
            # if k-k0 == 2 * num_iter_inner:
            #    print('anticipated lbfgs step', num_iter_inner)
            #    num_iter_inner *= 2
            zdot = -F_star_M(z, beta, F, prox_F, M, n)[1]
            def SSG(z):
                delta = beta / 100
                if lbfgs_addon == 1:
                    val1, grad1 = Fbeta(z, delta, zdot, F_star, prox_F, M, n)
                    val2, grad2 = F_star_M(z, beta, F, prox_F, M, n)
                elif lbfgs_addon == 2:
                    zdot2 = -F_star_M(z, beta, F, prox_F, M, n)[1]
                    val1, grad1 = Fbeta(z, delta, zdot2, F_star, prox_F, M, n)
                    val2, grad2 = F_star_M(z, beta, F, prox_F, M, n)
                return val1+val2
            
            def grad_SSG(z):
                delta = beta / 100
                if lbfgs_addon == 1:
                    val1, grad1 = Fbeta(z, delta, zdot, F_star, prox_F, M, n)
                    val2, grad2 = F_star_M(z, beta, F, prox_F, M, n)
                elif lbfgs_addon == 2:
                    # the true gradient is (I+delta H_star_M)(grad1+grad2) but F_star_M is usually not twice differentiable.
                    zdot2 = -F_star_M(z, beta, F, prox_F, M, n)[1]
                    val1, grad1 = Fbeta(z, delta, zdot2, F_star, prox_F, M, n)
                    val2, grad2 = F_star_M(z, beta, F, prox_F, M, n)
                return grad1+grad2

            res = scopt.minimize(fun=SSG, jac=grad_SSG, x0=z, method='L-BFGS-B',
                                 tol=min([1e-10, GG_algo_beta[k]/10]), options={'maxfun':num_iter_inner})
            ITERS[k] += 2 * res.njev
            gamma_ = np.zeros(z.shape)
            gamma_[:n] = gamma[0]
            gamma_[n:] = gamma[1]
            z_lbfgs = prox_F(res.x - gamma_ * F_star_M(res.x, beta, F, prox_F, M, n)[1], gamma, n)
            G_lbfgs = G(z_lbfgs, beta0, F, prox_F, M, n)
            G_lbfgs_beta = G(z_lbfgs, beta, F, prox_F, M, n)
            
            print('lbfgs', 'ssgx', SSG(res.x), 'ssgz', SSG(z_lbfgs), 'n', norm(res.x-z_lbfgs), 'ng', norm(grad_SSG(res.x)), 'gz', G_lbfgs, GG_algo[k], 'k', k, 'it', ITERS[k])
            if (G_lbfgs < GG_algo[k] and k0 == k+1) or (G_lbfgs_beta < GG_algo_beta[k]):
                z = z_lbfgs.copy()
                if k0 == k+1:
                    z_bar = z_lbfgs.copy()
                GG_algo[k] = G_lbfgs
                GG_algo_beta[k] = G_lbfgs_beta
                GG[k] = G(z, beta_print, F, prox_F, M, n)
                print('lbfgs step is a success')
            else:
                print('lbfgs step is not a success')
        if ITERS[k] > max_iter:
            break

    GG = GG[:k+1]
    GG_algo = GG[:k+1]
    ITERS = ITERS[:k+1]
    LL = LL[:k+1, :]
    if z_star is not None:
        return z, GG, GG_algo, ITERS, LL
    return z, GG, GG_algo, ITERS

def lbfgs_proxgrad(z0, max_iter, F, F_star, prox_F, M, n, beta0, verbose=1.):
    normM = norm(M)
    beta = beta0
    it = 0
    ITERS = []
    GG = []
    GG_lbfgs_beta = []
    ITERS.append(it)
    while it < max_iter:
        def SSG(z):
            delta = beta / 100
            zdot = -F_star_M(z, beta, F, prox_F, M, n)[1]
            val1, grad1 = Fbeta(z, delta, zdot, F_star, prox_F, M, n)
            val2, grad2 = F_star_M(z, beta, F, prox_F, M, n)
            return val1+val2
        
        def grad_SSG(z):
            delta = beta / 100
            # the true gradient is (I+delta H_star_M)(grad1+grad2) but F_star_M is usually not twice differentiable.
            zdot = -F_star_M(z, beta, F, prox_F, M, n)[1]
            val1, grad1 = Fbeta(z, delta, zdot, F_star, prox_F, M, n)
            val2, grad2, hess2 = F_star_M(z, beta, F, prox_F, M, n, hessian=lambda z:sp.csc_matrix((len(z), len(z))))
            return (sp.csc_matrix(sp.diags(np.ones(len(zdot)))) + hess2) @ (grad1+grad2)

        res = scopt.minimize(fun=SSG, jac=grad_SSG, x0=z, method='L-BFGS-B',
                             tol=min([1e-10]), options={'maxfun':(max_iter - it)//2})
        it += 2 * res.njev
        gamma = beta / (normM**2 + 2 * beta**2)
        gamma_ = gamma[0] * np.ones(z.shape)
        z_lbfgs = prox_F(res.x - gamma_ * F_star_M(res.x, beta, F, prox_F, M, n)[1], gamma, n)
        ITERS.append(it)
        GG.append(G(z_lbfgs, beta_print, F, prox_F, M, n))
        GG_lbfgs_beta.append(G(z_lbfgs, beta, F, prox_F, M, n))

        print(len(ITERS), ITERS[-1], GG[-1], GG_lbfgs_beta[-1], beta, norm(grad_SSG(res.x)))
        
        if GG_lbfgs_beta[-1] > 1e-10:
            beta /= 2

    return z_lbfgs, GG, GG_lbfgs_beta, ITERS


do_toy_problem = True
if do_toy_problem:
    # min c.dot(x) : x >= 0, Ax <= b, d = 3, n = 4
    d = 3
    n = 4
    A = np.array([[2,4,5,7], [1,1,2,2], [1,2,3,3]])
    c = -np.array([7,9,18,17])
    b = np.array([41,17,24])
    cb = np.concatenate([c, b])

    M = np.vstack([np.hstack([np.zeros((n,n)), -A.T]),
                   np.hstack([A, np.zeros((d,d))])])
    normM = norm(M)
    normA = norm(A)

    def proj_Ystar(y):
        # Ystar = {[y1, 3-y1, 4-y1], y1 \in [0,3]}
        y0_star = np.array([0, 3, 4])
        y1_star = np.array([3, 0, 1])
        ll = -(y - y0_star).dot(y0_star - y1_star) / norm(y0_star - y1_star)**2
        ll = max(0, min(1, ll))
        return ll * y1_star + (1-ll) * y0_star
        
    x_star = np.array([3, 0, 7, 0])
    z_star = np.zeros(d+n)
    z_star[:n] = x_star
    z_star[n:] = proj_Ystar(z_star[n:])

    def F(z):
        return np.sum(z<0)*1e10 + cb @ z

    def F_star(z, n):
        zmcb = z - cb
        return np.sum(zmcb<0)*1e10
        
    def prox_F(z, gamma, n):
        if type(gamma) != np.ndarray and type(gamma) != list:
            gamma = [gamma, gamma]
        p = z.copy()
        p[:n] -= gamma[0] * cb[:n]
        p[n:] -= gamma[1] * cb[n:]
        return np.maximum(0, p)

    max_iter = 5000
    z0 = 0*z_star.copy()

    print('pdhg')
    z_cp, GG_cp = pdhg(z0, max_iter, F, prox_F, M, n)
    print('rapdhg')
    z_racp, GG_racp = rapdhg(z0, max_iter, F, prox_F, M, n)
    print('asgard')
    z_asg, GG_asg = asgard(z0, max_iter//2, F, prox_F, M, n)
    print('prox_grad_sg')
    z, GG = proximal_gradient_sg(z0, max_iter//2, F, prox_F, M, n)
    print('acc_prox_grad_sg')
    z_ac, GG_ac, GG_algo, ITERS = acc_prox_grad_sg(z0, max_iter, F, F_star, prox_F, M, n, np.zeros(2))
    print('acc_prox_grad_sg_lbgfs_addon')
    z_ac_bfgs, GG_ac_bfgs, GG_algo_bfgs, ITERS_bfgs = acc_prox_grad_sg(z0, max_iter//2, F,
                                                                       F_star, prox_F, M, n, np.zeros(2), lbfgs_addon=1)
    z_ac_bfgs2, GG_ac_bfgs2, GG_algo_bfgs2, ITERS_bfgs2 = acc_prox_grad_sg(z0, max_iter//2, F,
                                                                       F_star, prox_F, M, n, np.zeros(2), lbfgs_addon=2)

    plt.semilogy(GG_cp, ':'); plt.semilogy(GG_racp, '-.');
    plt.semilogy(np.arange(0, max_iter, 2), GG, '--'); plt.semilogy(ITERS, GG_ac, linestyle=(0,(5,10)));
    plt.semilogy(ITERS_bfgs, GG_ac_bfgs)
    # plt.semilogy(ITERS_bfgs2, GG_ac_bfgs2)
    plt.semilogy(np.arange(0, max_iter, 2), GG_asg, linestyle=(0, (10, 5)));
    plt.legend(['pdhg', 'rapdhg', 'prox_grad', 'acc_prox_grad', 'acc_prox_grad_lbfgs', 'asgard'])  # 'bfgs2'])
    plt.ylabel('self-centered smoothed gap')
    plt.xlabel('proximal gradient evaluations')
    plt.xlim([0, max_iter])
    ylim = plt.ylim()
    plt.ylim([1e-13, ylim[1]])
    plt.show()

def read_through_pythonapi(cbfdata):

    # Append row, columns and non-zeros
    m = cbfdata.mapnum
    N = cbfdata.varnum

    # Objective sense: MIN

    # Objective (cx + c0)
    c0 = cbfdata.objbval
    c = sp.csc_matrix((cbfdata.objaval, (cbfdata.objasubj,
                       np.zeros(len(cbfdata.objasubj)))),
                      shape=(N, 1)).toarray().ravel()

    # Constraints (Ax - b)
    b = -sp.csc_matrix((cbfdata.bval, (cbfdata.bsubi,
                       np.zeros(len(cbfdata.bsubi)))),
                      shape=(m, 1)).toarray().ravel()
    A = sp.csc_matrix((cbfdata.aval, (cbfdata.asubi, cbfdata.asubj)),
                      shape=(m,N))

    if cbfdata.varstacknum == 1 and cbfdata.varstackdomain[0] == 'F':
        g = None
        blocks = None
    else:
        blocks = [0]
        g = []
        j = 0
        for k in range(cbfdata.varstacknum):
            blocks.append(j + cbfdata.varstackdim[k])
            if cbfdata.varstackdomain[k] == 'Q':
                g.append('second_order_cone')
            elif cbfdata.varstackdomain[k] == 'QR':
                print('Not implemented')
            elif cbfdata.varstackdomain[k] == 'L+':
                g.append('ineq_const')
            else:
                g.append('zero')
            j = j + cbfdata.varstackdim[k]

    blocks_h = [0]
    h = []
    j = 0
    for k in range(cbfdata.mapstacknum):
        blocks_h.append(j + cbfdata.mapstackdim[k])
        if cbfdata.mapstackdomain[k] == 'Q':
            h.append('second_order_cone')
        elif cbfdata.mapstackdomain[k] == 'QR':
            print('Not implemented')
        elif cbfdata.mapstackdomain[k] == 'L=':
            h.append('eq_const')
        elif cbfdata.mapstackdomain[k] == 'L+':
            h.append('ineq_const')
        else:
          print('Not implemented')
        j = j + cbfdata.mapstackdim[k]

    problem = cd_solver.Problem(N, h=h, blocks_h=blocks_h, Ah=A, bh=b,
                                f=["linear"], Af=c, bf=[-c0],
                                g=g, blocks=blocks)
    return problem

"""
def mps_to_cdsolver(pymps_problem):
    # Append row, columns and non-zeros
    constraints = pymps_problem.get_constraints()
    rhs = pymps_problem.get_rhs()
    variables = pymps_problem.get_variables()
    m = len(constraints)
    N = len(variables)
    for key in variables.keys():
        assert(variables[key]['type'] == 'Continuous')

    # Objective sense: MIN

    # Objective (cx + c0)
    objective = pymps_problem.get_objectives()
    assert(len(objective) == 1)
    objective = objective[pymps_problem.objective_names()[0]]['coefficients']
    c0 = 0
    assert(len(objective) == N)
    c = np.array([objective[key] for key in variables.keys()])

    # Bound constraints
    g = []
    bg = []
    Dg = []
    key2index = {}
    i = 0
    blocks = [0]
    for key in variables.keys():
        key2index[key] = i
        if 'lower' in variables[key].keys():
            if 'upper' in variables[key].keys():
                low = variables[key]['lower']
                up = variables[key]['upper']
                g.append('box_zero_one')
                bg.append(low)
                Dg.append(up-low)
            else:
                g.append('ineq_const')
                bg.append(low)
                Dg.append(1.)
        elif 'upper' in variables[key].keys():
            g.append('ineq_const')
            bg.append(up)
            Dg.append(-1.)
        else:
            g.append('zero')
            bg.append(0.)
            Dg.append(0.)
        if len(g) > 1 and g[-1] == g[-1]:
            g.pop()
            blocks[-1] += 1
        else:
            blocks.append(blocks[-1]+1)
        i += 1
    bg = np.array(bg)
    Dg = sp.dia_matrix((Dg, [0]), shape=(len(Dg), len(Dg)))

    # Constraints
    bh = []
    Ah_data = []
    Ah_colindices = []
    Ah_indptr = [0]
    h = []
    blocks_h = [0]
    for key in constraints.keys():
        bh.append(rhs[key])
        coefs = pymps_problem.get_constraints()[key]['coefficients']
        Ah_data += [coefs[kk] for kk in coefs.keys()]
        Ah_colindices += [key2index[kk] for kk in coefs.keys()]
        Ah_indptr.append(len(Ah_colindices))
        if constraints[key]['type'] == 'E':
            h.append('eq_const')
        elif constraints[key]['type'] == 'L':
            h.append('ineq_const')
            bh[-1] = -bh[-1]
            Ah_data[-len(coefs):] = -np.array(Ah_data[-len(coefs):])
        elif constraint[key]['type'] == 'G':
            h.append('ineq_const')
        if len(h) > 1 and h[-1] == h[-2]:
            h.pop()
            blocks_h[-1] += 1
        else:
            blocks_h.append(blocks_h[-1]+1)

    bh = np.array(bh)
    Ah = sp.csr_matrix((Ah_data, Ah_colindices, Ah_indptr))

    problem = cd_solver.Problem(N, h=h, blocks_h=blocks_h, Ah=Ah, bh=bh,
                                f=["linear"], Af=c, bf=[-c0],
                                g=g, blocks=blocks)
    return problem
"""

"""
def mps_to_cdsolver_extended(pymps_problem):
    # Append row, columns and non-zeros
    constraints = pymps_problem.get_constraints()
    rhs = pymps_problem.get_rhs()
    variables = pymps_problem.get_variables()
    m = len(constraints)
    N = len(variables)
    for key in variables.keys():
        assert(variables[key]['type'] == 'Continuous')

    # Objective sense: MIN

    # Objective (cx + c0)
    objective = pymps_problem.get_objectives()
    assert(len(objective) == 1)
    objective = objective[pymps_problem.objective_names()[0]]['coefficients']
    c0 = 0
    assert(len(objective) == N)
    c = np.array([objective[key] for key in variables.keys()])

    # Bound constraints
    g = []
    bg = []
    Dg = []
    key2index = {}
    i = 0
    blocks = [0]
    for key in variables.keys():
        key2index[key] = i
        if 'lower' in variables[key].keys():
            if 'upper' in variables[key].keys():
                low = variables[key]['lower']
                up = variables[key]['upper']
                g.append('box_zero_one')
                bg.append(low)
                Dg.append(up-low)
            else:
                g.append('ineq_const')
                bg.append(low)
                Dg.append(1.)
        elif 'upper' in variables[key].keys():
            g.append('ineq_const')
            bg.append(up)
            Dg.append(-1.)
        else:
            g.append('zero')
            bg.append(0.)
            Dg.append(0.)
        blocks.append(blocks[-1]+1)
        i += 1
    bg = np.array(bg)
    Dg = sp.dia_matrix((Dg, [0]), shape=(len(Dg), len(Dg)))

    # Constraints
    bh = []
    Ah_data = []
    Ah_colindices = []
    Ah_indptr = [0]
    h = []
    blocks_h = [0]
    for key in constraints.keys():
        bh.append(rhs[key])
        coefs = pymps_problem.get_constraints()[key]['coefficients']
        Ah_data += [coefs[kk] for kk in coefs.keys()]
        Ah_colindices += [key2index[kk] for kk in coefs.keys()]
        Ah_indptr.append(len(Ah_colindices))
        if constraints[key]['type'] == 'E':
            h.append('eq_const')
        elif constraints[key]['type'] == 'L':
            h.append('ineq_const')
            bh[-1] = -bh[-1]
            Ah_data[-len(coefs)] = -Ah_data[-len(coefs)]
        elif constraint[key]['type'] == 'G':
            h.append('ineq_const')
        blocks_h.append(blocks_h[-1]+1)

    bh = np.array(bh)
    Ah = sp.csr_matrix((Ah_data, Ah_colindices, Ah_indptr))

    problem = cd_solver.Problem(N, h=h, blocks_h=blocks_h, Ah=Ah, bh=bh,
                                f=["linear"], Af=c, bf=[-c0],
                                g=g, blocks=blocks)
    return problem
"""

def cdsolver_to_cvxpy(problem):
    
    # use this function on the problem before adding slack variables
    c = problem.Af[0].toarray()
    Ah = sp.csr_matrix(problem.Ah)  # for fast row access
    bh = problem.bh
    h = problem.h
    blocks_h = problem.blocks_h

    x = cvxpy.Variable(problem.N)
    objective = cvxpy.Minimize(c @ x - problem.bf[0])

    constraints = []
    for k in range(len(problem.h)):
        if h[k] == 'eq_const':
            constraints.append(Ah[blocks_h[k]:blocks_h[k+1], :].dot(x) - bh[blocks_h[k]:blocks_h[k+1]] == 0)
        elif h[k] == 'ineq_const':
            constraints.append(Ah[blocks_h[k]:blocks_h[k+1], :].dot(x) - bh[blocks_h[k]:blocks_h[k+1]] >= 0)
        elif h[k] == 'second_order_cone':
            constraints.append(
                cvxpy.SOC(Ah[blocks_h[k], :].dot(x) - bh[blocks_h[k]],
                          Ah[blocks_h[k]+1:blocks_h[k+1], :].dot(x) - bh[blocks_h[k]+1:blocks_h[k+1]])
                              )
        else:
            raise(NotImplementedError)
    g = problem.g
    blocks = problem.blocks
    for j in range(len(g)):
        if g[j] == 'zero':
            pass
        if g[j] == 'box_zero_one':
            constraints += [problem.Dg.data[0][j] * x[blocks[j]:blocks[j+1]] - problem.bg[blocks[j]:blocks[j+1]] >= 0, problem.Dg.data[0][j] * x[blocks[j]:blocks[j+1]] - problem.bg[blocks[j]:blocks[j+1]] <= 1]
        if g[j] == 'ineq_const':
            constraints += [problem.Dg.data[0][j] * x[blocks[j]:blocks[j+1]] - problem.bg[blocks[j]:blocks[j+1]] >= 0]
    return cvxpy.Problem(objective, constraints)

@njit
def proj_soc(y):
    norm_y_1_n = np.linalg.norm(y[1:])
    if y[0] >= norm_y_1_n:
        return y
    elif y[0] <= -norm_y_1_n:
        return 0*y
    else:
        new_norm = max([0., y[0] + norm_y_1_n]) / 2.
        y_new = y.copy()
        y_new[1:] = (new_norm / norm_y_1_n) * y[1:]
        y_new[0] = np.linalg.norm(y_new[1:])  # = new_norm
        return y_new

def cd_solver_to_FM(problem):
    c = problem.Af[0].toarray().ravel()
    Ah = sp.csr_matrix(problem.Ah)  # for fast row access
    bh = problem.bh
    bf = problem.bf
    h = np.array(problem.h)
    g = np.array(problem.g)
    blocks_h = problem.blocks_h
    blocks = problem.blocks
    Dg = problem.Dg
    bg = problem.bg

    d, n = Ah.shape
    M = sp.csc_matrix(sp.vstack([sp.hstack([sp.csc_matrix((n,n)), -Ah.T]),
                   sp.hstack([Ah, sp.csc_matrix((d,d))])]))

    @njit
    def F(z):
        x = z[:n]
        y = z[n:]
        val = c @ x + bh @ y - bf[0]
        for j in range(len(g)):
            if g[j] == 'box_zero_one':
                val += np.sum(x[blocks[j]:blocks[j+1]] > 1.00001) * 1e27 + np.sum(x[blocks[j]:blocks[j+1]] < -0.00001) * 1e27
        for k in range(len(h)):
            if h[k] == 'eq_const':
                pass
            elif h[k] == 'ineq_const':
                if (y[blocks_h[k]:blocks_h[k+1]] < -1e-13).any():
                    val += 1e30
            elif h[k] == 'second_order_cone':
                if -y[blocks_h[k]] - np.linalg.norm(y[blocks_h[k]+1:blocks_h[k+1]]) < -1e-13:
                    val += 1e33
            else:
                raise(NotImplementedError)
        return val

    @njit
    def F_star(z, n):
        x = z[:n]
        y = z[n:]
        ymbh = y - bh
        val = bf[0]
        if (np.abs(x - c) > 1e-10).any():
            val += 1e24
        for k in range(len(h)):
            if h[k] == 'eq_const':
                if (np.abs(ymbh[blocks_h[k]:blocks_h[k+1]]) > 1e-10).any():
                    val += 1e27
            elif h[k] == 'ineq_const':
                if (ymbh[blocks_h[k]:blocks_h[k+1]] < -1e-10).any():
                    val += 1e30
            elif h[k] == 'second_order_cone':
                if ymbh[blocks_h[k]] - np.linalg.norm(ymbh[blocks_h[k]+1:blocks_h[k+1]]) < -1e-10:
                    val += 1e33
            else:
                raise(NotImplementedError)
        return val

    @njit
    def prox_F(z, gamma, n):
        x = z[:n].copy()
        y = z[n:].copy()
        x -= gamma[0] * c
        y -= gamma[1] * bh
        for j in range(len(g)):
            if g[j] == 'box_zero_one':
                x[blocks[j]:blocks[j+1]] = np.maximum(np.minimum(1, x[blocks[j]:blocks[j+1]]), 0)
        for k in range(len(h)):
            if h[k] == 'eq_const':
                pass
            elif h[k] == 'ineq_const':
                y[blocks_h[k]:blocks_h[k+1]] = np.maximum(0, y[blocks_h[k]:blocks_h[k+1]])
            elif h[k] == 'second_order_cone':
                y[blocks_h[k]:blocks_h[k+1]] = \
                    -proj_soc(-y[blocks_h[k]:blocks_h[k+1]])
            else:
                raise(NotImplementedError)
        return np.concatenate((x,y))

    return F, F_star, prox_F, M

def primal_feasibility(problem, x):
    h = problem.h
    feas = np.zeros(len(h))
    blocks_h = problem.blocks_h
    for k in range(len(problem.h)):
        LX = problem.Ah[blocks_h[k]:blocks_h[k+1]] @ x - problem.bh[blocks_h[k]:blocks_h[k+1]]
        if h[k] == 'eq_const':
            feas[k] = norm(LX)
        elif h[k] == 'ineq_const':
            feas[k] = norm(np.minimum(0, LX)) 
        elif h[k] == 'second_order_cone':
            feas[k] = norm(LX - proj_soc(LX))
        else:
            raise(NotImplementedError)
    return feas

def dual_feasibility(problem, y):
    val = 0.
    h = problem.h
    for k in range(len(problem.h)):
        if h[k] == 'eq_const':
            pass
        elif h[k] == 'ineq_const':
            val += norm(np.minimum(0, y))
        elif h[k] == 'second_order_cone':
            val += norm(y - proj_soc(y))
        else:
            raise(NotImplementedError)
    return norm(prob0.Ah.T @ y_cvxpy + prob0.Af), val

do_socp = True

if do_socp:
    folder = '.'
    filename = 'qssp30.cbf'  # example from cblib 2014
    data = next(CBFdata.CBFdata(folder + '/' + filename).iterator())
    prob0 = read_through_pythonapi(data)
    del data
    print('problem loaded')

    cvx_py = True
    if cvx_py:
        problem_cvxpy = cdsolver_to_cvxpy(prob0)
        problem_cvxpy.solve(verbose=True, solver='CLARABEL')
        x_cvxpy = problem_cvxpy.variables()[0].value
        y_cvxpy = np.zeros(prob0.Ah.shape[0])
        i = 0
        for const in problem_cvxpy.constraints:
            if type(const.dual_value) == list:
                # soc constraint
                size_const = len(const.dual_value[0]) + len(const.dual_value[1])
                y_cvxpy[i] = -const.dual_value[0][0]
                y_cvxpy[i+1:i+size_const] = -const.dual_value[1].ravel()
            else:
                size_const = len(const.dual_value)
                y_cvxpy[i:i+size_const] = const.dual_value
            i += size_const
        z_cvxpy = np.concatenate([x_cvxpy, y_cvxpy])

    #cd_solver.coordinate_descent(prob0, algorithm='pure-cd', verbose=1, fixed_restart_period='adaptive', restart_period=10, print_style='smoothed_gap')
    #x_cds = prob0.sol
    #y_cds = prob0.dual_sol
    #z_cds = np.concatenate([x_cds, y_cds])

    F, F_star, prox_F, M = cd_solver_to_FM(prob0)
    max_iter = 10000
    z0 = np.zeros(M.shape[0])
    cbar = 0.25
    ratio = 10.
    b = 2
    t = 2
    product = cbar * t * norm(M)**2 / b**2
    beta0 = np.array([np.sqrt(ratio*product), np.sqrt(product/ratio)])
    n = prob0.N
    if os.path.isfile('expe/qssp30_pdhg.npz'):
        data = np.load('expe/qssp30_pdhg.npz')
        z_cp = data['z_cp']
        GG_cp = data['GG_cp']
    else:
        print('pdhg')
        z_cp, GG_cp = pdhg(z0, max_iter, F, prox_F, M, n)
        np.savez('expe/qssp30_pdhg.npz', z_cp=z_cp, GG_cp=GG_cp)
    if os.path.isfile('expe/qssp30_rapdhg.npz'):
        data = np.load('expe/qssp30_rapdhg.npz')
        z_racp = data['z_racp']
        GG_racp = data['GG_racp']
    else:
        print('rapdhg')
        z_racp, GG_racp = rapdhg(z0, max_iter, F, prox_F, M, n)
        np.savez('expe/qssp30_rapdhg.npz', z_racp=z_racp, GG_racp=GG_racp)
    if os.path.isfile('expe/qssp30_asgard.npz'):
        data = np.load('expe/qssp30_asgard.npz')
        z_asgard = data['z_asgard']
        GG_asgard = data['GG_asgard']
    else:
        print('asgard')
        z_asgard, GG_asgard = asgard(z0, max_iter, F, prox_F, M, n)
        np.savez('expe/qssp30_asgard.npz', z_asgard=z_asgard, GG_asgard=GG_asgard)
    if os.path.isfile('expe/qssp30_prox_grad_sg.npz'):
        data = np.load('expe/qssp30_prox_grad_sg.npz')
        z = data['z']
        GG = data['GG']
    else:
        print('prox_grad_sg')
        z, GG = proximal_gradient_sg(z0, max_iter//2, F, prox_F, M, n)
        np.savez('expe/qssp30_prox_grad_sg.npz', z=z, GG=GG)
    filename = 'expe/qssp30_acc_prox_grad_sg_cbar='+str(cbar)+'.npz'
    if os.path.isfile(filename):
        data = np.load(filename)
        z_ac = data['z_ac']
        GG_ac = data['GG_ac']
        ITERS_ac = data['ITERS_ac']
    else:
        print('acc_prox_grad_sg')
        z_ac, GG_ac, GG_algo, ITERS_ac, LL = acc_prox_grad_sg(z0, max_iter//2, F, F_star, prox_F, M, n, beta0, z_star=z_cvxpy)
        np.savez(filename, z_ac=z_ac, GG_ac=GG_ac, ITERS_ac=ITERS_ac)
    filename = 'expe/qssp30_acc_prox_grad_sg_lbfgs_addon_cbar='+str(cbar)+'.npz'
    if os.path.isfile(filename):
        data = np.load(filename)
        z_ac_bfgs = data['z_ac_bfgs']
        GG_ac_bfgs = data['GG_ac_bfgs']
        ITERS_bfgs = data['ITERS_bfgs']
    else:
        print('acc_prox_grad_sg_lbgfs_addon')
        z_ac_bfgs, GG_ac_bfgs, GG_algo_bfgs, ITERS_bfgs, LL_bfgs = acc_prox_grad_sg(z0, max_iter//2, F,
                                                                           F_star, prox_F, M, n, beta0, lbfgs_addon=1, z_star=z_cvxpy)
        np.savez(filename, z_ac_bfgs=z_ac_bfgs, GG_ac_bfgs=GG_ac_bfgs, ITERS_bfgs=ITERS_bfgs)
    filename = 'expe/qssp30_acc_prox_grad_sg_lbfgs2_addon_cbar='+str(cbar)+'.npz'
    if os.path.isfile(filename):
        data = np.load(filename)
        z_ac_bfgs2 = data['z_ac_bfgs2']
        GG_ac_bfgs2 = data['GG_ac_bfgs2']
        ITERS_bfgs2 = data['ITERS_bfgs2']
    else:
        print('acc_prox_grad_sg_lbgfs2_addon')
        z_ac_bfgs2, GG_ac_bfgs2, GG_algo_bfgs2, ITERS_bfgs2, LL_bfgs2 = acc_prox_grad_sg(z0, max_iter, F,
                                                                           F_star, prox_F, M, n, beta0, lbfgs_addon=2, z_star=z_cvxpy)
        np.savez(filename, z_ac_bfgs2=z_ac_bfgs2, GG_ac_bfgs2=GG_ac_bfgs2, ITERS_bfgs2=ITERS_bfgs2)
    filename = 'expe/qssp30_lbfgs.npz'
    if os.path.isfile(filename):
        data = np.load(filename)
        z_lbfgs = data['z_lbfgs']
        GG_lbfgs = data['GG_lbfgs']
        ITERS_lbfgs = data['ITERS_lbfgs']
    else:
        print('-------------lbgfs--------------')
        z_lbfgs, GG_lbfgs, GG_algo_lbfgs, ITERS_lbfgs = lbfgs_proxgrad(z0, max_iter, F,
                                                                           F_star, prox_F, M, n, beta0)
        np.savez(filename, z_lbfgs=z_lbfgs, GG_lbfgs=GG_lbfgs, ITERS_lbfgs=ITERS_lbfgs)


    plt.semilogy(GG_cp, ':'); plt.semilogy(GG_racp, '-.');
    plt.semilogy(np.arange(0, 2*len(GG), 2), GG, '--'); plt.semilogy(ITERS_ac, GG_ac, linestyle=(0,(5,10)));
    plt.semilogy(ITERS_bfgs, GG_ac_bfgs); plt.semilogy(ITERS_bfgs2, GG_ac_bfgs2);
    #plt.semilogy(ITERS_lbfgs, GG_lbfgs)
    plt.semilogy(np.arange(0, max_iter, 2), GG_asgard[:max_iter//2], linestyle=(0, (10, 5)));
    plt.legend(['pdhg', 'rapdhg', 'prox_grad', 'acc_prox_grad', 'acc_prox_grad_lbfgs', 'asgard']) #, 'acc_prox_grad_lbfgs2', 'lbfgs'])
    plt.ylabel('self-centered smoothed gap')
    plt.xlabel('proximal gradient evaluations')
    plt.xlim([0, max_iter])
    ylim = plt.ylim()
    #plt.ylim([1e-13, ylim[1]])
    #plt.xlim([0, 10000])
    plt.show()


"""
do_large_lp = False
if do_large_lp:
    filename = 'ex10.npz'  # example from https://miplib2010.zib.de/ where we only consider the root node of the B&B process
    if os.path.isfile(filename):
        lp_problem = np.load(filename, allow_pickle=True)['lp_problem'].flatten()[0]
    else:
        lp_problem = mps.read_mps('ex10.mps')
        np.savez(filename, lp_problem=lp_problem)
    lp_problem_cd_solver = mps_to_cdsolver(lp_problem)
    lp_problem_cd_solver_pure_cd = mps_to_cdsolver_extended(lp_problem)
    lp_problem_cvxpy = cdsolver_to_cvxpy(lp_problem_cd_solver)
    F, F_star, prox_F, M = cd_solver_to_FM(lp_problem_cd_solver)

    print('lp problem ex10 loaded')
    
    max_iter = 5000
    z0 = np.zeros(M.shape[0])
    cbar = 0.25
    ratio = 1.
    b = 2
    t = 2
    product = cbar * t * norm(M)**2 / b**2
    beta0 = np.array([np.sqrt(ratio*product), np.sqrt(product/ratio)])
    n = lp_problem_cd_solver.N

    filename = 'expe/ex10_cvxpy.npz'
    if os.path.isfile('expe/ex10_cvxpy.npz'):
        z_cvxpy = np.load(filename)['z_cvxpy']
    else:
        lp_problem_cvxpy.solve(verbose=True, solver='MOSEK')
        x_cvxpy = lp_problem_cvxpy.variables()[0].value
        y_cvxpy = []
        i = 0
        for const in lp_problem_cvxpy.constraints:
            size_const = len(const.dual_value)
            y_cvxpy += const.dual_value.tolist()
            i += size_const
        z_cvxpy = np.concatenate([x_cvxpy, np.array(y_cvxpy)])
        np.savez(filename, z_cvxpy=z_cvxpy)
    filename = 'expe/ex10_purecd.npz'
    if os.path.isfile(filename):
        data = np.load(filename)
        z_purecd = data['z_purecd']
    else:
        print('-------------pure-cd--------------')
        cd_solver.coordinate_descent(lp_problem_cd_solver_pure_cd, algorithm='pure-cd', verbose=2.,
                                     print_style='smoothed_gap', average=True,
                                     restart_period=10, fixed_restart_period='adaptive',
                                     show_restart_info=False, max_iter=10*max_iter)
        x = lp_problem_cd_solver_pure_cd.sol
        y = lp_problem_cd_solver_pure_cd.dual_sol
        z_purecd = np.concatenate((x,y))
        np.savez(filename, z_purecd=z_purecd)
        
    if os.path.isfile('expe/ex10_pdhg.npz'):
        data = np.load('expe/ex10_pdhg.npz')
        z_cp = data['z_cp']
        GG_cp = data['GG_cp']
    else:
        print('---------pdhg-------')
        z_cp, GG_cp = pdhg(z0, max_iter, F, prox_F, M, n)
        np.savez('expe/ex10_pdhg.npz', z_cp=z_cp, GG_cp=GG_cp)
    if os.path.isfile('expe/ex10_rapdhg.npz'):
        data = np.load('expe/ex10_rapdhg.npz')
        z_racp = data['z_racp']
        GG_racp = data['GG_racp']
    else:
        print('--------rapdhg--------')
        z_racp, GG_racp = rapdhg(z0, max_iter, F, prox_F, M, n)
        np.savez('expe/ex10_rapdhg.npz', z_racp=z_racp, GG_racp=GG_racp)
    filename = 'expe/ex10_prox_grad_sg.npz'
    if os.path.isfile(filename):
        data = np.load(filename)
        z = data['z']
        GG = data['GG']
    else:
        print('--------prox_grad_sg--------')
        z, GG = proximal_gradient_sg(z0, max_iter//2, F, prox_F, M, n)
        np.savez(filename, z=z, GG=GG)
    filename = 'expe/ex10_acc_prox_grad_sg_cbar='+str(cbar)+'.npz'
    if os.path.isfile(filename):
        data = np.load(filename)
        z_ac = data['z_ac']
        GG_ac = data['GG_ac']
        ITERS_ac = data['ITERS_ac']
    else:
        print('--------acc_prox_grad_sg--------')
        z_ac, GG_ac, GG_algo, ITERS_ac = acc_prox_grad_sg(z0, max_iter, F, F_star, prox_F, M, n, beta0)
        np.savez(filename, z_ac=z_ac, GG_ac=GG_ac, ITERS_ac=ITERS_ac)
    filename = 'expe/ex10_acc_prox_grad_sg_lbfgs_addon_cbar='+str(cbar)+'.npz'
    if os.path.isfile(filename):
        data = np.load(filename)
        z_ac_bfgs = data['z_ac_bfgs']
        GG_ac_bfgs = data['GG_ac_bfgs']
        ITERS_bfgs = data['ITERS_bfgs']
    else:
        print('--------acc_prox_grad_sg_lbgfs_addon--------')
        z_ac_bfgs, GG_ac_bfgs, GG_algo_bfgs, ITERS_bfgs = acc_prox_grad_sg(z0, max_iter, F,
                                                                                    F_star, prox_F, M, n, beta0, lbfgs_addon=1)
        np.savez(filename, z_ac_bfgs=z_ac_bfgs, GG_ac_bfgs=GG_ac_bfgs, ITERS_bfgs=ITERS_bfgs)
    filename = 'expe/ex10_acc_prox_grad_sg_lbfgs2_addon_cbar='+str(cbar)+'.npz'
    if os.path.isfile(filename):
        data = np.load(filename)
        z_ac_bfgs2 = data['z_ac_bfgs2']
        GG_ac_bfgs2 = data['GG_ac_bfgs2']
        ITERS_bfgs2 = data['ITERS_bfgs2']
    else:
        print('--------acc_prox_grad_sg_lbgfs2_addon--------')
        z_ac_bfgs2, GG_ac_bfgs2, GG_algo_bfgs2, ITERS_bfgs2 = acc_prox_grad_sg(z0, max_iter, F,
                                                                           F_star, prox_F, M, n, beta0, lbfgs_addon=2, z_star=z_cvxpy)
        np.savez(filename, z_ac_bfgs2=z_ac_bfgs2, GG_ac_bfgs2=GG_ac_bfgs2, ITERS_bfgs2=ITERS_bfgs2)
    filename = 'expe/ex10_lbfgs.npz'
    if os.path.isfile(filename):
        data = np.load(filename)
        z_lbfgs = data['z_lbfgs']
        GG_lbfgs = data['GG_lbfgs']
        ITERS_lbfgs = data['ITERS_lbfgs']
    else:
        print('-------------lbgfs--------------')
        z_lbfgs, GG_lbfgs, GG_algo_lbfgs, ITERS_lbfgs = lbfgs_proxgrad(z0, 30, F,
                                                                           F_star, prox_F, M, n, beta0)
        np.savez(filename, z_lbfgs=z_lbfgs, GG_lbfgs=GG_lbfgs, ITERS_lbfgs=ITERS_lbfgs)

    plt.semilogy(GG_cp)
    plt.semilogy(GG_racp)
    plt.semilogy(np.arange(0,2*len(GG), 2), GG)
    plt.semilogy(ITERS_ac, GG_ac)
    plt.semilogy(ITERS_bfgs, GG_ac_bfgs)
    plt.legend(['pdhg', 'rapdhg', 'prox_grad', 'acc_prox_grad', 'acc_prox_grad_lbfg'])
    plt.ylabel('self-centered smoothed gap')
    plt.xlabel('proximal gradient evaluations')
    plt.xlim([0, max_iter])
    plt.show()
"""
