clc;
clear;
%close all;

%% 参数设置
sigma_n = 0.1;
gamma = 0.5;

% 信号参数
B = 5e5;        %信号带宽
Tp = 100e-6;    %脉宽100us
fs = 2 * B;     %采样频率
Ts = 1 / fs;    %采样周期
K = B / Tp;     %线性调频率
fc = 1e8;       %载波频率

Tr = 1e-3;

t = 0: 1/fs: Tr - 1/fs;
t2 = 0: 1/fs/2: Tr - 1/fs/2;

c = 3e8; % 光速

distance_max = (Tr-Tp) * c / 2;

target_scattering = [0.8, 1, 0.9]; %扩展目标各点散射强度


%% 生成矩阵 A
% 生成发射信号 signal_t 及 
N = Tr * fs;
N_high = Tp * fs;

signal_t  = zeros(1, N);
signal_td  = zeros(1, N);
for i = 1:N_high
    tp = (i - 1) * (1 / fs);
    signal_t(1, i) = exp(1j*2*pi*(fc*tp+0.5*K*tp.^2));
    tp2 = (i - 0.5) * (1 / fs);
    signal_td(1, i + 1) = exp(1j*2*pi*(fc*tp2+0.5*K*tp2.^2));
end

A = generate_matrix_new(signal_t.', signal_td.');

temp = 0;
for i = 1: size(A, 1)

    for j = 1: size(A, 2)

        temp = temp + abs(A(mod(i, size(A, 1))+1, mod(j+1, size(A, 2))+1) - A(i, j));

    end

end

%% 生成回波 y
% 设置目标 - 扩展目标，由三个点组成
distance1 = 52000;
tau1 = distance1 * 2 / c;
n_tau1 = round(tau1 * fs);

alpha1 = 0.6; % 扩展目标整体散射强度
signal_r1_t1  = zeros(1, N);
signal_r2_t1  = zeros(1, N);
signal_r3_t1  = zeros(1, N);

for i = 1:N
    temp = i - n_tau1;
    if temp >= 1 && temp <= N_high
        signal_r1_t1(1, i) = alpha1 * target_scattering(1) * signal_t(1, temp);
        if i + 1 <= N
            signal_r2_t1(1, i + 1) = alpha1 * target_scattering(2) * signal_t(1, temp);
        end
        if i + 1 <= N
            signal_r3_t1(1, i + 2) = alpha1 * target_scattering(3) * signal_t(1, temp);
        end
    end
end
signal_r1 = signal_r1_t1 + signal_r2_t1 + signal_r3_t1;

% 回波
signal_r = signal_r1;

% 加入噪声
noise = random('Normal', 0, sigma_n/sqrt(2), 1, length(signal_r)) + 1j * random('Normal', 0, sigma_n/sqrt(2), 1, length(signal_r));
signal_r_n = signal_r + noise;

y = signal_r_n.';

%% 理论 x
distance_node = round((distance1 * 2 / c) * fs);
x_t = zeros(1, 2 * N);
for i = 1: length(target_scattering)
    x_t((distance_node + i) * 2 - 1) = alpha1 * target_scattering(i);
end
x = x_t.';

%% Experiment

%% Parameters setting
lambda = 0.002;
alpha = 1/4;
delta = 1e-8*alpha;
iter_max = round(2000/alpha);
n = size(A, 2);

% J = A'*A;
J1 = A*A';
lambda_J=eig(J1);
% histogram(lambda_J, 100);

%% Normalized
A = A / sqrt(lambda_J(end));
y = y / sqrt(lambda_J(end));
sigma_n = sigma_n / sqrt(lambda_J(end));

%% cVAMP
tic;
[x_VAMP, x_d, hat_Q1, sigma_d, ifcvg] = cVAMPa_dampling(y, A, lambda, alpha, delta, iter_max, sigma_n);
toc;

%% cvx
tic;
cvx_begin quiet
    variable x_cvx(n, 1) complex
    z = lambda*sum(abs(x_cvx)) + 0.5*sum(pow_abs((y - A * x_cvx), 2));
    minimize(z)
cvx_end

[x_d_cal, hat_Q1_cal, sigma_d_cal] = cal_debiased_LASSO(x_cvx, A, y, lambda, sigma_n);
toc;

sigma_ex = std(x_d_cal - x, 1);
tmp = (x_d_cal - x)/sigma_ex;

[h_r, p_r, k_r, c_r] = kstest(real(tmp)*sqrt(2));
[h_i, p_i, k_i, c_i] = kstest(imag(tmp)*sqrt(2));

%% results
% whether cVAMP algorithm converges
ifcvg 

% whether the output of cVAMP converges to the LASSO solution
sum(abs(x_cvx - x_VAMP))

% check the results from cVAMP and "calculation"
abs(hat_Q1 - hat_Q1_cal)
abs(sigma_d - sigma_d_cal)
sum(abs(x_d - x_d_cal))

% accuracy of estimating the variance
abs(sigma_d_cal - sigma_ex)/abs(sigma_ex)

% p-value of KS-test 
% the larger, the higher probability it is drawn from Gaussian distribution
p_r
p_i