
function [newnext, m2, b2]=nextwaypoint(next,xwp,ywp,X,Y)
% function [newnext]=nextwaypoint(next,next,xwp,ywp,X,Y)
% Function to determine if the aircraft has passed the 
% waypoint with index next. If so, next is incremented by 1.
% If not, next is unchanged.

if ywp(next)-ywp(next-1)==0
    % waypoint path line is vertical
    %disp('The waypoint line is vertical')
    m2=0;
    b2=xwp(next);
    xnextm1=xwp(next-1)-b2;
    xa=X-b2;
    if sign(xnextm1)==sign(xa)
        %disp('Aircraft is in Region 1')
        newnext=next;
    else
        %disp('Aircraft is in Region 2')
        newnext=next+1;
    end
else
    % waypoint path line is not vertical
    if xwp(next)-xwp(next-1)==0
        % waypoint path is horizontal, the perpendicular is vertical
        %disp('The waypoint line is horizontal')
        m2=inf;
        b2=0;
        if sign(ywp(next)-ywp(next-1))==sign(ywp(next)-Y)
            %disp('Aircraft is in Region 1')
            newnext=next;
        else
            %disp('Aircraft is in Region 2')
            newnext=next+1;
        end      
    else
        % waypoint path is not vertical
        m1=(xwp(next)-xwp(next-1))/(ywp(next)-ywp(next-1));
        b1=xwp(next)-m1*ywp(next);
        m2=-1/m1;
        b2=xwp(next)-m2*ywp(next);
        xnextm1=xwp(next-1)-(m2*ywp(next-1)+b2);
        xa=X-(m2*Y+b2);
        if sign(xnextm1)==sign(xa)
            %disp('Aircraft is in Region 1')
            newnext=next;
        else
            %disp('Aircraft is in Region 2')
            newnext=next+1;
        end
    end
end