% paz

% absolute constants
d2r = pi/180.;
r2d = 1/d2r;

% sonar constants
Ro = .08255 % distance from az drive axis to pencil beam transducer (m)
factor = 0.002; % converts profile range to meters
meters_per_point = 3/500;

% deployment constants
pb_h = 1.055 % elevation
pb_pitch = 0*d2r; % this is
az_pitch = -3*d2r;
az_roll = -2*d2r;
tidx = 1; % time index
nblank = 25;
ntrim = 0;
fn = 'az2009-12-08_raw.cdf'
%fn = 'az2009-12-09_raw.cdf'
%Open the data file
ncload(fn,'time','time2','scan','points','rots','headangle','azangle','profile_range');
nc = netcdf(fn);

% TODO: loop over time indices and file names here
raw_data = nc{'raw_image'}(:);
ncclose;
%%
[naz,nangle]=size(headangle);
% (but last angle data is no good )

% array for storing transformed points
pt = NaN*ones(4,naz,nangle-1); 
for iaz=1:naz; % sweep over azimuths (naz = 60)
   
   pb_hdg = d2r*azangle(iaz);
   % Rotation around vertical axis (heading)
   % (these matrices are fixed for each azimuth)
   Rz = [ cos(pb_hdg)   -sin(pb_hdg)   0        0 ;...
          sin(pb_hdg)    cos(pb_hdg)   0        0 ;...
          0           0          1        0 ;...
          0           0          0        1];
   % TODO - move next two matrices out of the azimuth loop
   % Rotation around fore/aft axis (pitch)
   Ry = [ cos(az_pitch)  0          sin(az_pitch) 0;...
          0           1          0          0;...
         -sin(az_pitch)  0          cos(az_pitch) 0;...
          0           0          0          1];
   % Rotation around starboard/port axis (roll)
   Rx = [ 1           0          0         0;...
          0           cos(az_roll) -sin(az_roll) 0;...
          0           sin(az_roll)  cos(az_roll) 0;...
          0           0          0         1];
   % Order matters
   Rt = Rx*Ry*Rz;
   
   % Imagenex idea of target range
   pb_pr = profile_range(iaz,:)*factor;
   
   %% Ellyn's approach: max gradient in returns
   %iaz = 20;
   %range = 3/500*(1:500);
   %hangle = squeeze(headangle(iaz,1:end-1));
   %mesh(hangle',range,r)
   %mesh(hangle',range(1:end-1),diff(r))
   r = squeeze(raw_data(iaz,nblank:end-ntrim,1:end-1));

   [val,idx] = max( diff(r) );
   [nr,nc]=size(r);
   pb_sr = zeros(1,nc);
   for ii=1:nc % TODO vectorize this!
      pb_sr(ii)=(idx(ii)+1+nblank)*3/500;
      val(ii)=r(idx(ii)+1,ii);
   end
%    plot(pb_sr)
%    shg
%    pause
   %%
   
   pb_angle = headangle(iaz,1:end-1)*d2r;
   % calculate location of bottom return in pencil coordinates
   pb_x = Ro;
   pb_y = pb_sr.*sin(pb_angle+pb_pitch);
   pb_z = pb_h-pb_sr.*cos(pb_angle+pb_pitch);
   figure(3);clf
   title(['pb_hdg = ',num2str(pb_hdg)])
   figure(3); hold on
   plot(pb_y,pb_z,'.')
   shg
   ok = find( pb_sr>.3 & pb_sr<3 & pb_z > -1 );
   fprintf(1,'Az = %f, ok = %d \n',pb_hdg,length(ok))
   for iang=1:length(ok) % sweep over pencil headangles (nangle = 463)
      p = [ pb_x ; pb_y(iang); pb_z(iang); 0 ];
      ptt=Rt*p;
      pt(1:3,iaz,iang)=ptt(1:3,:); % these are x,y,z of each point
      pt(4,iaz,iang)=val(iang);  % this is the amplitude
   end
end

figure(1);clf
%plot(squeeze(pt(1,:,:)),squeeze(pt(2,:,:)),'.k') 
scatter( squeeze(pt(1,:)),squeeze(pt(2,:)),12,squeeze(pt(3,:)),'filled')

figure(2); clf
plot3(pt(1,:),pt(2,:),pt(3,:),'.b')
xlabel('x')
ylabel('y')
zlabel('z')
shg

%%
figure(3); clf
subplot(211)
plot(pt(1,:),pt(3,:),'.b')
xlabel('x')
zlabel('z')
axis([-3 3 -.5 .5])
grid on
subplot(212)
plot(pt(2,:),pt(3,:),'.b')
xlabel('y')
zlabel('z')
axis([-3 3 -.5 .5])
grid on
shg
%%
if(0)
figure(3);clf
scatter3( squeeze(pt(1,:)),squeeze(pt(2,:)),squeeze(pt(3,:)),12,squeeze(pt(4,:)),'filled')
end