% -------------------------------------------------------
%       averaged.m
%
%            first skips 61 bytes of header for file then
%            for each of 400 pings
%               read 13 bytes of header
%               read 400 pings per record  (in arc)
%               each ping is 251 bytes of data
%               1 bytes per sample
%
% --------------------------------------------------------

%ii=input('pick a file number:');

fname='fanbeam.81b';
%
fid =fopen(fname);
%find the beginning of the file
fseek(fid,0,-1)

sstr=fscanf(fid,'%c',3000);
stind=strfind(sstr,'IMX')
fseek(fid,stind(1)-1,-1)
%sstr2=fscanf(fid,'%c',20)


len=384;

%[X1,count] = frea))'
[X1,count] = fread(fid,'uint8');
fclose(fid);

lon=floor(length(X1)./384)
x1 = reshape(X1(1:(len*lon)),len,lon);
%
% clear X1 count fid 
%
%header type, should be IMX 
%hdr(1:34)=setstr(X1(1:34)')
%hdr(35:61)=X1(35:61)
imx=setstr(x1(1:3,:));
   if all(imx(1:3,1)'=='IMX'), disp('Mode = IMX, OK'), end
   if all(imx(1:3,1)'=='IGX'), disp('Mode is  IGX'), end
% sonar head ID, should be 16   
id=(x1(4,:));
   if all(id==16), disp('ID = 16, OK'), end
   if all(id~=16), disp('ID is not 16'), end
% serial Status, should be 64   
ss=(x1(5,:));
   if all(ss==64), disp('Serial Status = 64, OK'), end
   if all(ss~=64), disp('Serial Status is not 64'), end
% head position   
   high=bitand(63,x1(7,:));
   low=bitand(127,x1(6,:));
   d=bitand(64,x1(7,:));
   hp=-210+0.15.*(128.*high+low);
% range, should be 10 for 3 m   
rr=(x1(8,:));
disp(['Range = ' num2str(mean(rr)) ', OK'])
% profile Rainge - should vary with data   
pr=(256*x1(10,:)+x1(9));
   if (pr==0), disp('Profile Range = 0, OK'), end
   if (pr~=0), disp('Profile Range is not 0'), end
   high=bitand(127,x1(12,:));
   low=bitand(127,x1(11,:));
% number of data points, with 25, should be 251 points
num=127*high+low;
   if all(num==251), disp('Number of Data Bytes = 251, OK'), end
   if all(num~=251), disp('Number of Data Bytes is not 251'), end
%   
% clear high low imx id ss r pr num 
%
%loafd gps position
for ii=1:lon
lat_i(ii)=str2num(setstr(x1(285:286,ii)'))+str2num(setstr(x1(288:293,ii)'))/60;
long_i(ii)=str2num(setstr(x1(297:299,ii)'))+str2num(setstr(x1(301:306,ii)'))/60;
end
lat=clean0(lat_i,21,3);
long=clean0(long_i,21,3);
clg
plot(long,lat,'.-')
hold on
plot(long_i,lat_i,'.-r')

figure(3);clg
close all
mstruct = defaultm('utm');
mstruct.zone = '19T';
mstruct.geoid = almanac('earth','geoid','m','grs80');
mstruct = defaultm(utm(mstruct));
[x_gps,y_gps] = mfwdtran(mstruct,lat,-long);
x_gps=0*x_gps-x_gps(1);
y_gps=0*y_gps-y_gps(1);
%sp=spap2(1,4,x_gps,y_gps);
 spx=csaps(1:length(x_gps),x_gps,.0000001);
  spy=csaps(1:length(y_gps),y_gps,.0000001);
  clg
plot(x_gps,y_gps,'.');hold on
plot(ppval(spx,1:length(x_gps)),ppval(spy,1:length(y_gps)),'r')

x_gps=ppval(spx,1:length(x_gps));
y_gps=ppval(spy,1:length(y_gps));
%x_gps=linspace(1,100,length(x_gps));
%y_gps=linspace(1,100,length(y_gps));

% plot head position
if 0
figure(1)
clf
plot(1:length(hp),hp,1:length(d),d),grid
title([fname])
xlabel('Step'),ylabel('Degrees Orientation')
%
% plot linear 
figure(2)
clf;
imagesc(x1(12:251+12,:));
colormap(jet);
colorbar
title([fname])
%
% plot polar
end
figure(3)
clf
   usenr=len;
	nt=lon;nr=251;
	theta=hp*pi/180;
	% R=linspace(0,210,nr)';
	h=140;
	r=linspace(0,30,nr)';
    %r=sqrt(r.^2-h.^2);
	%R= r.*sin(acos(h./r));R=R(:);
	x=r*cos(theta);y=r*sin(theta);x(imag(x)~=0)=0;y(imag(y)~=0)=0;
	Z = zeros(nr,nt);
    Z=dup([1:length(x)]',251)';
	sx1 = x1(12:11+nr,:);
    %for ii=1:20:lon
	%pcolor(x(:,ii:ii+20)./100+dup(x_gps(ii:ii+20)',251)',y(:,ii:ii+20)./100+dup(y_gps(ii:ii+20)',251)',sx1(:,ii:ii+20));shading flat;colormap(jet)
    	surface(x(:,:)+dup(x_gps(:),251)',y(:,:)+dup(y_gps(:),251)',Z,sx1(:,:));shading flat;colormap(jet)

    hold on
	axis('square'); 
   axis('image')
%axis([-500 500 -500 500])
   %caxis([min(min((sx1))) max(max((sx1)))]);
   caxis([0 110])
   %drawnow;pause;
   %end
   
   %title([fname,': ', datestr(dt)])
   xlabel('cm');ylabel('cm');set(gca,'xtick',[-500:100:500]);
    
   set(gcf,'paperpos',[.25 .25 4 4])
%eval(['print -dpng imagesima/' fname '.png']) 
end
end
% cleanup   
%   clear