classdef manipulatorCollisionBodyValidator < nav.StateValidator
%MANIPULATORCOLLISIONBODYVALIDATOR Validate a state of a rigid body tree against a cell-array of convex collision bodies
%   The validator performs a state and motion validity check of a
%   state that corresponds to the joint configuration of a rigid body tree.
%
%   SV = manipulatorCollisionBodyValidator creates a state validator with a default
%   manipulatorStateSpace object with default values for validation distance and
%   environment.
%
%   SV = manipulatorCollisionBodyValidator(SS) creates a state
%   validator based on the given manipulator state space object, SS. The
%   environment of the manipulator is empty.
%
%   SV = manipulatorCollisionBodyValidator(_,"PropertyName",PropertyValue)
%   sets properties on the object using name-value arguments.
%
%   MANIPULATORCOLLISIONBODYVALIDATOR Properties:
%       StateSpace            - State space used for validation
%       ValidationDistance    - Distance resolution for validating motion between configurations
%       IgnoreSelfCollision   - Ignore self-collision checks
%       SkippedSelfCollisions - Body pairs skipped for checking self-collisions
%       Environment           - Obstacles in the environment
%
%   MANIPULATORCOLLISIONBODYVALIDATOR  Methods:
%       copy            - Create deep copy of state validator
%       isStateValid    - Validate the input joint state
%       isMotionValid   - Validate the motion between two states
%
%   Example:
%   sv = manipulatorCollisionBodyValidator;
%
%   % Check for a valid state.
%   isStateValid(sv, sampleUniform(sv.StateSpace))
%
%   % Add a collisionBox to the environment.
%   sv.Environment{end+1} = collisionBox(0.1,0.1,0.1);

%   Copyright 2021-2023 The MathWorks, Inc.

%#codegen
    properties(Access = private)
        StateValidatorInternal
    end

    properties (Access = {?nav.algs.internal.InternalAccess})
        %SkipStateValidation Skip validation in certain member functions
        %   This switch is used by internal functions only
        SkipStateValidation = false
    end

    properties
        %ValidationDistance - Distance resolution for validating motion between configurations
        %   The validation distance determines the number of interpolated nodes
        %   between two adjacent nodes of the tree that should be checked for
        %   validity.
        %
        %   Default: 0.1
        ValidationDistance = 0.1

        %IgnoreSelfCollision - Ignore self-collision checks
        %   To ignore collisions between manipulator bodies while
        %   validating states or motion, set this property to true or 1.
        %
        %   Default: false
        IgnoreSelfCollision = false

        %Environment - Obstacles in the environment
        %   Obstacles in the environment, specified as a cell array of
        %   collision objects like collisionMesh or collisionBox.
        %
        %   Default: {}
        Environment
    end

    properties
        %SkippedSelfCollisions - Body pairs skipped for checking self-collisions
        %   Specify which pairs of bodies to ignore for self-collisions during
        %   planning. The property can take the following values:
        %
        %   - "parent" - Skip collision checking between child and parent
        %                bodies.
        %   - "adjacent" - Skip collision checking between bodies on adjacent
        %                indices.
        %   - Skip collision checking between specific body pairs,
        %   specified as a cell-array of character vectors of size
        %   NUMPAIRS-by-2. Each row contains a pair of body names to skip
        %   for collision checking. NUMPAIRS is the number of body pairs to
        %   skip for collision checking.
        %
        %   Default: 'parent'
        SkippedSelfCollisions
    end

    methods
        function set.Environment(obj, env)
            obj.validateEnvironment(env);
            tempenv = env;
            for i = 1:length(tempenv)
                tempenv{i} = copy(tempenv{i});
            end
            obj.Environment = tempenv;
            obj.updateValidator();
        end

        function set.ValidationDistance(obj, valdist)
            robotics.internal.validation.validatePositiveNumericScalar(valdist,...
                                                                       getClassName(obj),...
                                                                       obj.ValidationDistancePropertyName);
            obj.ValidationDistance = valdist;
            obj.updateValidator();
        end

        function set.IgnoreSelfCollision(obj, flag)
            obj.IgnoreSelfCollision = robotics.internal.validation.validateLogical(flag,...
                                                                                   getClassName(obj), ...
                                                                                   obj.IgnoreSelfCollisionPropertyName);
            obj.updateValidator();
        end

        function set.SkippedSelfCollisions(obj,value)
            if(~iscell(value))
                obj.SkippedSelfCollisions=validatestring(...
                    value, {...
                             lower(char(robotics.manip.internal.SkippedSelfCollisionType.ADJACENT)), ...
                             lower(char(robotics.manip.internal.SkippedSelfCollisionType.PARENT)), ...
                           }, ...
                    getClassName(obj),'SkippedSelfCollisions');
            else
                validateattributes(value,{'cell'},{'size',[nan,2]},...
                                   "manipulatorCollisionBodyValidator","SkippedSelfCollisions");
                obj.SkippedSelfCollisions=value;
            end
            obj.updateSkippedSelfCollisions(obj.SkippedSelfCollisions);
        end

    end

    properties(Constant, Access=private)
        %EnvironmentPropertyName
        EnvironmentPropertyName = 'Environment';

        %ValidationDistancePropertyName
        ValidationDistancePropertyName = 'ValidationDistance';

        %IgnoreSelfCollisionPropertyName
        IgnoreSelfCollisionPropertyName = 'IgnoreSelfCollision';

        %SkippedSelfCollisionsPropertyName
        SkippedSelfCollisionsPropertyName = 'SkippedSelfCollisions';

    end

    properties(Constant, Access=private)
        %DefaultValidationDistance
        DefaultValidationDistance = 0.1

        %DefaultEnvironment
        DefaultEnvironment = {}

        %DefaultIgnoreSelfCollision
        DefaultIgnoreSelfCollision = false

        %DefaultSkippedSelfCollisions
        DefaultSkippedSelfCollisions = lower(char(robotics.manip.internal.SkippedSelfCollisionType.PARENT))
    end

    methods(Access=private, Static)
        function parser = makeParser()
        %Construct a name-value pair parser for the properties
            names = {
                manipulatorCollisionBodyValidator.EnvironmentPropertyName,...
                manipulatorCollisionBodyValidator.ValidationDistancePropertyName, ...
                manipulatorCollisionBodyValidator.IgnoreSelfCollisionPropertyName ...
                manipulatorCollisionBodyValidator.SkippedSelfCollisionsPropertyName ...
                    };
            defaults = {
                manipulatorCollisionBodyValidator.DefaultEnvironment,...
                manipulatorCollisionBodyValidator.DefaultValidationDistance ...
                manipulatorCollisionBodyValidator.DefaultIgnoreSelfCollision, ...
                manipulatorCollisionBodyValidator.DefaultSkippedSelfCollisions, ...
                       };
            parser = robotics.core.internal.NameValueParser(names, defaults);
        end

        function throwSkippedSelfCollisionsBehaviorChangeWarning(parser,varargin)
        %throwSkippedSelfCollisionsBehaviorChangeWarning Throws SkippedSelfCollisionsUnspecified warning
        %   If self-collision checking is ignored, this warning is not thrown.
            ignoreSelfCollision = ...
                parser.parameterValue(manipulatorCollisionBodyValidator.IgnoreSelfCollisionPropertyName);
            if(~ignoreSelfCollision)
                robotics.manip.internal.throwSkippedSelfCollisionsBehaviorChangeWarning(...
                    'manipulatorplanning:SkippedSelfCollisionsUnspecified',...
                    parser.Names,varargin{:});
            end
        end
    end

    methods(Access=private)
        function validateEnvironment(obj, env)
        %validateEnvironment Validation of input env for checkCollision
        %   The env are passed as a cell-array of collision
        %   objects. The function returns true if the env are a
        %   valid input to checkCollision
            validateattributes(env, {'cell'}, {}, ...
                               getClassName(obj),...
                               obj.EnvironmentPropertyName);
            for i = 1:length(env)
                if(~isa(env{i}, 'robotics.core.internal.CollisionGeometryBase'))
                    robotics.manip.internal.error(...
                        'manipulatorplanning:InvalidCollisionBodyEnvironment');
                end
            end
        end

        function name = getClassName(~)
            name = 'manipulatorCollisionBodyValidator';
        end

        function updateValidator(obj)
        %updateValidator Update the internal validator except its SkippedSelfCollisions
            obj.StateValidatorInternal.IgnoreSelfCollision = obj.IgnoreSelfCollision;
            obj.StateValidatorInternal.Environment = obj.Environment;
            obj.StateValidatorInternal.ValidationDistance = obj.ValidationDistance;
        end

        function updateSkippedSelfCollisions(obj,val)
        %updateSkippedSelfCollisions Update the internal validator's SkippedSelfCollisions
            obj.StateValidatorInternal.updateSkippedSelfCollisionType(val);
        end
    end

    methods
        function obj = manipulatorCollisionBodyValidator(varargin)
            parser = manipulatorCollisionBodyValidator.makeParser();
            if(nargin == 0)
                ss = manipulatorStateSpace;
                parser.parse(varargin{:});
                manipulatorCollisionBodyValidator.throwSkippedSelfCollisionsBehaviorChangeWarning(...
                    parser,varargin{:});
            else
                ss = varargin{1};
                parser.parse(varargin{2:end});
                manipulatorCollisionBodyValidator.throwSkippedSelfCollisionsBehaviorChangeWarning(...
                    parser,varargin{2:end});
            end
            obj@nav.StateValidator(ss);
            env = ...
                parser.parameterValue(obj.EnvironmentPropertyName);
            ignoreSelfCollision = ...
                parser.parameterValue(obj.IgnoreSelfCollisionPropertyName);
            skippedSelfCollisions = ...
                parser.parameterValue(obj.SkippedSelfCollisionsPropertyName);
            validationDistance = ...
                parser.parameterValue(obj.ValidationDistancePropertyName);
            obj.StateValidatorInternal = robotics.manip.internal.ManipulatorStateValidator(...
                ss, env,validationDistance,skippedSelfCollisions);
            obj.Environment = env;
            obj.ValidationDistance = validationDistance;
            obj.IgnoreSelfCollision = ignoreSelfCollision;
            obj.SkippedSelfCollisions = skippedSelfCollisions;
        end

        function newObj = copy(obj)
        %copy Create a deep copy of the state validator
            newObj = manipulatorCollisionBodyValidator(...
                copy(obj.StateSpace), ...
                obj.EnvironmentPropertyName, obj.Environment, ...
                obj.IgnoreSelfCollisionPropertyName, obj.IgnoreSelfCollision, ...
                obj.SkippedSelfCollisionsPropertyName, obj.SkippedSelfCollisions, ...
                obj.ValidationDistancePropertyName, obj.ValidationDistance);
        end

        function [isValid, lastValid] = isMotionValid(obj, state1, state2)
        %isMotionValid Validate the motion between two states
        %   [ISVALID, LASTVALID] = isMotionValid(SV, STATE1, STATE2)
        %   interpolates between STATE1 and STATE2 validates each state in the
        %   interpolation. The function also returns the last valid state
        %   along the interpolated path.

            narginchk(3, 3);
            if(~obj.SkipStateValidation)
                nav.internal.validation.validateStateVector(...
                    state1, obj.StateSpace.NumStateVariables, ...
                    'isMotionValid', 'state1');
                nav.internal.validation.validateStateVector(...
                    state2, obj.StateSpace.NumStateVariables, ...
                    'isMotionValid', 'state2');
            end
            [isValid, lastValid] = obj.StateValidatorInternal.isMotionValid(state1, state2);
        end

        function isValid = isStateValid(obj, state)
        %isStateValid Validate the input joint state
        %   ISVALID = isStateValid(SV, STATE) checks if the input state of the
        %   rigid body tree robot model is in collision with the environment or
        %   itself, or is out of state bounds. STATE must be an
        %   N-by-NumStateVariables matrix, representing N state samples. ISVALID
        %   is an N-element vector of logical values, with TRUE indicating that
        %   the corresponding state sample in STATE is valid.

            narginchk(2, 2);
            if(~obj.SkipStateValidation)
                nav.internal.validation.validateStateMatrix(...
                    state, nan, obj.StateSpace.NumStateVariables, ...
                    'isStateValid', 'state');
            end
            numStates = size(state, 1);
            lowerBound = repmat(obj.StateSpace.StateBounds(:, 1)', numStates, 1);
            upperBound = repmat(obj.StateSpace.StateBounds(:, 2)', numStates, 1);
            isSphericalDoF=robotics.manip.internal.RigidBodyTreeUtils.identifySphericalPrimitive(...
                obj.StateValidatorInternal.Robot);
            isStateWithinBounds=(lowerBound <= state) & (state <= upperBound);
            isStateWithinBoundsForNonSphericalDoF=isStateWithinBounds(:,~isSphericalDoF);
            isInBounds = all(isStateWithinBoundsForNonSphericalDoF,2);
            isValid = isInBounds & obj.StateValidatorInternal.isStateValid(state);
        end

    end
end
