Added debug mode to print less to screen to speed up path planning.