Added dynamic encoder threshold values.
Cleaned up code a bit.
Added moved more of the unnecessary code to the debug mode.