Code review
Code cleanup
Implemented queue control

Implemented queue control - but still too many time spend to Rx/Tx