Started writing crucial methods for my a* alghorithm