function [rxVisionStruct, rxRadarStruct, rxIMUStruct, rxLaneStruct, cmd, currentStep, vFrame, tElapsedRcv] = ...
    dataReceiver(canChRx, tcpipObj, vFrame, frameSize)
% dataReceiver Receives and decodes all sensor and vision information.
%
% This function receives all the CAN FD messages from the CAN FD 
% receive channel and bundles them into corresponding sensor structures for
% the example. It also receives the corresponding video frame data from the
% TCP/IP connection.
%
% Copyright 2017 The MathWorks, Inc.

% Default initialize return values.
rxVisionStruct = 0;
rxRadarStruct = 0;
rxIMUStruct = 0;
rxLaneStruct = 0;
cmd = 0;
currentStep = 0;
tElapsedRcv.CAN = 0;
tElapsedRcv.Video = 0;

% Receive a single message.
cmdMsg = receive(canChRx, 1);

% Check if no messages were received on the channel.
if isempty(cmdMsg)
    return;
end

% Check if a command message was received.
if strcmp(cmdMsg.Name, 'TxCmd')
    % Check if the command indicates sensor data is available.
    cmd = cmdMsg.Signals{1}.cmd;
    if cmd == 3
        tStartCAN = tic;
        
        % Get the command details.
        currentStep = cmdMsg.Signals{1}.currentStep;
        numMsgs = cmdMsg.Signals{1}.numMsgs;
        
        % Wait until a full set of sensor data messages is available.
        while canChRx.MessagesAvailable < numMsgs
            pause(0.001); % do nothing
        end
                
        % Receive the messages.
        rxMsgs = receive(canChRx,numMsgs);
        
        % Separate the individual CAN FD messages.
        visionMsgs = rxMsgs(strcmp(rxMsgs.Name, 'Vision'), :);
        radarMsgs  = rxMsgs(strcmp(rxMsgs.Name, 'Radar'), :);
        laneMsgs   = rxMsgs(strcmp(rxMsgs.Name, 'Lane'), :);
        imuMsgs    = rxMsgs(strcmp(rxMsgs.Name, 'IMU'), :);
        
        % Create structures from messages for processing.
        rxVisionStruct = generateRxVisionStruct(visionMsgs);
        rxRadarStruct = generateRxRadarStruct(radarMsgs);
        rxLaneStruct = generateRxLaneStruct(laneMsgs);
        rxIMUStruct = generateRxIMUStruct(imuMsgs);

        tElapsedRcv.CAN = toc(tStartCAN);
        
        tStartVideo = tic;        
        % Receive video frame data from transmitter over TCP/IP.
        vFrame(:) = fread(tcpipObj, frameSize, 'uint8');
        
        tElapsedRcv.Video = toc(tStartVideo);
    end
end
end

function rxVisionStruct = generateRxVisionStruct(visionMsgs)
% generateRxVisionStruct Create the vision sensor structures required for the example.
%
%   If no vision sensor messages were received create a default structure for
%   this time step. If vision sensor messages were received, use the received
%   messages to create a corresponding vision structure required.

rxVisionStruct = struct(...
    'timeStamp',  uint64(0),...
    'object',     repmat(struct(...
        'id',             uint16(0),...
        'classification', uint8(0),...         % Unknown;
        'position',       single([0 0 0]'),... % (m)
        'velocity',       single([0 0 0]),...  % (m/s)
        'size',           single([0 0 0])),... % (m)
        [10 1]),...
    'numObjects', uint16(0));

if ~isempty(visionMsgs)
    localVisionMsgSignals = visionMsgs.Signals;
    rxVisionStruct.timeStamp  = uint64(seconds(visionMsgs.Time(1))*1e6);
    rxVisionStruct.numObjects = uint16(localVisionMsgSignals{1}.vision_numObjects);
    for currentMsg = 1:rxVisionStruct.numObjects
        rxVisionStruct.object(currentMsg).id             = uint16(localVisionMsgSignals{currentMsg}.vision_id);
        rxVisionStruct.object(currentMsg).classification = uint8(localVisionMsgSignals{currentMsg}.vision_classification);
        rxVisionStruct.object(currentMsg).position(1)    = single(localVisionMsgSignals{currentMsg}.vision_x);
        rxVisionStruct.object(currentMsg).position(2)    = single(localVisionMsgSignals{currentMsg}.vision_y);
        rxVisionStruct.object(currentMsg).velocity(1)    = single(localVisionMsgSignals{currentMsg}.vision_vx);
        rxVisionStruct.object(currentMsg).size(2)        = single(localVisionMsgSignals{currentMsg}.vision_width);
    end
end
end

function rxRadarStruct = generateRxRadarStruct(radarMsgs)
% generateRxRadarStruct Create the radar sensor data structures required for the example.
%
%   If no radar sensor messages were received create a default structure for
%   this time step. If radar sensor messages were received, use the received
%   messages to create a corresponding radar structure.

rxRadarStruct = struct(...;
    'timeStamp', uint64(0),...
    'object',    repmat(struct( ...
        'id',        uint16(0),...
        'status',    uint8(0),...         % NotActive;
        'position',  single([0 0 0])',... % (m)
        'velocity',  single([0 0 0]),...  % (m/s)
        'amplitude', single(0),...        % (dB)
        'rangeMode', uint8(0),...         % None;
        'rangeRate', single(0)),...
        [36 1]),...
    'numObjects', uint16(0));

if ~isempty(radarMsgs)
    localRadarMsgSignals = radarMsgs.Signals;
    rxRadarStruct.timeStamp = uint64(seconds(radarMsgs.Time(1))*1e6);
    rxRadarStruct.numObjects = uint16(localRadarMsgSignals{1}.radar_numObjects);
    for currentMsg = 1:rxRadarStruct.numObjects
        rxRadarStruct.object(currentMsg).id          = uint16(localRadarMsgSignals{currentMsg}.radar_id);
        rxRadarStruct.object(currentMsg).status      = uint8(localRadarMsgSignals{currentMsg}.radar_status);
        rxRadarStruct.object(currentMsg).position(1) = single(localRadarMsgSignals{currentMsg}.radar_x);
        rxRadarStruct.object(currentMsg).position(2) = single(localRadarMsgSignals{currentMsg}.radar_y);
        rxRadarStruct.object(currentMsg).velocity(1) = single(localRadarMsgSignals{currentMsg}.radar_vx);
        rxRadarStruct.object(currentMsg).velocity(2) = single(localRadarMsgSignals{currentMsg}.radar_vy);
        rxRadarStruct.object(currentMsg).rangeMode   = uint8(localRadarMsgSignals{currentMsg}.radar_rangeMode);
    end
end
end

function rxIMUStruct = generateRxIMUStruct(imuMsgs)
% generateRxIMUStruct Create the IMU sensor data structures required for the example.
%
%   If no IMU sensor messages were received create a zero padding structure for
%   this time step. If IMU sensor messages were received, use the received
%   messages to create a corresponding IMU structure.

localIMUMsgSignals = imuMsgs.Signals;
rxIMUStruct.timeStamp = uint64(seconds(imuMsgs.Time(1))*1e6);

if isempty(imuMsgs)
    rxIMUStruct.velocity = single(0);
    rxIMUStruct.yawRate = single(0);
else
    rxIMUStruct.velocity = single(localIMUMsgSignals{1}.imu_velocity);
    rxIMUStruct.yawRate = single(localIMUMsgSignals{1}.imu_yawRate);
end
end

function rxLaneStruct = generateRxLaneStruct(laneMsgs)
% generateRxLaneStruct Create the lane report structures required for the example.
%
%   If no lane report messages were received create a default structure for
%   this time step. If lane report messages were received, use the received
%   messages to create a corresponding lane structure.

localLaneMsgSignals = laneMsgs.Signals;

if isempty(laneMsgs)
    rxLaneStruct.left = struct(...
        'isValid',      uint8(0),...
        'confidence',   uint8(0),...  % None;
        'boundaryType', uint8(0),...  % None;
        'offset',       single(0),... % Distance (m) to the lane marker measured laterally from the y-axis at camera.y = 0
        'headingAngle', single(0),... % Heading angle of lane marker (rad)
        'curvature',    single(0));   % Curvature parameter in polynomial model
    rxLaneStruct.right = rxLaneStruct.left;
else
    % Left lane structure.
    rxLaneStruct.left.isValid      = uint8(localLaneMsgSignals{1}.leftLane_isValid);
    rxLaneStruct.left.confidence   = uint8(localLaneMsgSignals{1}.leftLane_confidence);
    rxLaneStruct.left.boundaryType = uint8(localLaneMsgSignals{1}.leftLane_boundaryType);
    rxLaneStruct.left.offset       = single(localLaneMsgSignals{1}.leftLane_offset);
    rxLaneStruct.left.headingAngle = single(localLaneMsgSignals{1}.leftLane_headingAngle);
    rxLaneStruct.left.curvature    = single(localLaneMsgSignals{1}.leftLane_curvature);
    
    % Right lane structure.
    rxLaneStruct.right.isValid      = uint8(localLaneMsgSignals{1}.rightLane_isValid);
    rxLaneStruct.right.confidence   = uint8(localLaneMsgSignals{1}.rightLane_confidence);
    rxLaneStruct.right.boundaryType = uint8(localLaneMsgSignals{1}.rightLane_boundaryType);
    rxLaneStruct.right.offset       = single(localLaneMsgSignals{1}.rightLane_offset);
    rxLaneStruct.right.headingAngle = single(localLaneMsgSignals{1}.rightLane_headingAngle);
    rxLaneStruct.right.curvature    = single(localLaneMsgSignals{1}.rightLane_curvature);
end
end
