%function fxy = p2xyfan(fsettings)
% p2xyfan - rotate Imagenex fan sonar data and convert from polar to xy
%
% Reads netcdf file created by mkrawcdf
%
% fsettings.ik holds the index of the scan to process
%
% nc{'imagedata'}(time,points,scan) image data with header removed
% nc{'head_pos'}(time,scan) head position in counts
% nc{'headangle'}(scan) head rotation angle in degrees
% nc{'profile_range'}(time,scan) Imagenex computed range to surface
% nc{'nDataBytes'}(scan) = number of byte in the ping
% nc.HeadType. = IGX/IMX etc.
% nc.HeadID. = ID number
% nc.Range = range setting
% nc.NReturnBytes = number of bytes in the ping + header
% nc.DegPerStep = from FanSwitches.StepSize;
% nc.SectorWidth = from FanSwitches.SectorWidth;
%

% plottype = 'square' | 'polar' | 'prange'
% fangle = fan-beam orientation
%
% Still need to pass 'settings' to get or change these params:
%    Height = instrument installed height,
%    fanadcp_off = adcp compass value,
%    magnetic_variation= declination
%    dxy= size of grid= .01 is smallest resolution
% These should come from the metafile, but if supplied, will be used
%
% Outputs:
% Single matrices of Xplot and Yplot that data is interpolated into
% Single matrices of thi and rho, after rotation
% Zs matrix of rotated image (images, if more than 1 sweep)


%  csherwood@usgs.gov
%  July 15, 2009

%% These are provided here for convenience...remove when functionalizing
whole_fname = 'D:\crs\data\MVCO2007\8369fan_raw.cdf'
ncr = netcdf(whole_fname);
fsettings.ncr = ncr; %netcdf id
fsettings.ik = 100; % image number to process
fsettings.dxy = 0.02; % resolution of resulting x, y image (m)
fsettings.plottype = 'polar';
fsettings.plottype = 'square';
fsettings.rot = 0;

% fsettings.USGS_mooring_number = 8369;
% fsettings.rot = 0;
% fsettings.Sonar_Azimuth = 0;
% fsettings.SectorWidth = ncs.SectorWidth(:);
% fsettings.AngleSweepAround = 0;
% %settings.StepSize = 0.6;
% fsettings.StepSize = ncs.StepSize(:);
% fsettings.DataPoints = ncs.DataPoints(:);
% %settings.Height = 0.65;
% fsettings.Height = ncs.Height(:);
% fsettings.datayear = ncs.datayear(:);

make_plot = 1;

%% Start of real function

% Following fields in fsettings MUST be provided by user. Other fields
% are either optional or can be read from raw data cdf file. However, if the
% user provides values, they will override values read from the cdf file.
ncr = fsettings.ncr;
plottype = fsettings.plottype;
ik = fsettings.ik;
dxy = fsettings.dxy;

jt= ncr{'time'}(ik)+ncr{'time2'}(ik)./86400000;
fprintf(1,'Processing %s\n',datestr(gregorian(jt),'HHMM dd-mmm-yy'));

tmp = size(ncr{'raw_image'});
ntimes=tmp(1); npoints=tmp(2); nscans=tmp(3);

% rot is heading of sonar zero (geographical convention, 0 - 360 degrees)
% If not known, default is zero, and plot will be in sonar orientation.

% If user provides values in fsettings, they will be used; otherwise values
% from the cdf file will be used (and returned in fsettings)
if ~isfield(fsettings,'rot')
   fsettings.rot = 0.;
end
rot = fsettings.rot;

if isfield(fsettings,'Height')
   Height=fsettings.Height;
else
   Height=ncr.Height(:);
   fsettings.Height = Height;
end
if isfield(fsettings,'dxy')
   dxy=fsettings.dxy;
else
   dxy=ncr.dxy(:);
end

% trim the redundant first and last ping
imagedata = ncr{'raw_image'}(ik,:,2:nscans-1);
prange = ncr{'profile_range'}(ik,2:nscans-1);
headangle=ncr{'headangle'}(2:nscans-1);
mnStepSize=mean(diff(headangle(10:20)));
[npoints,nangles] = size(imagedata);  % necessary to
slantrange = ncr.Range(:)/npoints:(ncr.Range(:)/npoints): ncr.Range(:);

sectorswept = 1:mnStepSize:ncr.SectorWidth(:)+(mnStepSize)+1;
sectorswept = sectorswept(2:length(sectorswept)-1);
% trim range to real area of data
nearfieldcutoff = 52;
%farfieldcutoff = npoints;
farfieldcutoff = npoints-1; % last point always seems to be 255
imagedata = imagedata(nearfieldcutoff:farfieldcutoff,:);
slantrange = slantrange(nearfieldcutoff:farfieldcutoff);

horizrange = real(sqrt(slantrange.^2 - Height^2));

% set up the interplolation grid
Xplot = (- ncr.Range(:):dxy:ncr.Range(:));
Yplot = Xplot;
% pre-allocate Zs
Zs=ones(ncr.sweep(:),length(Xplot), length(Yplot));

%% experiment to find best match of two sweeps
if(1)
   % make indices into forward and backward sweeps
   % with enough padding to compare shifted subsets
   % calculate covariance for two arrays and use to find best shift value
ipad = 15;
swi1 = (1+ipad:((nangles/2)-ipad))';
swi2 =( (nangles-ipad):-1:((nangles/2+1)+ipad))';
clear mdd
j=1;
for shif = 3:1:13
   imbar = 0.5*(imagedata(:,swi1)+imagedata(:,swi2+shif));
   C=cov(imagedata(:,swi1), imagedata(:,swi2+shif));
   figure(1); clf
   imagesc(imbar)
   mdd(j,:)=[shif C(1,2)];
   disp(mdd(j,:))
   j=j+1;   
end
[val_cor,idx_cor]=max(mdd(:,2));
best_shift = mdd(idx_cor,1)
end
   
%%
n=0;
for kk=1:ncr.sweep(:)
   % may want to only do the first sweep
   % kk=1;
   if ncr.sweep(:) > 1
      imdata=imagedata(:,n+1:(length(sectorswept)*kk)+(kk));
      ha=headangle(n+1:(length(sectorswept)*kk)+(kk));
   else
      imdata=imagedata;
      ha=headangle;
   end
   % have to flip both because interp2 requires monotonic increasing ha
   if (mod(kk,2)==0)
      imdata=fliplr(imdata);
      %ha must be flipped to get monotonic increase for the interp
      % first sweep is always - to +, 2nd, 4th, etc are + to -
      ha=flipud(ha);
      % in back sweep ha never gets to -174.  To get sweep 1 & 2 images to
      % overlay after interp, have to decrease all by .15
      % ha=ha+1.05;   % I think there's about a 7 step lag
      ha=ha-mnStepSize;
   end
   if (ha(1)== ha(2))
      ha(1)=ha(2)-mnStepSize;
   end
   if (ha(end)== ha(end-1))
      ha(end)=ha(end)+mnStepSize;
   end
   %% Image processing here
   % now get the new size for plotting
   [npoints,nangles] = size(imdata);

   if(0) % attempt to fit surface and remove...does not help
   [mx,my] = meshgrid(1:nangles,1:npoints);
   % fit a smooth surface to the data
   [zg,xg,yg]=gridfit(mx,my,imdata,1:20:nangles,1:10:npoints,'smooth',.6);
   % interpolate that surface to location of all sonar points
   mz = interp2(xg,yg,zg,mx,my);
   % subtract the smooth surface, then make all data positive
   imdata = imdata-mz;
   imdata = imdata-min(imdata(:));
   end
   
   if(1) % remove minimum values and scale by std dev.
      min_along_range = min(imdata');
      imdata = imdata-repmat(min_along_range',1,nangles);
      std_along_range = std(imdata');
      imdata = imdata./repmat(std_along_range',1,nangles);
   end
   %% range / azimuth plot
   figure(1); clf
   d = dist(imdata(:));
   clims = [d.pct5 d.pct95];
   imagesc(imdata,clims)

   %% polar plot
   figure(2); clf
   settings.fillval = 255; % white background
   clear fakeheadangle
   % these are x,y locations we want image data for
   [Xi, Yi] = meshgrid(Xplot,Yplot);
   % radial equivalents, thi ranges from 0 to 360
   [ri, thi]=pcoord(Xi, Yi);
   % rotate the angles we want to interpolate the data to
   % (subtract the value...same as ADDing rot to the data)
   thi = thi-rot;
   % make thi range from -180 to 180
   thi(thi>180)=thi(thi>180)-360;
   thi(thi< -180)=thi(thi< -180)+360;
   
   % pad ranges and angle of sweep
   % headangle(1)=-174; headangle(nangles)=161.25
   % fakeheadangle = linspace(headangle(1), headangle(nangles), ...
   %		     nangles)';
   sr = [0,slantrange(1,1)-.1,...
      slantrange(1,:), slantrange(1,npoints)+.1, 9]';
   % pad angles to get complete circle
   % first 3 of first image are always the same
   %if kk == 1
   %xx=[ha(3):mnStepSize:ha(nangles-1)+mnStepSize];
   %hfill=[ha(3)-(2*mnStepSize) ha(3)-mnStepSize];
   fh=[ha(1:nangles)];
   if (diff(diff(fh)) ~= median(diff(fh)))
      fakeheadangle=[ha(1):mnStepSize:ha(nangles-1)+mnStepSize]';
      % there was also often duplication of the first 3
      % headangles, which may account for problems with the size
      % of the computed fakeheadangle, so add angles to the end if
      % needed
      if length(fakeheadangle)< nangles
         nmiss=nangles-length(fakeheadangle);
         for ik=1:nmiss
            fakeheadangle(end+ik)=fakeheadangle(end)+(ik+1)*mnStepSize;
         end
      end
      lopad = [-179.999 min(fakeheadangle)-mnStepSize];
      hipad = [max(fakeheadangle)+mnStepSize 180];
      nlo = length(lopad);
      nhi = length(hipad);
      fakeheadangle = [lopad fakeheadangle' hipad];
   else
      fakeheadangle=[ha(1:nangles)];
      % pad for the dead zone area
      lopad = [-179.999 min(ha)-mnStepSize];
      hipad = [max(ha)+mnStepSize 180];
      nlo = length(lopad);
      nhi = length(hipad);
      fakeheadangle = [lopad fakeheadangle' hipad];
   end
   
   % pad data
   Zpad = (settings.fillval)*ones(npoints+nlo+nhi,nangles+nlo+nhi);
   %Zpad = (settings.fillval)*ones(npoints+4,nangles);
   % if we let the condition by <0, it does a better job in terms of look
   % but the log10 that follows makes those 0's -inf, which makes writing
   % into the short later barf.
   imdata(imdata<(0))=(settings.fillval);
   % set the 0's == 1 to allow the log10 to operate correctly and still
   % get the effect of having missing values replaced by a dark pixel
   % instead of a light one
   imdata(imdata==0)=1;
   
   % kludge...last row (449) of range values always seems to be 252
   % imdata(449,:)=(settings.fillval);
   Zpad(nlo+1:npoints+nhi,nlo+1:nangles+nhi)=imdata;
   % the log10 is needed to more evenly distribute the data in bins
   % omitting it leads to a very dark image
   Zpad = log10(Zpad);
   
   % interpolate onto polar shape
   Zi = interp2(fakeheadangle,sr,Zpad, thi,ri);
   %    Zi(Zi<1)=(8); % produces weird contrasty plot
   imagesc(Xplot,Yplot,Zi)
   set(gca,'ydir','normal') % because imagesc is sidedownup
   colormap gray;
   set(gca,'Fontsize',12);
   set(gca,'Xtick',[-5:1:5])
   set(gca,'Ytick',[-5:1:5])
   
   if(1), % plot range circles
      hold on
      for icirc = 1:5,
         h = circle(icirc,0,0);
         set(h,'color',[.8 .7 .7])
      end
   end
   axis square
   
   % annotate plot
   ts = sprintf('Sweep=%d, rot=% 3.0f',kk,rot)
   text(0.01,0.03,ts,...
      'units','normalized','color','k','horizontalalignment','left','fontsize',12)
   text(0.8,0.95,'\uparrow North',...
      'units','normalized','color','k','fontsize',14)
   
   ts = datestr(gregorian(jt),'HHMM dd-mmm-yy');
   text(0.99,0.03,ts,...
      'units','normalized','color','k','horizontalalignment','right','fontsize',12);
   hold off
   %clear imdata sr ha fakeheadangle xx hfill Zpad
   
   
   % put Zi into a structure, so both images may be accessed
   %  Zs{ik,kk}=Zi;
   Zs(kk,:,:)=Zi;
   % increment n for next image
   n=n+length(sectorswept)+1;
end
% let the user know where we are
if mod(ik,10) == 0
   disp(['processing record ' num2str(ik) ' of ' num2str(length(ncr{'time'}))])
end
fxy.x = Xplot;
fxy.y = Yplot;
fxy.thi = thi;
fxy.ri = ri;
fxy.zs = Zs;
clear Xi Yi sr fakeheadangle