clc
clear
close all;

%% 参数设置
B = 5e6;              % 信号带�??5MHz
Tp = 20e-6;          % 脉�??100us
fs = 2 * B;             % 采样频率
Ts = 1 / fs;         % 采样周期
K = B / Tp;         % 线性调频率
fc = 1.25e9;        % 载波频率1.25GHz
PRF = 5000;         % 脉冲重�?��?�率
Tr = 1 / PRF;       % 脉冲重�?�间�?



numP = 64;          % 脉冲数量

t = 0: 1 / fs: Tr - 1 / fs;
tP = 0: 1 / fs: Tr * numP - 1 / fs;

c = 3e8;            % 光�?

slow_len = 128;     

%%
sigma_n = 0.1;
alpha_prop = 0.8;

% SNR = -30;
SNR = [-5,0,5,8,10,12,15,20,25];
len_SNR = length(SNR);

P_fa = [1e-4, 5e-4, 1e-3, 5e-3, 1e-2, 5e-2, 1e-1, 5e-1, 1];
len_P_fa = length(P_fa);

rep_time = 200;

% rou >= c / 2B = 15(m)

%%
delta_VAMP = 1e-6;      
iter_max = 1000;   
lambda_val = 0.05;
lambda = zeros(slow_len,1) + lambda_val;

%% 产生发射信号
N = Tr * fs;
N_high = Tp * fs;

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

multiple_r = signal_t(1, 1: N) * signal_t(1, 1: N)';
% figure(1);
% subplot(311)
% plot(tP,real(signal_t));
% xlabel('时间/t');ylabel('幅度');
% title('发射信号');

%% 设置�?�? 1
distance1 = 18000;
v1 = 187.5;

tau1 = distance1 * 2 / c;
n_tau1 = round(tau1 * fs);

f_d1 = 2 * fc * v1 / c;

alpha1 = alpha_prop;
% alpha1 = 0;

signal_r1  = zeros(1, N * numP);
for j = 1: numP
    for i = 1: N
        temp = i - n_tau1;
        if temp >= 1 && temp <= N_high
            tp = ((j - 1) * N + i - n_tau1) * (1 / fs);
            signal_r1(1, i + (j-1)*N) = alpha1 * exp(1j*2*pi*((fc - f_d1) * tp + 0.5 * K * tp .^ 2));
        end
    end
end

%% 生成回波
signal_r = signal_r1;
% figure(1);
% subplot(312)
% plot(tP, real(signal_r));
% xlabel('时间/t');
% ylabel('幅度');
% title('回波信号');

%%
P_fa_CROD_cnt = zeros(len_SNR, len_P_fa, rep_time);
P_d_CROD_cnt = zeros(len_SNR, len_P_fa, rep_time);

F_ori = dftmtx(slow_len);
F = F_ori(1:numP,:);
multiple_d = F(:,1)' * F(:,1);
F_inv = conj(F)/slow_len;

A = (sqrt(slow_len) * eye(numP)) * F_inv;

n = slow_len;
m = numP;
gamma = numP / slow_len;

parfor rep = 1: rep_time
% for rep = 1: rep_time
    
    for cnt_SNR = 1: len_SNR
        
        %% �?声�?�理
        noise = random('Normal', 0, sigma_n/sqrt(2), 1, N * numP) + 1j * random('Normal', 0, sigma_n/sqrt(2), 1, N * numP);
        alpha_SNR = sqrt(10^(SNR(cnt_SNR)/10) * sigma_n^2 / (multiple_r * multiple_d));
        signal_r_n = signal_r * alpha_SNR + noise;
        
%         figure(1);
%         subplot(313)
%         plot(tP, real(signal_r_n));
%         xlabel('时间/t');
%         ylabel('幅度');
%         title('回波+�?声信�?');


        %% 匹配滤波——按Tr划分
        Srange = zeros(N, numP);

        % A = generate_matrix(transpose(signal_t(1, 1: N)), 1);

        count_fa=0;

        for i = 1: numP
            sr = signal_r_n(1, 1 + (i - 1) * N: i * N);
            st = signal_t(1, 1 + (i - 1) * N: i * N);

        %     A = generate_matrix(transpose(st), 1);
        %     mf = A' * transpose(sr);
        %     filterred_rf_r = mf ./ multiple_r;

            % 匹配滤波
            rf_r_fft_conj = conj(fft(st));
            filterred_rf_r = ifft(fft(sr) .* rf_r_fft_conj);
            filterred_rf_r = filterred_rf_r ./ multiple_r;

            Srange(:,i) = filterred_rf_r';

        end


        % figure(101)
        % mesh(abs(Srange))
        % title('按Tr进�?�匹配滤波结�?')

        %% R匹配滤波——分布验�?
        % Before MF
        % a+bi
        % a~N(0,sigma_n^2 / 2)
        % b~N(0,sigma_n^2 / 2)

        % After MF
        % a+bi
        % a~N(0,sigma_n^2 / 2 / multiple_r)
        % b~N(0,sigma_n^2 / 2 / multiple_r)
        % a^2 + b^2 ~ chi^2(2) * sigma_n^2 / 2 / multiple_r

        % stat_R = abs(Srange).^2;
        % P_fa = 0.1;
        % kd = sigma_n^2 * chi2inv(1 - P_fa, 2) / 2 / multiple;
        % sum(sum(stat_R>kd))/numel(stat_R)

        %% 多普勒滤�?
        Srd = zeros(N, slow_len);
        
        count_Pfa_d = zeros(length(P_fa), N);
        count_Pd_d = zeros(length(P_fa), 1);
        
        target_node_d = round(v1 / (c / fc * PRF / 2 / slow_len)) + 1;
        target_node_r = n_tau1 + 1;
        
%         for i = 1: N
        for i = target_node_r-N_high: target_node_r+N_high
            x_slow = Srange(i, :);
            y = (sqrt(slow_len) * eye(numP)) * transpose(x_slow);

            [x_LASSO,y_d] = cVAMPro(y,A,lambda,delta_VAMP,iter_max);
            
            % CROD求去�?
            rho_active = sum(abs(x_LASSO) > 1e-3)/n;
            Q_hat = (gamma - rho_active)/(1 - rho_active);
            Rho = sum((abs(x_LASSO) > 1e-3).* (2 - lambda./(Q_hat*abs(x_LASSO) + lambda))) / 2 / n;
            diff = 1;
            while(diff > 1e-4)
                Rho_pre = Rho;
                Rho = sum((abs(x_LASSO) > 1e-3).* (2 - lambda./((gamma-Rho)/(1-Rho)*abs(x_LASSO) + lambda))) / 2 / n;
                diff = abs(Rho - Rho_pre);
            end
            Q_hat = (gamma-Rho)/(1-Rho);
            x_d_CROD = x_LASSO + A'*(y - A*x_LASSO)/Q_hat;

            % CROD求门限和检验统计量
            RSS = sum(abs(y - A * x_LASSO).^2)/m;
            chi = Rho*(1 - Rho)/(gamma - Rho);
            if chi ~= 0
                chi_temp = sqrt((chi+1)*(chi+1)-4*gamma*chi);
                z = -(1 - chi + chi_temp) / (2*chi);
                z_prime = -(1 - 2*gamma*chi + chi + chi_temp) / (2*chi*chi*chi_temp);
                G_prime = (z + 1/chi);
                G_wprime = (z_prime + 1/chi/chi);
                chi_hat = gamma/2*G_wprime*RSS/(G_prime - chi*G_wprime)...
                    + (G_prime*G_prime/2 - gamma/2*G_wprime)*sigma_n*sigma_n/(G_prime - chi*G_wprime);
            else
                G_prime = gamma;
                G_wprime = gamma*(1-gamma);
                chi_hat = gamma/2*G_wprime*RSS/(G_prime - chi*G_wprime)...
                    + (G_prime*G_prime/2 - gamma/2*G_wprime)*sigma_n*sigma_n/(G_prime - chi*G_wprime);
            end
            sigma_CROD = sqrt(2*chi_hat) / Q_hat;

            sigma_CROD = sigma_CROD*0.895;

            stat_CROD = abs(x_d_CROD / sigma_CROD).^2;

            h_thd = chi2inv(1 - P_fa, 2) / 2;
            
            % 检�?
            for j = 1:length(P_fa)

                if i ~= target_node_r
                    count_Pfa_d(j,i) = sum(stat_CROD > h_thd(j)) / slow_len;
                else
                    stat_index = ones(size(stat_CROD));
                    stat_index(target_node_d) = 0;
                    count_Pfa_d(j,i) = sum(stat_CROD(stat_index > 0) > h_thd(j)) / sum(stat_index);
                    count_Pd_d(j) = stat_CROD(target_node_d) > h_thd(j);
                end

            end
            
            Srd(i, :) = fftshift(transpose(x_d_CROD ./ multiple_d ./ 2));

        end
        
        target_near_count = count_Pfa_d(:,target_node_r-N_high: target_node_r+N_high);
%         P_fa_actual = mean(count_Pfa_d,2);
        P_fa_actual = mean(target_near_count,2);
        
        for cnt_h_th = 1: len_P_fa
            P_fa_CROD_cnt(cnt_SNR, cnt_h_th, rep) = P_fa_actual(cnt_h_th);
            P_d_CROD_cnt(cnt_SNR, cnt_h_th, rep) = count_Pd_d(cnt_h_th);
        end

    end
    
    fprintf('%d\n', rep);
    
end

P_fa_CROD = mean(P_fa_CROD_cnt, 3);
P_d_CROD = mean(P_d_CROD_cnt, 3);

%% plot
figure(1);
loglog(P_fa,P_fa_CROD(1,:), 'linewidth', 2);
% hold on;
% grid on;
% loglog(P_fa,P_fa_MF(2,:), 'linewidth', 2);
% loglog(P_fa,P_fa_MF(3,:), 'linewidth', 2);
% loglog(P_fa,P_fa_MF(4,:), 'linewidth', 2);
% loglog(P_fa,P_fa_MF(5,:), 'linewidth', 2);
% loglog(P_fa,P_fa_MF(6,:), 'linewidth', 2);
% legend('SNR = 0','SNR = 2','SNR = 4','SNR = 6','SNR = 8','SNR = 10');
xlabel('P_fa');
ylabel('Actual P_fa');

figure(2);
semilogx(P_fa,P_d_CROD(1,:), 'linewidth', 2);
% hold on;
% grid on;
% semilogx(P_fa,P_d_MF(2,:), 'linewidth', 2);
% semilogx(P_fa,P_d_MF(3,:), 'linewidth', 2);
% semilogx(P_fa,P_d_MF(4,:), 'linewidth', 2);
% semilogx(P_fa,P_d_MF(5,:), 'linewidth', 2);
% semilogx(P_fa,P_d_MF(6,:), 'linewidth', 2);
% legend('SNR = 0','SNR = 2','SNR = 4','SNR = 6','SNR = 8','SNR = 10');
xlabel('P_fa');
ylabel('P_d');

% save test_Rmf_Dmf.mat ...
%     distance_temp...
%     speed_temp...
%     Srd...
%     Srange...
%     tP...
%     signal_t...
%     signal_r...
%     signal_r_n...
%     F...
%     slow_len;

save test_MC_Pfa_Rmf_Dcs.mat ...
    SNR...
    P_fa...
    P_fa_CROD...
    P_d_CROD...
    sigma_n...
    B...
    Tp...
    fs...
    Ts...
    K...
    fc...
    PRF...
    Tr...
    numP...
    t...
    tP...
    slow_len;