Matrix math debug, works now
Removed debug code