% Unit 4 (MATLAB Course @ UAM)
% (c) Alexander Knebe

function [fp]=p4interpol(x,f,xp)

% we may have a vector of points xp
fp = zeros(1,length(xp));

for ip=1:length(xp)
    % find xp in array x()
    il  = find(xp(ip)>x);
    ir  = find(xp(ip)<x);
    
    % check whether we have either 2 points to the left or 2 points to the right
    if length(il)<2 | length(ir)<2
        fp(ip)=-1e40; % some ridiculous number
    else
        % get the relevant indizes (2 left of xp, 2 right of xp)
        il  = max(il);
        ill = il-1;
        ir  = il+1;
        irr = il+2;
        
        % generate matrix
        A = [1 x(ill) x(ill).^2 x(ill).^3;
            1 x(il)  x(il).^2  x(il).^3;
            1 x(ir)  x(ir).^2  x(ir).^3;
            1 x(irr) x(irr).^2 x(irr).^3];
        
        % calculate coefficients an
        a = inv(A)*[f(ill); f(il); f(ir); f(irr)];
        
        % use polynomial to calculate f(xp)
        fp(ip) = a(1)+a(2).*xp(ip)+a(3).*xp(ip).^2+a(4).*xp(ip).^3;
    end
end
end