// File: /workspace/github_cpp_makefile_star2_O2/canzarlab[P]Trajan/geno/augmentedLagrangian.cpp
// Function: AugmentedLagrangian::solve()
SolverStatus AugmentedLagrangian::solve() 
{  
  static const double gamma = 2;
  //  static const double rhoMin = 1E-6;
  //  static const double rhoMax = 10;
    
  Scalar rho = 0;
//  Vector y = Vector::Zero(_genoNLP.getM());
  _genoNLP.getStartingPointDual(_y.data());
  //  std::cout << "y= " << y.transpose() << std::endl;
  AugmentedNLP augmentedNLP(_genoNLP, rho, _y);
  size_t mEqualities = augmentedNLP.mEqualities();
  size_t mInequalities = augmentedNLP.mInequalities();
  std::vector<int> cType = augmentedNLP.cType();
  

  /*
  _genoNLP.getStartingPoint(_x.data());
  Vector dummyG(_genoNLP.getN());
  _genoNLP.functionValueAndGradient(_x.data(), _f, dummyG.data());
  double dummyF;
  augmentedNLP.functionValueAndGradient(_x.data(), dummyF, dummyG.data());
  Vector constraintError = augmentedNLP.constraintError();

  for (size_t i = 0; i < mInequalities; ++i)
    if (constraintError(mEqualities + i) < 0)
      constraintError(mEqualities + i) = 0;
  std::cout << "constraint Error = " << constraintError.transpose() << std::endl;
  rho = 2 * (std::abs(_f) + 1) / constraintError.squaredNorm();
  if (rho < rhoMin)
    rho = rhoMin;
  if(rho > rhoMax)
    rho = rhoMax;
  */

  Vector constraintError;
  Vector cl = augmentedNLP.cl();
  Vector cu = augmentedNLP.cu();
  Vector dummyG(_genoNLP.getN());
  rho = 1.0;

  LBFGSB solver(augmentedNLP, _correctionPairs, _verbose);
  //NEWTONB solver(augmentedNLP, _correctionPairs);
  if (_tol != -1) solver.setParameter("pgtol", _tol);
  if (_tolFun != -1) solver.setParameter("factr", _tolFun);
  double oldFactor = INF;
  double tau = 0.5;
  double lambdaMin = -1E20;
  double lambdaMax = 1E20;
  double muMax = 1E20;
  double muMin = -1E20;
  SolverStatus status = SOLVED;
  
  for (size_t iter = 0; iter < _maxIter; ++iter) 
  {
    if (_verbose)
      std::cout << "rho = " << rho << std::endl;

    augmentedNLP.setParams(rho, _y);
    solver.restart();
    status = solver.solve();
    if ((status != SOLVED) && (status != SUBOPTIMAL))
      break;
    constraintError = augmentedNLP.constraintError();
    Vector trueConstraintError(constraintError);
    for (size_t i = 0; i < mInequalities; ++i) {
      if (cType[i+mEqualities] == 2)
        if (trueConstraintError(mEqualities + i) > 0)
          trueConstraintError(mEqualities + i) = 0;

      if (cType[i+mEqualities] == 4)
	if (trueConstraintError(mEqualities + i) < 0)
	  trueConstraintError(mEqualities + i) = 0;
    }
    double constraintErrorNorm = 
      trueConstraintError.lpNorm<Eigen::Infinity>();

    _x = Vector::Map(solver.x(), _x.size());

    //    std::cout << "cl " << cl.transpose() << std::endl;
    //    std::cout << "cu " << cu.transpose() << std::endl;
    //    std::cout << "constraint Values " << augmentedNLP.constraintValues().transpose() << std::endl;
    //    std::cout << "y " << y.transpose() << std::endl;
    if (constraintErrorNorm < _constraintsTol)
      break;

    //    y = y + rho * constraintError;

    _y.head(mEqualities) +=  rho * constraintError.head(mEqualities);

    //safeguard Lagrange multiplier for equality constraints
    for (size_t i = 0; i < mEqualities; ++i) {
      if (_y(i) < lambdaMin)
	_y(i) = lambdaMin;
      if (_y(i) > lambdaMax)
	_y(i) = lambdaMax;
    }


    Vector constraintValues = augmentedNLP.constraintValues();

    // compute new Lagrange multiplier and safeguard it
    for (size_t i = 0; i < mInequalities; ++i) {
      if (cType[i+mEqualities] == 2) {
	_y(i+mEqualities) += rho * constraintError(i+mEqualities);
        if (_y(i+mEqualities) > 0)
          _y(i+mEqualities) = 0;
      }

      if (cType[i+mEqualities] == 3) {
	double yLB = std::min(0.0, _y(i+mEqualities));
	double yUB = std::max(0.0, _y(i+mEqualities));
	yLB += rho * (constraintValues(i+mEqualities) - cl(i));
	yUB += rho * (constraintValues(i+mEqualities) - cu(i));
	yLB = std::min(0.0, yLB);
	yUB = std::max(0.0, yUB);

	_y(i+mEqualities) = yLB + yUB;

      }

      if (cType[i+mEqualities] == 4) {
        _y(i+mEqualities) += rho * constraintError(i+mEqualities);
 	if (_y(i+mEqualities) < 0)
	  _y(i+mEqualities) = 0;
      }

      if (_y(i+mEqualities) < muMin)
        _y(i+mEqualities) = muMin;
      if (_y(i+mEqualities) > muMax)
	_y(i+mEqualities) = muMax;
    }
    
    //    double factor = constraintErrorNorm;

    double factor = 0;
    // need to have the if there since if there are no equalities Eigen will make a mistake
    if (mEqualities)  factor = constraintError.head(mEqualities).lpNorm<Eigen::Infinity>();

    //    std::cout << "factor = " << factor << std::endl;
    for (size_t i = 0; i < mInequalities; ++i) {
      factor = std::max(std::abs(std::max(constraintError(mEqualities + i), -_y(mEqualities + i) / rho)), factor);
    }
    if (factor > tau * oldFactor) {
      rho *= gamma;
    }
    oldFactor = factor;
  }
  _genoNLP.functionValueAndGradient(_x.data(), _f, dummyG.data());
  return status;
}

// Assembly:
pushq %rbp
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0x508, %rsp            # imm = 0x508
movq %rdi, %rbx
andq $0x0, 0x10(%rsp)
leaq 0x38(%rdi), %r14
movq (%rdi), %rdi
movq 0x38(%rbx), %rsi
movq (%rdi), %rax
callq *0x40(%rax)
movq (%rbx), %rsi
leaq 0x170(%rsp), %r15
xorpd %xmm0, %xmm0
movq %r15, %rdi
movq %r14, %rdx
callq 0xcf20
movq 0x20(%r15), %rbp
movq 0x28(%r15), %rax
movq %rax, 0x40(%rsp)
leaq 0x1e8(%rsp), %rsi
leaq 0x60(%rsp), %rdi
callq 0xd22c
xorpd %xmm0, %xmm0
movapd %xmm0, 0x20(%rsp)
leaq 0x1c8(%rsp), %rsi
leaq 0xd8(%rsp), %rdi
callq 0xe746
leaq 0x1d8(%rsp), %rsi
leaq 0xc8(%rsp), %rdi
callq 0xe746
movq (%rbx), %rdi
movq (%rdi), %rax
callq *0x18(%rax)
leaq 0x2a8(%rsp), %rsi
movq %rax, (%rsi)
leaq 0xb8(%rsp), %rdi
callq 0xcea2
movabsq $0x3ff0000000000000, %rax # imm = 0x3FF0000000000000
movq %rax, 0x10(%rsp)
movq 0x18(%rbx), %rdx
movzbl 0x60(%rbx), %ecx
leaq 0x2a8(%rsp), %rdi
leaq 0x170(%rsp), %rsi
callq 0x12ab6
movsd 0x50(%rbx), %xmm0
ucomisd 0x44e3c(%rip), %xmm0    # 0x513e0
jne 0xc5a8
jnp 0xc5eb
leaq 0x44e69(%rip), %rsi     # 0x51418
leaq 0x118(%rsp), %rdi
leaq 0x210(%rsp), %rdx
callq 0xb7a6
movsd 0x50(%rbx), %xmm0
leaq 0x2a8(%rsp), %rdi
leaq 0x118(%rsp), %rsi
callq 0x130c0
leaq 0x118(%rsp), %rdi
callq 0x9708
movsd 0x58(%rbx), %xmm0
ucomisd 0x44de8(%rip), %xmm0    # 0x513e0
jne 0xc5fc
jnp 0xc63f
leaq 0x44e1b(%rip), %rsi     # 0x5141e
leaq 0xf8(%rsp), %rdi
leaq 0x210(%rsp), %rdx
callq 0xb7a6
movsd 0x58(%rbx), %xmm0
leaq 0x2a8(%rsp), %rdi
leaq 0xf8(%rsp), %rsi
callq 0x130c0
leaq 0xf8(%rsp), %rdi
callq 0x9708
leaq 0x28(%rbx), %rax
movq %rax, 0x78(%rsp)
movq $0x0, 0x50(%rsp)
movsd 0x44d8f(%rip), %xmm0    # 0x513e8
movsd %xmm0, 0x48(%rsp)
leaq 0xe8(%rsp), %r12
leaq 0x210(%rsp), %r13
xorl %eax, %eax
movq %rbx, 0x38(%rsp)
cmpq 0x48(%rbx), %rax
jae 0xcc80
movq %rax, 0x88(%rsp)
cmpb $0x1, 0x60(%rbx)
jne 0xc6b7
movq 0x8a91b(%rip), %rdi     # 0x96fb0
leaq 0x44d88(%rip), %rsi     # 0x51424
callq 0x9330
movsd 0x10(%rsp), %xmm0
movq %rax, %rdi
callq 0x9540
movq %rax, %rdi
callq 0x9170
movsd 0x10(%rsp), %xmm0
movsd %xmm0, 0x1b0(%rsp)
leaq 0x1b8(%rsp), %rdi
movq %r14, %rsi
callq 0xe787
leaq 0x2a8(%rsp), %rdi
callq 0x12516
leaq 0x2a8(%rsp), %rdi
callq 0x1274e
movl %eax, %ebx
cmpl $0x1, %eax
movq %rbx, 0x50(%rsp)
ja 0xcc7b
leaq 0x20(%rsp), %rdi
leaq 0x200(%rsp), %rsi
callq 0xe787
movq %r12, %rdi
leaq 0x20(%rsp), %rsi
callq 0xe746
movq %rbp, %r15
movq 0x40(%rsp), %rbx
subq $0x1, %rbx
jb 0xc796
movq 0x60(%rsp), %rax
cmpl $0x2, (%rax,%r15,4)
jne 0xc761
movq %r12, %rdi
movq %r15, %rsi
callq 0xd27a
movsd (%rax), %xmm0
ucomisd 0x44cb8(%rip), %xmm0    # 0x51408
jbe 0xc761
movq %r12, %rdi
movq %r15, %rsi
callq 0xd27a
andq $0x0, (%rax)
movq 0x60(%rsp), %rax
cmpl $0x4, (%rax,%r15,4)
jne 0xc791
movq %r12, %rdi
movq %r15, %rsi
callq 0xd27a
xorpd %xmm0, %xmm0
ucomisd (%rax), %xmm0
jbe 0xc791
movq %r12, %rdi
movq %r15, %rsi
callq 0xd27a
andq $0x0, (%rax)
incq %r15
jmp 0xc727
movq %r12, %rdi
callq 0xea40
movsd %xmm0, 0x58(%rsp)
leaq 0x2a8(%rsp), %rdi
callq 0x130b4
movq 0x38(%rsp), %rcx
movq 0x30(%rcx), %rdx
movq %r13, %rdi
movq %rax, %rsi
callq 0xebcc
movq 0x78(%rsp), %rdi
movq %r13, %rsi
callq 0xebf9
movq 0x38(%rsp), %rax
movsd 0x8(%rax), %xmm0
movsd %xmm0, 0x80(%rsp)
ucomisd 0x58(%rsp), %xmm0
ja 0xcc47
leaq 0x138(%rsp), %rbx
movq %rbx, %rdi
leaq 0x20(%rsp), %rsi
movq %rbp, %rdx
callq 0xd304
movq %r13, %rdi
leaq 0x10(%rsp), %rsi
movq %rbx, %rdx
callq 0xd2b0
leaq 0x270(%rsp), %rbx
movq %rbx, %rdi
movq %r14, %rsi
movq %rbp, %rdx
callq 0xd304
movq %rbx, %rdi
movq %r13, %rsi
callq 0xd318
xorl %r15d, %r15d
cmpq %r15, %rbp
je 0xc8a9
movq %r14, %rdi
movq %r15, %rsi
callq 0xd27a
movsd 0x44b9b(%rip), %xmm0    # 0x513f0
ucomisd (%rax), %xmm0
jbe 0xc873
movq %r14, %rdi
movq %r15, %rsi
callq 0xd27a
movabsq $-0x3bea50e2874a73c0, %rcx # imm = 0xC415AF1D78B58C40
movq %rcx, (%rax)
movq %r14, %rdi
movq %r15, %rsi
callq 0xd27a
movsd (%rax), %xmm0
ucomisd 0x44b6e(%rip), %xmm0    # 0x513f8
jbe 0xc8a4
movq %r14, %rdi
movq %r15, %rsi
callq 0xd27a
movabsq $0x4415af1d78b58c40, %rcx # imm = 0x4415AF1D78B58C40
movq %rcx, (%rax)
incq %r15
jmp 0xc83d
leaq 0x138(%rsp), %rdi
leaq 0x1a0(%rsp), %rsi
callq 0xe746
movq %rbp, %r15
movq 0x40(%rsp), %rbx
xorl %r13d, %r13d
subq $0x1, %rbx
jb 0xcb67
movq 0x60(%rsp), %rax
cmpl $0x2, (%rax,%r15,4)
jne 0xc949
movsd 0x10(%rsp), %xmm0
movsd %xmm0, 0x8(%rsp)
leaq 0x20(%rsp), %rdi
movq %r15, %rsi
callq 0xd27a
movsd (%rax), %xmm0
movsd %xmm0, 0x18(%rsp)
movq %r14, %rdi
movq %r15, %rsi
callq 0xd27a
movsd 0x8(%rsp), %xmm0
mulsd 0x18(%rsp), %xmm0
addsd (%rax), %xmm0
movsd %xmm0, (%rax)
movq %r14, %rdi
movq %r15, %rsi
callq 0xd27a
movsd (%rax), %xmm0
ucomisd 0x44ad0(%rip), %xmm0    # 0x51408
jbe 0xc949
movq %r14, %rdi
movq %r15, %rsi
callq 0xd27a
andq $0x0, (%rax)
movq 0x60(%rsp), %rax
movl (%rax,%r15,4), %eax
cmpl $0x3, %eax
jne 0xca8f
movq %r14, %rdi
movq %r15, %rsi
callq 0xd27a
movsd (%rax), %xmm0
movsd %xmm0, 0x18(%rsp)
movq %r14, %rdi
movq %r15, %rsi
callq 0xd27a
movsd (%rax), %xmm0
movsd %xmm0, 0xa8(%rsp)
movsd 0x10(%rsp), %xmm0
movsd %xmm0, 0xb0(%rsp)
leaq 0x138(%rsp), %rdi
movq %r15, %rsi
callq 0xd27a
movsd (%rax), %xmm0
movsd %xmm0, 0x8(%rsp)
leaq 0xd8(%rsp), %rdi
movq %r13, %rsi
callq 0xd27a
movsd 0x8(%rsp), %xmm0
subsd (%rax), %xmm0
movsd %xmm0, 0x8(%rsp)
movsd 0x10(%rsp), %xmm0
movsd %xmm0, 0xa0(%rsp)
leaq 0x138(%rsp), %rdi
movq %r15, %rsi
callq 0xd27a
movsd (%rax), %xmm0
movsd %xmm0, 0x98(%rsp)
leaq 0xc8(%rsp), %rdi
movq %r13, %rsi
callq 0xd27a
movsd (%rax), %xmm0
movsd %xmm0, 0x90(%rsp)
movq %r14, %rdi
movq %r15, %rsi
callq 0xd27a
xorpd %xmm0, %xmm0
movsd 0x18(%rsp), %xmm2
minsd %xmm0, %xmm2
movsd 0xa8(%rsp), %xmm3
maxsd %xmm0, %xmm3
movsd 0xb0(%rsp), %xmm1
mulsd 0x8(%rsp), %xmm1
addsd %xmm2, %xmm1
movsd 0x98(%rsp), %xmm4
subsd 0x90(%rsp), %xmm4
movsd 0xa0(%rsp), %xmm2
mulsd %xmm4, %xmm2
addsd %xmm3, %xmm2
maxsd %xmm0, %xmm2
minsd %xmm0, %xmm1
addsd %xmm2, %xmm1
movsd %xmm1, (%rax)
movq 0x60(%rsp), %rax
movl (%rax,%r15,4), %eax
cmpl $0x4, %eax
jne 0xcafa
movsd 0x10(%rsp), %xmm0
movsd %xmm0, 0x8(%rsp)
leaq 0x20(%rsp), %rdi
movq %r15, %rsi
callq 0xd27a
movsd (%rax), %xmm0
movsd %xmm0, 0x18(%rsp)
movq %r14, %rdi
movq %r15, %rsi
callq 0xd27a
movsd 0x8(%rsp), %xmm0
mulsd 0x18(%rsp), %xmm0
addsd (%rax), %xmm0
movsd %xmm0, (%rax)
movq %r14, %rdi
movq %r15, %rsi
callq 0xd27a
xorpd %xmm0, %xmm0
ucomisd (%rax), %xmm0
jbe 0xcafa
movq %r14, %rdi
movq %r15, %rsi
callq 0xd27a
andq $0x0, (%rax)
movq %r14, %rdi
movq %r15, %rsi
callq 0xd27a
movsd 0x448e3(%rip), %xmm0    # 0x513f0
ucomisd (%rax), %xmm0
jbe 0xcb2b
movq %r14, %rdi
movq %r15, %rsi
callq 0xd27a
movabsq $-0x3bea50e2874a73c0, %rcx # imm = 0xC415AF1D78B58C40
movq %rcx, (%rax)
movq %r14, %rdi
movq %r15, %rsi
callq 0xd27a
movsd (%rax), %xmm0
ucomisd 0x448b6(%rip), %xmm0    # 0x513f8
jbe 0xcb5c
movq %r14, %rdi
movq %r15, %rsi
callq 0xd27a
movabsq $0x4415af1d78b58c40, %rcx # imm = 0x4415AF1D78B58C40
movq %rcx, (%rax)
incq %r13
incq %r15
jmp 0xc8c9
testq %rbp, %rbp
je 0xcb94
movq %rbp, %rdx
leaq 0x210(%rsp), %rbx
movq %rbx, %rdi
leaq 0x20(%rsp), %rsi
callq 0xd304
movq %rbx, %rdi
callq 0xeee8
movsd %xmm0, 0x8(%rsp)
jmp 0xcb9e
xorpd %xmm0, %xmm0
movsd %xmm0, 0x8(%rsp)
movq %rbp, %r15
movq 0x40(%rsp), %rbx
subq $0x1, %rbx
jb 0xcbfc
leaq 0x20(%rsp), %rdi
movq %r15, %rsi
callq 0xd27a
movq %rax, %r13
movq %r14, %rdi
movq %r15, %rsi
callq 0xd27a
movsd (%rax), %xmm0
xorpd 0x447ed(%rip), %xmm0    # 0x513c0
divsd 0x10(%rsp), %xmm0
maxsd (%r13), %xmm0
andpd 0x447e9(%rip), %xmm0    # 0x513d0
movsd 0x8(%rsp), %xmm1
maxsd %xmm0, %xmm1
movsd %xmm1, 0x8(%rsp)
incq %r15
jmp 0xcba6
movsd 0x48(%rsp), %xmm0
mulsd 0x447f6(%rip), %xmm0    # 0x51400
movsd 0x8(%rsp), %xmm1
ucomisd %xmm0, %xmm1
leaq 0x210(%rsp), %r13
jbe 0xcc2e
movsd 0x10(%rsp), %xmm0
addsd %xmm0, %xmm0
movsd %xmm0, 0x10(%rsp)
movq 0x138(%rsp), %rdi
callq 0x93c0
movsd 0x8(%rsp), %xmm0
movsd %xmm0, 0x48(%rsp)
movq 0xe8(%rsp), %rdi
callq 0x93c0
movq 0x88(%rsp), %rax
incq %rax
movsd 0x80(%rsp), %xmm0
ucomisd 0x58(%rsp), %xmm0
movq 0x38(%rsp), %rbx
jbe 0xc676
jmp 0xcc80
movq 0x38(%rsp), %rbx
movq (%rbx), %rdi
movq 0x28(%rbx), %rsi
movq 0xb8(%rsp), %rcx
addq $0x20, %rbx
movq (%rdi), %rax
movq %rbx, %rdx
callq *0x48(%rax)
leaq 0x2a8(%rsp), %rdi
callq 0xd334
movq 0xb8(%rsp), %rdi
callq 0x93c0
movq 0xc8(%rsp), %rdi
callq 0x93c0
movq 0xd8(%rsp), %rdi
callq 0x93c0
movq 0x20(%rsp), %rdi
callq 0x93c0
leaq 0x60(%rsp), %rdi
callq 0xd85a
leaq 0x170(%rsp), %rdi
callq 0xd468
movq 0x50(%rsp), %rax
addq $0x508, %rsp            # imm = 0x508
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
movq %rax, %rbx
leaq 0xf8(%rsp), %rdi
jmp 0xcd22
jmp 0xcd5a
movq %rax, %rbx
leaq 0x118(%rsp), %rdi
callq 0x9708
jmp 0xcd8d
jmp 0xcd5a
jmp 0xcd5a
movq %rax, %rbx
jmp 0xcd9a
movq %rax, %rbx
jmp 0xcdb4
movq %rax, %rbx
jmp 0xcdc1
movq %rax, %rbx
jmp 0xcdd5
movq %rax, %rbx
jmp 0xcda7
jmp 0xcd6b
jmp 0xcd70
jmp 0xcd6b
jmp 0xcd6b
jmp 0xcd6b
jmp 0xcd5a
jmp 0xcd6b
movq %rax, %rbx
jmp 0xcd8d
jmp 0xcd70
jmp 0xcd70
jmp 0xcd70
jmp 0xcd70
jmp 0xcd70
jmp 0xcd6b
movq %rax, %rbx
jmp 0xcd80
movq %rax, %rbx
movq 0x138(%rsp), %rdi
callq 0x93c0
movq 0xe8(%rsp), %rdi
callq 0x93c0
leaq 0x2a8(%rsp), %rdi
callq 0xd334
movq 0xb8(%rsp), %rdi
callq 0x93c0
movq 0xc8(%rsp), %rdi
callq 0x93c0
movq 0xd8(%rsp), %rdi
callq 0x93c0
movq 0x20(%rsp), %rdi
callq 0x93c0
leaq 0x60(%rsp), %rdi
callq 0xd85a
leaq 0x170(%rsp), %rdi
callq 0xd468
movq %rbx, %rdi
callq 0x9640