Made some not too good looking design and cleaned the code

Sign-off Martin Agnälv (martinagnalv@gmail.com)