First try at implementing all functionality

*fingers crossed*
Added a test program too

Still don't know if it works though