% Input:
% S: function handle (Support function - 2-Dimensional)
% 
% gamma: double (defines Lgamma)
% epsilon: double (error tolerance)
% maxIter: Integer
% PI: Matrix
% S_nd: function handle (support function)

% Output:
% ymax: double

% Description:
% Dichotomic Search presented as algorithm 8.4 in the dissertation from Colas Le Guernic "Calcul d'Atteignabilité des Systèmes Hybrides à Partie Continue Linéaire" 2009 page 116 with error tolerance epsilon and max. number of
% iterations maxIter

function [ymax] = dichotomicSearch(S,~,~,gamma,epsilon,maxIter,PI,S_nd)

% initial values of dichotomic search
theta0 = 0;
theta1 = pi;

[~,~,Q] = S_nd(PI' * [cos(theta0);sin(theta0)]);
[~,~,P] = S_nd(PI' * [cos(theta1);sin(theta1)]);
Q = PI * Q;
P = PI * P;

ymin = -Inf;
ymax = Inf;

h = @(l) (S([cos(l);sin(l)])-gamma*cos(l))/sin(l);
count = 0;

while ymax- ymin > epsilon
  
    theta = choose(theta0,theta1,Q,P);
    [~,~,xy] = S_nd(PI' * [cos(theta);sin(theta)]);
    xy = PI * xy;
 
   if xy(1) < gamma
        P = xy;
        theta1 = theta;
    else
        Q = xy;
        theta0 = theta;
    end;
    
    % defines line PQ
    PQ(1) = (Q(2)-P(2))/((Q(1)-P(1)));
     
     if isnan(PQ(1))
         PQ(1) = 0;
     end;
     
     if PQ(1) == Inf || PQ(1) == -Inf
        PQ(2) = P(1);    
     else
        PQ(2) = P(2) - PQ(1)*P(1);
    end;
     
    
     % Intersect PQ and Lgamma
     intersection = intersectLines(PQ(1),PQ(2),Inf,gamma);
     gamma=intersection(1);
     ymin= intersection(2);
     
    [ymax] = min(  h(theta), ymax  );  
    
    count = count +1;
    
    if count > maxIter
        break;
    end;
end;


% Input:
% theta0: double (specifying an angle)
% theta1: double (specifying an angle)

% Output:
% result: double

% Description:
% Chooses the next theta to use
function [result] = choose(theta0,theta1,~,~)
result = (theta0+theta1)/2;
