%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% PURPOSE: Plot measured ranges to the beacons.  This file needs only the tof's
%          from the ps8000.
%
% AUTHOR:  Rob McEwen
% DATE:    01/02/26
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
  if exist('dataPS8000') ~= 1, return, end;

  if exist('lblgrid') ~= 1, 
    disp(' lblgrid array is not present.')
    return
  end;
  
  XmitChnl = 4;               %Receive channel number plus one
  
  str = [' ',titleStr,' Plotted on ',myDate];
  PaperPosn = [.5 .5 3.75 2.5];
  FigurePosn = [5 30 815 760];
%
% Find the channels that have data. indxCh is an array that contains the channel
% numbers of channels with data, plus one.  The plus one is because the first
% column of the dataPS8000 array is time.  
%
% numChan is the number of channels with data.
%  
% bcnNum(i) is the beacon number for the i_th channel.
%
  numChan = 0;
  for i = 2:11,
    if( norm(dataPS8000(:,i)) ~= 0 ) 
      numChan = numChan+1;
      indxCh(numChan,1) = i; 
      if( i~= XmitChnl ),       %The interrogation channel (3)
		                %does't have a corresponding beacon.
	bcnNum(numChan) = find( lblgrid(bcnIndx,:) == ( indxCh(numChan) - 1 ) );
      end
    end
  end

  nstrips = numChan;
  rangeEst = 0*ones(length(dataNav),numChan);
  angleEst = 0*ones(length(dataNav),numChan);
  
  if exist( 'ps8fig' ) ~= 1
    ps8fig = figure;
    winpos( position(1,:) );
  else
    figure(ps8fig);
  end
  set( ps8fig, 'Name', 'PS8000 Plot; Ranges to Beacons', ...
	       'DeleteFcn', 'clear ps8fig' );

  tfinal = max(dataPS8000(:,1));
  for i = 1:numChan,
    %
    % indx selects nonzero tof's from the array.
    %
    indx = find( dataPS8000(:,indxCh(i) ));
    if( indxCh(i) ~= XmitChnl ), 
      %
      % Compute the slant range to the beacon as measured by the Edgetech:
      %
      range = (dataPS8000( indx, indxCh(i) ) - ...
	       lblgrid(totIndx,bcnNum(numChan)) )*sos/2;
      %
      % Compute the slant range to the beacon from the navigation data.  When
      % the lbl is in the loop, this should be identical to "range", above.
      %
      % First loop through the time array (first column of dataPS8000) and
      % identify the index of the closest time value in dataNav.
      %
      % clear angleEst;
      %for j=1:length(indx),
      for j=1:length(dataNav),

	if 0
	[minval, ix ] = min( abs( dataPS8000( indx(j), 1 ) - dataNav(:,1) ) );
	if minval > 1, 
	  disp('Danger: Large time difference between Nav and ps8000')
	end
	indxNav(j,1)  = ix;
	end

	indxNav(j,1)  = j;
	%
	% Find the range (r) from the vehicle (V) to the beacon (B)
        % coordinitized in the Earth (E) frame:
	%
	r_V_B_E = [lblgrid(northIndx,bcnNum(i)) - (dataNav(indxNav(j),mPos_x))
                   lblgrid(eastIndx, bcnNum(i)) - (dataNav(indxNav(j),mPos_y))
                   lblgrid(depthIndx,bcnNum(i)) - (dataNav(indxNav(j),mDepth))];
	rangeEst(j,i) = norm(r_V_B_E);
	%
	% Find the relative angle to the beacon in vehicle coordinates.
	% This would be more properly done in all 3 dimensions, using the
        % 3x3 direction cosine matrix T_B_E.  But, for a simple first cut, I'll
        % just ignore pitch and roll, and compute only the vehicle-referenced
        % heading to each beacon.  Anyway, we can assume that pitch and roll are
        % small for steady-state, straight-and-level flight.
	%
	psi = dataNav(indxNav(j),mPsi);
	cpsi = cos(psi);
	spsi = sin(psi);
	T_B_E = [cpsi spsi
		-spsi cpsi];
	r_V_B_B = T_B_E * r_V_B_E(1:2);  %Beware of the switch to 2 dimensions 
        %r_V_B_B = r_V_B_B/norm(r_V_B_B); %Make it into a unit vector
	%psiB = asin(r_V_B_B(2));
	%if( r_V_B_B(1) < 0. ), psiB = pi - psiB; end   %Handle the quadrants
	psiB = atan2(r_V_B_B(2),r_V_B_B(1));
	angleEst(j,i) = psiB;
      end
    end %usegrid
    
    end
  
    polar( angleEst, rangeEst, '.' )
    grid on;
    zoom on;
    title(['Beacon Locations in Body-Fixed Polar Coords, Meters, Deg ',str]);

return
