Working on a* algorithm