% DOWNLOAD NOTE: Rename this file to fmcw_range_doppler_angle_demo.m before running it in MATLAB.
% The code below is otherwise unchanged.

%% From Chirps to Range-Doppler-Angle Maps
% Reproducible companion simulation for the ArthaVedya FMCW radar article.
%
% Requirements: MATLAB R2020a or newer. No specialist toolbox is required.
% Run this script directly. Outputs are written to fmcw_matlab_outputs.
%
% Model: ideal separable point targets plus seeded complex AWGN. Hann
% windows are implemented explicitly so Signal Processing Toolbox is not
% required. The simulation does not model multipath, antenna patterns,
% chirp nonlinearity, phase noise, mutual coupling, target extent, range
% migration, hardware calibration, or TDM-MIMO timing.

clear;
close all;
clc;

scriptPath = mfilename('fullpath');
if isempty(scriptPath)
    scriptDirectory = pwd;
else
    scriptDirectory = fileparts(scriptPath);
end
outputDirectory = fullfile(scriptDirectory, 'fmcw_matlab_outputs');
if ~exist(outputDirectory, 'dir')
    mkdir(outputDirectory);
end

%% Radar configuration
c = 299792458.0;
fc = 77e9;
lambda = c / fc;
bandwidth = 1e9;
rampTime = 50e-6;
idleTime = 10e-6;
chirpInterval = rampTime + idleTime;
slope = bandwidth / rampTime;
fs = 10e6;
nFast = round(fs * rampTime);
nChirps = 128;
nAntennas = 12;
antennaSpacing = lambda / 2;
snrDb = 7.0;

targetName = {'A'; 'B'; 'C'; 'D'};
targetRangeM = [8.10; 16.50; 16.50; 16.50];
targetVelocityMps = [2.00; -3.00; -3.00; 1.00];
targetAngleDeg = [-25.0; 8.0; 32.0; -6.0];
targetAmplitude = [1.00; 0.82; 0.66; 0.58];

purple = [101, 87, 217] / 255;
violet = [139, 124, 246] / 255;
teal = [22, 166, 161] / 255;
orange = [229, 138, 43] / 255;
dark = [23, 32, 51] / 255;
mid = [101, 112, 137] / 255;
gridColor = [217, 214, 234] / 255;

%% Figure 1: chirp delay and beat frequency
tUs = linspace(0, 58, 600);
tauVisualUs = 8.0;  % Enlarged only for the explanatory drawing.
txMHz = nan(size(tUs));
rxMHz = nan(size(tUs));
txMask = tUs >= 0 & tUs <= 50;
rxMask = tUs >= tauVisualUs & tUs <= 50 + tauVisualUs;
txMHz(txMask) = 20 * tUs(txMask);
rxMHz(rxMask) = 20 * (tUs(rxMask) - tauVisualUs);

figure1 = figure('Color', 'white', 'Position', [80, 80, 1500, 1020]);
layout1 = tiledlayout(2, 1, 'TileSpacing', 'compact', 'Padding', 'compact');

ax1 = nexttile;
hold(ax1, 'on');
hTx = plot(ax1, tUs, txMHz, 'Color', purple, 'LineWidth', 3);
hRx = plot(ax1, tUs, rxMHz, 'Color', teal, 'LineWidth', 3);

% Beat-frequency separation at one instant.
xBeat = 32;
yRx = 20 * (xBeat - tauVisualUs);
yTx = 20 * xBeat;
plot(ax1, [xBeat, xBeat], [yRx, yTx], 'Color', orange, 'LineWidth', 2.2);
plot(ax1, xBeat, yRx, '^', 'Color', orange, 'MarkerFaceColor', orange, 'MarkerSize', 8);
plot(ax1, xBeat, yTx, 'v', 'Color', orange, 'MarkerFaceColor', orange, 'MarkerSize', 8);
plot(ax1, [36.0, xBeat, xBeat], [400, 400, (yRx + yTx) / 2], 'Color', orange, 'LineWidth', 1.6);
text(ax1, 39.0, 400, 'f_b = S\tau', 'Interpreter', 'tex', ...
    'HorizontalAlignment', 'center', 'VerticalAlignment', 'middle', ...
    'Color', orange, 'FontSize', 13, 'FontWeight', 'bold', ...
    'BackgroundColor', 'white', 'EdgeColor', orange, 'Margin', 5);

% Propagation delay between the two chirp start times.
delayY = 75;
plot(ax1, [0, 0], [0, delayY], '--', 'Color', mid, 'LineWidth', 1.0);
plot(ax1, [tauVisualUs, tauVisualUs], [0, delayY], '--', 'Color', mid, 'LineWidth', 1.0);
plot(ax1, [0.35, tauVisualUs - 0.35], [delayY, delayY], 'Color', dark, 'LineWidth', 1.7);
plot(ax1, 0.35, delayY, '>', 'Color', dark, 'MarkerFaceColor', dark, 'MarkerSize', 8);
plot(ax1, tauVisualUs - 0.35, delayY, '<', 'Color', dark, 'MarkerFaceColor', dark, 'MarkerSize', 8);
text(ax1, tauVisualUs / 2, 205, '\tau = 2R/c', 'Interpreter', 'tex', ...
    'HorizontalAlignment', 'center', 'VerticalAlignment', 'middle', ...
    'Color', dark, 'FontSize', 12, 'FontWeight', 'bold', ...
    'BackgroundColor', 'white', 'Margin', 3);

xlim(ax1, [-1, 59]);
ylim(ax1, [-35, 1050]);
xlabel(ax1, 'Time within chirp (microseconds; delay enlarged for visibility)');
ylabel(ax1, 'Frequency offset from start (MHz)');
title(ax1, 'Dechirping converts propagation delay into a beat frequency');
legend(ax1, [hTx, hRx], {'Transmitted chirp', 'Delayed echo'}, ...
    'Location', 'northwest', 'Box', 'off');
grid(ax1, 'on');
ax1.GridColor = gridColor;
ax1.GridAlpha = 0.75;
ax1.FontSize = 11;

ax2 = nexttile;
exampleRangeM = 15.0;
exampleTau = 2 * exampleRangeM / c;
exampleBeatHz = slope * exampleTau;
tIf = linspace(0, 3e-6, 900);
beat = cos(2 * pi * exampleBeatHz * tIf);
plot(ax2, tIf * 1e6, beat, 'Color', purple, 'LineWidth', 2.1);
xlim(ax2, [0, 3]);
ylim(ax2, [-1.2, 1.35]);
xlabel(ax2, 'Fast time (microseconds)');
ylabel(ax2, 'Normalized IF amplitude');
title(ax2, ['Worked configuration: R = 15 m gives f_b \approx ', ...
    num2str(exampleBeatHz / 1e6, '%.2f'), ' MHz'], 'Interpreter', 'tex');
grid(ax2, 'on');
ax2.GridColor = gridColor;
ax2.GridAlpha = 0.75;
ax2.FontSize = 11;
text(ax2, 2.97, -1.08, 'Illustrative ideal tone; no noise or hardware impairments', ...
    'HorizontalAlignment', 'right', 'Color', mid, 'FontSize', 9);
title(layout1, 'From linear chirp to low-frequency range information', ...
    'FontSize', 18, 'FontWeight', 'bold');
exportgraphics(layout1, fullfile(outputDirectory, ...
    'figure-1-fmcw-chirp-and-beat-matlab.png'), 'Resolution', 180);

%% Build the ideal complex radar cube x[n,m,p]
rng(20260910, 'twister');
n = reshape(0:nFast - 1, [], 1, 1);
m = reshape(0:nChirps - 1, 1, [], 1);
p = reshape(0:nAntennas - 1, 1, 1, []);
cube = complex(zeros(nFast, nChirps, nAntennas));

for k = 1:numel(targetRangeM)
    beatFrequency = 2 * slope * targetRangeM(k) / c;
    dopplerFrequency = 2 * targetVelocityMps(k) / lambda;
    spatialCyclesPerElement = antennaSpacing * sind(targetAngleDeg(k)) / lambda;
    initialPhase = 2 * pi * rand;

    phase = 2 * pi * (beatFrequency * n / fs + ...
        dopplerFrequency * m * chirpInterval + ...
        spatialCyclesPerElement * p) + initialPhase;
    cube = cube + targetAmplitude(k) * exp(1j * phase);
end

signalPower = mean(abs(cube(:)).^2);
noisePower = signalPower / (10^(snrDb / 10));
noise = sqrt(noisePower / 2) * ...
    (randn(size(cube)) + 1j * randn(size(cube)));
cube = cube + noise;

%% Range, Doppler and angle processing
nRangeFft = 1024;
nDopplerFft = 256;
nAngleFft = 512;

% Explicit Hann windows: no Signal Processing Toolbox required.
rangeWindow = 0.5 - 0.5 * cos(2 * pi * (0:nFast - 1)' / (nFast - 1));
dopplerWindow = 0.5 - 0.5 * cos(2 * pi * (0:nChirps - 1)' / (nChirps - 1));
angleWindow = 0.5 - 0.5 * cos(2 * pi * (0:nAntennas - 1)' / (nAntennas - 1));
rangeWindow = reshape(rangeWindow, [], 1, 1);
dopplerWindow = reshape(dopplerWindow, 1, [], 1);

rangeFft = fft(cube .* rangeWindow, nRangeFft, 1);
rangeFft = rangeFft(1:nRangeFft / 2, :, :);
rangeDoppler = fftshift(fft(rangeFft .* dopplerWindow, nDopplerFft, 2), 2);
rangeDopplerPower = sum(abs(rangeDoppler).^2, 3);

rangeFrequency = (0:nRangeFft / 2 - 1)' * fs / nRangeFft;
rangeAxisM = c * rangeFrequency / (2 * slope);
dopplerFrequencyAxis = (-nDopplerFft / 2:nDopplerFft / 2 - 1) ...
    / (nDopplerFft * chirpInterval);
velocityAxisMps = lambda * dopplerFrequencyAxis / 2;

% Angle spectrum at the range-Doppler cell shared by targets B and C.
[~, rangeIndex] = min(abs(rangeAxisM - targetRangeM(2)));
[~, velocityIndex] = min(abs(velocityAxisMps - targetVelocityMps(2)));
spatialVector = reshape(rangeDoppler(rangeIndex, velocityIndex, :), 1, []);
spatialVector = spatialVector .* angleWindow.';
angleFft = fftshift(fft(spatialVector, nAngleFft, 2), 2);
spatialFrequency = (-nAngleFft / 2:nAngleFft / 2 - 1) / nAngleFft;
sinTheta = spatialFrequency * lambda / antennaSpacing;
validAngle = abs(sinTheta) <= 1;
angleAxisDeg = asind(sinTheta(validAngle));
anglePower = abs(angleFft(validAngle)).^2;

rangeDopplerDb = 10 * log10(rangeDopplerPower + eps);
rangeDopplerDb = rangeDopplerDb - max(rangeDopplerDb(:));
angleDb = 10 * log10(anglePower + eps);
angleDb = angleDb - max(angleDb(:));

%% Figure 2: range-Doppler map and angle spectrum
figure2 = figure('Color', 'white', 'Position', [100, 100, 1600, 720]);
layout2 = tiledlayout(1, 2, 'TileSpacing', 'compact', 'Padding', 'compact');

axRd = nexttile;
imagesc(axRd, velocityAxisMps, rangeAxisM, rangeDopplerDb);
set(axRd, 'YDir', 'normal');
colormap(axRd, parula(256));
caxis(axRd, [-45, 0]);
xlim(axRd, [-6, 5]);
ylim(axRd, [3, 22]);
xlabel(axRd, 'Radial velocity (m/s; positive = approaching)');
ylabel(axRd, 'Range (m)');
title(axRd, 'Range-Doppler map');
colorbarHandle = colorbar(axRd);
colorbarHandle.Label.String = 'Relative RD power (dB)';
hold(axRd, 'on');

labelIndices = [1, 2, 4];
for k = labelIndices
    if k == 2
        displayName = 'B + C';
    else
        displayName = targetName{k};
    end
    text(axRd, targetVelocityMps(k) + 0.12, targetRangeM(k) + 0.30, ...
        displayName, 'Color', 'white', 'FontSize', 10, ...
        'FontWeight', 'bold', 'BackgroundColor', dark, 'Margin', 3);
end

axAngle = nexttile;
hold(axAngle, 'on');
plot(axAngle, angleAxisDeg, angleDb, 'Color', purple, 'LineWidth', 2.4);
plot(axAngle, [8, 8], [-42, 2], '--', 'Color', teal, 'LineWidth', 1.6);
plot(axAngle, [32, 32], [-42, 2], '--', 'Color', orange, 'LineWidth', 1.6);
xlim(axAngle, [-45, 55]);
ylim(axAngle, [-42, 2]);
xlabel(axAngle, 'Angle of arrival (degrees)');
ylabel(axAngle, 'Relative angle power (dB)');
title(axAngle, 'Angle spectrum at R = 16.5 m, v = -3 m/s');
legend(axAngle, {'Angle spectrum', 'Target B: 8 deg', 'Target C: 32 deg'}, ...
    'Location', 'northwest', 'Box', 'off');
grid(axAngle, 'on');
axAngle.GridColor = gridColor;
axAngle.GridAlpha = 0.75;
axAngle.YAxisLocation = 'right';
title(layout2, ...
    'The angle dimension separates targets that share one range-Doppler cell', ...
    'FontSize', 18, 'FontWeight', 'bold');
exportgraphics(layout2, fullfile(outputDirectory, ...
    'figure-2-range-doppler-angle-matlab.png'), 'Resolution', 180);

%% Write reproducibility tables
rangeResolutionM = c / (2 * bandwidth);
nyquistRangeM = c * (fs / 2) / (2 * slope);
maximumUnambiguousVelocityMps = lambda / (4 * chirpInterval);
velocityBinSpacingMps = lambda / (2 * nChirps * chirpInterval);
spatialSinThetaBinSpacing = lambda / (nAntennas * antennaSpacing);
spatialBinSpacingNearBoresightDeg = asind(spatialSinThetaBinSpacing);
rawCubeSizeComplexDoubleBytes = numel(cube) * 16;

parameter = {
    'carrier_frequency'
    'wavelength'
    'sweep_bandwidth'
    'ramp_time'
    'idle_time'
    'chirp_repetition_interval'
    'chirp_slope'
    'adc_sampling_rate'
    'fast_time_samples'
    'chirps_per_frame'
    'array_elements'
    'element_spacing'
    'range_resolution'
    'nyquist_limited_range_assuming_abs_fb_lt_fs_over_2'
    'maximum_unambiguous_velocity'
    'velocity_bin_spacing_no_zero_padding'
    'spatial_sin_theta_bin_spacing_no_zero_padding'
    'spatial_bin_spacing_near_boresight'
    'raw_cube_size_complex_double'
    };

value = [
    fc
    lambda
    bandwidth
    rampTime
    idleTime
    chirpInterval
    slope
    fs
    nFast
    nChirps
    nAntennas
    antennaSpacing
    rangeResolutionM
    nyquistRangeM
    maximumUnambiguousVelocityMps
    velocityBinSpacingMps
    spatialSinThetaBinSpacing
    spatialBinSpacingNearBoresightDeg
    rawCubeSizeComplexDoubleBytes
    ];

unit = {
    'Hz'
    'm'
    'Hz'
    's'
    's'
    's'
    'Hz/s'
    'samples/s'
    'samples/chirp'
    'chirps'
    'elements'
    'm'
    'm'
    'm'
    'm/s'
    'm/s'
    'dimensionless'
    'deg'
    'bytes'
    };

configurationTable = table(parameter, value, unit);
writetable(configurationTable, fullfile(outputDirectory, ...
    'fmcw-worked-configuration-matlab.csv'));

targetTable = table(targetName, targetRangeM, targetVelocityMps, ...
    targetAngleDeg, targetAmplitude, 'VariableNames', ...
    {'name', 'range_m', 'velocity_mps', 'angle_deg', 'amplitude'});
writetable(targetTable, fullfile(outputDirectory, ...
    'fmcw-simulated-targets-matlab.csv'));

%% Numerical summary
fprintf('Created outputs in: %s\n', outputDirectory);
fprintf('Wavelength: %.6f mm\n', lambda * 1e3);
fprintf('Chirp slope: %.6f MHz/us\n', slope / 1e12);
fprintf('Range resolution: %.6f m\n', rangeResolutionM);
fprintf('Nyquist-limited range: %.6f m\n', nyquistRangeM);
fprintf('Maximum unambiguous velocity: %.6f m/s\n', ...
    maximumUnambiguousVelocityMps);
fprintf('Velocity bin spacing: %.6f m/s\n', velocityBinSpacingMps);