Added mode switch and cleaned up the code