// File: /workspace/github_cpp_makefile_star2_O2/Kei18[P]pibt/./src/solver/cbs.cpp
// Function: CBS::invoke(CTNode*, std::vector<Agent*, std::allocator<Agent*>>&)
void CBS::invoke(CTNode* node, Agents& block) {
  // calc path
  if (node->c.empty()) {  // initail
    Paths paths;
    for (int i = 0; i < A.size(); ++i) paths.push_back({});
    for (auto a : block) {
      auto itr = std::find_if(A.begin(), A.end(),
                              [a](Agent* b) { return a == b; });
      paths[std::distance(A.begin(), itr)] = AstarSearch(a, node);
    }
    node->paths = paths;
  } else {
    Agent* a;
    for (auto c : node->c) {
      a = c->a;
      auto itr = std::find_if(A.begin(), A.end(),
                              [a](Agent* b) { return a == b; });
      node->paths[std::distance(A.begin(), itr)].clear();
    }
    for (auto c : node->c) {
      a = c->a;
      auto itr = std::find_if(A.begin(), A.end(),
                              [a](Agent* b) { return a == b; });
      node->paths[std::distance(A.begin(), itr)] = AstarSearch(a, node);
    }
  }
  if (!node->valid) return;
  calcCost(node, block);
  formalizePathAgents(node, block);
}

// Assembly:
pushq %rbp
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0x38, %rsp
movq %rsi, %r14
movq %rdi, %r15
movq (%rsi), %rax
movq 0x8(%rsi), %rcx
cmpq %rcx, %rax
movq %rdx, 0x18(%rsp)
je 0x205b9
cmpq %rcx, %rax
je 0x204e5
movq (%rax), %rdx
movq (%rdx), %r8
movq 0x20(%r15), %rdx
movq 0x28(%r15), %rsi
movq %rsi, %r9
subq %rdx, %r9
movq %r9, %r10
sarq $0x5, %r10
movq %rdx, %rdi
testq %r10, %r10
jle 0x20477
cmpq %r8, (%rdi)
je 0x204b8
cmpq %r8, 0x8(%rdi)
je 0x204a8
cmpq %r8, 0x10(%rdi)
je 0x204ae
cmpq %r8, 0x18(%rdi)
je 0x204b4
addq $0x20, %rdi
decq %r10
addq $-0x20, %r9
jmp 0x2044e
sarq $0x3, %r9
cmpq $0x1, %r9
je 0x2049f
cmpq $0x2, %r9
je 0x20496
cmpq $0x3, %r9
jne 0x204bb
cmpq %r8, (%rdi)
je 0x204b8
addq $0x8, %rdi
cmpq %r8, (%rdi)
je 0x204b8
addq $0x8, %rdi
cmpq %r8, (%rdi)
cmoveq %rdi, %rsi
jmp 0x204bb
addq $0x8, %rdi
jmp 0x204b8
addq $0x10, %rdi
jmp 0x204b8
addq $0x18, %rdi
movq %rdi, %rsi
subq %rdx, %rsi
shrq $0x3, %rsi
movq 0x18(%r14), %rdx
imulq $0x18, %rsi, %rdi
movq (%rdx,%rdi), %rsi
cmpq %rsi, 0x8(%rdx,%rdi)
je 0x204dc
addq %rdi, %rdx
movq %rsi, 0x8(%rdx)
addq $0x8, %rax
jmp 0x20427
movq (%r14), %r13
movq 0x8(%r14), %rbp
movq %rsp, %r12
cmpq %rbp, %r13
je 0x206f3
movq (%r13), %rax
movq (%rax), %rdx
movq 0x20(%r15), %rax
movq 0x28(%r15), %rbx
movq %rbx, %rcx
subq %rax, %rcx
movq %rcx, %rsi
sarq $0x5, %rsi
testq %rsi, %rsi
jle 0x2053d
cmpq %rdx, (%rax)
je 0x2057e
cmpq %rdx, 0x8(%rax)
je 0x2056e
cmpq %rdx, 0x10(%rax)
je 0x20574
cmpq %rdx, 0x18(%rax)
je 0x2057a
addq $0x20, %rax
decq %rsi
addq $-0x20, %rcx
jmp 0x20514
sarq $0x3, %rcx
cmpq $0x1, %rcx
je 0x20565
cmpq $0x2, %rcx
je 0x2055c
cmpq $0x3, %rcx
jne 0x20581
cmpq %rdx, (%rax)
je 0x2057e
addq $0x8, %rax
cmpq %rdx, (%rax)
je 0x2057e
addq $0x8, %rax
cmpq %rdx, (%rax)
cmoveq %rax, %rbx
jmp 0x20581
addq $0x8, %rax
jmp 0x2057e
addq $0x10, %rax
jmp 0x2057e
addq $0x18, %rax
movq %rax, %rbx
movq (%r15), %rax
movq %r12, %rdi
movq %r15, %rsi
movq %r14, %rcx
callq *0x28(%rax)
subq 0x20(%r15), %rbx
shrq $0x3, %rbx
imulq $0x18, %rbx, %rdi
addq 0x18(%r14), %rdi
movq %r12, %rsi
callq 0x18466
movq %r12, %rdi
callq 0x990e
addq $0x8, %r13
jmp 0x204ef
movq %rdx, %rbp
movq %rsp, %r12
andq $0x0, 0x10(%r12)
xorps %xmm0, %xmm0
movaps %xmm0, (%r12)
xorl %ebx, %ebx
leaq 0x20(%rsp), %r13
movq 0x28(%r15), %rax
subq 0x20(%r15), %rax
sarq $0x3, %rax
cmpq %rbx, %rax
jbe 0x2060b
xorps %xmm0, %xmm0
movaps %xmm0, 0x20(%rsp)
andq $0x0, 0x30(%rsp)
movq %r12, %rdi
movq %r13, %rsi
callq 0x15146
movq %r13, %rdi
callq 0x990e
incq %rbx
jmp 0x205d4
movq (%rbp), %r13
movq 0x8(%rbp), %rbp
leaq 0x20(%rsp), %r12
cmpq %rbp, %r13
je 0x206df
movq (%r13), %rdx
movq 0x20(%r15), %rax
movq 0x28(%r15), %rbx
movq %rbx, %rcx
subq %rax, %rcx
movq %rcx, %rsi
sarq $0x5, %rsi
testq %rsi, %rsi
jle 0x20663
cmpq %rdx, (%rax)
je 0x206a4
cmpq %rdx, 0x8(%rax)
je 0x20694
cmpq %rdx, 0x10(%rax)
je 0x2069a
cmpq %rdx, 0x18(%rax)
je 0x206a0
addq $0x20, %rax
decq %rsi
addq $-0x20, %rcx
jmp 0x2063a
sarq $0x3, %rcx
cmpq $0x1, %rcx
je 0x2068b
cmpq $0x2, %rcx
je 0x20682
cmpq $0x3, %rcx
jne 0x206a7
cmpq %rdx, (%rax)
je 0x206a4
addq $0x8, %rax
cmpq %rdx, (%rax)
je 0x206a4
addq $0x8, %rax
cmpq %rdx, (%rax)
cmoveq %rax, %rbx
jmp 0x206a7
addq $0x8, %rax
jmp 0x206a4
addq $0x10, %rax
jmp 0x206a4
addq $0x18, %rax
movq %rax, %rbx
movq (%r15), %rax
movq %r12, %rdi
movq %r15, %rsi
movq %r14, %rcx
callq *0x28(%rax)
subq 0x20(%r15), %rbx
shrq $0x3, %rbx
imulq $0x18, %rbx, %rdi
addq (%rsp), %rdi
movq %r12, %rsi
callq 0x18466
movq %r12, %rdi
callq 0x990e
addq $0x8, %r13
jmp 0x20618
leaq 0x18(%r14), %rdi
movq %rsp, %rsi
callq 0x22858
movq %rsp, %rdi
callq 0x92ec
cmpb $0x1, 0x40(%r14)
jne 0x2071b
movq %r15, %rdi
movq %r14, %rsi
movq 0x18(%rsp), %rbx
movq %rbx, %rdx
callq 0x2074e
movq %r15, %rdi
movq %r14, %rsi
movq %rbx, %rdx
callq 0x20880
addq $0x38, %rsp
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
jmp 0x2072c
movq %rax, %rbx
jmp 0x2073e
movq %rax, %rbx
leaq 0x20(%rsp), %rdi
callq 0x990e
movq %rsp, %rdi
callq 0x92ec
movq %rbx, %rdi
callq 0x6650