Edited after testing on Arduino