Broke functionality out to C++ classes then rewrote the main sketch. Untested