%% wavelet L1 examples.
% first show some wavelet functions and their values

nx = 256;
xr = [0:1/(nx-1):1]';
f1 = zeros(size(xr)); 
f1(2:nx-1) = 1/(nx-2) *[1:nx-2];f1(nx) = 1;
f2 = zeros(size(xr)); 
f2(floor(0.15*nx):floor(0.2*nx)) = 0.2; f2(ceil(0.2*nx):floor(0.35*nx)) = 0.6; f2(ceil(0.35*nx):end) = 1;
f3 = zeros(size(xr)); 
f3(floor(0.0625*nx):floor(0.33*nx)) = 0.8; f3(ceil(0.33*nx):end) = 1;
f4 = zeros(size(xr)); 
f4(floor(0.12*nx):floor(0.89*nx)) = 0.1; f4(ceil(0.89*nx):end) = 1;


%% wavelets the hard way...
wname =  'haar';%'db4';%'coif4';%'db4'; %'haar';%
Nw = floor(log2(nx));
ExplicitWM = true;                   % this builds wavelets as a matrix
nx = length(xr);

if ExplicitWM
[lnCT,lnS] = wavedec(f2,Nw,wname);
nwc = length(lnCT); % number of wavelet coefficients;
disp('Building explicit wavelet matrix');
WM = zeros(nwc,nx);
tic;
for j = 1:nx
    xp = zeros(nx,1);
    xp(j) = 1;
    
    WM(:,j) = wavedec(xp,Nw,wname);
end
toc;
disp('Inverting Wavelet Matrix');
tic;
WMI = pinv(WM);
toc;
end
%% show the wavelet matrix
figure; imagesc(WM);

figure;
%subplot(4,2,1); imagesc(WM);
subplot(4,2,1); plot(WM(192,:)); 
subplot(4,2,2); plot(WM(202,:)); 
subplot(4,2,3); plot(WM(72,:)); 
subplot(4,2,4); plot(WM(94,:)); 
subplot(4,2,5); plot(WM(42,:)); 
subplot(4,2,6); plot(WM(54,:)); 
subplot(4,2,7); plot(WM(20,:)); 
subplot(4,2,8); plot(WM(28,:)); 


%%
figure(12); clf;
subplot(3,4,1); plot(xr,f1);
subplot(3,4,2); plot(xr,f2);
subplot(3,4,3); plot(xr,f3);
subplot(3,4,4); plot(xr,f4);


wf1 = abs(WM*f1);
wf2 = abs(WM*f2);
wf3 = abs(WM*f3);
wf4 = abs(WM*f4);

subplot(3,4,5); plot(wf1);title(['WL1 : ',num2str(sum(wf1))]);
subplot(3,4,6); plot(wf2);title(['WL1: ',num2str(sum(wf2))]);
subplot(3,4,7); plot(wf3);title(['WL1 : ',num2str(sum(wf3))]);
subplot(3,4,8); plot(wf4);title(['WL1 : ',num2str(sum(wf4))]);


subplot(3,4,9); plot(wf1.^2/2);title(['WL2 : ',num2str(sum(wf1.^2)/2)]);
subplot(3,4,10); plot(wf2.^2/2);title(['WL2 : ',num2str(sum(wf2.^2)/2)]);
subplot(3,4,11); plot(wf3.^2/2);title(['WL2 : ',num2str(sum(wf3.^2)/2)]);
subplot(3,4,12); plot(wf4.^2/2);title(['WL2 : ',num2str(sum(wf4.^2)/2)]);

%% Do regularised inversion using TV and quadratic programming
%
%addpath('../Lin1D/');
sigma = 0.1; % std.dev. of added white noise

f = zeros(nx,1);
f(floor(0.187*nx):floor(0.2*nx)) = -1; f(floor(0.312*nx):floor(0.35*nx)) = 1.5; f(floor(0.5*nx):floor(0.6*nx)) = 2; % arbitrary function

[y,K] = linblur(f,0.04);
yn = y + sigma*randn(nx,1); % add white noise, 

figure (3);clf; 
subplot(1,2,1);hold on;
plot(y,'k');
plot(yn,'g');
title('Data Space');
subplot(1,2,2);hold on;
plot(f,'k');
title('Solution Space');

%% set up matrices for quadratic programming
 
onen = ones(nwc,1);
szeron = sparse(nx,nx); % sparse n X n zero

% choose regln parameter and set up matrices
alphaTVQ = 8e-2;
H = [2*K'*K zeros(nx,2*nwc) ; zeros(2*nwc,nx+2*nwc)];
b = [-2*K'*yn; alphaTVQ*onen; alphaTVQ*onen];
B = sparse(nx+2*nwc,nx+2*nwc);
B(nx+1:nx+2*nwc,nx+1:nx+2*nwc) = -speye(2*nwc);
Ceq = sparse(nx,3*nx);
Ceq(1:nwc,1:nx) = WM; 
Ceq(1:nwc,nx+1:nx+nwc)   = -speye(nwc);
Ceq(1:nwc,nx+nwc+1:nx+2*nwc) = speye(nwc);


%% sanity checks;
Wf = WM*f;
vp = zeros(nwc,1); vm = vp;
dfp = find(Wf>0);
dfm = find(Wf<0);
vp(dfp) = Wf(dfp);
vm(dfm) = -Wf(dfm);

h = [f;vp;vm];
Ch = Ceq*h;


%% solve it...
x0 = [yn;zeros(2*nwc,1)]; % initial step
nsol = nx+2*nwc;
[xqp,fval,exitflag,output,lambda]= quadprog(H,b,B,zeros(nsol,1),Ceq,zeros(nwc,1));
xfmincon = fmincon(@(x) (0.5*x'*H*x + b'*x),x0,B,zeros(nsol,1),Ceq,zeros(nwc,1));

figure(3);
subplot(1,2,2);
plot(xqp(1:nx),'--m');
plot(xfmincon(1:nx),'c');
subplot(1,2,1);
plot(K*xqp(1:nx),'--m');
plot(K*xfmincon(1:nx),'c');

%% include TK1 as a comparison
% for orthonormal bases, this is same as Tk0!
alphaTK1 =  alphaTVQ;
xTK1 = (K'*K + alphaTK1*WM'*WM)\(K'*yn);

figure(3);
subplot(1,2,2);
plot(xTK1,'b');
legend('f_{true}','f_{QP}','f_{fmincon}','f_{TK1}');

subplot(1,2,1);
plot(K*xTK1,'b');
legend('g_{0}','g^{obs}','g_{QP}','g_{fmincon}','g_{TK1}');


%%
function [y,K] = linblur(f,s)
n = size(f,1);
K = zeros(n,n);
h = 1/n;
C = h/sqrt(2*pi*s*s);
x = [1:n];
k = C*exp(-x.^2 * h*h/(2*s*s));
for i = 1:n
    K(i,1:i) = k(i:-1:1);
    K(i,i+1:n) = k(2:n-i+1);
end
y = K*f;
end