Ported it to c without arduino dependencies. Switched from printf to a