228

def pd_ipm(X, t, reg_coef, max_iter=100, tol_feas=1e-10, tol_gap=1e-6, tau_param=10, bt_params=np.array([1e-4,0.8]), display=False):
    def backtrack(lambdas, mu, v1, v2, r_dual, r_primal, r_center1, r_center2):
        alpha = 1.
        if np.any(d_lambda > 0):
            alpha = min(alpha, min((C - lambdas[d_lambda > 0]) / d_lambda[d_lambda > 0]))
        if np.any(d_lambda < 0):
            alpha = min(alpha, min(-lambdas[d_lambda < 0] / d_lambda[d_lambda < 0]))
        if np.any(d_v1 < 0):
            alpha = min(alpha, min(-v1[d_v1 < 0] / d_v1[d_v1 < 0]))
        if np.any(d_v2 < 0):
            alpha = min(alpha, min(-v2[d_v2 < 0] / d_v2[d_v2 < 0]))
        alpha *= 0.9
        r_norm = max(norm(r_dual, np.inf), abs(r_primal), norm(r_center1, np.inf), norm(r_center2, np.inf))
        i = 0
        print('%.15f' % r_norm)
        while i < 100:
            i += 1
            lambdas_ = lambdas + alpha * d_lambda
            mu_ = mu + alpha * d_mu
            v1_ = v1 + alpha * d_v1
            v2_ = v2 + alpha * d_v2

            #r_dual_ = TXXT.dot(lambdas_) - 1. + mu_ * t - v1_ + v2_
            r_dual_ = X.dot(X.T.dot(t * t * lambdas_)) - 1. + mu_ * t - v1_ + v2_
            r_primal_ = lambdas_.dot(t)
            r_center1_ = -1. / tau + v1_ * lambdas_
            r_center2_ = 1. / tau + v2_ * (lambdas_ - C)

            r_norm_ = max(norm(r_dual_, np.inf), abs(r_primal_), norm(r_center1_, np.inf), norm(r_center2_, np.inf))
            if r_norm_ < (1. - alpha * c1) * r_norm:
                return alpha
            alpha *= beta
        return alpha

    C = reg_coef
    n, d = X.shape
    c1, beta = bt_params

    #TXXT = t * t.reshape((-1, 1)) * X.dot(X.T)

    lambdas = np.zeros(n)
    lambdas[t < 0] = C / 2. / sum(t < 0)
    lambdas[t > 0] = C / 2. / sum(t > 0)
    v1 = 1. / lambdas
    v2 = 1. / (C - lambdas)
    mu = 0.

    iteration = 0
    tau = 1.
    while iteration < max_iter:
        iteration += 1
        tau = 2. * n * min(1. / tol_gap, tau_param / (v1.dot(lambdas) + v2.dot(C - lambdas)))

        #r_dual = TXXT.dot(lambdas) - 1. + mu * t - v1 + v2
        r_dual = X.dot(X.T.dot(t * t * lambdas)) - 1. + mu * t - v1 + v2
        r_primal = lambdas.dot(t)
        r_center1 = -1. / tau + v1 * lambdas
        r_center2 = 1. / tau + v2 * (lambdas - C)
        if norm(r_dual, np.inf) <= tol_feas and abs(r_primal) <= tol_feas and v1.dot(lambdas) + v2.dot(C - lambdas) <= tol_gap:
            break
        #A = np.bmat([
        #    [TXXT + np.diag(v1 * 1. / lambdas - v2 * 1. / (lambdas - C)), t.reshape((-1, 1))],
        #    [t.reshape((1, -1)), np.zeros((1, 1))],
        #])
        #B = -np.bmat([r_dual + np.diag(1. / lambdas).dot(r_center1) - np.diag(1. / (lambdas - C)).dot(r_center2), [r_primal]]).T
        # D = np.array(solve(A, B)).reshape(-1)
        #d_lambda = D[:n]
        #d_mu = D[n]
        #print(d_mu)

        #AA = np.diag(v1 * 1. / lambdas - v2 * 1. / (lambdas - C))
        aainv = 1. / (v1 * 1. / lambdas - v2 * 1. / (lambdas - C))
        #AAinv = np.diag(1. / (v1 * 1. / lambdas - v2 * 1. / (lambdas - C)))
        #P = TXXT + AA
        y = -(r_dual + r_center1 * 1. / lambdas - r_center2 * 1. / (lambdas - C))
        y0 = r_primal

        #T = np.diag(t)
        taat = lil_matrix((n,n))
        taat.setdiag(t * aainv * t)

        y1 = (X.T).dot(t * aainv * y)
        Ay2 = (X.T.dot(taat)).dot(X) + np.identity(d)
        y2 = np.array(solve(Ay2, y1.T)).reshape(-1)
        Pinvy = aainv * (y - t * X.dot(y2))

        t1 = (X.T).dot(t * aainv * t)
        At2 = (X.T.dot(taat)).dot(X) + np.identity(d)
        t2 = np.array(solve(At2, t1.T)).reshape(-1)
        Pinvt = aainv * (t - t * X.dot(t2))

        d_lambda = Pinvy - Pinvt * (t.dot(Pinvy) - y0) / t.dot(Pinvt)
        d_mu = (t.dot(Pinvy) - y0) / t.dot(Pinvt)

        d_v1 = -v1 * d_lambda * 1. / lambdas + (-v1 * lambdas + 1. / tau) / lambdas
        d_v2 = -v2 * d_lambda * 1. / (lambdas - C) - (v2 * (lambdas - C) + 1. / tau) / (lambdas - C)
        alpha = backtrack(lambdas, mu, v1, v2, r_dual, r_primal, r_center1, r_center2)
        lambdas += alpha * d_lambda
        mu += alpha * d_mu
        v1 += alpha * d_v1
        v2 += alpha * d_v2