Simplified path logic

plus bug fixes. Gotta redo path smoothing.