Updated A*, finds correct path