clc
clear
close all

%% Parameters

N = 64*1000; %length of the signal
k = 5;  %sparsity of the signal in the dictionary
d = 11; %number of quantization levels

b = 20*1000; %beginning of the envelope
l = N-b; %lenght of the envelope
cenv = 1/(l+1); %coefficient of the envelope
env = ones (N, 1); %prepares the vector

for j = b:N
    env(j) = 1-((j-b)*cenv);
end

ienv = ones (N, 1);
% for j = b:N
%     ienv(j) = (1/env(j)); 
% end

ienv(b:N) = 1./env(b:N);



% % % % % % % Check_env_ienv = env .* ienv;
% 'DR' ... Douglas-Rachford algorithm
% 'LP' ... linear programming (requires Matlab optimization toolbox)
algorithm =   'DR' %'LP'%
                    


%% Dictionary and the original sparse signal
%  A = dctmtx(N)';  % dictionary in which the signal will be sparse = inverse DCT^
A = @(x) idct(x);  % dictionary in which the signal will be sparse = inverse DCT
At = @(x) dct(x); %its inverse (= transpose)

if k>N
    error('Sparsity cannot be greater then signal length/number of atoms');
end

% generate random support
support = randperm(N);
support = support(1:k);

% Determine the coefficient values
x_support =  1 + 3*rand([k 1]);
x_support =  x_support .* (((rand([k 1])>.5)*2)-1); %randomizing signs
x = zeros(N,1);
x(support) = x_support; %complete vector including zero coefs

% synthesize signal
y = A(x);

y_env = y .* env;

% or just load a fixed dataset
% clear
% load('dequantization_dataset_01')

% % show coefficients
fig_coef_Y = figure;
Y = sgram(y, 16000, 'dynrange',50);
hold on;
title('Koeficienty Gaborovy transformace původního signálu');

fig_coef_Y_env = figure;
Y = sgram(y_env, 16000, 'dynrange',50);
hold on;
title('Koeficienty Gaborovy transformace efektovaného signálu')

fs = 16000;
%% % WINPARAM % % %
% [param.w, param.a] = win_param(64, fs, 4);
length_win = 30;
rel_shift = 4;

w = length_win*fs/1000;
w_temp = (w/rel_shift - floor(w/rel_shift)); 
    if w_temp ~= 0
        w = (round(w/rel_shift))*rel_shift;
    end

a = w/rel_shift;
M = w;
% h_support =  1 + 3*rand([500 1]);
% 
% test = fft(h_support);

% % % /WINPARAM % % %
%% frame
F = frame('dgtreal', {'hann',w}, a, M);
F = frametight(F);

FrAna = frana(F, y);
FrSyn = frsyn(F, FrAna);
FrSyn(N+1 : end, :) = [];

% fig_frana_frsyn = figure;
% h_check_frana_frsyn = plot(y-FrSyn);
% hold on;
% title('difference between original signal and ana-syn reconstruction');

%% Quantize signal
%Determine the max and min value of the signal. The quantization
%levels are spread uniformly between them
min_y_env = min(min(y_env));    
max_y_env = max(max(y_env));
range_env = max_y_env - min_y_env;

quant_step_env = range_env / (d-1);
dec_bounds_env = (min_y_env+quant_step_env/2) : quant_step_env : (max_y_env-quant_step_env/2);  %decision boundaries lie in the middle
quant_levels_env = min_y_env : quant_step_env : max_y_env; %quantization levels

%use Matlab function to quantize signal
[index_env, y_quant_env] = quantiz(y_env, dec_bounds_env, quant_levels_env);
y_quant_env = y_quant_env';

%constraints for the signal samples;
lower_dec_bounds_env = y_quant_env - (quant_step_env/2);
upper_dec_bounds_env = y_quant_env + (quant_step_env/2);

%the highest and lowest boundary coincide precisely with the quantization levels!:
min_quant_level_env = quant_levels_env(1);
max_quant_level_env = quant_levels_env(end);
upper_dec_bounds_env(upper_dec_bounds_env > max_quant_level_env) = max_quant_level_env;
lower_dec_bounds_env(lower_dec_bounds_env < min_quant_level_env) = min_quant_level_env;



%% Show original vs. quantized signal
%define colors
grey = 0.6* ones(1,3);
lightgrey = 0.8* ones(1,3);
black = [0,0,0];
blue = [0.251,0.584,0.808];
orange = [0.851,0.325,0.098];
green = [0 1 0];

%plot of the signals

fig_time_env = figure;
h_orig = plot(y, '.-', 'Color', black);
hold on;
h_orig_env = plot(y_env, '.-', 'Color', blue);
h_quant_env = plot(y_quant_env, '.-', 'Color', orange);
title('Původní a kvantovaný signál - efektovaný');
 
%plot signal constraints
h_signal_constr_env = plot(upper_dec_bounds_env, 'Color', lightgrey);
plot(lower_dec_bounds_env, 'Color', lightgrey);

for j=1:d 
    yPos = quant_levels_env(j);
    h_q_lev_env = plot(ones(1,N) * yPos, 'Color', grey);
end

for j=1:(d-1) 
    yPos = dec_bounds_env(j);
    h_dec_b_env = plot(ones(1,N) * yPos, ':', 'Color', grey); 
end

axis tight

%legend
uistack(h_orig_env,'top');
uistack(h_quant_env,'top');
h_legend_time_env = legend([h_orig, h_orig_env, h_quant_env, h_q_lev_env, h_dec_b_env, h_signal_constr_env ], 'Původní', 'Ztlumený', 'kvantovaný', 'kvantizační hladiny', 'rozhodovací úrovně', 'meze signálu');

%quanization noise
fig_quant_noise_env = figure;
quant_noise_env = y_env - y_quant_env;
h_quant_noise_env = plot(quant_noise_env, 'Color', blue);
hold on;
title('Kvantizační šum - efektovaný signál');



%% Sparse dequantization
switch algorithm
    case 'LP'  %dequantization using linear programming (doubles the number of variables)
        f = (ones([1 2*N]));
        
        b = [-lower_dec_bounds; upper_dec_bounds];
        Amatrix = A(eye(N)); %generate explicit dictionary matrix
        A_ = [A -A; -A A];
        A_ = [Amatrix -Amatrix; -Amatrix Amatrix];
        lb = zeros(2*N,1);  %all variables must be nonnegative
        
        w = linprog(f,A_,b,[],[],lb);  %l1-minimization via lin. program
        
        uv = reshape(w,N,2);      %split w into two
        u = uv(:, 1);
        v = uv(:, 2);
        x_reconstructed = v - u;    %sparse vector is determined by subtracting the two non-negative
        y_dequant = A(x_reconstructed);    %reconstruct signal
        
        sol = x_reconstructed;
        
    case 'DR'  % Douglas-Rachford
        
        param.lower_lim = lower_dec_bounds_env;
        param.upper_lim = upper_dec_bounds_env;
        
        param.lower_lim = param.lower_lim .* ienv;
        param.upper_lim = param.upper_lim .* ienv;
        
        indi_thr = 10e-5; % threshold for identifying the point to lie in the set
        
        f1.eval = @(x) norm(x,1);
        f1.prox = @(x,T) sign(x).*max(abs(x)-T, 0);
        
%         f2.eval = @(x) 1 / (1 - ( any((A(x(:))-param.upper_lim)>indi_thr)) || any((A(x(:))-param.lower_lim) < -indi_thr) ) - 1; %zero if x is inside boundaries, Inf otherwise
%         f2.prox = @(x,T) At(proj_box(A(x),[],param)); %box projection in the signal space (thanks to DCT being orthogonal)
%         
%         f2.eval = @(x) 1 / (1 - ( any((A(x(:))-param.upper_lim)>indi_thr)) || any((A(x(:))-param.lower_lim) < -indi_thr) ) - 1; %zero if x is inside boundaries, Inf otherwise
        f2.eval = @(x) eps;
%         O = proj_box(A(x),[],param;
%         P = A(x);
%         N =  O - P ;
%         M = pinv(A);
%         f2.prox = @(x,T) x - M; %
%         Ox = proj_box(frsyn(F, x),[],param);
%         FrAnaProx = frana(F, (FrSyn - proj_box(FrSyn,[],param)));
%         FrAnaProx(N+1 : end, :) = [];

        f2.prox = @(x,T) x - frana(F, ((postpad(frsyn(F, x),N) - proj_box(postpad(frsyn(F, x),N),[],param)))); 

        
        % UNLOCBOX version %%%%%%%%%%%%%
%         setting different parameter for the simulation
        paramsolver.verbose = 5;  % display parameter
        paramsolver.maxit = 300;        % maximum iteration
        paramsolver.tol = 10e-7;        % tolerance to stop iterating
        paramsolver.lambda = 1;   % step for DR algorithm (default 1)
        paramsolver.gamma = 1e-2;        % here threshold for soft thresholding
        
        
       
        [sol, info] = douglas_rachford(frana(F, y_quant_env), f1, f2, paramsolver); %% tady jsem to předtím krmil y_quant bez obálky
        info

        sol = f2.prox(sol,[]); %final projection into the constraints
       
        y_dequant = frsyn(F, sol);
        
        fig_y_Previous = figure;
        plot(y);
        title('Signál původní - před obálkou');
        
        fig_y_Faded = figure;
        plot(y_env);
        title('Signál původní - ztlumený');
        
        
        y_dequant = postpad(y_dequant,N);
        
        fig_y_Gab = figure;
        plot(y_dequant);
        title('Signál rekonstruovaný pomocí Gaborovy transformace');
        
%         pause
%         
%         %% manual version %%%%%%%%%%%%%      
%         starting point
%         DR_y = A(y_quant);
%         DR_x_old = DR_y;
%         
%         relat_change_coefs = 1;
%         relat_change_obj = 1;
%         cnt = 1; %iteration counter
%         obj_eval = [];
%         
%         while relat_change_coefs > 0.00001
%         while relat_change_obj > 
%             DR: gamma = 1paramsolver.tol
%             DR_x = f2.prox(DR_y,[]);
%             obj_eval = [obj_eval, f1.eval(DR_x) + f2.eval(DR_x)]; %record values of objective function
%             DR_y = DR_y + paramsolver.lambda*(f1.prox(2*DR_x-DR_y, paramsolver.gamma)-DR_x);
%             if cnt > 1
%                 relat_change_coefs = norm(DR_x-DR_x_old) / norm(DR_x_old);
%                 relat_change_obj = norm(obj_eval(end) - obj_eval(end-1)) / norm(obj_eval(end-1));
%                 if paramsolver.verbose > 1
%                     fprintf('  relative change in coefficients: %e \n', relat_change_coefs);
%                     fprintf('  relative change in objective fun: %e \n', relat_change_obj);
%                     fprintf('\n');
%                 end
%             end
%             DR_x_old = DR_x;
%             cnt = cnt + 1;
%             
%         end
%         
%         DR_x = f2.prox(DR_y); %final projection into the constraints
%         y_dequant = A(DR_x); %dequantized signal
%         
%         disp(['Finished after ' num2str(cnt) ' iterations.'])
%         
%         
%         compare UNLOCBOX with manual
%         figure
%         plot([y_dequant A(sol)])
%         norm(y_dequant - A(sol))
%         title('UNLOCBOX vs. manual solution')
%         
%         behaviour of objective through iterations
%         figure
%         plot(obj_eval)
%         title('Objective function value (after projection into constraints in each iteration)')
    
end




% % Show results
% % 
% % figure(fig_time)
% % h_dequant = plot(y_dequant, '.-', 'Color', green);
% % uistack(h_dequant,'top');
% % delete(h_legend_time)
% % h_legend_time = legend([h_orig, h_quant, h_dequant, h_q_lev, h_dec_b, h_signal_constr ], 'original', 'quantized', 'dequantized', 'quantiz. levels', 'decision bounds', 'signal constraints');
% % title('Original, quantized and dequantized signals')
% % 
% % quantization and reconstruction errors
% % figure(fig_quant_noise)
% % h_dequant_error = plot(y-y_dequant, 'Color', green);
% % title('Quantization error and error of reconstruction (i.e. original - reconstr.)');
% % axis tight
% % legend([h_quant_noise h_dequant_error], 'Quantizat. error', 'Error of reconstr.')
% % 
% % Previous and reconstructed signal
% % fig_compare_ienv = figure;
% % h_pre = plot(y, '.-', 'Color', blue);
% % hold on;
% % h_post = plot(y_dequant, '.-', 'Color', orange);
% % h_times = plot(y_quant_env .* ienv, '.-', 'Color', green);
% % title('Original and reconstructed signal');
% % 
% % for j=1:d %quantization levels
% %     yPos = quant_levels(j);
% %     h_q_lev = plot(ones(1,N) * yPos, 'Color', grey);
% % end
% % 
% % for j=1:(d-1) %decision boundaries
% %     yPos = dec_bounds(j);
% %     h_dec_b = plot(ones(1,N) * yPos, ':', 'Color', grey); 
% % end
% % 
% % axis tight
% % legend([h_pre h_post h_times], 'Original', 'Reconstructed', 'Multiplied only')
% % uistack(h_pre,'top');
% % uistack(h_post,'top');
% % uistack(h_times,'top');
% % 
% % coefficients of reconstructed signal
% % figure(fig_coef)
% % hold on
% % h_coefs_dequant = bar(sol,'FaceColor',green);
% % title('Coefficients of original and reconstructed signals');
% % legend([h_coefs_orig h_coefs_dequant], 'Coefs of orig. signal', 'Coefs of dequant. signal')
% % axis tight
% % 
% % 
% % figure(fig_time)
% % axis tight

%%Affected signal
figure(fig_time_env)
h_dequant = plot(y_dequant, '.-', 'Color', green);
uistack(h_dequant,'top');
delete(h_legend_time_env)
h_legend_time_env = legend([h_orig, h_orig_env, h_quant_env, h_dequant, h_q_lev_env, h_dec_b_env, h_signal_constr_env ], 'Původní', 'Ztlumený', 'kvantovaný','dekvantovaný', 'kvantizační hladiny', 'rozhodovací úrovně', 'meze signálu');
title('Původní, kvantovaný a dekvantovaný signál')

%quantization and reconstruction errors
figure(fig_quant_noise_env)
h_dequant_error = plot(y-y_dequant, 'Color', green);
title('Kvantizační chyba a chyba rekonstrukce');
axis tight
legend([h_quant_noise_env h_dequant_error], 'Kvantizační chyba', 'Chyba rekonstrucke')

%Previous and reconstructed signal
fig_compare_ienv = figure;
h_pre = plot(y, '.-', 'Color', blue);
hold on;
h_post = plot(y_dequant, '.-', 'Color', orange);
h_times = plot(y_quant_env .* ienv, '.-', 'Color', green);
title('Původní a rekonstruovaný signál');

for j=1:d %quantization levels
    yPos = quant_levels_env(j);
    h_q_lev = plot(ones(1,N) * yPos, 'Color', grey);
end

for j=1:(d-1) %decision boundaries
    yPos = dec_bounds_env(j);
    h_dec_b = plot(ones(1,N) * yPos, ':', 'Color', grey); 
end

axis tight
legend([h_pre h_post h_times], 'Původní', 'Dekvantovaný', 'Pronásobený');
uistack(h_pre,'top');
uistack(h_post,'top');
uistack(h_times,'top');

%coefficients of reconstructed signal

% fig_coef = figure;
% h_coefs_analog = bar(frana(F, y),'FaceColor', blue);
% hold on;
% h_coefs_dequant = bar(sol,'FaceColor',green);
% title('Gabor coefficients of original and reconstructed signals');
% legend([h_coefs_analog h_coefs_dequant], 'Coefs of orig. signal', 'Coefs of dequant. signal');
axis tight


figure(fig_time_env);
axis tight

