% Process USGS data
%    From Doug Wilson at Imagenex

% files 12:05 or earlier were logged with 5m range setting
fid1 = fopen('\home\data\sonar_tests\all_images0207\I0221\S1022011.P10','r');
fid2 = fopen('\home\data\sonar_tests\all_images0207\I0221\S1022011.P55','r');
fid3 = fopen('\home\data\sonar_tests\all_images0207\I0221\S1022012.P05','r');

A1 = fread(fid1,'uchar');
A2 = fread(fid2,'uchar');
A3 = fread(fid3,'uchar');


lA1 = length(A1)
A1t = A1(62:lA1);
lA1t = length(A1t);
npings1 = lA1t/513;
A1m = reshape(A1t,513, npings1);
hp1  = bitand(A1m(6,:),127) + 128.*bitand(A1m(7,:), 63);  % high bit is zero, and 
										% bit 6 is direction of scan, so remove it 
hpangledeg1 = (0.3.*(hp1 - 600));	% Angle in radians for Matla
hpangle1    = hpangledeg1.*pi./180;



lA2 = length(A2)
A2t = A2(62:lA2);
lA2t = length(A2t);
npings2 = lA2t/513;
A2m = reshape(A2t,513, npings2);
hp2  = bitand(A2m(6,:),127) + 128.*bitand(A2m(7,:), 63);  % high bit is zero, and 
										% bit 6 is direction of scan, so remove it 
hpangledeg2 = (0.3.*(hp2 - 600));	% Angle in radians for Matla
hpangle2    = hpangledeg2.*pi./180;



lA3 = length(A3)
A3t = A3(62:lA3);
lA3t = length(A3t);
npings3 = lA3t/513;
A3m = reshape(A3t,513, npings3);
hp3  = bitand(A3m(6,:),127) + 128.*bitand(A3m(7,:), 63);  % high bit is zero, and 
hpangledeg3 = (0.3.*(hp3 - 600));	% Angle in radians for Matla
hpangle3    = hpangledeg3.*pi./180;






%%% % % % % %% % % % % %% %% % % %% % % %% % % % %% % % % %  % % %   % %% 
%  CREATE IMAGES
npoints = 500;
SampPerMeter = 100;  % fir 5m range, 5 meters is represented in 500 points

   r  = 1:npoints(1);
   rr = r./SampPerMeter(1);
	Xr = rr'*cos(hpangle1);
	Yr = rr'*sin(hpangle1);
   fhand = figure
   colorthreshold = 0;
   % colorthreshold = input('Color threshold for black (1:100)? ');  % This is a noise floor  
   D = A1m(13:512,:)-colorthreshold;  	% Trim off header
   Dz = find(D<0);									% Make all values below
   D(Dz) = 0;											% threshold = 0
   figure
   axhan = pcolor(Yr,-Xr,D);				% you can use surf or mesh if desired here
  
	shading('flat')
   colormap('gray')
   brighten(0.5);
     axis('equal')
     axis([-2 2 -1.5 0])
	grid on
	title(['Data from file S1022011.P10'])

   figure
   axhan = pcolor(Yr,-Xr,(A2m(13:512,:)-colorthreshold));				% you can use surf or mesh if desired here

	shading('flat')
   colormap('gray')
   brighten(0.5);
	axis('equal')
     axis([-2 2 -1.5 0])
	grid on
	title(['Data from file S1022011.P55'])

   figure
   axhan = pcolor(Yr,-Xr,(A3m(13:512,:)-colorthreshold));				% you can use surf or mesh if desired here

	shading('flat')
   colormap('gray');
   brighten(0.5);
	axis('equal')
     axis([-2 2 -1.5 0])
	grid on
	title(['Data from file S1022012.P05'])