classdef HelperLaneGroupMatcher < handle
    %HelperLaneGroupMatcher Match HERE HDLM lane groups to a given points.
    %
    %   matcher = HelperLaneGroupMatcher(laneRoadRef, laneTopology) returns
    %   a HERE HD Live Map lane group matcher corresponding to the table of
    %   lane-road references (from the LaneRoadReferences layer) given by
    %   laneRoadRef and the table of lane topology (from the LaneTopology
    %   layer) given by laneTopology.
    %
    %   This helper class can be used to match a lane group to a given
    %   geographic coordinate and link. The approach described below uses
    %   two layers: LaneRoadReferences and Lane Topology. The
    %   LaneRoadReferences layer enable users to translate a position on
    %   the Road Model (given by a link) to a corresponding position on the
    %   HD Lane Model (given by a lane group). Next, the LaneTopology layer
    %   gives geometry data which can be used to consider data which exists
    %   spatially within the boundaries of each of the candidate lane
    %   groups. Such an approach is prone to error and subject to the
    %   accuracy of the recorded GPS data.
    
    %   Copyright 2018 The MathWorks, Inc.
        
    properties
        CurrentLaneGroup 
    end
    
    properties (Hidden, SetAccess = immutable)
        LaneRoadReference
        LaneTopology
    end
    
    methods
        function this = HelperLaneGroupMatcher(laneRoadRef, laneTopology)
            % Class constructor.
            this.LaneRoadReference = laneRoadRef;
            this.LaneTopology = laneTopology;
        end
        
        function [laneGroup, isForward, geometry] = match(this, link, latitude, longitude, velocity)
            %match Find the lane group that matches the given coordinates.
            
            % Find all lane group candidates relevant to the given link
            isForward = true;
            laneGroup = [];
            candidates = this.LaneRoadReference.LaneGroupId{...
                this.LaneRoadReference.LinkId == link};
            
            % For each candidate, check if the coordinate exists within the
            % boundary of the lane group
            for idx = 1:numel(candidates)
                k = this.LaneTopology.LaneGroupId == candidates(idx);
                
                % Get boundary polygon
                geometry = [...
                    this.LaneTopology.LeftGeometry{k}; ...
                    this.LaneTopology.RightGeometry{k}];
                geometry = geometry(boundary(geometry), :);
                
                % Check if the point exists within the boundary polygon
                if inpolygon(latitude, longitude, geometry(:,1), geometry(:,2))
                    laneGroup = candidates(idx);
                    if isempty(this.CurrentLaneGroup) || laneGroup ~= this.CurrentLaneGroup
                        this.CurrentLaneGroup = laneGroup;
                    end
                    lgVector = this.LaneTopology.LeftGeometry{k}(end,:) ...
                        - this.LaneTopology.LeftGeometry{k}(1,:);
                    isForward = dot(velocity, lgVector) > 0;
                    break
                end
                
            end
        end
        
    end
end