// File: /workspace/github_cpp_makefile_star2_O2/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 0x2d7d3
movq %rdx, %r15
movq 0x18(%rbx), %rdx
callq 0x17270
movq 0x10(%r12), %r14
testq %r14, %r14
je 0x2d6d7
xorps %xmm0, %xmm0
andq $0x0, 0x10(%rsp)
movaps %xmm0, (%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 0x2d644
movq 0x8(%rax), %rdi
movq %r15, %rsi
callq 0x2d58b
testb %al, %al
je 0x2d644
movq 0x20(%rsp), %rax
addq $0x8, %rax
jmp 0x2d6bd
movq (%r15), %r13
movq 0x8(%r15), %r15
movl $0x2710, %r12d          # imm = 0x2710
cmpq %r15, %r13
je 0x2d6a8
movq (%r13), %rsi
movq %rsi, 0x18(%rsp)
movq %rbx, %rdi
movq %r14, %rdx
callq 0x32cd4
movl %eax, %ebp
cmpl %r12d, %eax
jge 0x2d693
movq (%rsp), %rax
cmpq %rax, 0x8(%rsp)
je 0x2d681
movq %rax, 0x8(%rsp)
movq %rsp, %rdi
leaq 0x18(%rsp), %rsi
callq 0x171da
movl %ebp, %r12d
jmp 0x2d6a2
jne 0x2d6a2
movq %rsp, %rdi
leaq 0x18(%rsp), %rsi
callq 0x171da
addq $0x8, %r13
jmp 0x2d651
movq (%rsp), %rax
movq 0x8(%rsp), %rcx
movq %rcx, %rdx
subq %rax, %rdx
cmpq $0x8, %rdx
jne 0x2d6f6
movq (%rax), %rbx
leaq 0x20(%rsp), %rdi
callq 0x990e
movq %rsp, %rdi
callq 0x990e
jmp 0x2d7c1
movq 0x8(%r12), %rdi
movq %r15, %rsi
callq 0x2d58b
testb %al, %al
je 0x2d7bb
movq 0x8(%r12), %rbx
jmp 0x2d7c1
movq 0x20(%rbx), %rdx
movq 0x28(%rbx), %rsi
movq %rsi, %rdi
subq %rdx, %rdi
movq %rdi, %r8
sarq $0x5, %r8
movq %rax, %r9
cmpq %rcx, %r9
je 0x2d6bd
movq (%r9), %rbx
movq %rdi, %r11
movq %rdx, %r10
movq %r8, %r14
testq %r14, %r14
jle 0x2d758
movq (%r10), %r15
cmpq %rbx, 0x8(%r15)
je 0x2d7a9
movq 0x8(%r10), %r15
cmpq %rbx, 0x8(%r15)
je 0x2d799
movq 0x10(%r10), %r15
cmpq %rbx, 0x8(%r15)
je 0x2d79f
movq 0x18(%r10), %r15
cmpq %rbx, 0x8(%r15)
je 0x2d7a5
addq $0x20, %r10
decq %r14
addq $-0x20, %r11
jmp 0x2d71f
sarq $0x3, %r11
cmpq $0x1, %r11
je 0x2d78c
cmpq $0x2, %r11
je 0x2d77f
cmpq $0x3, %r11
jne 0x2d6c0
movq (%r10), %r11
cmpq %rbx, 0x8(%r11)
je 0x2d7a9
addq $0x8, %r10
movq (%r10), %r11
cmpq %rbx, 0x8(%r11)
je 0x2d7a9
addq $0x8, %r10
movq (%r10), %r11
cmpq %rbx, 0x8(%r11)
cmovneq %rsi, %r10
jmp 0x2d7a9
addq $0x8, %r10
jmp 0x2d7a9
addq $0x10, %r10
jmp 0x2d7a9
addq $0x18, %r10
addq $0x8, %r9
cmpq %rsi, %r10
jne 0x2d70e
jmp 0x2d6c0
movq (%r15), %rax
movq (%rax), %rbx
movq %rbx, %rax
addq $0x38, %rsp
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
movq 0x2c7be(%rip), %rdi     # 0x59f98
leaq 0xfda5(%rip), %rsi      # 0x3d586
callq 0x63a0
leaq 0xcd76(%rip), %rsi      # 0x3a563
movq %rax, %rdi
callq 0x63a0
pushq $0x1
popq %rdi
callq 0x6450
jmp 0x2d804
movq %rax, %rbx
jmp 0x2d811
movq %rax, %rbx
leaq 0x20(%rsp), %rdi
callq 0x990e
movq %rsp, %rdi
callq 0x990e
movq %rbx, %rdi
callq 0x6650
nop