Fixed some bugs after testing A*