First implementation stage close. Next stage testing + refactoring

Signed-off-by: Patrick Meppe <tapmeppe@gmail.com>