bagFile = matlab.internal.examples.downloadSupportFile("ros","rosbags/simulated_lidar_radar_driving_798.bag");

bag = rosbag(bagFile);
disp(bag.AvailableTopics(:,["NumMessages", "MessageType"]))

lidarBagSel = select(bag,"Topic","/ego/lidar_scan");
lidarMsgs = readMessages(lidarBagSel,"DataFormat","struct");
stateBagSel = select(bag,"Topic","/ego/state");
stateMsgs = readMessages(stateBagSel,"DataFormat","struct");
radarMsgs = cell(bag.AvailableTopics{"/ego/radar_1/detections","NumMessages"},4);
for idxRadar = 1:4
    radarBagSel = select(bag,"Topic",sprintf("/ego/radar_%d/detections",idxRadar));
    radarMsgs(:,idxRadar) = readMessages(radarBagSel,"DataFormat","struct");
end

clockBagSel = select(bag,"Topic","/clock");
clockMsg = readMessages(bag,1,"DataFormat","struct");
startTime = double(clockMsg{1}.Clock_.Sec) + double(clockMsg{1}.Clock_.Nsec)*1e-9;

[lidarInfo, radarInfo, radarParameters] = helperRadarLidarInfo;

radarTrackingAlgorithm = helperROSRadarTracker(radarInfo);
radarConfig = fuserSourceConfiguration("SourceIndex",1,...
    "IsInitializingCentralTracks",true,...
    "CentralToLocalTransformFcn",@central2radar,...
    "LocalToCentralTransformFcn",@radar2central);

lidarTrackingAlgorithm = helperLidarTrackingAlgorithm(lidarInfo);
lidarConfig = fuserSourceConfiguration("SourceIndex",2,...
    "IsInitializingCentralTracks",true);

fuser = trackFuser("SourceConfigurations",{radarConfig;lidarConfig},...
    "StateTransitionFcn",lidarTrackingAlgorithm.StateTransitionFcn,...
    "ProcessNoise",diag([1 3 1]),...
    "HasAdditiveProcessNoise",false,...
    "AssignmentThreshold",[250 inf],...
    "ConfirmationThreshold",[3 5],...
    "DeletionThreshold",[5 5],...
    "StateFusion","Custom",...
    "CustomStateFusionFcn",@helperRadarLidarFusionFcn);

displayHelper = helperROSSensorFusionDisplay;

for idx = 1:numel(lidarMsgs)
    % Extract time on first sensor reading.
    % Make time relative to rosbag start time for easier tracking.
    lidarTimeStamp = lidarMsgs{idx}.Header.Stamp;
    lidarTime = double(lidarTimeStamp.Sec) + ...
        double(lidarTimeStamp.Nsec)*1e-9 - startTime;
    radarTimeStamp = radarMsgs{idx,1}.Header.Stamp;
    radarTime = double(radarTimeStamp.Sec) + ...
        double(radarTimeStamp.Nsec)*1e-9 - startTime;
    
    % Extract vehicle state and modify structures for processing.
    egoPose = struct;
    stateMsg = stateMsgs{idx};
    positionMsg = stateMsg.Pose.Pose.Position;
    egoPose.Position = [positionMsg.X ; positionMsg.Y ; positionMsg.Z];
    % Orientation in degrees.
    orientQuat = rosReadQuaternion(stateMsg.Pose.Pose.Orientation);
    orientEul = eulerd(orientQuat,"XYZ","point");
    egoPose.Roll = orientEul(1);
    egoPose.Pitch = orientEul(2);
    egoPose.Yaw = orientEul(3);
    % By convension, nav_msgs/Odometry velocity is provided in the child
    % reference frame (the vehicle). The fusion requires velocity in the world
    % reference frame.
    velMsg = stateMsg.Twist.Twist.Linear;
    egoPose.Velocity = rotatepoint(orientQuat,...
        [velMsg.X velMsg.Y velMsg.Z]);
    
    % Extract point cloud from lidar for processing
    % This lidar provided no RGB or intensity information
    lidarXYZPoints = rosReadXYZ(lidarMsgs{idx});
    ptCloud = pointCloud(lidarXYZPoints);
    
    % Extract radar detections into a single array using metadata to
    % specify the source sensor.
    nDetections = sum(cellfun(@(msg) numel(msg.Detections),radarMsgs(idx,:)));
    radarDetections = cell(nDetections,1);  % Preallocate
    idxDetection = 1;
    for idxRadar = 1:size(radarMsgs,2)
        for idxRadarDetection = 1:numel(radarMsgs{idx,idxRadar}.Detections)
            detMsg = radarMsgs{idx,idxRadar}.Detections(idxRadarDetection);
            detTime = double(detMsg.Header.Stamp.Sec) + ...
                double(detMsg.Header.Stamp.Nsec)*1e-9 - startTime;
            measureMsg = detMsg.Pose.Pose.Position;
            measurement = [measureMsg.X ; measureMsg.Y ; measureMsg.Z];
            % Measurement noise is stored in the bounding box field due to
            % this message type containing Pose instead of PoseCovariance.
            measureNoise = diag([detMsg.BoundingBoxLwh.X detMsg.BoundingBoxLwh.Y detMsg.BoundingBoxLwh.Z]);
            % Store signal-to-noise ratio in Score field.
            objectAttributes = struct("TargetIndex",detMsg.Id,"SNR",detMsg.Score);
            radarDetections{idxDetection} = objectDetection(detTime,measurement,...
                "MeasurementNoise",measureNoise,...
                "SensorIndex",idxRadar,...
                "ObjectClassID",0,...
                "ObjectAttributes",{objectAttributes},...
                "MeasurementParameters",{radarParameters(idxRadar)});
            idxDetection = idxDetection + 1;
        end
    end
    
    % Generate sensor tracks and analysis information like the bounding box
    % detections and point cloud segmentation information.
    radarTracks = radarTrackingAlgorithm(egoPose, radarDetections, radarTime);
    [lidarTracks, lidarDetections, segmentationInfo] = ...
        lidarTrackingAlgorithm(egoPose, ptCloud, lidarTime);
    localTracks = [radarTracks ; lidarTracks];
    
    % Update the fuser. The first call must contain one local track.
    if ~(isempty(localTracks) && ~isLocked(fuser))
        fusedTracks = fuser(localTracks,lidarTime);
    else
        fusedTracks = objectTrack.empty(0,1);
    end
    
    % Update the display
    updateSensorData(displayHelper,ptCloud,radarDetections)
    plotTracks(displayHelper,radarTracks,lidarTracks,fusedTracks,egoPose)
end

function centralTrack = radar2central(radarTrack)
% Initialize a track of the correct state size
centralTrack = objectTrack('State',zeros(10,1),...
    'StateCovariance',eye(10));

% Sync properties of radarTrack except State and StateCovariance with
% radarTrack See syncTrack defined below.
centralTrack = syncTrack(centralTrack,radarTrack);

xRadar = radarTrack.State;
PRadar = radarTrack.StateCovariance;

H = zeros(10,7); % Radar to central linear transformation matrix
H(1,1) = 1;
H(2,2) = 1;
H(3,3) = 1;
H(4,4) = 1;
H(5,5) = 1;
H(8,6) = 1;
H(9,7) = 1;

xCentral = H*xRadar;  % Linear state transformation
PCentral = H*PRadar*H'; % Linear covariance transformation

PCentral([6 7 10],[6 7 10]) = eye(3); % Unobserved states

% Set state and covariance of central track
centralTrack.State = xCentral;
centralTrack.StateCovariance = PCentral;
end

function radarTrack = central2radar(centralTrack)
% Initialize a track of the correct state size
radarTrack = objectTrack('State',zeros(7,1),...
    'StateCovariance',eye(7));

% Sync properties of centralTrack except State and StateCovariance with
% radarTrack See syncTrack defined below.
radarTrack = syncTrack(radarTrack,centralTrack);

xCentral = centralTrack.State;
PCentral = centralTrack.StateCovariance;

H = zeros(7,10); % Central to radar linear transformation matrix
H(1,1) = 1;
H(2,2) = 1;
H(3,3) = 1;
H(4,4) = 1;
H(5,5) = 1;
H(6,8) = 1;
H(7,9) = 1;

xRadar = H*xCentral;  % Linear state transformation
PRadar = H*PCentral*H'; % Linear covariance transformation

% Set state and covariance of radar track
radarTrack.State = xRadar;
radarTrack.StateCovariance = PRadar;
end

function tr1 = syncTrack(tr1,tr2)
props = properties(tr1);
notState = ~strcmpi(props,'State');
notCov = ~strcmpi(props,'StateCovariance');

props = props(notState & notCov);
for i = 1:numel(props)
    tr1.(props{i}) = tr2.(props{i});
end
end