Remove duplicated code and make PES packet a vector