Refactor computations

changes lookahead and vehicle stop calculations for better speed and stability