Replace loop/sleep with pthread_cond_timedwait

This should result in better performance
Move around some locks to avoid helgrind warnings