%%
clc;
clear;

config_parameters;

range = [250, 140, 167, 128, 12];
velocity = [16, 76, 155, 125, 463];

global C_n f_n;
C_n = zeros(M, len);
f_n = zeros(M, len);

for n_idx = 1:M

    for t_idx = 1:len
        C_n(n_idx, t_idx) = floor(rand * (M - 1));
        f_n(n_idx, t_idx) = f_c + C_n(n_idx, t_idx) * Delta_f;
    end

end

T_x = zeros(M, len);
R_x = zeros(M, len);
R_d = zeros(M, len);

for n_idx = 1:M

    for t_idx = 1:len
        t = range_t(t_idx);
        T_x(n_idx, t_idx) = T_x_func(t);
        R_x(n_idx, t_idx) = R_x_func(t, range(n_idx), velocity(n_idx));
        R_d(n_idx, t_idx) = R_d_func(t, range(n_idx), velocity(n_idx));
    end

end

save temp.mat

%%

clc
clear
load temp.mat

pred_range = zeros(1, M);
range_idx = zeros(1, M);
pred_velocity = zeros(1, M);

for n_idx = 1:M

    [pred_range(n_idx), range_idx(n_idx)] = get_range(T_x(n_idx, :), R_x(n_idx, :), range(n_idx));
    pred_velocity(n_idx) = get_velocity(T_x(n_idx, :), R_x(n_idx, :), velocity(n_idx));

end

%%
r_range = 1:500;
v_range = 1:500;

[X, Y] = meshgrid(r_range, v_range);
pred_z = zeros(length(r_range), length(v_range));
origin_z = zeros(length(r_range), length(v_range));

for n_idx = 1:M
    pred_z(round(pred_range(n_idx)), round(pred_velocity(n_idx))) = 1;
    origin_z(round(range(n_idx)), round(velocity(n_idx))) = 1;
end

figure(1)
subplot(211);
mesh(X, Y, pred_z);
title("Predict range-velocity reconstruction under M = 5");
xlabel("Range (m)");
ylabel("Velocity (m/s)");

subplot(212);
mesh(X, Y, origin_z);
title("Origin range-velocity data under M = 5");
xlabel("Range (m)");
ylabel("Velocity (m/s)");