% FUNCTION [X_FIT, Y_FIT] = PARABOLIC_FIT(X, Y)
% 
% Parabolic fit on sampled function: Y = F(X).
% 
% Input:    X   3-by-N  with X(1,j) < X(2,j) < X(3,j)
%           Y   3-by-N  with Y = F(X)
%
% Output:   X_FIT   1-by-N  with X(1,j) <= X_FIT(j) <= X(3,j)
%           Y_FIT   1-by-N  with Y_FIT ~ F(X_FIT)

function [x_fit, y_fit] = parabolic_fit(x, y)

if nargin < 2
  error('parabolic_fit needs 2 parameters');
end

if prod( (x(1,:)<x(2,:)) & (x(2,:)<x(3,:)) ) < 1
  error('parabolic_fit needs x(1,j) < x(2,j) < x(3,j)');
end

% x(2,:) = x(1,:) + h(1,:)
% x(3,:) = x(1,:) + h(2,:)

h(1,:) = x(2,:) - x(1,:);
h(2,:) = x(3,:) - x(1,:);

% Polynomial
% Pj(x) = a + b ( x - x(1,j) ) + x ( x - x(1,j) ) * ( x - x(2,j) )

a = y(1,:)
b = ( y(2,:) - y(1,:) ) ./ h(1,:)
c = ( y(3,:) - y(1,:) ) ./ ( h(1,:) .* h(2,:) ) - b ./ h(1,:)

x_fit = repmat(NaN, 1, size(x,2));
y_fit = repmat(NaN, 1, size(x,2));

% 1) Flat case

aux = find( (c == 0) & (b == 0) );

if ~isempty(aux)
  x_fit(aux) = x(2,:);
  y_fit(aux) = y(2,:);
end
  
% 2) Non-flat linear or concave case
  
aux = find( ( c >= 0 ) & ( b ~= 0 ) );

if ~isempty(aux)
  n = length(aux);
  
  [aux2, index] = max( y(:,aux), [], 1 );
  y_fit(aux) = aux2;
  aux2 = x(:,aux);
  x_fit(aux) = aux2( sub2ind( [3 n], index, 1:n ) );
end

% 3) Convex case -> return local maxima
%    This local maxima is between x(1,j) and x(3,j)
%    since the parabola is convex

aux = find( (c < 0) & (b ~= 0) );

if ~isempty(aux)
  h0 = h(1,aux) / 2 - b(aux) ./ (2 * c(aux));
  x_fit(aux) = x(1,aux) + h0;
  y_fit(aux) = a(aux) + b(aux) .* h0 + c(aux) .* h0 .* (h0-h(1,aux));
end


% DEBUG
%figure;
%for z = 1:length(x_fit)
%  clf;
%  plot(x(2,:),y(2,:)); hold on;
%  xx = x(1,z) + (0:100) * (x(3,z) - x(1,z)) / 100;
%  yy = a(z) + b(z) * (xx-x(1,z)) + c(z) * (xx - x(1,z)) .* (xx - x(2,z));
%  plot(xx,yy,'r');
%  stem(x_fit(z), y_fit(z), 'g');
%  pause;
%end