Found a Matrix class. Oh my god so much better. Left old code in just in case.