function cor_list = UR_shortest_path_cor(v1,v2,airways)
% UR_shortest_path_cor - Use Dykstra's algorithm to find shortest cor path
% On input:
%     v1 (int): index for road launch vertex
%     v2 (int): index for road land vertex
%     airways (airways data structure): describes airway
% On output:
%     cor_list (nx1 vector): list of corridors to traverse
% Call:
%     cors = UR_shortest_path_cor(1,2,aw);
% Author:
%     T. Henderson
%     UU
%     Summer 2019
%

UPPER_ZONE = 1;
LOWER_ZONE = 2;
ENTER_ZONE = 3;
EXIT_ZONE = 4;
CIRCLE_ZONE = 5;
LAUNCH_ZONE = 6;
LAND_ZONE = 7;
MIN_DIST = 0.5;

path = [];

cors = airways.corridors;
num_cors = length(cors(:,1));
v_ind1 = v1;
v_ind2 = v2;

vertexes = airways.cor_vertexes;
edges = airways.cor_edges;
indexes_up = find(cors(:,7)==LAUNCH_ZONE&cors(:,10)==v_ind1);
indexes_down = find(cors(:,7)==LAND_ZONE&cors(:,11)==v_ind2);
heights = cors(indexes_up,3);
[vals,indexes] = sort(heights,'ascend');
c_ind1 = cors(indexes_up(indexes(1)),8);
pt1 = vertexes(c_ind1,1:3);
heights = cors(indexes_down,6);
[vals,indexes] = sort(heights,'ascend');
c_ind2 = cors(indexes_down(indexes(1)),9);
pt2 = vertexes(c_ind2,1:3);

launch_vertexes = airways.launch_vertexes;
land_vertexes = airways.land_vertexes;
num_vertexes = length(vertexes(:,1));
num_edges = length(edges(:,1));

if isempty(find(launch_vertexes==v_ind1))
    return
end
if isempty(find(land_vertexes==v_ind2))
    return
end

unvisited = [1:num_vertexes];
visited = [];
distance = Inf*ones(num_vertexes,1);
current = c_ind1;
distance(current) = 0;
previous = zeros(num_vertexes,1);

done = 0;
while done==0
    x_cur = vertexes(current,1);
    y_cur = vertexes(current,2);
    z_cur = vertexes(current,3);
%    plot3(x_cur,y_cur,z_cur,'r*');
    indexes1 = find(current==edges(:,1));
    neighbors = unique([edges(indexes1,2)]');
    num_neighbors = length(neighbors);
    nei_un = [];
    for n = 1:num_neighbors
        if isempty(find(neighbors(n)==visited))
            nei_un = [nei_un,neighbors(n)];
        end
    end
    [current,nei_un];
    num_nei_un = length(nei_un);
    for n = 1:num_nei_un
        index = nei_un(n);
        d = norm([x_cur,y_cur,z_cur]-...
            [vertexes(index,1),vertexes(index,2),vertexes(index,3)])...
            + distance(current);
        if d<distance(index)
            distance(index) = d;
            previous(index) = current;
        end
    end
    index = find(current==unvisited);
    unvisited(index) = [];
    visited = [visited,current];
    t_min = Inf;
    num_unvisited = length(unvisited);
    for n = 1:num_unvisited
       %if distance(unvisited(n))<Inf
        if distance(n)<Inf
            t_min = distance(n);
            break;
        end
    end
    if ~isempty(find(visited==c_ind2))
        path = [c_ind2];
        pr = previous(c_ind2);
        while pr~=c_ind1
            path = [pr,path];
            pr = previous(pr);
        end
        path = [c_ind1,path];
        done = 1;
    elseif t_min==Inf
        % no solution
        return
    else
        num_unvisited = length(unvisited);
        d_min = Inf;
        d_ind = 0;
        for n = 1:num_unvisited
            index = unvisited(n);
            if distance(index)<d_min
                d_min = distance(index);
                d_ind = index;
            end
        end
        current = d_ind;
    end
    tch = 0;
end

len_path = length(path);
cor_list = zeros(len_path-1,1);
for s = 1:len_path-1
    cor_list(s) = find(cors(:,8)==path(s)&cors(:,9)==path(s+1));
end
tch = 0;
