Optimization

Things to do until you give up and move it to C++