FRANCO

% Specifica delle porte seriali per la comunicazione
controlPort = serialport("COM5", 115200); % Porta COM per configurazione
dataPort = serialport("COM6", 921600);   % Porta COM per dati in arrivo

% Percorso del file di configurazione
configFilePath = "AOP_6m_default.cfg";

% Funzione per inviare il file di configurazione
applyRadarConfiguration(controlPort, configFilePath);

disp("Radar configurato e pronto a ricevere dati.");

% Acquisizione dati in tempo reale
disp("Avvio acquisizione dati...");
figure;
hold on;
grid on;
xlabel('X [m]');
ylabel('Y [m]');
zlabel('Z [m]');
title('Visualizzazione dati radar in tempo reale');
while true
    if dataPort.NumBytesAvailable > 0
        % Leggi dati dal radar
        rawData = read(dataPort, dataPort.NumBytesAvailable, 'uint8');

        % Parsing dati
        parsedData = parseRadarData(rawData);

        % Visualizzazione dati
        visualizeRadarData(parsedData);
    end
    pause(0.05); % Per evitare sovraccarichi di CPU
end

%% Funzione per inviare il file di configurazione al radar
function applyRadarConfiguration(controlPort, configFilePath)
    % Leggi il file di configurazione
    fileID = fopen(configFilePath, 'r');
    if fileID == -1
        error("Impossibile aprire il file di configurazione: %s", configFilePath);
    end

    % Leggi tutti i comandi dal file
    configCommands = textscan(fileID, '%s', 'Delimiter', '\n');
    fclose(fileID);

    % Invia ogni comando al radar
    disp("Invio comandi di configurazione...");
    for i = 1:length(configCommands{1})
        command = strtrim(configCommands{1}{i}); % Rimuovi spazi vuoti
        if ~isempty(command) && command(1) ~= '%' % Ignora righe vuote o commenti
            writeline(controlPort, command);
            pause(0.1); % Attendi per consentire l'elaborazione
        end
    end
    disp("Configurazione completata.");
end

%% Funzione per il parsing dei dati
function parsedData = parseRadarData(rawData)
    % Definisci la struttura dei dati parsati
    parsedData = struct('x', [], 'y', [], 'z', [], 'velocity', []);

    % Definisci la parola magica per sincronizzare i pacchetti
    magicWord = [2 1 4 3 6 5 8 7];
    magicWordBytes = length(magicWord);

    % Cerca la parola magica nei dati grezzi
    startIdx = strfind(rawData', magicWord);
    if isempty(startIdx)
        warning('Parola magica non trovata nei dati ricevuti.');
        return;
    end

    % Estrai l'header del frame
    frameHeaderLength = 40; % Lunghezza dell'header del frame in byte
    frameHeader = rawData(startIdx + magicWordBytes : startIdx + magicWordBytes + frameHeaderLength - 1);

    % Estrai il numero di TLV nel frame
    numTLVs = typecast(uint8(frameHeader(35:38)), 'uint32');

    % Imposta il puntatore all'inizio dei TLV
    pointer = startIdx + magicWordBytes + frameHeaderLength;

    % Processa ciascun TLV
    for tlvIdx = 1:numTLVs
        % Leggi l'header del TLV
        tlvType = typecast(uint8(rawData(pointer:pointer+3)), 'uint32');
        tlvLength = typecast(uint8(rawData(pointer+4:pointer+7)), 'uint32');
        pointer = pointer + 8; % Salta l'header del TLV

        % Verifica che ci siano abbastanza dati rimanenti
        if pointer + tlvLength > length(rawData)
            warning('Lunghezza del TLV supera i dati disponibili.');
            return;
        end

        % Processa il TLV in base al tipo
        switch tlvType
            case 1 % Punti rilevati
                % Numero di punti rilevati
                numDetectedPoints = (tlvLength - 8) / 16; % 16 byte per punto
                if numDetectedPoints > 0
                    % Estrai i dati dei punti
                    pointData = rawData(pointer:pointer + numDetectedPoints * 16 - 1);
                    pointData = typecast(uint8(pointData), 'single');
                    pointData = reshape(pointData, [4, numDetectedPoints]);

                    % Assegna i dati parsati alla struttura
                    parsedData.x = pointData(1, :);
                    parsedData.y = pointData(2, :);
                    parsedData.z = pointData(3, :);
                    parsedData.velocity = pointData(4, :);
                end
            % Aggiungi altri casi per altri tipi di TLV se necessario
            otherwise
                % TLV non supportato
                warning('Tipo di TLV non supportato: %d', tlvType);
        end

        % Aggiorna il puntatore alla fine del TLV corrente
        pointer = pointer + tlvLength - 8; % Sottrai 8 byte dell'header del TLV
    end
end

%% Funzione per la visualizzazione dei dati
function visualizeRadarData(parsedData)
    if isempty(parsedData.x)
        return; % Nessun dato da visualizzare
    end

    % Scatter 3D per visualizzare i punti
    scatter3(parsedData.x, parsedData.y, parsedData.z, 50, parsedData.velocity, 'filled');
    colormap('jet');
    colorbar;
    drawnow; % Aggiorna il grafico in tempo reale
end