To be moved to arduino in c++