More functions

Most functions are still yet to work. Also cleaned the code, moved all
the functions above the main loop.