// File: /workspace/github_cpp_makefile_star2_O1/Kei18[P]pibt/./src/solver/pibt.cpp
// Function: PIBT::chooseNode(Agent*, std::vector<Node*, std::allocator<Node*>>)
Node* PIBT::chooseNode(Agent* a, Nodes C) {
  if (C.empty()) {
    std::cout << "error@PIBT::chooseNode, C is empty" << "\n";
    std::exit(1);
  }

  // randomize
  std::shuffle(C.begin(), C.end(), *MT);

  if (!a->hasGoal()) {
    if (inArray(a->getNode(), C)) {  // try to stay
      return a->getNode();
    } else {
      return C[0];  // random walk
    }
  }

  Nodes cs;
  int minCost = 10000;
  int cost;
  Node* g = a->getGoal();

  // fast implementation
  Nodes p = G->getPath(a->getNode(), g);
  if (p.size() > 1 && inArray(p[1], C)) return p[1];

  for (auto v : C) {
    cost = pathDist(v, g);
    if (cost < minCost) {
      minCost = cost;
      cs.clear();
      cs.push_back(v);
    } else if (cost == minCost) {
      cs.push_back(v);
    }
  }

  if (cs.size() == 1) return cs[0];

  // tie break
  bool contained;
  for (auto v : cs) {  // avoid tabu list
    contained = std::any_of(A.begin(), A.end(),
                            [v](Agent* b){ return b->getNode() == v; });
    if (!contained) return v;
  }

  return cs[0];
}

// Assembly:
pushq %rbp
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0x38, %rsp
movq %rsi, %r12
movq %rdi, %rbx
movq (%rdx), %rdi
movq 0x8(%rdx), %rsi
cmpq %rsi, %rdi
je 0x387dd
movq %rdx, %r15
movq 0x18(%rbx), %rdx
callq 0x1b60e
movq 0x10(%r12), %r14
testq %r14, %r14
je 0x3876a
xorps %xmm0, %xmm0
movaps %xmm0, (%rsp)
movq $0x0, 0x10(%rsp)
movq 0x38(%rbx), %rsi
movq 0x8(%r12), %rdx
movq (%rsi), %rax
leaq 0x20(%rsp), %rdi
movq %r14, %rcx
callq *0x18(%rax)
movq 0x20(%rsp), %rax
movq 0x28(%rsp), %rcx
subq %rax, %rcx
cmpq $0x9, %rcx
jb 0x385ac
movq 0x8(%rax), %rax
leaq 0x18(%rsp), %rdx
movq %rax, (%rdx)
movq (%r15), %rdi
movq 0x8(%r15), %rsi
callq 0x19ae8
cmpq 0x8(%r15), %rax
je 0x385ac
movq 0x20(%rsp), %rax
addq $0x8, %rax
jmp 0x3879b
movq (%r15), %r13
movq 0x8(%r15), %r15
cmpq %r15, %r13
je 0x38658
movl $0x2710, %r12d          # imm = 0x2710
movq (%r13), %rsi
movq %rsi, 0x18(%rsp)
movq %rbx, %rdi
movq %r14, %rdx
callq 0x3f248
movl %eax, %ebp
cmpl %r12d, %eax
jge 0x38603
movq (%rsp), %rax
cmpq %rax, 0x8(%rsp)
je 0x385ed
movq %rax, 0x8(%rsp)
movq 0x8(%rsp), %rsi
cmpq 0x10(%rsp), %rsi
je 0x38629
movq 0x18(%rsp), %rax
movq %rax, (%rsi)
jmp 0x3861c
jne 0x38624
movq 0x8(%rsp), %rsi
cmpq 0x10(%rsp), %rsi
je 0x38638
movq 0x18(%rsp), %rax
movq %rax, (%rsi)
movl %r12d, %ebp
addq $0x8, 0x8(%rsp)
jmp 0x38648
movl %r12d, %ebp
jmp 0x38648
movq %rsp, %rdi
leaq 0x18(%rsp), %rdx
callq 0x1c69a
jmp 0x38648
movq %rsp, %rdi
leaq 0x18(%rsp), %rdx
callq 0x1c69a
movl %r12d, %ebp
addq $0x8, %r13
movl %ebp, %r12d
cmpq %r15, %r13
jne 0x385c2
movq (%rsp), %rax
movq 0x8(%rsp), %rcx
movq %rcx, %rdx
subq %rax, %rdx
cmpq $0x8, %rdx
je 0x3879b
cmpq %rcx, %rax
sete %dl
je 0x38796
movq 0x20(%rbx), %rsi
movq 0x28(%rbx), %rdi
movq %rdi, %r8
subq %rsi, %r8
movq %r8, %r9
sarq $0x5, %r9
andq $-0x20, %r8
addq %rsi, %r8
leaq 0x1(%r9), %r10
leaq 0x10(%rsi), %r11
movq %rax, %r14
movq (%r14), %r15
movq %rsi, %r13
testq %r9, %r9
jle 0x386f0
movq %r11, %r12
movq %r10, %r13
movq -0x10(%r12), %rbp
cmpq %r15, 0x8(%rbp)
je 0x3873f
movq -0x8(%r12), %rbp
cmpq %r15, 0x8(%rbp)
je 0x38745
movq (%r12), %rbp
cmpq %r15, 0x8(%rbp)
je 0x3874f
movq 0x8(%r12), %rbp
cmpq %r15, 0x8(%rbp)
je 0x3874b
decq %r13
addq $0x20, %r12
cmpq $0x1, %r13
jg 0x386b5
movq %r8, %r13
movq %rdi, %rbp
subq %r13, %rbp
sarq $0x3, %rbp
cmpq $0x1, %rbp
je 0x3872d
cmpq $0x2, %rbp
je 0x3871e
movq %rdi, %r12
cmpq $0x3, %rbp
jne 0x3874f
movq (%r13), %r12
cmpq %r15, 0x8(%r12)
je 0x3873a
addq $0x8, %r13
movq (%r13), %r12
cmpq %r15, 0x8(%r12)
je 0x3873a
addq $0x8, %r13
movq (%r13), %r12
cmpq %r15, 0x8(%r12)
cmovneq %rdi, %r13
movq %r13, %r12
jmp 0x3874f
addq $-0x10, %r12
jmp 0x3874f
addq $-0x8, %r12
jmp 0x3874f
addq $0x8, %r12
cmpq %rdi, %r12
cmoveq %r15, %rbx
je 0x38796
addq $0x8, %r14
cmpq %rcx, %r14
sete %dl
jne 0x386a4
jmp 0x38796
movq 0x8(%r12), %rax
movq %rsp, %rdx
movq %rax, (%rdx)
movq (%r15), %rdi
movq 0x8(%r15), %rsi
callq 0x19ae8
cmpq 0x8(%r15), %rax
je 0x3878e
movq 0x8(%r12), %rbx
jmp 0x387cb
movq (%r15), %rax
movq (%rax), %rbx
jmp 0x387cb
testb $0x1, %dl
je 0x3879e
movq (%rax), %rbx
movq 0x20(%rsp), %rdi
testq %rdi, %rdi
je 0x387b5
movq 0x30(%rsp), %rsi
subq %rdi, %rsi
callq 0x6380
movq (%rsp), %rdi
testq %rdi, %rdi
je 0x387cb
movq 0x10(%rsp), %rsi
subq %rdi, %rsi
callq 0x6380
movq %rbx, %rax
addq $0x38, %rsp
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
movq 0x297c4(%rip), %rdi     # 0x61fa8
leaq 0x12bc3(%rip), %rsi     # 0x4b3ae
callq 0x6360
leaq 0xfd6c(%rip), %rsi      # 0x48563
movq %rax, %rdi
callq 0x6360
movl $0x1, %edi
callq 0x6440
jmp 0x38810
movq %rax, %rbx
jmp 0x3882a
movq %rax, %rbx
movq 0x20(%rsp), %rdi
testq %rdi, %rdi
je 0x3882a
movq 0x30(%rsp), %rsi
subq %rdi, %rsi
callq 0x6380
movq (%rsp), %rdi
testq %rdi, %rdi
je 0x38840
movq 0x10(%rsp), %rsi
subq %rdi, %rsi
callq 0x6380
movq %rbx, %rdi
callq 0x6620