Cleaned up Test and moved code to other classes. Seems to work, but will need more testing (found errors around edges)