function [pathOpt, kinematicInfo, solnInfo] = optimizePath(varargin)
%optimizePath Optimize path while maintaining safe distance from obstacle
%
%   OPTPATH = optimizePath(INPUTPATH) the travel time and smoothness of the
%   specified path INPUTPATH for car-like robots in an empty map for the
%   default parameters. The number of states in the OPTPATH can be
%   different than in the INPUTPATH.
%
%   OPTPATH = optimizePath(INPUTPATH, MAP) optimizes the INPUTPATH in the
%   specified MAP for the default parameters. The returned optimized path
%   OPTPATH tries to maintain a safety margin to obstacles in the map MAP.
%
%   OPTPATH = optimizePath(INPUTPATH, OPTIONS) optimizes the INPUTPATH in
%   an empty map by parameters specified in OPTIONS. Parameters like safety
%   margin, maximum velocity, maximum acceleration can be specified in
%   OPTIONS as an optimizePathOptions object.
%
%   OPTPATH = optimizePath(INPUTPATH, MAP, OPTIONS) optimizes the INPUTPATH
%   in the specified MAP for by parameters specified in OPTIONS
%
%   [OPTPATH, KINETICINFO] = optimizePath(...) also returns KINETICINFO as
%   a structure that contains the kinetic information of the optimized
%   path.
%
%   Fields of KINETICINFO:
%       TimeStamps - Time corresponding to the pose in the optimized path.
%       Velocity - Velocity to travel between two consecutive poses.
%       AngularVelocity - Angular Velocity between two consecutive poses.
%
%   [OPTPATH, KINETICINFO, SOLNINFO] = optimizePath(...) also returns
%   SOLNINFO as a structure that contains the solution information of the
%   optimized path.
%
%   Fields of SOLNINFO:
%       NumIteration - Number of iterations performed.
%       Cost - Final cost of the path.
%
%   Example:
%
%       % Setup environment.
%       exampleMaps = load("exampleMaps.mat");
%       map = binaryOccupancyMap(exampleMaps.complexMap);
%       validator = validatorOccupancyMap;
%       validator.Map = map;
%       validator.ValidationDistance = 0.01;
%
%       % Use hybrid A* planner to plan the path from start to goal in
%       % the map for SE2 states
%       planner = plannerHybridAStar(validator, MinTurningRadius=2);
%       start = [6 3 pi/2];
%       goal = [38 17 0];
%       pathObj = plan(planner, start, goal);
%       inpath = pathObj.States;
%
%       % Configure options for optimization
%       options = optimizePathOptions;
%       % Use the same min turning radius as in planner.
%       options.MinTurningRadius = 2;
%       % While optimizing the path don't use thrice the poses as in
%       % input path
%       options.MaxPathStates = height(inpath) * 3;
%       % Try to maintain a distance of 0.75 from the obstacles.
%       options.ObstacleSafetyMargin = 0.75;
%
%       % Optimize the path generated by the planner
%       optpath = optimizePath(inpath, map, options);
%
%       % Visualize input path and optimized path in the map.
%       show(map);
%       hold on;
%       quiver(inpath(:,1), inpath(:,2), ...
%           cos(inpath(:,3)), sin(inpath(:,3)), 0.1);
%       quiver(optpath(:,1), optpath(:,2), ...
%           cos(optpath(:,3)), sin(optpath(:,3)), 0.1);
%
%   See also optimizePathOptions, validatorOccupancyMap, plannerHybridAStar

%   Copyright 2021-2023 The MathWorks, Inc.

%#codegen

    narginchk(1,3);

    coder.varsize('path');

    [path, map, options, hasMapInput] = validateInputs(varargin);

    % Convert optimizePathOptions object in to struct for optimizeTEB.
    params = getOptionStruct(options);

    % Compute orientation if N-by-2 matrix was passed as input path.
    if(width(path)==2)
        path = assignOrientation(path);
    end

    % Ensure all angles are in -pi to pi range to ensure proper conditioning
    % of optimization.
    path(:, 3) = robotics.internal.wrapToPi(path(:,3));

    if hasMapInput
        % Extract obstacle list from map and adjust SafetyMargin accordingly
        [obstaclelist,resolution] = extractObstacleList(map);
        % Obstacle margin inflated considering map resolution. i.e. add
        % half-diagonal around the map.
        cellHalfDiagLen = 1/(sqrt(2)*resolution);
        params.ObstacleSafetyMargin = cellHalfDiagLen ...
            + params.ObstacleSafetyMargin;
        params.ObstacleInclusionDistance = cellHalfDiagLen ...
            + params.ObstacleInclusionDistance;
        params.ObstacleCutOffDistance = cellHalfDiagLen ...
            + params.ObstacleCutOffDistance;
    else
        % If the map was not passed as input then pass empty obstacle list
        % to optimizeTEB
        obstaclelist = zeros(0,2);
    end

    if coder.target("MATLAB")
        [p, ki, si] = nav.algs.internal.mex.optimizeTEB(...
            path, obstaclelist, params);
    else
        [p, ki, si] = nav.algs.internal.impl.optimizeTEB(...
            path, obstaclelist, params);
    end

    pathOpt = p;
    kinematicInfo = ki;
    solnInfo = struct('Cost',si.Cost,'NumIteration',si.NumIteration, 'Err', si.Err);

end

function [path, map, arglist, hasMapInput]=validateInputs(input)
% Function to validate all inputs to optimizePath function.

    numargin = numel(input);

    % Supported map types
    mapClassOptions = {'binaryOccupancyMap','occupancyMap', ...
        'vehicleCostmap'};

    % Default values for options
    arglist = optimizePathOptions;

    % If map is not passed it will remain true or else it should be should
    % be set to false.
    hasMapInput = false;

    if numargin == 1
        % Parsing optimizePath(INPUTPATH) syntax.
        validateInputPath(input{1});
        path = input{1};
        map = binaryOccupancyMap;
    elseif numargin == 2
        % Parsing optimizePath(INPUTPATH, MAP) syntax.
        %       OR
        % Parsing optimizePath(INPUTPATH, OPTIONS) syntax.

        % First input
        validateInputPath(input{1});
        path = input{1};

        % Second input
        mapOrOption = input{2};
        isBOM = isa(mapOrOption,mapClassOptions{1}); % binaryOccupancyMap
        isOM = isa(mapOrOption,mapClassOptions{2}); % occupancyMap
        isVC = isa(mapOrOption,mapClassOptions{3}); % vehicleCostmap
        isSupportedMapType = isBOM || isOM || isVC;

        if isSupportedMapType
            % Parsing optimizePath(INPUTPATH, MAP) syntax.
            % Validate to throw right error, standard MATLAB errors which
            % user is used to.
            map = validateAndGetMap(mapOrOption, mapClassOptions);
            hasMapInput = true;

        elseif(isa(input{2},'optimizePathOptions'))
            % Parsing optimizePath(INPUTPATH, OPTIONS) syntax.
            arglist = validateOptimizePathOptions(mapOrOption, 2);
            map = binaryOccupancyMap;
        else
            coder.internal.error('nav:navalgs:optimizepath:InvalidArgument');
        end
    elseif numargin == 3
        % Parsing optimizePath(INPUTPATH, MAP, OPTIONS) syntax.

        % First input
        validateInputPath(input{1});
        path = input{1};

        % Second input
        map = validateAndGetMap(input{2}, mapClassOptions);
        hasMapInput = true;

        % Third input
        arglist = validateOptimizePathOptions(input{3}, 3);
    else
        % For Codegen
        path = zeros(0,3);
        map = binaryOccupancyMap;
    end

    % Verify that "interpolation distance", roughly refVel * refDelT is not
    % too high when compared to the environment.
    delTVelProd = arglist.ReferenceDeltaTime * arglist.MaxVelocity;
    safetyMarginDiameter = 2 * arglist.ObstacleSafetyMargin;

    if hasMapInput
        safetyMarginDiameter = safetyMarginDiameter ...
            + (sqrt(2)/map.Resolution);
        mapDiagLen = norm([diff(map.XWorldLimits) diff(map.YWorldLimits)]);

        % Check if the safetyMarginDiameter is not higher than the diagonal
        % of the map, if it's higher then there is  no space in the map for
        % the poses if it's to successfully keep the safety margin.
        coder.internal.errorIf(safetyMarginDiameter > mapDiagLen, ...
            'nav:navalgs:optimizepath:SafetyMarginGreaterThanMap');

        % Check if path is inside map.
        validatePathInsideMap(path, map);

        % Check if start and Goal pose are obstacle free
        validateStartGoalState(path,map)
    end

    % Check if the "interpolation distance" is not higher than the diameter
    % of safety circle, if it's higher then pose can jump over the
    % obstacles without realizing.
    coder.internal.errorIf(delTVelProd > safetyMarginDiameter, ...
        'nav:navalgs:optimizepath:DelTVelGreaterThanSafetyMargin');

end

function validateInputPath(path)
%validateInputPath checks if input path is in order. It checks for three
%condition.
% 	1. Matrix of type double.
% 	2. Columns are either 2 or three
% 	3. It has minimum three rows

    % Validation of Input 1 path to be of double and 2D array.
    validateattributes(path,{'double'},{'2d','nonempty','nonnan'},...
        'optimizePath','inputPath');

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

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

    % Check the orthogonality between distance vector & orientation vector 
    % of the input path. Display error when orthogonality condition is
    % met for all points on the path.
    if width(path)==3
        dotDkQk = dotOrientationWithTravelDir(path);
        dotDkQkThreshold = 1e-3;
        coder.internal.errorIf(all(abs(dotDkQk) < dotDkQkThreshold), ...
            'nav:navalgs:optimizepath:InputPathOrthogonality');
    end
end

function map = validateAndGetMap(mapIn, mapClassOptions)
%validateAndGetMap validates if map is of one of the supported types
% It checks if map belongs to one of the classes specified in
% mapClassOptions. It also checks if map is of vehicleCostMap type, if yes
% then it converts to occupancyMap.

    validateattributes(mapIn, mapClassOptions,...
        {'nonempty','scalar'},'optimizePath', 'map', 2);

    if isa(mapIn, 'vehicleCostmap')
        % Extract map resolution
        resolution = 1/mapIn.CellSize;
        % Create occupancyMap
        map = occupancyMap(mapIn.Costmap, resolution);
    else
        map = mapIn;
    end

end

function validatePathInsideMap(path, map)
%validatePathInsideMap Checks if input path is inside the boundaries of the map.

    rightOfleftBoundary = path(:,1) >= map.XWorldLimits(1);
    leftOfRightBoundary = path(:,1) <= map.XWorldLimits(2);
    aboveOfBottomBoundary = path(:,2) >= map.YWorldLimits(1);
    belowOfTopBoundary = path(:,2) <= map.YWorldLimits(2);

    % If all four condition are true then all pose are inside the map.
    isInisdeMap = rightOfleftBoundary & leftOfRightBoundary & ...
        aboveOfBottomBoundary & belowOfTopBoundary;

    % If they are not throw an error
    coder.internal.errorIf(any(~isInisdeMap), ...
        'nav:navalgs:optimizepath:InputPathOutsideMap')
end


function validateStartGoalState(path,map)
%validateStartGoalState check if the start and goal state are not on
%obstacles
    start = path(1,1:2);
    goal = path(end,1:2);

    if (isa(map,"binaryOccupancyMap") ||isa(map,"occupancyMap"))
        isValid = checkOccupancy(map,[start;goal]);

    else
        isValid = checkOccupied(map,[start;goal]);
 
    end
    
    coder.internal.errorIf(isValid(1)==1, ...
        'nav:navalgs:optimizepath:InvalidStart')
    coder.internal.errorIf(isValid(2)==1, ...
        'nav:navalgs:optimizepath:InvalidGoal')
    

end

function path = assignOrientation(pathIn)

    % deltaX and deltaY between poses.
    delta = pathIn(2:end,:) - pathIn(1:end-1,:);
    dirT = atan2(delta(:,2), delta(:,1));
    % Assigning direction to last pose.
    dir = [dirT;dirT(end)];

    % Assigning path to output.
    path = [pathIn,dir];

end

function [obstacleList,resolution] = extractObstacleList(inputMap)
%validateAndExtractObstacleList Validates input map and extracts all the
% obstacle positions.It also adds obstacles to the map boundary for keeping
% the optimization in bound

    % 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

    % Updating parameters to add boundary obstacles
    gridSize = inputMap.GridSize;

    % Setting boundary manually for creating bounding box
    boundaryCornerGrids = [0,0;gridSize(1)+1 0;0 gridSize(2)+1;gridSize+1];
    boundaryEdges = [[1:gridSize(1);zeros(1,gridSize(1))]';...
                     [zeros(1,gridSize(2));1:gridSize(2)]';...
                     [1:gridSize(1);(gridSize(2)+1)*ones(1,gridSize(1))]';...
                     [(gridSize(1)+1)*ones(1,gridSize(2));1:gridSize(2)]';];
    boundary = [boundaryCornerGrids;boundaryEdges];
    % Changing grid coordinates to world frame
    boundaryCoordinates = inputMap.grid2world(boundary);

    % Updating output parameters.
    obstacleList = [obstacleList;boundaryCoordinates];
    resolution = inputMap.Resolution;
end

function [arglist] = validateOptimizePathOptions(input,inputNum)
% Validating if ArgumentList is an object of optimizePathOptions
    validateattributes(input,{'optimizePathOptions'},{'scalar','nonempty'},'optimizePath','ArgumentList',inputNum);
    arglist = input;
end

function s = getOptionStruct(options)
    publicProperties = properties(options);
    s = struct();
    for fi = 1:numel(publicProperties)
        s.(publicProperties{fi}) = options.(publicProperties{fi});
    end
end


function dotDkQk = dotOrientationWithTravelDir(path)
% dotOrientationWithTravelDir Compute the dot product between the distance vector and
% orientation vector of the input path
    x = path(:,1);
    y = path(:,2);
    yaw = path(1:end-1,3);
    % Compute different components of the distance and orientation vector
    cosBeta = cos(yaw);
    sinBeta = sin(yaw);
    delX = diff(x); 
    delY = diff(y);
    delS = sqrt(delX.^2+delY.^2);
    % Normalize distance vector
    delX = delX./delS;
    delY = delY./delS;
    % Compute the dot product between distance vector and orientation
    % vector
    dotDkQk = delX .* cosBeta + delY .* sinBeta;
end
