function [nodes,edges,paths,trajectories,lanes,lane_degrees,angles,...
    co_lanes,launch_nodes,land_nodes] = UTM_zipper_lanes
% UTM_zipper_lanes - create testing lane structure for zipper protocol
% On input:
%     N/A
% On output:
%     nodes (kx3 array): locations of lane endpoints
%     edges (mx2 array): lanes defeined as directed edges between nodes
%     paths(kxk struct array).traj: sequence of node indexes from launch
%       nodes to land nodes
%     trajectories(kxk struct array): lane index sequence between nodes
%     lanes (nx6 array): lane entry point, exit point per row
%     co_lanes (struct vector).lanes: lane indexes of connected lanes (?)
%     launch_nodes (1xp vector): launch node/lane indexes
%     land_nodes (1xq vector): land node/lane indexes
% Call:
%     [no,ed,pa,traj,lanes,co_lanes,launch_no,land_no] = UTM_zipper_lanes;
% Author:
%     T. Henderson
%     UU
%     Summer 2020
%

ZERO_THRESH = 0.0001;

launch_nodes = [1,3,6,8,9,11,14,16,17,19];
land_nodes = [2,4,5,7,10,12,13,15,18,20];

% Set up lanes
nodes = [];
x_vals = [0:10:30];
y_vals = [0:10:40];
for yi = 1:5
    y = y_vals(yi);
    for xi = 1:4
        x = x_vals(xi);
        nodes = [nodes;x,y,0];
    end
end
nodes2 = nodes;
nodes2(:,3) = 10;
nodes = [nodes;nodes2];
num_nodes = length(nodes(:,1));

edges = [...
    1,21; 22,2; 3,23; 24,4; 25,5; 6,26; 27,7; 8,28; 9,29; 30,10; 11,31;...
    32,12; 33,13; 14,34; 35,15; 16,36; 17,37; 38,18; 19,39; 40,20;...
    21,22; 22,23; 23,24; 25,21; 22,26; 27,23; 24,28; 25,26; 26,27;...
    27,28; 29,25; 30,26; 27,31; 28,32;...
    30,29; 30,31; 32,31; 33,29; 34,30; 31,35; 32,36; 33,34; 35,34;...
    35,36; 37,33; 38,34; 35,39; 36,40; 38,37; 39,38; 40,39
    ];
num_edges = length(edges(:,1));

for n1 = 1:40
    for n2 = 1:40
        paths(n1,n2).traj = [];
    end
end

paths(1,2).traj = [1,21,22,2];
paths(1,4).traj = [1,21,22,23,24,4];
paths(1,5).traj = [1,21,22,26,27,31,35,34,30,29,25,5];
paths(1,7).traj = [1,21,22,26,27,7];
paths(1,10).traj = [1,21,22,26,27,31,35,34,30,10];
paths(1,12).traj = [1,21,22,23,24,28,32,12];
paths(1,13).traj = [1,21,22,26,27,31,35,39,38,37,33,13];
paths(1,15).traj = [1,21,22,26,27,31,35,15];
paths(1,18).traj = [1,21,22,26,27,31,35,39,38,18];
paths(1,20).traj = [1,21,22,23,24,28,32,36,40,20];

paths(3,2).traj = [3,23,24,28,32,31,35,34,30,29,25,21,22,2];
paths(3,4).traj = [3,23,24,4];
paths(3,5).traj = [3,23,24,28,32,31,35,34,30,29,25,5];
paths(3,7).traj = [3,23,24,28,32,31,35,34,30,26,27,7];
paths(3,10).traj = [3,23,24,28,32,31,35,34,30,10];
paths(3,12).traj = [3,23,24,28,32,12];
paths(3,13).traj = [3,23,24,28,32,36,40,39,38,37,33,13];
paths(3,15).traj = [3,23,24,28,32,31,35,15];
paths(3,18).traj = [3,23,24,28,32,36,40,39,38,18];
paths(3,20).traj = [3,23,24,28,32,36,40,20];

paths(6,2).traj = [6,26,27,31,35,34,30,29,25,21,22,2];
paths(6,4).traj = [6,26,27,23,24,4];
paths(6,5).traj = [6,26,27,31,35,34,30,29,25,5];
paths(6,7).traj = [6,26,27,7];
paths(6,10).traj = [6,26,27,31,35,34,30,10];
paths(6,12).traj = [6,26,27,28,32,12];
paths(6,13).traj = [6,26,27,31,35,39,38,37,33,13];
paths(6,15).traj = [6,26,27,31,35,15];
paths(6,18).traj = [6,26,27,31,35,39,38,18];
paths(6,20).traj = [6,26,27,28,32,36,40,20];

paths(8,2).traj = [8,28,32,31,35,34,30,29,25,21,22,2];
paths(8,4).traj = [8,28,32,31,35,34,30,26,27,23,24,4];
paths(8,5).traj = [8,28,32,31,35,34,30,29,25,5];
paths(8,7).traj = [8,28,32,31,35,34,30,26,27,7];
paths(8,10).traj = [8,28,32,31,35,34,30,10];
paths(8,12).traj = [8,28,32,12];
paths(8,13).traj = [8,28,32,36,40,39,38,37,33,13];
paths(8,15).traj = [8,28,32,31,35,15];
paths(8,18).traj = [8,28,32,36,40,39,38,18];
paths(8,20).traj = [8,28,32,36,40,20];

paths(9,2).traj = [9,29,25,21,22,2];
paths(9,4).traj = [9,29,25,21,22,23,24,4];
paths(9,5).traj = [9,29,25,5];
paths(9,7).traj = [9,29,25,26,27,7];
paths(9,10).traj = [9,29,25,26,27,31,35,34,30,10];
paths(9,12).traj = [9,29,25,26,27,28,32,12];
paths(9,13).traj = [9,29,25,26,27,31,35,39,38,37,33,13];
paths(9,15).traj = [9,29,25,26,27,31,35,15];
paths(9,18).traj = [9,29,25,26,27,31,35,39,38,18];
paths(9,20).traj = [9,29,25,26,27,28,32,36,40,20];

paths(11,2).traj = [11,31,35,34,30,29,25,21,22,2];
paths(11,4).traj = [11,31,35,34,30,26,22,23,24,4];
paths(11,5).traj = [11,31,35,34,30,29,25,5];
paths(11,7).traj = [11,31,35,34,30,26,27,7];
paths(11,10).traj = [11,31,35,34,30,10];
paths(11,12).traj = [11,31,35,34,30,26,27,28,32,12];
paths(11,13).traj = [11,31,35,39,38,37,33,13];
paths(11,15).traj = [11,31,35,15];
paths(11,18).traj = [11,31,35,39,38,18];
paths(11,20).traj = [11,31,35,36,40,20];

paths(14,2).traj = [14,34,30,29,25,21,22,2];
paths(14,4).traj = [14,34,30,26,27,23,24,4];
paths(14,5).traj = [14,34,30,29,25,5];
paths(14,7).traj = [14,34,30,26,27,7];
paths(14,10).traj = [14,34,30,10];
paths(14,12).traj = [14,34,30,26,27,28,32,12];
paths(14,13).traj = [14,34,30,31,35,39,38,37,33,13];
paths(14,15).traj = [14,34,30,31,35,15];
paths(14,18).traj = [14,34,30,31,35,39,38,18];
paths(14,20).traj = [14,34,30,26,27,28,32,36,40,20];

paths(16,2).traj = [16,36,40,39,38,37,33,29,25,21,22,2];
paths(16,4).traj = [16,36,40,39,38,34,30,26,27,23,24,4];
paths(16,5).traj = [16,36,40,39,38,37,33,29,25,5];
paths(16,7).traj = [16,36,40,39,38,34,30,26,27,7];
paths(16,10).traj = [16,36,40,39,34,30,10];
paths(16,12).traj = [16,36,40,39,38,34,30,26,27,28,32,12];
paths(16,13).traj = [16,36,40,39,38,37,33,13];
paths(16,15).traj = [16,36,40,39,38,34,30,31,35,15];
paths(16,18).traj = [16,36,40,39,38,18];
paths(16,20).traj = [16,36,40,20];

paths(17,2).traj = [17,37,33,29,25,21,22,2];
paths(17,4).traj = [17,37,33,29,25,21,22,23,24,4];
paths(17,5).traj = [17,37,33,29,25,5];
paths(17,7).traj = [17,37,33,29,25,26,27,7];
paths(17,10).traj = [17,37,33,34,30,10];
paths(17,12).traj = [17,37,33,34,30,26,27,28,32,12];
paths(17,13).traj = [17,37,33,13];
paths(17,15).traj = [17,37,33,34,30,31,35,15];
paths(17,18).traj = [17,37,33,34,30,31,35,39,38,18];
paths(17,20).traj = [17,37,33,34,30,31,35,36,40,20];

paths(19,2).traj = [19,39,38,37,33,29,25,21,22,2];
paths(19,4).traj = [19,39,38,37,33,29,25,21,22,23,24,4];
paths(19,5).traj = [19,39,38,37,33,29,25,5];
paths(19,7).traj = [19,39,38,34,30,26,27,7];
paths(19,10).traj = [19,39,38,34,30,10];
paths(19,12).traj = [19,39,38,34,30,26,27,28,32,12];
paths(19,13).traj = [19,39,38,37,33,13];
paths(19,15).traj = [19,39,38,34,30,31,35,15];
paths(19,18).traj = [19,39,38,18];
paths(19,20).traj = [19,39,38,34,30,31,35,36,40,20];

for e1 = 1:20
    for e2 = 1:20
        trajectories(e1,e2).traj = [];
        if ~isempty(paths(e1,e2).traj)
            waypts = paths(e1,e2).traj;
            num_pts = length(waypts);
            for p = 1:num_pts-1
                index = find(edges(:,1)==waypts(p)&edges(:,2)==waypts(p+1));
                trajectories(e1,e2).traj = [trajectories(e1,e2).traj,index];
            end
        end
    end
end

lanes = zeros(num_edges,6);
for e = 1:num_edges
    index1 = edges(e,1);
    index2 = edges(e,2);
    lanes(e,:) = [nodes(index1,:),nodes(index2,:)];
end
num_lanes = num_edges;

for e = 1:num_lanes
    entry_pt = lanes(e,1:3);
    exit_pt = lanes(e,4:6);
    entry_indexes_in = find(lanes(:,4)==entry_pt(1)&lanes(:,5)==entry_pt(2)...
        &lanes(:,6)==entry_pt(3));
    entry_indexes_out = find(lanes(:,1)==entry_pt(1)&lanes(:,2)==entry_pt(2)...
        &lanes(:,3)==entry_pt(3));
    exit_indexes_in = find(lanes(:,4)==exit_pt(1)&lanes(:,5)==exit_pt(2)...
        &lanes(:,6)==exit_pt(3));
    exit_indexes_out = find(lanes(:,1)==exit_pt(1)&lanes(:,2)==exit_pt(2)...
        &lanes(:,3)==exit_pt(3));
    co_lanes(e).entry_in = setdiff(entry_indexes_in',e);
    co_lanes(e).exit_in = setdiff(exit_indexes_in',e);
    co_lanes(e).entry_out = setdiff(entry_indexes_out',e);
    co_lanes(e).exit_out = setdiff(exit_indexes_out',e);
    co_lanes(e).all = [co_lanes(e).entry_in,co_lanes(e).entry_out,...
        co_lanes(e).exit_in,co_lanes(e).exit_out];
end

angles = pi*ones(num_lanes,num_lanes);
for e1 = 1:num_lanes-1
    pt11 = lanes(e1,1:3);
    pt12 = lanes(e1,4:6);
    d1 = norm(pt12-pt11);
    for e2 = e1+1:num_lanes
        pt21 = lanes(e2,1:3);
        pt22 = lanes(e2,4:6);
        d2 = norm(pt22-pt21);
        connected = 1;
        if norm(pt11-pt21)<ZERO_THRESH
            d3 = norm(pt12-pt22);
            theta = posori(acos(-(d3^2-d1^2-d2^2)/(2*d1*d2)));
        elseif norm(pt11-pt22)<ZERO_THRESH
            d3 = norm(pt21-pt12);
            theta = posori(acos(-(d3^2-d1^2-d2^2)/(2*d1*d2)));
        elseif norm(pt12-pt21)<ZERO_THRESH
            d3 = norm(pt22-pt11);
            theta = posori(acos(-(d3^2-d1^2-d2^2)/(2*d1*d2)));
        elseif norm(pt22-pt12)<ZERO_THRESH
            d3 = norm(pt21-pt11);
            theta = posori(acos(-(d3^2-d1^2-d2^2)/(2*d1*d2)));
        else
            connected = 0;
        end
        if connected==1
            angles(e1,e2) = theta;
            angles(e2,e1) = theta;
        end
    end
end

lane_degrees = zeros(num_lanes,4);
lane_degrees(:,2) = 1;
lane_degrees(:,3) = 1;
for e1 = 1:num_lanes
    pt11 = lanes(e1,1:3);
    pt12 = lanes(e1,4:6);
    for e2 = 1:num_lanes
        if e1~=e2
            pt21 = lanes(e2,1:3);
            pt22 = lanes(e2,4:6);
            if norm(pt11-pt21)<ZERO_THRESH
                lane_degrees(e1,2) = lane_degrees(e1,2) + 1;
            elseif norm(pt11-pt22)<ZERO_THRESH
                lane_degrees(e1,1) = lane_degrees(e1,1) + 1;
            elseif norm(pt12-pt21)<ZERO_THRESH
                lane_degrees(e1,4) = lane_degrees(e1,4) + 1;
            elseif norm(pt22-pt12)<ZERO_THRESH
                lane_degrees(e1,3) = lane_degrees(e1,3) + 1;
            end
        end
    end
end

tch = 0;
