// File: 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
subq $0x740, %rsp            # imm = 0x740
movq %rdi, -0x10(%rbp)
movq %rsi, -0x18(%rbp)
movq %rdx, -0x20(%rbp)
movq %rcx, -0x28(%rbp)
movq %r8, -0x30(%rbp)
movq %r9, -0x38(%rbp)
movsd %xmm0, -0x40(%rbp)
movq -0x10(%rbp), %rdi
callq 0x3bd0
movq %rax, -0x618(%rbp)
movq -0x10(%rbp), %rdi
callq 0x3bf0
movq %rax, %rcx
movq -0x618(%rbp), %rax
cmpq %rcx, %rax
jne 0x2568
movq -0x18(%rbp), %rdi
callq 0x3bd0
movq %rax, -0x620(%rbp)
movq -0x10(%rbp), %rdi
callq 0x3bd0
movq %rax, %rcx
movq -0x620(%rbp), %rax
cmpq %rcx, %rax
jne 0x2568
movq -0x20(%rbp), %rdi
callq 0x3bd0
movq %rax, -0x628(%rbp)
movq -0x20(%rbp), %rdi
callq 0x3bf0
movq %rax, %rcx
movq -0x628(%rbp), %rax
cmpq %rcx, %rax
jne 0x2568
movq -0x20(%rbp), %rdi
callq 0x3bd0
movq %rax, -0x630(%rbp)
movq -0x10(%rbp), %rdi
callq 0x3bd0
movq %rax, %rcx
movq -0x630(%rbp), %rax
cmpq %rcx, %rax
jne 0x2568
movq -0x28(%rbp), %rdi
callq 0x3bd0
movq %rax, -0x638(%rbp)
movq -0x28(%rbp), %rdi
callq 0x3bf0
movq %rax, %rcx
movq -0x638(%rbp), %rax
cmpq %rcx, %rax
jne 0x2568
movq -0x28(%rbp), %rdi
callq 0x3bd0
movq %rax, -0x640(%rbp)
movq -0x18(%rbp), %rdi
callq 0x3bf0
movq %rax, %rcx
movq -0x640(%rbp), %rax
cmpq %rcx, %rax
jne 0x2568
movq -0x30(%rbp), %rdi
callq 0x3bd0
movq %rax, -0x648(%rbp)
movq -0x10(%rbp), %rdi
callq 0x3bd0
movq %rax, %rcx
movq -0x648(%rbp), %rax
cmpq %rcx, %rax
jne 0x2568
movq -0x30(%rbp), %rdi
callq 0x3bf0
movq %rax, -0x650(%rbp)
movq -0x18(%rbp), %rdi
callq 0x3bf0
movq %rax, %rcx
movq -0x650(%rbp), %rax
cmpq %rcx, %rax
je 0x2593
movq 0xc2a61(%rip), %rdi     # 0xc4fd0
leaq 0x61cd2(%rip), %rsi     # 0x64248
callq 0x2140
movq %rax, %rdi
movq 0xc2a23(%rip), %rsi     # 0xc4fa8
callq 0x2150
movb $0x0, -0x1(%rbp)
jmp 0x2d73
movq -0x18(%rbp), %rdi
callq 0x3c10
movq %rax, -0x60(%rbp)
leaq -0x58(%rbp), %rdi
leaq -0x60(%rbp), %rsi
callq 0x3c40
movq -0x10(%rbp), %rax
movq %rax, -0x668(%rbp)
movq -0x18(%rbp), %rax
movq %rax, -0x660(%rbp)
movq -0x28(%rbp), %rdi
callq 0x3d40
movq %rax, -0x658(%rbp)
jmp 0x25d5
movq -0x660(%rbp), %rdi
movq -0x658(%rbp), %rax
movq %rax, -0xd0(%rbp)
leaq -0xd0(%rbp), %rsi
callq 0x3cf0
movq %rdx, -0x678(%rbp)
movq %rax, -0x670(%rbp)
jmp 0x2606
movq -0x678(%rbp), %rax
movq -0x670(%rbp), %rcx
movq %rcx, -0xc8(%rbp)
movq %rax, -0xc0(%rbp)
movq -0x30(%rbp), %rdi
callq 0x3c10
movq %rax, -0x680(%rbp)
jmp 0x2634
movq -0x680(%rbp), %rax
movq %rax, -0xe8(%rbp)
leaq -0xb8(%rbp), %rdi
leaq -0xc8(%rbp), %rsi
leaq -0xe8(%rbp), %rdx
callq 0x3dc0
jmp 0x265e
movq -0x668(%rbp), %rsi
leaq -0xa0(%rbp), %rdi
leaq -0xb8(%rbp), %rdx
callq 0x3c80
jmp 0x267a
leaq -0x78(%rbp), %rdi
leaq -0xa0(%rbp), %rsi
callq 0x3e10
jmp 0x268c
leaq -0x78(%rbp), %rdi
callq 0x3e50
movq %rax, -0x688(%rbp)
jmp 0x269e
movq -0x688(%rbp), %rax
movq %rax, -0x108(%rbp)
leaq -0x100(%rbp), %rdi
leaq -0x108(%rbp), %rsi
callq 0x3e80
jmp 0x26c1
movq -0x20(%rbp), %rax
movq %rax, -0x6a0(%rbp)
movq -0x30(%rbp), %rax
movq %rax, -0x698(%rbp)
movq -0x28(%rbp), %rdi
callq 0x3d40
movq %rax, -0x690(%rbp)
jmp 0x26e9
movq -0x698(%rbp), %rdi
movq -0x690(%rbp), %rax
movq %rax, -0x178(%rbp)
leaq -0x178(%rbp), %rsi
callq 0x3cf0
movq %rdx, -0x6b0(%rbp)
movq %rax, -0x6a8(%rbp)
jmp 0x271a
movq -0x6b0(%rbp), %rax
movq -0x6a8(%rbp), %rcx
movq %rcx, -0x170(%rbp)
movq %rax, -0x168(%rbp)
movq -0x30(%rbp), %rdi
callq 0x3c10
movq %rax, -0x6b8(%rbp)
jmp 0x2748
movq -0x6b8(%rbp), %rax
movq %rax, -0x180(%rbp)
leaq -0x160(%rbp), %rdi
leaq -0x170(%rbp), %rsi
leaq -0x180(%rbp), %rdx
callq 0x3dc0
jmp 0x2772
movq -0x6a0(%rbp), %rsi
leaq -0x148(%rbp), %rdi
leaq -0x160(%rbp), %rdx
callq 0x3c80
jmp 0x278e
leaq -0x120(%rbp), %rdi
leaq -0x148(%rbp), %rsi
callq 0x3e10
jmp 0x27a3
movq -0x20(%rbp), %rsi
leaq -0x198(%rbp), %rdi
callq 0x3ec0
jmp 0x27b5
movl $0x0, -0x19c(%rbp)
leaq -0x1b8(%rbp), %rdi
leaq -0x198(%rbp), %rsi
callq 0x3ec0
jmp 0x27d4
jmp 0x27d6
jmp 0x27d8
movl -0x19c(%rbp), %eax
incl %eax
movl %eax, -0x19c(%rbp)
leaq -0x100(%rbp), %rdi
leaq -0x198(%rbp), %rsi
callq 0x3ef0
movq %rdx, -0x6c8(%rbp)
movq %rax, -0x6c0(%rbp)
jmp 0x2809
movq -0x6c8(%rbp), %rax
movq -0x6c0(%rbp), %rcx
movq %rcx, -0x2e8(%rbp)
movq %rax, -0x2e0(%rbp)
leaq -0x2d8(%rbp), %rdi
leaq -0x2e8(%rbp), %rsi
leaq -0x78(%rbp), %rdx
callq 0x3f40
jmp 0x283e
leaq -0x100(%rbp), %rdi
leaq -0x198(%rbp), %rsi
callq 0x3ef0
movq %rdx, -0x6d8(%rbp)
movq %rax, -0x6d0(%rbp)
jmp 0x2861
movq -0x6d8(%rbp), %rax
movq -0x6d0(%rbp), %rcx
movq %rcx, -0x440(%rbp)
movq %rax, -0x438(%rbp)
movq -0x18(%rbp), %rdx
leaq -0x430(%rbp), %rdi
leaq -0x440(%rbp), %rsi
callq 0x3f40
jmp 0x2896
movq -0x28(%rbp), %rax
movq %rax, -0x6f0(%rbp)
leaq -0x58(%rbp), %rdi
leaq -0x198(%rbp), %rsi
callq 0x3ef0
movq %rdx, -0x6e8(%rbp)
movq %rax, -0x6e0(%rbp)
jmp 0x28c1
movq -0x6e8(%rbp), %rax
movq -0x6e0(%rbp), %rcx
movq %rcx, -0x4b8(%rbp)
movq %rax, -0x4b0(%rbp)
movq -0x18(%rbp), %rdx
leaq -0x4a8(%rbp), %rdi
leaq -0x4b8(%rbp), %rsi
callq 0x3f40
jmp 0x28f6
movq -0x6f0(%rbp), %rsi
leaq -0x490(%rbp), %rdi
leaq -0x4a8(%rbp), %rdx
callq 0x4050
jmp 0x2912
leaq -0x468(%rbp), %rdi
leaq -0x490(%rbp), %rsi
callq 0x40c0
jmp 0x2927
leaq -0x418(%rbp), %rdi
leaq -0x430(%rbp), %rsi
leaq -0x468(%rbp), %rdx
callq 0x4000
jmp 0x2943
leaq -0x3d8(%rbp), %rdi
leaq -0x418(%rbp), %rsi
leaq -0x58(%rbp), %rdx
callq 0x4150
jmp 0x295c
leaq -0x390(%rbp), %rdi
leaq -0x3d8(%rbp), %rsi
leaq -0x198(%rbp), %rdx
callq 0x41a0
jmp 0x2978
leaq -0x340(%rbp), %rdi
leaq -0x390(%rbp), %rsi
leaq -0x78(%rbp), %rdx
callq 0x41f0
jmp 0x2991
leaq -0x2c0(%rbp), %rdi
leaq -0x2d8(%rbp), %rsi
leaq -0x340(%rbp), %rdx
callq 0x3f90
jmp 0x29ad
leaq -0x248(%rbp), %rdi
leaq -0x2c0(%rbp), %rsi
leaq -0x120(%rbp), %rdx
callq 0x4240
jmp 0x29c9
leaq -0x198(%rbp), %rdi
leaq -0x248(%rbp), %rsi
callq 0x42b0
jmp 0x29de
leaq -0x4e8(%rbp), %rdi
leaq -0x198(%rbp), %rsi
leaq -0x1b8(%rbp), %rdx
callq 0x42e0
jmp 0x29fa
leaq -0x4d0(%rbp), %rdi
leaq -0x4e8(%rbp), %rsi
callq 0x4350
jmp 0x2a0f
leaq -0x4d0(%rbp), %rdi
callq 0x4390
movsd %xmm0, -0x6f8(%rbp)
jmp 0x2a25
movsd -0x6f8(%rbp), %xmm1
movaps 0x615dc(%rip), %xmm0    # 0x64010
pand %xmm0, %xmm1
movsd -0x40(%rbp), %xmm0
ucomisd %xmm1, %xmm0
jbe 0x2b4f
movq 0xc2582(%rip), %rdi     # 0xc4fd0
leaq 0x61830(%rip), %rsi     # 0x64285
callq 0x2140
movq %rax, -0x700(%rbp)
jmp 0x2a63
movq -0x700(%rbp), %rdi
movl -0x19c(%rbp), %esi
callq 0x21a0
movq %rax, -0x708(%rbp)
jmp 0x2a7e
movq -0x708(%rbp), %rdi
movq 0xc251c(%rip), %rsi     # 0xc4fa8
callq 0x2150
jmp 0x2a93
movl $0x3, -0x4ec(%rbp)
jmp 0x2b6e
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0xd8(%rbp)
movl %eax, -0xdc(%rbp)
jmp 0x2d68
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0xd8(%rbp)
movl %eax, -0xdc(%rbp)
jmp 0x2d5f
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0xd8(%rbp)
movl %eax, -0xdc(%rbp)
jmp 0x2d53
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0xd8(%rbp)
movl %eax, -0xdc(%rbp)
jmp 0x2d47
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0xd8(%rbp)
movl %eax, -0xdc(%rbp)
jmp 0x2d3b
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0xd8(%rbp)
movl %eax, -0xdc(%rbp)
jmp 0x2d2f
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0xd8(%rbp)
movl %eax, -0xdc(%rbp)
leaq -0x4d0(%rbp), %rdi
callq 0x43e0
jmp 0x2d2f
leaq -0x1b8(%rbp), %rdi
leaq -0x198(%rbp), %rsi
callq 0x43b0
jmp 0x2b64
movl $0x0, -0x4ec(%rbp)
leaq -0x4d0(%rbp), %rdi
callq 0x43e0
movl -0x4ec(%rbp), %eax
testl %eax, %eax
je 0x2b88
jmp 0x2b86
jmp 0x2b8d
jmp 0x27d6
movq -0x28(%rbp), %rax
movq %rax, -0x720(%rbp)
leaq -0x58(%rbp), %rdi
leaq -0x198(%rbp), %rsi
callq 0x3ef0
movq %rdx, -0x718(%rbp)
movq %rax, -0x710(%rbp)
jmp 0x2bb8
movq -0x718(%rbp), %rax
movq -0x710(%rbp), %rcx
movq %rcx, -0x5b8(%rbp)
movq %rax, -0x5b0(%rbp)
movq -0x18(%rbp), %rdx
leaq -0x5a8(%rbp), %rdi
leaq -0x5b8(%rbp), %rsi
callq 0x3f40
jmp 0x2bed
movq -0x720(%rbp), %rsi
leaq -0x590(%rbp), %rdi
leaq -0x5a8(%rbp), %rdx
callq 0x4050
jmp 0x2c09
leaq -0x568(%rbp), %rdi
leaq -0x590(%rbp), %rsi
callq 0x40c0
jmp 0x2c1e
leaq -0x58(%rbp), %rdi
leaq -0x198(%rbp), %rsi
callq 0x3ef0
movq %rdx, -0x730(%rbp)
movq %rax, -0x728(%rbp)
jmp 0x2c3e
movq -0x730(%rbp), %rax
movq -0x728(%rbp), %rcx
movq %rcx, -0x608(%rbp)
movq %rax, -0x600(%rbp)
movq -0x10(%rbp), %rdx
leaq -0x5f8(%rbp), %rdi
leaq -0x608(%rbp), %rsi
callq 0x3f40
jmp 0x2c73
movq -0x30(%rbp), %rdi
callq 0x3c10
movq %rax, -0x738(%rbp)
jmp 0x2c85
movq -0x738(%rbp), %rax
movq %rax, -0x610(%rbp)
leaq -0x5e0(%rbp), %rdi
leaq -0x5f8(%rbp), %rsi
leaq -0x610(%rbp), %rdx
callq 0x4450
jmp 0x2caf
leaq -0x540(%rbp), %rdi
leaq -0x568(%rbp), %rsi
leaq -0x5e0(%rbp), %rdx
callq 0x4400
jmp 0x2ccb
movq -0x38(%rbp), %rdi
leaq -0x540(%rbp), %rsi
callq 0x44c0
jmp 0x2cdd
movb $0x1, -0x1(%rbp)
movl $0x1, -0x4ec(%rbp)
leaq -0x1b8(%rbp), %rdi
callq 0x43e0
leaq -0x198(%rbp), %rdi
callq 0x43e0
leaq -0x120(%rbp), %rdi
callq 0x43e0
leaq -0x100(%rbp), %rdi
callq 0x43e0
leaq -0x78(%rbp), %rdi
callq 0x43e0
leaq -0x58(%rbp), %rdi
callq 0x43e0
jmp 0x2d73
leaq -0x1b8(%rbp), %rdi
callq 0x43e0
leaq -0x198(%rbp), %rdi
callq 0x43e0
leaq -0x120(%rbp), %rdi
callq 0x43e0
leaq -0x100(%rbp), %rdi
callq 0x43e0
leaq -0x78(%rbp), %rdi
callq 0x43e0
leaq -0x58(%rbp), %rdi
callq 0x43e0
jmp 0x2d81
movb -0x1(%rbp), %al
andb $0x1, %al
addq $0x740, %rsp            # imm = 0x740
popq %rbp
retq
movq -0xd8(%rbp), %rdi
callq 0x21f0
nopl (%rax)