Removed the busy polling, added a blocking Timer instead
Cleaned up the structs, reorganized the code