% est_transrot.m                                              Steve Mann, 1992
%
% 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,K);               % 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
%
%
% 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);

%%%%mask = isnan(E1).*isnan(E2);  % NaN mask: regions where either is undefined
% doesnt work because nanmask makes it still nan

[El,Em,En] = derivatives(E1,E2);

%%%mask = isnan(El).*isnan(Em).*isnan(En); % this mask may be more encompassing

%ElEm = sum(sum(El.*Em.*mask));
ElEm = sum(sum(chnan(El,0).*chnan(Em,0)));

%ElEn = sum(sum(El.*En.*mask));
ElEn = sum(sum(chnan(El,0).*chnan(En,0)));

%EmEm = sum(sum(Em.*Em.*mask));
EmEm = sum(sum(chnan(Em,0).*chnan(Em,0)));

%EmEn = sum(sum(Em.*En.*mask));            % used twice; e.g. EmEn = EnEm
EmEn = sum(sum(chnan(Em,0).*chnan(En,0))); % used twice; e.g. EmEn = EnEm

%EnEn = sum(sum(En.*En.*mask));
EnEn = sum(sum(chnan(En,0).*chnan(En,0)));

[M,N]= size(El);                    % size of any of the 3 derivative matrices
[n m]=meshdom(1:N,M:-1:1);          % variables ``x'' and ``y''
A = m.*En - n.*Em;                  % xg_y - yg_x
%AEl  = sum(sum(A.*El.*mask));
AEl  = sum(sum(chnan(A,0).*chnan(El,0)));

%AEm  = sum(sum(A.*Em.*mask));
AEm  = sum(sum(chnan(A,0).*chnan(Em,0)));

%AEn  = sum(sum(A.*En.*mask));
AEn  = sum(sum(chnan(A,0).*chnan(En,0)));

%AA   = sum(sum(A.*A.*mask));
AA   = sum(sum(chnan(A,0).*chnan(A,0)));

DERIVATIVES = [EmEm EmEn AEm; EmEn EnEn AEn; AEm AEn AA];% matrix of derivatives
Derivatives = [ElEm; ElEn; AEl];                         % vector of derivatives

parameters = DERIVATIVES\Derivatives;

trans_m = parameters(1);
trans_n = parameters(2);
rot     = parameters(3);

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('trans_m=%g pels;  trans_n=%g pels;  rot=%g =%go',...
  trans_m, trans_n, rot, rot/o))
  p1 = [trans_m, trans_n, 0, rot, 0,0,0];  % p1 will be returned if user calls
					   % this function without semicolon
end%if