function [x,y,s] = ipdriver(c,A,b,tol,maxit,printlevel)
%IPDRIVER    Primal-dual interior-point method.
%
%  This is the driver function of a very simple IPM for solving the
%  linear programming problem
%
%    min c'x subject to A*x=b, x>=0,            (1)
%
%  where A has full row rank. In each iteration NEWTONSOLVE[R] is called
%  for solving the linear system of step equations.
%
%  [x,y,s] = ipdriver(c,A,b) finds the primal-dual solution x, (y,s) of
%  the linear program in form (1). y are the Lagrange multipliers to the
%  equality constraints and s>=0 the Lagrange multipliers to the
%  nonnegativity constraints on x.
%
%  [x,y,s] = ipdriver(c,A,b,tol) specifies the termination tolerance.
%  The method terminates when the relative primal residual, the relative
%  dual residual and the duality measure are all below tol.
%  Default: 1e-7.
%
%  [x,y,s] = ipdriver(c,A,b,tol,maxit) specifies the maximum number of
%  iterations. Default: 100.
%
%  [x,y,s] = ipdriver(c,A,b,tol,maxit,printlevel) sets the printlevel.
%  0: turn off iteration output
%  1: print primal and dual residual and duality measure
%  2: print centering parameter and step length
%  3: print residuals in the solution of the step equations
%  Default: 1
%
%  See ipexample.m for an illustrative example.
%
%  Lukas Schork, 2015

  [m n] = size(A);
  IPMTT = 0;
    
  % Make sure that b and c are column vectors of dimension m and n.
  if (size(b,2) > 1) b = b'; end
  if (size(c,2) > 1) c = c'; end
  if (~isequal(size(c),[n,1]) || ~isequal(size(b),[m,1]))
    error('problem dimension incorrect');
  end

  % Make sure that A is sparse and b, c are full.
  if (~issparse(A)) A = sparse(A); end
  if (issparse(b)) b = full(b); end
  if (issparse(c)) c = full(c); end

  % Set default values for missing parameters.
  if (nargin < 4 || isempty(tol)) tol = 1e-7; end
  if (nargin < 5 || isempty(maxit)) maxit = 100; end
  if (nargin < 6 || isempty(printlevel)) printlevel = 1; end
  pl = printlevel;

  % ********************************************************************
  % Initialization
  % ==============
  %
  % Choose initial point (x,y,s). We make the very simple choice
  %
  %   x(i)=1, y(j)=0, s(i)=1 for 1<=i<=n, 1<=j<=m.
  %
  % This point will be primal and dual infeasible, but it is perfectly
  % centered.
  % ********************************************************************

  x = ones(n,1);
  y = zeros(m,1);
  s = ones(n,1);

  % Set fixed parameters, see below.
  sigma1   = 0.5;
  sigmamin = 0.05;
  sigmamax = 0.95;

  % Initialize persistent variables.
  iter = 0;
  alpha_x = 0;
  alpha_s = 0;

  % ********************************************************************
  % IPM Main-Loop
  % =============
  %
  % In each step of the interior-point method we solve the Newton system
  %
  %   [ A 0  0 ] (dx)   (b-A*x)            =: (r)
  %   [ 0 A' I ] (dy) = (c-A'*y-s)         =: (p)
  %   [ S 0  X ] (ds)   (simga*mu*e-X*S*e) =: (q),
  %
  % where mu=dot(x,s)/n and sigma\in[sigmamin,sigmamax]. The next
  % iterate is given by
  %
  %   x = x+alpha_x*dx, y = y+alpha_s*dy, s = s+alpha_s*ds,
  %
  % where alpha_x and alpha_s are the primal and dual step length.
  % ********************************************************************

  header(pl);

  while (iter < maxit)

    % Check for termination. We have found a sufficiently accurate
    % solution if the relative primal residual, the relative dual
    % residual and the duality measure are all below tol.
    r = b-A*x;
    p = c-A'*y-s;
    mu = dot(x,s)/n;
    if (norm(r)/norm(b) < tol && norm(p)/norm(c) < tol && mu < tol)
      fprintf('optimal solution found\n');
      break;
    end

    iter = iter+1;

    % Choose the centering parameter sigma. In the first iteration we
    % use the default value sigma1. In all later iterations we choose
    % simga depending on the step lengths in the previous iteration.
    if (iter == 1)
      sigma = sigma1;
    else
      sigma = max(1-alpha_x,1-alpha_s)^5;
    end
    sigma = min(sigma,sigmamax);
    sigma = max(sigma,sigmamin);
    q = sigma*mu*ones(n,1) - x.*s;

    % Solve the Newton system and calculate residuals.
    tic
    NS = newtonsolver(A,x,s);
    [dx,dy,ds] = newtonsolve(NS,r,p,q);
    OneIterTT = toc;
    IPMTT = IPMTT + OneIterTT;
    
    res1 = norm(A*dx-r);
    res2 = norm(A'*dy+ds-p);
    res3 = norm(x.*ds+s.*dx-q);

    % Determine primal and dual step length. Calculate "step to the
    % boundary" alphamax_x and alphamax_s. Then choose 0 < rho < 1
    % heuristically and set step length = rho * step to the boundary.
    idx = dx < 0;
    ids = ds < 0;
    alphamax_x = min([1;-x(idx)./dx(idx)]);
    alphamax_s = min([1;-s(ids)./ds(ids)]);
    rho = 0.95;
    alpha_x = rho*alphamax_x;
    alpha_s = rho*alphamax_s;

    % Make step.
    x = x+alpha_x*dx; y = y+alpha_s*dy; s = s+alpha_s*ds;

    % Print iteration output.
    xinf = norm(A*x-b);
    sinf = norm(A'*y+s-c);
    mu = dot(x,s)/n;
    output(pl,iter,xinf,sinf,mu,sigma,alpha_x,alpha_s,res1,res2,res3);

  end % while (iter < maxiter)

  % The IPM has terminated either because the solution accuracy is
  % reached or the maximum number of iterations is exceeded. Print
  % result.
  %IPMTT = toc;
  
  fprintf('iterations: %4d\n', iter);
  fprintf('primal feasibility: %8.2e\n', norm(A*x-b));
  fprintf('dual feasibility: %8.2e\n', norm(A'*y+s-c));
  fprintf('complementarity: %8.2e\n', dot(x,s)/n);
  fprintf('time: %g\n', IPMTT);
end


% ======================================================================
% header
% ======================================================================

function header(pl)
  if (pl >= 1)
    fprintf(' ');
    fprintf('%4s    ', 'iter');
    fprintf('%8s  ', 'pr feas');
    fprintf('%8s  ', 'dl feas');
    fprintf('%8s  ', 'mu');
  end
  if (pl >= 2)
    fprintf('  ');
    fprintf('%8s  ', 'sigma');
    fprintf('%8s  ', 'alpha_x');
    fprintf('%8s  ', 'alpha_s');
  end
  if (pl >= 3)
    fprintf('  ');
    fprintf('%8s  ', 'res1');
    fprintf('%8s  ', 'res2');
    fprintf('%8s  ', 'res3');
  end
  if (pl >= 1)
    fprintf('\n ====    ========  ========  ========');
  end
  if (pl >= 2)
    fprintf('    ========  ========  ========');
  end
  if (pl >= 3)
    fprintf('    ========  ========  ========');
  end
  if (pl >= 1) fprintf('\n'); end
end


% ======================================================================
% output
% ======================================================================

function output(pl,it,xinf,sinf,mu,sigma,alpha_x,alpha_s,res1,res2,res3)
  if (pl >= 1)
    fprintf(' ');
    fprintf('%4d    ', it);
    fprintf('%8.2e  ', xinf);
    fprintf('%8.2e  ', sinf);
    fprintf('%8.2e  ', mu);
  end
  if (pl >= 2)
    fprintf('  ');
    fprintf('%8.2e  ', sigma);
    fprintf('%8.2e  ', alpha_x);
    fprintf('%8.2e  ', alpha_s);
  end
  if (pl >= 3)
    fprintf('  ');
    fprintf('%8.2e  ', res1);
    fprintf('%8.2e  ', res2);
    fprintf('%8.2e  ', res3);
  end
  if (pl >= 1) fprintf('\n'); end
end
