% est_pchirp2r.m         (c) Steve Mann.  This version written 1992-1994
%
% recursively estimates parameters of the pchirp2 motion group
%
% Use: p = est_pchirp2r(E1,E2);                     % units of pchirp2
% or : est_pchirp2r(E1,E2);                         % nicely formatted screen
%
%      est_pchirp2r(E1,E2,3);                       % does 3 iterations
%                                                   % default is 2 iterations
%
%      est_pchirp2r(E1,E2,3,p0);                    % use starting dechirp, p0
%
% Example: p=est_pchirp2r(A,B,3);
%          p=est_pchirp2r(A,B,3,p);                 % pick up where left off
%          p=est_pchirp2r(A,B,3,p);                 % recursively, can repeat
%
% sign convention: x or m down, y or n to right
%
% properly handles situation when one or both images contain some NaN points
%
% [p, mse_hist]=est_pchirp2r(A,B,3);  % gives mean square error (mse) plot data
%
% calls est_pchirp2.m   (recursively)

function [p_out,mse_hist] = multi_parameter_function(E1,E2,K,p0);

if nargin==0
  error('est_pchirp2r: you must supply some input images')
end%if

if nargin==1
  error('est_pchirp2r: you must supply 2 (two) input images')
end%if

if nargin==2
  disp('est_pchirp2r: number of iterations not specified; defaulting to 2')
  disp('              (est once, then interpolate once, and est residuals)')
  K=2;
end%if

if K<=0
  disp('est_pchirp2: number of iterations must be positive ...exiting...')
  return
end%if

[M, N]  = size(E1);
[M2,N2] = size(E2);
if (M2~=M) | (N2~=N)
  error(...
    sprintf('pchirp2r: images must be same size; %gby%g and %gby%g differ',...
            M,N,M2,N2)...
       )
end%if

% amplitude used for display of error image on same fixed range as E1
amplitudeE1=max2nan(E1)-min2nan(E1);

% DISPLAY display figures in successively newly created windows
fig_n = gcf;
%if fig_n < 5  % limit maximum number of windows
figure(fig_n+1); % open new window
set(gcf,'Name',sprintf('  given %g iterations,...',K))
clg
subplot(221)
tvs(E1)
title('Reference frame: E1')
subplot(221);xlabel('IEEE TIP Sept 97 Vol6 No9, US Pat.     ')
drawnow
subplot(222)
tvs(E2)
xlabel('5706416 mann@eecg.toronto.edu')
title('Comparison frame: E2(k=1)')
drawnow

disp1='est_pchirp2r: ';
disp2='formatted state-variables at each iteration displayed below:';
disp([disp1 disp2])

if nargin <= 3 
  disp('est_pchirp2: estimating initial parameters from the two images:')
  p0 = est_pchirp2(E1,E2);
  disp(sprintf('est 1: %+10.6f %+10.6f %+10.6f    %+10.6f %+10.6f %+10.6f',...
               p0(1),p0(2),p0(3),p0(4),p0(5),p0(6)))
  disp(sprintf('       %+10.6f                          %+10.6f',p0(7),p0(8)))
else% more than 3 input arguments; therefore p0 has been specified
  if min(size(p0)) ~= 1
    error('pchirp2r: starting guess must be a vector (1 by 8 or 8 by 1)')
  end%if
  if length(p0) ~= 8
    error('pchirp2r: starting guess must be a vector of length 8')
  end%if
  if K==1
    disp('est_pchirp2r: you specified p0, and K=1, so this function')
    disp('              will do nothing other than display the images and')
    disp('              some diagnostic text')
  end%if
  disp1=sprintf('set 1:%+10.6f %+10.6f %+10.6f    %+10.6f %+10.6f %+10.6f',...
                p0(1),p0(2),p0(3),p0(4),p0(5),p0(6));
  disp([disp1])
  disp(sprintf('      %+10.6f                          %+10.6f',p0(7),p0(8)))
end%if

p_comp=p0;  % initialize composite group parameters

mse_hist = [];  % mean squared error history (for mse plot)
for k = 2:K  % don't need to dechirp first time (K=1)
% E2d = pchirp2(p_comp,E2); %leaving out interp_order... prints default
%                            % good for following flow of algorithm
  E2d = pchirp2(p_comp,E2,1,-1,NaN); %E2dechirped; bilinear; no antialias
  % DISPLAY display
  subplot(223)
  tvs(E2d)
  title1=sprintf('Lookpainting E2 at iteration%g',k);
  xlabel('should converge to E1')
  title(title1)
  drawnow
  subplot(224)
  abserr=abs(E2d-E1)/amplitudeE1;   % will be on interval [0,1]
  tv(abserr*(length(colormap)-1))
  title(sprintf('|E1-E2(%g)|/a',k));
  xlabel('where a = |max2nan(E1)-min2nan(E1)|')
  sqrerr=(E2d-E1);
  mse=mean2nan(sqrerr.*sqrerr);
  mse_hist = [mse_hist mse];
  ylabel(sprintf('mse=%g',mse))
  drawnow

  p1 = est_pchirp2(E1,E2d); %estimate residual parameters after desampling
  p_comp = pcompose(p1,p_comp);  % not Abelian, so not equal to p_comp \circ p1
  disp1=sprintf('it%2.0f: %+10.6f %+10.6f %+10.6f    %+10.6f %+10.6f %+10.6f',...
                k,p_comp(1),p_comp(2),p_comp(3),p_comp(4),p_comp(5),p_comp(6));
  disp([disp1])
  disp1=sprintf('       %+10.6f                          %+10.6f',p_comp(7),p_comp(8));
  disp(disp1)
  p0 = p1; % new initial parameters for next iteration
  set(gcf,'Name',sprintf('given %g iterations (done it %g),...',K,k))
end%for

if (nargout ~= 2) & (nargout ~= 1) & (nargout ~= 0)
  error(sprintf('pchirp2r: nargout must be 0, 1, or 2, you had %g',nargout))
end%if

if nargout == 0
  x=[0; 0; 1; 1];
  y=[0; 1; 0; 1];
  one=ones(4,1);
  xp=(p_comp(1)*x+p_comp(2)*y+p_comp(3))./(p_comp(7)*x+p_comp(8)+one);
  yp=(p_comp(4)*x+p_comp(5)*y+p_comp(6))./(p_comp(7)*x+p_comp(8)+one);
  disp('est_pchirp2r: 0 output args given so I''ll tell you a little more,')
  disp('              like where E1 corners correspond to E2')
  disp1=sprintf('it%2.0f: %+9.1f/%g %+9.1f/%g %+9.1f/%g %+9.1f/%g',...
                k,M*xp(1),M,M*xp(2),M,M*xp(3),M,M*xp(4),M);
  disp([disp1])
  disp1=sprintf('       %+9.1f/%g %+9.1f/%g %+9.1f/%g %+9.1f/%g',...
                N*yp(1),N,N*yp(2),N,N*yp(3),N,N*yp(4),N);
  disp(disp1)
end%if

if nargout ~= 0
  p_out = p_comp;  % return composite result
end%if

if mse<500
  set(gcf,'Name',sprintf('  given %g iterations, found in SAME ``VIDEO ORBIT''''',K))
else%if
  set(gcf,'Name',sprintf('  given %g iterations, found in DIFFERENT ``VIDEO ORBIT''''',K))
end%if