Removed unnecessary code, if life is funny I will restore them soon.
Tinkered a bit about writing logic, then I found a similar project (see comments in board.h)