function [B,V,Z,E] = fitpoly1(D,R,C,L,P,U,Z)
% FITPOLY1 Fit polynomial to points
%
% [B,V,Z,E] = FITPOLY1(D,R,C,L,P,U) where D is the polynomial degree,
% R and C are the column vectors of the points (rows and columns,
% resp.) under consideration, L is the hyperparameter for a Gaussian
% likelihood model, indicating the variance of the fit, and P are the
% hyperparameters for the prior model of the form:
%
%  P = [ baseMean  baseVar  ; % Constant term
%        slantMean slantVar ; % Linear term
%        curvMean  curvVar  ] % Quadratic term
%
%  U is the log of the range of the row data points for the uniform
%  distribution. Possibilities might be log(realmax-realmin),
%  log(range(R)) or something in between.
%
% Returned fit parameters B have the form
%
%  B = [baseOff slant curv]
%
% V is the resulting optimized expected likelihood. Z is the Nx2
% posterior probability matrix for the curve, and background model,
% (columns, respectively).
%
% E is the approximate log evidence of the model, as described by
% MacKay [0].
%
%   That is, 
%
%      log Evidence := log Likelihood + log Occam factor,
%
%    where
%
%       log Likelihood := log P(D| w_MP, H_i )
%
%    is the best fit likelihood of the data D with parameters w_MP
%    under model H_i. Similarly, the log Occam factor is
%
%       log Occam factor := log P(w_MP | H_i) + K/2 * log (2*pi) 
%                             -1/2 * log(det(A)),
%
%    where K is the dimensionality of w_MP, log P(w_MP | H_i) is the
%    prior probability of the best-fit parameters under the model H_i
%    and the latter terms represents the determinant of a Gaussian
%    covariance matrix, approximated by the negative Hessian A of the
%    log posterior of w at w_MP. That is,
%
%       A = -\nabla_w\nabla_w log P(w_MP | D, H_i) 
%
%    See equation (6) in MacKay [0].
%
%  [0] David J.C. MacKay, Bayesian interpolation. Neural
%  Computation 4(3):415-447, May 1992. doi:10.1162/neco.1992.4.3.415
  
% Copyright (c) 2012  
% Jerod Weinman
% jerod@acm.org
  
% $Id: fitpoly1.m 101 2014-10-24 18:21:04Z weinman $
 
% Jerod Weinman
% jerod@acm.org
% /home/data/svn/vision/tools/package/GPL_STATEMENT
 

% Convergence threshold delta-Likelihood
tau = 1e-6;
    
% Set persistent variables in the helper functions
mstep(D,R,C,L,P);
likelihoods(D,R,C,L,P,U);

B = P(:,1); % Initialize to means

    
% Initialize the prior (mixing weights) for the components
%  Base       = 1
%  Background = 2
W = log([1 1]/2); % W := log P(z==k)

% Initial posterior component weights
if nargin<7
  Z = estep(W,B);
end;

% Initial likelihood value
Vn = likelihood(W,B,Z);

% Initial updated parameters
[W,B] = mstep(Z);

V = -Inf;


while( 2*abs(Vn-V) >= tau*(abs(Vn)+abs(V)+eps) )
    
    V = Vn;
    
    Z = estep(W,B); % E Step
    [W,B] = mstep(Z); % M Step
    
    % Resulting likelihood (actually, posterior)
    Vn = likelihood(W,B,Z);
    
    if ( Vn-V <0 )
        warning(sprintf('Likelihood decrease: %f',Vn-V));
    end
end;

V=Vn;

if nargout>=3    % Return posteriors
  Z = estep(W,B);
end

if nargout>=4    % Return evidence
  H = hessian( @(bb)likelihood(W,bb,Z), B);
  K = size(H,1);
  E = Vn + K/2 * log(2*pi) - 1/2 * log(det(-H));
end

function Z = estep(W,B)
% ESTEP Current posteriors of mixture model
%
% Z = ESTEP(W,B) where W are the prior parameters and B are the fit parameters

  
  [Pxf,Pxb,Px] = likelihoods(W,B);
      
    %%%
    %%% Calculate log posteriors using Bayes' rule
    %%%
    Z(:,1) = W(1) + (Pxf - Px);
    Z(:,2) = W(2) + (Pxb - Px);
    
%Z = exp(Z);
        

    
function [W,B] = mstep(varargin)
% MSTEP Maximum posterior of mixture model
%
% [W,B] = MSTEP(Z) where W are the priors B are the line fit parameters 
    
    persistent D R C L P %C0
    
    if nargin==5
      [D,R,C,L,P] = varargin{:};
      %      C0 = (max(C)+min(C))/2; % Center of coordinates
      return;
    else
      [Z] = varargin{:};
    end;
    
    
    %%%
    %%% First, calculated updated marginal component weights
    %%%
    W = logsumexp(Z) - log(size(Z,1));
    %W = log(sum(Z)) - log(size(Z,1));
    
    %%%
    %%% Finally, calculated updated parameters for Lines
    %%%
    
    % Set up a column of ones to indicate where each line's offset data is used
    oC = ones(size(C));
    CC = C;

    % Coordinates (Input)
    % Figure out how to generalize this to arbitrary integral D>=0
    switch D
      case 0
        A = oC;
      case 1
        A = [ oC CC ]; % Linear data terms
      case 2
        A = [ oC CC CC.^2 ]; % Quadratic data terms
      otherwise
        error('Unsupported degree')
    end
    A = [ A ; eye(D+1) ] ; % Prior terms
    
    % Predictions (Output)
    b = [ R ;  ... % Observations
          P(:,1) ];              % Prior means
    
    % Weights
    w = [ exp(Z(:,1)) / L ; ...  % Observations          
          1./P(:,2) ]; % Priors
    
    B = lscov(A,b,w);
    
    

function [Pxf,Pxb,Px,Pf]  = likelihoods(varargin)
% LIKELIHOODS Likelihood terms of mixture model
%
% [Pxf,Pxb,Px,Pf] = LIKELIHOODS(W,B) where W are the log priors
% and B are the fit parameters. Pxf are the log-conditionals of the
% foreground model, Pxb are the log-conditionals of the background
% model, Px are the log-marginals and Pf is the log-prior for B.

    persistent D R C L P U %C0
    
    if nargin==6
      [D,R,C,L,P,U] = varargin{:};
      %C0 = (max(C)+min(C))/2; % Center of coordinates
      return;
    else
      [W,B] = varargin{:};
    end;
    
    %%%
    %%% First, calculate marginal for X=[R C] 
    %%% using mixing weights and current parameters
    %%%
    

    %-- Polynomial Model
    
    % Likelihood
    N = log(sqrt(2 * pi * L )); % Normalizer

    % Mean/prediction 
    switch D
      case 0
        M = B(1);
      case 1
        M = B(1) + B(2)*(C); 
      case 2
        M = B(1) + B(2)*(C) + B(3)*(C.^2);
      otherwise
        error('Unsupported degree');
    end
    
    Pxf = (-1/2 * (R-M).^2/L) - N ; % Gaussian likelihood
    
    % Prior 
    fin = ~isinf(P(:,2)); % Finite variances
    Pf = sum(-1/2 * (B-P(fin,1)).^2 ./P(fin,2) - log(sqrt(2*pi * P(fin,2))));
    
    % Evidence (Marginal) term
    Px = Pxf + W(1);
    
    %-- Background Model 
    % Uniform
    Pxb = -U;
    
    % Evidence (Marginal) term
    Px = logsumexp([Px ...
                    Pxb(ones(size(Px))) + W(2)], 2);

    
function L = likelihood(W,B,Z)
  
  [Pxf,Pxb,Px,Pf] = likelihoods(W,B);
  
  L = sum(Px) + Pf;
