Added complete functionality

Add A* path finding