% Input:
% s1,s2: scalar
% fun: function handle
% epsilon: scalar (error tolerance)

% Ouput:
% p1,p2,p3,p4: scalar

% Description:
% Performs the pivot selection according to Algorithm 3.3 for RayAlgo as descirbed in Ray's
% dissertation "Reachability Analysis of Hybrid Systems Using Support
% Functions" 2012 on page 42

function [p1,p2,p3,p4] = pivotSelection(s1,s2,fun,epsilon)
fs1 = fun(s1);
fs2 = fun(s2);

p1 = s1;
p4 = s2;

if fs1 > fs2
    
    p2 = p1 + (p4-p1)/2;
    s = p2 + (p4 - p2)/2;
    sprev = p2;
    
    while fun(p4) <= fun(s) 
        if collinear(p4,s,sprev,epsilon)
            p1 = p4;
            p2 = p4;
            p3 = p4;
            return;
        end;
        sprev = s;
        s = s+ (p4-s)/2;
    end;
    p3 = s;
    
elseif fs1 < fs2
    p3 = p1 + (p4 - p1)/2;
    s = p1 + (p3-p1)/2;
    sprev = p3;
    while fun(p1) <= fun(s)
        if collinear(p1,s,sprev,epsilon)
            p4 = p1;
            p3 = p1;
            p2 = p1;
            return;
        end;
        sprev = s;
        s = p1 + (s - p1)/2;
    end;
    p2 = s;
else
    p2 = p1 + (p4-p1)/3;
    p3 = p1 + 2*(p4-p1)/3;
    if fun(p2) == fun(p4) || fun(p3) == fun(p4)
        p1 = p4;
        p2 = p4;
        p3 = p4;
        return;
    end;
end;


% Input:
% a,b,c: vector
% epsilon: double (tolerance)

% Output:
% x: boolean

% Description:
% Checks if the points a,b and c are collinear with regard to tolerance epsilon
function [x] = collinear(a,b,c,epsilon)

if a == b || a == c || b == c
    x = false;
else

    if dot(a-b,a-c) < epsilon
        x = true;
    else
        x = false;
    end;
    
end;
        