Code organized and minor performance optimizations. Still needs to test with huge matrices and benchmark the runtime.