% PINTEGRATE    integrate the running pairwise est.
%
%   P_cumulative = pintegrate(P)
%
%   P_cumulative now one longer than input pairwise parameters
%
%   Example use: Pcum = pintegrate(P);
%                Pcum = pintegrate(P,mref); % reference to frame mref of images
%                Pcum = pintegrate(P,3); % reference to frame 3 (P(4,:) is ref)
%                % mref must be on interval [0,mframes), eg 0to29 for 30 frames
%                Pcum = pintegrate(P,Pref)  % ref on parameter rather than mref
%                Pcum = pintegrate(P,P(3+1,:))% should have same effect,but now
%                                             % can use a Pref not even in P
%   Use with idr:Pcum = pintegrate(p);
%                Pframe10 = Pcum(11,:);
%                [trash, Pextra] = idr(v10);  % frame 10
%                P=pintegrate(p,pcompose(pinverse(Pextra),Pframe10))
%
%                p32 = P(3,:);  % maps frame3 to frame2
%                p30 = Pcum(4,:);  % maps frame3 to frame
%                Pcum(1,:) maps frame 0 to frame 0
%                Pcum(2,:) maps frame 1 to frame 0
%
% Now the identity goes at the beginning to keep track of different ref. frames

function Pcum = integratebyrepeatedpcomposition(P,mref)

[M,N] = size(P);
mframes=M+1;
disp('pintegrate: NOTE: changed parameter list; now P0 at beginning');
disp(sprintf('            generating cumulative %d long from pairwise %d long for %d frames',mframes,M,mframes))

if N~= 8
  error('pintegrate: parameters must be row vectors arranged in 8-wide matrix')
end%if

if M==N
  disp('pintegrate: warning; input is square; double check parameters are across rows')
end%if

if nargin==1
  disp('pintegrate: defaulting to frame 0 as reference frame')
end%if
Pcum = [[1 0 0   0 1 0   0 0];P(1,:)];
       %initialize identity=p_{0,0} and p_{1,0}(first row)
for m=2:M
  Pcum = [Pcum;pcompose(P(m,:),Pcum(m,:))];
end%for

if nargin==2
  [mframes,trash]=size(Pcum); % number of frames
  if max(max(size(mref)))==1
    Pref=Pcum(mref+1,:);
  else%if
    Pref=mref;   % assuming a parameter was passed directly
    [M_Pref, N_Pref] = size(Pref);
    if M_Pref ~= 1
      error('pintegrate: input parameter must be a ROW VECTOR')
    end%if
    if N_Pref ~= 8
      error('pintegrate: input parameter must be a row vector of LENGTH 8')
    end%if
  end%if
  Pcum_referenced=NaN*Pcum; % initialize
  for m=1:mframes
    Pcum_referenced(m,:)=pcompose(Pcum(m,:),pinverse(Pref));
  end%for
  Pcum=Pcum_referenced;
  clear Pcum_referenced
% the following approach only worked for mref <= l1
%  if mref==0
%    disp( 'pintegrate: will deal with specification of frame 0 later; right now:')
%    error('          : if you want to ref on frame 0, don''t specify a reference frame')
%  end%if
%  disp('pintegrate: working on part before ref frame (identity)')
%  Pcum_before_ref=pinverse(P(mref,:));
%  for m = 1:mref-1 % if mref=1, nothing happens here so Pcum_before_ref gets 1 entry above
%    Pcum_before_ref = [pcompose(pinverse(P(mref-m+1,:)),Pcum_before_ref(m,:));Pcum_before_ref];
%  end%for
%  disp('pintegrate: working on including and after ref frame (identity)')
%  Pcum_ref_and_after = [1 0 0 0 1 0 0 0];
%  for m = 2:mframes-mref
%    Pcum_ref_and_after =...
%      [Pcum_ref_and_after;pcompose(...
%                                   P(m+mref-1,:),...
%                                   Pcum_ref_and_after(m-1,:)...
%                                  )...
%      ];
%  end%for
%  Pcum = [Pcum_before_ref; Pcum_ref_and_after];
end%if