% FUNCTION [R,X] = CONSTANT_STEP( P )
%
% To see parameters: use "dbtype constant_step" to see code.
%
% Important note: planar array
%
% For a non-planar array, it would be important to use a more
% generic way of picking points out of each sphere.
%
% For arrays that are quite stretched in space (e.g. like a strline),
% it would be important to use a more adequate shape than sphere
% (e.g. ellipsoid). Or find a generic way to create the shape.

function [r,X] = constant_step( p )

if nargin < 1
  error( 'constant_step needs one parameter' );
end

required_param = 'geometry';
check_param( required_param, fieldnames( p ) );

p_default.up_factor = 5;
p_default.r_min     = [];
p_default.el_max    = 70 / 180 * pi;   % in radians

pairs = [];
n_channels = size( p.geometry, 2 )
for a = 1:n_channels
  for b = a+1:n_channels
    pairs = [pairs; a b];
  end
end
p_default.pairs = pairs;
pairs = [];

p = fill_default( p, p_default );

if isempty( p.r_min )
  r_geometry = sqrt( sum( p.geometry( 1,:).^2 + p.geometry(2,:).^2,1));
  p.r_min         = max( r_geometry ) * 2;
end



% Verbosity

p

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


% Initial radius
% We don't expect to locate anything beyond that distance
% (meters)
r = 5;

% Step -> our average precision in time-delay space
t_step_square = ( 1 / p.up_factor ) ^ 2;


n_pairs = size( p.pairs, 1 );

% We will now go along the x axis, decreasing radius, 
% picking locations so that the distance in time-delay space
% between two consecutive locations is constant.

% The chosen distance in time-delay space is euclidian,
% normalized by the number of pairs (P):
%
% d( tau_1, tau_2 ) = ( 1/P * sum for j from 1 to P of 
%   (( tau_1_j - tau_2_j ) ^ 2)  ) ^ 1/2

% So constant distance in time-delay space between two positions
% x_k and x_k+1 means the following sum is constant against k:
%
% sum for j = 1..P of (( ||x_k - m_a_j|| - ||x_k - m_b_j||
%                       -||x_k+1 - m_a_j|| + ||x_k+1 - m_b_j|| )^2 )
%
% where m_a_j and m_b_j are the spatial positions of microphones
% defining pair j.


% Circle of points used for selecting radiuses
a = (0:15) / 16 * 2 * pi;
the_circle = [cos(a); sin(a); 0*a];

t = location_to_timedelay( p.geometry, p.pairs, r * the_circle );

% Abortion constants
the_precision = 1e-3;
max_iter      = 500;

for a = 2:100
  
  % Starting point for searching next position
  new_r = 0.5 * r(end);
  
  % Search
  finished = 0;
  iter = 0;
  previous_new_r = r(end);
  
  while ~finished

    iter = iter + 1;
    
    % Do we need to get more precise?
    
    new_t = location_to_timedelay( p.geometry, p.pairs, new_r * the_circle );
    aux = sum( (new_t - t) .^ 2, 1 ) / n_pairs;
    the_dist_square = mean( aux );    
    current_precision = abs( the_dist_square - t_step_square ) / t_step_square;
    finished = current_precision < the_precision ;
    
    % Can we iter more?
    if ~finished
      finished = iter >= max_iter;
    end
    
    % Can we???
    if ~finished
      the_diff = abs(new_r - previous_new_r);
      
      if the_dist_square < t_step_square
	previous_new_r = new_r;
	new_r = new_r -  the_diff / 2;
      else
	aux = previous_new_r;
	previous_new_r = new_r;
	new_r = new_r + the_diff / 2;
      end
    end
    
  end
  
  % DEBUG
  disp(sprintf(['a:%d done in %d iterations (final precision:' ...
			      ' %.5f)'], a, iter, current_precision));
  
  % Put it into the memory
  r = [r, new_r];
  t = new_t;
  
  % Overflow?
  if iter >= max_iter
    break;
  end

  % Finished? (too close to the array)
  if r(end) < p.r_min
    r = r(1:end-1)
    break;
  end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Now that we have found radiuses, 
% let's determine which points we'll pick from each sphere

rand('state',sum(100*clock));

% List of all points
X = [];
t = [];

% Abortion constants
the_precision = 1e-3;
max_iter = 500;

for a = 1:length(r)
  
  % DEBUG
  disp(sprintf('starting sphere #%d - radius %.4f, starts at point #%d', ...
	       a, r(a), size(X,2)+1));
  start_index = size(X,2) + 1;
  
  
  % Sphere's radius
  sr = r(a);
  
  % We start in the horizontal plane (elevation 0)
  el = 0;
  
  % Starting vertical angle between two circles
  v_angle = 10/180*pi;
  
  % Go around horizontal circles, picking points with 
  % constant interval in time-delay space
  
  finished = 0;
  
  while ~finished

    %DEBUG
    disp(sprintf(['starting circle at elevation %.3f rad - starts at' ...
		  ' point #%d'], el, size(X,2) + 1));
    
    % 1) Go around horizontal circle at current elevation
    circle_finished = 0;
    
    % Starting horizontal angle between two points
    h_angle = 5/180*pi;
  
    % Pick a starting point at random
    % az_start = -pi + 2 * pi * rand;
    
    % No, let's put aside randomness for a second
    az_start = 0;
    az = az_start;

    X_start = [ sr * cos(el) * cos(az) ; sr * cos(el) * sin(az) ; sr * sin(el) ];
    t_start = location_to_timedelay( p.geometry, p.pairs, X_start );
    
    X = [ X, X_start ];
    t = [ t, t_start ];

    %DEBUG
    %disp(sprintf( 'point of azimuth %.3f added', az ));
    
    while ~circle_finished
      
      % Find next point
      
      previous_new_az = az;
      new_az = az + h_angle;
      next_point_found = 0;
      iter = 0;
      while (~next_point_found) & (iter<max_iter) 
	
	iter = iter + 1;
	
	new_X = [ sr * cos(el) * cos(new_az) ; sr * cos(el) * sin(new_az) ; sr * sin(el) ];
	new_t = location_to_timedelay( p.geometry, p.pairs, new_X );
	the_dist_square = sum( (new_t - t(:,end)) .^ 2, 1 ) / n_pairs;
	next_point_found = abs( the_dist_square - t_step_square ) / t_step_square < the_precision ;
	
	if ~next_point_found

	  the_diff = abs( new_az - previous_new_az );
	  
	  if the_dist_square < t_step_square
	    previous_new_az = new_az;
	    new_az = new_az + the_diff / 2;
	  else
	    previous_new_az = new_az;
	    new_az = new_az - the_diff / 2;
	  end
	  
	end
	
      end
      
      circle_finished = ~next_point_found;
      
      if ~circle_finished
	
	% Have we gone around the circle?
	
	if new_az - az_start > 2 * pi
	  % Yes: move to higher elevation
	  circle_finished = 1;
	else            
	  % Update h_angle: this is not necessary,
	  % but might improve speed
	  h_angle = new_az - az;
	  
	  % No: add the new point
	  X  = [X, new_X];
	  t = [t, new_t];
	  az = new_az;
	  
	  %DEBUG
	  %disp(sprintf( 'point of azimuth %.3f added' , az));
	  
	end
      end
    end
    
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    
    % 2) Find next elevation
    
    previous_new_el = el;
    new_el = el + v_angle;    
    next_elevation_found = 0;
    iter = 0;
    while (~next_elevation_found) & (iter < max_iter)
      
      iter = iter + 1;
      
      new_X = [ sr * cos(new_el) * cos(az_start) ; sr * cos(new_el) * sin(az_start) ; sr * sin(new_el) ];
      new_t = location_to_timedelay( p.geometry, p.pairs, new_X );
      the_dist_square = sum( (new_t - t_start) .^ 2 ) / n_pairs;
      next_elevation_found = abs( the_dist_square - t_step_square ) / t_step_square < the_precision ;
      
      if ~next_elevation_found

	the_diff = abs( new_el - previous_new_el );
	if the_dist_square < t_step_square
	  previous_new_el = new_el;
	  new_el = new_el + the_diff / 2;
	else
	  previous_new_el = new_el;
	  new_el = new_el - the_diff / 2;
	end
      end
      
    end

    finished = ~next_elevation_found;

    if ~finished
      % Update v_angle: not necessary but might improve speed
      v_angle = new_el - el;
      
      el = new_el;
      
      %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
      finished = el > min( p.el_max, pi / 2 );
    end
    
  end

  % DEBUG
  disp(sprintf('%d points in sphere #%d\n', 1+size(X,2) - start_index, a))
  
end