// File: /workspace/github_cpp_makefile_star2_O3/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, %r15
movq %rdi, %rbx
movq (%rdx), %rdi
movq 0x8(%rdx), %rsi
cmpq %rsi, %rdi
je 0x3855f
movq %rdx, %r12
movq 0x18(%rbx), %rdx
callq 0x1b62f
movq 0x10(%r15), %r14
testq %r14, %r14
je 0x38521
xorps %xmm0, %xmm0
movaps %xmm0, (%rsp)
movq $0x0, 0x10(%rsp)
movq 0x38(%rbx), %rsi
movq 0x8(%r15), %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
movq 0x8(%r12), %r15
cmpq $0x9, %rcx
jb 0x3833d
movq 0x8(%rax), %rax
leaq 0x18(%rsp), %rdx
movq %rax, (%rdx)
movq (%r12), %rdi
movq %r15, %rsi
callq 0x19b2e
movq %rax, %r15
cmpq 0x8(%r12), %rax
je 0x3833d
movq 0x20(%rsp), %rax
movq 0x8(%rax), %rbx
jmp 0x384f9
movq (%r12), %r12
cmpq %r15, %r12
je 0x383e7
movl $0x2710, %r13d          # imm = 0x2710
movq (%r12), %rsi
movq %rsi, 0x18(%rsp)
movq %rbx, %rdi
movq %r14, %rdx
callq 0x3efbe
movl %eax, %ebp
cmpl %r13d, %eax
jge 0x38392
movq (%rsp), %rax
movq 0x8(%rsp), %rsi
cmpq %rax, %rsi
je 0x38381
movq %rax, 0x8(%rsp)
movq %rax, %rsi
cmpq 0x10(%rsp), %rsi
je 0x383b8
movq 0x18(%rsp), %rax
movq %rax, (%rsi)
jmp 0x383ab
jne 0x383b3
movq 0x8(%rsp), %rsi
cmpq 0x10(%rsp), %rsi
je 0x383c7
movq 0x18(%rsp), %rax
movq %rax, (%rsi)
movl %r13d, %ebp
addq $0x8, 0x8(%rsp)
jmp 0x383d7
movl %r13d, %ebp
jmp 0x383d7
movq %rsp, %rdi
leaq 0x18(%rsp), %rdx
callq 0x1c5cc
jmp 0x383d7
movq %rsp, %rdi
leaq 0x18(%rsp), %rdx
callq 0x1c5cc
movl %r13d, %ebp
addq $0x8, %r12
movl %ebp, %r13d
cmpq %r15, %r12
jne 0x38350
movq (%rsp), %rdi
movq 0x8(%rsp), %rax
movq %rax, %rcx
subq %rdi, %rcx
sete %dl
cmpq $0x8, %rcx
sete %cl
orb %dl, %cl
jne 0x384ec
movq 0x20(%rbx), %rcx
movq 0x28(%rbx), %rdx
movq %rdx, %rsi
subq %rcx, %rsi
movq %rsi, %r8
sarq $0x5, %r8
movq %rsi, %r9
andq $-0x20, %r9
addq %rcx, %r9
movq %rdx, %r10
subq %r9, %r10
leaq 0x1(%r8), %r11
leaq 0x10(%rcx), %r14
movq %rdi, %r15
movq (%r15), %rbx
movq %rsi, %r13
movq %rcx, %r12
testq %r8, %r8
jle 0x3848a
movq %r14, %r12
movq %r11, %r13
movq -0x10(%r12), %rbp
cmpq %rbx, 0x8(%rbp)
je 0x384ca
movq -0x8(%r12), %rbp
cmpq %rbx, 0x8(%rbp)
je 0x384d0
movq (%r12), %rbp
cmpq %rbx, 0x8(%rbp)
je 0x384da
movq 0x8(%r12), %rbp
cmpq %rbx, 0x8(%rbp)
je 0x384d6
decq %r13
addq $0x20, %r12
cmpq $0x1, %r13
jg 0x3844c
movq %r10, %r13
movq %r9, %r12
sarq $0x3, %r13
cmpq $0x1, %r13
je 0x384bc
cmpq $0x2, %r13
je 0x384ae
cmpq $0x3, %r13
jne 0x384ef
movq (%r12), %r13
cmpq %rbx, 0x8(%r13)
je 0x384da
addq $0x8, %r12
movq (%r12), %r13
cmpq %rbx, 0x8(%r13)
je 0x384da
addq $0x8, %r12
movq (%r12), %r13
cmpq %rbx, 0x8(%r13)
cmovneq %rdx, %r12
jmp 0x384da
addq $-0x10, %r12
jmp 0x384da
addq $-0x8, %r12
jmp 0x384da
addq $0x8, %r12
cmpq %rdx, %r12
je 0x384ef
addq $0x8, %r15
cmpq %rax, %r15
jne 0x38438
movq (%rdi), %rbx
movq 0x20(%rsp), %rax
testq %rax, %rax
je 0x3850d
movq 0x30(%rsp), %rsi
subq %rax, %rsi
movq %rax, %rdi
callq 0x6380
movq (%rsp), %rdi
testq %rdi, %rdi
je 0x3854d
movq 0x10(%rsp), %rsi
subq %rdi, %rsi
callq 0x6380
jmp 0x3854d
movq 0x8(%r15), %rax
movq %rsp, %rdx
movq %rax, (%rdx)
movq (%r12), %rdi
movq 0x8(%r12), %rsi
callq 0x19b2e
cmpq 0x8(%r12), %rax
je 0x38546
movq 0x8(%r15), %rbx
jmp 0x3854d
movq (%r12), %rax
movq (%rax), %rbx
movq %rbx, %rax
addq $0x38, %rsp
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
movq 0x29a42(%rip), %rdi     # 0x61fa8
leaq 0x12eb8(%rip), %rsi     # 0x4b425
callq 0x6360
leaq 0xffea(%rip), %rsi      # 0x48563
movq %rax, %rdi
callq 0x6360
movl $0x1, %edi
callq 0x6440
jmp 0x38592
movq %rax, %rbx
jmp 0x385ac
movq %rax, %rbx
movq 0x20(%rsp), %rdi
testq %rdi, %rdi
je 0x385ac
movq 0x30(%rsp), %rsi
subq %rdi, %rsi
callq 0x6380
movq (%rsp), %rdi
testq %rdi, %rdi
je 0x385c2
movq 0x10(%rsp), %rsi
subq %rdi, %rsi
callq 0x6380
movq %rbx, %rdi
callq 0x6620