Remove duplicate code and simplify draw entities function