%
% 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( '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=1;
jj = 1;
first = 1;
firstFwd = 1;
soln = [ [];[]; [] ];

veh_dia = 3;  % Exaggerated for plot
veh_len = 20; 
rud_len = 5;  % Length of rudder in plot
exag    = 5;  % deltaR angle exaggeration for plot
%
% Wall present flag.  But, it's in dataDC, which is annoying.
%
% indxf = find(logical(bitand(dataDC(:,newEasting),2^6)));
%
% Interpolate to idt
%
% 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.
%

%2016.335.04 Sim
%for( i=indxfn2(4000,ta.data(:,1)):indxfn2(4700,ta.data(:,1)))
%
%2016.335.03
%for( i=10000:12000 )   
%
% 2016.335.04
%for( i = [[indxfn2(3400,idt.data(:,1)):indxfn2(3990,idt.data(:,1))] ...
%         [indxfn2(4475,idt.data(:,1)):indxfn2(5090,idt.data(:,1))]] )
%
%2016.109.01 
%for( i = [indxfn2(3620,idt.data(:,1)):indxfn2(3640,idt.data(:,1))] )
%for( i = [indxfn2(3203,idt.data(:,1)):indxfn2(3640,idt.data(:,1))] )

if( strcmp(DirName,'2017.025.03'))
  domain = [indxfn2(4400,idt.data(:,1)):indxfn2(5000,idt.data(:,1))];
end

if( strcmp(DirName,'2017.030.00'))
 domain = [indxfn2(3750,idt.data(:,1)):indxfn2(3900,idt.data(:,1))];
% domain = [indxfn2(4450,idt.data(:,1)):indxfn2(4500,idt.data(:,1))];
end

if( strcmp(DirName,'2017.030.01'))
% domain = [indxfn2(3650,idt.data(:,1)):indxfn2(3850,idt.data(:,1))];
  domain = [indxfn2(3800,idt.data(:,1)):indxfn2(3850,idt.data(:,1))];
end

if( strcmp(DirName,'2017.058.02'))
% domain = [indxfn2(3650,idt.data(:,1)):indxfn2(3850,idt.data(:,1))];
  domain = [indxfn2(4470,idt.data(:,1)):indxfn2(4480,idt.data(:,1))];
end

if( strcmp(DirName,'2017.190.07'))
  domain = [indxfn2(3000,idt.data(:,1)):indxfn2(3250,idt.data(:,1))];
end

for( i = domain )

  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;
  
  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
%
% Include forward sonar in least-squares computation?
%
  includeFwd = 0;
  if( includeFwd )
    x = [x;xFwd];
    y = [y;yFwd];
  end
  
  if( isempty(y) || isempty(x) ) 
    continue;
  end
  %
  % Solve for the least-squares fit here in Matlab:
  %
  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.
  %
  %if( i>2 && idt.data(i,1) > wpw.data(j,1) && idt.data(i-1,1) <= wpw.data(j,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( dataDC(jj,1) < idt.data(i-1,1) && jj < length(dataDC(:,1)) )
      jj=jj+1;
    end
    %
    % Compute the line using the raw least-squares solution computed by the
    % vehicle:
    %
    % 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 ];

    %
    % Compute the line using the median-filtered least squares soln from the
    % vehicle.
    %
    psiNormalData = (wpw.data(j,wpw.beamIndxLSMedian)-150 - 90)*pi/180;
    normalMagData = wpw.data(j,wpw.rangeLSMedian);

    ylineData = tan(psiNormalData)*xline + normalMagData/cos(psiNormalData);
    wallData = [-normalMagData*sin(psiNormalData) normalMagData*cos(psiNormalData) 0];
		
    
    normalData = [ 0 0 0;
		  wallData];
    %
    % Replace the Matlab soln above with the minimum-beam solution from the
    % vehicle:
    %
    psiNormal = (wpw.data(j,wpw.beamIndxMin)-150 - 90)*pi/180;
    normalMag = wpw.data(j,wpw.minBeamRange);

    yline = tan(psiNormal)*xline + normalMag/cos(psiNormal);
    wall  = [-normalMag*sin(psiNormal) normalMag*cos(psiNormal) 0];
    
    normal = [ 0 0 0;
	       wall];
    %
    % Rudder angle:
    deltaR = [ 0                                        -veh_len/2;
	       -rud_len*sin(exag*dataDC(jj,cDeltaR))  -veh_len/2-rud_len*cos(exag*dataDC(jj,cDeltaR))];
    
    j = j+1;
  else
    ylineData  = [];
    wallData   = [];
    normalData = [];
  end
  
  
  
  cmap = colormap;
  
  if( first )
    %hSonar=plot3(y,x, 0*x, 'k*');     %3D
    hSonar=plot(y,x, 'k*');
    grid on
    hold on
    if( indxFwd )
      %hFwdSonar = plot3(yFwd,xFwd, 0*xFwd,'r*');    %3D
      hFwdSonar = plot(yFwd,xFwd,'r*');
    else
      %hFwdSonar = plot3(0,0,0,'r*');          %3D
      hFwdSonar = plot(0,0,'r*');
    end
    % hLine=plot3(yline,xline, 0*xline, 'b');                          %3D
    % hnormal=plot3(normal(:,2),normal(:,1), normal(:,3), 'r');        %3D
    % hWall=plot3(wall(2),wall(1), wall(3),'r*');                      %3D
    hLine=plot(yline,xline, 'b');
    hnormal=plot(normal(:,2),normal(:,1), 'r');
    hWall=plot(wall(2),wall(1),'r*');
    
    if( ylineData )
      % hLineData   = plot3(ylineData,xline, 0*xline, 'b--');                            %3D
      % hnormalData = plot3(normalData(:,2),normalData(:,1),normalData(:,3), 'r--'); 
      % hWallData   = plot3(wallData(2),wallData(1), wallData(3),'m*');                  %3D
      hLineData   = plot(ylineData,xline, 'b--');
      hnormalData = plot(normalData(:,2),normalData(:,1),'r--');
      hWallData   = plot(wallData(2),wallData(1),'m*');
    else
      % hLineData   = plot3(0,0,0, 'b--');     %3D
      % hnormalData = plot3(0,0,0, 'r--');     %3D
      % hWallData   = plot3(0,0,0, 'm*');      %3D
      hLineData   = plot(0,0, 'b--');
      hnormalData = plot(0,0, 'r--');
      hWallData   = plot(0,0, 'm*');
    end

    if( 0 )
      %-----------------------------------3D Vehicle -----------------------------%
      %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')
    else
      %-----------------------------------2D Vehicle -----------------------------%
      tt=-pi:0.01:pi;
      x0=0; % x0,y0 ellipse centre coordinates
      y0=0;
      xveh = x0 + veh_dia/2*cos(tt);
      yveh = y0 + veh_len/2*sin(tt);
      hveh = plot(xveh,yveh);
      fill(xveh,yveh,'y');
      hrud = plot(deltaR(:,1), deltaR(:,2),'linewidth',2);
      axis equal
      set(gca,'xlim',[-100,100]);
      set(gca,'ylim',[-50,100]);
      hTxt=text(-40,-40,sprintf('Time = %.1f',idt.data(i,1)));
    end
    first = 0;
    hold off
  else
    hSonar.XData = y;
    hSonar.YData = x;
    %hSonar.ZData = 0*x;              %3D
    if( indxFwd )
      hFwdSonar.XData = yFwd;
      hFwdSonar.YData = xFwd;
      %hFwdSonar.ZData = 0*xFwd;              %3D
    else
      hFwdSonar.XData = [];
      hFwdSonar.YData = [];
      %hFwdSonar.ZData = [];              %3D
    end
    hLine.XData = yline;
    hLine.YData = xline;
    %hLine.ZData = 0*xline;              %3D
    hWall.XData = wall(2);
    hWall.YData = wall(1);
    %hWall.ZData = wall(3);              %3D
    hnormal.XData = normal(:,2);
    hnormal.YData = normal(:,1);
    %hnormal.ZData = normal(:,3);              %3D
    hrud.XData = deltaR(:,1);
    hrud.YData = deltaR(:,2);
    if( ylineData )
      hLineData.XData = ylineData;
      hLineData.YData = xline;
      %hLineData.ZData = 0*xline;              %3D
      hWallData.XData = wallData(2);
      hWallData.YData = wallData(1);
      %hWallData.ZData = wallData(3);              %3D
      hnormalData.XData = normalData(:,2);
      hnormalData.YData = normalData(:,1);
      %hnormalData.ZData = normalData(:,3);              %3D
    else
      hLineData.XData = [];
      hLineData.YData = [];
      %hLineData.ZData = [];              %3D
      hWallData.XData = [];
      hWallData.YData = [];
      %hWallData.ZData = [];              %3D
      hnormalData.XData = [];
      hnormalData.YData = [];
      %hnormalData.ZData = [];              %3D
    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
  
  if( strcmp(DirName,'2017.030.01') && idt.data(i,1) > 3828 || ...
      strcmp(DirName,'2017.058.02') && idt.data(i,1) > 4470 )
    input(sprintf('Paused at t = %.2f, i=%d',idt.data(i,1),i));
  end
  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(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)']
