function rho = WindowLS(A,b,p)
% A is m-by-n, b is m-by-1, and p satisfies n < p <= m
% rank(A(i:i+p-1,:)) = n for i=1:m-p+1
% rho is a column (m-p+1)-vector where rho(i) is the minimum value of
% norm(A(i:i+p-1,:)x - b(i:i+p-1),2)

% Fact: if F is p-by-n and g is p-by-1, and QR = [F g], then
% abs(R(n+1,n+1)) = min norm(Fx - g).

[m,n] = size(A);
rho = zeros(m-p+1,1);
C = [A b];
[Q,R] = qr(C(1:p,:));
rho(1) = abs(R(n+1,n+1));
for i=1:m-p
% Have Q(1:p,1:p)*R(1:p,1:n+1) = C(i:i+p-1,1:n+1)
% Remove the top row. Compute rotations Gp,...,G2 so
% QR = (Q*Gp'*...*G2')(G2*...*Gp*R) where
% e1'* (Q*Gp'*...*G2') = [+-1,0,...,0]
% and (G2*...*Gp*R) is upper Hessenberg
for k=p:-1:2
[c,s] = Givens(Q(1,k-1),Q(1,k));
Q(:,[k-1 k]) = Q(:,[k-1 k])*[c -s;s c];
if k-1<=n+1
R(k-1:k,k-1:n+1) = [c s;-s c]*R(k-1:k,k-1:n+1);
end
end
% Revise Q and R so have Q*R = C(i+1:i+p,1:n+1)
Q = [Q(2:p,2:p) zeros(p-1,1);zeros(1,p-1) 1];
R = [R(2:p,:);A(i+p,:) b(i+p)];
% Upper triangularize R...
for k=1:n+1
[c,s] = Givens(R(k,k),R(p,k));
R([k p],k:n+1) = [c s;-s c]*R([k p],k:n+1);
Q(:,[k p]) = Q(:,[k p])*[c -s; s c];
end
rho(i+1) = abs(R(n+1,n+1));
end

function [c,s] = Givens(x,y)
% Determines c,s with c^2 + s^2 = 1 so that
% [c s;-s c]*[x;y] = [r;0]

% Since -sx + cy = 0 implies cy = sx we have...
if x==0 and y==0
c = 1; s = 0;
else
if abs(x) > abs(y)
tau = y/x; c = 1/sqrt(1 + tau^2); s = c*tau;
else
tau = x/y; s = 1/sqrt(1 + tau^2); c = s*tau;
end
end