% This program produces Fig. 4 from paper:
% Stankovic, L. & Dakovic, M. Circuits Syst Signal Process (2018). https://doi.org/10.1007/s00034-018-0955-9
% Copyright by M. Dakovic, Lj. Stankovic, Oct. 2018.


% CS parameters
clear all
rng('default')
rand(8,1);
randn(16,1);
Ks=8;
N=128;
M=64;

% Measurement matrix
%Amm=randn(N);
Amm=dctmtx(N);
pp=randperm(N);pp=pp(1:M);
Amm=Amm(pp,:); pp3=pp;

% Normalization
for m=1:M
    Amm(m,:)=Amm(m,:)/sqrt(sum(Amm(m,:).^2));
    if sum(Amm(m,:))<0, Amm(m,:)=-Amm(m,:); end
end

X0=zeros(N,1);
pp=randperm(N,Ks);
X0(pp)=sign(rand(Ks,1)-0.5).*(3+randn(Ks,1));
y=Amm*X0;

% Choose M columns from measuring matrix with good conditional number
C0=Inf;
for it=1:1000
    pp=randperm(N);
    p1=sort(pp(1:M));
    p2=sort(pp(M+1:end));
     
    AM=Amm(:,p1);
    A3=Amm(:,p2);
    cc=cond(AM);
    if cc<C0, AMo=AM; C0=cc; pp1=p1;pp2=p2; A3o=A3; end
end
AM=AMo;
p1=pp1;
p2=pp2;
A3=A3o;

% Median form formulation
A=[eye(N-M);inv(AM)*A3];
b=[zeros(N-M,1);inv(AM)*y];

[~,K]=size(A);

% Gradient based soultion
Xm=max(abs(b));
X=zeros(K,1);
Delta=Xm/10;
Nit=10000;

for it=1:Nit
    if any(it==10*[100,200,300,400,500,600,700,800,900])
        Delta=Delta/sqrt(10);
    end
    g=zeros(K,1);
    
    for k=1:K
        X1=X;X1(k)=X1(k)-Delta;
        X2=X;X2(k)=X2(k)+Delta;
        M1=sum(abs(A*X1-b));
        M2=sum(abs(A*X2-b));
        g(k)=(M2-M1)/(2*Delta);
    end
    
    X=X-2*Delta*g;
    M3=sum(abs(A*X-b));
    MM(it)=M3;
end


Xremaining=inv(AM)*y-inv(AM)*A3*X;
Xfull=zeros(N,1);
Xfull(p2)=X;
Xfull(p1)=Xremaining;

figure(1)
subplot(2,3,1)
stem(0:N-1,X0,'.')
a=axis;a(2)=N;axis(a);
title('Original signal','fontweight','normal')
xlabel('k')

subplot(2,3,2)
stem(1:M,y,'r.')
a=axis;a(2)=M+1;axis(a);
title('Measurements','fontweight','normal')
xlabel('measurement index')

subplot(2,3,3)
stem(0:N-1,Xfull,'.')
a=axis;a(2)=N;axis(a);
title('Reconstructed signal','fontweight','normal')
xlabel('k')

cond(AM)