Functional A star implementation. Test later