route planning done
deleted own Bresenham because it is not needed anymore