function [Ts, values, obs, index_combinations] = h3d_guess_translation(p, ht1, ht2, Rest)
	% function [Ts, values] = h3d_guess_translation(p, ht1, ht2, Rest)
	% 
	% Guesses the translation given the rotation Rest.
	%
	% - Ts is a list of 3x1 vectors.
	% - values is a vector.
	
	% Find good directions for the correlation
	fprintf('Finding peaks on HT...\n');
	peaks = h3d_find_hs_peak(ht1);
	peaks = peaks(2:4,:);
	
	
	% Peaks that we already used for directions
	peaks_used = zeros(3,0);
	% Counter of directions used
	
	obs = {};
	
	for i=1:size(peaks,2)
		peak = peaks(:,i);
		
		% Do not use this direction if it is too close to one that we already used.
		
		if closer_than_deg_or_parallel(peak, peaks_used, 15)
			fprintf('Discarding peak at [%1.2f %1.2f %1.2f] (too close)\n', peak(1),peak(2),peak(3));
			continue
		end
		
		
		% Do not use this direction if the other spectrum is very poor
			
		[f1, i1, j1] = h3d_coords_s2_to_cell(peak, ht1.cube_ncells);
		[f2, i2, j2] = h3d_coords_s2_to_cell(Rest * peak, ht2.cube_ncells);
		

		if(ht2.cube_hs(f2,i2,j2) <= 0.0001 * ht1.cube_hs(f1,i1,j1) )
			fprintf('Discarding peak at [%1.2f %1.2f %1.2f] (value of other hs is %f, mine is %f)\n', peak(1),peak(2),peak(3), ht2.cube_hs(f2,i2,j2), ht1.cube_hs(f1,i1,j1) );
			continue;
		end

		fprintf('Using peak at %s \n', display_v3(peak));
		peaks_used(1:3, size(peaks_used,2) + 1 ) = peak;


		s1 = squeeze(ht1.cube_ht(f1,i1,j1,:));
		s2 = squeeze(ht2.cube_ht(f2,i2,j2,:));


		if p.guess_tran_two_birds_one_stone
			[f1b, i1b, j1b] = h3d_coords_s2_to_cell(-peak, ht1.cube_ncells);
			[f2b, i2b, j2b] = h3d_coords_s2_to_cell(-Rest * peak, ht2.cube_ncells);

			s1b = squeeze(ht1.cube_ht(f1b,i1b,j1b,:));
			s2b = squeeze(ht2.cube_ht(f2b,i2b,j2b,:));
			s1b = s1b(end:-1:1);			
			s2b = s2b(end:-1:1);
			
			s1 = s1 - s1b;
			s2 = s2 - s2b;
			
					
			other_direction = ht2.cube_normals{f2,i2,j2};
			other_direction_wished = Rest * peak;
		
			mismatch = angle_between_vectors(other_direction, other_direction_wished);
			fprintf('Mismatch: %f deg\n', rad2deg(mismatch));
		end
	
%		s1u = s1.^2; s1u = s1u / norm(s1u);
%		s2u = s2.^2; s2u = s2u / norm(s2u);
		s1u = s1;
		s2u = s2;

%		fprintf('Peak #%d: xcorr along rho...\n',i);
		
		
		[xc_unsmoothed, lags] = xcorr(s2u,s1u);
		
		xc = medfilt1(xc_unsmoothed, 3);
		
		% Find local maxima of correlation
		[maxima_coords, maxima_values] = h3d_matrix_local_maxima(xc);
		if size(maxima_values) == 0
			continue;
		end
		maxima_indices = maxima_coords(1,:); % take only column
		
		% Sort
		[unused,I] = sort(maxima_values, 'descend');
		maxima_values = maxima_values(I);
		maxima_indices = maxima_indices(I);
		
		maxima_delta = lags(maxima_indices)* ht1.rho_cell_size;
		
		num_dir = numel(obs) + 1;
		obs{num_dir} = {};
		
		lags_used = [];
		
		for j=1:numel(maxima_delta)
			if(maxima_values(j) <= 0.01 * maxima_values(1))
				continue
			end
			
			too_close = 0;
			for prev=1:j-1
				if abs(maxima_delta(prev)-maxima_delta(j)) < 0.2
					too_close = 1;
				end
			end
			if too_close
				continue
			end
			
			num_hyp = numel(obs{num_dir})+1;
			
			if num_hyp > p.guess_tran_max_num_hyp
				break
			end
			obs{num_dir}{num_hyp}.M_row = (Rest*peak)';
			obs{num_dir}{num_hyp}.Z_row = maxima_delta(j);
			obs{num_dir}{num_hyp}.weight = maxima_values(j);
			
			lags_used(num_hyp) = maxima_delta(j);
			lags_used_values(num_hyp) = maxima_values(j);


			fprintf(' %s * T =     %f   (value=%f) \n', display_v3(Rest*peak), maxima_delta(j),  maxima_values(j));
		end
	
	
			if numel(p.debug_true_T) == 3
				true_lag_m = (Rest * peak)' * p.debug_true_T;
				fprintf(' %s * true_T = %f \n', display_v3(Rest*peak), true_lag_m);
			end

	
		if  p.debug_show_translation
			figure;
			subplot(2,1,1);
			hold on;
			
			
			plot(s1u,'r.');
			plot(s2u,'b.');
            
			subplot(2,1,2); hold on;
				plot(lags*ht1.rho_cell_size,xc_unsmoothed,'y.');
			plot(lags*ht1.rho_cell_size,xc,'.');
			
			
			for h=1:numel(lags_used)
				lag = lags_used(h);
				plot([lag lag],[0 0.5]*max(xc_unsmoothed),'r-');
			end
			
			
			if numel(p.debug_true_T) == 3
				true_lag_m = (Rest * peak)' * p.debug_true_T;
				plot([true_lag_m true_lag_m],[0.5 1]*max(xc_unsmoothed),'k-');
			end
			a = axis;
%			bound = 0.3 * max(lags*ht1.rho_cell_size);
%			axis([-bound bound a(3) a(4)]);
			
			xlabel('m');
			title(sprintf('Peak #%d: direction = %s',num_dir,display_v3(peak))) 
			
			pause(1)
			
		end


		if(size(peaks_used,2) >= p.guess_tran_max_directions)
			break;
		end
	end
	

	obs
	
	% Now we have
	% 	obs{direction}{hypothesis}.M_row 
	% 	obs{direction}{hypothesis}.Z_row
	% we have to do all combinations.
	
	index_combinations = {[]};
	
	ndirections = numel(obs);
	for i=1:ndirections
		new_combinations = {};
		num_hyp = numel(obs{i});
		for h=1:num_hyp
			for j=1:numel(index_combinations)
				new_combinations{numel(new_combinations)+1} = [index_combinations{j} h];
			end
		end
		index_combinations = new_combinations;
	end

	
	Ts = {};
	values = [];
	
	ncombinations = numel(index_combinations);
	fprintf('We had %d combinations (%d directions, max %d hyp per dir.)\n', ncombinations,  p.guess_tran_max_directions, p.guess_tran_max_num_hyp);
	pause
	
	for a=1:ncombinations
		combination = index_combinations{a};
		
		M = zeros(0, 3);
		weights = [];
		Z = zeros(0, 1);

		row = 1;
		for i=1:ndirections
			h = combination(i);
			
		%	fprintf('Using hypo %d for direction %d\n', h, i);

			if(obs{i}{h}.weight>0)
			
				M(row,1:3) = obs{i}{h}.M_row;
				Z(row,:) =   obs{i}{h}.Z_row;
				weights(row) = obs{i}{h}.weight;		
			
				row = row+1;
			end
		end
		
%		if row < 4
%			continue;
%		end
		
		[t_est, cov, chi] = solve_least_squares(M, Z, weights, p.debug_true_T);
		
		Ts{end + 1} = t_est;
%		values(end + 1) = sum(weights(1:3));
		values(end + 1) = sum(weights);
		
	end
	

	% sort the hypotheses
	[Y,I] = sort(values, 'descend');
	Ts = {Ts{I}};
	values = values(I);

	
	fprintf('Summary:\n');
	for i=1:numel(Ts)

		R = Rest;
		T = Ts{i};
		val = values(i);

		if numel(p.debug_true_T) == 3
			e_r = h3d_rotation_error(R, p.debug_true_R);
			e_t = (T -  p.debug_true_T);
			s = sprintf('error R: %03.1f deg; T: %3.1f cm', e_r, norm(e_t) * 100);
		else
			s= '';
		end
		fprintf('%d Val = %1.3f   R = %s  T = %s   %s\n', i, val, display_rot_aa(R), display_v3(T),s);	
	end
	
	
	
function [t_est,cov,chi] = solve_least_squares(M, Y, weights, true_t)
	W = diag(1./weights);
	cov = inv(M' * inv(W) * M);
	t_est = inv(M' * inv(W) * M) * M'  * inv(W) * Y;
	
	residuals = (M * t_est - Y);

	chi = residuals' * inv(W) * residuals;
	
	%chi = max(abs(residuals));
	
if false	
	true_Y = M * true_t;
	errors = abs(residuals);
	
%	[Y'; (M * t_est)'; true_Y'; abs(residuals')]
	
	fprintf('obser:\n');
	Y'
	fprintf('actual:\n');
	(M * true_t)'
	fprintf('weight:\n');
	M
	Y
	weights
	fprintf('t_est: %s \n', display_v3(t_est));
	fprintf('t_tru: %s \n', display_v3(true_t));
	
	%chi = residuals' * (W) * residuals;
	%chi = residuals' * residuals;	
	error
end
	
function res = closer_than_deg_or_parallel(v, others, max_deg)
	% closer_than_deg(v, others, max_deg)
	%   v: 3x1 vector
	%   others: 3xn matrix
	%   returns 1 if v or -v is closer than max_deg to at least 
	%   one column of others
	
	n = size(others,2);
	
	for i=1:n
	%	fprintf('Between %s and %s : %f deg\n', display_v3(v), display_v3(others(:,i)),...
	 % 	 rad2deg(angle_between_vectors(v, others(:,i))) );
	%	fprintf('Between %s and %s : %f deg\n', display_v3(v), display_v3(-others(:,i)),...
	%	  	 rad2deg(angle_between_vectors(v, -others(:,i))) );
	
		if angle_between_vectors(v, others(:,i)) < deg2rad(max_deg)
			res = 1; return;
		end
		
		if angle_between_vectors(-v, others(:,i)) < deg2rad(max_deg)
			res = 1; return;
		end
	end
	
	res = 0;