Cleanup

Refactored the code. Removed a lot of declarations in the global namespace, changed to local declarations which destroy on each loop. Also simplified a few lines. Did not have a chance to verify on actual arduino.