function [p,lam] = solve_trust_region_subproblem(Bk,g,delta,lam0)
% A pedagogical solver for the trust region subproblem

n = size(Bk,1);

[okay,R,p,q] = getchol(Bk,0,g);
if okay
    if norm(p)<delta
        lam = 0;
        return
    end
end

if nargin>3
    lam = lam0;
else
    lam = 1;
end
first = true;

for i=1:100 % maxiter should be an option 
    assert(lam > 0);
    [okay,~,p,q] = getchol(Bk,lam,g);
    if ~okay && ~first
        % then lam is too small, bisect back to oldlam, which was good
        lam = lam + (oldlam - lam)/2;
        continue
    elseif first && ~okay
        % keep increasing until we get somewhere
        found = false;
        for j=1:20 % maxfinditer should be an option
            lam = 2*lam;
            [okay,~,p,q] = getchol(Bk,lam,g);
            if okay, found=true; break; end
        end
        assert(found);
    end
    first = false;
    if abs(norm(p)-delta) < 1e-6 % tol should be an option
        return
    end
    
    oldlam = lam;
    lam = lam + (norm(p)/norm(q))^2 * (norm(p) - delta)/delta;
end
error('too many iterations');


function [okay,R,p,q] = getchol(Bk,lam,g)
n = size(Bk,1);
try
    R = chol(Bk + lam*eye(n));
    p1 = R'\(-g);
    p = R\p1;
    q = R'\p;
    okay = true;
catch
    okay = false;
    R = [];
    p = [];
    q = [];
end
        