Ugliest code in the history of mankind, also tries to plan a path with a margin