Fix possible NPE, code rewritten