Add #ifdefs so that unused code is not compiled. Reduces binary size by about 700 bytes.
Add -Os to linker options (no difference detected).
Change some minor things as suggested by cppcheck. Mostly reducing variable scope.