Added a proper test program and cleaned up some of the code. Took out debug text.