function the_boxes = pick_from_segments(Y, X, A, S, n_segments, ...
					delta_t, box_size, data)
% function pick_from_segments(Y, X, A, S, n_segments, delta_t, box_size, data)
%
% Given a collection of segmented lines (given by their verticies), 
% travel along each segmented line, extracting boxes of a given
% size from the given image data, at given intervals along the path.
%
% X is the list of X coordinates of verticies
% Y is the list of Y coordinates of verticies
% A is the orientation angle at the corresponding vertex
% S is the list of segment numbers of verticies
% N_SEGMENTS is the number of segments
% DELTA_T is the spacing (in pixels) along the lines at which we
%         will extract boxes
% BOX_SIZE is the length of the side of the box that we will extract
% DATA is the image from which we are picking
%
%
% Example:
%
% Suppose we have two line segments, one with verticies 
% (a,b) (c,d) and (e,f), and the other with (g,h) (i,j).
% Then we would have:
%
% X = [a,c,e,g,i];
% Y = [b,d,f,h,j];
% S = [1,1,1,2,2];
% N_SEGMENTS = 2;
%
% 2002-07-02 Tobin Fricke <tobin@splorg.org>, Weizmann Institute of Science
% $Id: pick_from_segments.m,v 1.3 2002/07/21 08:41:23 tobin Exp $

colors = 'ymcrgbk';
boxes = 0;

% Process each segment

for n=1:n_segments,
  
  subplot(1,2,1), imagesc(data);
  % :: Select only those picks belonging to this segment

  disp(sprintf('Processing segment number %d',n));
  
  % Extract the information corresponding just to this group
  
  indicies = find(S==n);
  
  SX = X(indicies)';
  SY = Y(indicies)';
  SA = A(indicies)';
 
  % :: Display the picks, using a variety of colors

  hold on;
  plot(SY,SX,[ colors(mod(n,length(colors))+1) 'x' ]);
  hold off;

  % Drawing a line through a single point is ambiguous
  
  if (length(SX) < 2), continue; end
  
  % :: Fit a line to the points of a single segment 
  % The line will have the form y = mx+b.
  
  % <¡OBS!> This won't work for vertical or nearly-vertical segments
  % We should find a better way to paramaterize a line and fit it
  % to points to resolve this hazard.
  
  % This is the system of equations we wish to solve:
  %
  % [ y_0 ]     [ x_0   1.0 ]
  % [ y_1 ]     [ x_1   1.0 ]   [ m ]
  % [ ... ]  =  [ ...   1.0 ] x [ b ]
  % [ y_n ]     [ x_n   1.0 ]
  %
  %    Y     =       V            M
  
  V = ones(length(SX),2);
  V(:,1) = SX;
  
  % Solve in the least squares sense (see 'MLDIVIDE' and
  % http://icg.harvard.edu/~apm21b/Supplementary_Notes/sn_leastsq.pdf )
  
  M = V\SY; 
  
  m = M(1);  % slope
  b = M(2);  % intercept

  % Draw in the lines
  
  x(1) = min(SX);
  x(2) = max(SX);
  
  line([m*x(1)+b m*x(2)+b],[x(1) x(2)]);
  
  % :: Now project each point onto that line, in order to get their
  % linear coordinates in the segment.  We do this by shifting the
  % coordinate system (translating along the y axis) so that the
  % line of best fit goes through the origin.  Then we can project
  % the position vector of each 'pick' onto a unit vector along the
  % line, using the dot product.  Yay! Hurray for dot products!
  
  %SY = SY - b;              % shift of coordinate system by 'b'
  
  u = [1 m]/norm([1 m]);    % make a unit direction vector from 'm'
  
  SC = (SX*u(1))+((SY-b)*u(2)); % form the dot products 
      
  % now the points are sorted by their coordinate so that we can
  % interpolate along the path they form

  picks = zeros(length(SC),3);  % clear/allocate the matrix
  
  picks(:,1) = SC;
  picks(:,2) = SX;
  picks(:,3) = SY;
  picks(:,4) = SA;
  
  picks = sortrows(picks);      % sort by the coordinate along the line

  SC = picks(:,1);
  SX = picks(:,2);
  SY = picks(:,3);
  SA = picks(:,4);
  
  clear picks;
  
  % Now it's a simple matter to connect the dots
  
  hold on,
  plot(SY,SX,'c'); % colors(mod(n,length(colors))+1)
  plot(SY,SX, 'mx');                                     
  hold off;

  % :: Interpolation along the filament
  
  % Find the values of the "parameter" along the line segments
  
  ST = zeros(length(SX));
  for n=2:length(SX),
    ST(n) = ST(n-1) + sqrt((SX(n) - SX(n-1))^2 + (SY(n) - SY(n-1))^2);
  end
  
  % Now we "drive"!! (c.f. REM)
  
  n = 1;
  for t=0:delta_t:max(ST),
    % From the value of the "parameter" (t) we have to interpolate
    % an (x,y) position and also the orientation angle at that
    % point.  We'll just interpolate linearly.

    while ((t<ST(n)) | (t>ST(n+1))), n = n+1; end
   
    % ST(n) <= t < ST(n+1)
    
    disp(sprintf(' ST(%d) = %0.2f, t = %0.2f, ST(%d+1) = %0.2f',n,ST(n),t,n,ST(n+1)));
    % right now we should be between picks N and N+1

    alpha = (ST(n+1) - t) / (ST(n+1) - ST(n));
    beta = 1 - alpha;
    
    % interpolate the position
    
    % Actually we should probably be using Brescham's (sp)
    % algorithm

    disp(sprintf('interpolating between [n=%d] (%d,%d) and (%d, %d); alpha=%0.2f, beta = %0.2f',n,SX(n),SY(n),SX(n+1),SY(n+1),alpha,beta)); 
    x = round(alpha * SX(n) + beta * SX(n+1));
    y = round(alpha * SY(n) + beta * SY(n+1));
    
    % interpolate the orientation angle
    
    % <¡OBS!> need to do polarity checking!  otherwise results will
    % be nonsensical in cases of polarity mismatches
  
    % BUG: I think instead of interpolating between the orientation
    % angles at the verticies, we should just take the slope of the
    % line as the orientation (after perhaps reversing the order of
    % the verticies if the polarity is wrong)
    
    % TODO: We should have an option to "turn off" picking if
    % delta_theta exceeds some threshold.
    
    q = rad2deg(angle(alpha * exp(i*deg2rad(SA(n))) + ...
                      beta *  exp(i*deg2rad(SA(n+1)))));
   
    % Okay, kids, here we go! woo!!
    
    disp(sprintf('Interpolated position: t=%0.2f (x=%0.2f,y=%0.2f,theta=%0.2f)',t,x,y,q));

%    subplot(1,2,1),imagesc(data);
    subplot(1,2,1),
    hold on, plot([y],[x],'yx'), hold off;

    boxes = boxes +1;
    the_boxes(boxes,:,:) = image_extract_and_align(data, [x y], [box_size box_size],q);
    subplot(1,2,2),imagesc(squeeze(the_boxes(boxes,:,:)));
    
    caxis([0 1]);
%    disp('--pause--');
    pause(0.0);

  end
  
  disp(sprintf('Exported %d boxes',boxes));
  
end

















