%
% 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
%
% Let the games begin:
  tic  
  fprintf('Beginning the patch plot.  Please wait...\n');
% axis equal;
  grid on
  set(gca,'zdir','reverse');
%
% Begin Stevesy's code:

V = [];
FVCD = [];
for x = 0:1
    for y = 0:1
        for z = 0:1
            V(end+1,:) = [x y z];
            FVCD(end+1,:) = [0.5 0.5 z];
        end
    end
end

F =[1 3 4 2 1;
    5 7 8 6 5;
    1 2 6 5 1;
    3 4 8 7 3;
    1 5 7 3 1;
    4 8 6 2 4];

minX = min(data(:,1));
minY = min(data(:,3));
minZ = min(data(:,5));
maxZ = max(data(:,6));
total_faces = 0;
cHaxby = haxby(256);

%%
tic

%Preallocate memory.  length of V is 8.  Every entry in 'data' is a cube.
%V_all is every vertex; there are 8 per cube.  Three columns for the
%cartesian position.

oct.V_all    = zeros(length(data(:,1))*length(V(:,1)),3);
oct.FVCD_all = oct.V_all;

% 7:12 in data is the face-visble flag. Since each cube has 6 faces there
% are 6 columns.  So we only need to preallocate for the ones that are
% visible.  Each face is defined by closed square, so it's the four
% points.  It's the index of V_all of each of the points in the square
% plus the starting point.  5th column may be unnecessary.
oct.F_all    = zeros(sum(sum(data(:,7:12))),5);

nv = 0;
nf = 0;

for i = 1:length(data(:,1))
    V_out = V;
    V_out(:,1) = data(i,1)+V_out(:,1)*(data(i,2)-data(i,1)); %-minX;
    V_out(:,2) = data(i,3)+V_out(:,2)*(data(i,4)-data(i,3)); %-minY;
    V_out(:,3) = data(i,5)+V_out(:,3)*(data(i,6)-data(i,5));
    F_out = F(data(i,7:12)'==1,:);

    FVCD_out = cHaxby(1+floor((V_out(:,3)-minZ)*255/(maxZ-minZ)),:);
    
    total_faces = total_faces + length(F_out(:,1));
    %patch('Vertices',V_out,'Faces',F_out,'FaceVertexCData',FVCD_out,'FaceColor','interp','EdgeColor','k','LineStyle',':')
    %hold on;
    %
    % Big long matrix V_all is all the verticies of all the cubes. And now
    % V_out is the verticies of one cube.
    oct.V_all   (nv+1:(nv+length(V_out(:,1))),:) = V_out;
    oct.FVCD_all(nv+1:(nv+length(V_out(:,1))),:) = FVCD_out;
    % Adding the faces for the current cube to the matrix for all the
    % faces:  Added nv because F_out is only for the small matrix (V_out), but we
    % need to index on the large matrix (oct.V_all).
    oct.F_all   (nf+1:(nf+length(F_out(:,1))),:) = F_out + nv;
    % Number of vertices we added to the complete matrix:
    nv = nv+length(V_out(:,1));    %length of V_out should be 8 every time.
    nf = nf+length(F_out(:,1));    %Variable based on the number of
                                   %visible faces.
end
patch('Vertices',oct.V_all,'Faces',oct.F_all,'FaceVertexCData',oct.FVCD_all,...
      'FaceColor','interp','EdgeColor','k','LineStyle',':')
fprintf('Patch finished in %f seconds.\n',toc);
fprintf('Start plotting vehicle track and sonar data:\n');
%
% 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),...
 bergPlotPtrVeh = plot3(wpw.data(:,wpw.northings), wpw.data(:,wpw.eastings),...
			 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);

  tic
  
   bergPlotPtrW = ...
       plot3( ...
              cos( beamAngle_N ) .* wpw.data(:,wpw.minBeamRange)  +  ...
	      wpw.data(:,wpw.northings),...
	      sin( beamAngle_N ) .* wpw.data(:,wpw.minBeamRange)  +  ...
	      wpw.data(:,wpw.eastings),...
	      wpw.data(:,wpw.depth), 'r.');

  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( ...
             cos( beamAngle_N ) .* wpw.data(:,wpw.rangeLS)  +  ...
	     wpw.data(:,wpw.northings),...
	     sin( beamAngle_N ) .* wpw.data(:,wpw.rangeLS)  +  ...
	     wpw.data(:,wpw.eastings),...
             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( ...
             cos( beamAngle_N ) .* wpw.data(:,wpw.rangeLSMedian)  +  ...
	     wpw.data(:,wpw.northings),...
             sin( beamAngle_N ) .* wpw.data(:,wpw.rangeLSMedian)  +  ...
	     wpw.data(:,wpw.eastings),...
             wpw.data(:,wpw.depth),'m.');
  lstr(length(lstr)+1,1) = {'Least Squares with Median Filter'};
  fprintf('Finished plotting sonar data in %f seconds.\n',toc);
  fprintf('Axis labels, drawnow and ''view'':\n');
  tic
  
  %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('Northings, meters');
  ylabel('Eastings, meters');
  zlabel('Depth, meters')
  title(['Estimates of Berg Location ', titleStr]);

  currAz = -30;
  currEl =  30;
  view(currAz,currEl)
  drawnow;
  fprintf('Finished drawing in %f seconds.\n',toc);
  fprintf('Finished the whole shebang in %f seconds.\n',toc);
  
