Removed throwing exception - drastically increased speed. Also changed openlist to a queue and closed list to a hashset instead of lists