// File: canzarlab[P]Trajan/geno/augmentedLagrangian.cpp
// Function: AugmentedNLP::functionValueAndGradient(double const*, double&, double*)
virtual bool functionValueAndGradient(const Scalar* x,
					Scalar& functionValue,
					Scalar* gradient)
  {
    _genoNLP.functionValueAndGradient(x, functionValue, gradient);

    _genoNLP.functionValueConstraints(x, _constraintValues.data());

    computeConstraintError();
    
    Vector toAdd(_constraintError);
    // for equality constraints
    toAdd.head(_mEqualities) += _y.head(_mEqualities) / _rho;

    // for inequality constraints
    toAdd.tail(_mInequalities) += _y.tail(_mInequalities) / _rho;
    for (size_t i = 0; i < _mInequalities; ++i) {
      if (_cType[i+_mEqualities] == 2)
        if (toAdd(_mEqualities + i) > 0)
          toAdd(_mEqualities + i) = 0;

      if (_cType[i+_mEqualities] == 4)
	if (toAdd(_mEqualities + i) < 0)
	  toAdd(_mEqualities + i) = 0;
    }
    functionValue += _rho/2 * toAdd.squaredNorm();

    Vector::MapType gradientMapX(gradient, _n);
    Vector gradientX(_n);

    Vector v = _rho * toAdd;
    _genoNLP.gradientConstraintsTimesVector(x, v.data(), gradientX.data());

    gradientMapX += gradientX;
    //    std::cout << "const " << _constraintValues.transpose() << std::endl;
    //    std::cout << "toAdd " << toAdd.transpose() << std::endl;
    return true;
  }

// Assembly:
pushq %rbp
movq %rsp, %rbp
subq $0x2c0, %rsp            # imm = 0x2C0
movq %rdi, -0x8(%rbp)
movq %rsi, -0x10(%rbp)
movq %rdx, -0x18(%rbp)
movq %rcx, -0x20(%rbp)
movq -0x8(%rbp), %rax
movq %rax, -0x250(%rbp)
movq 0x8(%rax), %rdi
movq -0x10(%rbp), %rsi
movq -0x18(%rbp), %rdx
movq -0x20(%rbp), %rcx
movq (%rdi), %rax
movq 0x48(%rax), %rax
callq *%rax
movq -0x250(%rbp), %rdi
movq 0x8(%rdi), %rax
movq %rax, -0x260(%rbp)
movq -0x10(%rbp), %rax
movq %rax, -0x258(%rbp)
addq $0x30, %rdi
callq 0x13cd0
movq -0x260(%rbp), %rdi
movq -0x258(%rbp), %rsi
movq %rax, %rdx
movq (%rdi), %rax
movq 0x70(%rax), %rax
callq *%rax
movq -0x250(%rbp), %rdi
callq 0x15c20
movq -0x250(%rbp), %rsi
addq $0x90, %rsi
leaq -0x30(%rbp), %rdi
callq 0x144f0
movq -0x250(%rbp), %rax
movq %rax, %rsi
addq $0x48, %rsi
movq 0x20(%rax), %rdx
leaq -0xc0(%rbp), %rdi
callq 0x14810
jmp 0x14f52
movq -0x250(%rbp), %rdx
addq $0x40, %rdx
leaq -0x88(%rbp), %rdi
leaq -0xc0(%rbp), %rsi
callq 0x15f30
jmp 0x14f72
movq -0x250(%rbp), %rax
movq 0x20(%rax), %rdx
leaq -0x108(%rbp), %rdi
leaq -0x30(%rbp), %rsi
callq 0x14810
jmp 0x14f8f
leaq -0x108(%rbp), %rdi
leaq -0x88(%rbp), %rsi
callq 0x15fe0
jmp 0x14fa4
movq -0x250(%rbp), %rax
movq %rax, %rsi
addq $0x48, %rsi
movq 0x28(%rax), %rdx
leaq -0x198(%rbp), %rdi
callq 0x16040
jmp 0x14fc4
movq -0x250(%rbp), %rdx
addq $0x40, %rdx
leaq -0x160(%rbp), %rdi
leaq -0x198(%rbp), %rsi
callq 0x15f30
jmp 0x14fe4
movq -0x250(%rbp), %rax
movq 0x28(%rax), %rdx
leaq -0x1d0(%rbp), %rdi
leaq -0x30(%rbp), %rsi
callq 0x16040
jmp 0x15001
leaq -0x1d0(%rbp), %rdi
leaq -0x160(%rbp), %rsi
callq 0x15fe0
jmp 0x15016
movq $0x0, -0x1d8(%rbp)
movq -0x250(%rbp), %rcx
movq -0x1d8(%rbp), %rax
cmpq 0x28(%rcx), %rax
jae 0x15195
movq -0x250(%rbp), %rax
movq %rax, %rdi
addq $0x78, %rdi
movq -0x1d8(%rbp), %rsi
addq 0x20(%rax), %rsi
callq 0x145d0
cmpl $0x2, (%rax)
jne 0x150eb
movq -0x250(%rbp), %rax
movq 0x20(%rax), %rsi
movq -0x1d8(%rbp), %rax
addq %rax, %rsi
leaq -0x30(%rbp), %rdi
callq 0x145f0
movq %rax, -0x268(%rbp)
jmp 0x15087
movq -0x268(%rbp), %rax
movsd (%rax), %xmm0
xorps %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jbe 0x150e9
movq -0x250(%rbp), %rax
movq 0x20(%rax), %rsi
movq -0x1d8(%rbp), %rax
addq %rax, %rsi
leaq -0x30(%rbp), %rdi
callq 0x145f0
movq %rax, -0x270(%rbp)
jmp 0x150c2
movq -0x270(%rbp), %rax
xorps %xmm0, %xmm0
movsd %xmm0, (%rax)
jmp 0x150e9
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0xc8(%rbp)
movl %eax, -0xcc(%rbp)
jmp 0x15374
jmp 0x150eb
movq -0x250(%rbp), %rax
movq %rax, %rdi
addq $0x78, %rdi
movq -0x1d8(%rbp), %rsi
addq 0x20(%rax), %rsi
callq 0x145d0
cmpl $0x4, (%rax)
jne 0x1517c
movq -0x250(%rbp), %rax
movq 0x20(%rax), %rsi
movq -0x1d8(%rbp), %rax
addq %rax, %rsi
leaq -0x30(%rbp), %rdi
callq 0x145f0
movq %rax, -0x278(%rbp)
jmp 0x15135
movq -0x278(%rbp), %rax
xorps %xmm0, %xmm0
ucomisd (%rax), %xmm0
jbe 0x1517a
movq -0x250(%rbp), %rax
movq 0x20(%rax), %rsi
movq -0x1d8(%rbp), %rax
addq %rax, %rsi
leaq -0x30(%rbp), %rdi
callq 0x145f0
movq %rax, -0x280(%rbp)
jmp 0x1516c
movq -0x280(%rbp), %rax
xorps %xmm0, %xmm0
movsd %xmm0, (%rax)
jmp 0x1517c
jmp 0x1517e
movq -0x1d8(%rbp), %rax
addq $0x1, %rax
movq %rax, -0x1d8(%rbp)
jmp 0x15021
movq -0x250(%rbp), %rax
movsd 0x40(%rax), %xmm0
movsd 0xf61ef(%rip), %xmm1    # 0x10b398
mulsd %xmm1, %xmm0
movsd %xmm0, -0x290(%rbp)
leaq -0x30(%rbp), %rdi
callq 0x160a0
movsd %xmm0, -0x288(%rbp)
jmp 0x151c8
movq -0x250(%rbp), %rax
movsd -0x288(%rbp), %xmm2
movsd -0x290(%rbp), %xmm0
movq -0x18(%rbp), %rcx
movsd (%rcx), %xmm1
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, (%rcx)
movq -0x20(%rbp), %rcx
movq %rcx, -0x2a0(%rbp)
movq 0x10(%rax), %rax
movq %rax, -0x298(%rbp)
leaq -0x1f2(%rbp), %rdi
callq 0x160e0
jmp 0x15217
movq -0x298(%rbp), %rdx
movq -0x2a0(%rbp), %rsi
leaq -0x1f0(%rbp), %rdi
leaq -0x1f2(%rbp), %rcx
callq 0x16120
jmp 0x1523a
movq -0x250(%rbp), %rsi
addq $0x10, %rsi
leaq -0x208(%rbp), %rdi
callq 0x16180
jmp 0x15253
movq -0x250(%rbp), %rsi
addq $0x40, %rsi
leaq -0x248(%rbp), %rdi
leaq -0x30(%rbp), %rdx
callq 0x161e0
jmp 0x15270
leaq -0x218(%rbp), %rdi
leaq -0x248(%rbp), %rsi
callq 0x16280
jmp 0x15285
movq -0x250(%rbp), %rax
movq 0x8(%rax), %rax
movq %rax, -0x2b8(%rbp)
movq -0x10(%rbp), %rax
movq %rax, -0x2b0(%rbp)
leaq -0x218(%rbp), %rdi
callq 0x13cd0
movq %rax, -0x2a8(%rbp)
jmp 0x152b7
leaq -0x208(%rbp), %rdi
callq 0x13cd0
movq %rax, -0x2c0(%rbp)
jmp 0x152cc
movq -0x2c0(%rbp), %rcx
movq -0x2a8(%rbp), %rdx
movq -0x2b0(%rbp), %rsi
movq -0x2b8(%rbp), %rdi
movq (%rdi), %rax
movq 0x78(%rax), %rax
callq *%rax
jmp 0x152f3
leaq -0x1f0(%rbp), %rdi
leaq -0x208(%rbp), %rsi
callq 0x162c0
jmp 0x15308
leaq -0x218(%rbp), %rdi
callq 0xff40
leaq -0x208(%rbp), %rdi
callq 0xff40
leaq -0x30(%rbp), %rdi
callq 0xff40
movb $0x1, %al
andb $0x1, %al
addq $0x2c0, %rsp            # imm = 0x2C0
popq %rbp
retq
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0xc8(%rbp)
movl %eax, -0xcc(%rbp)
jmp 0x15368
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0xc8(%rbp)
movl %eax, -0xcc(%rbp)
leaq -0x218(%rbp), %rdi
callq 0xff40
leaq -0x208(%rbp), %rdi
callq 0xff40
leaq -0x30(%rbp), %rdi
callq 0xff40
movq -0xc8(%rbp), %rdi
callq 0xa7c0
nopl (%rax)