Self test updates

Added proper code to actually do some self testing. This definitely
needs to be improved, but it's a start.