Simple A*-Algorithm implmenetation