Adding path finding

Implemented path finding algorithm A*