The optimization routines with nonlinear constrains are definitely not trivial. I have the source code for feasible sequential quadratic programming of Prof. A. Tits. And it contains more than 10.000 lines of code. So we need a specialist to code it. Best Przem