Made it properly general with output files.
Fixed some bad matrix operations.