function airways_out = UR_gen_corridors1(airways)
% UR_gen_corridors - create airway corridors (lanes)
% On input:
%     airways (airway data structure): only has roads
% On output:
%     airways_out (airway data structure): has corridors
%       .vertexes (nx2 array): roadway vertexes (eventually needs 3D)
%       .edges (kx2 array): edges between road vertexes
%       .corridors (px9 array): corridor definitions
%          [x1 y1 z1 x2 y2 z2 zone cor_ver1 cor_ver2 rd_ver1 rd_ver2]
%       .cor_vertexes (qx3 array): corridor vertexes
%       .cor_edges (mx2 array): corridor edges
%       .launch_vertexes (sx1 vector): possible launch (road) vertexes
%       .land_vertexes (tx1 vector): possible landing (road) vertexes
% Call:
%     g_airways = UR_gen_corridors(g_airways);
% Author:
%     T. Henderson
%     UU
%     Summer 2019
%

global g_radius g_offset g_del
global g_z_upper g_z_lower g_z_circle g_z_launch g_z_land 

UPPER_ZONE = 1;
LOWER_ZONE = 2;
ENTER_ZONE = 3;
EXIT_ZONE = 4;
CIRCLE_ZONE = 5;
LAUNCH_ZONE = 6;
LAND_ZONE = 7;
Z_UPPER = g_z_upper; %501; % 534
Z_LOWER = g_z_lower; %499; % 467
Z_CIRCLE = g_z_circle; %500;
Z_LAUNCH = g_z_launch; %498;
Z_LAND = g_z_land; %498;
ZONE_INDEX = 7;
COR_HEAD_INDEX = 8;
COR_TAIL_INDEX = 9;
VERTEX1_INDEX = 10;
VERTEX2_INDEX = 11;

airways_out = airways;

vertexes = airways.vertexes;
num_vertexes = length(vertexes(:,1));
for v = 1:num_vertexes
    circle_points(v).points = [];
end

edges = airways.edges;
num_edges = length(edges(:,1));
corridors = [];
num_corridors = 0;
cor_vertexes = [];
cor_edges = [];
for e = 1:num_edges
    index1 = edges(e,1);
    index2 = edges(e,2);
    if index1==1
        tch = 0;
    end
    v1 = [vertexes(index1,:)]';
    v2 = [vertexes(index2,:)]';
    dir = v2 - v1;
    dir = dir/norm(dir);
    P1 = v1 + dir*(g_radius + g_offset);
    P2 = v2 - dir*(g_radius + g_offset);
    up = [-dir(2);dir(1);0];
    down = [dir(2);-dir(1);0];
    P3 = v1 + dir*g_radius;
    P11 = P3 + g_del*up;
    P12 = P3 + g_del*down;
    P4 = v2 - dir*g_radius;
    P21 = P4 + g_del*up;
    P22 = P4 + g_del*down;

    angle = posori(atan2(dir(2),dir(1)));
    if angle>=0&angle<pi
        V1_zone = UPPER_ZONE;
        V1_z = Z_UPPER;
        V2_zone = LOWER_ZONE;
        V2_z = Z_LOWER;
    else
        V1_zone = LOWER_ZONE;
        V1_z = Z_LOWER;
        V2_zone = UPPER_ZONE;
        V2_z = Z_UPPER;
    end
    
    PC11 = P1';
    PC11(3) = V1_z;
    cor_vertexes = [cor_vertexes;PC11];
    PC11_index = length(cor_vertexes(:,1));
    PC12 = P2';
    PC12(3) = V1_z;
    cor_vertexes = [cor_vertexes;PC12];
    PC12_index = length(cor_vertexes(:,1));
    cor_edges = [cor_edges;PC11_index,PC12_index];

    PC21 = P1';
    PC21(3) = V2_z;
    cor_vertexes = [cor_vertexes;PC21];
    PC21_index = length(cor_vertexes(:,1));
    PC22 = P2';
    PC22(3) = V2_z;
    cor_vertexes = [cor_vertexes;PC22];
    PC22_index = length(cor_vertexes(:,1));
    cor_edges = [cor_edges;PC21_index,PC22_index];
    
    P11c = P11';
    P11c(3) = Z_CIRCLE;
    cor_vertexes = [cor_vertexes;P11c];
    P11c_index = length(cor_vertexes(:,1));
    P12c = P12';
    P12c(3) = Z_CIRCLE;
    cor_vertexes = [cor_vertexes;P12c];
    P12c_index = length(cor_vertexes(:,1));
    cor_edges = [cor_edges;P11c_index,P12c_index];

    P21c = P21';
    P21c(3) = Z_CIRCLE;
    cor_vertexes = [cor_vertexes;P21c];
    P21c_index = length(cor_vertexes(:,1));
    P22c = P22';
    P22c(3) = Z_CIRCLE;
    cor_vertexes = [cor_vertexes;P22c];
    P22c_index = length(cor_vertexes(:,1));
    cor_edges = [cor_edges;P21c_index,P22c_index];

    % Corridor 1
    num_corridors = num_corridors + 1;
    corridors(num_corridors,:) = ...
        [PC11,PC12,V1_zone,PC11_index,PC12_index,index1,index2];
    cor_edges = [cor_edges; PC11_index,PC12_index];
    % Corridor 2
    num_corridors = num_corridors + 1;
    corridors(num_corridors,:) = ...
        [PC22,PC21,V2_zone,PC22_index,PC21_index,index1,index2];
    cor_edges = [cor_edges; PC22_index,PC21_index];
    circle_points(index1).points = ...
        [circle_points(index1).points;P11c_index;P12c_index];
    circle_points(index2).points = ...
        [circle_points(index2).points;P21c_index;P22c_index];
    if V1_z==Z_UPPER
        % Corridor 3 (option 1)
        num_corridors = num_corridors + 1;
        corridors(num_corridors,:) = [P11c,PC11,EXIT_ZONE,...
            P11c_index,PC11_index,index1,index2];
        cor_edges = [cor_edges; P11c_index,PC11_index];
        exits(index1).index = P11c_index;
        % Corridor 4 (option 1)
        num_corridors = num_corridors + 1;
        corridors(num_corridors,:) = [PC21,P12c,ENTER_ZONE,...
            PC21_index,P12c_index,index1,index2];
        cor_edges = [cor_edges; PC21_index,P12c_index];
        entries(index1).index = P12c_index;
        % Corridor 5 (option 1)
        num_corridors = num_corridors + 1;
        corridors(num_corridors,:) = [PC12,P22c,ENTER_ZONE,...
            PC12_index,P22c_index,index1,index2];
        cor_edges = [cor_edges; PC12_index,P22c_index];
        entries(index2).index = P22c_index;
        % Corridor 6 (option 1)
        num_corridors = num_corridors + 1;
        corridors(num_corridors,:) = [P21c,PC22,EXIT_ZONE,...
            P21c_index,PC22_index,index1,index2];
        cor_edges = [cor_edges; P21c_index,PC22_index];
        exits(index2).index = P21c_index;
    else
        % Corridor 3 (option 2)
        num_corridors = num_corridors + 1;
        corridors(num_corridors,:) = [PC21,P11c,ENTER_ZONE,...
            PC21_index,P11c_index,index1,index2];
        cor_edges = [cor_edges; PC21_index,P11c_index];
        entries(index1).index = P11c_index;
        % Corridor 4 (option 2)
        num_corridors = num_corridors + 1;
        corridors(num_corridors,:) = [P12c,PC11,EXIT_ZONE,...
            P12c_index,PC11_index,index1,index2];
        cor_edges = [cor_edges; P12c_index,PC11_index];
        exits(index1).index = P12c_index;
        % Corridor 5 (option 2)
        num_corridors = num_corridors + 1;
        corridors(num_corridors,:) = [PC12,P21c,ENTER_ZONE,...
            PC12_index,P21c_index,index1,index2];
        cor_edges = [cor_edges; PC12_index,P21c_index];
        entries(index2).index = P21c_index;
        % Corridor 6 (option 2)
        num_corridors = num_corridors + 1;
        corridors(num_corridors,:) = [P22c,PC22,EXIT_ZONE,...
            P22c_index,PC22_index,index1,index2];
        cor_edges = [cor_edges; P22c_index,PC22_index];
        exits(index2).index = P22c_index;
    end
end

% sort circle points by angle
for v = 1:num_vertexes
    indexes = circle_points(v).points;
    pts = cor_vertexes(indexes,:);
    center = vertexes(v,1:2);
    num_pts = length(pts(:,1));
    if num_pts<3
        ptc = vertexes(v,:);
        ptc(3) = pts(1,3);
        ptm = (pts(1,:)+pts(2,:))/2;
        dir = ptm - ptc;
        dir = -dir/norm(dir);
        pt3 = ptc + g_radius*dir;
        cor_vertexes = [cor_vertexes; pt3];
        ind3 = length(cor_vertexes(:,1));
        circle_points(v).points = [circle_points(v).points;ind3];
        pts(3,:) = pt3;
        num_pts = 3;
    end
    angles = [];
    for p = 1:num_pts
        angles(p) = posori(atan2(pts(p,2)-center(2),pts(p,1)-center(1)));
    end
    [vals,s_indexes] = sort(angles);
    try
        circle_points(v).points = circle_points(v).points(s_indexes);
    catch
        tch = 0;
    end
end

% create circle corridors
for v = 1:num_vertexes
    indexes = circle_points(v).points;
    pts = cor_vertexes(indexes,:);        
    [num_pts,dummy] = size(pts);
    for p = 1:num_pts-1
        % Create roundabout corridor
        num_corridors = num_corridors + 1;
        corridors(num_corridors,:) = [pts(p,:),...
            pts(p+1,:),CIRCLE_ZONE,indexes(p),indexes(p+1),v,v];
        cor_edges = [cor_edges; indexes(p),indexes(p+1)];
    end
    % Create corridor to complete roundabout connections
    num_corridors = num_corridors + 1;
    corridors(num_corridors,:) = [pts(end,:),...
        pts(1,:),CIRCLE_ZONE,indexes(end),indexes(1),v,v];
    cor_edges = [cor_edges; indexes(end),indexes(1)];
end

% create launch/land corridors
launch_vertexes = airways.launch_vertexes;
num_launch_vertexes = length(launch_vertexes);
for v = 1:num_launch_vertexes
    % Create launch corridor
    index_v = launch_vertexes(v);
    pt2_index = entries(index_v).index;
    pt2 = cor_vertexes(pt2_index,:);
    pt1 = [pt2(1:2),Z_LAUNCH];
    cor_vertexes = [cor_vertexes;pt1];
    pt1_index = length(cor_vertexes(:,1));
    num_corridors = num_corridors + 1;
    corridors(num_corridors,:) = [pt1,pt2,LAUNCH_ZONE,pt1_index,...
        pt2_index,index_v,index_v];
end

land_vertexes = airways.land_vertexes;
num_land_vertexes = length(land_vertexes);
for v = 1:num_land_vertexes
    % Create land corridor
    index_v = land_vertexes(v);
    pt2_index = exits(index_v).index;
    pt2 = cor_vertexes(pt2_index,:);
    pt1 = [pt2(1:2),Z_LAND];
    cor_vertexes = [cor_vertexes;pt1];
    pt1_index = length(cor_vertexes(:,1));
    num_corridors = num_corridors + 1;
    corridors(num_corridors,:) = [pt2,pt1,LAND_ZONE,pt2_index,...
        pt1_index,index_v,index_v];
end

airways_out.corridors = corridors;
airways_out.cor_vertexes = cor_vertexes;
airways_out.cor_edges = corridors(:,8:9);
tch = 0;
