Add sleep functions to thread

Remove unused untility functions
Refactor template args
Add small test for thread, mutex and cond_var