 
% 
% Simple drawing function that shows obstacle detections (1 sensors). 
% Should be used together with tbot.naiveObstacleDetection(.) function. 
% 
% Receives: 
%   - (x,y,theta) robot's pose
%   - detectionFlag - detection flag (logical) 
%   - detectionSettings - settings struct provied by tbot.naiveObstacleDetection(.)
%
% Versions: 
% v01 - initial RML release (3 sensors) 
% v02 - UC Summer School 2023 update (added sensor settings, only suports 1 sensor)
% ToD0: add more sensors, draw circular profile in each detection   


function drawObstacleSensor(x, y, theta, detectionFlag, detectionSettings)

    % get robot's pose 
    %x = pose(1);
    %y = pose(2);
    %theta = pose(3);

    % get number of detection sensors 
    nsensors = detectionSettings.nSensors; 

    if( nsensors > 1) error( 'drawObstacleSensor(.) only supports 1 sensor' ); end  

    % get plot radius (range threshold)
    r = detectionSettings.rangeThreshold;
    
    % get slice Aperture (in radians)
    aperture = detectionSettings.aperture;     

    % set robot sensor location 
    xs = x - 0.0305 * cos(theta); 
    ys = y - 0.0305 * sin(theta);

    % draw (frontal facing) sensor
    if(detectionFlag)     
        fill(   [xs, xs + r*cos(theta + aperture), xs + r*cos(theta - aperture) ], ... 
                [ys, ys + r*sin(theta + aperture), ys + r*sin(theta - aperture)], 'r', 'FaceAlpha', 0.75);
    else 
        fill(   [xs, xs + r*cos(theta + aperture), xs + r*cos(theta - aperture) ], ... 
                [ys, ys + r*sin(theta + aperture), ys + r*sin(theta - aperture)], 'w', 'FaceAlpha', 0.5);
    end 


