function [range_cm] = hokuyoRangeNavLab(s1, fireString, theta)
% s1 (serial port object) and fireString are set up by hokuyoRangeIntensity_Init
% return Range in CM, intensity in ???, and any error codes
% Theta has been established by hokuyoRangeIntensity_Init
maxedOut = 1000;

%Intensity = nan(N,M);
%AGC = nan(N,M);
% FIRE THE LASER!
fprintf(s1,fireString);
% laser is a string 'GXXXYYYZZ'
%   G:  Get range data
%   XXX three digit # is starting angle in counts (min is 000)
%   YYY three digit # is end angle (max is 768)
%   ZZ two digits cluster size (returns min dist in cluster)
%   count goes CCW, but dead until 44
%   straight ahead is 384
%   ends at 768 but dead after 725
%   nominal angular  range is 270 degrees
%
[N,M] = size(theta);
%err = nan(N,M);
range_cm = maxedOut*ones(N,M);

try  % taking a scan 
    
    % Read Response
    % need to make sure you read all data!
    % ciel(769/inc) *2+Line feeds + status (1-2 numbers?) + command echo (9)
    % expect a message 1576 characters long for full scan?
    response = fread(s1,1);
    while response~='G'
        response = fread(s1,1)
    end
    
    
    response = fread(s1, 88);
    
    %1st 8 are echo of original command, LF(8), error message(10?)?
    % can error message be 2 digits?
    if char(response(10))~='0'
        disp('error taking scan?')
        char(response(9))
        char(response(10))
        char(response(11))
        %return % see manual, but their error messages don't make sense
    end
    
    % next line feed (code 10) begins raneg info
    lineFeeds = find(10 == response);
    nonLineFeeds = find(10 ~= response);
    startIndexRange = lineFeeds(2)+1;
    endIndexrange = length(response);
    %removing all line feeds
    RangeRaw = response(nonLineFeeds);
    % starting after second line feed
    % subtract offset of 30 base 16
    RangeRaw = double(uint8(RangeRaw(startIndexRange-2:end)))-3*16;
    % one range is two numbers representing second 6 bits, then 1st 6 bits
    for i = 1:2:length(RangeRaw)-1
        range_cm( (i+1)/2) = .1*(RangeRaw(i)*64 + RangeRaw(i+1));
        %   These are the cryptic error codes.   0-2
        if range_cm( (i+1)/2) < 2
            err((i+1)/2) = 10*range_cm( (i+1)/2);
            if range_cm( (i+1)/2) == 0
                % Says range might be less than min dist
                % but I observe thsi is really a low intensity return
                % code
                range_cm( (i+1)/2) = maxedOut;
            elseif (range_cm( (i+1)/2) == 1.6)||(range_cm( (i+1)/2) == 0.6)
                % might be more than max dist
                range_cm( (i+1)/2) = maxedOut;
            elseif (range_cm( (i+1)/2) == .1)||(range_cm( (i+1)/2) == 0.2)||(range_cm( (i+1)/2) == .3)||(range_cm( (i+1)/2) == 0.4)||(range_cm( (i+1)/2) == 0.5)
                % Hmmm this woudl include the "Low Light" Codes
                range_cm( (i+1)/2) = maxedOut;
            else
                % may have error code in previous or last time just use
                % value from prev step
                if i~=1
                    range_cm( (i+1)/2) = range_cm( (i-1)/2);
                    
                else
                    range_cm( (i+1)/2) =  maxedOut;
                end
            end
        end
        %   Big digit is 1-5 small digit fluctuates up to 63
        % Not sure these lines are correct
        %Intensity( (i+5)/6) = (RangeRaw(i+2)*1024 + RangeRaw(i+3)*16 );
        %AGC( (i+5)/6) = (RangeRaw(i+4)*64 + RangeRaw(i+5) );
        
    end
catch
    disp('Warning:  Laser did not respond.   Returning max values.');
end
disp('Flush buffer....')
N = s1.BytesAvailable();
while (N~=0)
    fread(s1,N);
    N = s1.BytesAvailable();
end