% c19_super_ig.m
% demo of incremental gradients for 1D super-resolution problem,
% with two sensors with offset sampling.

N = 100;
forw1 = @(arg, x) x(1:2:end) + x(2:2:end);
back1 = @(arg, y) col([y y]');
B{1} = fatrix2('forw', forw1, 'back', back1, 'idim', N, 'odim', N/2);

forw2 = @(arg, x) x(2:2:end-1) + x(3:2:end);
back2 = @(arg, y) [0; col([y y]'); 0];
B{2} = fatrix2('forw', forw2, 'back', back2, 'idim', N, 'odim', N/2-1);

A = [B{1}; B{2}]; % overall system matrix

forwc = @(arg, x) circshift(x, 1) - x;
backc = @(arg, x) circshift(x, -1) - x;
C = fatrix2('forw', forwc, 'back', backc, 'idim', N, 'odim', N);

xtrue = hanning(N); xtrue(70) = 1; % test signal

clf, pl = @(i) subplot(230+i);

pl(1)
plot(xtrue, '.-'), title 'xtrue'

rng(0)
ytrue = A * xtrue;
y = ytrue + 0.001 * randn(size(ytrue));
yb{1} = y(1:N/2);
yb{2} = y(N/2+1:end);
pl(4)
plot(y, '.-'), title 'yi'

x0 = zeros(N,1);

reg = 1.3;
delta = 0.01;
pot = @(t) delta^2 * (sqrt(1 + (t/delta).^2) - 1);
dpot = @(t) t ./ sqrt(1 + (t/delta).^2);
cost_fun = @(x) 0.5 * norm(y - A*x).^2 + reg * sum(pot(C * x));

niter = 100;

step = 1 / (2*2 + reg * 4); % Lipschitz = max curv for SQS

%% run PGD (same as SQS with max curvature)
pl(2)
plot(x0, '.-'), title 'x_0', drawnow
x = x0;
cost_sqs = zeros(niter+1,1);
cost_sqs(1) = cost_fun(x);
for iter=1:niter
	grad = A' * (A * x - y) + reg * (C' * dpot(C * x));
	x = x - step * grad;
	plot(x, '.-'), title 'x PGD'
%	drawnow
	cost_sqs(1+iter) = cost_fun(x);
end
hold on, plot(xtrue, 'r-'), hold off
axis([0 N -0.02 1.02])

pl(3)
plot(0:niter, cost_sqs, '.-'), xlabel 'iter', ylabel 'cost', title 'PGD'


%% run incremental gradient
pl(5)
plot(x0, '.-'), title 'x_0', drawnow
x = x0;
cost_ig = zeros(niter+1,1);
cost_ig(1) = cost_fun(x);
nblock = 2;
for iter=1:niter
	step_n = step / max(iter-30, 1); % under-relaxation
	for block=1:nblock
		Ab = B{block};
		grad = nblock * Ab' * (Ab * x - yb{block}) ...
			+ reg * (C' * dpot(C * x));
		x = x - step_n * grad;
		plot(x, '.-'), title 'x IG'
	end
	cost_ig(1+iter) = cost_fun(x);
end
hold on, plot(xtrue, 'r-'), hold off
axis([0 N -0.02 1.02])

pl(6)
plot(0:niter, cost_ig, '.-'), xlabel 'iter', ylabel 'cost', title 'IG'


iter = 0:niter;
ii = 1:11;
pl(3) % early cost plot to show IG is 2x faster
plot(iter(ii), cost_sqs(ii), 'b.-', iter(ii), cost_ig(ii), 'g.-')
xlabel 'iter', ylabel 'cost'
legend('PGD=SQS', 'IG')

pl(6) % zoomed cost plot to show non-convergence of IG
plot(0:niter, cost_sqs, 'b.-', 0:niter, cost_ig, 'g.-')
xlabel 'iter', ylabel 'cost'
legend('PGD=SQS', 'IG')
axis([0 niter 0.0235 0.0245]), grid
