Simple A* implementation from course TDT4136