clc;
clear;
close('all');
%% 连接设备
% 获取摄像头列表
VCI_USBCAN2 = 4;

if CAN_CONNECT(1000) ~= 1
    return;
end



listCamera = webcamlist;
if isempty(listCamera)
    error('未连接摄像头');
else
    for idx = 1:length(listCamera)
        fprintf('[%d] %s\n', idx, listCamera{idx});
    end
end
% 连接摄像头
cameraId = 1;
deviceCamera = webcam(cameraId);
deviceCamera.Resolution = '1280x720';
% 连接雷达
deviceRadar = tcpclient('192.168.31.88', 5000);
empty_cnt = 0;
%% 生成figure
figure;
hDataImage = imagesc(nan(480, 640, 3));
axis image;
set(gca, 'XTick', [], 'YTick', []);
hTitle = title('', 'Interpreter', 'none', 'FontSize', 16);
%% 采集数据
pathData = fullfile('D:', 'RadarEye', string(datetime, 'yyyyMMdd_HHmmss_SSS'));
mkdir(pathData);

while isvalid(deviceRadar)
    pause(0.1);
    % 获取雷达数据
    numByte = deviceRadar.NumBytesAvailable;
    if numByte > 0
        dataRadar = uint8(read(deviceRadar, numByte, 'uint8'));
        empty_cnt = 0;
    else
        empty_cnt = empty_cnt + 1;
    end

    if empty_cnt > 3
        figure('Color', 'white'); % 创建白色背景的图形窗口
        plot(0,0,'k.'); % 绘制一个黑色点作为参考点
        text(0,0,'网络连接异常','FontSize',20,'HorizontalAlignment','center','Color','red'); % 添加文字
        break;
    end

    % 获取摄像头数据
    dataImage = snapshot(deviceCamera);

    %获取飞控数据
    [id,data,num] = Can_Rx(5);
    % 初始化为 NaN
    q0 = NaN; q1 = NaN; q2 = NaN; q3 = NaN;
    x  = NaN; y  = NaN; z  = NaN; valid = NaN;
    
    foundIDs = false(1,4); % 用于标记四个 ID 是否已收到
    
    % 遍历接收到的帧
    for i = 1:num
        switch id(i)
            case hex2dec('2345')  % q0 q1
                vals = typecast(uint8(data(i,:)), 'single'); 
                q0 = vals(1);
                q1 = vals(2);
                foundIDs(1) = true;
            case hex2dec('2346')  % q2 q3
                vals = typecast(uint8(data(i,:)), 'single'); 
                q2 = vals(1);
                q3 = vals(2);
                foundIDs(2) = true;
            case hex2dec('2347')  % x y
                vals = typecast(uint8(data(i,:)), 'single'); 
                x = vals(1);
                y = vals(2);
                foundIDs(3) = true;
            case hex2dec('2348')  % z valid
                vals = typecast(uint8(data(i,:)), 'single'); 
                z = vals(1);
                valid = vals(2);
                foundIDs(4) = true;
        end
    
        % 如果四个 ID 都收到了，就结束循环
        if all(foundIDs)
            break;
        end
    end
    poseData = single([q0, q1, q2, q3, x, y, z, valid]);
    poseBytes = typecast(poseData, 'uint8');  % 转为字节流
    dataAll = [dataRadar(:); poseBytes(:)];
    % 保存数据
    name = string(datetime, 'yyyyMMdd_HHmmss_SSS');
    imwrite(dataImage, fullfile(pathData, sprintf('%s.jpg', name)));
    fileRadar = fopen(fullfile(pathData, sprintf('%s.bin', name)), 'wb+');
    fwrite(fileRadar, dataAll, 'uint8');
    fclose(fileRadar);
    %% 绘图显示
    hDataImage.CData = dataImage;
    hTitle.String = sprintf('%s: %d Byte', name, numByte);
    drawnow;
end

function [id,data,num] = Can_Rx(time_out_ms)
    num = 0;
    id = [];
    data = [];
    VCI_USBCAN2 = 4;
    len = VCI_GetReceiveNum(VCI_USBCAN2,0,0);
    if len
         [GetLength,Frames]=VCI_Receive(VCI_USBCAN2,0,0,2500,30);
         if GetLength   
             
             id(1:GetLength) = Frames(1:GetLength,1);
             num = GetLength;
             for i = 1:GetLength
                 % 将每个字节转换为十六进制字符串并存储在元胞中
                 byteData = Frames(i,8:15);
                 data(i,1:8) = byteData;
             end
         end
    end
end


function InitConfig = VCI_INIT_CONFIG(baudrate)
    AccCode = hex2dec('0x80000008');
    AccMask =hex2dec('0xFFFFFFFF');
    Reserved = 0;
    Filter= 0;
    Timing0 =hex2dec('0x00');
    if (baudrate == 500)
        Timing1 =hex2dec('0x1c');
    else
        Timing1 =hex2dec('0x14');
    end
    
    Mode =0;
    InitConfig=[AccCode AccMask Reserved Filter Timing0 Timing1 Mode];

end

function isOK = CAN_CONNECT(baudrate)
    isOK = 1;
    VCI_USBCAN2 = 4;

    if VCI_OpenDevice(VCI_USBCAN2,0,0) 
        fprintf("VCI_OpenDevice successed \r\n");
        vci_initconfig = VCI_INIT_CONFIG(baudrate); %波特率500k，正常模式
    
        ret = VCI_InitCAN(VCI_USBCAN2, 0, 0, vci_initconfig);
        if ret == 1
            fprintf('call VCI_InitCAN1 successed \r\n');
        else
            fprintf('call VCI_InitCAN1 error \r\n');
            isOK = 0;
        end
        ret = VCI_StartCAN(VCI_USBCAN2, 0, 0);
    
        if ret == 1
            fprintf('call VCI_StartCAN successed \r\n');
        else
            fprintf('call VCI_StartCAN error \r\n');
            isOK = 0;
        end
        pause(0.1);
    else
        fprintf("VCI_OpenDevice failed \r\n");
        isOK = 0;
    end

end

delete(deviceRadar);
clear deviceRadar;
clear('cam');