First train algorithm, WIP