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-5;

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


%% data processing
fprintf(['File: ', filename, '\n']);
[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);
fprintf('   1: generate_chirp_mtx done.\n');

% first antenna
Raw_Echo_antenna1 = Raw_Echo(1, :, :);
[echo_r_mtx_cs_antenna1, sigma_n_o_cs_antenna1] = range_process_CS(Raw_Echo_antenna1, A, sigma_n, lambda_r);
fprintf('   2: first antenna range_process_CS done.\n');

% first pulse
Raw_Echo_pulse1 = Raw_Echo(:, :, 1);
[echo_r_mtx_cs_pulse1, sigma_n_o_cs_pulse1] = range_process_CS(Raw_Echo_pulse1, A, sigma_n, lambda_r);
fprintf('   3: first pulse range_process_CS done.\n');

[echo_rd_mtx_cs, stat_RD_cs] = doppler_process_CS(echo_r_mtx_cs_antenna1, sigma_n_o_cs_antenna1, lambda_d, gamma_d);
fprintf('   4: doppler_process_CS done.\n');

[target_list_RD_cs, target_map_cs] = rd_detection(stat_RD_cs, P_fa);
fprintf('   5: rd_detection done.\n');

[target_list_RDA, RA_map] = angle_est(echo_r_mtx_cs_pulse1, target_list_RD_cs, d, fc);
fprintf('   6: angle_est done.\n');


%% 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(1)
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_cs;
save(['./output/', filename, '_cs_part.mat'],...
    'target_list_RDA', 'echo_rd_mtx', 'RA_map', 'P_fa');