Added uint16_t and did some refactoring