Improve CQueue template

Add Unit test