function b = rq_fnm(X, y, p)
% Construct the dual problem of quantile regression
% Solve it with lp_fnm
% 
%
[m n] = size(X);
u = ones(m, 1);
a = (1 - p) .* u;
b = -lp_fnm(X', -y', X' * a, u, a)';

function y = lp_fnm(A, c, b, u, x)
% Solve a linear program by the interior point method:
% min(c * u), s.t. A * x = b and 0 < x < u
% An initial feasible solution has to be provided as x
%
% Function lp_fnm of Daniel Morillo & Roger Koenker
% Translated from Ox to Matlab by Paul Eilers 1999
% Modified by Roger Koenker 2000--

% Set some constants
  beta = 0.9995;
  small = 1e-5;
  max_it = 50;
  [m n] = size(A);

% Generate inital feasible point
  s = u - x;
  y = (A' \  c')';
  r = c - y * A;
  z = r .* (r > 0);
  w = z - r;
  gap = c * x - y * b + w * u;

% Start iterations
  it = 0;
  while gap > small & it < max_it
    it = it + 1;

%   Compute affine step
    q = 1 ./ (z' ./ x + w' ./ s);
    r = z - w;
    Q = sparse(1:n,1:n,q);
    AQ = A * Q;
    AQA = AQ * A';
    rhs = AQ * r';
    dy = (AQA \ rhs)';
    dx = q .* (dy * A - r)';
    ds = -dx;
    dz = -z .* (1 + dx ./ x)';
    dw = -w .* (1 + ds ./ s)';
    
%   Compute maximum allowable step lengths
    fx = bound(x, dx);
    fs = bound(s, ds);
    fw = bound(w, dw);
    fz = bound(z, dz);
    fp = min(fx, fs);
    fd = min(fw, fz);
    fp = min(min(beta * fp), 1);
    fd = min(min(beta * fd), 1);

%   If full step is feasible, take it. Otherwise modify it
    if min(fp, fd) < 1
    
%     Update mu
      mu = z * x + w * s;
      g = (z + fd * dz) * (x + fp * dx) + (w + fd * dw) * (s + fp * ds);
      mu = mu * (g / mu) ^3 / ( 2* n);

%     Compute modified step
      dxdz = dx .* dz';
      dsdw = ds .* dw';
      xinv = 1 ./ x;
      sinv = 1 ./ s;
      xi = mu * (xinv - sinv);
      rhs = rhs + A * ( q .* (dxdz - dsdw -xi));
      dy = (AQA \ rhs)';

      dx = q .* (A' * dy' + xi - r' -dxdz + dsdw);
      ds = -dx;
      dz = mu * xinv' - z - xinv' .* z .* dx' - dxdz';
      dw = mu * sinv' - w - sinv' .* w .* ds' - dsdw';

%     Compute maximum allowable step lengths
      fx = bound(x, dx);
      fs = bound(s, ds);
      fw = bound(w, dw);
      fz = bound(z, dz);
      fp = min(fx, fs);
      fd = min(fw, fz);
      fp = min(min(beta * fp), 1);
      fd = min(min(beta * fd), 1);
      
    end

%   Take the step
    x = x + fp * dx;
    s = s + fp * ds;
    y = y + fd * dy;
    w = w + fd * dw;
    z = z + fd * dz;

    gap = c * x - y * b + w * u;
    %disp(gap);
  end

function b = bound(x, dx)
% Fill vector with allowed step lengths
% Support function for lp_fnm
b = 1e20 + 0 * x;
f = find(dx < 0);
b(f) = -x(f) ./ dx(f);
