function [PencilData, settings] = showpencil(fname, settings);
% SHOWPENCIL - Display Imagenex Pencil Beam Sonar images taken with Robin Singer's data logger
% [imagedata, headangle, prange] = showpencil(fname, settings);
%
% 
%
% fname = file name
% datayear = the year the data was taken
% imagedata = the image as [angle, point]
% headangle = the head position in degrees
% prange = profile range in cm
%

% 01 may 07 (MM) exlain what settings are and provide some if user doesn't
% etm mods 8/06 to disallow "wild" points and where yi < .4
%   calls values > .2 above the median wild, and finds any points <.1
%   and sets them to NaN before interpolating onto the output grid
%   the output grid lower limit was changed from -2.8 to -2.5
% Revisions by Chris Sherwood 23 June 2006
% Written by Marinna Martini 8/28/02
% for the U.S. Geological Survey, WOods Hole Field Center

nheaderbytes = 13;

fid = fopen(fname,'r');
if fid == -1,
    disp(['error reading file ',fname]);
    return
else
    disp(['Now reading file ', fname]);
end

% deal with settings
if ~exist('settings','var'),
    settings.pencil_vert_ang = 0.0;
end
if ~isfield(settings,'datayear'),
    settings.datayear = 1950;
    disp('using default data year of 1950, ')
    disp('you may wish to provide settings.datayear = a realistic year')
    disp('useage = [PencilData, settings] = showpencil(fname, settings)')
end

% read the file header
[fheader, numread] = fread(fid,61,'uchar');
% 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');
settings.PencilTime = julian([settings.datayear, MM, dd, hh, mm, 0]);

% decipher the first 13 bytes of sonar header so we know what we are reading
[sheader, numread] = fread(fid,nheaderbytes,'uchar');
disp('The header')
dec2hex(sheader)'

% imagenex return data header will tell us the total number of return data bytes
% conver the first three bytes into an ASCII string
return_data_header = char(sheader(1:3,:))';
disp(sprintf('Return Data Header = %s',return_data_header));
head_ID = dec2hex(sheader(4));
disp(sprintf('Header ID = %s',head_ID))
% we can skip the serial status for now
% head position
headposhi = bitand(63,sheader(7));
headposlo = bitand(127,sheader(6));
headpos = headposhi*127+headposlo;
stepdir = bitget(sheader(7),6);
% translate from 0 to 2800 meaning -210 to 210 in 0.15 degree steps
headangle = 0.15*(headpos-1400);
disp(sprintf('Head angle = %f', headangle))
% sonar head range
sonar_head_range = sheader(8);
disp(sprintf('Sonar Head Range setting = %d m',sonar_head_range));
% profile range
prangehi = bitand(127,sheader(10));
prangelo = bitand(127,sheader(9));
prange = prangehi*127+prangelo;
disp(sprintf('First Digitized Range Value Above Threshold = %d cm', prange))
% Number of echo data bytes returned
ndatabyteshi = bitand(127,sheader(12));
ndatabyteslo = bitand(127,sheader(11));
ndatabytes = ndatabyteshi*127+ndatabyteslo;
disp(sprintf('Number of data bytes = %d', ndatabytes))

% 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)
ndatabits = 8;

switch(return_data_header),
case {'IMX'},   % switch data command for data points = 25
    if ndatabits == 4, nreturnbytes = 141; end  % 128 Data Byes, 256 Points
    if ndatabits == 8, nreturnbytes = 265; end  % 252 Data Byes, 252 Points
    if ndatabits == 14, nreturnbytes = 513; end % 500 Data Byes, 250 Points
case {'IGX'},   % switch data command for data points = 50
    if ndatabits == 4, 
        nreturnbytes = 265; % 252 Data Byes, 504 Points
    else
        nreturnbytes = 513; % 500 Data Byes, 500 Points
                            % 500 Data Byes, 250 Points
    end
case {'IQX'}, 
    % there is no IQX for the pencil beam sonar
case {'IPX'},  % Profile = ON
    nreturnbytes = 13;  % 0 Data Byes, 0 Points
end
disp(sprintf('Number of return bytes per ping = %d',nreturnbytes));
npoints = nreturnbytes - nheaderbytes;

% now figure out how many pings of data are in this file from the file size and header info.
% 61 bytes of file header
fseek(fid,0,'bof'); 
filebeg = ftell(fid);
fseek(fid,0,'eof'); % go to the end of the file
filebytes = ftell(fid)-filebeg-118; % byte from beginning of file

npings = round(filebytes./nreturnbytes);
disp(sprintf('Number of useable pings = %d (maybe)',npings))

frewind(fid);

% skip the 61 bytes of file header
[fheader, numread] = fread(fid,61,'uchar');

% guess at a dataholdersize
imagedata = zeros(npings, nreturnbytes-nheaderbytes);
headangle = zeros(npings, 1);
prange = zeros(npings,1);
pingidx = 0;

while 1,
    pingstart = ftell(fid);
    if pingstart < 0, break; end
    pingidx = pingidx+1;
    
    % read header
    [sheader, numread] = fread(fid,nheaderbytes,'uchar');
    if numread < nheaderbytes, break; end
    % grab the position
    % head position
    headposhi = bitand(63,sheader(7));
    headposlo = bitand(127,sheader(6));
    headpos = headposhi*127+headposlo;
    stepdir = bitget(sheader(7),6);
    headangle(pingidx) = headpos;
    % profile range
    prangehi = bitand(127,sheader(10));
    prangelo = bitand(127,sheader(9));
    prange(pingidx) = prangehi*127+prangelo;
%      [sheader(6) sheader(7) sheader(8)];
 %     etm_headang(pingidx)=.3*(bitor(bitshift(sheader(6),7),sheader(7))-600);   
    % read image data
    [pingdata, numread] = fread(fid,nreturnbytes-nheaderbytes,'uint8');
    if numread < nreturnbytes-nheaderbytes, break; end
    imagedata(pingidx,:) = pingdata';
        
end
fclose(fid);
% translate from 0 to 2800 meaning -210 to 210 in 0.15 degree steps (not
% sure this is right; field log says 132 sector at 0.3 deg - CRS)
dang = 0.30;   % was 3
headangle = dang*(headangle-600);

% remove last column
imagedata = imagedata(:,1:end-1);
[npings, npoints] = size(imagedata);
% the sign of this value determines whether nomsr is higher at the right or
% left- + makes it higher at the end, - makes it higher at the start
% this SHOULD compensate for small errors in mounting angle
settings.pencil_vert_ang = 0.0;   % original value = -2
headangle = headangle(1:npings)';    % trim it if there's an extra point
%  headangle=headangle*1.04;  % etm mod to flatten the shape
hang = (pi/180)*(headangle+settings.pencil_vert_ang);
dsr = sonar_head_range/npoints;      % delta slant range
sr = (dsr/2:dsr:sonar_head_range-dsr/2);

% this is the nominal elevation computed from the data
%nom_el = 1.1;
nom_el = .7;     % sensor is mounted 0.91 mab
nomsr = nom_el./cos(hang);
imagedatamf = zeros(size(imagedata));
imagedatapf = zeros(size(imagedata));
imagedataff = imagedatapf;
sri = 1:length(sr);      
srbot = zeros(size(nomsr));
mgbot = zeros(size(nomsr));
% use filtering to get an answer in slant range space.
for i=1:npings
   ii = find(sr>nomsr(i),1,'first');
   imagedatamf(i,:)=medfilt(imagedata(i,:),25);
   P = polyfit( sri(ii-50:ii+50), imagedata(i,ii-50:ii+50), 2 );
   imagedatapf(i,ii-50:ii+50) = polyval(P,sri(ii-50:ii+50));
   imagedataff(i,:)=imagedatamf(i,:);
   imagedataff(i,ii-50:ii+50) = 0.5*(imagedatamf(i,ii-50:ii+50)+...
      imagedatapf(i,ii-50:ii+50));
   steroids = imagedatamf(i,:);
   steroids(ii-50:ii+50)=steroids(ii-50:ii+50)+imagedatapf(i,ii-50:ii+50);
   [Y,I]=max(steroids(1:400));
   mgbot(i)=Y;
   srbot(i)=sr(I);
   if(0)
      figure(3); clf
      plot(sri,imagedata(i,:),'.');
      hold on
      plot(sri,imagedatamf(i,:),'-k');
      plot(sri,imagedatapf(i,:),'-r');
      drawnow; shg; pause
   end
end
%%
% interpolate into x,y space
xx = -3.2:.04:3.2;
yy = (.2:.01:1.4)';
nxx = length(xx);
nyy = length(yy);
xx = repmat(xx,nyy,1);
yy = repmat(yy,1,nxx);
sri = sqrt(xx.^2+yy.^2);
angi = atan(xx./yy);
hangpad = [-pi/2 hang(1)-eps hang hang(end)+eps pi/2 ]';
hangpad = repmat(hangpad,1,npoints+4);
srpad = [-1 sr(1)-eps sr sr(end)+eps 10 ];
srpad = repmat(srpad,npings+4,1);
% pad top and bottom with two rows
padval = -1;
imagedatapad = [padval*ones(2,npoints); imagedata; padval*ones(2,npoints)];
% pad left and right columns
imagedatapad = [padval*ones(npings+4,2) imagedatapad  padval*ones(npings+4,2)];
imi = zeros(nyy,nxx);
clf
imi=griddata(hangpad,srpad,imagedatapad,angi,sri,'linear');
figure(1)
pcolor(xx,yy,imi); shading flat; set(gca,'ydir','reverse'); shg
%
%  plot where imi == 127...
for ik=1:161
ll=find(imi(:,ik)>=127);
if ~isempty(ll) %where the bottom is found
 pp=find(yy(ll,ik)> .6);  %blank vals < .6
 if isempty(pp)
     vals(ik)=NaN;
     mnvals(ik)=NaN;
 else
  vals(ik)=min(yy(ll(pp),ik));
  mnvals(ik)=mean(yy(ll(pp),ik));
 end
else
vals(ik)=NaN;
mnvals(ik)=NaN;
end
end
mm=find(imi==127);
figure(4)
clf
plot(xx(mm),yy(mm),'.')
hold on
plot(xx(1,:),mnvals,'r*')
plot(xx(1,:),vals,'gx')
set(gca,'Ydir','Rev')
xlabel('xx (xdist)')
ylabel('yy (elev.)')
title ('method comparison for pencil beam')
%%
%plot linear
%figure(1)
%clf;
%imagesc(hang,sr,imagedata')
%%caxis([ 80 128 ])
%colormap(jet);
%colorbar
%hold on
%plot(hang,nomsr,'-w','linewidth',2)
%plot(hang,srbot,'-r','linewidth',2)
%ylabel('Slant Range (m)')
%closexlabel('Head Angle (rad)')

%title(['Fan beam image taken ',datestr(datenum(gdate))])
figure(2)
clf;
el = srbot.*cos(hang);
hd = srbot.*sin(hang);
hh=plot(nomsr.*sin(hang),nomsr.*cos(hang),'-k');
set(hh,'color',[.6 .6 .6],'linewidth',3)
hold on
plot(hd,el,'-k');
scatter(hd,el,22,mgbot,'filled');
   locs=find(abs(diff(el)) > .2); %detect wild pts
    plot(hd(locs),el(locs),'rx')
%plot(hd,el,'-k')
axis([-3,3,0,1.5])
set(gca,'ydir','reverse')
  if (~isempty(locs))
    fillvals=ones(length(locs),1)*NaN;
    el(locs)= fillvals;
  end

[X,I]=sort(hd);
Y = el(I);
lc=find(Y <= 0.01);
 px=find(X < -2.5);
 if (~isempty(px))
     X(px)=NaN;
     Y(px)=NaN;
 end

if (~isempty(lc))
  Y(lc)=NaN;
end

%plot(X,Y,'-k');

dx = 0.02;
% with angle 2 instead of -2, [hd(1) hd(end)]= [-2.1 2.5]
xi = [-2.1 :dx: 2.5];  % creates 231 points
%xi = [-2.8:dx:2.2];  % 251 pts
yi = NaN*ones(size(xi));
for i=1:length(xi),
  ii=find( X>xi(i)-3*dx & X<xi(i)+3*dx );
%  yi(i) = nanmean(Y(ii));
  jj = ii(find( ~isnan( X(ii)+Y(ii) ) ) );
  if(jj)
     p = 1; % p: 0 = moving average; 2 = inverse distance; 3 = inverse dist. squared etc.
     W = abs( (X(jj)-xi(i)).^p )./sum( abs( (X(jj)-xi(i)).^p ) );
     yi(i) = sum(W.*Y(jj));
  end
end
  %figure(1)   % show original yi variation
  %plot(diff(yi))
   
  % now check whether there any we missed
  lx=find(yi < .4);
  if (~isempty(lx))
     fillvals=ones(length(lx),1)*1e35;
     yi(lx)=fillvals;
  end
  
figure(2)
plot(xi,yi,'-c','linewidth',2)

ts = datestr(datenum(gregorian(settings.PencilTime)))
text(1,.25,ts);
figure(4)
plot(xi,yi,'-c','linewidth',2)

PencilData.imagedata = imagedata;
PencilData.headangle = headangle;
PencilData.slantrange = sr;
PencilData.xdist = xi;
PencilData.zelev = yi;
PencilData.xx = xx;
PencilData.yy = yy;
PencilData.imi = imi;
PencilData.mnvals = mnvals;
PencilData.minval = vals;