Optimized a bit, now about 2x faster