Changed how the gcost adds up in the a* search algorithm