Trying to speed up. Added stopping criteria.
Restructured to let stuff be reused, starting to look messy.
Tried using QR decomposition instead of numpys inverter. Atually
slower! Also tried using LAPACK from python but that was also slow.
Not sure it was actually working.