Don't really like the logic as is, but synthesis should clean it up. Should work now