Path finding with a star implemented