Fix priority queue, better drawing
Added obstacles
Fix guess calculation