// File: /workspace/github_cpp_makefile_star2_O3/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 $0x48, %rsp
movq %rsi, %r14
movq %rdi, %r15
movq (%rsi), %rax
movq 0x8(%rsi), %rcx
cmpq %rcx, %rax
movq %rdx, 0x40(%rsp)
je 0x278b8
movq 0x18(%r14), %rdx
movq (%rax), %rsi
movq (%rsi), %r8
movq 0x20(%r15), %rsi
movq 0x28(%r15), %rdi
movq %rdi, %r10
subq %rsi, %r10
movq %r10, %r11
sarq $0x5, %r11
testq %r11, %r11
jle 0x27700
andq $-0x20, %r10
movq %r10, %r9
addq %rsi, %r9
incq %r11
leaq 0x10(%rsi), %r10
cmpq %r8, -0x10(%r10)
je 0x27737
cmpq %r8, -0x8(%r10)
je 0x2773d
cmpq %r8, (%r10)
je 0x2774c
cmpq %r8, 0x8(%r10)
je 0x27748
decq %r11
addq $0x20, %r10
cmpq $0x1, %r11
jg 0x276d4
movq %rdi, %r10
subq %r9, %r10
jmp 0x27703
movq %rsi, %r9
sarq $0x3, %r10
cmpq $0x1, %r10
je 0x2772b
cmpq $0x2, %r10
je 0x27722
cmpq $0x3, %r10
jne 0x27732
cmpq %r8, (%r9)
je 0x27743
addq $0x8, %r9
cmpq %r8, (%r9)
je 0x27743
addq $0x8, %r9
cmpq %r8, (%r9)
cmoveq %r9, %rdi
movq %rdi, %r10
jmp 0x2774c
addq $-0x10, %r10
jmp 0x2774c
addq $-0x8, %r10
jmp 0x2774c
movq %r9, %r10
jmp 0x2774c
addq $0x8, %r10
subq %rsi, %r10
shrq $0x3, %r10
leaq (%r10,%r10,2), %rdi
movq (%rdx,%rdi,8), %rsi
cmpq %rsi, 0x8(%rdx,%rdi,8)
je 0x2776a
leaq (%rdx,%rdi,8), %rdi
movq %rsi, 0x8(%rdi)
addq $0x8, %rax
cmpq %rcx, %rax
jne 0x276a3
movq (%r14), %r13
movq 0x8(%r14), %rbp
cmpq %rbp, %r13
je 0x27a73
leaq 0x20(%rsp), %r12
movq (%r13), %rax
movq (%rax), %rdx
movq 0x20(%r15), %rbx
movq 0x28(%r15), %rax
movq %rax, %rdi
subq %rbx, %rdi
movq %rdi, %rsi
sarq $0x5, %rsi
testq %rsi, %rsi
jle 0x277ea
andq $-0x20, %rdi
movq %rdi, %rcx
addq %rbx, %rcx
incq %rsi
addq $0x10, %rbx
cmpq %rdx, -0x10(%rbx)
je 0x27821
cmpq %rdx, -0x8(%rbx)
je 0x27827
cmpq %rdx, (%rbx)
je 0x27836
cmpq %rdx, 0x8(%rbx)
je 0x27832
decq %rsi
addq $0x20, %rbx
cmpq $0x1, %rsi
jg 0x277be
movq %rax, %rdi
subq %rcx, %rdi
jmp 0x277ed
movq %rbx, %rcx
sarq $0x3, %rdi
cmpq $0x1, %rdi
je 0x27815
cmpq $0x2, %rdi
je 0x2780c
cmpq $0x3, %rdi
jne 0x2781c
cmpq %rdx, (%rcx)
je 0x2782d
addq $0x8, %rcx
cmpq %rdx, (%rcx)
je 0x2782d
addq $0x8, %rcx
cmpq %rdx, (%rcx)
cmoveq %rcx, %rax
movq %rax, %rbx
jmp 0x27836
addq $-0x10, %rbx
jmp 0x27836
addq $-0x8, %rbx
jmp 0x27836
movq %rcx, %rbx
jmp 0x27836
addq $0x8, %rbx
movq (%r15), %rax
movq %r12, %rdi
movq %r15, %rsi
movq %r14, %rcx
callq *0x28(%rax)
subq 0x20(%r15), %rbx
shrq $0x3, %rbx
movq 0x18(%r14), %rax
leaq (%rbx,%rbx,2), %rcx
movq (%rax,%rcx,8), %rdi
movq 0x10(%rax,%rcx,8), %rsi
movaps 0x20(%rsp), %xmm0
movups %xmm0, (%rax,%rcx,8)
movq 0x30(%rsp), %rdx
movq %rdx, 0x10(%rax,%rcx,8)
xorps %xmm0, %xmm0
movaps %xmm0, 0x20(%rsp)
movq $0x0, 0x30(%rsp)
testq %rdi, %rdi
je 0x278a6
subq %rdi, %rsi
callq 0x6380
movq 0x20(%rsp), %rdi
testq %rdi, %rdi
je 0x278a6
movq 0x30(%rsp), %rsi
subq %rdi, %rsi
callq 0x6380
addq $0x8, %r13
cmpq %rbp, %r13
jne 0x2778c
jmp 0x27a73
movq %rdx, %rbp
xorps %xmm0, %xmm0
movaps %xmm0, 0x20(%rsp)
movq $0x0, 0x30(%rsp)
movq 0x28(%r15), %rax
cmpq 0x20(%r15), %rax
je 0x27925
xorl %ebx, %ebx
leaq 0x20(%rsp), %r12
movq %rsp, %r13
xorps %xmm0, %xmm0
movaps %xmm0, (%rsp)
movq $0x0, 0x10(%rsp)
movq %r12, %rdi
movq %r13, %rsi
callq 0x18c56
movq (%rsp), %rdi
testq %rdi, %rdi
je 0x27911
movq 0x10(%rsp), %rsi
subq %rdi, %rsi
callq 0x6380
incq %rbx
movq 0x28(%r15), %rax
subq 0x20(%r15), %rax
sarq $0x3, %rax
cmpq %rbx, %rax
ja 0x278e0
movq (%rbp), %r13
movq 0x8(%rbp), %rbp
cmpq %rbp, %r13
je 0x27a5b
movq %rsp, %r12
movq (%r13), %rdx
movq 0x20(%r15), %rbx
movq 0x28(%r15), %rax
movq %rax, %rdi
subq %rbx, %rdi
movq %rdi, %rsi
sarq $0x5, %rsi
testq %rsi, %rsi
jle 0x27994
andq $-0x20, %rdi
movq %rdi, %rcx
addq %rbx, %rcx
incq %rsi
addq $0x10, %rbx
cmpq %rdx, -0x10(%rbx)
je 0x279cb
cmpq %rdx, -0x8(%rbx)
je 0x279d1
cmpq %rdx, (%rbx)
je 0x279e0
cmpq %rdx, 0x8(%rbx)
je 0x279dc
decq %rsi
addq $0x20, %rbx
cmpq $0x1, %rsi
jg 0x27968
movq %rax, %rdi
subq %rcx, %rdi
jmp 0x27997
movq %rbx, %rcx
sarq $0x3, %rdi
cmpq $0x1, %rdi
je 0x279bf
cmpq $0x2, %rdi
je 0x279b6
cmpq $0x3, %rdi
jne 0x279c6
cmpq %rdx, (%rcx)
je 0x279d7
addq $0x8, %rcx
cmpq %rdx, (%rcx)
je 0x279d7
addq $0x8, %rcx
cmpq %rdx, (%rcx)
cmoveq %rcx, %rax
movq %rax, %rbx
jmp 0x279e0
addq $-0x10, %rbx
jmp 0x279e0
addq $-0x8, %rbx
jmp 0x279e0
movq %rcx, %rbx
jmp 0x279e0
addq $0x8, %rbx
movq (%r15), %rax
movq %r12, %rdi
movq %r15, %rsi
movq %r14, %rcx
callq *0x28(%rax)
subq 0x20(%r15), %rbx
shrq $0x3, %rbx
movq 0x20(%rsp), %rax
leaq (%rbx,%rbx,2), %rcx
movq (%rax,%rcx,8), %rdi
movq 0x10(%rax,%rcx,8), %rsi
movaps (%rsp), %xmm0
movups %xmm0, (%rax,%rcx,8)
movq 0x10(%rsp), %rdx
movq %rdx, 0x10(%rax,%rcx,8)
xorps %xmm0, %xmm0
movaps %xmm0, (%rsp)
movq $0x0, 0x10(%rsp)
testq %rdi, %rdi
je 0x27a4e
subq %rdi, %rsi
callq 0x6380
movq (%rsp), %rdi
testq %rdi, %rdi
je 0x27a4e
movq 0x10(%rsp), %rsi
subq %rdi, %rsi
callq 0x6380
addq $0x8, %r13
cmpq %rbp, %r13
jne 0x27939
leaq 0x18(%r14), %rdi
leaq 0x20(%rsp), %rsi
callq 0x2ab9c
leaq 0x20(%rsp), %rdi
callq 0xb508
cmpb $0x1, 0x40(%r14)
jne 0x27a9b
movq %r15, %rdi
movq %r14, %rsi
movq 0x40(%rsp), %rbx
movq %rbx, %rdx
callq 0x27adc
movq %r15, %rdi
movq %r14, %rsi
movq %rbx, %rdx
callq 0x27c5a
addq $0x48, %rsp
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
jmp 0x27aac
movq %rax, %rbx
jmp 0x27aca
movq %rax, %rbx
movq (%rsp), %rdi
testq %rdi, %rdi
je 0x27aca
movq 0x10(%rsp), %rsi
subq %rdi, %rsi
callq 0x6380
leaq 0x20(%rsp), %rdi
callq 0xb508
movq %rbx, %rdi
callq 0x6620