%
% PURPOSE: Check Least-Squares computation of 2016.109.01.
%

  MonitorPositions = get(0,'MonitorPositions');
  position = MonitorPositions(end,:);
  %position = get(0,'screensize');
  npposition(1:2) = position(1:2) + round([position(3)*.02 position(4)*.04]);
  npposition(3:4) = round([position(3)*.95 position(4)*.90]);
  position = npposition;


if( exist( 'movingSideSonarNFig' ) ~= 1 )
    movingSideSonarNFig = figure;
    set(gcf,'position',npposition)
    set(gcf, 'Name', 'twoSideSonar', 'DeleteFcn', 'clear movingSideSonarNFig' );
    set(gca,'fontsize',fontsize,'fontweigh',fontweight)
    set(gcf,'defaultaxesfontsize',fontsize,'defaulttextfontsize',fontsize)
    
    PauseButton_h = uicontrol('Style', 'pushbutton',...
                              'String', 'Click to Pause','FontSize',16,...
                              'BackgroundColor','g',...
                              'Position', [20 20 180 50 ],...
                              'Callback',@pause_Callback);
    % 
    % Beware that the string 'Click to Pause' is also the state.  If you change
    % this string, you must also change it in pause_Callback.m
    % and in the waitfor() below.
    %
else
    try
        figure(movingSideSonarNFig);
    catch
        %
        % The figure has apparently been deleted, but the
        % handle remains.  Delete the handle and start over.
        %
        clear movingSideSonarNFig
        movingSideSonarNFig = figure;
    end
end


xline = [-50:50]';

j=1;
k=1;
first = 1;
firstFwd = 1;
soln = [ [];[]; [] ];

%
% Wall present flag.  But, it's in dataDC, which is annoying.
%
% indxf = find(logical(bitand(dataDC(:,newEasting),2^6)));
%
% Interpolate to idt
%

%2016.335.04 Sim
%for( i=indxfn2(4000,ta.data(:,1)):indxfn2(4700,ta.data(:,1)))
%
% 2015.335.04
%
% I'm going to assume that the startTime and NegTrunc process has
% synchronized time the various logs.  For example, idt.data(indx,1) and
% idt_fwd(indx,1) will be within .2 seconds.
%
for( i = [[indxfn2(3400,idt.data(:,1)):indxfn2(3990,idt.data(:,1))] ...
         [indxfn2(4475,idt.data(:,1)):indxfn2(5090,idt.data(:,1))]] )
%for( i=10000:12000 )   %2016.335.03
%for( i=1:length(idt.data(:,1) ))
  indx           = find(idt.data(i,5:124));
  beamAngle_B    = (indx-150)*pi/180;

  % indxFwd         = indx( indx > 105 );
  % indxSide        = indx( indx <= 105 );
  % beamAngleFwd_B  = (indxFwd-150)*pi/180;
  % beamAngleSide_B = (indxSide-150)*pi/180;

  indxFwd         = find(idt_fwd.data(i,5:124));
  beamAngleFwd_B  = (indxFwd-60)*pi/180;
  indxSide        = indx;
  beamAngleSide_B = (indxSide-150)*pi/180;
  %
  % Set:
  %   indxSide = indx;
  %   beamAngleSide_B = beamAngle_B;.
  %
  if( indxFwd )
    xFwd = idt_fwd.data(i,indxFwd+4).*cos(beamAngleFwd_B);
    yFwd = idt_fwd.data(i,indxFwd+4).*sin(beamAngleFwd_B);
    x = idt.data(i,indxSide+4).*cos(beamAngleSide_B);
    y = idt.data(i,indxSide+4).*sin(beamAngleSide_B);
  else
    xFwd = [];
    yFwd = [];
    x = idt.data(i,indx+4).*cos(beamAngle_B);
    y = idt.data(i,indx+4).*sin(beamAngle_B);
  end

  if( isempty(y) || isempty(x) ) 
    continue;
  end
  x=x';
  y=y';
  A = [x ones(length(x),1)];
  
  soln1  =  (A'*A)\(A'*y);
  slope   = soln1(1);
  psiWall = atan(slope);
  yInter = soln1(2);
  %soln   = [ soln [soln1; i] ];

  yline = slope*xline + yInter;

  wall = [ -yInter*cos(psiWall)*sin(psiWall) yInter*(cos(psiWall))^2 0];

  normal=[ 0 0 0;
	   wall];

  %
  % Now find the line computed on board.
  %
  % i is the index into idt time.
  % j indexes wpw.
  % k indexes dataNav and dataDC.
  % idt.data(i,1) ~= wpw.data(j,1) ~= dataNav(k,1).
  %
  if( i>1 && idt.data(i,1) > wpw.data(j,1) && j < length(wpw.data(:,1)) )
    while( wpw.data(j,1) < idt.data(i-1,1) && j < length(wpw.data(:,1)) )
      j=j+1;
    end
    while( dataNav(k,1) < idt.data(i-1,1) && k < length(dataNav(:,1)) )
      k = k+1;
    end
    psiWallData = wpw.data(j,wpw.psiWall_B);
    yInterData  = wpw.data(j,wpw.yInterWall_B);

    ylineData = tan(psiWallData)*xline +  yInterData;
    wallData = [ -yInterData*cos(psiWallData)*sin(psiWallData) ...
		 yInterData*(cos(psiWallData))^2 0 ];
  
    normalData = [ 0 0 0;
		  wallData];
    j = j+1;
  else
    ylineData  = [];
    wallData   = [];
    normalData = [];
  end
  
  cmap = colormap;
  %
  % 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.
  %
  % N is the inertially fixed, or Newtonian frame.
  %
  % The transformation T_B_LV, is 3-2-1 Euler.  The matrix below, 
  % T_LV_B, is the transpose.
  %
  % See procNavData.m for the vectorized version, where phi, theta and psi
  % also depend on time, but the computation is without looping.
  %
  phi    = dataNav(k,mPhi);
  theta  = dataNav(k,mTheta);
  psi    = dataNav(k,mPsi);
  cpsi   = cos(psi);
  spsi   = sin(psi);
  ctheta = cos(theta);
  stheta = sin(theta);
  cphi   = cos(phi);
  sphi   = sin(phi);
  %Initialize it.
  T_LV_B = zeros(3,3);  
  %
  T_LV_B(1,1) =  cpsi.*ctheta; 
  T_LV_B(2,1) =  spsi.*ctheta; 
  T_LV_B(3,1) = -stheta; 

  T_LV_B(1,2) = -spsi.*cphi + cpsi.*stheta.*sphi; 
  T_LV_B(2,2) =  cpsi.*cphi + sphi.*stheta.*spsi; 
  T_LV_B(3,2) =  ctheta.*sphi; 

  T_LV_B(1,3) =  spsi.*sphi + cpsi.*cphi.*stheta; 
  T_LV_B(2,3) = -cpsi.*sphi + stheta.*spsi.*cphi;
  T_LV_B(3,3) =  ctheta.*cphi; 

  T_B_LV = T_LV_B';
  
  %
  % Now convert everything from B to LVLH (or NED).  The origin of the
  % reference frame is the C.G. of the vehicle.
  %
  % Suppose Beams_B = [Beam0 Beam1 Beam2 ...] where Beami is 3x1, and so
  % the matrix is 3x120.  Then
  %
  % Beams_LV = T_LV_B * Beams_B 
  %
  % and
  %
  % Beams_LV' = Beams_B' * T_B_LV
  %
  Side_LV = [x y 0*y]*T_B_LV;
  x_LV = Side_LV(:,1);
  y_LV = Side_LV(:,2);
  z_LV = Side_LV(:,3);
  
  if( indxFwd )
    Fwd_LV  = [ xFwd yFwd 0*yFwd ]*T_B_LV;
    xFwd_LV = Fwd_LV(:,1);
    yFwd_LV = Fwd_LV(:,2);
    zFwd_LV = Fwd_LV(:,3);
  end

  line_LV  = [xline yline 0*yline]*T_B_LV;
  xline_LV = line_LV(:,1);
  yline_LV = line_LV(:,2);
  zline_LV = line_LV(:,3);

  normal_LV = normal*T_B_LV;
  wall_LV   = wall*T_B_LV;
  
  lineData_LV   = [xlineData ylineData 0*ylineData]*T_B_LV;
  normalData_LV = normalData*T_B_LV;
  wallData_LV   = wallData*T_B_LV;
  
  return
  
  
  if( first )
    %
    % Swap the x and y vectors, and flip the sign on z to switch from
    % Matlab's z-up plot frame to NED with z down.
    %
    hSonar=plot3(Side_LV(:,2), Side_LV(:,1), -Side_LV(:,3), 'k*');
    grid on
    hold on
    if( indxFwd )
      hFwdSonar = plot3(Fwd_LV(:,2), Fwd_LV(:,1), -Fwd_LV(:,3), 'r*');
    else
      hFwdSonar = plot3(0,0,0,'r*');
    end
    hLine=plot3(line_LV(:,2),line_LV(:,1), -line_LV(:,3), 'b');
    hnormal=plot3(normal_LV(:,2),normal_LV(:,1), -normal_LV(:,3), 'r');
    hWall=plot3(wall_LV(2),wall_LV(1), -wall_LV(3),'r*');
    
    if( ylineData )
      hLineData   = plot3(lineData_LV(:,2), lineData_LV(:,1), -lineDataLV(:,3), 'b--');
      hnormalData = plot3(normalData_LV(:,2),normalData_LV(:,1), -normalData_LV(:,3), 'r--');
      hWallData   = plot3(wallData_LV(2),wallData_LV(1), -wallData_LV(3),'m*');
    else
      hLineData   = plot3(0,0,0, 'b--');
      hnormalData = plot3(0,0,0, 'r--');
      hWallData   = plot3(0,0,0, 'm*');
    end
    
    %plot(0,0,'o','linewidth',5)
    %[x_veh y_veh]=circle(0,0,3);
    %fill(y_veh, x_veh,'y')
    %Scale size up by two:
    [xveh yveh zveh]=ellipsoid( 0,0,0, 1, 15, 1, 100);
    
    %cdata = ones(length(zveh),1)*[1 1 0];  %Make it yellow
    %hveh = surf(xveh,yveh,zveh,cmap,'linestyle','none');
    %hveh = surf(xveh,yveh,zveh,zveh,'linestyle','none');
    %caxis([55 62]);
    hveh = surf(xveh,yveh,zveh,'linestyle','none','facecolor',cmap(60,:));
    hlight = light;
    hlight.Position = [0 -10 10];
    hlight.Style = 'local';
    %surf(xveh,yveh,zveh,'linestyle','none','facecolor','y');
    axis equal
    [xtail ytail ztail] = cylinder([15 15]*.0254*2, 100);
    %Scale and Displace upward by L/2:
    ztail = ztail*12*.0254*2 + 15;
    htail = surf(xtail,ytail,ztail,'linestyle','none','facecolor','b');
    set(gca,'xlim',[-100 100],'ylim',[-100 100], 'zlim',[-10 10]);
    rotate(htail, [1 0 0], 90);
    hTxt=text(-100,-100,10,sprintf('Time = %.1f',idt.data(i,1)));
    % hTxt=text(-100,-100,10,sprintf(['Time = %.1f\n'],...
    % 	     ['Range from Side =%.1f\nRange from Both'], ...
    %           idt.data(i,1), abs(cos(psiWall)*yInter), ...
    % 	      abs(cos(psiWallData)*yInterData)));
    title('Vehicle-Fixed Frame, Top View. $\hat{x}$ Ahead, $\hat{y}$ Right',...
	  'interpreter','latex')
    first = 0;
    hold off
  else
    hSonar.XData = y;
    hSonar.YData = x;
    hSonar.ZData = 0*x;
    if( indxFwd )
      hFwdSonar.XData = yFwd;
      hFwdSonar.YData = xFwd;
      hFwdSonar.ZData = 0*xFwd;
    else
      hFwdSonar.XData = [];
      hFwdSonar.YData = [];
      hFwdSonar.ZData = [];
    end
    hLine.XData = yline;
    hLine.YData = xline;
    hLine.ZData = 0*xline;
    hWall.XData = wall(2);
    hWall.YData = wall(1);
    hWall.ZData = wall(3);
    hnormal.XData = normal(:,2);
    hnormal.YData = normal(:,1);
    hnormal.ZData = normal(:,3);
    if( ylineData )
      hLineData.XData = ylineData;
      hLineData.YData = xline;
      hLineData.ZData = 0*xline;
      hWallData.XData = wallData(2);
      hWallData.YData = wallData(1);
      hWallData.ZData = wallData(3);
      hnormalData.XData = normalData(:,2);
      hnormalData.YData = normalData(:,1);
      hnormalData.ZData = normalData(:,3);
    else
      hLineData.XData = [];
      hLineData.YData = [];
      hLineData.ZData = [];
      hWallData.XData = [];
      hWallData.YData = [];
      hWallData.ZData = [];
      hnormalData.XData = [];
      hnormalData.YData = [];
      hnormalData.ZData = [];
    end
    hTxt.String=sprintf('Time = %.1f',idt.data(i,1));
    % hTxt.String=sprintf(['Time = %.1f\n'],...
    % 	     ['Range from Side =%.1f\nRange from Both'], ...
    %           idt.data(i,1), abs(cos(psiWall)*yInter), ...
    % 	      abs(cos(psiWallData)*yInterData));
    drawnow update
  end
  
  %input(sprintf('Paused at t = %.2f, i=%d',idt.data(i,1),i));

  pause(0.1)    
  waitfor( PauseButton_h, 'String','Click to Pause' );


  %
  % Check to see if the user has deleted the figure.  If so,
  % exit the loop
  try
    figure(movingSideSonarNFig);
  catch
    %
    % The figure has apparently been deleted, but the
    % handle remains.  Delete the handle and bust out.
    %
    clear movingSideSonarNFig;
    break;
  end

  %fprintf('End; i=%d\n',i);

end

%[ indx'  indx'-150 idt.data(i,indx+4)']
