// File: schlagenhauf[P]lqr_solve/lqr_solve.cpp
// Function: main
int main() {
  Mtx A;
  Mtx B;
  Mtx Q;
  Mtx R;
  Mtx N;
  Mtx K;
  A.resize(4, 4);
  B.resize(4, 1);
  Q.resize(4, 4);
  R.resize(1, 1);
  N.resize(4, 1);

  A << 0.9904, 0.04772, 0.004251, 0.0007791, -0.3764, 0.9061, 0.167, 0.03211,
      0.002975, -0.004629, 0.9985, 0.04999, 0.1309, -0.1814, -0.06348, 0.9982;
  B << -0.00241, -0.09491, -9.478e-05, -0.0007852;
  Q << 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0;
  R << 100;
  N << 0, 0, 0, 0;

  if (!compGainMatrix(A, B, Q, R, N, &K))
    return 1;

  Eigen::IOFormat fmt(4, 0, ", ", "\n", "[", "]");
  std::cout << "K: " << K.format(fmt) << std::endl;

  return 0;
}

// Assembly:
pushq %rbp
movq %rsp, %rbp
subq $0x6e0, %rsp            # imm = 0x6E0
movl $0x0, -0x4(%rbp)
leaq -0x20(%rbp), %rdi
callq 0x44f0
leaq -0x38(%rbp), %rdi
callq 0x44f0
jmp 0x2db6
leaq -0x60(%rbp), %rdi
callq 0x44f0
jmp 0x2dc1
leaq -0x78(%rbp), %rdi
callq 0x44f0
jmp 0x2dcc
leaq -0x90(%rbp), %rdi
callq 0x44f0
jmp 0x2dda
leaq -0xa8(%rbp), %rdi
callq 0x44f0
jmp 0x2de8
leaq -0x20(%rbp), %rdi
movl $0x4, %edx
movq %rdx, %rsi
callq 0x4510
jmp 0x2dfb
leaq -0x38(%rbp), %rdi
movl $0x4, %esi
movl $0x1, %edx
callq 0x4510
jmp 0x2e10
leaq -0x60(%rbp), %rdi
movl $0x4, %edx
movq %rdx, %rsi
callq 0x4510
jmp 0x2e23
leaq -0x78(%rbp), %rdi
movl $0x1, %edx
movq %rdx, %rsi
callq 0x4510
jmp 0x2e36
leaq -0x90(%rbp), %rdi
movl $0x4, %esi
movl $0x1, %edx
callq 0x4510
jmp 0x2e4e
movabsq $0x3fefb15b573eab36, %rax # imm = 0x3FEFB15B573EAB36
movq %rax, -0xd0(%rbp)
leaq -0xc8(%rbp), %rdi
leaq -0x20(%rbp), %rsi
leaq -0xd0(%rbp), %rdx
callq 0x4600
jmp 0x2e78
movabsq $0x3fa86ec17ebaf102, %rax # imm = 0x3FA86EC17EBAF102
movq %rax, -0xd8(%rbp)
leaq -0xc8(%rbp), %rdi
leaq -0xd8(%rbp), %rsi
callq 0x4640
movq %rax, -0x578(%rbp)
jmp 0x2ea5
movq -0x578(%rbp), %rdi
movabsq $0x3f71697f1f9acffa, %rax # imm = 0x3F71697F1F9ACFFA
movq %rax, -0xe0(%rbp)
leaq -0xe0(%rbp), %rsi
callq 0x4640
movq %rax, -0x580(%rbp)
jmp 0x2ed2
movq -0x580(%rbp), %rdi
movabsq $0x3f4987908299a2d4, %rax # imm = 0x3F4987908299A2D4
movq %rax, -0xe8(%rbp)
leaq -0xe8(%rbp), %rsi
callq 0x4640
movq %rax, -0x588(%rbp)
jmp 0x2eff
movq -0x588(%rbp), %rdi
movabsq $-0x4027e90ff9724745, %rax # imm = 0xBFD816F0068DB8BB
movq %rax, -0xf0(%rbp)
leaq -0xf0(%rbp), %rsi
callq 0x4640
movq %rax, -0x590(%rbp)
jmp 0x2f2c
movq -0x590(%rbp), %rdi
movabsq $0x3fecfec56d5cfaad, %rax # imm = 0x3FECFEC56D5CFAAD
movq %rax, -0xf8(%rbp)
leaq -0xf8(%rbp), %rsi
callq 0x4640
movq %rax, -0x598(%rbp)
jmp 0x2f59
movq -0x598(%rbp), %rdi
movabsq $0x3fc5604189374bc7, %rax # imm = 0x3FC5604189374BC7
movq %rax, -0x100(%rbp)
leaq -0x100(%rbp), %rsi
callq 0x4640
movq %rax, -0x5a0(%rbp)
jmp 0x2f86
movq -0x5a0(%rbp), %rdi
movabsq $0x3fa070b8cfbfc654, %rax # imm = 0x3FA070B8CFBFC654
movq %rax, -0x108(%rbp)
leaq -0x108(%rbp), %rsi
callq 0x4640
movq %rax, -0x5a8(%rbp)
jmp 0x2fb3
movq -0x5a8(%rbp), %rdi
movabsq $0x3f685f06f6944674, %rax # imm = 0x3F685F06F6944674
movq %rax, -0x110(%rbp)
leaq -0x110(%rbp), %rsi
callq 0x4640
movq %rax, -0x5b0(%rbp)
jmp 0x2fe0
movq -0x5b0(%rbp), %rdi
movabsq $-0x408d0a24463065f9, %rax # imm = 0xBF72F5DBB9CF9A07
movq %rax, -0x118(%rbp)
leaq -0x118(%rbp), %rsi
callq 0x4640
movq %rax, -0x5b8(%rbp)
jmp 0x300d
movq -0x5b8(%rbp), %rdi
movabsq $0x3feff3b645a1cac1, %rax # imm = 0x3FEFF3B645A1CAC1
movq %rax, -0x120(%rbp)
leaq -0x120(%rbp), %rsi
callq 0x4640
movq %rax, -0x5c0(%rbp)
jmp 0x303a
movq -0x5c0(%rbp), %rdi
movabsq $0x3fa9984a0e410b63, %rax # imm = 0x3FA9984A0E410B63
movq %rax, -0x128(%rbp)
leaq -0x128(%rbp), %rsi
callq 0x4640
movq %rax, -0x5c8(%rbp)
jmp 0x3067
movq -0x5c8(%rbp), %rdi
movabsq $0x3fc0c154c985f06f, %rax # imm = 0x3FC0C154C985F06F
movq %rax, -0x130(%rbp)
leaq -0x130(%rbp), %rsi
callq 0x4640
movq %rax, -0x5d0(%rbp)
jmp 0x3094
movq -0x5d0(%rbp), %rdi
movabsq $-0x4038c7e28240b780, %rax # imm = 0xBFC7381D7DBF4880
movq %rax, -0x138(%rbp)
leaq -0x138(%rbp), %rsi
callq 0x4640
movq %rax, -0x5d8(%rbp)
jmp 0x30c1
movq -0x5d8(%rbp), %rdi
movabsq $-0x404fbfc6540cc78f, %rax # imm = 0xBFB04039ABF33871
movq %rax, -0x140(%rbp)
leaq -0x140(%rbp), %rsi
callq 0x4640
movq %rax, -0x5e0(%rbp)
jmp 0x30ee
movq -0x5e0(%rbp), %rdi
movabsq $0x3feff141205bc01a, %rax # imm = 0x3FEFF141205BC01A
movq %rax, -0x148(%rbp)
leaq -0x148(%rbp), %rsi
callq 0x4640
jmp 0x3114
leaq -0xc8(%rbp), %rdi
callq 0x47b0
movabsq $-0x409c41dd1a21ea36, %rax # imm = 0xBF63BE22E5DE15CA
movq %rax, -0x170(%rbp)
leaq -0x168(%rbp), %rdi
leaq -0x38(%rbp), %rsi
leaq -0x170(%rbp), %rdx
callq 0x4600
jmp 0x314a
movabsq $-0x4047b3fa6defc7a4, %rax # imm = 0xBFB84C059210385C
movq %rax, -0x178(%rbp)
leaq -0x168(%rbp), %rdi
leaq -0x178(%rbp), %rsi
callq 0x4640
movq %rax, -0x5e8(%rbp)
jmp 0x3177
movq -0x5e8(%rbp), %rdi
movabsq $-0x40e7276bffad74c3, %rax # imm = 0xBF18D89400528B3D
movq %rax, -0x180(%rbp)
leaq -0x180(%rbp), %rsi
callq 0x4640
movq %rax, -0x5f0(%rbp)
jmp 0x31a4
movq -0x5f0(%rbp), %rdi
movabsq $-0x40b64543d6ef657e, %rax # imm = 0xBF49BABC29109A82
movq %rax, -0x188(%rbp)
leaq -0x188(%rbp), %rsi
callq 0x4640
jmp 0x31ca
leaq -0x168(%rbp), %rdi
callq 0x47b0
movq $0x0, -0x1b0(%rbp)
leaq -0x1a8(%rbp), %rdi
leaq -0x60(%rbp), %rsi
leaq -0x1b0(%rbp), %rdx
callq 0x4600
jmp 0x31fa
movq $0x0, -0x1b8(%rbp)
leaq -0x1a8(%rbp), %rdi
leaq -0x1b8(%rbp), %rsi
callq 0x4640
movq %rax, -0x5f8(%rbp)
jmp 0x3221
movq -0x5f8(%rbp), %rdi
movq $0x0, -0x1c0(%rbp)
leaq -0x1c0(%rbp), %rsi
callq 0x4640
movq %rax, -0x600(%rbp)
jmp 0x3248
movq -0x600(%rbp), %rdi
movq $0x0, -0x1c8(%rbp)
leaq -0x1c8(%rbp), %rsi
callq 0x4640
movq %rax, -0x608(%rbp)
jmp 0x326f
movq -0x608(%rbp), %rdi
movq $0x0, -0x1d0(%rbp)
leaq -0x1d0(%rbp), %rsi
callq 0x4640
movq %rax, -0x610(%rbp)
jmp 0x3296
movq -0x610(%rbp), %rdi
movq $0x0, -0x1d8(%rbp)
leaq -0x1d8(%rbp), %rsi
callq 0x4640
movq %rax, -0x618(%rbp)
jmp 0x32bd
movq -0x618(%rbp), %rdi
movq $0x0, -0x1e0(%rbp)
leaq -0x1e0(%rbp), %rsi
callq 0x4640
movq %rax, -0x620(%rbp)
jmp 0x32e4
movq -0x620(%rbp), %rdi
movq $0x0, -0x1e8(%rbp)
leaq -0x1e8(%rbp), %rsi
callq 0x4640
movq %rax, -0x628(%rbp)
jmp 0x330b
movq -0x628(%rbp), %rdi
movq $0x0, -0x1f0(%rbp)
leaq -0x1f0(%rbp), %rsi
callq 0x4640
movq %rax, -0x630(%rbp)
jmp 0x3332
movq -0x630(%rbp), %rdi
movq $0x0, -0x1f8(%rbp)
leaq -0x1f8(%rbp), %rsi
callq 0x4640
movq %rax, -0x638(%rbp)
jmp 0x3359
movq -0x638(%rbp), %rdi
movabsq $0x3ff0000000000000, %rax # imm = 0x3FF0000000000000
movq %rax, -0x200(%rbp)
leaq -0x200(%rbp), %rsi
callq 0x4640
movq %rax, -0x640(%rbp)
jmp 0x3386
movq -0x640(%rbp), %rdi
movq $0x0, -0x208(%rbp)
leaq -0x208(%rbp), %rsi
callq 0x4640
movq %rax, -0x648(%rbp)
jmp 0x33ad
movq -0x648(%rbp), %rdi
movq $0x0, -0x210(%rbp)
leaq -0x210(%rbp), %rsi
callq 0x4640
movq %rax, -0x650(%rbp)
jmp 0x33d4
movq -0x650(%rbp), %rdi
movq $0x0, -0x218(%rbp)
leaq -0x218(%rbp), %rsi
callq 0x4640
movq %rax, -0x658(%rbp)
jmp 0x33fb
movq -0x658(%rbp), %rdi
movq $0x0, -0x220(%rbp)
leaq -0x220(%rbp), %rsi
callq 0x4640
movq %rax, -0x660(%rbp)
jmp 0x3422
movq -0x660(%rbp), %rdi
movq $0x0, -0x228(%rbp)
leaq -0x228(%rbp), %rsi
callq 0x4640
jmp 0x3442
leaq -0x1a8(%rbp), %rdi
callq 0x47b0
movabsq $0x4059000000000000, %rax # imm = 0x4059000000000000
movq %rax, -0x250(%rbp)
leaq -0x248(%rbp), %rdi
leaq -0x78(%rbp), %rsi
leaq -0x250(%rbp), %rdx
callq 0x4600
jmp 0x3478
leaq -0x248(%rbp), %rdi
callq 0x47b0
movq $0x0, -0x278(%rbp)
leaq -0x270(%rbp), %rdi
leaq -0x90(%rbp), %rsi
leaq -0x278(%rbp), %rdx
callq 0x4600
jmp 0x34ab
movq $0x0, -0x280(%rbp)
leaq -0x270(%rbp), %rdi
leaq -0x280(%rbp), %rsi
callq 0x4640
movq %rax, -0x668(%rbp)
jmp 0x34d2
movq -0x668(%rbp), %rdi
movq $0x0, -0x288(%rbp)
leaq -0x288(%rbp), %rsi
callq 0x4640
movq %rax, -0x670(%rbp)
jmp 0x34f9
movq -0x670(%rbp), %rdi
movq $0x0, -0x290(%rbp)
leaq -0x290(%rbp), %rsi
callq 0x4640
jmp 0x3519
leaq -0x270(%rbp), %rdi
callq 0x47b0
leaq -0x20(%rbp), %rdi
leaq -0x38(%rbp), %rsi
leaq -0x60(%rbp), %rdx
leaq -0x78(%rbp), %rcx
leaq -0x90(%rbp), %r8
leaq -0xa8(%rbp), %r9
movsd 0x60ad5(%rip), %xmm0    # 0x64020
callq 0x23f0
movb %al, -0x671(%rbp)
jmp 0x3558
movb -0x671(%rbp), %al
testb $0x1, %al
jne 0x3658
jmp 0x3568
movl $0x1, -0x4(%rbp)
movl $0x1, -0x294(%rbp)
jmp 0x3a1c
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x40(%rbp)
movl %eax, -0x44(%rbp)
jmp 0x3a97
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x40(%rbp)
movl %eax, -0x44(%rbp)
jmp 0x3a8e
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x40(%rbp)
movl %eax, -0x44(%rbp)
jmp 0x3a85
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x40(%rbp)
movl %eax, -0x44(%rbp)
jmp 0x3a7c
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x40(%rbp)
movl %eax, -0x44(%rbp)
jmp 0x3a70
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x40(%rbp)
movl %eax, -0x44(%rbp)
jmp 0x3a64
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x40(%rbp)
movl %eax, -0x44(%rbp)
leaq -0xc8(%rbp), %rdi
callq 0x47b0
jmp 0x3a64
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x40(%rbp)
movl %eax, -0x44(%rbp)
leaq -0x168(%rbp), %rdi
callq 0x47b0
jmp 0x3a64
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x40(%rbp)
movl %eax, -0x44(%rbp)
leaq -0x1a8(%rbp), %rdi
callq 0x47b0
jmp 0x3a64
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x40(%rbp)
movl %eax, -0x44(%rbp)
leaq -0x270(%rbp), %rdi
callq 0x47b0
jmp 0x3a64
leaq -0x3a9(%rbp), %rdi
movq %rdi, -0x680(%rbp)
callq 0x2200
movq -0x680(%rbp), %rdx
leaq 0x60c35(%rip), %rsi     # 0x642ae
leaq -0x3a8(%rbp), %rdi
callq 0x21b0
jmp 0x3687
leaq -0x3d1(%rbp), %rdi
movq %rdi, -0x688(%rbp)
callq 0x2200
movq -0x688(%rbp), %rdx
leaq 0x60c09(%rip), %rsi     # 0x642b1
leaq -0x3d0(%rbp), %rdi
callq 0x21b0
jmp 0x36b6
leaq -0x3f9(%rbp), %rdi
movq %rdi, -0x690(%rbp)
callq 0x2200
movq -0x690(%rbp), %rdx
leaq 0x60bdc(%rip), %rsi     # 0x642b3
leaq -0x3f8(%rbp), %rdi
callq 0x21b0
jmp 0x36e5
leaq -0x421(%rbp), %rdi
movq %rdi, -0x698(%rbp)
callq 0x2200
movq -0x698(%rbp), %rdx
leaq 0x62d22(%rip), %rsi     # 0x66428
leaq -0x420(%rbp), %rdi
callq 0x21b0
jmp 0x3714
leaq -0x449(%rbp), %rdi
movq %rdi, -0x6a0(%rbp)
callq 0x2200
movq -0x6a0(%rbp), %rdx
leaq 0x60b7d(%rip), %rsi     # 0x642b2
leaq -0x448(%rbp), %rdi
callq 0x21b0
jmp 0x3743
leaq -0x471(%rbp), %rdi
movq %rdi, -0x6a8(%rbp)
callq 0x2200
movq -0x6a8(%rbp), %rdx
leaq 0x60b4e(%rip), %rsi     # 0x642b2
leaq -0x470(%rbp), %rdi
callq 0x21b0
jmp 0x3772
movq %rsp, %rax
leaq -0x470(%rbp), %rcx
movq %rcx, 0x10(%rax)
leaq -0x448(%rbp), %rcx
movq %rcx, 0x8(%rax)
leaq -0x420(%rbp), %rcx
movq %rcx, (%rax)
movl $0x20, 0x18(%rax)
leaq -0x388(%rbp), %rdi
movl $0x4, %esi
xorl %edx, %edx
leaq -0x3a8(%rbp), %rcx
leaq -0x3d0(%rbp), %r8
leaq -0x3f8(%rbp), %r9
callq 0x47e0
jmp 0x37c6
leaq -0x470(%rbp), %rdi
callq 0x2090
leaq -0x471(%rbp), %rdi
callq 0x2160
leaq -0x448(%rbp), %rdi
callq 0x2090
leaq -0x449(%rbp), %rdi
callq 0x2160
leaq -0x420(%rbp), %rdi
callq 0x2090
leaq -0x421(%rbp), %rdi
callq 0x2160
leaq -0x3f8(%rbp), %rdi
callq 0x2090
leaq -0x3f9(%rbp), %rdi
callq 0x2160
leaq -0x3d0(%rbp), %rdi
callq 0x2090
leaq -0x3d1(%rbp), %rdi
callq 0x2160
leaq -0x3a8(%rbp), %rdi
callq 0x2090
leaq -0x3a9(%rbp), %rdi
callq 0x2160
movq 0xc1773(%rip), %rdi     # 0xc4fd0
leaq 0x60a51(%rip), %rsi     # 0x642b5
callq 0x2140
movq %rax, -0x6b0(%rbp)
jmp 0x3872
leaq -0x570(%rbp), %rdi
leaq -0xa8(%rbp), %rsi
leaq -0x388(%rbp), %rdx
callq 0x4aa0
jmp 0x388e
movq -0x6b0(%rbp), %rdi
leaq -0x570(%rbp), %rsi
callq 0x4a60
movq %rax, -0x6b8(%rbp)
jmp 0x38aa
movq -0x6b8(%rbp), %rdi
movq 0xc16f0(%rip), %rsi     # 0xc4fa8
callq 0x2150
jmp 0x38bf
leaq -0x570(%rbp), %rdi
callq 0x4af0
movl $0x0, -0x4(%rbp)
movl $0x1, -0x294(%rbp)
leaq -0x388(%rbp), %rdi
callq 0x4b10
jmp 0x3a1c
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x40(%rbp)
movl %eax, -0x44(%rbp)
jmp 0x39da
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x40(%rbp)
movl %eax, -0x44(%rbp)
jmp 0x39c2
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x40(%rbp)
movl %eax, -0x44(%rbp)
jmp 0x39aa
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x40(%rbp)
movl %eax, -0x44(%rbp)
jmp 0x3992
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x40(%rbp)
movl %eax, -0x44(%rbp)
jmp 0x397a
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x40(%rbp)
movl %eax, -0x44(%rbp)
jmp 0x3962
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x40(%rbp)
movl %eax, -0x44(%rbp)
leaq -0x470(%rbp), %rdi
callq 0x2090
leaq -0x471(%rbp), %rdi
callq 0x2160
leaq -0x448(%rbp), %rdi
callq 0x2090
leaq -0x449(%rbp), %rdi
callq 0x2160
leaq -0x420(%rbp), %rdi
callq 0x2090
leaq -0x421(%rbp), %rdi
callq 0x2160
leaq -0x3f8(%rbp), %rdi
callq 0x2090
leaq -0x3f9(%rbp), %rdi
callq 0x2160
leaq -0x3d0(%rbp), %rdi
callq 0x2090
leaq -0x3d1(%rbp), %rdi
callq 0x2160
leaq -0x3a8(%rbp), %rdi
callq 0x2090
leaq -0x3a9(%rbp), %rdi
callq 0x2160
jmp 0x3a64
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x40(%rbp)
movl %eax, -0x44(%rbp)
jmp 0x3a0e
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x40(%rbp)
movl %eax, -0x44(%rbp)
leaq -0x570(%rbp), %rdi
callq 0x4af0
leaq -0x388(%rbp), %rdi
callq 0x4b10
jmp 0x3a64
leaq -0xa8(%rbp), %rdi
callq 0x43e0
leaq -0x90(%rbp), %rdi
callq 0x43e0
leaq -0x78(%rbp), %rdi
callq 0x43e0
leaq -0x60(%rbp), %rdi
callq 0x43e0
leaq -0x38(%rbp), %rdi
callq 0x43e0
leaq -0x20(%rbp), %rdi
callq 0x43e0
movl -0x4(%rbp), %eax
addq $0x6e0, %rsp            # imm = 0x6E0
popq %rbp
retq
leaq -0xa8(%rbp), %rdi
callq 0x43e0
leaq -0x90(%rbp), %rdi
callq 0x43e0
leaq -0x78(%rbp), %rdi
callq 0x43e0
leaq -0x60(%rbp), %rdi
callq 0x43e0
leaq -0x38(%rbp), %rdi
callq 0x43e0
leaq -0x20(%rbp), %rdi
callq 0x43e0
movq -0x40(%rbp), %rdi
callq 0x21f0
nopl (%rax)