clc
clear
% rng(1)

%% 参数设置
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;        % 光速

%%
sigma_n = 0.1;      % 噪声标准差
alpha_prop = 0.8;   % 目标散射点强度

SNR = 50;           % 信噪比（积累后信噪比）

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

slow_len = 128;     % 多普勒维采样点

% 是否有目标，0无目标，1有目标
has_target = 0;

%% 产生发射信号
% t=-Tp/2:1/fs:Tp/2-1/fs;
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

% signal_t  = 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));
% end

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

%%
% 距离匹配滤波增益
multiple_r = signal_t(1, 1: N) * signal_t(1, 1: N)';

% 多普勒匹配滤波增益
F_ori = dftmtx(slow_len);
F = F_ori(1:numP,:);
multiple_d = F(:,1)' * F(:,1);

%% 设置目标 1
distance1 = 18000;
v1 = 187.5;

tau1 = distance1 * 2 / c;   % 时延
n_tau1 = round(tau1 * fs);  % 对应采样点
    
f_d1 = 2 * fc * v1 / c;     % 多普勒频率

% 根据 SNR 设置回波散射强度
if has_target
    alpha1 = alpha_prop * sqrt(10^(SNR/10) * sigma_n^2 / (multiple_r * multiple_d));
else
    alpha1 = 0;
end

% 生成目标 1 回波
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 = (temp - 1) * (1 / fs);
            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_r1  = zeros(1, N);
% for i = 1:N
%     temp = i - n_tau1;
%     if temp >= 1 && temp <= N_high
%         signal_r1(1, i) = alpha1 * signal_t(1, temp);
%     end
% end

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

%% 噪声处理
noise = random('Normal', 0, sigma_n/sqrt(2), 1, N * numP) + 1j * random('Normal', 0, sigma_n/sqrt(2), 1, N * numP);
signal_r_n = signal_r + noise;

figure(1);
subplot(313)
plot(tP, real(signal_r_n));
xlabel('时间/t');
ylabel('幅度');
title('回波+噪声信号');


%% 对回波求多普勒频移
% s_R_slow = zeros(1, slow_len);
% 
% slow_freqs = ((0:slow_len - 1) .* (1 / Tr)) / slow_len;
% 
% init_idx = 1;
% while abs(signal_r_n(init_idx)) == 0
%     init_idx = init_idx + 1;
% end
% 
% for i = 0:numP - 1
%     s_R_slow(i + 1) = signal_r_n(init_idx + i * N + 1);
% end
% 
% s_R_fft = fftshift(fft(s_R_slow));

% figure(3)
% plot(real(s_R_fft))

% [~, max_index_s_R] = max(s_R_fft);
% freq_s_R = slow_freqs(max_index_s_R);
% 
% f_d = 1 / Tr - freq_s_R;
% v = f_d * c / (2 * (fc - B / 2))


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

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;
    
    % 匹配滤波
    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;
    
%     figure(2000)
%     plot(abs(filterred_rf_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)

%% 多普勒滤波
% F_ori = dftmtx(slow_len);
% F = F_ori(1:numP,:);
% multiple_d = F(:,1)' * F(:,1);

F_inv = conj(F)/slow_len;
% ttttttt = F_inv * transpose(F);

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

% 放大到行正交
A = (sqrt(slow_len) * eye(numP)) * F_inv;

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

% 设定虚警率
P_fa = [1e-4, 5e-4, 1e-3, 5e-3, 1e-2, 5e-2, 1e-1, 5e-1, 1];
count_Pfa_d = zeros(size(length(P_fa), N));

Srd = zeros(N, slow_len);
for i = 1: N
% for i = 1201: 1201
    x_slow = Srange(i, :); % 慢时间采样
    y = (sqrt(slow_len) * eye(numP)) * transpose(x_slow); % 进行相应放大

    % LASSO 求解
%     x_LASSO = FISTA(y, A, lambda_val, delta_VAMP);
    [x_LASSO,y_d] = cVAMPro(y,A,lambda,delta_VAMP,iter_max);
%     x_LASSO = x_LASSO ./ multiple_d ./ 2;

    % 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;    % 门限
    
    % 检测
    % 目标点位
    target_node_d = round(v1 / (c / fc * PRF / 2 / slow_len)) + 1;
    target_node_r = n_tau1 + 1;
    
    for j = 1:length(P_fa)

        if i ~= target_node_r % 非目标点的距离慢采样结果（H0假设）
            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; % 去掉目标点（H0假设不考虑目标点）
            count_Pfa_d(j,i) = sum(stat_CROD(stat_index > 0) > h_thd(j)) / sum(stat_index);
        end
        
    end

    
%     if i ~= target_node_r
%         count_Pfa_d(i) = sum(stat_CROD > h_thd) / slow_len;
%     else
%         stat_index = ones(size(stat_CROD));
%         stat_index(target_node_d) = 0;
%         count_Pfa_d(i) = sum(stat_CROD(stat_index > 0) > h_thd) / sum(stat_index);
%     end
    
%     count_Pfa_d(i)
    
%     figure(444)
%     % 归一化
%     plot(abs(x_d_CROD ./ multiple_d ./ 2))
%     figure(555)
%     % 统计量
%     plot(stat_CROD)

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

%% DCS——分布验证
output = mean(count_Pfa_d,2); % 把所有多普勒维检测结果求平均得到总体虚警率

figure(4000)
loglog(P_fa,output)
xlabel('P_fa Set')
ylabel('Actual P_fa')
title('P_fa')

%%
% Srd_bf_mf = zeros(N, slow_len);
% for i = 1: N
%     Srd_bf_mf(i, :) = fftshift(fft([Srange_bf_mf(i, :),zeros(1,slow_len-numP)]));
% end

% figure(102)
% mesh(abs(Srd))

% figure(105)
% mesh(abs(Srd_bf_mf))

% figure(1)
% subplot(414)
% t2 = 0: (c / fs / 2) : (Tr * numP * c / 2 - c / fs / 2);
% plot(tP, abs(filterred_rf_r) ./ N_high)
% % plot(abs(filterred_rf_r))
% xlabel('距离')
% title('匹配滤波')

% distance_temp = (0:N - 1) * fs * c / N / 2 / K;

%% RD 绘制
Srd = Srd';

lambda = c / fc;
% 40*lambda*PRF/2/128
distance_temp = 0: (c / fs / 2) : (Tr * c / 2 - c / fs / 2);
speed_temp = (-PRF / 2: PRF / slow_len: PRF / 2 - PRF / slow_len) * lambda / 2 ;
% speed_temp = (-numP / 2: numP / slow_len: numP / 2 - numP / slow_len) * lambda / Tr / numP / 2 ;

figure(5)
[X, Y] = meshgrid(distance_temp, speed_temp);
mesh(X, Y, abs(Srd));
xlabel('距离(m)');
ylabel('速度(m/s)');
zlabel('信号幅值');
title('2维RD图');

figure(6)
imagesc(distance_temp, speed_temp, abs(Srd));
title('Range-Doppler Image');
xlabel('Range (m)');
ylabel('Speed (m/s)');
set(gca, 'FontSize', 18);
set(gcf, 'position', [200, 300, 800, 600]);
set(gca,'fontsize',20,'fontname','Times');

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