Fixed some errors found in testing and debuging. Should work well now, although some more testing would be nice.