% TRIPOD_PLOT
% Script to plot tripods used for Ripples DRI measurements at MVCO

%% Particle tripod 836
% location of instruments wrt dock measurements
% measurements use red armpit as reference; distances are in m

p(1).n = 'R';
p(1).ln = 'red armpit';
p(1).xm = 0;
p(1).ym = 0;
p(1).zm = 0;

p(2).n = 'G';
p(2).ln = 'green armpit';
p(2).xm = -.945;
p(2).ym = -1.645;
p(2).zm = 0;

p(3).n = 'B';
p(3).ln = 'blue armpit';
p(3).xm = .93;
p(3).ym = -1.645;
p(3).zm = 0;

p(4).n = 'PB';
p(4).ln = 'Dual-axis Pencil Beam 3937';
p(4).xm = .06;
p(4).ym = -1.645-.56;
p(4).zm = 1.10;
[r,az]=pcoord(-.035,-.55);
p(4).az = az;

p(5).n = 'FB';
p(5).ln = 'Fan Beam';
p(5).xm = 0;
p(5).ym = -1.645+.51;
p(5).zm = 0.65;
[r,az]=pcoord(-.05,-1.12);
p(5).az = az;

p(6).n = 'ADCP';
p(6).ln = 'ADCP 159';
p(6).xm = .155;
p(6).ym = -1.645+.975;
p(6).zm = 2.06;
%[r,az]=pcoord(.23,.235); % orientation of beam 3
% notes not clear...try
[r,az]=pcoord(.23,.135); % orientation of beam 3
p(6).az = az;

p(7).n = 'Cam';
p(7).ln = 'Camera 2';
p(7).xm = -.48;
p(7).ym = -1.645+.975;
p(7).zm = .96;
p(7).az = 180; % orientation of camera view

p(8).n = 'Xmiss';
p(8).ln = 'Transmissometer 794';
p(8).xm = mean([.615,.745]);
p(8).ym = -1.645+mean([.48,.275]);
p(8).zm = .79;
p(8).az = NaN;

% calculate the midpoint of the blue/yellow leg
p(9).n = 'X';
p(9).ln = 'Midpoint of blue/red leg';
p(9).ln = 'Diver siting';
p(9).xm = mean( [p(3).xm p(1).xm]);
p(9).ym = mean( [p(3).ym p(1).ym]);
[r,az]=pcoord(p(2).xm-p(9).xm,p(2).ym-p(9).ym)
p(9).az = az;

%% relocate origin and rotate
if(1)
   % reference everything to fan location
   iref = 5;
   xoff = p(iref).xm;
   yoff = p(iref).ym;
   for i=1:length(p)
      p(i).xm=p(i).xm-xoff;
      p(i).ym=p(i).ym-yoff;
   end
   % rotate tripod locations wrt to ADCP az
 
   adcp_az = 158; % recorded value during deployment
   %adcp_az = 128; % guess if ADCP compass correction sign was wrong
   ref_az = adcp_az;
   rot = -p(iref).az+ref_az+90 - 6; %-6 allows to overlay sidescan
%  -90 puts matlab 0 degrees at cartesian 0
   % rot = -adcp_az +90;
   for i=1:length(p)
      [r,az]=pcoord(p(i).xm,p(i).ym);
      azr = az+rot;
      [p(i).xm,p(i).ym]=xycoord(r,azr);
      p(i).az=p(i).az+rot+360*(p(i).az<0);
   end
else
   iref = 1;
   ref_az = 0;
end
%% plot sonar tripod
figure(1); clf
xoff = .05;
yoff = .05;
for i=4:length(p)
   plot(p(i).xm,p(i).ym,'x')
   hold on
   text(p(i).xm+xoff,p(i).ym+yoff,p(i).n)
end
% plot legs
lcolor = [1 0 0;0 1 0;0 0 1];
for i=1:3
   h=plot(p(i).xm,p(i).ym,'o')
   set(h,'color',lcolor(i,:))
   hold on
   text(p(i).xm+xoff,p(i).ym+yoff,p(i).n)
end
% plot instruments with vectors for orientation
for iv=4:7
   h=arrow(p(iv).xm,p(iv).ym,.5,p(iv).az);
   set(h,'linestyle','--','color','c');
end
for iv=9
   h=arrow(p(iv).xm,p(iv).ym,2,p(iv).az);
   set(h,'linestyle','--','color',[.4 1 .4]);
end
% text
yval = 2.2;
if(1)
   fprintf(1,'t836_plot_rot.m\n')
   fprintf(1,'Tripod oriented to %s with az = %5.1f\n',...
      p(iref).ln,ref_az);
   for iv=[4:7 9],
      yval = yval-.2;
      fprintf(1,'%s, az = %5.1f\n',p(iv).ln,p(iv).az);
   end
   fprintf(1,'\n')
   for iv = [1:8]
      fprintf(1,'%s \nx y z: %f %f %f az: %f\n',...
         p(iv).ln,p(iv).xm,p(iv).ym,p(iv).zm,p(iv).az)
   end
end
axis square
axis([-2 2 -2 2])
title('Particle Tripod 836')