function [flights,M] = UTM_zipper(max_steps,H_X,d_S)
% UTM_zipper - real-time deconfliction protocol simulation
% On input:
%     max_steps (int): number of steps in simulation
%     H_X (float): minimum lane headway distance
%     d_S (float): minimum Euclidean headway distance
% On output:
%     flights (flight struct): flight info
%       .status (int): 1 (WAITING); 0 (IN_FLIGHT); -1 (DONE)
%       .lane_index (int): index into current lane sequence
%       .lane_dists (1xn vector): distance travelled along each lane
%       .lane_speeds (1xn vector): final speed in each lane
%       .start_time (float): actua launch time
%       .original_start_time (float): desired launch time
%       .launch_node (int): index of launch lane (node?)
%       .land_node (int): index of land lane (node?)
%       .traj (1xn vector): lane indexes of trajectory
%       .lane_lengths(1xn vector): lengths of lanes in trajectory
%       .TOA (1xn vector): Time of Arrival at exit point of lane
%       .TOD (1xn vector): Time of Departure from entry point of each lane
%       .last_update (float): time of last state update for flight
%       .total_dist (float): total distance tavelled to current time
%       .num_restarts (int): number of time launch postponed
%     M (Matlab movie): movie of flights through lanes
% Call:
%     [res,M] = UTM_zipper(500,1.4142,1);
% Author:
%     T. Henderson
%     UU
%     Summer 2020
%

UTM_load_vars;

flights = [];

% Set up lanes:
%   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 connected to each lane (?)
%   launch_nodes (1xp vector): launch node/lane indexes
%   land_nodes (1xq vector): land node/lane indexes
[nodes,edges,paths,trajectories,lanes,lane_degrees,angles,co_lanes,...
    launch_nodes,land_nodes] = UTM_zipper_lanes;
num_nodes = length(nodes(:,1));
num_edges = length(edges(:,1));
num_lanes = length(lanes(:,1));
num_launch_nodes = length(launch_nodes);
num_land_nodes = length(land_nodes);
lane_lengths = zeros(num_lanes,1);
for k = 1:num_lanes
    lane_lengths(k) = norm(lanes(k,1:3)-lanes(k,4:6));
end

num_flights = 0;
m_count = 0;
cur_time = 0;

for s = 1:max_steps
    s
    if rand<NEW_FLIGHT_PROB    % generate a flight
        num_flights = num_flights + 1;
        f = num_flights;
        start_time = cur_time + MIN_DELAY + rand*(MAX_DELAY-MIN_DELAY);
        node1 = launch_nodes(randi(num_launch_nodes));
%        node1 = 1;
        node2 = land_nodes(randi(num_land_nodes));
%        node2 = 2;
        traj = trajectories(node1,node2).traj;
        num_lanes = length(traj);
        speed = MIN_SPEED + rand*(MAX_SPEED-MIN_SPEED);
        speed = MAX_SPEED;
        flights(f).status = WAITING;
        flights(f).lane_index = 1;
        flights(f).lane_dists = zeros(1,num_lanes);
        lane_speeds = speed*ones(1,num_lanes);
        flights(f).lane_speeds = lane_speeds;
        flights(f).start_time = start_time;
        flights(f).original_start_time = start_time;
        flights(f).launch_node = node1;
        flights(f).land_node = node2;
        flights(f).traj = traj;
        flights(f).lane_lengths = lane_lengths(traj);
        [TOD,TOA] = UTM_set_TOD_TOA(start_time,lane_speeds,lane_lengths);
        flights(f).TOD = TOD;
        flights(f).TOA = TOA;
        flights(f).last_update = cur_time;
        flights(f).total_dist = 0;
        flights(f).num_restarts = 0;
    end
    prev_time = cur_time;
    [cur_time,f] = UTM_next_event(flights);
    lane_c = Inf;
    if length(flights)>lane_c
        lane_index = flights(lane_c).lane_index;
        TODc = flights(lane_c).TOD(lane_index);
        TOAc = flights(lane_c).TOA(lane_index);
        lanec = flights(lane_c).traj(lane_index);
        distc = flights(lane_c).lane_dists(lane_index);
        [cur_time,lanec,TODc,TOAc,distc]
    end
    if f>0 % handle next event
        
        %advance flights
        flights = UTM_advance_flights(flights,lanes,cur_time);
        min_s = 323;
        if s>min_s
            ff1 = 18;
            ff2 = 41;
            f9_lane_index = flights(ff1).lane_index;
            f9_lane = flights(ff1).traj(f9_lane_index);
            f9_speed = flights(ff1).lane_speeds(f9_lane_index);
            f9_dist = flights(ff1).lane_dists(f9_lane_index);
            P1 = lanes(f9_lane,1:3);
            P2 = lanes(f9_lane,4:6);
            dirP = P2 - P1;
            dirP = dirP/norm(dirP);
            O1 = P1 + f9_dist*dirP;
            V1 = f9_speed*dirP;
            ts1 = cur_time;
            te1 = flights(ff1).TOA(f9_lane_index);
            f49_lane_index = flights(ff2).lane_index;
            f49_lane = flights(ff2).traj(f49_lane_index);
            f49_speed = flights(ff2).lane_speeds(f49_lane_index);
            f49_dist = flights(ff2).lane_dists(f49_lane_index);
            Q1 = lanes(f49_lane,1:3);
            Q2 = lanes(f49_lane,4:6);
            dirQ = Q2 - Q1;
            dirQ = dirQ/norm(dirQ);
            O2 = Q1 + f49_dist*dirQ;
            V2 = f49_speed*dirQ;
            ts2 = cur_time;
            te2 = flights(ff2).TOA(f49_lane_index);
            [min_d,t] = UTM_trajMinDist(O1,V1,ts1,te1,O2,V2,ts2,te2);
            if min_d<1
                tch = 0;
            end
        end
        % handle selected event
        lane_index = flights(f).lane_index;
        lane_number = flights(f).traj(lane_index);
        if flights(f).status==WAITING
            [min_dist,min_f] = UTM_last_in_lane(flights,lane_number);
            flights(f).last_update = cur_time;
            if min_dist<H_X
                % reschedule: too close
                start_time = cur_time + MIN_DELAY...
                    + rand*(MAX_DELAY-MIN_DELAY);
                flights(f).start_time = start_time;
                num_lanes = length(flights(f).traj);
                [TOD,TOA] = UTM_set_TOD_TOA(start_time,...
                    flights(f).lane_speeds,flights(f).lane_lengths);
                flights(f).TOD = TOD;
                flights(f).TOA = TOA;
                flights(f).num_restarts = flights(f).num_restarts + 1;
            else
                flights(f).status = IN_FLIGHT;
                flights = UTM_zipper_TD(flights,lanes,lane_degrees,...
                    co_lanes,edges,f,H_X,cur_time,...
                    flights(f).lane_dists(lane_index));
            end
        else % redo ZSD at new lane
            old_speeds = flights(f).lane_speeds;
            flights = UTM_zipper_TD(flights,lanes,lane_degrees,co_lanes,...
                edges,f,H_X,cur_time,flights(f).lane_dists(lane_index));
            new_speeds = flights(f).lane_speeds;
            if abs(new_speeds(lane_index)-old_speeds(lane_index))...
                    >ZERO_THRESH
                ZTD_flights = [];
                ZTD_lanes = [];
                CLOSED = [f];
                c_flights = UTM_conflicting_flights(flights,lanes,...
                    lane_degrees,co_lanes,f);
                if isempty(c_flights)
                    num_c_flights = 0;
                else
                    num_c_flights = length(c_flights(:,1));
                end
                for cc = 1:num_c_flights
                    if isempty(ZTD_flights)
                        ZTD_flights = c_flights(:,1);
                        ZTD_lanes = c_flights(:,2);
                    elseif isempty(find(c_flights(cc,1)==ZTD_flights))
                        ZTD_flights = [ZTD_flights; c_flights(cc,1)];
                        ZTD_lanes = [ZTD_lanes; c_flights(cc,2)];
                    end
                end
                ZTD_list = [ZTD_flights,ZTD_lanes];
                while ~isempty(ZTD_list)
                    fp = ZTD_list(1,1);
                    CLOSED = unique([CLOSED,fp]);
                    lane_number = ZTD_list(1,2);
                    lane_index = find(flights(fp).traj==lane_number);
                    ZTD_list = ZTD_list(2:end,:);
                    fp_old_speeds = flights(fp).lane_speeds;
                    flights = UTM_zipper_TD(flights,lanes,lane_degrees,...
                        co_lanes,edges,fp,H_X,cur_time,...
                        flights(fp).lane_dists(lane_index));
                    fp_new_speeds = flights(fp).lane_speeds;
                    if ~isequal(fp_old_speeds,fp_new_speeds)
                        c_flights = UTM_conflicting_flights(flights,...
                            lanes,lane_degrees,co_lanes,fp);
                        c_flights = UTM_remove_dups(c_flights,CLOSED);
                        if isempty(c_flights)
                            num_c_flights = 0;
                        else
                            num_c_flights = length(c_flights(:,1));
                        end
                        if isempty(ZTD_list)
                            ZTD_list = c_flights;
                        else
                            for cc = 1:num_c_flights
                                if isempty(find(c_flights(cc)==ZTD_list(:,1)))
                                    ZTD_list = [ZTD_list; c_flights(cc,:)];
                                end
                            end
                        end
                    end
                end
            end
            tch = 0;
        end
        for f = 1:num_flights
            c_flights = UTM_conflicting_flights(flights,lanes,...
                lane_degrees,co_lanes,f);
            ff1 = f;
            if ~isempty(c_flights)
                num_c_flights = length(c_flights(:,1));
                for cc = 1:num_c_flights
                    fp = c_flights(cc,1);
                    ff2 = fp;
                    lane_number = c_flights(cc,2);
                    lane_index = find(flights(fp).traj==lane_number);
                    f9_lane_index = flights(ff1).lane_index;
                    f9_lane = flights(ff1).traj(f9_lane_index);
                    f9_speed = flights(ff1).lane_speeds(f9_lane_index);
                    f9_dist = flights(ff1).lane_dists(f9_lane_index);
                    P1 = lanes(f9_lane,1:3);
                    P2 = lanes(f9_lane,4:6);
                    dirP = P2 - P1;
                    dirP = dirP/norm(dirP);
                    O1 = P1 + f9_dist*dirP;
                    V1 = f9_speed*dirP;
                    ts1 = cur_time;
                    te1 = flights(ff1).TOA(f9_lane_index);
                    f49_lane_index = flights(ff2).lane_index;
                    f49_lane = flights(ff2).traj(f49_lane_index);
                    f49_speed = flights(ff2).lane_speeds(f49_lane_index);
                    f49_dist = flights(ff2).lane_dists(f49_lane_index);
                    Q1 = lanes(f49_lane,1:3);
                    Q2 = lanes(f49_lane,4:6);
                    dirQ = Q2 - Q1;
                    dirQ = dirQ/norm(dirQ);
                    O2 = Q1 + f49_dist*dirQ;
                    V2 = f49_speed*dirQ;
                    ts2 = cur_time;
                    te2 = flights(ff2).TOA(f49_lane_index);
                    [min_d,t] = UTM_trajMinDist(O1,V1,ts1,te1,O2,V2,ts2,te2);
                    if min_d<d_S
                        flights = UTM_zipper_TD(flights,lanes,lane_degrees,...
                            co_lanes,edges,fp,H_X,cur_time,...
                            flights(fp).lane_dists(lane_index));
                    end
                end
            end
        end
        UTM_draw_zipper(flights,lanes);
        text(0,0,-1,num2str(s));
        drawnow
        m_count = m_count + 1;
        M(m_count) = getframe(gca);
        dists = UTM_zipper_dists(flights,lanes);
        min_val = UTM_min_dist(dists);
        cc = UTM_conflicts(flights,lanes);
        if min_val+ZERO_THRESH<d_S
            display(['Flights too close: Step: ',num2str(s)]);
        end
    end
end
res = flights;

tch = 0;
