classdef (Sealed) controllerTEB < nav.algs.internal.controllerTEBImpl ...
        & matlab.mixin.internal.indexing.Paren
%CONTROLLERTEB Avoid unseen obstacles with time optimal trajectories
%   The controllerTEB handle object creates a controller (local planner)
%   using the Timed Elastic Band (TEB) algorithm. The controller
%   enables a robot to follow a reference path typically generated by
%   a global planner, such as the RRT or Hybrid A*. Additionally, the
%   planner avoids obstacles and smooths out the path while optimizing the
%   travel time. Furthermore, the planner maintains a safe distance from
%   obstacles, known or unknown to the global planner. The object computes
%   velocity commands and optimal trajectory using the current pose and
%   current linear and angular velocity of the robot. It can be used for
%   most mobile robots and ground vehicles using standard motion models,
%   e.g. differential-drive and car-like robots.
%
%   TEB = CONTROLLERTEB(REFPATH) returns a TEB controller object that
%   computes the linear and angular velocity commands for a robot to follow
%   the reference path, REFPATH, and travel for 5 seconds in an obstacle
%   free environment. The REFPATH input sets the value of the ReferencePath
%   property.
%   REFPATH can be one of the following:
%       N-by-2 matrix where each row represents a pose in the path.
%       N-by-3 matrix where each row represents a pose in the path.
%       navPath object with SE(2) state space.
%   NOTE: If REFPATH is specified as an N-by-2 matrix, then the orientation
%   is computed using the headingFromXY function and appended as the third
%   column.
%
%   TEB = CONTROLLERTEB(REFPATH,MAP) attempts to avoid the obstacles in MAP
%   specified by a binaryOccupancyMap or occupancyMap object. The space
%   outside the map boundary is assumed to be free. The MAP input sets the
%   value of the Map property.
%
%   TEB = CONTROLLERTEB(...,Name=Value) specifies properties using one or
%   more name-value arguments in addition to the argument from the previous
%   syntax.
%
%   CONTROLLERTEB properties:
%
%   CostWeights           - Weights for cost function optimization
%   RobotInformation      - Robot geometry information for collision
%                           checking
%   ObstacleSafetyMargin  - Safe distance between robot and obstacles
%   NumIteration          - Number of iterations to optimize trajectory
%   MaxVelocity           - Maximum linear and angular velocity for
%                           velocity commands
%   MaxReverseVelocity    - Maximum velocity during reverse motion for
%                           velocity commands
%   MaxAcceleration       - Maximum linear and angular acceleration for
%                           velocity commands
%   MinTurningRadius      - Minimum turning radius for optimized path
%   ReferenceDeltaTime    - Reference travel time between two consecutive
%                           poses
%   LookAheadTime         - Look-ahead time
%   GoalTolerance         - Tolerance value to determine if goal is reached
%   ReferencePath         - Path to follow for controller
%   Map                   - Occupancy map representing environment
%
%   CONTROLLERTEB methods:
%
%   step                  - Compute velocity commands and optimal
%                           trajectory for subsequent time steps.
%   clone                 - Create local planner object with same property
%                           values
%
%   Example:
%
%       load("parkingMap.mat");
%       map = occupancyMap(map, 3);
%
%       % Setup and run global planner
%       validator = validatorOccupancyMap(stateSpaceSE2, Map=map);
%       validator.ValidationDistance = .1;
%       rng(42, 'twister')
%       rrtstar = plannerRRTStar(validator.StateSpace, validator);
%       rrtstar.MaxConnectionDistance = .2;
%       route = plan(rrtstar, [2 9 0], [27 18 -pi/2]);
%       refpath = route.States;
%       % RRT* gives random orientation which can cause unnecessary turns
%       headingToNextPose = headingFromXY(refpath(:,1:2));
%       % Align orientation to path except for start and goal
%       refpath(2:end-1, 3) = headingToNextPose(2:end-1);
%
%       localmap = occupancyMap(15, 15, map.Resolution);
%       teb = controllerTEB(refpath, localmap);
%       teb.LookAheadTime = 10; % In sec
%       teb.ObstacleSafetyMargin = .4; % In meters
%       % To generate more time-optimal trajectories, specify a larger
%       % value for CostWeights.Time. To follow the reference path closely
%       % keep the weight to a smaller value like 1e-3.
%       teb.CostWeights.Time = 100;
%
%       curpose = refpath(1,:);
%       curvel = [0 0];
%       simtime = 0;
%       % Reducing timestep can lead to more accurate path tracking
%       timestep = 0.1;
%       itr = 0;
%       goalReached = false;
%
%       while ~goalReached && simtime < 60
%           % Update map to keep robot in center of the map. Also update
%           % the map with new information from global map or measurement
%           % from sensors.
%           moveMapBy = curpose(1:2) - localmap.XLocalLimits(end)/2;
%           localmap.move(moveMapBy, FillValue=0.5);
%           syncWith(localmap, map);
%
%           if mod(itr,10)==0 % every 1 sec
%               % Generate new vel commands with teb
%               [velcmds, tstamps, curpath, info] = teb(curpose, curvel);
%               goalReached = info.HasReachedGoal;
%               feasibleDriveDuration = tstamps(info.LastFeasibleIdx);
%               % If robot is far from goal and only less than third of
%               % trajectory is feasible, then an option is to re-plan
%               % the path to follow to reach the goal.
%               if info.ExitFlag == 1 && ...
%                   feasibleDriveDuration < (teb.LookAheadTime/3)
%                   route = plan(rrtstar, curpose, [27 18 -pi/2]);
%                   refpath = route.States;
%                   headingToNextPose = headingFromXY(refpath(:,1:2));
%                   refpath(2:end-1,3) = headingToNextPose(2:end-1);
%                   teb.ReferencePath = refpath;
%               end
%               timestamps = tstamps + simtime;
%
%               % Show the updated information which is input to or output
%               % from controllerTEB
%               clf
%               show(localmap)
%               hold on
%               plot(refpath(:,1), refpath(:,2), ".-", Color="#EDB120", ...
%                    DisplayName="Reference Path")
%               quiver(curpath(:,1), curpath(:,2), ...
%                      cos(curpath(:,3)), sin(curpath(:,3)), .2, ...
%                      Color="#A2142F", DisplayName="Current Path")
%               quiver(curpose(:,1), curpose(:,2), ...
%                      cos(curpose(:,3)), sin(curpose(:,3)), .5, ...
%                      "o", MarkerSize=20, ShowArrowHead="off", ...
%                      Color="#0072BD", DisplayName="Start Pose")
%           end
%
%           simtime = simtime + timestep;
%           % Compute the instantaneous velocity to be sent to the robot
%           % from the series of timestamped commands generated by
%           % controllerTEB
%           velcmd = velocityCommand(velcmds, timestamps, simtime);
%           % Very basic robot model, should be replaced by simulator.
%           statedot = [velcmd(1)*cos(curpose(3)) ...
%                       velcmd(1)*sin(curpose(3)) ...
%                       velcmd(2)];
%           curpose = curpose + statedot * timestep;
%           curvel = velcmd;
%
%           if exist("hndl", "var")
%               delete(hndl)
%           end
%           hndl = quiver(curpose(:,1), curpose(:,2), ...
%                  cos(curpose(:,3)), sin(curpose(:,3)), .5, ...
%                  "o", MarkerSize=20, ShowArrowHead="off", ...
%                  Color="#D95319", DisplayName="Current Robot Pose");
%           itr = itr + 1;
%           drawnow
%       end
%       legend
%
%   See also VELOCITYCOMMAND, HEADINGFROMXY, PLANNERASTARGRID,
%   BINARYOCCUPANCYMAP, OCCUPANCYMAP, CONTROLLERPUREPURSUIT

%   Copyright 2022-2024 The MathWorks, Inc.

%#codegen


    properties
        %ROBOTINFORMATION Robot geometry information for collision checking
        %   Robot geometry information for collision checking, specified as
        %   a structure. The fields of the structure are:
        %
        %       Dimension - Size of the robot, specified as a two-element
        %                   positive vector of the form [length width] in
        %                   meters.
        %       Shape     - Shape of the robot, specified as "Rectangle" or
        %                   "Point".
        %
        %   NOTE:
        %       - If Shape is specified as "Point", the value specified
        %         for Dimension property is ignored and is set to [0 0].
        %       - If Shape is specified as "Rectangle", the robot's origin
        %         is considered to be the center of its rear edge.
        %
        % Default: struct("Dimension", [1 .67], "Shape", "Rectangle");
        RobotInformation

        %MAP Occupancy map representing environment
        %   Occupancy map representing the environment, specified as
        %   a binaryOccupancyMap object or occupancyMap object containing
        %   the obstacles in the vicinity of the robot. Space outside the
        %   boundary of the map is considered as free when optimizing
        %   trajectory. Larger maps can lead to degradation in performance.
        %
        % Default: binaryOccupancyMap()
        Map
    end

    properties (Dependent)

        %COSTWEIGHTS Weights for cost function optimization
        %   Weights for cost function optimization, specified as
        %   a structure. The fields of the structure are:
        %       Time       - Cost function weight for time, specified as
        %                    a positive scalar. To lower the travel time,
        %                    increase this weight value.
        %       Smoothness - Cost function weight for smooth motion,
        %                    specified as a positive scalar. To obtain
        %                    smoother path, increase this weight value.
        %       Obstacles  - Cost function weight for maintaining safe
        %                    distance from obstacles, specified as
        %                    a positive scalar. To maintain the safe
        %                    distance from obstacles, increase this weight
        %                    value.
        %
        % Default: struct("Time", 10, "Smoothness", 1000, "Obstacle", 50);
        CostWeights

        %OBSTACLESAFETYMARGIN Safe distance between robot and obstacles
        %   Safe distance between the robot and the obstacles, specified as
        %   a positive scalar in meters. Note that this is a soft
        %   constraint and may be violated.
        %
        % Default: 0.5 % In meters
        ObstacleSafetyMargin

        %NUMITERATION Number of iteration to optimize trajectory
        %   Number of times interpolation occurs and solver is called for
        %   trajectory optimization, specified as a positive scalar
        %   integer.
        %
        % Default: 2
        NumIteration

        %MAXVELOCITY Maximum limit of linear and angular velocity
        %   Maximum limit of linear and angular velocity for velocity
        %   commands, specified as positive two-element vector. First
        %   element corresponds to linear velocity limit and second element
        %   corresponds to angular velocity limit.
        %
        % Default: [0.8 1.6] % In [m/s rad/s]
        MaxVelocity

        %MAXACCELERATION Maximum limit of linear and angular acceleration
        %   Maximum limit of linear and angular acceleration for velocity
        %   commands, specified as positive two-element vector. First
        %   element corresponds to linear acceleration limit and second
        %   element corresponds to angular acceleration limit.
        %
        % Default: [2.4 4.8] % In [m/s^2 rad/s^2]
        MaxAcceleration

        %REFERENCEDELTATIME Reference travel time between consecutive poses
        %   Reference travel time between two consecutive poses, specified
        %   as a positive scalar. This property impacts the addition and
        %   deletion of poses for the optimized trajectory. Increase the
        %   value of this property to have fewer poses and reduce it to
        %   have more poses in the output path.
        %
        % Default: 0.3 % In seconds
        ReferenceDeltaTime

        %MINTURNINGRADIUS Minimum Turning Radius of turns in optimized path
        %   Minimum Turning radius of the vehicle the optimized path should
        %   adhere to, specified as a positive scalar. Increasing its value
        %   can limit sharp turns and the output trajectory might have more
        %   forward and reverse motion to get out of restricted space.
        %   Reducing the value will allow more sharp turns and in-place
        %   rotations, forward motion would also be preferred for optimized
        %   path.
        % Default: 0 % In meters
        MinTurningRadius

        %MAXREVERSEVELOCITY Maximum velocity for reverse motion
        %   Maximum Velocity of the vehicle while moving backwards or in
        %   reverse, specified as a positive scalar. If this value is NaN,
        %   first element of MaxVelocity property would be used to limit
        %   reversal velocity.
        %
        % Default: NaN % In m/s
        MaxReverseVelocity

        %LOOKAHEADTIME Look-ahead time
        %   Look-ahead time, specified as a positive scalar in seconds. The
        %   controller generates velocity commands and optimizes trajectory
        %   until the look-ahead time is reached. A higher look-ahead time
        %   generates velocity commands further into the future. This
        %   allows for reacting earlier to unseen obstacles, but it also
        %   increases execution time. A smaller look-ahead time reduces the
        %   reaction time to new unknown obstacles, but would allow the
        %   controller to run at a faster rate.
        %
        % Default: 5 % In seconds
        %
        % NOTE: This property impacts the number of velocity commands,
        % timestamps, and poses in path.
        LookAheadTime

        %REFERENCEPATH Path to follow for controller
        %   The path to follow for the controller, specified as an N-by-2
        %   or N-by-3 matrix, or navPath object with SE(2) state space.
        %   When specified as a matrix each row represents a pose in the
        %   path. Use the LookAheadTime property to select a part of the
        %   ReferencePath to optimize the trajectory and generate velocity
        %   commands.
        ReferencePath

        %GOALTOLERANCE Tolerance values to consider robot has reached goal
        %   Tolerance to consider the robot as reaching the goal, specified
        %   as a three-element positive row vector. Tolerance is between
        %   robot pose and goal pose for [x,y,heading]. All three tolerance
        %   values should have been met to consider robot has reached goal.
        %
        % Default: [0.1 0.1 0.1] % In [meters, meters, radian]
        GoalTolerance
    end

    properties(Access=private)

        % For operations on path, e.g. interpolation to find first collision
        % violation
        StateSpace

        % Radius of circle circumscribing cell in Map.
        CellHalfDiagLen
    end

    methods

        function obj = controllerTEB(refpath, varargin)
        %CONTROLLERTEB Construct object of Local Planner using Timed Elastic Band(TEB)

        % min 1 as refpath is mandatory and max is 18 (refpath+map+8 NV-pair)
            narginchk(1,24);

            % Configure default parameter values for TEB
            params = controllerTEB.defaultParams();
            % This call needs to be to static method as obj cannot be mentioned before initializing
            % superclass
            refpath = controllerTEB.parseValidateReferencePath(refpath);
            % Need to call it here before accessing obj
            obj@nav.algs.internal.controllerTEBImpl(refpath, params);

            % Only validate optional (map) input and/or name value pairs if they have been
            % specified by the user
            args = controllerTEB.parseValidateConstructorVaragin(varargin{:});

            obj.StateSpace = stateSpaceSE2;
            obj.StateSpace.StateBounds = [-inf inf; -inf inf; -pi pi];
            obj.StateSpace.SkipStateValidation = true;

            % The order matters here.
            % In normal flow while setting map we need to query SafetyMargin,
            % which requires CellHalfDiagLen, which requires Map. In
            % general there is data existing which allows the workflow to
            % execute successfully, but causes issues when no
            % initialization is done. If we do it in this order
            % CellHalfDiagLen is set with data from user input or default
            % data, which then correctly sets safety margin, which then
            % sets the Map.
            obj.CellHalfDiagLen = 1/(sqrt(2)*args.Map.Resolution);
            obj.ObstacleSafetyMargin = args.ObstacleSafetyMargin;
            obj.Map = args.Map;

            obj.CostWeights = args.CostWeights;
            obj.RobotInformation = args.RobotInformation;
            obj.NumIteration = args.NumIteration;
            obj.MaxVelocity = args.MaxVelocity;
            obj.MaxAcceleration = args.MaxAcceleration;
            obj.ReferenceDeltaTime = args.ReferenceDeltaTime;
            obj.MinTurningRadius = args.MinTurningRadius;
            obj.LookAheadTime = args.LookAheadTime;

            obj.MaxReverseVelocity = args.MaxReverseVelocity;
            obj.GoalTolerance = args.GoalTolerance;
        end

    end

    methods

        function set.Map(obj, map)
            validateattributes(map, {'binaryOccupancyMap', 'occupancyMap'}, ...
                               {'nonempty', 'scalar'}, mfilename, 'Map')
            obj.Map = map;
            % Save the user visible margin. Margin internally is dependent
            % on the cell size. Since, map is changing it's resolution
            % might be different, hence we need to ensure that following
            % stays true:
            % internal margin = user margin + Half of cell diagonal
            margin = obj.TEBParams.ObstacleSafetyMargin - obj.CellHalfDiagLen; %#ok<MCSUP>
            obj.CellHalfDiagLen = 1/(sqrt(2)*map.Resolution); %#ok<MCSUP>

            % Assigning user margin back adds the Half of cell diagonal to
            % user margin in it's setter. Setter updates the Safety margin,
            % CutOff distance, and Inclusion distance correctly.
            obj.ObstacleSafetyMargin = margin; %#ok<MCSUP>
        end

        function weights = get.CostWeights(obj)
            weights = struct( ...
                "Time", obj.TEBParams.WeightTime, ...
                "Smoothness", obj.TEBParams.WeightSmoothness, ...
                "Obstacle", obj.TEBParams.WeightObstacles);
        end

        function set.CostWeights(obj, weights)

            validateattributes(weights, 'struct', {'nonempty', 'scalar'}, mfilename, 'CostWeights');

            % Only validate struct fields in MATLAB, as Coder will only
            % generate code if the types are correct
            if coder.target("MATLAB")

                refFields = ["Time", "Smoothness", "Obstacle"];
                missingFieldIdx = ~isfield(weights, refFields);
                if any(missingFieldIdx)
                    coder.internal.error("nav:navalgs:controllerteb:MissingStructField", ...
                                         "CostWeights", strjoin(refFields(missingFieldIdx)));
                end

            end

            validateattributes(weights.Time, 'numeric', ...
                               {'nonempty', 'scalar', 'nonnan', 'finite', 'real', 'positive'}, ...
                               mfilename, 'CostWeights.Time');

            validateattributes(weights.Smoothness, 'numeric', ...
                               {'nonempty', 'scalar', 'nonnan', 'finite', 'real', 'positive'}, ...
                               mfilename, 'CostWeights.Smoothness');

            validateattributes(weights.Obstacle, 'numeric', ...
                               {'nonempty', 'scalar', 'nonnan', 'finite', 'real', 'positive'}, ...
                               mfilename, 'CostWeights.Obstacles');

            obj.TEBParams.WeightTime = weights.Time;
            obj.TEBParams.WeightSmoothness = weights.Smoothness;
            obj.TEBParams.WeightObstacles = weights.Obstacle;
        end

        function set.RobotInformation(obj, robotInfo)

            validateattributes(robotInfo, 'struct', {'nonempty', 'scalar'}, ...
                               mfilename, 'RobotInformation');

            % Only validate struct fields in MATLAB, as Coder will only
            % generate code if the types are correct
            if coder.target("MATLAB")
                refFields = ["Dimension", "Shape"];
                missingFieldIdx = ~isfield(robotInfo, refFields);
                if any(missingFieldIdx)
                    coder.internal.error("nav:navalgs:controllerteb:MissingStructField", ...
                                         "RobotInformation", strjoin(refFields(missingFieldIdx)));
                end
            end

            robshape = validatestring(robotInfo.Shape, {'Rectangle', 'Point'}, ...
                                      mfilename, 'RobotInformation.Shape');

            switch robshape
              case "Point"
                validateattributes(robotInfo.Dimension, 'numeric', ...
                                   {'nonempty', 'row', 'numel', 2, 'nonnan', 'finite', 'real'}, ...
                                   mfilename, 'RobotInformation.Dimension');

                obj.TEBParams.RobotDimension = [0 0];
              case "Rectangle"
                validateattributes(robotInfo.Dimension, 'numeric', ...
                                   {'nonempty', 'row', 'numel', 2, 'nonnan', 'finite', 'real', 'positive'}, ...
                                   mfilename, 'RobotInformation.Dimension');

                obj.TEBParams.RobotDimension = robotInfo.Dimension;
            end

            obj.RobotInformation = robotInfo;

        end

        function lat = get.LookAheadTime(obj)
            lat = obj.LookAheadDistance / obj.TEBParams.MaxVelocity(1);
        end

        function set.LookAheadTime(obj, lookAheadTime)
            validateattributes(lookAheadTime,...
                               'numeric',{'nonempty', 'scalar', 'nonnan', 'finite',  ...
                                          'real', 'positive'} , ...
                               mfilename, 'LookAheadTime');
            % Validate that Look-ahead-time is atleast 3 times ReferenceDeltaTime.
            validateLookAheadTime(obj, lookAheadTime);

            obj.LookAheadDistance = lookAheadTime * obj.MaxVelocity(1);
            setMaxPathStates(obj);
        end

        function validateLookAheadTime(obj, lookAheadTime)
        % validateLookAheadTime validate that the look ahead time is
        % atleast 3 times the ReferenceDeltaTime. This ensures the TEB
        % algorithm has atleast three poses for optimization.

            referenceDeltaT = obj.ReferenceDeltaTime;
            isLATInvalid = lookAheadTime < 3*referenceDeltaT;
            expectedLookAheadTime = 3*referenceDeltaT;

            errorString = sprintf("%.1f", expectedLookAheadTime);
            coder.internal.errorIf(isLATInvalid,"nav:navalgs:controllerteb:InValidLookAheadTime", ...
                                   errorString);
        end

        function refDelT = get.ReferenceDeltaTime(obj)
            refDelT = obj.TEBParams.ReferenceDeltaTime;
        end

        function set.ReferenceDeltaTime(obj, refDelT)
            validateattributes(refDelT,...
                               'numeric',{'nonempty', 'scalar', 'nonnan', 'finite',  ...
                                          'real', 'positive', 'nonsparse'} , ...
                               mfilename, 'ReferenceDeltaTime');
            obj.TEBParams.ReferenceDeltaTime = refDelT;
            setMaxPathStates(obj);
        end

        function set.MaxVelocity(obj, maxVel)
            validateattributes(maxVel,...
                               'numeric', {'nonempty', 'row', 'nonnan', 'finite',  ...
                                           'real', 'positive', 'nonsparse', 'numel', 2} , ...
                               mfilename, 'MaxVelocity');
            beforeMaxVel = obj.TEBParams.MaxVelocity;
            obj.TEBParams.MaxVelocity = maxVel(1);
            obj.TEBParams.MaxAngularVelocity = maxVel(2);
            obj.LookAheadDistance = (obj.LookAheadDistance/beforeMaxVel)*obj.TEBParams.MaxVelocity;
            setMaxPathStates(obj);
        end

        function maxVel = get.MaxVelocity(obj)
            maxVel = [obj.TEBParams.MaxVelocity ...
                      obj.TEBParams.MaxAngularVelocity];
        end

        function set.MaxReverseVelocity(obj, maxReverseVel)
            validateattributes(maxReverseVel,...
                               'numeric', {'nonempty', 'scalar',  ...
                                           'real', 'positive', 'nonsparse'} , ...
                               mfilename, 'MaxReverseVelocity');
            if isinf(maxReverseVel)
                validateattributes(maxReverseVel, 'numeric', {'finite'}, ...
                                   mfilename, 'MaxReverseVelocity'); % throw standardized error
            end
            obj.TEBParams.MaxReverseVelocity = maxReverseVel;
        end

        function maxReverseVel = get.MaxReverseVelocity(obj)
            maxReverseVel = obj.TEBParams.MaxReverseVelocity;
        end

        function set.MinTurningRadius(obj, minTurnRadius)
            validateattributes(minTurnRadius,...
                               'numeric', {'nonempty', 'scalar', 'nonnan', 'finite',  ...
                                           'real', 'nonnegative', 'nonsparse'} , ...
                               mfilename, 'MinTurningRadius');
            robType = ~(minTurnRadius > 0);
            obj.TEBParams.MinTurningRadius = minTurnRadius;
            obj.TEBParams.RobotType = double(robType);
        end

        function minTurnRadius = get.MinTurningRadius(obj)
            minTurnRadius = obj.TEBParams.MinTurningRadius;
        end

        function set.MaxAcceleration(obj, maxAccel)
            validateattributes(maxAccel,...
                               'numeric',{'nonempty', 'row', 'numel', 2, 'nonnan', 'finite',  ...
                                          'real', 'positive', 'nonsparse'} , ...
                               mfilename, 'MaxAcceleration');
            obj.TEBParams.MaxAcceleration = maxAccel(1);
            obj.TEBParams.MaxAngularAcceleration = maxAccel(2);
        end

        function maxAccel = get.MaxAcceleration(obj)
            maxAccel = [obj.TEBParams.MaxAcceleration ...
                        obj.TEBParams.MaxAngularAcceleration];
        end

        function set.NumIteration(obj, numIter)
            validateattributes(numIter,...
                               'numeric',{'nonempty', 'scalar', 'nonnan', 'finite',  ...
                                          'real', 'positive', 'integer', 'nonsparse'} , ...
                               mfilename, 'NumIteration');
            obj.TEBParams.NumIteration = numIter;
        end

        function numIter = get.NumIteration(obj)
            numIter = obj.TEBParams.NumIteration;
        end

        function set.ObstacleSafetyMargin(obj, margin)
            validateattributes(margin,...
                               'numeric',{'nonempty', 'scalar', 'nonnan', 'finite',  ...
                                          'real', 'positive', 'nonsparse'} , ...
                               mfilename, 'ObstacleSafetyMargin');
            margin = margin + obj.CellHalfDiagLen;
            obj.TEBParams.ObstacleSafetyMargin = margin;
            % Inline with default values
            obj.TEBParams.ObstacleCutOffDistance = 5*margin;
            % 1.5 factor is inline with default values, but also have minimum
            % obstacle inclusion distance as 0.2
            obj.TEBParams.ObstacleInclusionDistance = max(1.5*margin, 0.2);
        end

        function margin = get.ObstacleSafetyMargin(obj)
            margin = obj.TEBParams.ObstacleSafetyMargin - obj.CellHalfDiagLen;
        end

        function refPath = get.ReferencePath(obj)
            refPath = obj.ReferencePathInternal;
        end

        function set.ReferencePath(obj, refPath)
            obj.ReferencePathInternal = controllerTEB.parseValidateReferencePath(refPath);
            % If the user reset the reference path, the index in reference
            % path which is closest to the robot should also be reset to 1.
            obj.IdxCloseToRobot = 1;
        end

        function set.GoalTolerance(obj, goalTol)
            validateattributes(goalTol,...
                               'numeric', {'nonempty', 'row', 'numel', 3, 'nonnan', 'finite',  ...
                                           'real', 'positive', 'nonsparse'} , ...
                               mfilename, 'GoalTolerance');
            obj.GoalToleranceInternal = goalTol;
        end

        function goalTol = get.GoalTolerance(obj)
            goalTol = obj.GoalToleranceInternal;
        end

    end

    methods (Hidden)
        function [velcmds, tstamps, optpath, extraInfo] = parenReference(obj, startState, startvel)
        %PARENREFERENCE Alternate interface to step to enable variable(...) syntax
            [velcmds, tstamps, optpath, extraInfo] = step(obj, startState, startvel);
        end
    end

    methods(Access=public)

        function [velcmds, tstamps, optpath, extraInfo] = step(obj, curstate, curvel)
        %STEP velocity commands and optimal trajectory for subsequent time steps.
        %   [VELCMDS, TSTAMPS, OPTPATH] = step(TEB, CURSTATE, CURVEL)
        %   computes the linear and angular velocity commands, VELCMDS,
        %   with it's corresponding timestamp in TSTAMPS, and corresponding
        %   Optimized (smoother and safer) path OPTPATH for a robot's
        %   current pose CURSTATE and current velocity CURVEL.
        %   CURSTATE is 1-by-3 vector containing current X, Y, and theta.
        %   CURVEL is a 1-by-2 vector containing robot's current linear and
        %   angular velocity.
        %   VELCMDS is N-by-2 matrix, first column is linear velocity and
        %   second column is angular velocity. TSTAMPS is N-by-1 vector.
        %   OPTPATH is a N-by-3 matrix where the three column are x, y,
        %   theta. N is impacted by the property ReferenceDeltaTime and
        %   LookAheadTime, the algorithm tries to ensure that the
        %   difference between two consecutive TSTAMPS is around
        %   ReferenceDeltaTime, poses and timestamp are added if the gap is
        %   greater than ReferenceDeltaTime and are removed if the gap is
        %   lesser than it. Algorithm tries to keep last value of TSTAMPS
        %   around LookAheadTime, hence increasing it will increase N as
        %   well.
        %
        %   [..., EXTRAINFO] = step(TEB, CURSTATE, CURVEL) also returns
        %   a structure, EXTRAINFO, that contains extra information to
        %   evaluate the solution.
        %
        %   EXTRAINFO has following fields:
        %
        %         LastFeasibleIdx - Last index which is violation free.
        %                           Moving the robot beyond this will cause
        %                           it to violate the constraint indicated
        %                           by ExitFlag.
        %   DistanceFromStartPose - Distance of each pose in OPTPATH from
        %                           the first pose in OPTPATH.
        %          HasReachedGoal - Indicates whether the robot has
        %                           successfully reached the last pose in
        %                           the ReferencePath within a tolerance.
        %                           It returns as true if successful.
        %                           Otherwise, it returns false.
        %          TrajectoryCost - Cost of optimized trajectory for cost
        %                           functions in Timed Elastic Band
        %                           algorithm.
        %                ExitFlag - Information on output from step
        %                           function call
        %                           0 - Commands and Trajectory output of
        %                               step method are feasible
        %                           1 - Trajectory output of step method is
        %                               collision free till LastFeasibleIdx
        %                               then jumps over an obstacle.
        %                           2 - Pose at LastFeasibleIdx for
        %                               Trajectory output of step method
        %                               violates ObstacleSafetyMargin
        %                               constraint by more than 10%.
        %                           3 - Pose at LastFeasibleIdx for
        %                               Trajectory output of step method
        %                               violates MinTurningRadius
        %                               constraint by more than 10%.
        %                           4 - One or more Timestamp given as
        %                               output from step method was older
        %                               than previous one.
        %                           5 - Reference Path provided to
        %                               controllerTEB is farther than what
        %                               robot can cover in LookAheadTime
        %                               while travelling at Maximum Linear
        %                               Velocity from its current position

            arguments
                obj            {mustBeNonempty, mustBeA(obj, "controllerTEB")}
                curstate (1,3) ...
                    {mustBeNumeric, mustBeNonempty, mustBeNonNan, mustBeFinite, mustBeReal}
                curvel   (1,2) ...
                    {mustBeNumeric, mustBeNonempty, mustBeNonNan, mustBeFinite, mustBeReal}
            end

            % Extract obstacle list to get the current list of obstacles as some updates might have
            % had happened
            obsts = extractObstacleList(obj.Map);

            % Check if current robot pose is occupied and if last path on
            % the ReferencePath is occupied.
            laststate = obj.ReferencePath(end,:);
            startGoalColliding = areCapsulesCollidingWithOMCells( ...
                [curstate' laststate'], obj.RobotInformation.Dimension, ...
                obsts', obj.CellHalfDiagLen);
            if any(startGoalColliding(1,:))
                curStateStr = ['[' ...
                               sprintf('%.3f %.3f %.3f', curstate(1), curstate(2), curstate(3)) ...
                               ']'];
                coder.internal.error("nav:navalgs:controllerteb:InvalidCurrentPose", curStateStr);
            end

            if any(startGoalColliding(2,:))
                lastStateStr = ['[' ...
                                sprintf('%.3f %.3f %.3f', laststate(1), laststate(2), laststate(3)) ...
                                ']'];
                coder.internal.error("nav:navalgs:controllerteb:InvalidLastPose", lastStateStr);
            end

            obj.TEBParams.StartVelocity = curvel(1);
            obj.TEBParams.StartAngularVelocity = curvel(2);

            % Do the deed
            [velcmds, optpath, tstamps, ei] = stepImpl(obj, curstate, obsts);

            % Calculate/extract information for extra information output

            % Find the distance of all the poses from first pose along the optimized path
            % diff(X,1,1) ensures difference is always computed between consecutive poses.
            distFromStart = [0; cumsum(vecnorm(diff(optpath(:,1:2),1,1), 2, 2))];

            [lastFeasibleIdx, exitFlag] = findFirstViolation(obj, optpath, velcmds, diff(tstamps), ...
                                                             obsts, ei);

            if ei.SolutionValid && lastFeasibleIdx >= height(tstamps)
                exitFlag = 0;
                lastFeasibleIdx = height(tstamps);
                % Rest of the outputs stay the same
            end

            extraInfo = struct(...
                "LastFeasibleIdx", lastFeasibleIdx, ...
                "DistanceFromStartPose", distFromStart, ...
                "HasReachedGoal", ei.HasReachedGoal, ...
                "TrajectoryCost", ei.TrajectoryCost, ...
                "ExitFlag", exitFlag);

        end

        function objclone = clone(obj)
        %CLONE Create a deep clone of the controllerTEB object
        %   OBJCLONE = clone(OBJ) creates a deep clone of controllerTEB
        %   object, OBJ, and returns the new object in OBJCLONE.

            objclone = controllerTEB(obj.ReferencePath, obj.Map.copy());

            propnames = properties(obj);
            for itr = 1:numel(propnames)
                propname = propnames{itr};
                if propname == "Map" || propname == "ReferencePath"
                    continue;
                end
                objclone.(propname) = obj.(propname);
            end

        end

    end

    methods(Hidden, Static, Access = ?nav.algs.internal.InternalAccess)

        function params = defaultParams()
        %DEFAULTPARAMS Creates the structure and sets the values to default for controllerTEB

        % TEB.
            params = nav.algs.internal.createTEBDefaultParams();

            % We will now change the values to what controllerTEB considers default.
            params.MaxPathStates = 200;
            params.ReferenceDeltaTime = .3;

            params.MaxVelocity = 0.8;
            params.MaxReverseVelocity = NaN;
            params.MaxAngularVelocity = 1.6;
            params.MaxAcceleration = 2.4;
            params.MaxAngularAcceleration = 4.8;
            params.MinTurningRadius = 0;

            params.RobotDimension = [1 .67];
            params.RobotType = 1;

            % Parameters for solvers
            params.NumIteration = 2;
            params.MaxSolverIteration = 10;

            % Parameters for weights for cost function
            params.WeightTime = 10;
            params.WeightSmoothness = 1000;
            params.WeightForwardDrive = 10;
            params.WeightVelocity = 10;
            params.WeightAngularVelocity = 10;
            params.WeightAcceleration = 10;
            params.WeightAngularAcceleration = 10;
            params.WeightObstacles = 50;
            params.WeightMinTurningRadius = 100;

            % Parameters for obstacle
            params.ObstacleSafetyMargin = .5;
            params.ObstacleCutOffDistance = 5 * params.ObstacleSafetyMargin;
            params.ObstacleInclusionDistance = 1.5 * params.ObstacleSafetyMargin;

            % With this it will behave like optimizePath
            params.StartVelocity = 0;
            params.StartAngularVelocity = 0;
            params.EndVelocity = 0;
            params.EndAngularVelocity = 0;

        end

        function [names, vals] = propertyNameDefaultValues()
        %PROPERTYNAMEDEFAULTVALUES returns property names and their default values for controllerTEB

        % Property Names
            names = {'CostWeights', 'RobotInformation', 'ObstacleSafetyMargin', 'NumIteration', ...
                     'MaxVelocity', 'MaxReverseVelocity', 'MaxAcceleration', 'MinTurningRadius', 'ReferenceDeltaTime', ...
                     'LookAheadTime', 'GoalTolerance'};

            % Define default values
            robshape = "Rectangle";

            % access defaults
            params = controllerTEB.defaultParams();
            timew = params.WeightTime;
            smoothnessw = params.WeightSmoothness;
            obstaclesw = params.WeightObstacles;
            robdim = params.RobotDimension;
            defaultLooAheadTime = 5;
            defaultGoalTolerance = 0.1 * ones(1,3);

            % arrange in the same order as names
            vals = {...
                struct("Time", timew, "Smoothness", smoothnessw, "Obstacle", obstaclesw), ...
                struct("Dimension", robdim, "Shape", robshape), ...
                params.ObstacleSafetyMargin, params.NumIteration, ...
                [params.MaxVelocity params.MaxAngularVelocity], ...
                params.MaxReverseVelocity, ...
                [params.MaxAcceleration params.MaxAngularAcceleration], ...
                params.MinTurningRadius, params.ReferenceDeltaTime, defaultLooAheadTime, ...
                defaultGoalTolerance ...
                   };

        end

        function rp = parseValidateReferencePath(refpath)
        %PARSEVALIDATEREFERENCEPATH Extract and validate values specified for ReferencePath

            validateattributes(refpath, {'numeric', 'navPath'}, {}, mfilename, 'Reference Path');

            if isa(refpath, "navPath")
                coder.internal.errorIf(class(refpath.StateSpace) ~= "stateSpaceSE2", ...
                                       "nav:navalgs:controllerteb:InvalidPathInput");

                coder.internal.errorIf(refpath.NumStates < 3, ...
                                       'nav:navalgs:controllerteb:MinPathPoints');

                rp = refpath.States;
            else
                % for now keep a numeric path as catch all, if the reference path is not a special
                % class it should be a numeric matrix
                validateattributes(refpath, 'numeric', ...
                                   {'2d', 'nonempty', 'nonnan', 'finite', 'real'}, mfilename, 'ReferencePath')

                coder.internal.errorIf(width(refpath)>3||width(refpath)<2,...
                                       'nav:navalgs:controllerteb:InvalidPathInput');

                coder.internal.errorIf(height(refpath)<3, ...
                                       'nav:navalgs:controllerteb:MinPathPoints');

                if width(refpath) == 2
                    heading = headingFromXY(refpath);
                    rp = [refpath heading];
                else
                    rp = refpath;
                end
            end

            delRefPthNorm = diff(rp(:,1:2));
            delRefPthNorm = delRefPthNorm./vecnorm(delRefPthNorm, 2, 2);
            oriRefPthVec = [cos(rp(1:end-1,3)) sin(rp(1:end-1,3))];
            dotDkQk = dot(delRefPthNorm, oriRefPthVec);
            dotDkQkThreshold = 1e-3;
            coder.internal.errorIf(all(abs(dotDkQk) < dotDkQkThreshold, "all"), ...
                                   'nav:navalgs:controllerteb:InputPathOrthogonality');

        end

        function out = parseValidateConstructorVaragin(varargin)
        %PARSEVALIDATECONSTRUCTORVARAGIN Extract and validate optional inputs for controllerTEB
        %   Parse the optional inputs for controllerTEB constructor, that includes optional input
        %   Map and name-value pairs

            nvStartIdx = 1;

            if ~isempty(varargin)

                isFirstElemMap = isa(varargin{1}, "binaryOccupancyMap") || ...
                    isa(varargin{1}, "occupancyMap");
                if isFirstElemMap
                    % Parse map
                    nvStartIdx = 2;
                    out.Map = varargin{1};
                else % user has specified a second arg and it's not a map
                     % Check if it's a string
                    isFirstElemStr = ischar(varargin{nvStartIdx}) ...
                        || isStringScalar(varargin{nvStartIdx});
                    % Error if it's not a string/Name-Value (and obviously not a map)
                    coder.internal.errorIf(~isFirstElemStr, ...
                                           "nav:navalgs:controllerteb:InvalidArgument", ...
                                           "Second");
                    out.Map = binaryOccupancyMap();
                end
            else
                out.Map = binaryOccupancyMap();
            end

            % Parse Name-Value pair
            [defNames, defVal] = controllerTEB.propertyNameDefaultValues();
            prsr = matlabshared.autonomous.core.internal.NameValueParser(defNames, defVal);
            nvArgs = {varargin{nvStartIdx:end}};
            prsr.parse(nvArgs{:});

            % Assign all properties value from the parser.
            for itr = 1:numel(defNames)
                propname = defNames{itr};
                out.(propname) = prsr.parameterValue(propname);
            end

        end

    end

    methods(Hidden)

        function objcopy = copy(obj)
        %COPY Alternate interface for CLONE to enable standardized testing.
            objcopy = clone(obj);
        end

    end

    methods(Access=private)
        function [firstInfeasibleIdx, exitFlag] = findFirstViolation(...
            obj, curpath, velcmds, deltaT, obstLoc, extraInfo)
        % 0. Preparation of violation detection
        % Last feasible index for all the violations supported as of
        % now. Index is the flag value.
        % 1 - collision
        % 2 - unsafe trajectory
        % 3 - Min turning radius violation
        % 4 - Neg deltaT
        % 5 - Reference path is farther than look ahead distance
            lastFeasibleIndices = inf(1,5);

            % 1. Collision violation (obstacle jump)
            % Calculate the last index which doesn't go over obstacles.
            if ~isempty(obstLoc)

                % Filter obstacles that does not impact the result
                obstLoc = obj.filterObstacles(obstLoc, curpath);

                interval = obj.CellHalfDiagLen/sqrt(2); % Half of cell length
                capsz = obj.RobotInformation.Dimension;
                for itr = 1:(height(curpath)-1)
                    intermediatePoses = interpolate(obj.StateSpace, ...
                                                    curpath(itr,:), curpath(itr+1,:), ...
                                                    [0:interval:1 1]);
                    colliding = areCapsulesCollidingWithOMCells(intermediatePoses', capsz, ...
                                                                obstLoc', obj.CellHalfDiagLen);
                    if any(colliding, "all")
                        lastFeasibleIndices(1) = itr;
                        break;
                    end
                end
            end

            % 2. Safe Trajectory violation
            pod = extraInfo.PoseObstacleDist;
            if ~isempty(pod)
                unsafePoses = pod(:,3) < ...
                    (0.9*obj.TEBParams.ObstacleSafetyMargin)+obj.CellHalfDiagLen;
                % Row of First distance found to be violation
                violationRow = find(unsafePoses,1);
                if ~isempty(violationRow)
                    % First column in pod is the pose Id of the pose
                    % which violates safety margin. Subtract 1 to get the
                    % pose not in violation, that is last feasible pose.
                    lastFeasibleIndices(2) = pod(violationRow,1)-1;
                end
            end

            % 3. Sharp turn
            turnRadius = abs(velcmds(:,1)./velcmds(:,2));
            sharpTurn = turnRadius < 0.9*obj.TEBParams.MinTurningRadius;
            lfi = find(sharpTurn, 1);
            if ~isempty(lfi)
                lastFeasibleIndices(3) = lfi;
            end

            % 4. Negative DeltaT
            negDeltaT = deltaT < 0;
            lfi = find(negDeltaT, 1);
            if ~isempty(lfi)
                lastFeasibleIndices(4) = lfi; % first occurrence of negative deltaT
            end

            % 5. Far From Reference Path
            if extraInfo.FarFromReferencePath
                % In this case there is no trajectory, hence only current
                % state is valid and it would trigger the no action
                % response.
                lastFeasibleIndices(5) = 1;
            end

            % end. Get the minimum index, that will correspond to first
            % violation.
            [firstInfeasibleIdx, exitFlag] = min(lastFeasibleIndices);
        end
    end

end

function obstacleList = extractObstacleList(inputMap)
%EXTRACTOBSTACLELIST extracts XY positions of obstacles in input map

% Default value of output parameters
    obstacleList = zeros(0,2);

    if(isa(inputMap,'binaryOccupancyMap'))
        occMat = inputMap.occupancyMatrix();
    else % This can only be occupancyMap. vehicleCostmap was converted to this.
        occMat = (inputMap.occupancyMatrix() > inputMap.OccupiedThreshold);
    end

    % Extracting obstacle list
    [I, J] = find(occMat);
    if(~isempty(I))
        obstacleList = inputMap.grid2world([I, J]);
    end

end

function colliding = areCapsulesCollidingWithOMCells(poses, capsz, cellCenter, cellHalfDiag)

    obstv = zeros(size(cellCenter));
    capv = [cos(poses(end,:));sin(poses(end, :))];
    if coder.target("MATLAB")
        colliding = robotics.core.internal.mex.checkCollisionCapsule(...
            poses(1:2,:), capv, capsz(1), capsz(2)/2, ...
            cellCenter, obstv, 0, cellHalfDiag, true);
    else
        colliding = robotics.core.internal.impl.checkCollisionCapsule(...
            poses(1:2,:), capv, capsz(1), capsz(2)/2, ...
            cellCenter, obstv, 0, cellHalfDiag, true);
    end

end
