Refactor program and added a driver program. Has yet to test.