%
% PURPOSE: Check Least-Squares computation of 2016.109.01.
%
if( exist( 'movingSideSonarFig' ) ~= 1 )
    movingSideSonarFig = figure;
    %set(gcf,'position',npposition)
    set(gcf, 'Name', 'twoSideSonar', 'DeleteFcn', 'clear movingSideSonarFig' );
    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(movingSideSonarFig);
    catch
        %
        % The figure has apparently been deleted, but the
        % handle remains.  Delete the handle and start over.
        %
        clear movingSideSonarFig
        movingSideSonarFig = figure;
    end
end


xline = [-50:50]';

j=0;
first = 1;
soln = [ [];[]; [] ];
for( i=11360:11370 )
  j=j+1;
  indx = find(idt.data(i,5:124));
  beamAngle_B = (indx-150)*pi/180;
  x = idt.data(i,indx+4).*cos(beamAngle_B);
  y = idt.data(i,indx+4).*sin(beamAngle_B);
  x=x';
  y=y';
  A = [x ones(length(x),1)];
  
  soln1  =  (A'*A)\(A'*y);
  m = soln1(1);
  b = soln1(2);
  soln   = [ soln [soln1; i] ];

  yline = m*xline + b;
  
  if( first )
    hSonar=plot(y,x, 'k*');
    title('Vehicle, Top View. $\hat{x}$ Up, $\hat{y}$ Right','interpreter','latex')
    grid on
    hold on
    hLine=plot(yline,xline, 'b');
    %plot(0,0,'o','linewidth',5)
    [x_veh y_veh]=circle(0,0,3);
    fill(y_veh, x_veh,'y')
    axis equal
    set(gca,'xlim',[-100 100],'ylim',[-100 100]);
    hold off
  else
    hSonar.XData = y;
    hSonar.YData = x;
    hLine.XData = yline;
    hLine.YData = xline;
  end
  
  %input(sprintf('Paused at t = %.2f, i=%d',idt.data(i,1),i));

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


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

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

end

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