Optimization improvements
Add more tests, fixes