% Input:
% l: double (lower bound)
% u: double (upper bound)
% f: function handle (convex function to minimize)
% e: double (tolerance)

% Output:
% min1: double

% Description:
% Sandwich algorithmus from Ray's dissertation "Reachability Analysis of Hybrid Systems Using Support Functions" 2012 (pages 42-53) returning the minimum of function f

function [min1] = RayAlgo(l,u,e,f)

[p1,p2,p3,p4] = pivotSelection(l,u,f,e);

if p1 == p2 && p2 == p3 && p3 == p4
    min1 = f(p1) ;
    return;
end;

state = 0;
p5 = p4;
p = 0;
min1 = 0;
min2 = 0;
ub = Inf;
lb = -Inf;

while ub - lb > e
    
    fp1 = f(p1);
    fp2 = f(p2);
    fp3 = f(p3);
    fp4 = f(p4);
    fp5 = f(p5);
    fp = f(p);
    
switch state 
case 0
    if equals(fp2,fp3,e) %fp2 == fp3
		state = 3;
    elseif fp2 > fp3
		state = 1;
	elseif fp2 < fp3
		state = 2;
	end;

case 1
	[k12,t12] = generateLineEquation([p1;fp1],[p2;fp2]);
	[k34,t34] = generateLineEquation([p3;fp3],[p4;fp4]);
	x = intersectLines(k12,t12,k34,t34);
	min1 = x(2);

	[k23,t23] = generateLineEquation([p2;fp2],[p3;fp3]);
	x = intersectLines(k23,t23,Inf,p4);
	min2 = x(2);

	lb = min(min1,min2);	%lower bound
	ub = min(min(fp1,fp2),min(fp3,fp4));	% upper bound

	%choose subinterval
	if min2< min1
		% interval p3p4
		p = bisection_rule(p3,p4);
		state = 12;

	else
		% interval p2p3
		p = bisection_rule(p2,p3);
		state = 11;
	end;

case 2
    % TODO: S 60
    
    [k12,t12] = generateLineEquation([p1;fp1],[p2;fp2]);
	[k34,t34] = generateLineEquation([p3;fp3],[p4;fp4]);
	x = intersectLines(k12,t12,k34,t34);
	min1 = x(2);

	[k23,t23] = generateLineEquation([p2;fp2],[p3;fp3]);
	x = intersectLines(k23,t23,Inf,p1);
	min2 = x(2);

	lb = min(min1,min2);	%lower bound
	ub = min( min(fp1,fp2),min(fp3,fp4));	% upper bound

	%choose subinterval
	if min2< min1
		% interval p1p2
		p = bisection_rule(p1,p2);
		state = 22;

	else
		% interval p2p3
		p = bisection_rule(p2,p3);
		state = 21;
	end; 
    
case 3
    [k12,t12] = generateLineEquation([p1;fp1],[p2;fp2]);
	[k34,t34] = generateLineEquation([p3;fp3],[p4;fp4]);
	x = intersectLines(k12,t12,k34,t34); %x = [p;low] TODO p über maximum error rule !
    
    p = x(1);
    fp = f(p);
    lb = x(2);
    ub = min(min(fp1,fp2),min(fp3,fp4));
    
    if equals(fp,fp2,e)
        %fmin = fp;
        min1 = fp;
        state = 1000;
    else
        p5 = p4;
        p4 = p3;
        p3 = p;
        state = 4;
    end;
    
case 4
    [k12,t12] = generateLineEquation([p1;fp1],[p2;fp2]);
	[k34,t34] = generateLineEquation([p3;fp3],[p4;fp4]);
    x = intersectLines(k12,t12,k34,t34);
    x1 = x(1);
    min1 = x(2);
    
    [k23,t23] = generateLineEquation([p2;fp2],[p3;fp3]);
	[k45,t45] = generateLineEquation([p4;fp4],[p5;fp5]);
    x = intersectLines(k23,t23,k45,t45);
    x2 = x(1);
    min2 = x(2);
  
    lb = min(min1,min2);
    ub = fp3;
    
    if equals(min1,min2,e) 
        min1 = fp3;
        state = 1000;
    elseif min1 < min2
        %p2p3
        p = x1; % TODO p über maximum eror rule
        state = 41;
        
    else
        %p3p4
        p = x2;
        state = 42;
    end;
    
case 11
	% Betrachte Umbenennung
    if equals(fp,fp3,e) %fp == fp3
		p1 = p2;
		p2 = p;
		state = 3;
    elseif fp < fp3
		p5 = p4;
		p4 = p3;
		p3 = p;
		state = 4;
	else
		p1 = p2;
		p2 = p;
		state = 1;
	end;

    case 12
	% Betrachte Umbenennung
        if equals(fp,fp3,e)
            p1 = p2;
            p2 = p3;
            p3 = p;
            state = 3;
        elseif fp < fp3
            p1 = p2;
            p2 = p3;
            p3 = p;
            state = 1;
        else
            p5 = p4;
            p4 = p;
            state = 4;
        end;
    
    case 21 % TODO: stimmen die folge-states ?
        % Betrachte Umbenennung
        if equals(fp,fp2,e)
            p4 = p3;
            p3 = p;
            state = 3;
        elseif fp < fp2% TODO stimmen Umbenennungen
            p5 = p4;
            p4 = p3;
            p3 = p;
            state = 4;
        else
            %p5 = p4;
            p4 = p3;
            p3 = p;
            state = 2;
        end;

    case 22
        if equals(fp,fp2,e)
            p4 = p3;
            p3 = p2;
            p2 = p;
            state = 3;
        elseif fp < fp2
            p4 = p3;
            p3 = p2;
            p2 = p;
            state = 2;
        else
            %p5 = p4;
            p4 = p3;
            p3 = p2;
            p2 = p;
            state = 4;
        end;
    
    case 41
        %p2p3
        if equals(fp,fp3,e)
            p1 = p2;
            p2 = p;
            state = 3;
        elseif fp > fp3
            p1 = p2;
            p2 = p;
            state = 4;
        else
            p5 = p4;
            p4 = p3;
            p3 = p;
            state = 4;
        end;
   %     break;
        
    case 42
        if equals(fp,fp3,e)
            p1 = p2;
            p2 = p3;
            p3 = p;
            state = 3;
        elseif fp > fp3
            p5 = p4;
            p4 = p;
            state = 4;
        else
            p1 = p2;
            p2 = p3;
            p3 = p;
            state = 4;
        end;
    %    break;
        
    case 1000 % STOP state
        return;
       
end;
end;



% Input:
% p1,p2: double (abscissa of a point)

% Output:
% x: double

% Description:
% Returns the abscissa of the point in the middle between p1 and p2
function [x] = bisection_rule(p1,p2)
x = (p1+p2)/2;



% Input:
% a,b: double

% Output:
% x: boolean

% Description:
% determines if a and b are equal with regard to a specified tolerance
function [x]= equals(a,b,tolerance)

if abs(a-b) < tolerance
    x = true;
else
    x = false;
end;
