function [pict, x_gd, y_gd, z_gd] = plotpen07(PenData, HeaderData, plottype, settings);
% PLOTPEN07 - Plot Imagenex imaging sonar data
%  called by procsonarMM: [readfan & plotfan07] replace showfan07
% [pict, x_gd, y_gd, z_gd] = plotpen07(PenData, HeaderData, plottype, settings)
%
% readfan outputs structures containing the image, file and scan headers
% PenData.imagedata: [ping x bin double], image data with header removed
% HeaderData(:).ReturnDataHeaderType = IGX/IMX etc.
% HeaderData(:).HeadID = ID number
% HeaderData(:).SerialStatus 
% HeaderData(:).HeadPosition = head position in counts
% HeaderData(:).HeadAngle = head rotation angle in degrees
% HeaderData(:).StepDirection = direction of rotation, -1 = CCW, 1 = CW
% HeaderData(:).Range = range setting
% HeaderData(:).ProfileRange
% HeaderData(:).NDataBytes = number of byte in the ping
% HeaderData(:).NReturnBytes = number of bytes in the ping + header
% HeaderData.FileName = fname;
% HeaderData.NPoints = PenSwitches.DataPoints;
% HeaderData.StepSize = PenSwitches.StepSize;
% HeaderData.SectorWidth = PenSwitches.SectorWidth;
% plottype = 'square' | 'polar' | 'prange'
% still need to pass 'settings' to get these params: 
% instrument installed height, adcdp compass value, declination
% pict is a structure containing the image from which the elevation and max return are obtained
%  imagesc(pict.x, pict.y, pict.imi) should work
%  x_gd is the horizontal distance interpolated into
%  y_gd is the elevation (position of maximum return for each scan)
%  z_gd is the value of the maximum return for each scan

% based on showPen07 by EM
% reading the data is now replaced by readfan()

SampPerMeter=HeaderData.NPoints/HeaderData.Range(250);  %range settings should all be same
                                        % using mid-cval anyhow
  hang=deg2rad(HeaderData.HeadAngle);  % has to be radians for trig functions                                   
   r  = 1:HeaderData.NPoints;    %because the last point is already removed from imagedata
   rr = r./SampPerMeter(1);
	Yr = rr'*cos(hang);
	Xr = rr'*sin(hang);

       D = PenData.imagedata;  
       Dz = find(D<0);									% Make all values below
       D(Dz) = 0;											% threshold = 0

       % interpolate into x,y space
% xx and yy below are arbitrary numbers for getting the right general shape- they can be tweaked!
xx=[-HeaderData.Range(250):.0125:HeaderData.Range(250)];
yy=[.2:.0025:1.4]';
imi=griddata(Xr,Yr,D,xx,yy,'linear');
 pcolor(xx,-yy, imi); shading flat
 %title('interpd version')
 pict.imi=imi;
 pict.x=xx;
 pict.y=-yy;
  % find the elevations using the max return in each bin
  zz=max(imi);
  for ik=1:length(zz)
   if ~isnan(zz(ik))
    nn=find(imi(:,ik)==zz(ik),1,'first');
   else
    nn=1;
   end
  ly(ik)=nn;
  end
  
  % use the contiguous middle of the plot to select the x range
  % figure(2); plot(diff(yy(ly)))
   jmps=find(abs(diff(yy(ly))) >.2);
      if(jmps(end-1)-jmps(2) < HeaderData.Range(250).*100)
        strt_idx=1; end_idx=jmps(end);
      else
        strt_idx=jmps(2)+1;
        end_idx=jmps(end-1)-1;
      end
   x_gd=xx(strt_idx:end_idx);
   y_gd=-yy(ly(strt_idx:end_idx));
   z_gd=zz(ly(strt_idx:end_idx));
    
    %now remove everything greater than 3 std_devs of the mean
    mn_el=mean(y_gd);
    std_el=std(y_gd);
    gdvals=[mn_el-(3*std_el) mn_el+(3*std_el)];
    ng_idx=find(y_gd < gdvals(1) | y_gd > gdvals(2));
    y_gd(ng_idx)=NaN; 
     figure
      plot(x_gd,y_gd,'.')
       title([HeaderData.FileName(end-11:end) '; range setting= ' num2str(HeaderData.Range(250))])
       xlabel('horizontal distance along seafloor(m)')
       ylabel('depth(m)')