Few optimizations

Still 100% correctness but has errors with performance