Major Refactoring for XBee S2