Clean up
C++ 11, added tests