function [ echo_r_mtx, sigma_n_o ] = range_process_CS(echo_mtx, A, sigma_n, lambda, delta)
% Range processing for echo data using compressed sensing
%
% Usage: 
% [echo_r_mtx, sigma_n_o] = range_process_CS(echo_mtx, A, sigma_n, lambda, delta)
%
% Inputs:
%   echo_mtx:     Original echo data
%   A:            Chirp measurement matrix
%   sigma_n:      Input noise standard deviation
%   lambda:       LASSO weight
%   delta:        Convergence normalized difference(2e-7)
%
% Outputs:
%   echo_r_mtx:   Range processing echo data
%   sigma_n_o:    Output noise standard deviation


    % parameters
    if nargin < 5
        delta = 2e-7;
    end
    [M, N] = size(A);
    [lenA, M, lenP] = size(echo_mtx);
    
    % normalization
    J1 = A*A';
    lambda_J=eig(J1);
    A = A / sqrt(lambda_J(end));
    echo_mtx = echo_mtx ./ sqrt(lambda_J(end));
    sigma_n = sigma_n / sqrt(lambda_J(end));
    
    % compressed sensing
    echo_r_mtx = zeros(lenA, N, lenP);
    sigma_n_o_cnt = zeros(lenA, lenP);
    for numA = 1: lenA
        parfor numP = 1: lenP
            sr = echo_mtx(numA, :, numP);
            y = transpose(sr);
            % LASSO
            x_FISTA = FISTA(y, A, lambda, delta);
            % debiased LASSO
            [x_d, sigma_w] = cal_debiased_LASSO(x_FISTA, A, y, lambda, sigma_n);
            sigma_n_o_cnt(numA, numP) = abs(sigma_w);
            echo_r_mtx(numA, :, numP) = x_d;
        end
    end
    
    % calculate output noise
    sigma_n_o = mean(mean(sigma_n_o_cnt));
    
end


%% sub-functions
% algorithm for LASSO
% y: measurements
% A: measurement matrix
% lambda: LASSO weight
% delta: convergence normalized difference
% z: LASSO estimator
function [z] = FISTA(y, A, lambda, delta)

    x_pre = A'*y;
    t = 1;
    z = x_pre;
    z_pre = z;
    t_pre = t;
    N = size(A, 2);
    diff = 1;
    E = eig(A'*A);
    L = E(end);
    temp1 = A'*y/L;
    temp2 = eye(N) - A'*A/L;
    k = 0;
    
    while((diff > delta) && (k < 1000))
        temp = temp1 + temp2 * z_pre;
        x = sft_thd(temp, lambda/L);
        t = 0.5*(1 + sqrt(1+4*t_pre*t_pre));
        z = x + (x - x_pre) * (t_pre-1) / t;
        diff = mean(abs(z_pre - z)); 
        x_pre = x;
        z_pre = z;
        t_pre = t;
        k = k + 1;
    end

end


% soft threshold function
% x: processing object
% thd: threshold
% y: result
function y = sft_thd(x, thd)

    if isequal(size(x), size(thd))
        tmp = abs(x);
        y = x;
        y(tmp <= thd) = 0;
        y(tmp > thd) = (tmp(tmp > thd) - thd(tmp > thd)) .* x(tmp > thd) ./ tmp(tmp > thd);
    else
        tmp = abs(x);
        y = x;
        y(tmp <= thd) = 0;
        y(tmp > thd) = (tmp(tmp > thd) - thd) .* x(tmp > thd) ./ tmp(tmp > thd);
    end

end


% calculate debiased LASSO estimator
% x: LASSO estimator
% A: measurement matrix
% y: measurements
% lambda: LASSO weight
% sigma_n: input noise standard deviation
% x_d: debiased LASSO estimator
% sigma_d: equivalent noise standard deviation
function [x_d, sigma_d] = cal_debiased_LASSO(x, A, y, lambda, sigma)

    [M, N] = size(A);
    gamma = M/N;
    hat_Q1 = gamma;
    [~, D] = eig(A'*A);
    d = diag(D);

    diff = 1;
    T = 1000;
    t = 0;

    while (t < T) && (diff > 1e-6)
        Q1_pre = hat_Q1;
        rho = mean((2 - lambda./(hat_Q1*abs(x) + lambda)).*(abs(x) > 1e-4))/2;
        hat_Q1 = rho/mean(1./(d + (1-rho)*hat_Q1/rho));
        diff = abs(Q1_pre - hat_Q1);
        t = t+1;
    end

    x_d = x + 1/hat_Q1*A'*(y - A*x);

    chi = rho/hat_Q1;
    hat_Q2 = 1/chi - hat_Q1;

    t = -hat_Q2;
    t_prime = -1/mean((1./(d+hat_Q2)).^2);
    G_prime = t + 1/chi;
    G_wprime = t_prime + 1/chi/chi;
    RSS = sum(abs(y - A*x).^2)/M;
    hat_chi = gamma*G_wprime/(2*G_prime-2*chi*G_wprime)*RSS +...
        (-G_wprime*gamma+G_prime*G_prime)/(2*G_prime-2*chi*G_wprime)*sigma^2;
    sigma_d = sqrt(2*hat_chi)/hat_Q1;

end