works now, fixed varios bugs, replaced nanosleep as it is too slow for delays of <= 1ms