[TODO] Clean up and generate (abstract) code for geometric optimization
[TODO] Pay attention to get sign correct for negative equality
