function flights = UTM_advance_flights(flights_in,lanes,cur_time)
% UTM_advance_flights - update state of UAVs
% On input:
%     flights_in (flight struct): flight info
%     lanes (nx6 array): lane entry/exit points by row
%     cur_time (float): current time
% On output:
%     flights (flight struct): updated flight info
% Call:
%     flights = UTM_advance_flights(flights,lanes,cur_time);
% Author:
%     T. Henderson
%     UU
%     Summer 2020
%

WAITING = 1;
IN_FLIGHT = 0;
DONE = -1;
ZERO_THRESH = 10^(-5);

num_flights = length(flights_in);
flights = flights_in;

% advance flights
for f = 1:num_flights
    if flights(f).status==IN_FLIGHT  % in flight
        traj = flights(f).traj;
        lane_index = flights(f).lane_index;
        lane_dists = flights(f).lane_dists;
        lane_speeds = flights(f).lane_speeds;
        num_lanes = length(traj);
        lane_lengths = flights(f).lane_lengths;
        travel_time = cur_time - flights(f).last_update;
        done = travel_time<=ZERO_THRESH;
        while done==0
            time_for_lane = (lane_lengths(lane_index)...
                -lane_dists(lane_index))/lane_speeds(lane_index);
            if time_for_lane-travel_time<ZERO_THRESH...
                    &lane_index==num_lanes
                flights(f).status = DONE;
                flights(f).lane_dists = lane_lengths;
                flights(f).total_dist = sum(lane_lengths);
                done = 1;
            elseif time_for_lane-travel_time<ZERO_THRESH
                % go to next lane
                flights(f).lane_dists(lane_index)...
                    = lane_lengths(lane_index);
                travel_time = travel_time - time_for_lane;
                lane_index = lane_index + 1;
            else
                % didn't get out of lane
                flights(f).lane_dists(lane_index) = lane_dists(lane_index)...
                    + lane_speeds(lane_index)*travel_time;
                done = 1;
            end
            flights(f).total_dist = sum(flights(f).lane_dists);
        end
        flights(f).lane_index = lane_index;
    end
    flights(f).last_update = cur_time;
end
