Implemented A Star algorithm and A Star node class