[FIX] Fixed AVR-GCC internal compiler error + code improvements