Optimized slightly, refactored. Still not converging