classdef exampleHelperTruckStateSpace < nav.StateSpace
%TRUCKTRAILERSTATESPACE State Space for Tractor Trailer Planning
%   A custom state space used to capture the state and meta information of 
%   a simple two-body tractor trailer system. State vectors in this space
%   contain the following information: [x y theta beta v isGoal]
%       x       :   x-location of trailer's rear-axle (world frame)
%       y       :	y-location of trailer's rear-axle (world frame)
%       theta   :   orientation of trailer in world frame (world frame)
%       beta    :   interior angle between trailer/truck centerlines (body frame)
%       v       :   velocity along trailer's longitudinal axis (body frame)
%       isGoal  :   a flag determining whether the state represents the goal
    
    properties (SetAccess = protected)
        TruckParams
    end
    
    properties (Hidden)
        Bounds
        LowerBound
        StateSpan
    end
    
    methods
        function obj = exampleHelperTruckStateSpace(xyLimits, truckParams)
        %exampleHelperTruckStateSpace Creates a truck-trailer state space
            
            narginchk(2,2);
            
            % Construct bounds for underlying state space
            stateBounds = [-100 100; -100 100; -pi pi; truckParams.BetaLimit*[-1 1]; -1 1; -inf inf];
            
            if nargin > 0
                stateBounds(1:2,:) = xyLimits;
            end
            
            % Construct base state space
            obj = obj@nav.StateSpace('KinematicTruck', 6, stateBounds);
            obj.StateBounds = stateBounds;
            obj.TruckParams = truckParams;
        end
        
        function state = sampleUniform(obj, numSample)
        %sampleUniform Sample state using uniform distribution
        %   STATE = sampleUniform(SPACE) samples a state within the
        %   StateBounds using a uniform probability distribution.
        %
        %   STATE = sampleUniform(SPACE, NUMSAMPLES) returns NUMSAMPLE
        %   state samples within the StateBounds. NUMSAMPLE is a positive
        %   integer.
            
            narginchk(1,2);
            if nargin == 1
                numSample = 1;
            end
            
            % Sample a random velocity goal
            vDirection = obj.StateBounds(5,randi([1 2],1));
            
            % Create base state vector (0 interior angle, random velocity,
            % not a goal state).
            base = [obj.LowerBound(1:3) 0 vDirection nan];
            
            % Sample random SE2 states
            state = repmat(base,numSample,1);
            state(:,1:3) = state(:,1:3)+repmat(obj.StateSpan(1:3),numSample,1).*rand(numSample,3);
        end
        
        function sampleGaussian(~, varargin)
            error("Not implemented. Kinematic planners only utilize the sampleUniform method.");
        end
        
        function interpolate(~,varargin)
            error("Not implemented. Kinematic planners use the state-propagator's 'propagate' method.");
        end
        
        function distance(~,varargin)
            error("Not implemented. Kinematic planners use the state-propagator's distance method.");
        end
        
        function cObj = copy(obj)
        %copy Creates a deep copy of the object
            cObj = exampleHelperTruckStateSpace(obj.StateBounds(1:3,:), obj.TruckParams, obj.HeuristicCosts);
        end
        
        function state = enforceStateBounds(obj, state)
        %enforceStateBounds No-op
        end
    end
    
    methods (Access = protected)
        function updateStateBounds(obj, bounds)
            if ~isequal(bounds(3:5,:),obj.StateBounds(3:5,:))
                error('Only XY limits are directly settable.')
            end
            obj.LowerBound = bounds(:,1)';
            obj.StateSpan  = diff(bounds,[],2)';
            obj.Bounds = bounds';
        end
    end
end