Add init func

Add init function and move everything that needs initialization outside
main in it.