Hopefully fix formula for writing data, may not have though, still untested.
Add initialize and a few other functions, and add some basic test code to main.c