Tidy PIC32 Code up