a* fixed - now takes the right path, heuristics were not factored in correctly + some code refactoring