bug in test function, turned into something that can be dropped into arduino libraries, added comments for whats going on