Refactoring code, now essentially working

Change-Id: I8da555f7633aea1de5293c2435295fdf147fb061