Moved connection handling into own function and organized main loop