Java implementation of A Star Path finding algorithm