FIX pathfinding

+ Pathfinding now works (though movements are not very smooth).
+ Path highlighting.