%
% Relys on idt.northings, eastings and Psi.  These are computed and added to
% the idt struct in deltaT.m. 
% 
  lstr={};
  if exist( 'bergFigPtr' ) ~= 1
    bergFigPtr= figure;
    set( bergFigPtr, 'Name', 'Estimated Berg Location', 'DeleteFcn', 'clear bergFigPtr' );
    set(gcf,'position',position)
    bergFontsize=20;
    bergFontweight='normal';
    set(gca,'fontsize',bergFontsize,'fontweigh',bergFontweight)
    set(gcf,'defaultaxesfontsize',bergFontsize,'defaulttextfontsize', ...
	    bergFontsize)
    bergAxesPtr = gca;
  else
    figure(bergFigPtr);
  end
%
% Filter out all the time points from the WaypointWallLog that are not
% strictly increasing.  This happens each time a new WaypointWall starts. 
%
  bergIndx = find(diff(wpw.data(:,1)));

  bergPlotPtrVeh = plot3(wpw.data(:,wpw.eastings), wpw.data(:,wpw.northings),...
			 wpw.data(:,wpw.depth), 'b.');
  hold on
  grid on
  lstr(length(lstr)+1,1) = {'Vehicle'};
  
  beamAngle_B = (wpw.data(:,wpw.beamIndxMin) - 150)*pi/180;

  beam_B(1,1,:) = cos( beamAngle_B .* wpw.data(:,wpw.minBeamRange));
  beam_B(2,1,:) = sin( beamAngle_B .* wpw.data(:,wpw.minBeamRange));
  beam_B(3,1,:) = 0 .* wpw.data(:,wpw.minBeamRange);
  
  phi   = wpw.data(:,wpw.phi);
  theta = wpw.data(:,wpw.theta);
  psi   = wpw.data(:,wpw.psi);
  
  cpsi   = cos( psi );
  spsi   = sin( psi );
  ctheta = cos( theta );
  stheta = sin( theta );
  cphi   = cos( phi );
  sphi   = sin( phi );
  %
  % Form the direction cosine matrix that recoordinatizes a vector from the 
  % body frame (B) into the local-level-local-vertical (LV) navigation 
  % frame, which is (North, East, Down).  See Fossen, p.10, Eqn 2.11.
  % Here we will assume the LV frame is intertially fixed and thus
  % also a Newtonian (N) frame.
  %
  % N is the inertially fixed, or Newtonian frame.
  %
  % The transformation T_B_N, is 3-2-1 Euler.  The matrix below, T_N_B, is
  % the transpose.
  %
  T_N_B = zeros(3,3,length(wpw.data));     %Initialize it.
  %
  T_N_B(1,1,:) =  cpsi.*ctheta; 
  T_N_B(2,1,:) =  spsi.*ctheta; 
  T_N_B(3,1,:) = -stheta; 
  
  T_N_B(1,2,:) = -spsi.*cphi + cpsi.*stheta.*sphi; 
  T_N_B(2,2,:) =  cpsi.*cphi + sphi.*stheta.*spsi; 
  T_N_B(3,2,:) =  ctheta.*sphi; 

  T_N_B(1,3,:) =  spsi.*sphi + cpsi.*cphi.*stheta; 
  T_N_B(2,3,:) = -cpsi.*sphi + stheta.*spsi.*cphi;
  T_N_B(3,3,:) =  ctheta.*cphi; 
  
  for( i=1:length(wpw.data) )
    vehicle_N = [wpw.data(i,wpw.northings);
		 wpw.data(i,wpw.eastings);
		 wpw.data(i,wpw.depth)];
    beam_N(:,1,i) = T_N_B(:,:,i) * beam_B(:,1,i) + vehicle_N;
  end
  
  beamAngle_N = beamAngle_B + wpw.data(:,wpw.psi);
  
   bergPlotPtrW = ...
       plot3( sin( beamAngle_N ) .* wpw.data(:,wpw.minBeamRange)  +  ...
	      wpw.data(:,wpw.eastings),...
              cos( beamAngle_N ) .* wpw.data(:,wpw.minBeamRange)  +  ...
	      wpw.data(:,wpw.northings),...
	      wpw.data(:,wpw.depth), 'r.');


   
   
   
   
   set(gca,'zdir','reverse');
  lstr(length(lstr)+1,1) = {'Minimum Range'};

  beamAngle_B = (wpw.data(:,wpw.beamIndxLS) - 150)*pi/180;
  beamAngle_N = beamAngle_B + wpw.data(:,wpw.psi);

   bergPlotPtrWLS = ...
      plot3( sin( beamAngle_N ) .* wpw.data(:,wpw.rangeLS)  +  ...
	     wpw.data(:,wpw.eastings),...
             cos( beamAngle_N ) .* wpw.data(:,wpw.rangeLS)  +  ...
	     wpw.data(:,wpw.northings),...
             wpw.data(:,wpw.depth),'g.');
  lstr(length(lstr)+1,1) = {'Least Squares'};

  beamAngle_B = (wpw.data(:,wpw.beamIndxLSMedian) - 150)*pi/180;
  beamAngle_N = beamAngle_B + wpw.data(:,wpw.psi);

   bergPlotPtrWLSM = ...
      plot3( sin( beamAngle_N ) .* wpw.data(:,wpw.rangeLSMedian)  +  ...
	     wpw.data(:,wpw.eastings),...
             cos( beamAngle_N ) .* wpw.data(:,wpw.rangeLSMedian)  +  ...
	     wpw.data(:,wpw.northings),...
             wpw.data(:,wpw.depth),'m.');
  lstr(length(lstr)+1,1) = {'Least Squares with Median Filter'};
  
  legend(lstr, 'location','best')

    set(gca,'xticklabel',cellstr(num2str(get(gca,'xtick')','%d')))
    set(gca,'yticklabel',cellstr(num2str(get(gca,'ytick')','%d')))
  % if( strcmp(idt.dataSource,'DynamicControlLog') & ~idt.computeValues ) 
  %   northingsWall = idt.northings + cos(idt.Psi)
  %   bergPlotPtr = plot(dataDC(:,1), dataDC(:,rangeToWall),'x');
  % else
  %   fprintf('wallPlot: Post-processing not implemented.\n');
  % end
  grid on
  xlabel('Eastings, meters');
  ylabel('Northings, meters');
  zlabel('Depth, meters')
  title(['Estimates of Berg Location ', titleStr]);

