% Define MFSK waveform parameters

sample_rate = 151e6;
sweep_bandwidth = 150e6;
step_time = 2e-6;
steps_per_sweep = 1024;
frequency_offset = -294e3;
num_sweeps = 1;

% Generate MFSK waveform

time_per_step = step_time / steps_per_sweep;
time_vector = 0:1/sample_rate:(time_per_step * steps_per_sweep * num_sweeps);
mfsk_waveform = cos(2 * pi * (sweep_bandwidth * time_vector + frequency_offset
* floor(time_vector / time_per_step)));

% Simulate return echo (considering simplified target echoes)

% Assuming target echoes at specific distances and velocities
distance_car = 50; % in meters
velocity_car = -10; % in m/s
distance_truck = 55; % in meters
velocity_truck = 36; % in m/s

echo_car = cos(2 * pi * (sweep_bandwidth * time_vector - 2 * velocity_car *

1000 * time_vector / 3e8));
echo_truck = cos(2 * pi * (sweep_bandwidth * time_vector + 2 * velocity_truck
* 1000 * time_vector / 3e8));

echo_mfsk = echo_car + echo_truck + mfsk_waveform;

% Perform frequency analysis (FFT)

num_samples_per_step = round(sample_rate * step_time);
num_sweeps = length(echo_mfsk) / num_samples_per_step;

% Ensure both are integers

num_samples_per_step = round(num_samples_per_step);
num_sweeps = round(num_sweeps);

% Initialize empty matrix for FFT results

xf_dechirp = zeros(num_samples_per_step, num_sweeps);

% Perform FFT for each sweep

for sweep_idx = 1:num_sweeps
sweep = echo_mfsk((sweep_idx - 1) * num_samples_per_step + 1 : sweep_idx *
num_samples_per_step);
xf_dechirp(:, sweep_idx) = fft(sweep);
end

% Extract beat frequencies (considering simplified peak detection)

beatfreq_vec = (0:num_samples_per_step - 1).' / num_samples_per_step *
sample_rate;
peak_indices = find(abs(xf_dechirp(:, 1)) > 0.1 * max(abs(xf_dechirp(:,
1))))';
Fbeat = beatfreq_vec(peak_indices);

% Perform estimation (using equations)

lambda = 3e8 / sweep_bandwidth;
sweep_slope = sweep_bandwidth / (steps_per_sweep * step_time);
% Compute range and speed estimates
R = (lambda * Fbeat) / (2 * sweep_slope);
v = (-lambda * frequency_offset * 2 * pi) ./ (4 * step_time * Fbeat * 3e8);

% Display estimated range and speed

disp('Estimated Ranges: ');
disp(R);
disp('Estimated Speeds: ');
disp(v);

% Compute the maximum values of the FFT for each sweep

max_values = max(abs(echo_fft));

% Display the maximum values for each sweep

disp('Maximum values for each sweep:');
disp(max_values);

% Plot the full spectra to observe signal characteristics

figure;
plot(frequencies / 1e6, abs(echo_fft));
xlabel('Frequency (MHz)');
ylabel('Magnitude');
title('Frequency Spectra of All Sweeps');
grid on;