clc
clear
close all

%% load echo data
filename = 'Raw_Echo_60dB';
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-4;

% algorithm
gamma_r = 0.5;
gamma_d = 0.5;


%% data processing
[distance_v, speed_v] = get_range_speed_val(PRF, fs, fc, num_pulse, gamma_r, gamma_d);

[A, signal_t] = generate_chirp_mtx(PRF, B, fs, D);

% first antenna
Raw_Echo_antenna1 = Raw_Echo(1, :, :);
[echo_r_mtx_mf_antenna1, sigma_n_o_mf_antenna1] = range_process_MF(Raw_Echo_antenna1, A, sigma_n);

% first pulse
Raw_Echo_pulse1 = Raw_Echo(:, :, 1);
[echo_r_mtx_mf_pulse1, sigma_n_o_mf_pulse1] = range_process_MF(Raw_Echo_pulse1, A, sigma_n);

[echo_rd_mtx_mf, stat_RD_mf] = doppler_process_MF(echo_r_mtx_mf_antenna1, sigma_n_o_mf_antenna1, gamma_d);

[target_list_RD_mf, target_map_mf] = rd_detection(stat_RD_mf, P_fa);

[target_list_RDA, RA_map] = angle_est(echo_r_mtx_mf_pulse1, target_list_RD_mf, d, fc);


%% show result
element = 1;
list_r_idx = find(target_list_RDA(:, 1) == element);

target_list_RDA(:, 2) = distance_v(target_list_RDA(:, 2));
target_list_RDA(:, 3) = speed_v(target_list_RDA(:, 3));
figure(2)
scatter3(target_list_RDA(list_r_idx,2), target_list_RDA(list_r_idx,3),...
    target_list_RDA(list_r_idx,4), 'filled', 'o')
% xlim([17000, 19000])
% ylim([80, 160])
% zlim([-90, 90])


%% save
echo_rd_mtx = echo_rd_mtx_mf;
save(['./output/', filename, '_mf_part.mat'],...
    'target_list_RDA', 'echo_rd_mtx', 'RA_map', 'P_fa');