function action = LEM_UAS_agent0(percept)
% LEM_UAS_agent: agent that represents a UAS
% On Input:
%   percept (percept struct)
%       .pos (nX3): current location of the agent
%       .route (route struct): Change in the current route
%       .time (1 x 2): Current Time and change in time
% On Output:
%   action (1 x 3): ground speed vector and altitude
% Author
%   Vista Marston
%   UU
%   Summer 2021

persistent pos groundSpeed index route
action = [];
if isempty(route) % First time in the function
    index = 1;
    route = percept.route;
end

pos = percept.pos(:, 1:3); % Current Position Update
close = 2;

[rows, ~] = size(route);
% Reached the exit lane
if norm(route(index, 4:6) - pos) < close && index < rows
    index = index + 1;
end

% Time to Land?
if index == rows && norm(route(index, 4:6) - pos) < close 
    groundSpeed = [0,0];
    delt_alt = 0;
    route = [];
else % Nope: Still flying
    del_t = (route(index, 8) - percept.time(1)); 
    alt = (route(index, 6) - pos(:, 3));
    next_pos = route(index, 4:6);
    d = next_pos(:, 1:2) - pos(:,1:2);
    while (del_t < percept.time(2)) && (alt < close)
        action = [action; route(index, 4:6)]; % Add the current location
        del_t = del_t + (route(index + 1, 8) - route(index, 8));
        alt = 0;
        d = route(index + 1, 4:5) - next_pos(:, 1:2);
        if (del_t < percept.time(2))
            index = index + 1;
        end
    end
    delt_alt = alt/del_t;
    groundSpeed = d/del_t;
end

action = [action; groundSpeed, delt_alt];
