Merging code from c++ test project in arduino code base