classdef navGraph < nav.algs.internal.GraphBase
%navGraph Graph data structure for Navigation Toolbox
%   navGraph is the graph data structure for Navigation toolbox, which
%   aids the search-based planners.
%
%   The navGraph object allows you to create a graph and perform
%   computations on it. The navGraph object supports functionalities
%   that are frequently used by graph search algorithms. You can easily
%   implement Dijkstra, A*, or its variants using navGraph.
%
%   In graph theory, states represent nodes and links represent edges.
%   The states and links are represented by their corresponding row
%   index in the table. The navGraph is a directed graph that currently
%   supports unique names, with no self-loops in edges.
%
%   NAVGRAPHOBJ = navGraph(STATES, LINKS) creates the navGraph object with
%   nodes specified as a matrix of states and edges specified as matrix
%   of links(or endnodes). The STATES and LINKS inputs set the value
%   for States and Links properties respectively.
%
%   NAVGRAPHOBJ = navGraph(STATES, LINKS, Name=Value) specifies
%   additional parameters using one or more name-value arguments in
%   addition to the argument from the previous syntax.
%   Name-Value argument options:
%       Name            - State names, specified as a column vector of
%                         string or cell array of characters. The number
%                         of rows must be equal to the number of states,
%                         and its values must be unique.
%       Weight          - Link weight, specified as a numeric column
%                         vector. The number of rows must be equal to
%                         the number of links.
%
%   NAVGRAPHOBJ = navGraph(DIGRAPHOBJ) creates the navGraph object from
%   the data present in digraph object. DIGRAPHOBJ object nodes must
%   contain first column named "StateVector".
%
%   NAVGRAPHOBJ = navGraph(STATETABLE, LINKTABLE) creates a navGraph
%   object with state table and link table, that also contains the meta
%   data for the graph. The STATETABLE and LINKTABLE inputs set the
%   value of the States and Links properties, respectively. STATETABLE must
%   contain first column named "StateVector" and LINKTABLE must contain
%   first column named "EndStates".
%
%   NAVGRAPHOBJ = navGraph(__, Name=Value) specifies additional
%   parameters using one or more name-value arguments in addition to
%   the argument from the previous syntax.
%   Name-Value argument options:
%       LinkWeightFcn   - Link weight function, specified as function
%                         handle that computes the link weights in the
%                         absence of the Weight parameter. The
%                         LinkWeightFcn parameter sets the value of the
%                         LinkWeightFcn property.
%
%   navGraph properties:
%       States        - Read-only table for state, containing
%                       'StateVector' and other meta-data.
%       Links         - Read-only table for edges, containing
%                       'EndStates' and other meta-data.
%       LinkWeightFcn - Function handle for transition cost.
%
%   navGraph methods:
%       successors    - Find successive state IDs and costs
%       addstate      - Add new state(s) in graph
%       rmstate       - Removes state(s) from navGraph
%       findstate     - Finds stateID of the state,if exists in navGraph
%       addlink       - Add new link(s) in navGraph
%       rmlink        - Removes link(s) from navGraph
%       findlink      - Finds linkIDs for given state pairs,if exists
%                       in navGraph
%       show          - Displays graph
%       state2index   - Find index for queried state vector
%       index2state   - Find state vector for queried index
%       copy          - Create deep copy of graph base object
%
%
%   Examples:
%
%       % Load data for states and links
%       load navGraphData.mat
%
%       % Create navGraph from states and links data
%       navGraphObj1 = navGraph(data.states, data.links);
%
%       % Create navGraph from states and links data with names
%       navGraphObj2 = navGraph(data.states, data.links, Name=data.names);
%
%       % Create navGraph from states and links data with weight
%       % link function.
%       navGraphObj3 = navGraph(data.states, data.links, ...
%           LinkWeightFcn=@nav.algs.distanceManhattan);
%
%       % Create navGraph from states and links data with weights and names
%       navGraphObj4 = navGraph(data.states, data.links, ...
%                       Weight=data.linkWt);
%       navGraphObj5 = navGraph(data.states, data.links, ...
%                       Name=data.names);
%       navGraphObj6 = navGraph(data.states, data.links, ...
%                       Name=data.names, Weight=data.linkWt);
%
%       % Create navGraph from state and link tables
%       stateTable = table(data.states, data.names, data.numLanes,...
%            VariableNames={'StateVector', 'Name', 'Lanes'});
%       linkTable = table(data.links, data.linkWt, data.curvature,...
%            VariableNames={'EndStates', 'Weight', 'Curvature'});
%       navGraphObj7 = navGraph(stateTable, linkTable);
%
%       % Create navGraph from digraph object
%       stateTable = table(data.states, VariableNames={'StateVector'});
%       linkTable = table(data.links, VariableNames={'EndNodes'});
%       dgObj = digraph(linkTable, stateTable);
%       navGraphObj8 = navGraph(dgObj);
%
%   See also plannerAStar

%   Copyright 2022-2023 The MathWorks, Inc.

%#codegen


    properties (Access = protected)
        %Graph Digraph object to save graph internally and for outsourcing
        %certain functionalities.
        Graph
    end

    properties (Access = {?nav.algs.internal.InternalAccess})
        %Internal copies of data for performance

        % For look-up mode
        %SuccessorLookup Contains starting location and end location for
        % the sorted endstates of Links table for the given node.
        SuccessorLookup

        %LinkWeightLookup Stores endstates IDs and weight as matrix
        LinkWeightLookup

        % Flag for first run, useful for error out while 'SET' during
        % codegen
        isFirstRun=1;
    end

    methods

        function obj = navGraph(varargin)
        % navGraph constructor

        % Validate number of arguments:
        % Maximum number of arguments includes states, links
        % and name-value pairs for weight, name, link cost function
            narginchk(1, 8);

            % Validator required constructor inputs
            [inputType, numMinInputs] = navGraph.validateConstructorInputs(...
                varargin{:});

            % Parse the name-value pair inputs to constructor
            [propsStruct, props] = navGraph.validateConstructorNVPairInputs(inputType,...
                                                                            varargin{numMinInputs+1:end});

            % Create the navGraph based on the user input
            [StatesTable, LinksTable] = navGraph.createNavGraph(inputType, ...
                                                                propsStruct, props, varargin{:});

            % Pre-validation
            navGraph.validateStates(StatesTable);
            navGraph.validateLinks(LinksTable);

            %Validating that links must not contain incorrect state i.e.
            %non-existent states
            if(isnumeric(LinksTable.EndStates))
                if(any(LinksTable.EndStates>height(StatesTable),'all'))
                    coder.internal.error('nav:navalgs:navGraph:StatesInLinkNotFound',height(StatesTable))
                end
            end

            % Make sure all elements in the cell array are string scalars or character arrays
            if iscell(LinksTable.EndStates)
                LinksTable.EndStates = cellstr(LinksTable.EndStates);
            end

            % Removing duplicates from states and links
            [StatesTable,LinksTable] = navGraph.removeDuplicates(StatesTable,LinksTable);

            % Setting base object
            obj@nav.algs.internal.GraphBase(StatesTable, LinksTable);

            % Allow graph editing
            obj.allowEditGraph();

            % Sort links according to state IDs or names
            obj.sortLinks();

            % Create digraph object which is used for various operations
            % like finding a state, or link etc.
            obj.Graph = navGraph.createDigraph(obj.States, obj.Links);

            % Set public properties with default or user-specified values
            obj.LinkWeightFcn = propsStruct.LinkWeightFcn;

            obj.isFirstRun =0;
            obj.resetEditGraph();
        end

        function [sucIDs,costs] = successors(obj,currID)
        %successors Find successive state IDs and costs
        %
        % [SUCIDS, COSTS] = successors(NAVGRAPHOBJ, CURRID) finds the
        % state id SUCIDS and the cost of all states that are connected
        % by directed links to given CURRID. The costs are computed
        % using the Weight or LinkWeightFcn parameter. Returns empty
        % when successors not found.
        %
        % CURRID is specified as a positive integer scalar state ID, or
        % as a string scalar or character vector state name.
        % SUCIDS is returned as an integer column vector.
        % COSTS is returned as a numeric column vector of the same
        % size as SUCIDS.
        %
        %   Example:
        %       % Load example navGraph object.
        %       load navGraphData.mat;
        %
        %       % Find successors based on state ID.
        %       id = successors(navGraphObj, 3);
        %
        %       % Find successors based on name.
        %       [sucIDs,Cost] = successors(navGraphObj,"G");
        %
        %   See also navGraph

            validateattributes(currID,{'numeric','string',...
                                       'char'},{'nonempty'},'navGraph',...
                               'successors')

            % Converting name input to numeric.
            if isnumeric(currID)
                currIndex = currID;
                validateattributes(currIndex,{'numeric'},{'scalar','nonnan',...
                                                          'finite','positive','integer','<=',...
                                                          height(obj.States.StateVector)},'navGraph',...
                                   'successor index')
            elseif ischar(currID) || isstring(currID)
                if(isstring(currID))
                    validateattributes(currID,{'string'},{'scalar'},...
                                       'navGraph','successors');
                end
                currIndex = obj.findstate(currID);
            else
                validateattributes(currID,{'cellstr'},{'scalar','nonempty'},...
                                   'navGraph', 'successors')
            end

            % Finds the successors
            if(currIndex~=0)
                sucIDs = successors(obj.Graph,currIndex);
            else
                sucIDs = [];
            end
            costs = [];

            % Computing Cost
            if(~isempty(sucIDs))
                costs =obj.getLinkCost(currIndex,sucIDs);
            end
        end

        function ID = findstate(obj,stateData)
        %findstate Finds the numeric state ID of the state, if exists
        %
        % ID = findstate(NAVGRAPHOBJ, STATEDATA), finds the numeric state IDs
        % STATEIDS, if they exist in navGraph object. STATEDATA can either
        % be the name of states, state vector or states table.
        %
        % Specify the name of the states as a column vector of characters,
        % strings or cell array of character vectors.
        %
        % Specify the state vector as a matrix in which each row represents
        % a state vector. The state vectors must have same column size as
        % those in StateVector column of the navGraph States table.
        %
        % Specify the states table as a table with all columns as in navGraph
        % States table. Returns STATESIDS when all row elements of STATEDATA
        % are identical to row elements in navGraph States table.
        %
        % STATEIDS is returned as an integer column vector. Returns 0, when
        % the state is not found.
        %
        %   Example:
        %       % Load example navGraph object.
        %       load navGraphData.mat;
        %
        %       % Find state based on state vector.
        %       id1 = findstate(navGraphObj, [8 10 1;1 1 0.291876068170633]);
        %
        %       % Find state based on name.
        %       id2 = findstate(navGraphObj,["D";"A"]);
        %
        %       % Creating table for the state to search
        %       stateData = table([3 6 0.142170047601527;1 1 1],{'F';'A'},[2;2], ...
        %      'VariableNames',{'StateVector','Name','Lanes'});
        %
        %       % Finding state based on table input
        %       id3 = findstate(navGraphObj,stateData);
        %
        %   See also navGraph

            validateattributes(stateData,{'numeric','string','char',...
                                          'cell','table'},{'nonempty'},'navGraph',...
                               'findstate');

            ID = [];
            if isnumeric(stateData)
                % For state as state vector with same number of columns as
                % navGraph's state vector.
                validateattributes(stateData,{'numeric'},{'nonnan','finite',...
                                                          'ncols',width(obj.States.StateVector)},...
                                   'navGraph','findstate state')
                ID = zeros(height(stateData),1);
                for i = 1:height(stateData)
                    % Finding the closest nodes based on euclidean distance.
                    [idx,minDist] = closestStateID(obj,stateData(i,:));
                    % If both states coincides, we will return that id, else 0.
                    if(minDist<=eps)
                        ID(i) = idx;
                    end
                end

            elseif ischar(stateData)|| isstring(stateData)
                % Finding states given as names.
                if isstring(stateData)
                    if(stateData=="")
                        coder.internal.error('nav:navalgs:navGraph:InvalidFindStateInput')
                    end
                end
                nameData = cellstr(stateData);
                idx = findnode(obj.Graph, nameData);
                ID = [ID idx];
            elseif iscellstr(stateData)
                idx = findnode(obj.Graph,stateData);
                ID = [ID idx];
            elseif istable(stateData)
                % Finding states, when state are given as table rows.
                navGraph.validateStates(stateData,1);
                validateattributes(stateData.StateVector,{'numeric'},...
                                   {'nonnan','finite','ncols',size(obj.States.StateVector,2)},...
                                   'navGraph','stateData')
                ID = findStateFromTable(obj,stateData);
            else
                coder.internal.error('nav:navalgs:navGraph:InvalidFindStateInput');
            end
        end

        function id = addstate(obj,varargin)
        %addstate Add state(s) to the navGraph
        %
        %   IDs = addstate(nG, stateData) adds the state(s) to the graph,
        %       where stateData is the matrix of state vectors, with each
        %       row depicting a state. It returns IDs of added states in
        %       graph.
        %
        %   IDs = addstate(nG, stateDataCol1, stateDataCol2, . . . stateDataColN) Adds the
        %   state(s) to the graph, where stateDataCol1 represents state matrix. Other
        %   stateDataCol represents respective meta data columns as they appear in States
        %   table.
        %
        %   Example:
        %
        %   load navGraphData.mat
        %
        %   % Create navGraph from states and links data
        %   nG = navGraph(data.states, data.links);
        %   % Adds states with state vector (1,1,0.23) to graph.(Graph States only contains
        %   % state vectors)
        %   id = addstate(nG, [1 1 0.23]);
        %
        %   % Adds multiple states to graph with state vectors (1,1,0.23),(2,2,0.5), (3,3,0.7) and (4,4,0.75).
        %   id = addstate(nG, [1 1 0.23;2 2 0.5;3 3 0.7;4 4 0.75]);
        %
        %   nG2 = navGraph(data.states, data.links, Name=data.names);
        %   % Adding States to graph containing state vectors and names.
        %   id = addstate(nG2, [1 1 0.23;2 2 0.5], cellstr(["StateA"; "StateB"]));
        %
            numColStates = size(obj.States.Properties.VariableNames,2)+1;

            % Verifying if proper number of columns are input.
            if(nargin~=numColStates)
                coder.internal.error('nav:navalgs:navGraph:IncorrectNumOfInputColumn',numColStates,nargin);
            end

            % Verifying that all columns are of same type of already added
            % States
            numInRows = height(varargin{1});
            nameCol = find(strcmp('Name', obj.States.Properties.VariableNames));
            for i =1:numColStates-1

                if ~isempty(nameCol) && i==nameCol
                    classVar = {'cell', 'string'};
                else
                    classVar = {class(obj.States{:,i})};
                end

                numCol = width(obj.States{:,i});
                validateattributes(varargin{i},classVar,...
                                   {'nonempty','nrows',numInRows,'ncols',numCol},...
                                   'navGraph',obj.States.Properties.VariableNames{i});

                if ~isempty(nameCol) && i==nameCol && iscell(varargin{i})
                    varargin{i} = cellstr(varargin{i}); % Make sure all elements in the cell array are string scalars or character arrays
                end
            end

            stateData = table(varargin{:},'VariableNames',obj.States.Properties.VariableNames);
            validateattributes(stateData.StateVector,{'numeric'},...
                               {'nonempty','nonnan','finite'},'navGraph','addstate');

            % Remove duplicate state vectors from input stateData
            [~, occurIdx] = navGraph.getUniqueStateIDs(stateData.StateVector);
            stateData = stateData(occurIdx,:);

            if(isempty(obj.States))
                id = 1:height(varargin{1});
            else
                if(any(strcmp(obj.States.Properties.VariableNames,'Name')))
                    id = obj.findstate(stateData.Name);
                else
                    id = obj.findstate(stateData.StateVector);
                end
                id = find(id==0);
            end
            prevNumStates=height(obj.States);

            % Updating internal graph.
            obj.Graph = obj.Graph.addnode(stateData(id,:));

            if(coder.target('MATLAB'))
                if(any(strcmp(obj.States.Properties.VariableNames,'Name')))
                    isstr = isstring(obj.States.Name);
                else
                    isstr = false;
                end
            end

            obj.allowEditGraph();
            obj.States = obj.Graph.Nodes;
            obj.resetEditGraph();
            if(coder.target('MATLAB'))
                if(isstr)
                    obj.States.Name = string(obj.States.Name);
                end
            end
            id = (prevNumStates+1:height(obj.States.StateVector))';
            obj.resetLookup();
        end

        function rmstate(obj,state)
        %rmstate Removes state(s) (nodes) from the graph, along with associated links.
        %
        %   State can be numeric IDs or cell string in row or  vector
        %
        %   rmstate(nG, stateIDs) Removes the states from the graph.
        %   stateIDs can be single  id or row vector of multiple ids.
        %
        %   rmstate(nG, stateNames) Removes the states from the graph using
        %   name identifier.
        %
        %   stateNames can be single name or row of string array of multiple
        %   names.
        %
        %   Example:
        %
        %   load navGraphData.mat
        %   nG = navGraph(data.states, data.links, Name=data.names);
        %
        %   % Removes the state 2 from the graph.
        %   rmstate(nG, 2);
        %
        %   % Removes the state 2,3 and 4 from the graph.
        %   rmstate(nG, [2;3;4]);
        %
        %   nG = navGraph(data.states, data.links, Name=data.names);
        %   % Removes the state with name 'B' from the graph.
        %   rmstate(nG, 'B');
        %
        %   % Removes the state with name 'B','C','D' from the graph.
        %   rmstate(nG, ["B";"C";"D"]);

            if (isnumeric(state))
                validateattributes(state, {'numeric'},{'column','nonempty','positive','nonnan','finite','integer'},'navGraph','rmstate');
            else
                validateattributes(state, {'cell','char','string'},{'nonempty'},'navGraph','state');
                state = cellstr(state);
                validateattributes(state, {'cell'},{'column'},'navGraph','state');
            end
            if(iscellstr(state)||ischar(state)||isstring(state)||iscell(state))
                nodeID = obj.findstate(state);
            else
                nodeID = state;
                nodeID = nodeID(nodeID<=height(obj.States.StateVector));
            end
            nodeID = nodeID(nodeID~=0);
            obj.Graph = rmnode(obj.Graph,nodeID);

            if(coder.target('MATLAB'))
                if(any(strcmp(obj.States.Properties.VariableNames,'Name')))
                    isstr = isstring(obj.States.Name);
                else
                    isstr = false;
                end
            end

            obj.allowEditGraph();
            obj.States = obj.Graph.Nodes;
            if(coder.target('MATLAB'))
                if(isstr)
                    obj.States.Name = string(obj.States.Name);
                end
            end
            obj.resetEditGraph();

            linkT = navGraph.updateLinkTableColumn(obj.Graph.Edges,'EndStates');
            % Renaming edges to indices
            if(isnumeric(obj.Links.EndStates) && ~isnumeric(linkT.EndStates))
                linkT.EndStates = navGraph.edgeNamesToIndices(...
                    obj.States.Name, obj.Graph.Edges.EndNodes);
            end
            % Convert to string array if source data is a string array
            % (note that digraph stores the names as cellstr)
            if isstring(obj.Links.EndStates)
                linkT.EndStates = string(linkT.EndStates);
            end
            obj.allowEditGraph()
            obj.Links = linkT;
            obj.resetEditGraph();
            obj.resetLookup();
        end

        function ID = findlink(obj,statePair)
        %findlink Finds link IDs for given state pairs,if exists in navGraph
        %
        % ID = findlink(NAVGRAPHOBJ, STATEPAIRS), finds the numeric ID of
        % the link of the state pairs STATEPAIRS, if they exists in navGraph
        % object. STATEPAIRS can either be pairs of state names or pair of
        % state IDs.
        %
        % Specify the pairs of state names as N-by-2 string array or
        % N-by-2 cell array of character vectors.
        % Specify the pairs of state IDs as N-by-2 positive integers.
        %
        % ID is returned as an integer column vector. Returns 0, when
        % the link is not found
        %
        %   Example:
        %       % Load example navGraph object.
        %       load navGraphData.mat;
        %
        %       % Find link based on state id pair.
        %       ID = findlink(navGraphObj, [3 7]);
        %       id1 = findlink(navGraphObj, [3 7;4 5; 5 4]);
        %
        %       % Find link based on state name pairs as string.
        %       id2 = findlink(navGraphObj,["C" "G";"D" "E";"E" "D"]);
        %
        %       % Find link based on state name pairs as cell array of chars.
        %       id3 = findlink(navGraphObj,{'C' 'G';'D' 'E';'E' 'D'});
        %
        %   See also navGraph

            validateattributes(statePair,{'numeric','cell','string','char'},...
                               {'ncols',2},'navGraph','findlink');
            if ischar(statePair) ||isstring(statePair)
                state1ID = obj.findstate(statePair(:,1));
                state2ID = obj.findstate(statePair(:,2));
            elseif iscellstr(statePair)
                stateTo = statePair(:,1);
                stateFro = statePair(:,2);
                state1ID = obj.findstate(stateTo);
                state2ID = obj.findstate(stateFro);
            else
                validateattributes(statePair,{'numeric'},{'nonnan','finite',...
                                                          'nonempty','integer'},'navGraph','findlink')
                state1ID = statePair(:,1);
                state2ID = statePair(:,2);
            end
            ID = findedge(obj.Graph,state1ID,state2ID);
        end

        function id = addlink(obj,varargin)
        %addlink Add new links via column vectors for each column in table.
        %
        %   IDs = addlink(nG, linkData) adds the edge(s) to the graph,
        %   where linkData is the  matrix of endNodes, with each row
        %   depicting an edge.
        %
        %   IDs = addlink(nG, linkDataCol1, linkDataCol2, . . . linkDataColN) Adds the
        %   link(s) to the graph, where linkDataCol1 represents endnodes
        %   matrix. Other linkDataCol represents respective meta data columns
        %   as they appear in Links table.
        %
        %   Example:
        %
        %   load navGraphData.mat
        %
        %   nG = navGraph(data.states, data.links);
        %   % Adds link (1,3) to the graph.(Graph Links only contains EndNodes)
        %   id = addlink(nG, [1 3]);
        %
        %   % Adds multiple links (1,3),(1,4),(2,4) to graph.
        %   id = addlink (nG, [1 3;1 4;2 4]);
        %
        %   nG = navGraph(data.states, data.links, Weight=data.linkWt);
        %   % Adding Links to graph containing EndNodes and Weights.
        %   id = addlink (nG, [1 3;1 4;2 4], [12;20;8]);

            numColLinks = size(obj.Links.Properties.VariableNames,2)+1;

            % Verifying if proper number of columns are input.
            if(nargin~=numColLinks)
                coder.internal.error('nav:navalgs:navGraph:IncorrectNumOfInputColumn',numColLinks,nargin);
            end

            % Verifying if states are present in navGraph
            if(isempty(obj.States))
                coder.internal.error('nav:navalgs:navGraph:StatesNotPresent');
            end

            % Verifying that all columns are of same type of already added
            % States
            numInRows = height(varargin{1});
            % Validate remaining columns
            for i =1:numColLinks-1
                if i==1 %First column is EndStates and can be
                    classVar = {'numeric', 'string', 'cell'};
                else
                    classVar = {class(obj.Links{:,i})};
                end
                validateattributes(varargin{i},classVar,...
                                   {'nonempty','nrows',numInRows},...
                                   'navGraph',obj.Links.Properties.VariableNames{i});
                if i==1 && iscell(varargin{i}) % EndStates column is a cell array
                    varargin{i} = cellstr(varargin{i}); % Make sure all elements in the cell array are string scalars or character arrays
                end
            end

            % Create link data table to added to navGraph
            linkData = table(varargin{:},'VariableNames',obj.Links.Properties.VariableNames);

            % Validate links in more detail (like number of columns)
            obj.validateLinks(linkData);

            % Update EndStates format of input links if they are different
            % from original format defined during navGraph construction
            if isnumeric(obj.Links.EndStates) && ~isnumeric(linkData.EndStates)
                linkData.EndStates = obj.edgeNamesToIndices(obj.States.Name, linkData.EndStates);
            elseif ~isnumeric(obj.Links.EndStates) && isnumeric(linkData.EndStates)
                names = obj.States.Name';
                linkData.EndStates = names(linkData.EndStates);
            end
            if iscellstr(obj.Links.EndStates) && isstring(linkData.EndStates)
                linkData.EndStates = cellstr(linkData.EndStates);
            elseif isstring(obj.Links.EndStates) && iscellstr(linkData.EndStates)
                linkData.EndStates = string(linkData.EndStates);
            end

            % Fetching state names.
            stateNames = "";
            if(iscellstr(linkData.EndStates)||isstring(linkData.EndStates))
                stateNames = obj.States.Name;
            end

            % Removing duplicates and self loop links
            linkData = navGraph.omitSelfLoopAndDuplicateLinks(...
                linkData,stateNames);

            % Skipping the links already present in navGraph
            if(isempty(obj.Links)||isempty(linkData))
                id = 1:height(varargin{1});
            else
                id = obj.findlink(linkData.EndStates);
                id = find(id==0);
            end

            % Adding the links
            prevNumLinks=height(obj.Links);
            if(~isempty(linkData))
                links = [obj.Links;linkData(id,:)];
                id = (prevNumLinks+1:height(links.EndStates))';
            else
                links = obj.Links;
                id = [];
            end
            obj.allowEditGraph()
            obj.Links = links;
            sortedIndex = obj.sortLinks();
            obj.resetEditGraph()

            % Updated output id based on sorted indices
            id = find(ismember(sortedIndex, id));

            % Updating internal graph.
            obj.Graph = navGraph.createDigraph(obj.States, obj.Links);
            obj.resetLookup();
        end

        function rmlink(obj,link)
        %rmlink Removes links from navGraph, along with associated data.
        %
        %   Link must be specified as one of the following: numeric IDs,
        %   state pairs or cell string containing state pairs stored as a
        %   column vector.
        %
        %   rmlink (nG, linkIDs) Removes the links and associated data from
        %   the graph. The links are specified by linkIDs. linkIDs can be a
        %   scalar or column vector that specify the IDs of the links to be
        %   removed. The length of the vector specify the number of links
        %   to be removed.
        %
        %   rmlink (nG, stateIDs) Removes the links and associated data
        %   from the graph. The links are specified by stateIDs. stateIDs
        %   is a matrix with two columns where each row consists of start
        %   and end IDs of the states that form the link.
        %
        %   If the specified link does not exist in the navGraph, then no
        %   action is taken.
        %
        %   Example:
        %
        %   load navGraphData.mat
        %
        %   nG = navGraph(data.states, data.links);
        %   % Removes the link 2 from the graph.
        %   rmlink(nG, 2);
        %
        %   % Removes the links 2,3 and 4 from the graph.
        %   rmlink(nG, [2;3;4]);
        %
        %   nG = navGraph(data.states, data.links);
        %   % Removes the links going from state 1 to 2,from the graph.
        %   rmlink(nG, [1 2]);
        %
        %   % Removes the links connecting states (1,2), (3,4),(1,3) from the graph.
        %   rmlink(nG, [1 2;3 4;1 3]);

            validateattributes(link, {'numeric','cell','char','string'},{'nonempty'},'navGraph','rmlink');
            if(iscellstr(link)||ischar(link)||isstring(link)||size(link,2)==2)                
                if(isnumeric(link))
                    %When Link IDs are given
                    validateattributes(link, {'numeric'},{'positive','nonnan','finite'},'navGraph','rmlink');
                    numStates = height(obj.States);
                    link = link(all(link<=numStates,2),:);
                end
                if(~isempty(link))
                    linkID = obj.findlink(link);
                else
                    linkID = [];
                end
            else
                %When state pairs are given.
                validateattributes(link, {'numeric'},{'positive','nonnan','finite'},'navGraph','rmlink');
                numLinks = height(obj.Links);
                linkID = link(link<=numLinks);
            end
            linkID = linkID(linkID~=0);

            % Removing links from graph.
            obj.Graph = rmedge(obj.Graph,linkID);

            linkT = navGraph.updateLinkTableColumn(obj.Graph.Edges,'EndStates');
            % Renaming edges to indices
            if(isnumeric(obj.Links.EndStates) && ~isnumeric(linkT.EndStates))
                linkT.EndStates = navGraph.edgeNamesToIndices(...
                    obj.States.Name, obj.Graph.Edges.EndNodes);
            end
            % Convert to string array if source data is a string array as
            % digraph stores this to cell array
            if isstring(obj.Links.EndStates)
                linkT.EndStates = string(linkT.EndStates);
            end
            obj.allowEditGraph();
            obj.Links = linkT;
            obj.resetEditGraph();
            obj.resetLookup();
        end

        function statevector = index2state(obj,STATESIDS)
        %index2state Find state vector for queried index
        %
        % STATESVECTOR = index2state(NAVGRAPHOBJ,STATESIDS) returns the state
        % vector STATESVECTOR for queries STATESIDS, if they exist in the
        % navGraph object.
        %
        % Specify the STATESIDS as a positive integer column vector
        %
        % Example:
        %   % Loads example navGraph object
        %   load navGraphData.mat
        %
        %   % Returns states for queried indices
        %   states = index2state(navGraphObj,[2;4;7]);
        %
        %   See also navGraph

            validateattributes(STATESIDS,{'numeric'},{'nonempty','nonnan',...
                                                      'finite','integer','positive'},'navGraph','state2index');
            STATESIDS = STATESIDS(STATESIDS<=height(obj.States.StateVector));
            statevector = obj.States.StateVector(STATESIDS,:);
        end

        function STATEID = state2index(obj,state)
        %state2index Find index for queried state vector
        %
        % STATEID = state2index(NAVGRAPHOBJ,STATEVECTOR) finds the state
        % IDs STATEIDS for queried state vectors STATEVECTOR, if they exist
        % in the navGraph object. Specify the state vectors as a matrix in
        % which each row represents a state vector. The state vectors must
        % contain same number of columns as those in the StateVector column
        % of the navGraph States table.
        %
        % Example:
        %   % Loads example navGraph object
        %   load navGraphData.mat
        %
        %   % Returns states ID for queried state vector
        %   ID = state2index(navGraphObj,[1 1 0.291876068170633]);
        %
        %   See also navGraph

            validateattributes(state,{'numeric'},{'nonempty','nonnan',...
                                                  'finite','ncols',size(obj.States.StateVector,2)},...
                               'navGraph','state2index');
            STATEID = obj.findstate(state);
        end

        function grPlot = show(obj, varargin)
        %show Plot graph representation.
        %
        % show(NAVGRAPHOBJ) Plots graph representation
        %
        % GRPLOT = show(NAVGRAPHOBJ) Plots graph representation and
        % returns the graph plot object GRPLOT.
        %
        % GRPLOT = show(NAVGRAPHOBJ, Name=Value) specifies options using
        % one or more name-value arguments.The options include:
        %
        %       'Parent'    - Axes on which to plot graph representation,
        %                     specified as an Axes object
        %
        % Example:
        %   % Load example navGraph object.
        %   load navGraphData.mat;
        %
        %   % Show on default gca.
        %   show(navGraphObj);
        %
        %   % Show on given axes handle.
        %   f1 = figure;
        %   ax = axes(Parent = f1);
        %   gr = show(navGraphObj,Parent=ax);
        %
        %   See also navGraph

        % If called in code generation, throw incompatibility error
            coder.internal.errorIf(~coder.target('MATLAB'),...
                                   'nav:navalgs:prm:GraphicsSupportCodegen', 'show')
            narginchk(1,3);
            if(nargin==1)
                ax = gca;
            else
                % Get default properties
                propsDefault = struct('Parent',gca);

                % Parse name-value pair inputs and return struct containing
                % properties
                props = coder.internal.parseParameterInputs(propsDefault, struct(), varargin{:});
                propsStruct = coder.internal.vararginToStruct(props, propsDefault, varargin{:});
                ax = propsStruct.Parent;
            end
            graphPlotObj = plot(ax,obj.Graph);

            % Returning graphplot handle
            if nargout > 0
                grPlot = graphPlotObj;
            end
        end

        function newObj = copy(obj)
        %copy Deep copy the navGraph object
        % NAVGRAPHOBJNEW = copy(NAVGRAPHOBJOLD) creates a deep copy of the
        % navGraph object
        %
        %   See also navGraph
            newObj = navGraph(obj.States,obj.Links,'LinkWeightFcn',obj.LinkWeightFcn);
        end
    end

    methods(Hidden)

        function [IDs, minDist] = closestStateID(obj,state,varargin)
        %closestStateID Returns the closest state ID in States using
        % euclidean distance, If parameter 'all' is present then all
        % equidistant states will be return, otherwise only the first one
        % will be returned. minDist is the minimum distance of the closest
        % states.
        %
        %   Example:
        %       % Load example navGraph object.
        %       load navGraphData.mat;
        %
        %       % Finding the ID of closest state to given state
        %       [IDs, minDist] = closestStateID(navGraphObj,[8 7 0]);
        %
        %   See also navGraph

            narginchk(0,3);
            validateattributes(state,{'numeric'},{'nonempty','nonnan',...
                                                  'finite','nrows',1,'ncols',size(obj.States.StateVector,2)},...
                               'navGraph','closestStateID');
            % Finding distance from all states based on euclidean distance.
            dist = obj.distanceFromAllStates(state);
            % Finds the closest state index.
            [minDist,IDs] = min(dist);
            if(nargin==3)
                validatestring(varargin{1},{'all'}, 'navGraph');
                IDs = find(dist == min(dist))';
            end
        end

        function dist = distanceFromAllStates(obj,state)
        %distanceFromAllStates Finds the distance of given state, from all
        %the states present in navGraph.
        %
        %   Example:
        %       % Load example navGraph object.
        %       load navGraphData.mat;
        %
        %       % Finding the distance of all states from given state
        %       [dist] = distanceFromAllStates(navGraphObj,[8 7 0]);
        %
        %   See also navGraph
            dist = [];
            validateattributes(state,{'numeric'},{'nonempty','nonnan',...
                                                  'finite','nrows',1,'ncols',size(obj.States.StateVector,2)},...
                               'navGraph','distanceFromAllNodes');
            if(~isempty(obj.States.StateVector))
                % Euclidean distance
                dist = nav.algs.distanceEuclidean(obj.States.StateVector,state);
            end
        end
    end

    methods (Access = {?nav.algs.internal.InternalAccess})
        function generateLookup(obj)
        %generateLookup Specialized function for navGraph to pre-compute
        %link weights, considering the Cost function to be vectorized
        %and edges to be sorted.

        % Fetching links in numeric way
            edges = obj.Links.EndStates;

            if iscell(edges) || isstring(edges)
                %To find and hence convert string array to integers for
                %edges
                %edges = [obj.findstate(edges(:,1)), obj.findstate(edges(:,2))];
                % Remove the following lines after findstate in above line
                % is implemented
                edges = navGraph.edgeNamesToIndices(obj.States.Name, obj.Links.EndStates);
            end

            % Creates Successor Lookup for start and end indices for each
            % link.
            [counts, values] = groupcounts(edges(:,1));
            ind = [1; 1+cumsum(counts)];
            numNodes = height(obj.States.StateVector);
            obj.SuccessorLookup = zeros(numNodes, 2);
            obj.SuccessorLookup(values,:) = [ind(1:end-1), ind(2:end)-1];

            % Populate link data with link in integer and link weight.
            if any(strcmp(obj.Links.Properties.VariableNames, 'Weight'))
                Weight = obj.Links.Weight;
            else
                if nargin(obj.LinkWeightFcn)==2 ||...
                        any(strcmp(func2str(obj.LinkWeightFcn), navGraph.getStandardCostFcns()))
                    % @(state1, state2) costFunction(state1, state2,..)
                    states1 = obj.index2state(edges(:,1));
                    states2 = obj.index2state(edges(:,2));
                    Weight = obj.LinkWeightFcn(states1,states2);
                elseif nargin(obj.LinkWeightFcn)==3
                    % @(index1, index2, navGraphObj) costFunction(index1, index2, navGraphObj,..)
                    Weight = obj.LinkWeightFcn(edges(:,1), edges(:,2), obj);
                end
            end
            obj.LinkWeightLookup = [edges,Weight];
        end

        function resetLookup(obj)
        %resetLookup The lookup data needs to be recomputed
            obj.SuccessorLookup = zeros(0, 2);
            obj.LinkWeightLookup = zeros(0,width(obj.Links.EndStates)+1);
        end

        function ind = sortLinks(obj)
        % Sort links according to state IDs
            if isnumeric(obj.Links.EndStates)
                [obj.Links, ind] = sortrows(obj.Links, 'EndStates');
            elseif iscellstr(obj.Links.EndStates) || isstring(obj.Links.EndStates)
                edgeIndices = navGraph.edgeNamesToIndices(obj.States.Name, ...
                                                          obj.Links.EndStates);
                [~, ind] = sortrows(edgeIndices,[1,2]);
                obj.Links = obj.Links(ind,:);
            end
        end

        function cost = getLinkCost(obj,currIndex,targetIDs)
        %getLinkCost Get the link cost from the cost function or the
        %link table

            if(height(currIndex)==1)
                currIDs = repmat(currIndex,size(targetIDs));
            elseif (height(currIndex)==height(targetIDs))
                currIDs = currIndex;
            end

            if(~strcmp(obj.Links.Properties.VariableNames,'Weight'))
                cost = obj.getLinkFcn(currIDs,targetIDs);
            else
                cost = obj.getLinkWeightFromTable(currIDs,targetIDs);
            end
        end

        function cost = getLinkWeightFromTable(obj,id1,id2)
        %getLinkWeightFromTable Fetches link weight using the weight column
        %present in Links Table.

            lID = obj.findlink([id1,id2]);
            cost = obj.Links.Weight(lID);
        end

        function cost = getLinkFcn(obj,id1,id2)
        %getLinkFcn Computes cost using the given LinkWeightFcn.
            if(nargin(obj.LinkWeightFcn)==2 ||...
               any(strcmp(func2str(obj.LinkWeightFcn), navGraph.getStandardCostFcns())))
                % If LinkWeight function takes input of states.
                states1 = obj.index2state(id1);
                states2 = obj.index2state(id2);
                cost = obj.LinkWeightFcn(states1,states2);
            else
                % If LinkWeight function takes input of states indices.
                if coder.target('MATLAB')
                    cost = obj.LinkWeightFcn(id1,id2,obj);
                else
                    coder.internal.error('nav:navalgs:navGraph:CodegenNotSupported');
                end
            end
        end

        function ID = findStateFromTable(obj,stateData)
        %findStateFromTable Finds if the given stateData table rows are
        %present in navGraph.

            ID = zeros(height(stateData),1);
            for i = 1:height(stateData)
                % Finding the state based on distance using state vector.
                [idx,minDist] = closestStateID(obj,stateData.StateVector(i,:),'all');
                if(max(minDist)<=eps)
                    % Validate if state is already present by comparing
                    % rest of the data in table
                    [stateFound,idFound] = intersect(obj.States(idx,2:end),stateData(i,2:end));
                    if(~isempty(stateFound))
                        % If i'th row stateData is present, return its ID.
                        ID(i) = idx(idFound(1));
                    end
                end
            end
        end
    end

    methods (Access = protected)
        function validateLinkWeightFcn(obj,linkWeightFcn)
        %validateLinkWeightFcn For adding additional verification if required
        %while assigning LinkWeightFcn as per navGraph requirement.
            verifyCodegenCompatibility(obj,'linkWeightFcn');
            validateLinkWeightFcn@nav.algs.internal.GraphBase(obj,linkWeightFcn);

            % Validate number of input and output arguments
            nargIn = nargin(linkWeightFcn);
            nargOut = nargout(linkWeightFcn);
            % abs takes care of negative nargin when the function has
            % varargin or varargout
            coder.internal.errorIf((nargIn>=0 && nargIn<2) || ...  % without varargin
                                   (nargIn<0 && nargIn>-3 ) ... % with varargin
                                   , 'nav:navalgs:navGraph:InvalidLinkWeightFcn')
            coder.internal.errorIf(abs(nargOut)<1, 'nav:navalgs:navGraph:InvalidLinkWeightFcn')

            %Reset look-up mode properties
            obj.SuccessorLookup = zeros(0, 2);
            obj.LinkWeightLookup = zeros(0,width(obj.Links.EndStates)+1);
        end
        function verifyCodegenCompatibility(obj,arg)
        % If called in code generation, throw incompatibility error
            coder.internal.errorIf(~coder.target('MATLAB')&&obj.isFirstRun==0,'nav:navalgs:plannerastargrid:PropertySetInCodeGeneration', arg)
        end

        function postTableValuesUpdate(obj)
        %postTableValuesUpdate Post processing steps after updating the values in
        %the States or Links implemented by the inheriting classes

        % Call the superclass's method
            postTableValuesUpdate@nav.algs.internal.GraphBase(obj)

            % Create digraph object
            obj.Graph = navGraph.createDigraph(obj.States, obj.Links);

            % Reset lookup data
            obj.resetLookup();
        end
    end

    methods(Hidden, Static, Access = protected)

        function [inputType, numMinInputs] = validateConstructorInputs(varargin)
        % validateConstructorInputs Validate the constructor inputs
        % depending upon the input type. For example, if the first input
        % is a state table, then there must be minimum two inputs, with
        % the second input being the link table

        % Validate first input type if specified
            if nargin >= 1
                validateattributes(varargin{1}, {'numeric', 'table', 'digraph'}, ...
                                   {}, 'navGraph', 'first input')
            end

            if nargin == 0
                % Constructor signature:  navGraph()
                numMinInputs = 0;
                inputType = 'none';
            elseif isa(varargin{1}, 'numeric')
                % Constructor signature: navGraph(states, links, Name=Value)
                inputType = 'numeric';
                numMinInputs = 2;
                narginchk(numMinInputs, inf)
                validateattributes(varargin{1}, {'numeric'}, {'nonnan', 'finite'},...
                                   'navGraph', 'first input')
                if isnumeric(varargin{2})
                    validateattributes(varargin{2}, {'numeric'}, {'nonnan', 'finite'},...
                                       'navGraph', 'second input')
                else
                    validateattributes(varargin{2}, {'cell','string'}, {},...
                                       'navGraph', 'second input')
                end

            elseif isa(varargin{1}, 'table')
                % Constructor signature: navGraph(StateTable, LinkTable, Name=Value)
                inputType = 'table';
                numMinInputs = 2;
                narginchk(numMinInputs, inf)
                validateattributes(varargin{2}, {'table'}, {},...
                                   'navGraph', 'second input')
            elseif isa(varargin{1}, 'digraph')
                % Constructor signature: navGraph(digraphObj, Name=Value)
                inputType = 'digraph';
                numMinInputs = 1;
                narginchk(numMinInputs, inf)
            end
        end

        function validateStates(stateData,numRow)
        %validateStates For adding additional verification if required
        %while assigning States Table as per navGraph requirement.
            if(nargin==1)
                numRow = 2;
            end
            validateStates@nav.algs.internal.GraphBase(stateData);
            validateattributes(stateData.StateVector, {'numeric'}, ...
                               {'nonempty','nonnan','finite'}, 'navGraph', 'States')
            coder.internal.errorIf(width(stateData.StateVector)<2, 'nav:navalgs:navGraph:InvalidStateInput');
            coder.internal.errorIf(height(stateData.StateVector)<numRow, 'nav:navalgs:navGraph:InvalidStateInput');
        end

        function validateLinks(linkData)
        %validateLinks For adding additional verification if required
        %while assigning Links Table as per navGraph requirement.
            validateLinks@nav.algs.internal.GraphBase(linkData);
            validateattributes(linkData.EndStates,{'numeric','cell','char',...
                                                   'string'},{'nonempty','ncols',2}, 'navGraph','Links')
            if(isnumeric(linkData.EndStates))
                validateattributes(linkData.EndStates,{'numeric'},{'finite',...
                                                                   'nonnan','integer'},'navGraph','Links')

            end
        end

        function [propsStruct, props] = validateConstructorNVPairInputs(inputType, varargin)
        % validateConstructorNVPairInputs Validate name-value pair inputs
        % to the constructor

        % Get default properties
            propsDefault = navGraph.propertyDefaults;

            % Parse name-value pair inputs and return struct containing
            % properties
            props = coder.internal.parseParameterInputs(propsDefault, struct(), varargin{:});
            propsStruct = coder.internal.vararginToStruct(props, propsDefault, varargin{:});

            % If the first input type is table or digraph, then it is not
            % allowed to specify Name, Weight name-value pairs
            if strcmp(inputType, 'table') || strcmp(inputType, 'digraph')
                coder.internal.errorIf(~isempty(propsStruct.Name),...
                                       'nav:navalgs:navGraph:InvalidNameValuePairs', ...
                                       inputType)
                coder.internal.errorIf(~isempty(propsStruct.Weight),...
                                       'nav:navalgs:navGraph:InvalidNameValuePairs',...
                                       inputType)
            end
        end

        function defaults = propertyDefaults()
        % propertyDefaults Define the default values for the
        % public properties
            defaults = struct(...
                'LinkWeightFcn', @nav.algs.distanceEuclidean, ...
                'Weight', [],...
                'Name', {''});
        end

        function [stateTable,linkTable] = createNavGraph(inputType, propsStruct, props, varargin)
        %createNavGraph Convert constructor inputs into graph

        %Props stores whether property is default (~=0) or non-default (~=0)
            nameColumnDisabled = props.Name==0;
            weightColumnDisabled = props.Weight==0;

            switch inputType

              case 'none'
                % Create empty state and link when there are no inputs
                stateTable = table(zeros(0,2), VariableNames={'StateVector'});
                linkTable = table(zeros(0,2), VariableNames={'EndStates'});

              case 'numeric'
                % Create state and link tables from matrix inputs
                states = varargin{1};
                links = varargin{2};

                if nameColumnDisabled && (iscellstr(links) || isstring(links)) && width(links)==2
                    coder.internal.error('nav:navalgs:navGraph:MissingNameColumn')
                end

                % Create State table
                if nameColumnDisabled
                    stateTable = table(states, VariableNames={'StateVector'});
                else
                    validateattributes(propsStruct.Name, {'cell', 'string'},...
                                       {'ncols',1}, 'navGraph', 'Name')
                    name = convertStringsToChars(propsStruct.Name);
                    coder.internal.errorIf(height(states) ~= length(name), ...
                                           'nav:navalgs:navGraph:InvalidNameInput')
                    for ind=1:length(name)

                        validateattributes(name{ind}, {'char'}, {'nonempty'},...
                                           'navGraph', 'Name', ind)
                    end
                    stateTable = table(states, propsStruct.Name,...
                                       VariableNames={'StateVector', 'Name'});
                end

                % Create Link table
                if weightColumnDisabled
                    linkTable = table(links, VariableNames={'EndStates'});
                else
                    validateattributes(propsStruct.Weight, {'numeric'},...
                                       {'ncols', 1, 'nonnan', 'finite', 'real'},...
                                       'navGraph', 'Weight')
                    coder.internal.errorIf(height(links) ~= length(propsStruct.Weight), ...
                                           'nav:navalgs:navGraph:InvalidWeightInput')
                    linkTable = table(links, propsStruct.Weight, ...
                                      VariableNames={'EndStates', 'Weight'});
                end

              case 'table'
                % Create state and link tables from table inputs
                stateTable = varargin{1};
                linkTable = varargin{2};

              case 'digraph'
                % Create state and link tables from digraph input
                stateTable = varargin{1}.Nodes;
                linkTable = varargin{1}.Edges;
                linkTable = navGraph.updateLinkTableColumn(linkTable, 'EndStates');
            end
        end

        function Graph = createDigraph(statesTable, linksTable)
        % createDigraph Create digraph object from StatesTable and
        % LinksTable. The digraph object is used for various operations
        % like finding a state, or link etc.

        % Update EndStates column name to EndNodes
            linksTable = navGraph.updateLinkTableColumn(linksTable, 'EndNodes');
            % Create a digraph object
            Graph = digraph(linksTable, statesTable);
        end

        function linksTable = updateLinkTableColumn(linksTable, newName)
        % updateLinkTableColumn Update the name of the first column in
        % the link table from EndStates to EndNodes or vice-versa

        % For codegen purposes we cannot directly update the table
        % properties, so we need to make a deep copy
            columnNames = {newName, linksTable.Properties.VariableNames{2:end}};
            columnData = cell(1, length(columnNames));
            for i = 1:length(columnNames)
                columnData{i} = table2array(linksTable(:,i));
            end
            linksTable = table(columnData{:}, VariableNames=columnNames);
        end

        function egdeIndices = edgeNamesToIndices(name, endStates)
        % edgeNamesToIds Convert edges containing names to numeric
        % indices
            egdeIndices = zeros(size(endStates));
            for i=1:2
                egdeIndices(:,i) = cellfun(@(x) find(strcmp(name, x)),...
                                           endStates(:,i));
            end
        end

        function [StatesTable,LinksTable] = removeDuplicates(StatesTable,LinksTable)
        %removeDuplicates Removes duplicate elements from table

        % Fetching state names.
            stateNames = "";
            if(iscellstr(LinksTable.EndStates)||isstring(LinksTable.EndStates))
                stateNames = StatesTable.Name;
            end

            % Remove duplicate state vectors
            [StatesTable, LinksTable] = navGraph.omitDuplicateStates(StatesTable, LinksTable, stateNames);

            % Removing duplicates and self loop links
            LinksTable = navGraph.omitSelfLoopAndDuplicateLinks(...
                LinksTable,stateNames);

            %Validating links for incorrect state.
            if(isnumeric(LinksTable.EndStates))
                if(any(LinksTable.EndStates>height(StatesTable),'all'))
                    coder.internal.error('nav:navalgs:navGraph:StatesInLinkNotFound',height(StatesTable))
                end
            end
        end

        function [StatesTable, LinksTable] = omitDuplicateStates(StatesTable, LinksTable, stateNames)
        %omitDuplicateStates Removes all the duplicate states from
        %the state table, keeping only first occurrence. Removes the
        %links corresponding to the duplicate states and renumbers the
        %ids if link ids are provided.

        % Get end states IDs of links
            if (stateNames=="")
                linksEndStates = LinksTable.EndStates;
            else
                linksEndStates = navGraph.edgeNamesToIndices(StatesTable.Name,LinksTable.EndStates);
            end

            % Get state ids for unique state vector and the locations of their occurrences
            [stateIDs, occurIdx] = navGraph.getUniqueStateIDs(StatesTable.StateVector);

            % Remove duplicate states
            StatesTable = StatesTable(occurIdx,:);

            % Update EndStates of links with updated state ids after
            % removing duplicate states
            links = stateIDs(linksEndStates);
            if (stateNames=="")
                LinksTable.EndStates = links;
            else
                namesUpdated = stateNames(occurIdx);
                if iscellstr(LinksTable.EndStates)
                    namesUpdated = cellstr(namesUpdated);
                else
                    namesUpdated = string(namesUpdated);
                end
                LinksTable.EndStates = namesUpdated(links);
            end
        end

        function [linkT] = omitSelfLoopAndDuplicateLinks(LinkTable,stateNames)
        %omitSelfLoopAndDuplicateLinks Removes all the duplicate links from
        %the link table, keeping only first occurrence after sorting. Also
        %removes the self loops if exists from the link table.

        %Converting edges into numeric form, so as to bring uniformity
        %in sorting between integers or strings
            if(stateNames~="")
                %Converting edge names to edge indices
                link = navGraph.edgeNamesToIndices(stateNames,LinkTable.EndStates);
            else
                link = LinkTable.EndStates;
            end

            % Sorting the numeric links in increasing order
            [link,sortID] = sortrows(link,[1,2]);
            % Sorting the LinkTable in numeric order as link
            LinkTable = LinkTable(sortID,:);

            %Finding non-duplicate rows.
            [~,idCopy] = unique(link,'rows');

            %Finding rows without self loops
            idDup = find(link(:,1)~=link(:,2));

            %Extracting unique edges.
            idx = intersect(idCopy,idDup);
            linkT = LinkTable(idx,:);
        end

        function navCostFcns = getStandardCostFcns()
        % getStandardCostFcns Get standard cost functions shipping with
        % Navigation toolbox
            navCostFcns = {'nav.algs.distanceEuclidean',...
                           'nav.algs.distanceManhattan',...
                           'nav.algs.distanceEuclideanSquared'};
        end

        function [stateIDs, occurIdx] = getUniqueStateIDs(states)
        %getUniqueStateIDs Get unique states ids with in a specified
        %tolerance
        %Inputs:
        %   STATES : [N,2] matrix
        %Outputs:
        %   STATEIDS : Indices of unique element vector mapped to original
        %              input STATES vector
        %   OCCURIDX : Indices of first occurrences of unique elements in STATES
        %
        % Example:
        %    states = [1,2; 1,2; 1,2; 2,5; 3,4; 3,4; 5,7; 7,8; 7,8];
        %    getUniqueStateIDs outputs
        %       stateIDs = [1,1,1,2,3,3,4,5,5]
        %       occurIdx = [1,4,5,7,8];

            eps1 = eps(class(states)); % Compute epsilon based on class ('double' or 'single')
            states = floor(states/eps1)*eps1;
            [~, occurIdx, stateIDs] = unique(states,"rows", "stable");
            stateIDs = stateIDs'; %Needs to be a row vector while
                                  %dealing with Links table that has single
                                  %row
        end
    end
end
