Refactoring, rgkit integration, and more features!

Refactoring from MB6