Add Find Path Problem; fix small bugs of A*