Revert "Revert "last fix was incorrect""

This reverts commit dec56107c9cf0f794952171a121b0c8972bf0e85.
Revert "Ugliest code in the history of mankind, also tries to plan a path with a margin"

This reverts commit 1f2b1727df085c98566947f20f98d0a9f7b92c76.