clear all; clc;

dt = 0.1;
waypoints = [1 1;2 2;3 4;6 3;3 6;9 9]; % waypoints to move through
move_speed = 1; % movement speed of uav
waypoint_len = size(waypoints,1);
D = dist(waypoints');
dist_list = diag(D(1:end-1,2:end));
total_dist = sum(dist_list);
total_time = total_dist/move_speed;
t = dt:dt:total_time; % time parameter

% now we need to determine, based on time, what set of waypoints we are
% passing between

figure(1); clf; hold on;

for k=1:length(t)

    q_time = t(k); % time we are interested in evaluating
    q_dist = q_time*move_speed;
    dist_list_sum = [0;cumsum(dist_list)];
    for i=1:length(dist_list_sum)
        if q_dist <= dist_list_sum(i)
            wp_index = i;
            break;
        end
    end

    unit_vec = waypoints(wp_index,:) - waypoints(wp_index - 1,:);
    unit_vec = unit_vec/norm(unit_vec);
    q_point = waypoints(wp_index - 1,:) + unit_vec*(q_dist - dist_list_sum(wp_index - 1));
    plot(q_point(1), q_point(2), 'k.');

end