%% Load files and Plot

isac_files  = dataframe(2:end, dataframe(1,:)=="unit1_iq");
radar_files = dataframe(2:end, dataframe(1,:)=="unit1_radar");
rgb_files   = dataframe(2:end, dataframe(1,:)=="unit1_rgb");
lidar_files = dataframe(2:end, dataframe(1,:)=="unit1_lidar");

for i=1:n_rows
    fprintf("Row: %d | ", i);

    isac_file  = strcat(folder, isac_files(i));
    radar_file = strcat(folder, radar_files(i));
    rgb_file   = strcat(folder, rgb_files(i));
    lidar_file = strcat(folder, lidar_files(i));
    
    % Load Unit 1 - ISAC IQ
    load(isac_file, "waveform", "beam"); % import only iq

    % Load Unit 2 - Radar
    load(radar_file, "mag_data_static", "mag_data_dynamic");

    % Load Unit 3 - Image
    img = imread(rgb_file);

    % Load Unit 3 - Lidar Pointcloud
    ptCloud = pcread(lidar_file); % requires computer vision toolbox

    % Visualize Isac
    fprintf("Angle: %.1f\n", angles(beam - 21)); % 21 is the beam offset
    correlation = xcorr(waveform, tx_waveform); % correlate rx and tx signals
    [valR,idxR] = max(abs(correlation));
    snip = correlation(idxR-64:1:idxR+500);
    [pks, locs] = findpeaks(abs(snip));
    [~,idx_loc] = max(pks);
    % Z = zeros(size(R)); % uncomment this line for single scan visualization
    Z(beam - 21,:) = snip(locs(idx_loc):1:locs(idx_loc)+188); 

    if i == 1
        figure('Name', 'ISAC Sensing', 'NumberTitle','off');
        s = surf(R.*cos(deg2rad(PHI)), R.*sin(deg2rad(PHI)), abs(Z).^vis_e);
        colormap(map);
        set(gcf,'color','w');
        s.EdgeColor = 'none';
        s.FaceColor = 'interp';
        view(-90,90);
        xlim([0 15]);
        ylim([-15 15]);
    else
        s.CData = abs(Z).^vis_e;
    end

    % Visualize Radar
    if i == 1
        figure('Name', 'TI Radar', 'NumberTitle','off');
        radar_ax = imagesc(mag_data_static' + mag_data_dynamic');
        a_tick = [18,65,129,193,240];
        a_axis = rad2deg(angle(x_axis(:,1)+y_axis(:,1)*1i));
        r_tick = 1:50:233;
        r_axis = sqrt(x_axis(33,:).^2+y_axis(33,:).^2);
        set(gca(), 'XTick', a_tick, 'XTickLabel', round(a_axis(a_tick)), ...
                   'YTick', r_tick, 'YTickLabel', round(r_axis(r_tick)), ...
                   'YDir', 'normal')
        xlabel("angle (º)");
        ylabel("range (m)");
    else
        radar_ax.CData = mag_data_static' + mag_data_dynamic';
    end

    % Visualize Image
    if i == 1
        figure('Name', 'Camera', 'NumberTitle','off');
        im_ax = imshow(img);
    else
        im_ax.CData = img;
    end

    % Visualize Lidar
    if i == 1
        player = pcplayer([-4,5],[-3,10],[-3,3], MarkerSize=2);

        set(player.Axes, "CameraPosition", [-27.2231 -393.3579 391.6075], ...
                         "CameraTarget", [1.9202 9.2605 -4.7717], ...
                         "CameraViewAngle", 1.2238, ...
                         "CameraUpVector", [-0.0026 0.6954 0.7186]);
        player.Axes.Parent.Name = 'Lidar';
        player.Axes.Parent.NumberTitle = 'off';
    end
    view(player, ptCloud);
    drawnow;
end