Coding finished, pre-debug version
First round of debugging, seems to be working ok