function f = bug_2;

global obstacles
global startloc;
global endloc;
s = startloc; e = endloc;
pos = s;
i = 0;

path = [];

disp(obstacles)

while( ~(pos(1) == e(1) && pos(2) == e(2)) )
    newpos = e;
    dist = Inf;
    cur_obstacle = 0;
    cur_vertex = 0;
    obstacle_int = 0;
    %disp(sprintf('while loop pos=%d',pos))
    path = [path, [pos(1);pos(2)]];
    end_dist = ((e(2)-pos(2))^2 + (e(1)-pos(1))^2)^.5;
    %draw a line from me to goal
    g_m = (e(2) - pos(2))/(e(1) - pos(1));
    %for all obstacles
    for m = 1:length(obstacles)
        disp(obstacles(m))
        x_values = obstacles(m).xVals;
        y_values = obstacles(m).yVals;
        %for all obstacle segments
        for n = 1:(length(x_values) - 1)
            slope = (y_values(n+1)-y_values(n))/(x_values(n+1)-x_values(n));
            if (slope == Inf || slope == -Inf)
                intersection = [x_values(n);g_m*(x_values(n) - pos(1)) + pos(2)];
            else
                intersection = [-(g_m),1;-slope,1]...
                    \[pos(2) - g_m*pos(1);y_values(n) - slope*x_values(n)];
            end
            %if goal line intersects between points AND between pos and goal
            %ie: if x,y coords less than bigger seg x,y and greater than
            %smaller seg x,y
            %disp(intersection)
            if ( ((intersection(1) <= max([x_values(n),x_values(n+1)])) &&...
                    (intersection(1) >= min([x_values(n),x_values(n+1)])) &&...
                    (intersection(2) <= max([y_values(n),y_values(n+1)])) &&...
                    (intersection(2) >= min([y_values(n),y_values(n+1)]))) && ...
                    ((intersection(1) <= max([pos(1),e(1)])) &&...
                    (intersection(1) >= min([pos(1),e(1)])) &&...
                    (intersection(2) <= max([pos(2),e(2)])) &&...
                    (intersection(2) >= min([pos(2),e(2)]))) )
                int_dist = ((intersection(1) - pos(1))^2 + (intersection(2) - pos(2))^2)^.5;
                disp(intersection)
                %even if not closest, we'll need them later
                obstacles(m).intersections = [obstacles(m).intersections,intersection];
                %if min distance
                if (int_dist < dist && int_dist > .01)
                    %update min intersection point, obstacle, x/y vals
                    dist = int_dist;
                    %qH = [qH,intersection'];
                    newpos = intersection;
                    disp(newpos)
                    cur_obstacle = m;
                    cur_vertex = n;
                    obstacle_int = length(obstacles(m).intersections);
                    i = i+1;
                end
            end
        end
    end
    pos = newpos;
    path = [path,[newpos(1);newpos(2)]];
    %disp(sprintf('ob int newpos=%d',newpos))
    if (pos(1) == e(1) && pos(2) == e(2));
        %helpdlg('Success!');
        f = 'Success!';
        %disp 'success (no obstacles in the way)';
        line(path(1,:),path(2,:),'Color','k','LineWidth',3);
        return;
    end
    %if we haven't exited, we hit something.
    %new position contains the hit location.  we need to follow that
    %obstacle by turning right along the line.  THat means turning toward
    %the correct vertex of the obstacle.  
    %Four cases based on slope:
    if (cur_vertex == 0)
            cur_vertex = length(obstacles(cur_obstacle).xVals) - 1;
        end
        if (cur_vertex == length(obstacles(cur_obstacle).xVals))
            cur_vertex = 1;
        end
    travel_dir = 0;
    obslope = (obstacles(cur_obstacle).yVals(cur_vertex) - pos(2))/...
        (obstacles(cur_obstacle).xVals(cur_vertex) - pos(1));
    obslope
    myslope = (e(2) - pos(2))/(e(1) - pos(1));
    %Case1: Int. Line has a negative slope: Choose vertex w/ same x dir
    if (obslope < 0 && obslope > -Inf)
        if (e(1) > pos(1)) %if x is increasing
            if (obstacles(cur_obstacle).xVals(cur_vertex) > obstacles(cur_obstacle).xVals(cur_vertex + 1))
                travel_dir = -1;
            else
                travel_dir = 1;
            end
        else    %if x is decreasing
            if (obstacles(cur_obstacle).xVals(cur_vertex) > obstacles(cur_obstacle).xVals(cur_vertex + 1))
                travel_dir = 1;
            else
                travel_dir = -1;
            end
        end
    %Case 2: Pos slope: reverse x direction
    elseif (obslope > 0 && obslope < Inf)
        if (e(1) < pos(1)) %if x is decreasing
            if (obstacles(cur_obstacle).xVals(cur_vertex) > obstacles(cur_obstacle).xVals(cur_vertex + 1))
                travel_dir = -1;
            else
                travel_dir = 1;
            end
        else    %if x is increasing
            if (obstacles(cur_obstacle).xVals(cur_vertex) > obstacles(cur_obstacle).xVals(cur_vertex + 1))
                travel_dir = 1;
            else
                travel_dir = -1;
            end
        end
     %Case 3: slope = 0: x direction = y direction
    elseif (obslope == 0)
        if (e(2) > pos(2)) %ydir is +, choose next for x to increase
            if (obstacles(cur_obstacle).xVals(cur_vertex) > obstacles(cur_obstacle).xVals(cur_vertex + 1))
                travel_dir = -1;
            else
                travel_dir = 1;
            end
        else    %if ydir is -, choose next for x to decrease
            if (obstacles(cur_obstacle).xVals(cur_vertex) > obstacles(cur_obstacle).xVals(cur_vertex + 1))
                travel_dir = 1;
            else
                travel_dir = -1;
            end
        end
    else  %slope == +- Inf: change y dir if x&y dif
        if (e(1) > pos(1)) %x incr
            if (e(2) > pos(2)) %y incr
                if (obstacles(cur_obstacle).yVals(cur_vertex) > obstacles(cur_obstacle).yVals(cur_vertex + 1))
                    travel_dir = 1;
                else
                    travel_dir = -1;
                end
            elseif (e(2) < pos(2))  %y decr
                if (obstacles(cur_obstacle).yVals(cur_vertex) > obstacles(cur_obstacle).yVals(cur_vertex + 1))
                    travel_dir = 1;
                else
                    travel_dir = -1;
                end
            else  %y not changing
                if (obstacles(cur_obstacle).yVals(cur_vertex) > obstacles(cur_obstacle).yVals(cur_vertex + 1))
                    travel_dir = 1;
                else
                    travel_dir = -1;
                end
            end
        elseif (e(1) < pos(1))
            if (e(2) > pos(2)) %y incr
                if (obstacles(cur_obstacle).yVals(cur_vertex) > obstacles(cur_obstacle).yVals(cur_vertex + 1))
                    travel_dir = -1;
                else
                    travel_dir = 1;
                end
            elseif (e(2) < pos(2))  %y decr
                if (obstacles(cur_obstacle).yVals(cur_vertex) > obstacles(cur_obstacle).yVals(cur_vertex + 1))
                    travel_dir = -1;
                else
                    travel_dir = 1;
                end
            else  %y not changing
                if (obstacles(cur_obstacle).yVals(cur_vertex) > obstacles(cur_obstacle).yVals(cur_vertex + 1))
                    travel_dir = -1;
                else
                    travel_dir = 1;
                end
            end
        else %x not changing
            if(e(2) > pos(2))
                if (obstacles(cur_obstacle).xVals(cur_vertex) > obstacles(cur_obstacle).xVals(cur_vertex + 1))
                    travel_dir = -1;
                else
                    travel_dir = 1;
                end
            else
                if (obstacles(cur_obstacle).xVals(cur_vertex) > obstacles(cur_obstacle).xVals(cur_vertex + 1))
                    travel_dir = -1;
                else
                    travel_dir = 1;
                end
            end
        end
    end
    %disp(sprintf('initial vertex: %d', [obstacles(cur_obstacle).xVals(cur_vertex),obstacles(cur_obstacle).yVals(cur_vertex)]))
    %if initial vertex actually to the left, inc cur_vertex
    %disp(sprintf('traveldir: %d',travel_dir))
    initial_vertex = cur_vertex;
    if (travel_dir == 1)
        cur_vertex = cur_vertex + travel_dir;
    end
    if (cur_vertex == 0)
        cur_vertex = length(obstacles(cur_obstacle).xVals) - 1;
    end
    if (cur_vertex == length(obstacles(cur_obstacle).xVals))
        cur_vertex = 1;
    end
    
    %Now we run around the obstacle until we encounter our original goal
    %line between our hit and our goal
    around = 1;
    while(around == 1)
        path = [path, [obstacles(cur_obstacle).xVals(cur_vertex);obstacles(cur_obstacle).yVals(cur_vertex)]];
        next_vertex = cur_vertex + travel_dir;
        if (next_vertex == 0)
            next_vertex = length(obstacles(cur_obstacle).xVals) - 1;
        end
        if (next_vertex == length(obstacles(cur_obstacle).xVals))
            next_vertex = 1;
        end
        k = 1;
        while(k <= length(obstacles(cur_obstacle).intersections(1,:)) && around == 1)
            obstacles(cur_obstacle).intersections
            k
            cur_int = [obstacles(cur_obstacle).intersections(:,k)];
            %disp(sprintf('current intersection: %d',cur_int))
            cur_dist = ((e(1) - cur_int(1))^2 + (e(2) - cur_int(2))^2)^.5;
            end_dist = ((e(2)-pos(2))^2 + (e(1)-pos(1))^2)^.5;
            %if the intresection is between these points AND it is closer
            %to goal
            %disp(sprintf('current vertex: %d',[obstacles(cur_obstacle).xVals(cur_vertex),obstacles(cur_obstacle).yVals(cur_vertex)]))
            %disp(sprintf('next vertex: %d',[obstacles(cur_obstacle).xVals(next_vertex),obstacles(cur_obstacle).yVals(next_vertex)]))
            y_cur_vert = obstacles(cur_obstacle).yVals(cur_vertex);
            x_cur_vert = obstacles(cur_obstacle).xVals(cur_vertex);
            y_next_vert = obstacles(cur_obstacle).yVals(next_vertex);
            x_next_vert = obstacles(cur_obstacle).xVals(next_vertex);
            
            if (abs((cur_int(2)-y_cur_vert - (y_next_vert-y_cur_vert)/(x_next_vert-x_cur_vert)...
                    * (cur_int(1)-x_cur_vert))) <= .001 && cur_dist < end_dist)
                %then we move there and stop going around
                pos = cur_int;
                disp(sprintf('back on line pos= %d', pos))
                around = 0;
                
            elseif ( (x_next_vert-x_cur_vert) == 0 && ...
                    (cur_int(2)>min([y_cur_vert y_next_vert])) && (cur_int(2)<max([y_cur_vert y_next_vert]))...
                    && cur_dist < end_dist)
                %then we move there and stop going around
                pos = cur_int;
                disp(sprintf('back on line pos= %d', pos))
                around = 0;
            else
                k = k+1;
            end
        end
        cur_vertex = cur_vertex + travel_dir;       
        if (cur_vertex == 0)
            cur_vertex = length(obstacles(cur_obstacle).xVals) - 1;
        end
        if (cur_vertex == length(obstacles(cur_obstacle).xVals))
            cur_vertex = 1;
        end
        if (cur_vertex == initial_vertex)
            %errordlg('NO PATH TO GOAL');
            %disp('NO PATH TO GOAL');
            f = 'NO PATH TO GOAL';
            return;
        end
        disp(sprintf('new current vertex: %d',[obstacles(cur_obstacle).xVals(cur_vertex),obstacles(cur_obstacle).yVals(cur_vertex)]))
    end
    %Our position has been updated on the far side of the obstacle along
    %the line to the goal.  We now repeat the process

end
disp('success');
return;