This version of the algorithm works for moving along a single trajectory
Next will be the algorithm working with multiple trajectories being sent over time