Various bugs fixed. Test routine added.