% est_transrotr.m                                    Steve Mann, 1992
%
% recursive 2-D image group parameter est; adapted for Peleg: Steve Mann, 1993
%
% transrot (not transrot2 because the 2 is obvious; no such thing as 1D rot)
%
% estimates parameters of the rigid body motion group
%
% Use: [b,a] = est_transrot(E1,E2);                 % complex trans; rot mag=1
% or : [trans_m, trans_n, rot] = est_transrot(E1,E2)% 3 real output arguments
% or : est_transrot(E1,E2);                         % nicely formatted screen
%                                                   % output in radians + deg
%
%      est_transrotr(E1,E2,3);                      % does 3 itterations
%                                                   % default is 2 itterations
%
%      est_transrotr(E1,E2,3,a,b);                  % uses starting resample a,b
%
% sign convention: real down, imag to right
%
% properly handles situation when one or both images contain some NaN points
%
% Bugs???: Peleg Taylor series of Taylor series still needs to be double-checked

function [p1, p2, p3] = multi_parameter_function(E1,E2,K);

if nargin==1
  disp('est_transrotr: you must supply 2 images')
  return
end%if

if nargin ~=3
  disp('est_transrotr: number of itterations not specified; defaulting to 2')
  K=2;
end%if

% DISPLAY display
fig_n = gcf;
if fig_n < 5
  figure(fig_n+1); % open new window
else
  disp1=sprintf('est_transrotr: you got %g figures already: re-using fig %g',...
		 fig_n, fig_n);
  disp(disp1)
  clg
  figure(fig_n);
end%if
subplot(221)
tvs(E1)
title('Reference frame: E1')
drawnow
subplot(222)
tvs(E2)
title('Comparison frame: E2(k=1)')
drawnow

[b0,a0] = est_transrot(E1,E2);
a_comp=a0;  % initialize composite group parameters
b_comp=b0;
o=pi/180;
disp1='est_transrotr: ';
disp2='formatted state-variables at each itteration displayed below:';
disp([disp1 disp2])
disp1=sprintf('beg 1:init angle(a)=%+7.4f= %+9.4fo; ',...
              angle(a0),angle(a0)/o');
disp2=sprintf('init b=%+9.3f + %+9.3f*i',real(b0),imag(b0));
disp([disp1 disp2])
for k = 2:K  % don't need to desample first time (K=1)
%  E2d = desample(a_comp,E2,b_comp); %leaving out interp_order... prints default
%                           % good for following flow of algorithm
  E2d = desample(a_comp,E2,b_comp,1,-1,NaN);%E2desampled; bilinear; no antialias
  % DISPLAY display
  subplot(223)
  tvs(E2d)
  title1=sprintf('Desampled E2 at itteration%g',k);
  xlabel('should converge to E1')
  title(title1)
  drawnow
  subplot(224)
  tvs(abs(E2d-E1))
  title(sprintf('scaled |E1-E2(%g)|',k));
  drawnow

  [b1,a1] = est_transrot(E1,E2d); %estimate residual parameters after desampling
  %disp1=sprintf('est_transrotr: itteration%g: residual angle(a)=%g =%go;',...
  %              k,angle(a1),angle(a1)/o');
  %disp2=sprintf('             : residual b=%g + %g*i',real(b1),imag(b1));
  %disp(disp1)
  %disp(disp2)
  a_comp = a_comp*a1;
  b_comp = b_comp*a1+b1;
  disp1=sprintf('itt%2.0f:comp angle(a)=%+7.4f= %+9.4fo; ',...
                k,angle(a_comp),angle(a_comp)/o');
  disp2=sprintf('comp b=%+9.3f + %+9.3f*i',...
                real(b_comp),imag(b_comp));
  disp([disp1 disp2])
  a0 = a1; % new initial parameters for next itteration
  b0 = b1;
end%for

trans_m = real(b_comp);
trans_n = imag(b_comp);
rot     = angle(a_comp);

if nargout == 3
  p1 = trans_m;
  p2 = trans_n;
  p3 = rot;
end%if

if nargout == 2
  p1 = trans_m + i*trans_n;    % the ``b'' in the ax+b group
  p2 = exp(i*rot);             % the ``a'' in ax+b; contains zoom=1 and rot
end%if

if nargout == 1
  disp('only 1 output argument was given; still need to decide what to do here')
  disp('i have not decided what to do in the case of 1 output argument')
  disp('please use either 3,2, or 0 output arguments')
  return
end%if

if nargout == 0
  % disp('0 output arguments were given so I am displaying result to screen:')
  o = pi/180;
  disp(sprintf('rot=%g =%go;  trans_m=%g pels;  trans_n=%g pels',...
               rot, rot/o, trans_m, trans_n))
  p1 = [trans_m, trans_n, 0, rot, 0,0,0];  % p1 will be returned if user calls
					   % this function without semicolon
end%if