function varargout = CRCQP(varargin)
%[x, logs] = CRCQP(H, A, b, c, opts)
% This function solves a convex quadratic program with inequality
% constraints of the following form:
%       min_{x} 1/2*x'*H*x + c'*x,
%       s.t. A*x >= b.
%Input parameters
%H         : A matrix for the coefficients of the second order terms
%A         : A matrix for the coefficients for linear inequality
%constraints
%b         : A vector for the right hand side of inequality
%c         : A vector for the coefficients of the first order terms
%opts      : A parameter containing various options
%   opts.kernel: Kernel
%       [None|Gaussian]
%   opts.tol: convergence tolerance applied to residuals [scalar | {10e-8}]
%   opts.tol_mu: convergence tolerance applied to the complementarity
%       measurement mu [scalar | {10e-8}]
%   opts.iter_ubound: maximal number of iterations
%   opts.q_lower: minimum number of constraints to keep [integer or
%   'automatic' | {automatic}]
%   opts.q_lower_rel: q_lower = opts.q_lower_rel*size(A,1) [scalar | {opts.q_lower_rel/size(A,1)}]
%   opts.q_upper: maximum number of constraints to keep [integer | {size(A,1)}]
%   opts.q_upper_rel: q_upper = opts.q_upper_rel*size(A,1) [scalar | {opts.q_upper_rel/size(A,1)}]
%   opts.reduction: reduction technique
%       [NONADAPTIVE | ADAPTIVE | NONE]
%       NONADAPTIVE: fix size of Q to opts.q_upper
%       ADAPTIVE: shrink Q adaptively as the iteration approach an optimal
%           solution
%   opts.adaptive_reduction_controller:
%       [{MU} | GAP]
%       MU : Determines q based on MU, the complementarity measure
%       GAP : Determines q based on the relative duality gap
%   opts.beta: A parameter controlling the rate of decrease of the centrality 
%       measurement in determining the size of the index set. Any positive number
%       greater than 1. (mu^(1/opts.beta)) will determine the ratio of 
%       index set size to the number of patterns.
%   opts.index_basis: the basis for the selection of indexes [{s} | lambda | Lambda_Sinv]
%   opts.save_history: If true, the function returns history of x and Q
%
%OUTPUT
    % Start to measure cputime
    starting = cputime;
    
    if nargin < 4
        error('Not enough input arguments.');
    end

    H = varargin{1};
    A = varargin{2};
    b = varargin{3};
    c = varargin{4};

    if nargin == 5
        opts = varargin{5};
    else
        opts = [];
    end

    % m : number of constraints
    % n : number of variables
    [m,n] = size(A);
    
    if(n ~= size(H,1) || n ~= size(H,2))
        error('Size of the Hessian H is not valid.');
    end
    
    if isfield(opts, 'kernel')
        switch lower(opts.kernel)
            case 'none'
                sw_kernel = 0;
            case 'gaussian'
                sw_kernel = 1;
            otherwise
                error(sprintf('Not supported kernel %s\n', opts.kernel));
        end
    else
        sw_kernel = 0;
    end
    
    sw_sparse = 0;
    if issparse(A) && issparse(H)
        sw_sparse = 1;
    end
    sw_sparse = 0;
    
    % Extract tolerance
    if isfield(opts, 'tol_conv')
        tol_conv = opts.tol_conv;
    else
        tol_conv = 1e-4;
    end
    
    % Extract tolerance for complementarity
    if isfield(opts, 'tol_mu')
        tol_mu = opts.tol_mu;
    else
        tol_mu = 1e-8;
    end

    % Upper bound on number of iterations
    if isfield(opts, 'iter_ubound')
        iter_ubound = opts.iter_ubound;
    else
        iter_ubound = 2e2;
    end
    
    % Beta
    if isfield(opts, 'beta')
        beta = opts.beta;
    else
        beta = 4;
    end
    
    if isfield(opts, 'adaptive_reduction_controller')
        switch lower(opts.adaptive_reduction_controller) 
            case 'mu'
                sw_reduction_controller = 1;
            case 'gap'
                sw_reduction_controller = 2;
            case 'all'
                sw_reduction_controller = 3;
            otherwise
                error('opts.adaptive_reduction_controller must be either MU or GAP');
        end
    else
        sw_reduction_controller = 3;
    end
    
    if isfield(opts, 'eta')
        eta = opts.eta;
    else
        eta = 0.98;
    end
    
    sw_scale = 1;
    if isfield(opts, 'scale')
        switch lower(opts.scale)
            case 'none'
                sw_scale = 0;
            case 'scalar_scaling'
                sw_scale = 1;
%             case 'column_wise_scale'
%                 sw_scale = 2;
            otherwise
                error('Unknown scale options. It must be one of none/scalar_scaling/column_wise_scale');
        end
    end
    
%     scale_A = 1;
%     scale_A_inv = 1;
%     scale_b = 1;
%     scale_b_inv = 1;
%     scale_c = 1;
%     scale_c_inv = 1;
    
    if sw_scale == 1
        scale_A = normest(A);
        scale_A_inv = 1/scale_A;
        scale_b = norm(b);
        scale_c = norm(c);
%     elseif sw_scale == 2
%         for i = 1:n,
%             scale_A(i,1) = norm(A(:,i));
%         end
%         scale_A_inv = 1./scale_A;
% 
%         scale_b = norm(b);
%     
%         scale_c = norm(scale_A_inv.*c);
    end
    
    if sw_scale == 1
        if scale_b == 0
            scale_b = 1;
        end
        if scale_c == 0
            scale_c = 1;
        end
        scale_b_inv = 1/scale_b;
        scale_c_inv = 1/scale_c;
        if scale_A_inv ~= 1
            A = scale_A_inv * A;
        end
        if scale_b_inv ~= 1
            b = scale_b_inv * b;
        end
        if scale_c_inv ~= 1
            c = scale_c_inv * c;
        end
        scale_H = scale_b*scale_A_inv*scale_c_inv;
        if scale_H ~= 1
            H = scale_H*H;
        end
    end
    
    if isfield(opts, 'lambda_under')
        lambda_under = opts.lambda_under;
    else
        lambda_under = 1e-4;
    end
    
    if isfield(opts, 'lambda_max')
        lambda_max = opts.lambda_max;
    else
        lambda_max = 1e30;
    end
    
%     switch sw_scale
%         case 1
%             lambda_under = lambda_under*(scale_c)/scale_A;
%             lambda_max = lambda_max*(scale_c)/scale_A;
%         case 2    
%             lambda_under = lambda_under*(scale_c);
%             lambda_max = lambda_max*(scale_c);
%     end

    if isfield(opts, 'save_history')
        save_history = opts.save_history;
    else
        save_history = false;
    end
    
    % Choice of q_lower
    q_lower_automatic = true;
    q_lower = n; % Default choice: automatic (Actual 
    if (isfield(opts,'q_lower'))
        if isa(opts.q_lower, 'numeric')
            q_lower_automatic = false;
            if opts.q_lower > 1
                q_lower = opts.q_lower;
            else
                q_lower = round(m*opts.q_lower);
            end
        elseif ~strcmpi(opts.q_lower, 'automatic')
            error('Unknown input option to opts.q_lower');
        end
    end
    
    % Choice of q_upper
    if isfield(opts, 'q_upper')
        if opts.q_upper > 1
            q_upper = opts.q_upper;
        else
            q_upper = round(m*opts.q_upper);
        end
    else
        q_upper = m;
    end

    % Reduction method
    useReduction = true;
    if isfield(opts,'reduction')
        switch lower(opts.reduction)
            case {'nonadaptive'}
                sw_Q = 1;
            case {'adaptive'}
                sw_Q = 2;
            case {'none', 'no'}
                useReduction = false;
                sw_Q = 0;
            otherwise
                error('Unknown reduction technique. Check the option you gave.');
        end
    else
        sw_Q = 2;
    end

    % Strategy for the index selection
    if sw_Q ~= 0 && isfield(opts, 'index_basis')
        switch lower(opts.index_basis)
            case {'s'}
                strategy = 1;
            case {'lambda'}
                strategy = 2;
            case {'s_inv_lambda'}
                strategy = 3;
            otherwise
                error('Check the option. opts.index_basis must be one of z, lambda, and S_inv_lambda.');
        end
    else
        strategy = 1;
    end
    
    % Soft constraints
    sw_soft_constraints = 1;
%     if sw_scale == 1 || sw_scale == 2
%         tol_soft_constraints = scale_b*1e-14;
%     else
        tol_soft_constraints = 1e-12;
%     end

    % Time consumed for definining the index set
    time_Q = 0;
    % Time consumed for assembling the matrix for the normal equations
    time_Mat = 0;
    % Time consumed for decomposition the matrix for the normal equations
    time_chol = 0;
    
    bitmask_Q = zeros(m,1);
    
    % Automatic choices of lambda0
    % sw_lambda0
    % -1: lambda0 is given.
    %  0: lambda0 = ones(n,1);
    %  1: lambda0 is the minimal norm solution of A*lambda = c + H*x0
    %  2: lambda0 is as in Mehrotra (SIOPT, 1992), but modified for
    %  dual-feasibility
    
    % Set initial point (x, s, lambda)
    if isfield(opts, 'initial_point')
        if(isfield(opts.initial_point, 'x0'))
            x = opts.initial_point.x0;
            s = A*x - b;
            if ~isempty(find(s <= 0, 1))
                error('The initial point x0 you provided is not strictly feasible');
            end
        else
            disp('You should provide x0 if you want to use your own initial point.');
            error('\topts.initial_point.x0 = (your initial point).');
        end
        if(isfield(opts.initial_point, 'lambda0'))
            lambda = opts.initial_point.lambda0;
            sw_lambda0 = -1;
        else
            sw_lambda0 = 0;
        end
    else
        error('You should provide an initial point for x.');
    end

    % Set lambda0
    switch sw_lambda0
        case 0
%             switch sw_scale
%                 case 0
                    lambda = ones(m,1);
%                 case 1
%                     lambda = ones(m,1)*scale_c/scale_A;
%                 case 2
%                     lambda = ones(m,1)*scale_c;
%             end                    
        case 1
            [Qmat, Upper] = qr(full(A), 0);
            lambda = Qmat*(Upper'\(c + H*x));
            lambda = lambda + max(0, -1.5*min(lambda));
        case 2
            [Qmat, Upper] = qr(full(A), 0);
            lambda = Qmat*(Upper'\(c + H*x));
            lambda_pd = lambda + max(0, -1.5*min(lambda));
            lambda = lambda_pd + .5*(lambda_pd'*s)/sum(s);
    end
%     clear Qmat lambda_pd;
    
    itr_cnt = 0;
    
    % Define the maximum magnitude of the problem data
%     data_magnitude = max([norm(A, inf), norm(b, inf), norm(c, inf),
%     norm(H, inf), 1]);
    
    if (sw_Q == 0)
        Q = 1:m;
        lenQ = m;
    end
    
    q_mu = q_upper;

    test_stop = 0;
    while(~test_stop)
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        % Residuals
        % r_c = H*x + c - A'*lambda
        % r_b = A*x - b - s
        % r_s_lambda = -S*lambda
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%        
        Hx = H*x;
        r_c = Hx + c - A'*lambda;
        r_b = A*x - b - s;
        xTHx = .5*x'*Hx;
        obj_primal = xTHx + c'*x;
        obj_dual = -xTHx + b'*lambda;
        norm_HXPlusC = norm(Hx + c);
        
%         switch sw_scale
%             case 0
                relative_gap = abs((obj_primal-obj_dual)/(obj_primal+1));
                residual_primal = norm(r_b)/(1 + norm(s));
                residual_dual = norm(r_c)/(1 + norm(lambda));
                mu = (s'*lambda)/m;
%             case 1
%                 relative_gap = abs((obj_primal-obj_dual)/(obj_primal+scale_c*scale_b/scale_A));
%                 residual_primal = norm(r_b)/(scale_b + norm(s));
%                 residual_dual = norm(r_c)/(scale_c + scale_A*norm(lambda));
%                 mu = scale_A/(scale_b*scale_c)*(s'*lambda)/m;
%             case 2
%                 relative_gap = abs((obj_primal-obj_dual)/(obj_primal+scale_c*scale_b));
%                 residual_primal = norm(r_b)/(scale_b + norm(s));
%                 residual_dual = norm(scale_A_inv.*r_c)/(scale_c + norm(lambda));
%                 mu = scale_b/scale_c*(s'*lambda)/m;
%         end
        mu_trace(itr_cnt+1) = mu;
        
        if itr_cnt == 0
            initial_relative_gap = relative_gap;
            initial_mu = mu;
        end

        % Determine termination
        if nargout > 1
            residual_primal_trace(itr_cnt+1) = residual_primal;
            residual_dual_trace(itr_cnt+1) = residual_dual;
            varargout{2}.min_lambda(itr_cnt+1) = min(lambda);
            varargout{2}.min_s(itr_cnt+1) = min(s);
            varargout{2}.min_x(itr_cnt+1) = min(x);
        end
        
%         if( max([residual_primal, residual_dual, relative_gap]) < tol_conv)
        if( max([residual_dual, relative_gap]) < tol_conv || norm_HXPlusC == 0)
            % Termination criteria are satisfied
            flag = 1;
            break;
        end
        
        % Check whether iteration limit count is reached
        if itr_cnt >= iter_ubound
            flag = -1;
            warning('Iteration count reached the limit.');
            break;
        end
        
        % Increase iteration count
        itr_cnt = itr_cnt + 1;
        
%         %Ensure dual feasibility
%         if sw_soft_constraints
%             s = A*x - b;
%         end
        
        % Start to measure the timing for making the index set
        starting_Q = cputime;
        if sw_Q == 2
            % q_mu is iteration dependent
            if q_lower_automatic
                candidate_threshold = min(100*sqrt(mu),2);
                candidate_cnt = length(find(lambda./max(tol_soft_constraints, s) >= candidate_threshold));
                q_lower = candidate_cnt;
%                 if mu >= 1e-5 % A safe guard to q_lower
                q_lower = max(q_lower, n);
%                 end
                switch sw_reduction_controller
                    case 1
                    q_rate = (mu)^(1/beta);
                    case 2
                    q_rate = (relative_gap)^(1/beta);
                    case 3
%                     q_rate = max([relative_gap, mu, residual_dual])^(1/beta);
                    q_rate = max([mu, residual_dual])^(1/beta);
                end
                q_mu = max(ceil(q_rate*m), q_lower);
                q_mu = min(q_mu, q_upper);
            else
                q_rate = (mu)^(1/beta);
                q_mu = max([min([ceil(q_rate*m), q_upper]), q_lower]);
            end
        else 
            q_mu = q_upper;
        end
        time_Q = time_Q + (cputime-starting_Q);

        %Now determine indexes
%             q_mu
        starting_Q = cputime;
        if sw_Q ~= 0
            % Select indices based on
            if q_mu < m
                switch strategy
                    case 1 % s
                        [sorted, full_Q] = sort(s);
                    case 2 % lambda
                        [sorted, full_Q] = sort(lambda,1,'descend');
                    case 3 % S_inv_lambda
                        [sorted, full_Q] = sort(lambda./s,1,'descend');
                end
                % Choose the first q indices in the sorted list
                % according to the basis
                Q = full_Q(1:q_mu);
            else
                Q = 1:m;
            end
        end
        time_Q = time_Q + (cputime-starting_Q);

        % Save history
        if save_history
            Q_hist{itr_cnt} = Q;
            x_hist{itr_cnt+1} = x;
        end

        % Keep track of the number of patterns involved in the matrix
        % assembly
        if sw_Q ~= 0
            lenQ = length(Q);
            constraints_used(itr_cnt) = lenQ;
        end

        if sw_soft_constraints
            % Increase too small s
            s = max(tol_soft_constraints, s);
        end

        sw_use_cholupdate = 0;
        need_to_compute_matrix = true;
        factorization_failure_cnt = 0;
        while need_to_compute_matrix
            if save_history
                Q_hist{itr_cnt} = full_Q(1:q_mu);
            end
            % Form the matrix for the normal equation
            % Reduced matrix assembly
            starting_Mat = cputime;
            if factorization_failure_cnt == 0
                Mat = H + A(Q,:)'*spdiags(lambda(Q)./s(Q), 0, lenQ, lenQ)*A(Q,:);
            elseif ~sw_use_cholupdate
                Mat = Mat + A(Qdiff,:)'*spdiags(lambda(Qdiff)./s(Qdiff), 0, lenQdiff, lenQdiff)*A(Qdiff,:);
            end
            time_Mat = time_Mat + (cputime-starting_Mat);

            % Form the rhs for the equation
            rhs = -(H*x + c);

            % Get the affine scaling direction (Dx, Ds) and tilde_lambda
            % Dx = Mat\rhs;
            starting_chol = cputime;
            if sw_sparse
                if sw_Q == 0 && itr_cnt == 1
                    mat_ordering = symamd(Mat);
                elseif sw_Q ~= 0
                    mat_ordering = symamd(Mat);
                end
                warning('off');
                Upper = cholinc(Mat(mat_ordering, mat_ordering), 'inf');                    
                warning('on');
            else
                if factorization_failure_cnt == 0 || sw_use_cholupdate == 0
                    [Upper, pp] = chol(Mat);
                else
                    % This doesn't work
                    [Upper, pp] = cholupdate(Upper, (sqrt(lambda(Qdiff)./s(Qdiff))*A(Qdiff,:))');
                end
                if pp ~= 0
                    % factorization failure detected
                    need_to_compute_matrix = true;
                else
                    need_to_compute_matrix = false;
                end
            end
            if need_to_compute_matrix
                factorization_failure_cnt = factorization_failure_cnt + 1;
                if save_history
                    varargout{2}.history.factorization_failure_count(itr_cnt) = factorization_failure_cnt;
                end
                if q_mu >= m
%                     % The matrix is singular even if all the constraints
%                     % are used. Try cholesky infinity
                    Upper = cholinc(sparse(Mat),'inf');
                    need_to_compute_matrix = false;
%                     warning('Infinity Cholesky factorization is used due to numerical difficulties.');
                    % Declare failure and exit
%                     need_to_compute_matrix = false;
%                     flag = -2;
%                     test_stop = 1;
%                     warning('Can''t proceed since Cholesky factorization failed.');
%                     break;
                end
                q_mu_prev = q_mu;
                q_mu = min(q_mu * 2,m);
                Qdiff = full_Q(min(m,q_mu_prev+1):q_mu);
                lenQdiff = length(Qdiff);
            end
        end
        time_chol = time_chol + (cputime - starting_chol);
        if test_stop
            break;
        end

        warning('off');
        if sw_sparse
            Dx = Upper\(Upper'\rhs(mat_ordering,1));
            Dx = Dx(mat_ordering,1);
        else
            Dx = Upper\(Upper'\rhs);
        end
        warning('on');
        % Ds = A*Dx
        Ds = A*Dx;
%         Ds = AT'*Dx;
        tilde_lambda = -lambda./s.*Ds;

%         switch sw_scale
%             case 0
                norm_of_Dx = norm(Dx);
%             case 1
%                 norm_of_Dx = norm(scale_A*Dx)/scale_b;
%             case 2
%                 norm_of_Dx = norm(scale_A.*Dx)/scale_b;
%         end

        if norm_of_Dx == 0
            % terminate
            flag = 2;
            break;
        end
        
        % Find appropriate step size so that the step doesn't go out of
        %   the positive orthant
        idx_s_decr = find(Ds < 0);
        if isempty(idx_s_decr)
            bar_alpha = Inf;
        else
            bar_alpha = min(-s(idx_s_decr)./Ds(idx_s_decr));
        end
        
        alpha_p = min(max(eta*bar_alpha, bar_alpha-norm_of_Dx),1);
        varargout{2}.bar_alpha(itr_cnt) = bar_alpha;
        varargout{2}.alpha(itr_cnt) = alpha_p;
        
        % Get lambda
        tilde_lambda_under = min(tilde_lambda, 0);

%         switch sw_scale
%             case 0
                dual_violation = (norm_of_Dx)^2 ...
                                + (norm(tilde_lambda_under))^2;
%             case 1
%                 dual_violation = scale_c/scale_A*(norm_of_Dx)^2 ...
%                                 + scale_A/scale_c*(norm(tilde_lambda_under))^2;
%             case 2
%                 dual_violation = scale_c*(norm_of_Dx)^2 ...
%                                 + 1/scale_c*(norm(tilde_lambda_under))^2;
%         end

        lambda_plus = min(max(min(dual_violation, lambda_under), tilde_lambda), lambda_max);

        % Take the step
        lambda = lambda_plus;
        x = x + alpha_p*Dx;

        %Ensure dual feasibility
        if sw_soft_constraints
            s = A*x - b;
        else
            s = s + alpha_p*Ds;
        end
    end % end of main while
    ending = cputime;
    
%     if any(s < 0) || any(lambda < 0)
    if any(s < -sqrt(tol_soft_constraints)) || any(lambda < 0)
        error('Positiveness conditions are not satisfied');
    end
    
    s(s < 0) = 0;
    
    if sw_scale == 1
        scale_x = scale_b/scale_A;
        if scale_x ~= 1
            x = scale_x*x;
        end
        if scale_b ~= 1
            s = scale_b*s;
        end
        scale_lambda = scale_c/scale_A;
        if scale_lambda ~= 1
            lambda = scale_lambda*lambda;
        end
    end
    
    % Set output variables
    varargout{1} = x;
    
    if nargout >= 2
        varargout{2}.iterations = itr_cnt;
        varargout{2}.time_total = ending - starting;
        varargout{2}.constraints = m;
        varargout{2}.variables = n;
        varargout{2}.flag = flag;
        varargout{2}.s = s;
        varargout{2}.lambda = lambda;
        
        varargout{2}.mu = mu;
        varargout{2}.mu_trace = mu_trace;
        
        varargout{2}.obj_primal = obj_primal;
        varargout{2}.obj_dual = obj_dual;
        varargout{2}.duality_gap = relative_gap;
        varargout{2}.residual_primal = residual_primal;
        varargout{2}.residual_dual = residual_dual;
        varargout{2}.residual_primal_trace = residual_primal_trace;
        varargout{2}.residual_dual_trace = residual_dual_trace;
        
        varargout{2}.Cholesky_factor_rank = pp;
        
        if useReduction
            varargout{2}.constraints_used = constraints_used;
            varargout{2}.avg_constraints_used = sum(constraints_used)/length(constraints_used);
            varargout{2}.total_constraints_used = sum(constraints_used);
        end
        varargout{2}.time_Q = time_Q;
        varargout{2}.time_Matrix = time_Mat;
        varargout{2}.time_chol = time_chol;
        if save_history && itr_cnt > 1
            varargout{2}.history.Q = Q_hist;
            varargout{2}.history.x = x_hist;
        end
    end
end % end of function