function [PencilData, PencilHeader, PencilSwitches, AzimuthSwitches] = readrangeall(fname, settings)
% READRANGEALL - read in full image data from Imagenex pencil beam sonar circle
% [PencilData, PencilHeader, PencilSwitches, AzimuthSwitches] = readpenrange(fname);
% 
% fname = file name
%
% PencilData.imagedata: [ping x bin double], image data with header removed
% 
% HeaderData is a dump of the header info sent with *each ping*
% HeaderData.ReturnDataHeaderType = IGX/IMX etc.
% HeaderData.HeadID = ID number
% HeaderData.HeadPosition = head position in counts
% HeaderData.AAngle = azimuth drive rotation angle in degrees
% HeaderData.HeadAngle = pencil beam head rotation angle in degrees
% HeaderData.StepDirection = direction of rotation, -1 = CCW, 1 = CW
% HeaderData.Range = range setting
% HeaderData.ProfileRange = first digitized value above the threshold in cm
% HeaderData.NDataBytes = number of byte in the ping
% HeaderData.NReturnBytes = number of bytes in the ping + header
% HeaderData.StepSize = degrees per step of rotation
% HeaderData.FileName = name of the file read
% HeaderData.NPoints = number of data points returned for the ping
% HeaderData.SectorWidth = size of sector swept in degrees
%
% PencilSwitches is a dump of the switches sent to the sonar for this sweep
% PencilSwitches = 
%          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

% 23 jul 07 MM  fixed the bug of thefile
%               make shape of pencil image consistent with readpencil
% Written by Marinna Martini 5/22/07
% for the U.S. Geological Survey, WOods Hole Field Center

NRETURNHEADERBYTES = 12;
NWHSCFILEHEADERBYTES = 61;
MAXAZPOSITIONS = 180/0.3; % max number of azimuth points for one circle
PencilData = [];
PencilHeader = [];
PencilSwitches = [];
AzimuthSwitches = [];

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

if ~exist('settings','var') || ~isfield(settings,'verbose'),
    settings.verbose = 1;
end

% 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
        % 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');
        [PencilTime, PencilSwitches] = parseLoggerHeader(buffer, datayear(1));
        clear d
        buffer = fread(fid,27,'uchar');
        AzimuthSwitches = parseAzimuthswitches(buffer);
        % TODO need to put this in the header....
        AzimuthSwitches.StepSize = 3;
        
        % for speed, overestimate the size of data matrix needed
        nAzPositions = 60; %MAXAZPOSITIONS;
        % add 4 because this is what the logger does
        nPings = ceil(PencilSwitches.SectorWidth/PencilSwitches.StepSize)+4;
        nPoints = PencilSwitches.DataPoints;
        % this shape matches readpencil
        PencilData.imagedata = zeros(nAzPositions, nPoints, nPings);
        PencilHeader.AAngle = zeros(nAzPositions, 1);
        PencilHeader.HeadAngle = zeros(nAzPositions, nPings);
        PencilHeader.HeadPosition = PencilHeader.HeadAngle;
        PencilHeader.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(PencilSwitches.DataPoints,1);
        if strcmp(thename,'S1071317.R02'), % Marinna's screwup
            pad = 1;
        else pad = 0;
        end
        % there is a different multiplier to get Profile Range real units
        if PencilSwitches.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);
                    PencilHeader.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) & settings.verbose,
                        % 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                        
                        tempheader = PencilHeader;
                        tempdata.imagedata = squeeze(PencilData.imagedata(iAzPosition-1,:,:));
                        tempheader.NPoints = PencilSwitches.DataPoints;
                        tempheader.StepSize = PencilSwitches.StepSize;
                        tempheader.ProfileRange = PencilHeader.ProfileRange(iAzPosition-1,:);
                        tempheader.Range = ones(PencilSwitches.DataPoints,1).*PencilSwitches.Range;
                        tempheader.HeadAngle = PencilHeader.HeadAngle(iAzPosition-1,:);
                        tempheader.HeadPosition = PencilHeader.HeadPosition(iAzPosition-1,:);
                        %view_penMM(tempheader, tempdata, PencilSwitches);
                        if ~(isfield(settings,'Pencil_tilt'))
                            settings.Pencil_tilt=0;
                        end
                        plotpen07(tempdata, tempheader, 'square',settings);
                        title(sprintf('File %s, Azimuth %d',thename,PencilHeader.AAngle(iAzPosition-1)))
                        pause(3)
                    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 = parsePencilheader(buffer,PencilSwitches,0);
                    PencilHeader.HeadAngle(iAzPosition,iPing) = header.HeadAngle;
                    PencilHeader.HeadPosition(iAzPosition,iPing) = header.HeadPosition;
                    PencilHeader.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
                    PencilData.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;
                        PencilData.imagedata(iAzPosition,2:offset,iPing) = raw(1:offset-1)';
                        if strcmp(thename,'S1071317.R02'), % Marinna's screwup
                            PencilData.imagedata(iAzPosition,offset:end,iPing) = ones(header.NDataBytes+pad-offset,1).*NaN;
                        else  PencilData.imagedata(iAzPosition,offset+1:end,iPing) = ones(header.NDataBytes+pad-offset,1).*NaN;
                        end
                        %PencilData.imagedata(iAzPosition,offset+1:end,iPing) = zeros(header.NDataBytes+pad-offset,1);
                        %keyboard
                    else
                        PencilData.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
                
            PencilHeader.NPoints = PencilSwitches.DataPoints;
            PencilHeader.StepSize = PencilSwitches.StepSize;
            PencilHeader.SectorWidth = PencilSwitches.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));
PencilData.badping = badping;

fclose(fid);

return

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% read a WHSC logger file header from a S1*.F* file
%
function [PencilTime, PencilSwitches] = 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');
PencilTime = 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
PencilSwitches = parsePencilswitches(fheader(35:end));

return

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

PencilHeader.ReturnDataHeaderType = char(sheader(1:3))';
PencilHeader.HeadID = sheader(4);
b=dec2base(sheader(5),2,8); % get the bits
if b(7),  PencilHeader.SerialStatus = 'Switches Accepted'; end
if b(8),  PencilHeader.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
 PencilHeader.HeadPosition = bitand(63,sheader(7)).*128 + bitand(127,sheader(6));
 PencilHeader.HeadAngle = ( PencilHeader.HeadPosition-600).*0.3; % degrees
if bitget(sheader(7),6),  PencilHeader.StepDirection = 1;
else PencilHeader.StepDirection = -1;
end
PencilHeader.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));
PencilHeader.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));
PencilHeader.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)
PencilHeader.NReturnBytes = [];
PencilHeader.NZeroFillBytes = [];
switch(PencilHeader.ReturnDataHeaderType),
    case {'IMX'},   % switch data command for data points = 25
        if switches.DataBits == 4, % 128 Data Byes, 256 Points
            PencilHeader.NReturnBytes = 141; 
            PencilHeader.NPoints = 256;
        elseif switches.DataBits == 8, % 252 Data Byes, 252 Points
            PencilHeader.NReturnBytes = 265; 
            PencilHeader.NPoints = 252;
            PencilHeader.NZeroFillBytes = 19;
        elseif switches.DataBits == 16, % 500 Data Byes, 250 Points
            PencilHeader.NReturnBytes = 513; 
            PencilHeader.NPoints = 250;
        end 
    case {'IGX'},   % switch data command for data points = 50
        if switches.DataBits == 4,
            PencilHeader.NReturnBytes = 265; % 252 Data Byes, 504 Points
            PencilHeader.NPoints = 504;
        elseif switches.DataBits == 8,
            PencilHeader.NReturnBytes = 513; % 500 Data Byes, 500 Points
            PencilHeader.NPoints = 500;
            PencilHeader.NZeroFillBytes = 27; 
        else % 500 Data Bytes, 250 Points
            PencilHeader.NReturnBytes = 500; 
            PencilHeader.NPoints = 250;            
        end
    case {'IQX'},
        % there is no IQX for the pencil beam sonar
    case {'IPX'},  % Profile = ON
        PencilHeader.NReturnBytes = 13;  % 0 Data Byes, 0 Points
        PencilHeader.NZeroFillBytes = 15;
        PencilHeader.NPoints = 0;
    otherwise
        sprintf([PencilHeader.ReturnDataHeaderType,' not recognized'])
        return
end
if verbose,
    disp(PencilHeader)
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