clc
clear
close all

%% load echo data
filename = 'Raw_Echo_5dB';
load(['./data/', filename, '.mat']);


%% parameters
% ladar
PRF = 5000;
B = 5e6;
D = 0.1;
Tp = 2e-5;
fs = 5e6;
fc = 1.25e9;

% antenna
num_antenna = 18;
d = 0.12;

% echo
num_pulse = 64;
sigma_n = 0.1;

% detect
P_fa = 1e-6;

% algorithm
gamma_r = 0.5;
gamma_d = 0.5;
lambda_r = 0.005;
lambda_d = 0.15;

% accurate angle
sign_AA = 1;


%% data processing
fprintf('Using CS\n');
fprintf(['File: ', filename, '\n']);

tic;
if sign_AA == 0
    [ echo_rd_mtx, target_list_RDA, sigma_n_out ] = ...
    echo_processing_CS_v0( Raw_Echo, PRF, fs, fc, B, D, d, sigma_n, P_fa, lambda_r, lambda_d );
else
    [ echo_rd_mtx, target_list_RDA, sigma_n_out ] = ...
    echo_processing_CS( Raw_Echo, PRF, fs, fc, B, D, d, sigma_n, P_fa, lambda_r, lambda_d );
end
toc;


%% plot
Fontsize = 18;
plot_width = 800;
plot_height = 600;
Linewidth = 2;
Markersize = 8;

figure(1)
scatter3(target_list_RDA(:,1), target_list_RDA(:,2),...
    target_list_RDA(:,3), 'filled', 'o')
xlabel('Range(m)');
ylabel('Speed(m/s)');
zlabel('Angle(°)');
zlim([-90, 90]);
ylim([80, 150]);
xlim([17000, 19000]);
set(gca, 'FontSize', Fontsize);
title('node')
set(gcf, 'position', [100, 200, plot_width+100, plot_height+50]);
set(gca,'fontsize',18,'fontname','Times');


%% save
save(['./output/', filename, '_CS_Pfa', num2str(P_fa), '.mat'],...
    'target_list_RDA', 'echo_rd_mtx', 'sigma_n_out', 'P_fa');
fprintf(['[', filename, '_CS_Pfa', num2str(P_fa), '.mat]', ' saved.\n\n']);