corrected code + debug mode

- corrected a total failure of copy/paste (sorry)
- added (restored) a light debug mode to display values via Serial for
testing purpose