Add unittest
Move some funcs to .cpp file to avoid compilation errors using forward declares