ADD: Continued development of algorithm, works for a single train