AStar

Implemented AStar correctly.
Changed distance methods from Euclidian to Diagonal.
Minor bug fixes.