Improved state machine.. almost done.. test!