% Napredne metode digitalne obradbe signala - Projekt
% Compressed sensing - Potpuna rekonstrukcija uz smanjeni broj uzoraka
% Davor Bonaci, davor.bonaci@fer.hr

%% 1. Initialization
clear all;
close all hidden;
pack;

fs = 1e4;   % Sampling frequency, in Hz
N = 32;     % Number of samples
M = 27;     % Number of measurements
f1 = 150;   % Sine frequency one, in Hz
f2 = 400;   % Sine frequency two, in Hz

%% 2. Input signal and its spectrum
t = [0 : 1/fs : (N-1)/fs];
x = sin(2*pi*f1*t) + cos(2*pi*f2*t);
f = fs * (0 : N/2) / N;
X = fft(x);

figure, 
subplot(3, 1, 1), plot(t, x), grid, title('Real "analog" signal'), 
xlabel('Time, seconds'), ylabel('Amplitude'),
subplot(3, 1, 2), plot(f, abs(X(1 : N/2+1))), grid, 
title('Amplitude spectrum of the real "analog" signal'), 
xlabel('Frequency, Hz'), ylabel('Amplitude'),
subplot(3, 1, 3), plot(f, rad2deg(angle(X(1 : N/2+1)))), grid,
title('Phase spectrum of the real "analog" signal'), 
xlabel('Frequency, Hz'), ylabel('Phase, deegres');

%% 3. Computing representation of the input signal
%% in the psi-domain and the psi-matrix
%psi_inv = WavMat(wfilters('sym4'), N, 3);
%psi = inv(psi_inv);
%psi_inv = dctmtx(N)';
%psi = dctmtx(N);

psi_inv = WaveMtx(N, 3, 'sym4');
psi = pinv(psi_inv);

s = (psi_inv * x.').';
figure, stem(s), hold on;

%% Generating sparse signal
s_sparse = s;
s_sparse(find(abs(s_sparse) < 0.5)) = 0;

stem(s_sparse, 'r'), title('Signal in the psi-domain'),
xlabel('Sample'), ylabel('Amplitude'),
legend('Almost sparse signal', 'Really sparse signal');

x_sparse = (psi * s_sparse.').';

figure, plot(t, x), hold on, plot(t, x_sparse, 'r'),
title('Signal in the time domain'), xlabel('Time, seconds'), 
ylabel('Amplitude'), legend('Almost sparse signal', 'Really sparse signal');

x = x_sparse;
s = s_sparse;

%% 4. Computing measurement matrix and performing measurements
randn('state', 0);
fi = 1/N .* randn(M, N);
y = fi * x.';

%% 5. Reconstructing initial signal
theta = fi * psi;

cvx_begin
    variable s_est(length(s), 1);
    minimize( norm(s_est, 1) );
    subject to
        theta * s_est == y;
cvx_end

x_est = (psi * s_est).';

%% 6. Verification
max_error_s = max(abs(s - s_est.'))
max_error_x = max(abs(x - x_est))

figure, hold on, grid, title('Input signal and reconstruction in the psi-domain'),
stem(s, 'b'), stem(s_est, 'g'), stem(s_est.' - s, 'r'),
legend('Input', 'Reconstruction', 'Error');

figure, grid, hold on, title('Input signal and reconstructed signal'), 
xlabel('Time, seconds'), ylabel('Amplitude'),
plot(t, x, 'b'), plot(t, x_est, 'g'), plot(t, x-x_est, 'r'),
legend('Input', 'Reconstruction', 'Error');
