% sonar_tripod_plot07.m  (from MVCO_PARTICLE_TRIPOD_PLOT)
% csherwood@usgs.gov- tweaked during reconstruction Sept 5, 2007
% modified 11/23/07  emontgomery to show more background computations and
%   get sorking on 836 measurements
%
%   This program draws the tripod as configured, and and rotates to allow
%   overlay on the fan sonar image for validation. 
% The overall objective is to rotate the fan image so that fan position under 
% the ADCP 0 is plotted as "up", so that ADCP vectors can be added (with 
% N is up) and make sense.  This is what the trot variable (also known as 
% fan_adcp offset) is used for. 
% The blank area of the fan image is centered on fan 180, since the
% head rotates from -170 to +170.
%
%  I haven't figured out a straightforward way to feed measurements from the 
%  tripod assembly drawings to the program and get the geometry right, so a 
%  version of this program should be made for each tripod with sonar on it.

%   This program is for tripod 836, deployed at MVCO Sept-Oct 2007

% calc and plot tripod info
sft = 1; % scale for tripod size
ipt = 1; % flag to make plot
% raw distances are relative to ADCP location
% raw directions are relative to laser line

% sorry, no guidance on how to compute this yet.  Should be something like:
%   ADCP beam 3 direction (Magnetic) - fan 0 direction (Magnetic) (+/-180?)
trot = 157 % original ADCP heading
% Use trot = 0 to get the geometry right initally, then rotate the image. 
% if you display a fan image made with fanadcp_off = 0, the tripod drawn by
% this program with trot=0 should overlay propery.
%trot = 0
% for tripod 836 fbrot =180 + fa  where fa=fan angle (atand(5/112.5))
fbrot = 182.5; % nominal fan beam orientation wrt to beam 3 

% ADCP is initially the origin
aloc = [0 0];
aname = {'ADCP'};
dloc = [3,0];
dname = {'Beam 3'};

% Measurements are taken from the (crummy) figure drawn on the dock.
% Distances are meters from point below the ADCP
% Angles are meaured wrt to the laser line (and may be relative to 0 or
% 180)- measurements are along and perpendicular to the laser line.
%  The laser reference was the line between the red foot and halfway
%     between the blue and green feet. 
%
% Below are computations to get from the tripod diagram to the ranges and 
% bearings from the adcp to other landmarks 
% Here I've defined laser line 0 to be in the direction of the red leg.
% nomenclature : b_ == bearing_from_to; r_ == range_from_to
b_adcpred=atand((164.5-97.5)/15.5);
r_adcpred=sqrt((164.5-97.5)^2 + 15.5^2);
b_adcpblue=atand((93-15.5)/97.5);
r_adcpblue=sqrt((93-15.5)^2 + 97.5^2);
b_adcpgreen=atand((94.5+15.5)/97.5);
r_adcpgreen=sqrt((15.5+94.5)^2 + 97.5^2);

r_adcpfan=sqrt((97.5-51)^2 + 15.5^2);
b_adcpfan=atand((97.5-51)/15.5);

r_adcppen=sqrt((97.5+56)^2 + (15.5+6)^2);
b_adcppen=atand((15.5+6)/(97.5+56));

% now input ADCP centric postions - (divide by 100 to make units meters)
%  assign to range and bearing matices for each entity
% tripod legs
triloc = [r_adcpgreen/100,-180+b_adcpgreen;...   % Green
          r_adcpblue/100,180-b_adcpblue;... % Blue
          r_adcpred/100,b_adcpred-90];  % Red
triname = {'G';'B';'R'};

% Imaging sonar (fan)
floc = [r_adcpfan/100,-90-b_adcpfan];
fname = {'Fan'};

% Pencil sonar
ploc = [r_adcppen/100,-180+b_adcppen];
pname = {'Pen'};

% use trot =0 initially to get the simplest view of the tripod
%  xycoord converts matlab's polar to geographic coordinates, including all 
%  necessary rotations to go from 0 is east to 0 is north (UP)
% The rotation is incorporated here- must be done before the offset.
[ax,ay] = xycoord(sft*aloc(:,1),aloc(:,2)+trot);
[fx,fy] = xycoord(sft*floc(:,1),floc(:,2)+trot);
[px,py] = xycoord(sft*ploc(:,1),ploc(:,2)+trot);
[trix,triy] = xycoord(sft*triloc(:,1),triloc(:,2)+trot);

% recenter on Fan beam by computing the x-y offset
offanx = ax-fx;
offany = ay-fy;
% set up some points to indicate the 0 lines for each sensor 
[a0x,a0y]=xycoord(1.0,0); % point directly away from fan beam
[fb0x,fb0y]=xycoord(1.0,fbrot+trot); % point directly away from fan beam
[p0x,p0y]=xycoord(1.0,fbrot+trot); % point directly away from pencil beam

if(ipt),
   %figure(3);clf 
   % draw adcp
   plot(ax+offanx,ay+offany,'xg');  
     hold on
        plot([ax+offanx a0x+offanx],[ay+offany a0y+offany],'--g');  
          hold on
    h = text(ax(1)+offanx,ay(1)+offany+.05,aname);
   set(h,'horizontalalignment','right');
   set(h,'verticalalignment','bottom');

   %Draw fan
   %plot(fx,fy,'r*');  %Original fan, relative to adcp at center
   %  h = text(fx-0.1,fy+.05,['ori' fname]);
   plot(fx+offanx,fy+offany,'xr');  % this should be a [0,0]
    h = text(fx(1)+offanx-0.05,fy(1)+offany+.05,fname);
   plot([0 fb0x],[0 fb0y],'--r') % fan beam zero
   
   %draw in pencil location
   plot(px+offanx,py+offany,'gd');  % in fan centric coords
   h = text(px(1)+offanx-0.1,py(1)+offany+.05,pname);
      
% plot new (fan centric) tripod location, 
   plot(trix+offanx,triy+offany,'ok')
   hold on
    plot([trix;trix(1)]+offanx,[triy;triy(1)]+offany,'--k')
   for ik=1:3,
      h = text(trix(ik)+offanx +.02,triy(ik)+offany-.05,triname{ik});
      set(h,'horizontalalignment','left');
   end
   
   
   %circle(1*sft,fx+offanx,fy+offany);
   %circle(2*sft,fx+offanx,fy+offany);
   %axis([-2 2 -2 2]);
   %axis square
end