USI version is working
cleaned up USI code and is now under 500 bytes