Move code to headers, get CMake working