Clean up and organize code in main sketch, add comments