Program now works, added JUnit to test it