Added Constraint Optimizer

This needs more unit tests, but what the constraint optimizer itself works.