%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Estimate Finv given K's are fixed
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%% Modified not to use left or right endpoint of window (taken to be zero)
%% We accumulate just the same, just work with submatrix at end!

function [Finv]=est_Finv(images,w,K,skip,lambda)
%% Data fitting equations
%% A'A*x = A'b   Accumulate matrices A'A and A'b to save space
%% There are N*P*(P-1) equations and n unknowns
%% Lambda >=0 is smoothing term
N=length(w); % number of pixels in images
[pixels,P]=size(images);
A=zeros(N,N);
b=zeros(N,1);
for t1=1:P % first image
  disp(['t1=',num2str(t1)]);
  im1=images(:,t1)+1;
  for t2=t1+1:P % second image 
    disp(['t2=',num2str(t2)]);
    im2=images(:,t2)+1;
    for p=1:skip:pixels
      p1=im1(p);
      p2=im2(p);
      k=w(p1)*w(p2);
      A(p1,p1)=A(p1,p1)+k;    % A'A
      A(p2,p2)=A(p2,p2)+k;
      A(p1,p2)=A(p1,p2)-k;
      A(p2,p1)=A(p2,p1)-k;
      b(p1)=b(p1)+k*(K(t1)-K(t2));  % A'b
      b(p2)=b(p2)+k*(K(t2)-K(t1));
    end %pixels
  end %t2
end %t1

%% Smoothing terms g''(p)=0;  Only from 2 to N-1 since boundaries not used.
l=lambda^2;
for p1=3:N-2,
  A(p1-1,p1-1)=A(p1-1,p1-1)+1*l;
  A(p1-1,p1  )=A(p1-1,p1  )-2*l;
  A(p1-1,p1+1)=A(p1-1,p1+1)+1*l;
  A(p1  ,p1-1)=A(p1  ,p1-1)-2*l;
  A(p1  ,p1  )=A(p1  ,p1  )+4*l;
  A(p1  ,p1+1)=A(p1  ,p1+1)-2*l;
  A(p1+1,p1-1)=A(p1+1,p1-1)+1*l;
  A(p1+1,p1  )=A(p1+1,p1  )-2*l;
  A(p1+1,p1+1)=A(p1+1,p1+1)+1*l;
end;

%% Modifiy to work with 2:254 (ie., don't do endpoints of window)
%% Add contstraint that Finv(N-1)=0 (ie., 0db is maximum response)
%% Note: if I don't add this A is rank deficient!
A(N-1,N-1)=A(N-1,N-1)+1;
A=A(2:N-1,2:N-1);
b=b(2:N-1);

%% Solve using SVD
Finv=pinv(A)*b;

%% Flag invalid end values with NaN
Finv=[NaN;Finv;NaN];

%keyboard;
return % function est_Finv
