// File: /workspace/github_cpp_makefile_star2_O2/schlagenhauf[P]lqr_solve/lqr_solve.cpp
// Function: compGainMatrix(Eigen::Matrix<double, -1, -1, 0, -1, -1> const&, Eigen::Matrix<double, -1, -1, 0, -1, -1> const&, Eigen::Matrix<double, -1, -1, 0, -1, -1> const&, Eigen::Matrix<double, -1, -1, 0, -1, -1> const&, Eigen::Matrix<double, -1, -1, 0, -1, -1> const&, Eigen::Matrix<double, -1, -1, 0, -1, -1>*, double)
bool compGainMatrix(const Mtx &A, const Mtx &B, const Mtx &Q, const Mtx &R,
                    const Mtx &N, Mtx *K, double eps = 1e-15) {
  // check if dimensions are compatible
  if (A.rows() != A.cols() || B.rows() != A.rows() || Q.rows() != Q.cols() ||
      Q.rows() != A.rows() || R.rows() != R.cols() || R.rows() != B.cols() ||
      N.rows() != A.rows() || N.cols() != B.cols()) {
    std::cout << "One or more matrices have incompatible dimensions. Aborting."
              << std::endl;
    return false;
  }

  // precompute as much as possible
  Mtx B_T = B.transpose();
  Mtx Acal = A - B * R.inverse() * N.transpose();
  Mtx Acal_T = Acal.transpose();
  Mtx Qcal = Q - N * R.inverse() * N.transpose();

  // initialize P with Q
  Mtx P = Q;

  // iterate until P converges
  unsigned int numIterations = 0;
  Mtx Pold = P;
  while (true) {
    numIterations++;

    // compute new P
    P = Acal_T * P * Acal -
        Acal_T * P * B * (R + B_T * P * B).inverse() * B_T * P * Acal + Qcal;

    // update delta
    Mtx delta = P - Pold;
    if (fabs(delta.maxCoeff()) < eps) {
      std::cout << "Number of iterations until convergence: " << numIterations
                << std::endl;
      break;
    }
    Pold = P;
  }

  // compute K from P
  *K = (R + B_T * P * B).inverse() * (B_T * P * A + N.transpose());

  return true;
}

// Assembly:
pushq %rbp
movq %rsp, %rbp
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0x3c8, %rsp            # imm = 0x3C8
movsd %xmm0, -0x48(%rbp)
movq %r9, -0x80(%rbp)
movq %r8, -0x30(%rbp)
movq %rcx, -0x38(%rbp)
movq 0x8(%rdi), %rax
cmpq 0x10(%rdi), %rax
jne 0x27e1
movq %rsi, %r13
cmpq %rax, 0x8(%rsi)
jne 0x27e1
movq %rdx, %rbx
movq 0x8(%rdx), %rcx
cmpq %rax, %rcx
jne 0x27e1
cmpq 0x10(%rbx), %rcx
jne 0x27e1
movq -0x38(%rbp), %rcx
movq 0x8(%rcx), %rdx
cmpq 0x10(%rcx), %rdx
jne 0x27e1
movq 0x10(%r13), %rcx
cmpq %rcx, %rdx
jne 0x27e1
movq -0x30(%rbp), %rdx
cmpq %rax, 0x8(%rdx)
jne 0x27e1
movq -0x30(%rbp), %rax
cmpq %rcx, 0x10(%rax)
jne 0x27e1
movq %rdi, -0x40(%rbp)
leaq -0x240(%rbp), %rsi
movq %r13, (%rsi)
leaq -0x78(%rbp), %rdi
callq 0x354a
movq -0x38(%rbp), %rdi
callq 0x3104
leaq -0x2e0(%rbp), %rsi
movq %rax, (%rsi)
movq %r13, %rdi
callq 0x30da
leaq -0x338(%rbp), %rsi
movq %rax, (%rsi)
movq %rdx, 0x8(%rsi)
leaq -0x1b0(%rbp), %rdx
movq -0x30(%rbp), %rax
movq %rax, (%rdx)
leaq -0x3f0(%rbp), %rdi
callq 0x382e
leaq -0x240(%rbp), %rdi
leaq -0x3f0(%rbp), %rdx
movq -0x40(%rbp), %rsi
callq 0x30be
leaq -0xc8(%rbp), %r14
leaq -0x240(%rbp), %rsi
movq %r14, %rdi
callq 0x38bc
leaq -0x240(%rbp), %rsi
movq %r14, (%rsi)
leaq -0xb0(%rbp), %rdi
callq 0x101ec
movq -0x38(%rbp), %rdi
callq 0x3104
movq -0x30(%rbp), %r14
leaq -0x2e0(%rbp), %rsi
movq %rax, (%rsi)
movq %r14, %rdi
callq 0x30da
leaq -0x338(%rbp), %rsi
movq %rax, (%rsi)
movq %rdx, 0x8(%rsi)
leaq -0x1b0(%rbp), %rdx
movq %r14, (%rdx)
leaq -0x3f0(%rbp), %rdi
callq 0x382e
leaq -0x240(%rbp), %rdi
leaq -0x3f0(%rbp), %rdx
movq %rbx, %rsi
callq 0x30be
leaq -0x110(%rbp), %rdi
leaq -0x240(%rbp), %rsi
callq 0x38bc
leaq -0x60(%rbp), %rdi
movq %rbx, %rsi
callq 0x1046a
leaq -0x98(%rbp), %rdi
leaq -0x60(%rbp), %rsi
callq 0x1046a
pushq $0x1
popq %r12
leaq -0x60(%rbp), %r14
leaq -0x3f0(%rbp), %r15
leaq -0x240(%rbp), %rbx
leaq -0xb0(%rbp), %rdi
movq %r14, %rsi
callq 0x3136
movq %rax, -0xf8(%rbp)
movq %rdx, -0xf0(%rbp)
leaq -0x138(%rbp), %rdi
leaq -0xf8(%rbp), %rsi
leaq -0xc8(%rbp), %rdx
callq 0x104ee
leaq -0xb0(%rbp), %rdi
movq %r14, %rsi
callq 0x3136
movq %rax, -0xe8(%rbp)
movq %rdx, -0xe0(%rbp)
leaq -0x168(%rbp), %rdi
leaq -0xe8(%rbp), %rsi
movq %r13, %rdx
callq 0x104ee
leaq -0x78(%rbp), %rdi
movq %r14, %rsi
callq 0x3136
movq %rax, -0xd8(%rbp)
movq %rdx, -0xd0(%rbp)
leaq -0x150(%rbp), %rdi
leaq -0xd8(%rbp), %rsi
movq %r13, %rdx
callq 0x104ee
leaq -0x268(%rbp), %rdi
movq -0x38(%rbp), %rsi
leaq -0x150(%rbp), %rdx
callq 0x317c
leaq -0x290(%rbp), %rdi
leaq -0x268(%rbp), %rsi
callq 0x3198
leaq -0x378(%rbp), %rdi
leaq -0x168(%rbp), %rsi
leaq -0x290(%rbp), %rdx
callq 0x10578
leaq -0x1b0(%rbp), %rdi
leaq -0x378(%rbp), %rsi
leaq -0x78(%rbp), %rdx
callq 0x105ca
leaq -0x2e0(%rbp), %rdi
leaq -0x1b0(%rbp), %rsi
movq %r14, %rdx
callq 0x1061c
leaq -0x338(%rbp), %rdi
leaq -0x2e0(%rbp), %rsi
leaq -0xc8(%rbp), %rdx
callq 0x10676
movq %r15, %rdi
leaq -0x138(%rbp), %rsi
leaq -0x338(%rbp), %rdx
callq 0x3160
movq %rbx, %rdi
movq %r15, %rsi
leaq -0x110(%rbp), %rdx
callq 0x31e0
movq %r14, %rdi
movq %rbx, %rsi
callq 0x10812
movq %r15, %rdi
movq %r14, %rsi
leaq -0x98(%rbp), %rdx
callq 0x31fc
movq %rbx, %rdi
movq %r15, %rsi
callq 0x17a08
movq %rbx, %rdi
callq 0x17c78
movapd %xmm0, %xmm1
andpd 0x179ab(%rip), %xmm1    # 0x1a010
movsd -0x48(%rbp), %xmm0
movapd %xmm1, -0x120(%rbp)
ucomisd %xmm1, %xmm0
ja 0x2689
leaq -0x98(%rbp), %rdi
movq %r14, %rsi
callq 0x17dcc
jmp 0x26af
movq 0x4e940(%rip), %rdi     # 0x50fd0
leaq 0x17c16(%rip), %rsi     # 0x1a2ad
callq 0x2130
movl %r12d, %esi
movq %rax, %rdi
callq 0x20c0
movq %rax, %rdi
callq 0x2060
movq -0x240(%rbp), %rdi
callq 0x2140
incl %r12d
movsd -0x48(%rbp), %xmm0
ucomisd -0x120(%rbp), %xmm0
jbe 0x24dc
leaq -0x78(%rbp), %rdi
leaq -0x60(%rbp), %rsi
callq 0x3136
leaq -0x290(%rbp), %rsi
movq %rax, (%rsi)
movq %rdx, 0x8(%rsi)
leaq -0x1b0(%rbp), %rdi
movq %r13, %rdx
callq 0x104ee
leaq -0x338(%rbp), %rdi
leaq -0x1b0(%rbp), %rdx
movq -0x38(%rbp), %rsi
callq 0x317c
leaq -0x3f0(%rbp), %rdi
leaq -0x338(%rbp), %rsi
callq 0x3198
leaq -0x78(%rbp), %rdi
leaq -0x60(%rbp), %rsi
callq 0x3136
leaq -0x268(%rbp), %rsi
movq %rax, (%rsi)
movq %rdx, 0x8(%rsi)
leaq -0x378(%rbp), %rdi
movq -0x40(%rbp), %rdx
callq 0x104ee
leaq -0x138(%rbp), %rdx
movq -0x30(%rbp), %rax
movq %rax, (%rdx)
leaq -0x2e0(%rbp), %rdi
leaq -0x378(%rbp), %rsi
callq 0x3218
leaq -0x240(%rbp), %rdi
leaq -0x3f0(%rbp), %rsi
leaq -0x2e0(%rbp), %rdx
callq 0x17e36
leaq -0x240(%rbp), %rsi
movq -0x80(%rbp), %rdi
callq 0x17e87
movq -0x98(%rbp), %rdi
callq 0x2140
movq -0x60(%rbp), %rdi
callq 0x2140
movq -0x110(%rbp), %rdi
callq 0x2140
movq -0xb0(%rbp), %rdi
callq 0x2140
movq -0xc8(%rbp), %rdi
callq 0x2140
movq -0x78(%rbp), %rdi
callq 0x2140
movb $0x1, %al
jmp 0x27fe
movq 0x4e7e8(%rip), %rdi     # 0x50fd0
leaq 0x17a81(%rip), %rsi     # 0x1a270
callq 0x2130
movq %rax, %rdi
callq 0x2060
xorl %eax, %eax
addq $0x3c8, %rsp            # imm = 0x3C8
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
movq %rax, %rbx
jmp 0x2859
movq %rax, %rbx
jmp 0x2862
movq %rax, %rbx
jmp 0x287a
jmp 0x284a
jmp 0x2827
jmp 0x282c
jmp 0x284a
movq %rax, %rbx
jmp 0x286e
movq %rax, %rbx
jmp 0x2886
jmp 0x284a
jmp 0x284a
jmp 0x284a
jmp 0x284a
movq %rax, %rbx
movq -0x240(%rbp), %rdi
callq 0x2140
jmp 0x284d
movq %rax, %rbx
movq -0x98(%rbp), %rdi
callq 0x2140
movq -0x60(%rbp), %rdi
callq 0x2140
movq -0x110(%rbp), %rdi
callq 0x2140
movq -0xb0(%rbp), %rdi
callq 0x2140
movq -0xc8(%rbp), %rdi
callq 0x2140
movq -0x78(%rbp), %rdi
callq 0x2140
movq %rbx, %rdi
callq 0x21b0