% Define a threshold for detecting azimuthal changes
azimuth_threshold = 0.5;  % Degrees
range_limit = 2000;  % Maximum range in meters

% Compute differences between consecutive beams
azimuth_diff = abs(diff(lidar.azimuth(:, 1))); % Difference in azimuth across beams

% Identify indices where azimuth_diff is greater than threshold (scan regions)
scan_mask = azimuth_diff > azimuth_threshold;

% Identify start and end indices of continuous scan segments
scan_starts = find(diff([0; scan_mask]) == 1); % Start when scan_mask transitions from 0 to 1
scan_ends = find(diff([scan_mask; 0]) == -1);  % End when scan_mask transitions from 1 to 0

% Ensure there are valid scans
num_scans = length(scan_starts);
scans(num_scans) = struct('RWS', [], 'SNR', [], 'Beta', [], ...
                          'azimuth', [], 'elevation', [], 'ranges', [], ...
                          'xd', [], 'yd', [], 'zd', []);

% Find valid range bins (indices where range < 2000 m)
valid_range_bins = find(lidar.ranges < range_limit);

% Extract each scan separately
scan_count = 0;
for i = 1:num_scans
    start_idx = scan_starts(i);
    end_idx = scan_ends(i);

    % Ensure the scan has a significant azimuth variation
    azimuth_min = min(lidar.azimuth(start_idx:end_idx, 1));
    azimuth_max = max(lidar.azimuth(start_idx:end_idx, 1));
    azimuth_range = azimuth_max - azimuth_min;

    if azimuth_range > azimuth_threshold
        scan_count = scan_count + 1;

        % Extract only the valid range bins
        scans(scan_count).RWS = lidar.RWS(start_idx:end_idx, valid_range_bins);
        scans(scan_count).SNR = lidar.SNR(start_idx:end_idx, valid_range_bins);
        scans(scan_count).Beta = lidar.Beta(start_idx:end_idx, valid_range_bins);
        scans(scan_count).azimuth = lidar.azimuth(start_idx:end_idx, valid_range_bins);
        scans(scan_count).elevation = lidar.elevation(start_idx:end_idx, valid_range_bins);
        scans(scan_count).ranges = lidar.ranges(valid_range_bins);
        scans(scan_count).xd = lidar.xd(start_idx:end_idx, valid_range_bins);
        scans(scan_count).yd = lidar.yd(start_idx:end_idx, valid_range_bins);
        scans(scan_count).zd = lidar.zd(start_idx:end_idx, valid_range_bins);
    end
end

% Trim empty scan structures
if scan_count > 0
    scans = scans(1:scan_count);
end

% Display number of extracted scans
fprintf('Extracted %d scans from lidar data (limited to range < %d m).\n', scan_count, range_limit);

% Define SNR threshold for highlighting
high_snr_threshold = 1.5;  % Adjust based on data

% Generate output folder based on the lidar filename
lidar_filename = char(lidar.orig_filename); % Convert to character vector
[~, lidar_name, ~] = fileparts(lidar_filename);
out_folder = fullfile(pwd, lidar_name);
if ~exist(out_folder, 'dir')
    mkdir(out_folder);
end

%% Save plots in level-specific folders
for level = 1:scan_count
    scan = scans(level);

    % Create folder for the specific scan level
    level_folder = fullfile(out_folder, sprintf('Level_%d', level));
    if ~exist(level_folder, 'dir')
        mkdir(level_folder);
    end

    % Define mask for high SNR
    high_snr_mask = scan.SNR > high_snr_threshold;

    % Create grid that aligns correctly with `pcolor`
    x_centered = 0.5 * (scan.xd(1:end-1, 1:end-1) + scan.xd(2:end, 2:end));
    y_centered = 0.5 * (scan.yd(1:end-1, 1:end-1) + scan.yd(2:end, 2:end));
    snr_mask_shifted = high_snr_mask(1:end-1, 1:end-1); % Align with pcolor

    % Save RWS plot
    figure;
    pcolor(scan.xd, scan.yd, scan.RWS);
    shading flat;
    colorbar;
    title(sprintf('Radial Wind Speed (RWS) - Scan Level %d', level));
    xlabel('X (m)'); ylabel('Y (m)');
    hold on;
    contour(x_centered, y_centered, snr_mask_shifted, 1, 'r', 'LineWidth', 2);
    hold off;
    saveas(gcf, fullfile(level_folder, 'RWS.png'));
    close;

    % Save Logarithm of SNR plot
    figure;
    pcolor(scan.xd, scan.yd, log10(scan.SNR));
    shading flat;
    colorbar;
    title(sprintf('Log10(SNR) - Scan Level %d', level));
    xlabel('X (m)'); ylabel('Y (m)');
    saveas(gcf, fullfile(level_folder, 'Log10SNR.png'));
    close;

    % Save Beta (Backscatter) plot
    figure;
    pcolor(scan.xd, scan.yd, scan.Beta);
    shading flat;
    colorbar;
    title(sprintf('Backscatter (Beta) - Scan Level %d', level));
    xlabel('X (m)'); ylabel('Y (m)');
    saveas(gcf, fullfile(level_folder, 'Beta.png'));
    close;
end

% Save workspace
    save(fullfile(out_folder, sprintf('%s.mat', lidar_name)));