function [minPos,fmin] = polyinterp(points,doPlot,xminBound,xmaxBound)

% function [minPos] = polyinterp(points,doPlot,xminBound,xmaxBound)

%

%   Minimum of interpolating polynomial based on function and derivative

%   values

%

%   In can also be used for extrapolation if {xmin,xmax} are outside

%   the domain of the points.

%

%   Input:

%       points(pointNum,[x f g])

%       doPlot: set to 1 to plot, default: 0

%       xmin: min value that brackets minimum (default: min of points)

%       xmax: max value that brackets maximum (default: max of points)

%

%   set f or g to sqrt(-1) if they are not known

%   the order of the polynomial is the number of known f and g values minus 1

if nargin < 2

    doPlot = 0;

end

nPoints = size(points,1);

order = sum(sum((imag(points(:,2:3))==0)))-1;

% Code for most common case:

%   - cubic interpolation of 2 points

%       w/ function and derivative values for both

%   - no xminBound/xmaxBound

if nPoints == 2 && order ==3 && nargin <= 2 && doPlot == 0

    % Solution in this case (where x2 is the farthest point):

    %    d1 = g1 + g2 - 3*(f1-f2)/(x1-x2);

    %    d2 = sqrt(d1^2 - g1*g2);

    %    minPos = x2 - (x2 - x1)*((g2 + d2 - d1)/(g2 - g1 + 2*d2));

    %    t_new = min(max(minPos,x1),x2);

    [minVal minPos] = min(points(:,1));

    notMinPos = -minPos+3;

    d1 = points(minPos,3) + points(notMinPos,3) - 3*(points(minPos,2)-points(notMinPos,2))/(points(minPos,1)-points(notMinPos,1));

    d2 = sqrt(d1^2 - points(minPos,3)*points(notMinPos,3));

    if isreal(d2)

        t = points(notMinPos,1) - (points(notMinPos,1) - points(minPos,1))*((points(notMinPos,3) + d2 - d1)/(points(notMinPos,3) - points(minPos,3) + 2*d2));

        minPos = min(max(t,points(minPos,1)),points(notMinPos,1));

    else

        minPos = mean(points(:,1));

    end

    return;

end

xmin = min(points(:,1));

xmax = max(points(:,1));

% Compute Bounds of Interpolation Area

if nargin < 3

    xminBound = xmin;

end

if nargin < 4

    xmaxBound = xmax;

end

% Constraints Based on available Function Values

A = zeros(0,order+1);

b = zeros(0,1);

for i = 1:nPoints

    if imag(points(i,2))==0

        constraint = zeros(1,order+1);

        for j = order:-1:0

            constraint(order-j+1) = points(i,1)^j;

        end

        A = [A;constraint];

        b = [b;points(i,2)];

    end

end

% Constraints based on available Derivatives

for i = 1:nPoints

    if isreal(points(i,3))

        constraint = zeros(1,order+1);

        for j = 1:order

            constraint(j) = (order-j+1)*points(i,1)^(order-j);

        end

        A = [A;constraint];

        b = [b;points(i,3)];

    end

end

% Find interpolating polynomial

params = A\b;

% Compute Critical Points

dParams = zeros(order,1);

for i = 1:length(params)-1

    dParams(i) = params(i)*(order-i+1);

end

if any(isinf(dParams))

    cp = [xminBound;xmaxBound;points(:,1)].';

else

    cp = [xminBound;xmaxBound;points(:,1);roots(dParams)].';

end

% Test Critical Points

fmin = inf;

minPos = (xminBound+xmaxBound)/2; % Default to Bisection if no critical points valid

for xCP = cp

    if imag(xCP)==0 && xCP >= xminBound && xCP <= xmaxBound

        fCP = polyval(params,xCP);

        if imag(fCP)==0 && fCP < fmin

            minPos = real(xCP);

            fmin = real(fCP);

        end

    end

end

% Plot Situation

if doPlot

    figure(1); clf; hold on;

    % Plot Points

    plot(points(:,1),points(:,2),'b*');

    % Plot Derivatives

    for i = 1:nPoints

        if isreal(points(i,3))

            m = points(i,3);

            b = points(i,2) - m*points(i,1);

            plot([points(i,1)-.05 points(i,1)+.05],...

                [(points(i,1)-.05)*m+b (points(i,1)+.05)*m+b],'c.-');

        end

    end

    % Plot Function

    x = min(xmin,xminBound)-.1:(max(xmax,xmaxBound)+.1-min(xmin,xminBound)-.1)/100:max(xmax,xmaxBound)+.1;

    size(x)

    for i = 1:length(x)

        f(i) = polyval(params,x(i));

    end

    plot(x,f,'y');

    axis([x(1)-.1 x(end)+.1 min(f)-.1 max(f)+.1]);

    % Plot Minimum

    plot(minPos,fmin,'g+');

    if doPlot == 1

        pause(1);

    end

end