function [ rangeVec, coordinateVec, endOfRayCoordVec ] = sense(xSensor, ySensor, thetaSensor, alphaStart, alphaEnd,
							       map, numberRays, sensorRange, sensorNoiseVar)
%SENSE    LIDAR range sensor.
%   SENSE(x,y,theta,start,end,map,n,senseRange,noiseVar) simulates a LIDAR 
%   range sensor at position ('x','y') and orientation 'theta' in the
%   binary map 'map'. Orientation is set in radians.
%
%   The LIDAR sensor casts 'n' rays uniformly distributed between the
%   angles 'start' and 'end'. Positive angles are defined clockwise
%   measured in the local coordinate system in sensor x-direction (so a
%   'theta' equal to zero has the same orientation in the map as the sensor
%   itself).
%    NOTE:      positive x-axis in map points to the right, positive y-axis
%            points downwards, thus positive angles are clockwise.
%
%   The sensor can only sense objects up to the distance 'senseRange'
%   measured in units of 'map'. Assuming 'map' was created from a pixel
%   image, then the unit is pixels. If 1 pixel represents 1cm, then
%   'senseRange' will obviously be set in cm.
%
%   Sensor noise is modelled as additive 1D Gaussian with zero mean and
%   variance 'noiseVar' for each dimension separately. A value of zero
%    indicates no noise and thus perfect measurement.
%
%   ALL ARGUMENTS ARE REQUIRED.
%
%   The first return value is a n-dimensional vector containing the ranges
%   (euclidian distance) from the sensor to a sensed object within its
%   sensor range 'senseRange'. If no object was hit by sensor ray, the
%   range is returned as 'Inf'. Please keep in mind for further processing
%   of the data.
%
%   The second return value is a n-by-2-dimensional matrix. The first
%   column containing x-coordinates, the second column containing
%   y-coordinates of the sensed object. Both measured in the global
%   coordinate system defined by 'map'. If no object was sensed, the
%   coordinates are both set to 'Inf'. The second return value is optional.
%
%   The third return value is a a n-by-2-dimensional matrix. It contains
%   the x- and y-coordinates of the farthest points of each ray. The first
%   column contains all the x-coordinates, the second column all the
%   y-coordinates. NOTE: No checking on the coordinates being inside the
%   map boundaries will be done whatsoever.
%
%
%
%   Example:
%           load('mapLoops.mat');
%           
%           xRobot  = int32(100);
%           yRobot  = int32(80);
%           heading = 0;
%           noRays  = int32(8);
%           
%           [myRange,myCoord,myEndCoord] = sense(xRobot, yRobot, heading, -pi/2, pi/2, mapLoops, noRays, 500, 0.0);
%           
%           figure();
%           imshow(mapLoops);
%           hold on;
%           plot(xRobot, yRobot, 'bh');
%           for i = 1:length(myEndCoord)
%               plot( [xRobot, myEndCoord(i,1)], [yRobot, myEndCoord(i,2)], 'g-');
%           end
%           plot(myCoord(:,1), myCoord(:,2), 'rx');
%
%
%   Copyright 2012 Jonathan Bryan Schmalhofer
%
%   $Revision: 0.3 $  $Date: 2012-09-02 23:33:00 $


    
    %% Check & Recondition Input Arguments
    %
    
    % check number input arguments
    if nargin ~= 9
        error('Wrong number of input arguments. All nine have to be set.');
    end
    
    % Coordinates of sensor need to be integers
    if ~isinteger(xSensor) || ~isinteger(ySensor)        
        error('Sensor positions x and y need both to be integers.');
    end
    
    % if orientation of sensor was not set but is empty, we set it to zero
    if isempty(thetaSensor)
        thetaSensor = 0;
    end
    
    % if start angle of sensor was not set but is empty, we set it to zero
    if isempty(alphaStart)
        alphaStart = 0;
    end
    
    % if end angle of sensor was not set but is empty, we set it to zero
    if isempty(alphaEnd)
        alphaEnd = 0;
    end
    
    % angles with an absolute value bigger than 2*pi make no sense, so we use modulo-division to truncate them
    %     NOTE:    we allow negative values for angles, so to not mess up with the signs, instead of mod(a,b) we use
    %            MATLAB function rem(a,b)
    thetaSensor    = rem(thetaSensor, 2.0 * pi);
    alphaStart     = rem(alphaStart, 2.0 * pi);
    alphaEnd     = rem(alphaEnd, 2.0 * pi);
    
    % check if map is empty
    if isempty(map)
        error('The argument `map` must not be empty.');
    end
    
    % check if map is a binary map (type logical in MATLAB)
    if ~islogical(map)
        error('mapNoBinary', 'The argument `map` needs to be a binary matrix (MATLAB type `logical`).');
    end
    
    % check number of arrays
    if numberRays <= 0 || ~isinteger(numberRays) || isempty(numberRays)
        error('The number of rays to be casted must be a positive non-zero integer. It must not be empty.');
    end
    
    % check sensor range
    if sensorRange <= 0 || isempty(sensorRange)
        error('The sensor range must be positive non-zero and must not be empty.');
    end
    
    % check sensor noise - set to zero if not set
    if isempty(sensorNoiseVar)
        sensorNoiseVar = 0;
    end
    
    % check sensor noise for valid value
    if sensorNoiseVar < 0
        error('sensorNoiseNeg', 'The sensor noise must be zero or positive.');
    end
    
    
    
    %% Sense Step
    %

    % check if we are stuck in a wall, if yes: return just zeros (no further calculation needed)
    if map(ySensor,xSensor) == 1
        rangeVec         = zeros(numberRays,1);
        coordinateVec    = zeros(numberRays,2);
        endOfRayCoordVec= zeros(numberRays,2);
        return;
    end

    % initialize the return-vectors with 'Inf'
    rangeVec         = Inf(numberRays,1);
    coordinateVec    = Inf(numberRays,2);
    
    % convert sensor coordinates and 'numberRays'to double, so they can be
    % used for multiplication with other doubles
    numberRays      = double(numberRays);
    xSensor         = double(xSensor);
    ySensor         = double(ySensor);

    % if two or more rays are casted, they are uniformly distributed and casted starting with and at
	% 'alphaStart' clockwise up to 'alphaEnd'
    if numberRays >= 2
        deltaAlpha = (alphaEnd - alphaStart) / (numberRays - 1);
    else
        deltaAlpha = 0;
    end
    
    % vector containing angles of all n arrays in global coordinate system
    %
    % (first entry is the ray at angle 'alphaStart' relative to 'thetaSensor',
    % next entry is rotated by 'deltaAlpha' clockwise and so on...)
    allAlpha    = ones(numberRays,1) * (thetaSensor + alphaStart) + [0:(numberRays-1)]' .* deltaAlpha;

    radiusSteps    = ceil( sqrt( size(map,1)^2 + size(map,2)^2 ) );        % how many points should be picked along
	% the ray (we calculate the length of the diagonal of the map and round up - assuming that is the longest
	% possible distance)
    
    % matrices containing all points points X and Y along the rays - each row in a matrix contains all points of
	% a dimension (X or Y) along a ray starting from the sensor
    allX        = ones(numberRays,radiusSteps) * xSensor + (ones(numberRays,1) * linspace(0,sensorRange,
				radiusSteps)) .* (cos(allAlpha)*ones(1,radiusSteps));
    allY        = ones(numberRays,radiusSteps) * ySensor + (ones(numberRays,1) * linspace(0,sensorRange,
				radiusSteps)) .* (sin(allAlpha)*ones(1,radiusSteps));
    
    % save coordinates of endpoints of all rays for third return vector
    endOfRayCoordVec(:,1) = allX(:,end);
    endOfRayCoordVec(:,2) = allY(:,end);
    
    % because all our coordinates above are indices of matrix 'map', we have to
    % round them to integers and mark all indices which would be out of bounds of 'map'
    maxX            = size(map, 2);
    maxY            = size(map, 1);
    allX            = round(allX);        % first we round
    allX(allX<1)    = NaN;                % now all indices which are to small are marked as 'NaN'
    allX(allX>maxX)    = NaN;                % now all indices which are to big are marked as 'NaN'
    allY            = round(allY);        % first we round
    allY(allY<1)    = NaN;                % now all indices which are to small are marked as 'NaN'
    allY(allY>maxY)    = NaN;                % now all indices which are to big are marked as 'NaN'
    
    % loop all arays (unfortunately it seems it can't be done with at least one for-loop)
    for i = 1:numberRays
        % now save all points (x,y) in a long (n*radiusSteps)-by-2-matrix
        allPoints         = [allX(i,:)',allY(i,:)'];
        % delete every point that has at least one entry 'NaN' - WHY? Because the NaN indicates it is outside the
		% bounds of 'map'
        allPoints         = allPoints(~any(isnan(allPoints), 2), :);
        
        % get indices in 'map' of the points specified in 'allPoints' - these indices represent the pixels the
		% ray reaches
        %
        % NOTE: instead of row and column index we use linear index
		% (see: http://www.mathworks.de/company/newsletters/articles/Matrix-Indexing-in-MATLAB/matrix.html )
        linIndex = sub2ind(size(map), allPoints(:,2), allPoints(:,1));
        % now get the first entry along the ray that hits a wall
        num = find(map(linIndex) == 1, 1, 'first');
        % if we found one...
        if ~isempty(num)
            % ...we transform the linear index back to a row and column (equals y and x coordinates) and save it
            [r,c]                 = ind2sub(size(map),linIndex(num));
            c                   = round( c + randn(1) * sensorNoiseVar );    % add noise
            r                   = round( r + randn(1) * sensorNoiseVar );    % add noise
            c(c<=0)             = 1;
            r(r<=0)             = 1;
            coordinateVec(i,:)    = [c, r];
            % furthermore, we calculate the euclidian distance from that point where we hit the wall up to the sensor
            rangeVec(i)            = sqrt( (coordinateVec(i,1)-xSensor)^2 + (coordinateVec(i,2)-ySensor)^2 );
        end
    end
    
end