Added matrix math
Updated debugging log to use Log4J instead of stdout