Add functions to writing files and a new testing program

Several mistakes are discovered and fixed using the testing program.