classdef HelperLinkMatcher < handle
    %HelperLinkMatcher Match HERE HDLM links to a given points.
    %
    %   matcher = HelperLinkMatcher(linkTopology) returns a HERE HD Live
    %   Map link matcher corresponding to the table of link geometries
    %   (from the TopologyGeometry layer) given by linkTopology).
    %
    %   This helper class can be used to match a link to a given geographic
    %   coordinate. This approach considers the nearest link(s) spatially
    %   to the recorded geographic point and then additionally applies the
    %   direction of travel of the vehicle to determine the most probable
    %   link. The matched link is determined to be the link with the
    %   closest shape points to the specified geographic coordinate and
    %   along the direction of the velocity vector.
    
    %   Copyright 2018 The MathWorks, Inc.
    
    properties
        CurrentLink
    end
    
    properties (Hidden, SetAccess = immutable)
        Shape
    end
    
    methods
        function this = HelperLinkMatcher(linkTopology)
            this.Shape = linkTopology(repelem(1:height(linkTopology), ...
                cellfun('length', linkTopology.Geometry)), 1);
            this.Shape.Geometry = vertcat(linkTopology.Geometry{:});
        end
        
        function [link, linkLat, linkLon] = match(this, latitude, longitude, velocity)
            %match Match latitude-longitude point to HERE HDLM Link.
            
            % Find candidates based on spatial analysis
            link = this.findCandidates(latitude, longitude);
            
            % Filter candidates if more than one exists
            if numel(link) > 1
                link = this.filterCandidates(link, latitude, longitude, velocity);
            end
            
            linkIdxs = ismember(this.Shape.LinkId, link);
            linkLat = this.Shape.Geometry(linkIdxs, 1);
            linkLon = this.Shape.Geometry(linkIdxs, 2);
            
            this.CurrentLink = link;
        end
        
    end
    
    methods (Access = private)
        
        function candidates = findCandidates(this, latitude, longitude)
            %findCandidates Find candidate links from shape points.
            
            % Find link shape points within 10 meters
            d = dist(latitude, longitude, this.Shape.Geometry(:,1), ...
                this.Shape.Geometry(:,2));
            candidates = unique(this.Shape.LinkId(d < 10));
            
            % Add current link, if not included
            if ~isempty(this.CurrentLink) && ~any(candidates == this.CurrentLink)
                candidates = [candidates; this.CurrentLink];
            end
        end
        
        function candidates = filterCandidates(this, candidates, latitude, longitude, velocity)
            %filterCandidates Filter candidates.
            
            % Reduce list of candidate links to links that are in the
            % direction of travel
            velocityVector = ...
                repmat(abs(velocity/norm(velocity)), size(candidates));
            linkVector = velocityVector;
            
            % Get coarse direction of the defined shape of the link
            for idx = 1:numel(candidates)
                shapeIdxs = find(this.Shape.LinkId == candidates(idx));
                linkShape = this.Shape.Geometry(shapeIdxs(2),:) - ...
                    this.Shape.Geometry(shapeIdxs(1),:);
                linkVector(idx, :) = abs(linkShape/norm(linkShape));
            end
            
            % Reduce to candidates parallel to travel direction
            maxDot = max(dot(velocityVector, linkVector, 2));
            candidates = candidates(...
                abs(dot(velocityVector, linkVector, 2) - maxDot) < 0.005);
            
            % If multiple candidates still exist, do a more thorough
            % spatial analysis by calculating intermediate points along the
            % shape of the link.            
            if numel(candidates) > 1
                % Find the link with the minimum distance to the
                % latitude-longitude point
                minDist = 1000;
                minDistLink = candidates(1);
                for idx = 1:numel(candidates)
                    k = ismember(this.Shape.LinkId, candidates(idx));
                    % Get intermediate points of shape and check distance
                    [lat, lon] = addIntermPoints(this.Shape.Geometry(k,1), ...
                        this.Shape.Geometry(k,2));
                    M = min(dist(latitude, longitude, lat, lon));
                    if M < minDist
                        minDist = M;
                        minDistLink = candidates(idx);
                    end
                end
                candidates = minDistLink;
            end
        end
        
    end
end

function d = dist(lat1, lon1, lat2, lon2)
    %dist Calculate distance using equirectangular approximation.
    x = deg2rad(lon2 - lon1) .* cos(deg2rad(lat2 + lat1)/2);
    y = deg2rad(lat2 - lat1);
    d = sqrt(x.^2 + y.^2) * 6371 * 1000; % Earth radius ~= 6371 km
end

function [newLat, newLon] = addIntermPoints(lat, lon)
    %addPoints Add linearly spaced intermediate coordinates.
    step = 1e-5;
    newLat = [];
    newLon = [];
    for idx = 1:length(lat)-1
        if lat(idx + 1) > lat(idx)
            newLatSegment = lat(idx):step:lat(idx+1);
        else
            newLatSegment = lat(idx):-step:lat(idx+1);
        end
        newLon = [newLon, linspace(lon(idx), lon(idx+1), numel(newLatSegment))]; %#ok
        newLat = [newLat, newLatSegment]; %#ok        
    end
end