// File: /workspace/github_cpp_makefile_star2_O3/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 $0x1d8, %rsp            # imm = 0x1D8
movsd %xmm0, -0xe0(%rbp)
movq %r8, -0x30(%rbp)
movq 0x8(%rdi), %rax
cmpq 0x10(%rdi), %rax
jne 0x26ba
movq %rsi, %r13
cmpq %rax, 0x8(%rsi)
jne 0x26ba
movq %rcx, %r12
movq %rdx, %rbx
movq 0x8(%rdx), %rcx
cmpq %rax, %rcx
jne 0x26ba
cmpq 0x10(%rbx), %rcx
jne 0x26ba
movq 0x8(%r12), %rcx
cmpq 0x10(%r12), %rcx
jne 0x26ba
cmpq 0x10(%r13), %rcx
jne 0x26ba
movq -0x30(%rbp), %rdx
cmpq %rax, 0x8(%rdx)
jne 0x26ba
movq -0x30(%rbp), %rax
cmpq %rcx, 0x10(%rax)
jne 0x26ba
movq %r9, %r14
movq %rdi, %r15
leaq -0x188(%rbp), %rsi
movq %r13, (%rsi)
leaq -0x88(%rbp), %rdi
callq 0x3dd0
movq 0x8(%r12), %rax
cmpq 0x10(%r12), %rax
jne 0x2992
cmpq %rax, 0x10(%r13)
jne 0x29a9
movq %r13, -0x70(%rbp)
movq %r12, -0x68(%rbp)
movq -0x30(%rbp), %rcx
movq %rcx, -0x60(%rbp)
cmpq 0x10(%rcx), %rax
jne 0x29c3
leaq -0x188(%rbp), %rdi
leaq -0x70(%rbp), %rdx
movq %r15, %rsi
callq 0x36bc
leaq -0xd0(%rbp), %rdi
leaq -0x188(%rbp), %rsi
callq 0x4044
leaq -0x188(%rbp), %rsi
leaq -0xd0(%rbp), %rax
movq %rax, (%rsi)
leaq -0xb8(%rbp), %rdi
callq 0xf0c6
movq 0x8(%r12), %rax
cmpq 0x10(%r12), %rax
jne 0x2992
movq -0x30(%rbp), %rcx
cmpq %rax, 0x10(%rcx)
jne 0x29a9
leaq -0x70(%rbp), %rdx
movq %rcx, (%rdx)
movq %r12, 0x8(%rdx)
movq %rcx, 0x10(%rdx)
leaq -0x188(%rbp), %rdi
movq %rbx, %rsi
callq 0x36bc
leaq -0xf8(%rbp), %rdi
leaq -0x188(%rbp), %rsi
callq 0x4044
leaq -0x48(%rbp), %rdi
movq %rbx, %rsi
callq 0xf33a
movq %r14, -0xd8(%rbp)
leaq -0xa0(%rbp), %rdi
leaq -0x48(%rbp), %rbx
movq %rbx, %rsi
callq 0xf33a
movq -0xa8(%rbp), %rax
cmpq -0x40(%rbp), %rax
jne 0x26a0
movl $0x1, %r14d
movq -0x38(%rbp), %rcx
cmpq -0xc8(%rbp), %rcx
jne 0x28e8
cmpq 0x8(%r13), %rcx
jne 0x28e8
cmpq %rax, -0x78(%rbp)
jne 0x26a0
movq 0x8(%r12), %rax
cmpq -0x80(%rbp), %rax
jne 0x2907
movq 0x10(%r12), %rcx
cmpq 0x10(%r13), %rcx
jne 0x2907
cmpq %rcx, %rax
jne 0x2935
movq -0xb0(%rbp), %rcx
movq -0xc0(%rbp), %rax
leaq -0xb8(%rbp), %rdi
movq %rdi, -0x180(%rbp)
movq %rbx, -0x178(%rbp)
leaq -0xd0(%rbp), %rsi
movq %rsi, -0x170(%rbp)
movq %rdi, -0x168(%rbp)
movq %rbx, -0x160(%rbp)
movq %r13, -0x158(%rbp)
movq %r12, -0x150(%rbp)
leaq -0x88(%rbp), %rdx
movq %rdx, -0x148(%rbp)
movq %rbx, -0x140(%rbp)
movq %r13, -0x138(%rbp)
movq %rdx, -0x128(%rbp)
movq %rbx, -0x120(%rbp)
movq %rsi, -0x118(%rbp)
leaq -0xf8(%rbp), %rdx
movq %rdx, -0x108(%rbp)
cmpq -0xf0(%rbp), %rcx
jne 0x2973
cmpq -0xe8(%rbp), %rax
jne 0x2973
movq %rbx, %rdi
leaq -0x188(%rbp), %rsi
leaq -0x70(%rbp), %rdx
callq 0xf421
movq %rbx, -0x70(%rbp)
leaq -0xa0(%rbp), %rax
movq %rax, -0x68(%rbp)
movq -0x40(%rbp), %rax
cmpq -0x98(%rbp), %rax
jne 0x291e
movq -0x38(%rbp), %rax
cmpq -0x90(%rbp), %rax
jne 0x291e
leaq -0x188(%rbp), %rdi
leaq -0x70(%rbp), %rsi
callq 0x16c4e
movq -0x180(%rbp), %rax
testq %rax, %rax
jle 0x2954
cmpq $0x0, -0x178(%rbp)
jle 0x2954
movq -0x188(%rbp), %rcx
movq %rcx, -0x70(%rbp)
movq %rax, -0x68(%rbp)
leaq -0x70(%rbp), %rdi
leaq -0x1e8(%rbp), %rsi
leaq -0x188(%rbp), %rdx
callq 0x16e94
andpd 0x169b5(%rip), %xmm0    # 0x19010
movsd -0xe0(%rbp), %xmm1
ucomisd %xmm0, %xmm1
ja 0x2710
leaq -0xa0(%rbp), %rdi
movq %rbx, %rsi
leaq -0x70(%rbp), %rdx
callq 0x4179
movq -0x188(%rbp), %rdi
callq 0x2150
incl %r14d
movq -0xa8(%rbp), %rax
cmpq -0x40(%rbp), %rax
je 0x24c9
leaq 0x17115(%rip), %rdi     # 0x197bc
leaq 0x1719a(%rip), %rsi     # 0x19848
leaq 0x21b07(%rip), %rcx     # 0x241bc
jmp 0x28fd
movq 0x3d90f(%rip), %rbx     # 0x3ffd0
leaq 0x16ba8(%rip), %rsi     # 0x19270
movl $0x3c, %edx
movq %rbx, %rdi
callq 0x2140
movq (%rbx), %rax
movq -0x18(%rax), %rdi
addq %rbx, %rdi
movl $0xa, %esi
callq 0x2130
movsbl %al, %esi
movq %rbx, %rdi
callq 0x2030
movq %rax, %rdi
callq 0x20b0
xorl %eax, %eax
addq $0x1d8, %rsp            # imm = 0x1D8
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
movq 0x3d8b9(%rip), %rdi     # 0x3ffd0
leaq 0x16b8f(%rip), %rsi     # 0x192ad
movl $0x28, %edx
callq 0x2140
movl %r14d, %esi
movq 0x3d89e(%rip), %rdi     # 0x3ffd0
callq 0x20a0
movq %rax, %r14
movq (%rax), %rax
movq -0x18(%rax), %rdi
addq %r14, %rdi
movl $0xa, %esi
callq 0x2130
movsbl %al, %esi
movq %r14, %rdi
callq 0x2030
movq %rax, %rdi
callq 0x20b0
movq -0x188(%rbp), %rdi
callq 0x2150
movq -0x78(%rbp), %rax
cmpq -0x40(%rbp), %rax
jne 0x26a0
leaq -0x88(%rbp), %rax
movq %rax, -0x1b8(%rbp)
movq %rbx, -0x1b0(%rbp)
movq %r13, -0x1a8(%rbp)
movq -0x38(%rbp), %rax
cmpq 0x8(%r13), %rax
jne 0x28e8
leaq -0x70(%rbp), %rdi
leaq -0x1b8(%rbp), %rdx
movq %r12, %rsi
callq 0x3710
movq -0x30(%rbp), %rsi
movq -0x68(%rbp), %r14
movq -0x58(%rbp), %rax
movq 0x8(%r14), %rcx
cmpq 0x10(%rax), %rcx
jne 0x2935
movq -0x70(%rbp), %r12
movupd -0x60(%rbp), %xmm0
movapd %xmm0, -0x200(%rbp)
movq -0x78(%rbp), %rax
cmpq -0x40(%rbp), %rax
jne 0x26a0
leaq -0x88(%rbp), %rax
movq %rax, -0x1a0(%rbp)
movq %rbx, -0x198(%rbp)
movq %r15, -0x190(%rbp)
movq -0x38(%rbp), %rax
cmpq 0x8(%r15), %rax
jne 0x28e8
leaq -0x1c0(%rbp), %rdx
movq %rsi, (%rdx)
leaq -0x1e8(%rbp), %rdi
leaq -0x1a0(%rbp), %rsi
callq 0x3764
movq %r12, -0x188(%rbp)
movq %r14, -0x180(%rbp)
movaps -0x200(%rbp), %xmm0
movups %xmm0, -0x178(%rbp)
movupd -0x1e8(%rbp), %xmm0
movupd -0x1d8(%rbp), %xmm1
movupd %xmm1, -0x150(%rbp)
movupd %xmm0, -0x160(%rbp)
movq 0x8(%r14), %rax
movq -0x1d0(%rbp), %rcx
cmpq 0x10(%rcx), %rax
jne 0x29dd
leaq -0x188(%rbp), %rsi
movq -0xd8(%rbp), %rdi
callq 0x37bc
movq -0xa0(%rbp), %rdi
callq 0x2150
movq -0x48(%rbp), %rdi
callq 0x2150
movq -0xf8(%rbp), %rdi
callq 0x2150
movq -0xb8(%rbp), %rdi
callq 0x2150
movq -0xd0(%rbp), %rdi
callq 0x2150
movq -0x88(%rbp), %rdi
callq 0x2150
movb $0x1, %al
jmp 0x26fe
leaq 0x16ecd(%rip), %rdi     # 0x197bc
leaq 0x16f52(%rip), %rsi     # 0x19848
leaq 0x21986(%rip), %rcx     # 0x24283
movl $0x62, %edx
callq 0x2090
leaq 0x171c3(%rip), %rdi     # 0x19ad1
leaq 0x171f5(%rip), %rsi     # 0x19b0a
leaq 0x21a8c(%rip), %rcx     # 0x243a8
jmp 0x2988
leaq 0x171ac(%rip), %rdi     # 0x19ad1
leaq 0x171de(%rip), %rsi     # 0x19b0a
leaq 0x331c3(%rip), %rcx     # 0x35af6
jmp 0x2988
leaq 0x16dbd(%rip), %rdi     # 0x196f9
leaq 0x16dc7(%rip), %rsi     # 0x1970a
leaq 0x21c66(%rip), %rcx     # 0x245b0
movl $0x15f, %edx            # imm = 0x15F
callq 0x2090
leaq 0x17bcc(%rip), %rdi     # 0x1a527
leaq 0x17c09(%rip), %rsi     # 0x1a56b
leaq 0x33439(%rip), %rcx     # 0x35da2
movl $0x19b, %edx            # imm = 0x19B
callq 0x2090
leaq 0x17157(%rip), %rdi     # 0x19ad1
leaq 0x17189(%rip), %rsi     # 0x19b0a
leaq 0x21e39(%rip), %rcx     # 0x247c1
movl $0x74, %edx
callq 0x2090
leaq 0x16d60(%rip), %rdi     # 0x196f9
leaq 0x16d6a(%rip), %rsi     # 0x1970a
leaq 0x16d92(%rip), %rcx     # 0x19739
jmp 0x294a
leaq 0x16e0c(%rip), %rdi     # 0x197bc
leaq 0x16e91(%rip), %rsi     # 0x19848
leaq 0x16eb7(%rip), %rcx     # 0x19875
jmp 0x28fd
leaq 0x16df2(%rip), %rdi     # 0x197bc
leaq 0x16e77(%rip), %rsi     # 0x19848
leaq 0x16f84(%rip), %rcx     # 0x1995c
jmp 0x28fd
leaq 0x16dd8(%rip), %rdi     # 0x197bc
leaq 0x16e5d(%rip), %rsi     # 0x19848
leaq 0x336bb(%rip), %rcx     # 0x360ad
jmp 0x28fd
jmp 0x2a18
movq %rax, %rbx
jmp 0x2a38
movq %rax, %rbx
jmp 0x2a41
movq %rax, %rbx
jmp 0x2a59
jmp 0x2a18
movq %rax, %rbx
jmp 0x2a4d
movq %rax, %rbx
jmp 0x2a65
jmp 0x2a1d
jmp 0x2a18
movq %rax, %rbx
jmp 0x2a2c
movq %rax, %rbx
movq -0x188(%rbp), %rdi
callq 0x2150
movq -0xa0(%rbp), %rdi
callq 0x2150
movq -0x48(%rbp), %rdi
callq 0x2150
movq -0xf8(%rbp), %rdi
callq 0x2150
movq -0xb8(%rbp), %rdi
callq 0x2150
movq -0xd0(%rbp), %rdi
callq 0x2150
movq -0x88(%rbp), %rdi
callq 0x2150
movq %rbx, %rdi
callq 0x21b0