astar seems to be working, next step to make own implementations of hashmap, arraylist etc.