// File: /workspace/github_cpp_makefile_star2_O1/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 $0x1f8, %rsp            # imm = 0x1F8
movsd %xmm0, -0x90(%rbp)
movq %r8, -0x30(%rbp)
movq 0x8(%rdi), %rax
cmpq 0x10(%rdi), %rax
jne 0x2892
movq %rsi, %r13
cmpq %rax, 0x8(%rsi)
jne 0x2892
movq %rcx, %r12
movq %rdx, %rbx
movq 0x8(%rdx), %rcx
cmpq %rax, %rcx
jne 0x2892
cmpq 0x10(%rbx), %rcx
jne 0x2892
movq 0x8(%r12), %rdx
cmpq 0x10(%r12), %rdx
jne 0x2892
movq 0x10(%r13), %rcx
cmpq %rcx, %rdx
jne 0x2892
movq -0x30(%rbp), %rdx
cmpq %rax, 0x8(%rdx)
jne 0x2892
movq -0x30(%rbp), %rax
cmpq %rcx, 0x10(%rax)
jne 0x2892
movq %rdi, %r15
movq %r9, -0xb0(%rbp)
leaq -0x188(%rbp), %rsi
movq %r13, (%rsi)
leaq -0x88(%rbp), %rdi
callq 0x3e7a
movq 0x8(%r12), %rax
movq 0x10(%r12), %rcx
cmpq %rcx, %rax
jne 0x29a9
cmpq %rcx, 0x10(%r13)
jne 0x29c0
movq %r13, -0x70(%rbp)
movq %r12, -0x68(%rbp)
movq -0x30(%rbp), %rcx
movq %rcx, -0x60(%rbp)
cmpq 0x10(%rcx), %rax
jne 0x29da
leaq -0x188(%rbp), %rdi
leaq -0x70(%rbp), %rdx
movq %r15, %rsi
callq 0x36d2
leaq -0xf8(%rbp), %r14
leaq -0x188(%rbp), %rsi
movq %r14, %rdi
callq 0x3726
leaq -0x188(%rbp), %rsi
movq %r14, (%rsi)
leaq -0xe0(%rbp), %rdi
callq 0xf2b4
movq 0x8(%r12), %rax
movq 0x10(%r12), %rdx
cmpq %rdx, %rax
jne 0x29a9
movq -0x30(%rbp), %rsi
movq 0x10(%rsi), %rcx
cmpq %rdx, %rcx
jne 0x29c0
movq %rsi, -0x70(%rbp)
movq %r12, -0x68(%rbp)
movq %rsi, -0x60(%rbp)
cmpq %rcx, %rax
jne 0x29da
leaq -0x188(%rbp), %rdi
leaq -0x70(%rbp), %rdx
movq %rbx, %rsi
callq 0x36d2
leaq -0xc8(%rbp), %rdi
leaq -0x188(%rbp), %rsi
callq 0x3726
leaq -0x48(%rbp), %rdi
movq %rbx, %rsi
callq 0xf53a
leaq -0xa8(%rbp), %rdi
leaq -0x48(%rbp), %rsi
callq 0xf53a
movl $0x1, %ebx
movq -0x40(%rbp), %rax
cmpq %rax, -0xd0(%rbp)
jne 0x28e8
movq -0x38(%rbp), %rcx
cmpq -0xf0(%rbp), %rcx
jne 0x28ff
cmpq 0x8(%r13), %rcx
jne 0x28ff
cmpq %rax, -0x78(%rbp)
jne 0x28e8
movq -0x80(%rbp), %rax
cmpq %rax, 0x8(%r12)
jne 0x291e
movq 0x10(%r13), %rcx
cmpq %rcx, 0x10(%r12)
jne 0x291e
cmpq %rcx, %rax
jne 0x294c
movq -0xd8(%rbp), %rcx
movq -0xe8(%rbp), %rax
leaq -0xe0(%rbp), %rdi
movq %rdi, -0x180(%rbp)
leaq -0x48(%rbp), %r8
movq %r8, -0x178(%rbp)
leaq -0xf8(%rbp), %rsi
movq %rsi, -0x170(%rbp)
movq %rdi, -0x168(%rbp)
movq %r8, -0x160(%rbp)
movq %r13, -0x158(%rbp)
movq %r12, -0x150(%rbp)
leaq -0x88(%rbp), %rdx
movq %rdx, -0x148(%rbp)
movq %r8, -0x140(%rbp)
movq %r13, -0x138(%rbp)
movq %rdx, -0x128(%rbp)
movq %r8, -0x120(%rbp)
movq %rsi, -0x118(%rbp)
leaq -0xc8(%rbp), %rdx
movq %rdx, -0x108(%rbp)
cmpq -0xc0(%rbp), %rcx
jne 0x298a
cmpq -0xb8(%rbp), %rax
jne 0x298a
leaq -0x48(%rbp), %r14
movq %r14, %rdi
leaq -0x188(%rbp), %rsi
leaq -0x70(%rbp), %rdx
callq 0xf621
movq %r14, -0x70(%rbp)
leaq -0xa8(%rbp), %rax
movq %rax, -0x68(%rbp)
movq -0x40(%rbp), %rax
cmpq -0xa0(%rbp), %rax
jne 0x2935
movq -0x38(%rbp), %rax
cmpq -0x98(%rbp), %rax
jne 0x2935
leaq -0x188(%rbp), %rdi
leaq -0x70(%rbp), %rsi
callq 0x1730c
movq -0x180(%rbp), %rax
testq %rax, %rax
jle 0x296b
cmpq $0x0, -0x178(%rbp)
jle 0x296b
movq -0x188(%rbp), %rcx
movq %rcx, -0x70(%rbp)
movq %rax, -0x68(%rbp)
leaq -0x70(%rbp), %rdi
leaq -0x200(%rbp), %rsi
leaq -0x188(%rbp), %rdx
callq 0x17568
movapd %xmm0, %xmm1
andpd 0x1799e(%rip), %xmm1    # 0x1a010
movsd -0x90(%rbp), %xmm0
movapd %xmm1, -0x1d0(%rbp)
ucomisd %xmm1, %xmm0
ja 0x269e
leaq -0xa8(%rbp), %rdi
leaq -0x48(%rbp), %rsi
leaq -0x70(%rbp), %rdx
callq 0x41b8
jmp 0x26ee
movl $0x28, %edx
movq 0x3f926(%rip), %rdi     # 0x41fd0
leaq 0x17bfc(%rip), %rsi     # 0x1a2ad
callq 0x2140
movl %ebx, %esi
movq 0x3f911(%rip), %rdi     # 0x41fd0
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
incl %ebx
movsd -0x90(%rbp), %xmm0
ucomisd -0x1d0(%rbp), %xmm0
jbe 0x24c3
movq -0x78(%rbp), %rax
cmpq -0x40(%rbp), %rax
jne 0x28e8
leaq -0x88(%rbp), %rax
movq %rax, -0x1b8(%rbp)
leaq -0x48(%rbp), %rax
movq %rax, -0x1b0(%rbp)
movq %r13, -0x1a8(%rbp)
movq -0x38(%rbp), %rax
cmpq 0x8(%r13), %rax
jne 0x28ff
leaq -0x70(%rbp), %rdi
leaq -0x1b8(%rbp), %rdx
movq %r12, %rsi
callq 0x37c4
movq -0x30(%rbp), %rsi
movq -0x68(%rbp), %rbx
movq -0x58(%rbp), %rax
movq 0x8(%rbx), %rcx
cmpq 0x10(%rax), %rcx
jne 0x294c
movq -0x70(%rbp), %r14
movupd -0x60(%rbp), %xmm0
movapd %xmm0, -0x220(%rbp)
movq -0x78(%rbp), %rax
cmpq -0x40(%rbp), %rax
jne 0x28e8
leaq -0x88(%rbp), %rax
movq %rax, -0x1a0(%rbp)
leaq -0x48(%rbp), %rax
movq %rax, -0x198(%rbp)
movq %r15, -0x190(%rbp)
movq -0x38(%rbp), %rax
cmpq 0x8(%r15), %rax
jne 0x28ff
leaq -0x1d8(%rbp), %rdx
movq %rsi, (%rdx)
leaq -0x200(%rbp), %rdi
leaq -0x1a0(%rbp), %rsi
callq 0x3818
movq %r14, -0x188(%rbp)
movq %rbx, -0x180(%rbp)
movaps -0x220(%rbp), %xmm0
movups %xmm0, -0x178(%rbp)
movupd -0x200(%rbp), %xmm0
movupd -0x1f0(%rbp), %xmm1
movupd %xmm1, -0x150(%rbp)
movupd %xmm0, -0x160(%rbp)
movq 0x8(%rbx), %rax
movq -0x1e8(%rbp), %rcx
cmpq 0x10(%rcx), %rax
movq -0xb0(%rbp), %rdi
jne 0x29f4
leaq -0x188(%rbp), %rsi
callq 0x3870
movq -0xa8(%rbp), %rdi
callq 0x2150
movq -0x48(%rbp), %rdi
callq 0x2150
movq -0xc8(%rbp), %rdi
callq 0x2150
movq -0xe0(%rbp), %rdi
callq 0x2150
movq -0xf8(%rbp), %rdi
callq 0x2150
movq -0x88(%rbp), %rdi
callq 0x2150
movb $0x1, %al
jmp 0x28d6
movq 0x3f737(%rip), %rbx     # 0x41fd0
leaq 0x179d0(%rip), %rsi     # 0x1a270
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 $0x1f8, %rsp            # imm = 0x1F8
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
leaq 0x17ecd(%rip), %rdi     # 0x1a7bc
leaq 0x17f52(%rip), %rsi     # 0x1a848
leaq 0x22de5(%rip), %rcx     # 0x256e2
jmp 0x2914
leaq 0x17eb6(%rip), %rdi     # 0x1a7bc
leaq 0x17f3b(%rip), %rsi     # 0x1a848
leaq 0x22e95(%rip), %rcx     # 0x257a9
movl $0x62, %edx
callq 0x2090
leaq 0x181ac(%rip), %rdi     # 0x1aad1
leaq 0x181de(%rip), %rsi     # 0x1ab0a
leaq 0x22f9b(%rip), %rcx     # 0x258ce
jmp 0x299f
leaq 0x18195(%rip), %rdi     # 0x1aad1
leaq 0x181c7(%rip), %rsi     # 0x1ab0a
leaq 0x3573b(%rip), %rcx     # 0x38085
jmp 0x299f
leaq 0x17da6(%rip), %rdi     # 0x1a6f9
leaq 0x17db0(%rip), %rsi     # 0x1a70a
leaq 0x23175(%rip), %rcx     # 0x25ad6
movl $0x15f, %edx            # imm = 0x15F
callq 0x2090
leaq 0x18bb5(%rip), %rdi     # 0x1b527
leaq 0x18bf2(%rip), %rsi     # 0x1b56b
leaq 0x359b1(%rip), %rcx     # 0x38331
movl $0x19b, %edx            # imm = 0x19B
callq 0x2090
leaq 0x18140(%rip), %rdi     # 0x1aad1
leaq 0x18172(%rip), %rsi     # 0x1ab0a
leaq 0x23348(%rip), %rcx     # 0x25ce7
movl $0x74, %edx
callq 0x2090
leaq 0x17d49(%rip), %rdi     # 0x1a6f9
leaq 0x17d53(%rip), %rsi     # 0x1a70a
leaq 0x17d7b(%rip), %rcx     # 0x1a739
jmp 0x2961
leaq 0x17df5(%rip), %rdi     # 0x1a7bc
leaq 0x17e7a(%rip), %rsi     # 0x1a848
leaq 0x17ea0(%rip), %rcx     # 0x1a875
jmp 0x2914
leaq 0x17ddb(%rip), %rdi     # 0x1a7bc
leaq 0x17e60(%rip), %rsi     # 0x1a848
leaq 0x17f6d(%rip), %rcx     # 0x1a95c
jmp 0x2914
leaq 0x17dc1(%rip), %rdi     # 0x1a7bc
leaq 0x17e46(%rip), %rsi     # 0x1a848
leaq 0x35c33(%rip), %rcx     # 0x3863c
jmp 0x2914
jmp 0x2a2d
movq %rax, %rbx
jmp 0x2a4d
movq %rax, %rbx
jmp 0x2a56
movq %rax, %rbx
jmp 0x2a6e
jmp 0x2a2d
movq %rax, %rbx
jmp 0x2a62
movq %rax, %rbx
jmp 0x2a7a
jmp 0x2a2d
movq %rax, %rbx
jmp 0x2a41
movq %rax, %rbx
movq -0x188(%rbp), %rdi
callq 0x2150
movq -0xa8(%rbp), %rdi
callq 0x2150
movq -0x48(%rbp), %rdi
callq 0x2150
movq -0xc8(%rbp), %rdi
callq 0x2150
movq -0xe0(%rbp), %rdi
callq 0x2150
movq -0xf8(%rbp), %rdi
callq 0x2150
movq -0x88(%rbp), %rdi
callq 0x2150
movq %rbx, %rdi
callq 0x21b0