function [AzData, AzHeader, AzSwitches, AzimuthSwitches] = readrangeall(fname,mkplot)
% READRANGEALL - read in full image data from Imagenex pencil beam with Azimuth drivesonar circle
% [AzData, AzHeader, AzSwitches, AzimuthSwitches] = readrangeall(fname);
% 
% input arguments
% fname = file name to open -must have a suffix with .R in it
% mkplot ='y' to make plots, 'n' to not plot (faster)
%
% this program does NOT read single Pencil files correctly- use
% readpencil.m for Pencil data files.

% outputs:
% AzData.imagedata: [rot x ping x bin double], image data with header removed
% 
% AzHeader is a dump of the header info sent with *each ping*
% is is basically the same as a pencil header
% AzHeader.ReturnDataHeaderType = IGX/IMX etc.
% AzHeader.HeadID = ID number
% AzHeader.HeadPosition = head position in counts
% AzHeader.HeadAngle = head rotation angle in degrees
% AzHeader.StepDirection = direction of rotation, -1 = CCW, 1 = CW
% AzHeader.Range = range setting
% AzHeader.ProfileRange = first digitized value above the threshold in cm
% AzHeader.NDataBytes = number of byte in the ping
% AzHeader.NReturnBytes = number of bytes in the ping + header
% AzHeader.StepSize = degrees per step of rotation
% AzHeader.FileName = name of the file read
% AzHeader.NPoints = number of data points returned for the ping
% AzHeader.SectorWidth = size of sector swept in degrees
%
% AzSwitches is a dump of the switches sent to the sonar for this sweep
% AzSwitches = 
%          HeadID: 16
%           Range: 2
%        Reserved: [0 0 0 0 0 0]
%         RevHold: 'Reverse Step Direction'
%     MasterSlave: 'Send Data Slave Mode'
%       StartGain: 5
%            LOGF: 20
%      Absorption: 0.0600
%      TrainAngle: 0
%     SectorWidth: 150
%        StepSize: 0.3000
%     PulseLength: 10
%      DataPoints: 500
%        DataBits: 8
%          UpBaud: 6
%         Profile: 'OFF'
%       Calibrate: 'Calibrate'
%     SwitchDelay: 2
%       Frequency: 900
% AzimuthSwitches
%        HeadID: 31
%       Reserved: [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]
%        RevHold: 'Hold Head'
%    MasterSlave: 'Send Data Slave Mode'
%      StepAngle: 11264
%         UpBaud: 6
%      Calibrate: 'Calibrate'
%    SwitchDelay: 0


% Written by Marinna Martini 5/22/07
% for the U.S. Geological Survey, WOods Hole Field Center

% etm note 6/26/09
% the variable names uaws to say Pencil, but it should be ans is now azimuth

NRETURNHEADERBYTES = 12;
NWHSCFILEHEADERBYTES = 61;
N81AFILEHEADERBYTES = 100;
MAXAZPOSITIONS = 180/3.0; % max number of azimuth points for one circle
AzData = [];
AzHeader = [];
AzSwitches = [];

if ~exist('fname', 'var') || ~exist(fname, 'file'),
    [fname, pathname] = uigetfile( ...
       {'*.R*','WHSC logger files (S1*.P*)'});
    if isempty(fname), return, end
    fname = fullfile(pathname, fname);
end
fid = fopen(fname,'r');
if fid == -1,
    disp(['error reading file ',fname]);
    return
else
    disp(['Now reading file ', fname]);
end
AzHeader.FileName = fname;

% detect which file we have
% read the first few bytes
tag = fread(fid,3,'uchar');
fseek(fid,0,'bof');

switch(char(tag)')
    case 'The' % this is USGS WHSC logger data for azimuth
        % extract year info from file
        d = dir(fname);
        datayear = datevec(d.datenum);
        % first part is the file name, as written by the logger,
        % which is essentially the time stamp
        buffer = fread(fid,NWHSCFILEHEADERBYTES,'uchar');
        [AzTime, AzSwitches] = parseLoggerHeader(buffer, datayear(1));
        clear d
        buffer = fread(fid,27,'uchar');
        AzimuthSwitches = parseAzimuthswitches(buffer);
        % TODO need to put this in the header....
        % this shouldn't be hard-wired!!!
        AzimuthSwitches.StepSize = 3;
        MAXAZPOSITIONS = 180/AzimuthSwitches.StepSize; % max number of azimuth points for one circle

        % for speed, overestimate the size of data matrix needed
        nAzPositions = MAXAZPOSITIONS;
        % add 4 because this is what the logger does
        nPings = ceil(AzSwitches.SectorWidth/AzSwitches.StepSize)+4;
        nPoints = AzSwitches.DataPoints;
        AzData.imagedata = zeros(nAzPositions, nPoints, nPings);
        %PencilData.AAngle = zeros(nAzPositions, 1);
        %PencilData.PAngle = zeros(nAzPositions, nPings);
        %PencilData.ProfileRange = zeros(nAzPositions, nPings);
        disp(sprintf('Estimating %d azimuth positions and %d pings per position',nAzPositions, nPings));
        
        iPing = 0;
        iAzPosition = 0;
        nread = 13;
        raw = zeros(AzSwitches.DataPoints,1);
        if strcmp(fname,'S1071317.R02'), % Marinna's screwup
            pad = 1;
        else pad = 0;
        end
        % there is a different multiplier to get Profile Range real units
        if AzSwitches.Range < 3, factor = 0.002; % 0.002 x count = cm
        else factor = 1; % cm
        end
        badping = zeros(nAzPositions, 1);
        % now at the data
        % we won't know what's in this file ahead of time, must use a for
        % loop
        while nread >= NRETURNHEADERBYTES,
            [buffer, nread] = fread(fid,13,'uchar'); 
            if length(buffer) < 3, break; end
            switch(char(buffer(1:3))')
                case 'IAX'  % this is azimuth data
                    iAzPosition = iAzPosition+1;
                    header = parseAzimuthHeader(buffer,0);
                    AzHeader.AAngle(iAzPosition) = header.HeadAngle;
                    iPing = 0; % new az position, new set of pings
                    %disp('.');
                    fposAz = ftell(fid);
                    % disp(sprintf('Az header at %s',dec2hex(fposAz)))
                  % show the previous sweep
                    if iAzPosition > 1,
                        % build structures for ellyn's plot, imagedata = [npings x npoints]
% Ellyn - this is where I'd like to get a real plot to work, mine is a hack                        
%                         PenData.imagedata = squeeze(PencilData.imagedata(iAzPosition-1,:,:));
%                         header.NPoints = PencilSwitches.DataPoints;
%                         header.StepSize = PencilSwitches.StepSize;
%                         header.ProfileRange = PencilData.ProfileRange(iAzPosition-1,:);
%                         header.Range = ones(PencilSwitches.DataPoints,1).*PencilSwitches.Range;
%                         view_penMM(header, PenData, PencilSwitches);
                     if strcmp(lower(mkplot),'y')
                        clf
                        subplot(2,1,1)
                        pcolor(squeeze(AzData.imagedata(iAzPosition-1,:,:))); shading flat
                        ylabel('Sonar return, Decibels')
                        title(sprintf('Azimuth Rotation %d degrees with %d bad pings',AzHeader.AAngle(iAzPosition-1),badping(iAzPosition-1)))
                        subplot(2,1,2)
                        idx = find(AzHeader.ProfileRange(iAzPosition-1,:) == 0); % remove zeros
                        nzero = length(idx);
                        plot(AzHeader.ProfileRange(iAzPosition-1,:).*factor,'*')
                        title(sprintf('%s, zeros = %4.1f%%',fname,100*nzero/nPoints))
                        ylabel('Profile Range, cm from head')
                        pause(3)
                     end
                    end
                case {'IGX','IPX'}  % this is pencil data, 
                    % store it as a sweep, but first try read ping by ping
                    % should be able to read entire sweep & remove headers
                    % but really can't because of the data dropouts.  This
                    % is not horribly slow
                    iPing = iPing+1;
                    header = parseAzheader(buffer,AzSwitches,0);
                    % disp([iPing header.HeadAngle])
                    AzHeader.HeadAngle(iAzPosition,iPing) = header.HeadAngle;
                    AzHeader.ProfileRange(iAzPosition,iPing) = header.ProfileRange;
                    % read all the data
                    [raw, nread] = fread(fid,header.NDataBytes+pad,'uchar'); % read in all the ping data
                    % TODO the end-1 should be end for most data, this was for the botched dock test
                    AzData.imagedata(iAzPosition,1,iPing) = buffer(NRETURNHEADERBYTES+1);
                    if raw(end-pad) ~= 252,
                        % we probably overshot the IGX or IAX due to a data dropout
                        % find the number of bytes into raw where the next
                        offset = findstr('IGX',char(raw'));
                        if iPing == 1, % last good ping position was after the Az header
                            fseek(fid,fposAz+offset+NRETURNHEADERBYTES,'bof');
                        else % usually last good ping is int he sewwp somewhere
                            if isempty(fpos(iPing-1)+offset+NRETURNHEADERBYTES), keyboard; end
                            if (fpos(iPing-1)+offset+NRETURNHEADERBYTES) <=0, keyboard; end
                            fseek(fid,fpos(iPing-1)+offset+NRETURNHEADERBYTES,'bof');
                        end
                        % back up to last good ping position, now starting at next ping, we hope
                        disp(sprintf('missing terminator ping %d pos %s, %d bytes missing',iPing,dec2hex(ftell(fid))))
                        badping(iAzPosition) = badping(iAzPosition)+1;
                        AzData.imagedata(iAzPosition,2:offset,iPing) = raw(1:offset-1)';
                        if strcmp(fname,'S1071317.R02'), % Marinna's screwup
                            AzData.imagedata(iAzPosition,offset:end,iPing) = ones(header.NDataBytes+pad-offset,1).*NaN;
                        else  AzData.imagedata(iAzPosition,offset+1:end,iPing) = ones(header.NDataBytes+pad-offset,1).*NaN;
                        end
                        %PencilData.imagedata(iAzPosition,iPing,offset+1:end) = zeros(header.NDataBytes+pad-offset,1);
                        %keyboard
                    else
                        AzData.imagedata(iAzPosition,2:end,iPing) = raw(1:end-pad-1)';
                        fpos(iPing) = ftell(fid);
                    end
                    %disp(sprintf('ping %d %d',iPing,raw(end-pad)))
                otherwise
                    disp(sprintf('Data is unrecognized at %d',ftell(fid)))
                    %keyboard
            end
                
            AzHeader.NPoints = AzSwitches.DataPoints;
            AzHeader.StepSize = AzSwitches.StepSize;
            AzHeader.SectorWidth = AzSwitches.SectorWidth;
            
            %if iAzPosition == 10, break; end
        end % while
    otherwise
        disp('Data is unrecognized')
        return
end

disp(sprintf('Found %d azimuth positions and %d pings per position',iAzPosition, iPing));
AzData.badping = badping;
% trim
%PencilData.AAngle = PencilData.AAngle(1:iAzPosition);
%PencilData.PAngle = PencilData.PAngle(1:iAzPosition,:);
%PencilData.imagedata = PencilData.imagedata(1:iAzPosition,:);

fclose(fid);

return

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% read a WHSC logger file header from a S1*.F* file
%
function [AzTime, loggerSwitches] = parseLoggerHeader(fheader, datayear)

% bytes 1:33 are:
% The file name is \S1\S1031414.p12
% it is possible that this can vary
% extract the time information from the file header
cfheader = char(fheader);
MM = sscanf(cfheader(24:25),'%d');
dd = sscanf(cfheader(26:27),'%d');
hh = sscanf(cfheader(28:29),'%d');
mm = sscanf(cfheader(32:33),'%d');
AzTime = julian([datayear, MM, dd, hh, mm, 0]);
clear MM dd hh mm cfheader
% next thing the logger writes out are the switches.  29 bytes.
% not sure why byte #34 is written.  It is ASCII LF = x0A
% pull out switch data for indexing convenience
loggerSwitches = parsePencilswitches(fheader(35:end));

return

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% read the header that comes with each ping.
% applies to logger and live data
function AzHeader = parseAzheader(sheader, switches, verbose)

AzHeader.ReturnDataHeaderType = char(sheader(1:3))';
AzHeader.HeadID = sheader(4);
b=dec2base(sheader(5),2,8); % get the bits
if b(7),  AzHeader.SerialStatus = 'Switches Accepted'; end
if b(8),  AzHeader.SerialStatus = 'Character Overrun'; end
% head position
% explicitly the method from the Imagenex manual Yuk!
% HI = bitand(sheader(7),hex2dec('3E'));
% HI = bitshift(HI,-1); % shift right
% LO1 = bitand(sheader(7),hex2dec('01'));
% LO1 = bitshift(LO1,7);
% LO2 = bitand(sheader(6),hex2dec('7F'));
% LO = bitor(LO1, LO2);
% HI = bitshift(HI,8);
% HP = bitor(HI, LO);
% dec2bin(HP)
%  PencilHeader.HeadAngle = (HP-600)*0.3;
% disp(sprintf('IMagenex method head angle %f', PencilHeader.HeadAngle))
% Doug Wilson's method is more concise.  He probably didn't write the manual
 AzHeader.HeadPosition = bitand(63,sheader(7)).*128 + bitand(127,sheader(6));
 AzHeader.HeadAngle = ( AzHeader.HeadPosition-600).*0.3; % degrees
if bitget(sheader(7),6),  AzHeader.StepDirection = 1;
else AzHeader.StepDirection = -1;
end
AzHeader.Range = sheader(8); % meters
%PencilHeader.ProfileRange = bitand(127,sheader(10)).*127 + bitand(127,sheader(9)); % centimeters
% MM 10jul07 
%PencilHeader.ProfileRange = bitand(127,sheader(10)).*127 + bitand(127,sheader(9)); % meters
% bitshift(A, k) % negative = shift right
PRHB = bitshift(bitand(sheader(10), 126), -1);
PRLB = bitor(bitshift(bitand(sheader(10), 1), 7),bitand(sheader(9), 127));
AzHeader.ProfileRange = bitor(bitshift(PRHB,8),PRLB);
%PencilHeader.NDataBytes = bitand(127,sheader(12)).*127 + bitand(127,sheader(11)); 
PRHB = bitshift(bitand(sheader(12), 126), -1);
PRLB = bitor(bitshift(bitand(sheader(12), 1), 7),bitand(sheader(11), 127));
AzHeader.NDataBytes = bitor(bitshift(PRHB,8),PRLB);
% given the header info, figure out how many data point we're dealing with
% the number of data bits is hardwired to 8 in the logger source code (new_bits)
AzHeader.NReturnBytes = [];
AzHeader.NZeroFillBytes = [];
switch(AzHeader.ReturnDataHeaderType),
    case {'IMX'},   % switch data command for data points = 25
        if switches.DataBits == 4, % 128 Data Byes, 256 Points
            AzHeader.NReturnBytes = 141; 
            AzHeader.NPoints = 256;
        elseif switches.DataBits == 8, % 252 Data Byes, 252 Points
            AzHeader.NReturnBytes = 265; 
            AzHeader.NPoints = 252;
            AzHeader.NZeroFillBytes = 19;
        elseif switches.DataBits == 16, % 500 Data Byes, 250 Points
            AzHeader.NReturnBytes = 513; 
            AzHeader.NPoints = 250;
        end 
    case {'IGX'},   % switch data command for data points = 50
        if switches.DataBits == 4,
            AzHeader.NReturnBytes = 265; % 252 Data Byes, 504 Points
            AzHeader.NPoints = 504;
        elseif switches.DataBits == 8,
            AzHeader.NReturnBytes = 513; % 500 Data Byes, 500 Points
            AzHeader.NPoints = 500;
            AzHeader.NZeroFillBytes = 27; 
        else % 500 Data Bytes, 250 Points
            AzHeader.NReturnBytes = 500; 
            AzHeader.NPoints = 250;            
        end
    case {'IQX'},
        % there is no IQX for the pencil beam sonar
    case {'IPX'},  % Profile = ON
        AzHeader.NReturnBytes = 13;  % 0 Data Byes, 0 Points
        AzHeader.NZeroFillBytes = 15;
        AzHeader.NPoints = 0;
    otherwise
        sprintf([AzHeader.ReturnDataHeaderType,' not recognized'])
        return
end
if verbose,
    disp(AzHeader)
end

return


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% read the header that comes with each azimuth motion.
% applies to logger and live data
function AzimuthHeader = parseAzimuthHeader(sheader, verbose)

AzimuthHeader.ReturnDataHeaderType = char(sheader(1:3))';
AzimuthHeader.HeadID = sheader(4);
b=dec2base(sheader(5),2,8); % get the bits
if b(7),  AzimuthHeader.SerialStatus = 'Switches Accepted'; end
if b(8),  AzimuthHeader.SerialStatus = 'Character Overrun'; end
% azimuth angle
AzimuthHeader.HeadPosition = bitand(127,sheader(7)).*128 + bitand(127,sheader(6));
AzimuthHeader.HeadAngle = ( AzimuthHeader.HeadPosition-600).*0.3; % degrees
if (sheader(13) ~= 252),
    disp('Switch data termination byte not found')
end
if verbose,
    disp(AzimuthHeader)
end

return

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% read the settings sent to the sonar head
% these are saved at the beginning of each data file in the logger
function switches = parsePencilswitches(switchdata)

% first two bytes are switch data header ID = 0xFE44
if (switchdata(1) ~= 254) || (switchdata(2) ~= 68),
    disp('Switch data header ID not found')
end
switches.HeadID = switchdata(3);
switches.Range = switchdata(4); % 5 to 255 m in 5 m increments
switches.Reserved(1) = switchdata(5); % always 0
b=dec2base(switchdata(6),2,8); % get the bits
if b(1), switches.RevHold = 'Hold Head';
else switches.RevHold = 'Resume';
end
if b(7), switches.RevHold = 'Reverse Step Direction';
else switches.RevHold = 'Normal Operation';
end
b=dec2base(switchdata(7),2,8); % get the bits
% I'm not too sure how to interpret the manual on this one
% but this should not matter to logger data
if b(1),
    if b(7), switches.MasterSlave = 'Transmit Slave Mode';
    else switches.MasterSlave = 'Transmit Master Mode';
    end
end
if b(2),
    if b(7), switches.MasterSlave = 'Send Data Slave Mode';
    else switches.MasterSlave = 'Send Data Master Mode';
    end
end
switches.Reserved(2) = switchdata(8); % always 0
switches.StartGain = switchdata(9); % 0 to 40dB in 1 dB increments
switches.LOGF = switchdata(10).*10; % in dB
switches.Absorption = switchdata(11)./100; % dB/m
if switches.Absorption == 253,
    disp('Absorption value of 253 detected, this is not allowed')
end
switches.TrainAngle = (switchdata(12).*3)-180; % degrees
switches.SectorWidth = switchdata(13).*3; % degrees
% pencil beam step increments are 0.3 degrees
switches.StepSize = switchdata(14).*0.3; % degrees per step up to 2.4 
if switches.StepSize > 2.4,
    disp('Step size greater than 2.4 detected, this is not possible')
end
switches.PulseLength = switchdata(15).*10; % microseconds
switches.ProfileMinRange = switchdata(16)./10; % meters
switches.Reserved(3:5) = switchdata(17:19); % always 0
switches.DataPoints = switchdata(20).*10; % 250 for IMX and 500 for IGX
% resolution of data as data width...
% 4 bits = 2 data points per byte
% 8 bits = 1 data point per byte
% 16 bits = 2 bytes per data point
switches.DataBits = switchdata(21); % in bits
switches.UpBaud = switchdata(22); % not yet used
if switchdata(23), switches.Profile = 'ON';
else switches.Profile = 'OFF'; % return data will have IPX header
end
if switchdata(24), switches.Calibrate = 'Normal Operation';
else switches.Calibrate = 'Calibrate'; % head is moved to 0 degrees
end
% the delay to give a control program time to serially upload data
switches.SwitchDelay = switchdata(25).*2; % milliseconds 
if switches.SwitchDelay == 253,
    disp('Switch Delay value of 253 detected, this is not allowed')
end
switches.Reserved(7) = NaN;
switches.Frequency = ((switchdata(26)-100).*5)+675; % kHz
% termination byte
if (switchdata(27) ~= 253),
    disp('Switch data termination byte not found')
end
disp(switches)
return

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% read the settings sent to the azimuth drive
% these are saved at the beginning of each data file in the logger
function switches = parseAzimuthswitches(switchdata)

% first two bytes are switch data header ID = 0xFE44
if (switchdata(1) ~= 254) || (switchdata(2) ~= 68),
    disp('Switch data header ID not found')
end
if (switchdata(3) ~= 31) ,
    disp('This is not an azimuth drive header')
end
switches.HeadID = switchdata(3);
switches.Reserved(1) = switchdata(4); % always 0
switches.Reserved(2) = switchdata(5); % always 0
b=dec2base(switchdata(6),2,8); % get the bits
if b(1), switches.RevHold = 'Hold Head';
else switches.RevHold = 'Resume';
end
b=dec2base(switchdata(7),2,8); % get the bits
% I'm not too sure how to interpret the manual on this one
% but this should not matter to logger data
if b(1),
    if b(7), switches.MasterSlave = 'Transmit Slave Mode';
    else switches.MasterSlave = 'Transmit Master Mode';
    end
end
if b(2),
    if b(7), switches.MasterSlave = 'Send Data Slave Mode';
    else switches.MasterSlave = 'Send Data Master Mode';
    end
end
switches.Reserved(3:6) = switchdata(8:11); % always 0
%Byte 11 = Azimuth Step Angle & 0x7F = BITS 1 to 7
%Byte 12 = (Azimuth Step Angle & 0x3F80)>>7 =  BITS 8 to 14
switches.StepAngle = bitand(127,switchdata(12)).*128 + bitand(127,switchdata(11));
switches.Reserved(7:14) = switchdata(14:21); % always 0
switches.UpBaud = switchdata(22); % not yet used
switches.Reserved(15) = switchdata(23); % always 0
if switchdata(24), switches.Calibrate = 'Normal Operation';
else switches.Calibrate = 'Calibrate'; % head is moved to 0 degrees
end
% the delay to give a control program time to serially upload data
switches.SwitchDelay = switchdata(25).*2; % milliseconds 
if switches.SwitchDelay == 253,
    disp('Switch Delay value of 253 detected, this is not allowed')
end
switches.Reserved(16) = switchdata(26);
if (switchdata(27) ~= 253),
    disp('Switch data termination byte not found')
end
disp(switches)
return