function flights_out = LEM_advance_flights(flights,t,del_t,sigma)
% LEMS_advance_flights - perform physics of flight
% On input:
%     flights (flight struct): flight info
%     t (float): current time
%     del_t (float): time step
%     sigma (float): uncertainty in motion
% On output:
%     flights_out (flight struct): updated flight info
% Call:
%     f2 = LEM_advance_flights(f1,13.2,0.1,0.3);
% Author:
%     T. Henderson
%     UU
%     Fall 2020
%

ZERO_THRESH = 0.1;
FAILED = 0;

flights_out = flights;

num_flights = length(flights);

for f = 1:num_flights
    if flights(f).type~=FAILED
        plan = flights(f).plan;
        index = find(plan(:,1)<=t&t<plan(:,2));
        if ~isempty(index)
            route = flights(f).route;
            pt1 = flights(f).telemetry(end,1:3);
            pt2 = route(index,4:6);
            dir = pt2 - pt1;
            if norm(dir)<ZERO_THRESH
                if index==length(plan(:,1))
                    dir = route(end,4:6) - route(end,1:3);
                else
                    dir = route(index+1,4:6) - route(index+1,1:3);
                end
            end
            dir = dir/norm(dir);
            pt = pt1 + dir*route(index,9)*del_t;
            
            % add noise
            pt = pt + mvnrnd([0;0;0],sigma,3)';
            flights_out(f).telemetry = [flights_out(f).telemetry;...
                pt,dir,route(index,9)];
            tch = 0;
        end
    end
end
