// File: /workspace/github_cpp_makefile_star2_O2/Kei18[P]pibt/./src/solver/tp.cpp
// Function: TP::getPath(Agent*, Node*, Node*, int, bool)
Nodes TP::getPath(Agent* a,
                  Node* s,
                  Node* g,
                  int startTime,
                  bool futureCollision)
{
  Nodes path;
  Nodes C;
  AN_OLD *l, *n;
  int t, f, cost;
  std::string key;
  bool prohibited, goalCheck;

  Nodes pathends;
  Node* v;
  for (auto p : paths) {
    v = p[p.size() - 1];
    if (v != s && v != a->getNode()) pathends.push_back(v);
  }

  std::unordered_set<std::string> OPEN;
  std::unordered_map<std::string, AN_OLD*> table;

  t = startTime - 1;
  l = new AN_OLD { s, true, startTime - 1, pathDist(s, g), nullptr };
  key = getKey(t, s);
  table.emplace(key, l);
  OPEN = { key };

  bool invalid = true;
  while (!OPEN.empty()) {
    // argmin
    auto itr1 = std::min_element(OPEN.begin(), OPEN.end(),
                                 [&table] (std::string a, std::string b)
                                 { auto eleA = table.at(a);
                                   auto eleB = table.at(b);
                                   if (eleA->f == eleB->f) {
                                     return eleA->t > eleB->t;
                                   }
                                   return eleA->f < eleB->f;
                                 });
    key = *itr1;
    n = table.at(key);
    t = n->t;

    // check goal
    if (n->v == g) {
      goalCheck = true;

      if (futureCollision) {
        for (auto p : paths) {
          if (t + 1 >= p.size()) continue;

          for (auto itr = p.begin() + t + 1; itr < p.end(); ++itr) {
            if ((*itr) == n->v) {  // collision in future
              goalCheck = false;
              break;
            }
          }
          if (!goalCheck) break;
        }
      }

      if (goalCheck) {
        invalid = false;
        break;
      }
    }

    // update list
    n->open = false;
    OPEN.erase(itr1);

    // search neighbor
    C = { n->v };
    for (auto u : G->neighbor(n->v)) {
      if (!inArray(u, pathends)) C.push_back(u);
    }

    for (auto m : C) {
      t = n->t + 1;

      if (t > 0) {
        prohibited = false;

        for (auto p : paths) {
          if (t + 1 > p.size()) continue;
          if (p[t] == m) { // collision
            prohibited = true;
            break;
          }
          if (p[t] == n->v && p[t-1] == m) {  // intersection
            prohibited = true;
            break;
          }
        }

        if (prohibited) continue;
      }

      key = getKey(t, m);
      auto itr2 = table.find(key);
      if (itr2 == table.end()) {
        table.emplace(key, new AN_OLD { m, true, t, 100000, n });
      }
      l = table.at(key);
      if (!l->open) continue;

      cost = pathDist(m, g);
      f = t + cost;

      if (l->f > f) {
        l->t = t;
        l->f = f;
        l->p = n;
      }

      OPEN.insert(key);
    }
  }

  // back tracking
  if (!invalid) {  // check failed or not
    while (n->p) {
      path.push_back(n->v);
      n = n->p;
    }
    path.push_back(s);
    std::reverse(path.begin(), path.end());
  }

  return path;
}

// Assembly:
pushq %rbp
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0x168, %rsp            # imm = 0x168
movl %r9d, 0x10(%rsp)
movq %r8, 0x48(%rsp)
movq %rcx, %r14
movq %rdx, %r15
movq %rcx, 0xb0(%rsp)
xorps %xmm0, %xmm0
andq $0x0, 0x10(%rdi)
andq $0x0, 0x90(%rsp)
leaq 0x70(%rsp), %rax
movq %rax, -0x10(%rax)
andq $0x0, -0x8(%rax)
movq %rdi, 0x38(%rsp)
movups %xmm0, (%rdi)
movaps %xmm0, 0x80(%rsp)
movb $0x0, (%rax)
leaq 0x110(%rsp), %r12
movaps %xmm0, (%r12)
andq $0x0, 0x10(%r12)
movq 0x88(%rsi), %r13
movq %rsi, 0x40(%rsp)
movq 0x90(%rsi), %rbx
leaq 0xb8(%rsp), %rbp
cmpq %rbx, %r13
je 0x34d6a
movq %rbp, %rdi
movq %r13, %rsi
callq 0x15d3a
movq 0xc0(%rsp), %rax
movq -0x8(%rax), %rax
movq %rax, 0xa8(%rsp)
cmpq %r14, %rax
je 0x34d5c
cmpq 0x8(%r15), %rax
je 0x34d5c
movq %r12, %rdi
leaq 0xa8(%rsp), %rsi
callq 0x171da
movq %rbp, %rdi
callq 0x990e
addq $0x18, %r13
jmp 0x34d1d
leaq 0xe8(%rsp), %rax
movq %rax, -0x30(%rax)
pushq $0x1
popq %rcx
movq %rcx, -0x28(%rax)
xorps %xmm0, %xmm0
movups %xmm0, -0x20(%rax)
movl $0x3f800000, %edx       # imm = 0x3F800000
movl %edx, -0x10(%rax)
movups %xmm0, -0x8(%rax)
leaq 0x160(%rsp), %rax
movq %rax, -0x30(%rax)
movq %rcx, -0x28(%rax)
movups %xmm0, -0x20(%rax)
movl %edx, -0x10(%rax)
movups %xmm0, -0x8(%rax)
pushq $0x20
popq %rdi
callq 0x63b0
movq %rax, %r15
movl 0x10(%rsp), %ebx
decl %ebx
movq %r14, (%rax)
movb $0x1, 0x8(%rax)
movl %ebx, 0xc(%rax)
movq 0x40(%rsp), %rdi
movq %r14, %rsi
movq 0x48(%rsp), %rdx
callq 0x32cd4
movl %eax, 0x10(%r15)
andq $0x0, 0x18(%r15)
movq %r15, 0x50(%rsp)
leaq 0x18(%rsp), %rdi
movl %ebx, %esi
movq %r14, %rdx
callq 0x32f4c
leaq 0x60(%rsp), %rbx
leaq 0x18(%rsp), %r14
movq %rbx, %rdi
movq %r14, %rsi
callq 0x6470
movq %r14, %rdi
callq 0x61d0
leaq 0x130(%rsp), %rdi
leaq 0x50(%rsp), %rdx
movq %rbx, %rsi
callq 0x358a0
leaq 0x18(%rsp), %rdi
leaq 0x60(%rsp), %rsi
callq 0x64e0
leaq 0xb8(%rsp), %rdi
leaq 0x18(%rsp), %rsi
pushq $0x1
popq %rdx
callq 0x35c92
leaq 0x18(%rsp), %rdi
callq 0x61d0
movq 0xd0(%rsp), %r14
testq %r14, %r14
je 0x35270
movq 0xc8(%rsp), %rbx
testq %rbx, %rbx
je 0x34f0e
movq %rbx, %r15
leaq 0x18(%rsp), %r13
movq (%rbx), %rbx
testq %rbx, %rbx
je 0x34f16
leaq 0x8(%rbx), %rsi
movq %r13, %rdi
callq 0x64e0
leaq 0x8(%r15), %rsi
leaq 0xf0(%rsp), %rdi
callq 0x64e0
leaq 0x130(%rsp), %rbp
movq %rbp, %rdi
movq %r13, %rsi
callq 0x35fda
movq (%rax), %r12
movq %rbp, %rdi
leaq 0xf0(%rsp), %rsi
callq 0x35fda
movq (%rax), %rax
movl 0x10(%rax), %ecx
cmpl %ecx, 0x10(%r12)
jne 0x34ee9
movl 0xc(%r12), %ecx
cmpl 0xc(%rax), %ecx
setg %bpl
jmp 0x34eed
setl %bpl
leaq 0xf0(%rsp), %rdi
callq 0x61d0
movq %r13, %rdi
callq 0x61d0
testb %bpl, %bpl
cmovneq %rbx, %r15
jmp 0x34e7f
xorl %r15d, %r15d
leaq 0x18(%rsp), %r13
leaq 0x8(%r15), %rsi
leaq 0x60(%rsp), %rdi
callq 0x6240
leaq 0x130(%rsp), %rdi
leaq 0x60(%rsp), %rsi
callq 0x35fda
movq (%rax), %rbx
movq 0x48(%rsp), %rax
cmpq %rax, (%rbx)
jne 0x34fda
cmpb $0x0, 0x1a0(%rsp)
je 0x3522e
movslq 0xc(%rbx), %r13
movq 0x40(%rsp), %rax
movq 0x88(%rax), %r14
movq 0x90(%rax), %rbp
leaq 0x1(%r13), %r12
shlq $0x3, %r13
cmpq %rbp, %r14
je 0x35246
leaq 0x18(%rsp), %rdi
movq %r14, %rsi
callq 0x15d3a
movq 0x18(%rsp), %rcx
movq 0x20(%rsp), %rax
movq %rax, %rdx
subq %rcx, %rdx
sarq $0x3, %rdx
cmpq %r12, %rdx
jbe 0x34fbd
addq %r13, %rcx
addq $0x8, %rcx
cmpq %rax, %rcx
jae 0x34fbd
movq (%rcx), %rdx
addq $0x8, %rcx
cmpq (%rbx), %rdx
jne 0x34faa
jmp 0x34fcd
leaq 0x18(%rsp), %rdi
callq 0x990e
addq $0x18, %r14
jmp 0x34f74
leaq 0x18(%rsp), %r13
movq %r13, %rdi
callq 0x990e
movb $0x0, 0x8(%rbx)
leaq 0xb8(%rsp), %rdi
movq %r15, %rsi
callq 0x36026
movq (%rbx), %rax
movq %rax, 0x18(%rsp)
leaq 0x80(%rsp), %rdi
movq %r13, %rsi
leaq 0x20(%rsp), %rdx
callq 0x31e94
movq 0x40(%rsp), %rax
movq 0x38(%rax), %rsi
movq %rbx, 0x10(%rsp)
movq (%rbx), %rdx
movq %r13, %rdi
callq 0x164ca
movq 0x18(%rsp), %rbx
movq 0x20(%rsp), %r14
cmpq %r14, %rbx
je 0x3506a
movq (%rbx), %rdi
movq %rdi, 0xf0(%rsp)
leaq 0x110(%rsp), %rsi
callq 0x353b4
testb %al, %al
jne 0x35064
leaq 0x80(%rsp), %rdi
leaq 0xf0(%rsp), %rsi
callq 0x171da
addq $0x8, %rbx
jmp 0x3502e
movq %r13, %rdi
callq 0x990e
movq 0x80(%rsp), %r15
movq 0x88(%rsp), %rax
movq %rax, 0xa0(%rsp)
cmpq 0xa0(%rsp), %r15
je 0x34e55
movq %r15, 0x58(%rsp)
movq (%r15), %r14
movq 0x10(%rsp), %rax
movl 0xc(%rax), %r15d
leal 0x1(%r15), %eax
movl %eax, 0xc(%rsp)
testl %r15d, %r15d
js 0x3512b
movq 0x40(%rsp), %rax
movq 0x88(%rax), %rbp
movq 0x90(%rax), %r13
leaq 0x2(%r15), %rbx
movl 0xc(%rsp), %r12d
cmpq %r13, %rbp
je 0x3512b
leaq 0x18(%rsp), %rdi
movq %rbp, %rsi
callq 0x15d3a
movq 0x18(%rsp), %rax
movq 0x20(%rsp), %rcx
subq %rax, %rcx
sarq $0x3, %rcx
cmpq %rbx, %rcx
jb 0x3511b
movq (%rax,%r12,8), %rcx
cmpq %r14, %rcx
je 0x35216
movq 0x10(%rsp), %rdx
cmpq (%rdx), %rcx
jne 0x3511b
cmpq %r14, (%rax,%r15,8)
je 0x35216
leaq 0x18(%rsp), %rdi
callq 0x990e
addq $0x18, %rbp
jmp 0x350d2
leaq 0x18(%rsp), %rbx
movq %rbx, %rdi
movl 0xc(%rsp), %esi
movq %r14, %rdx
callq 0x32f4c
leaq 0x60(%rsp), %r15
movq %r15, %rdi
movq %rbx, %rsi
callq 0x6470
movq %rbx, %rdi
callq 0x61d0
leaq 0x130(%rsp), %rdi
movq %r15, %rsi
callq 0x35ff8
movq 0x58(%rsp), %r15
testq %rax, %rax
jne 0x351b1
pushq $0x20
popq %rdi
callq 0x63b0
movq %r14, (%rax)
movb $0x1, 0x8(%rax)
movl 0xc(%rsp), %ecx
movl %ecx, 0xc(%rax)
movl $0x186a0, 0x10(%rax)    # imm = 0x186A0
movq 0x10(%rsp), %rcx
movq %rcx, 0x18(%rax)
movq %rax, 0x18(%rsp)
leaq 0x130(%rsp), %rdi
leaq 0x60(%rsp), %rsi
movq %rbx, %rdx
callq 0x36128
leaq 0x130(%rsp), %rdi
leaq 0x60(%rsp), %rsi
callq 0x35fda
movq (%rax), %rax
movq %rax, 0x50(%rsp)
cmpb $0x1, 0x8(%rax)
jne 0x35225
movq 0x40(%rsp), %rdi
movq %r14, %rsi
movq 0x48(%rsp), %rdx
callq 0x32cd4
movl 0xc(%rsp), %edx
addl %edx, %eax
movq 0x50(%rsp), %rcx
cmpl %eax, 0x10(%rcx)
jle 0x35202
movl %edx, 0xc(%rcx)
movl %eax, 0x10(%rcx)
movq 0x10(%rsp), %rax
movq %rax, 0x18(%rcx)
leaq 0xb8(%rsp), %rdi
leaq 0x60(%rsp), %rsi
callq 0x36252
jmp 0x35225
leaq 0x18(%rsp), %rdi
callq 0x990e
movq 0x58(%rsp), %r15
addq $0x8, %r15
jmp 0x3508a
testq %r14, %r14
jne 0x35246
jmp 0x35270
movq 0x38(%rsp), %rdi
movq %rbx, %rsi
callq 0x171da
movq 0x18(%rbx), %rbx
cmpq $0x0, 0x18(%rbx)
jne 0x35235
leaq 0xb0(%rsp), %rsi
movq 0x38(%rsp), %rdi
callq 0x171da
movq 0x38(%rsp), %rax
movq (%rax), %rdi
movq 0x8(%rax), %rsi
callq 0x18697
leaq 0x130(%rsp), %rdi
callq 0x35522
leaq 0xb8(%rsp), %rdi
callq 0x22af2
leaq 0x110(%rsp), %rdi
callq 0x990e
leaq 0x60(%rsp), %rdi
callq 0x61d0
leaq 0x80(%rsp), %rdi
callq 0x990e
movq 0x38(%rsp), %rax
addq $0x168, %rsp            # imm = 0x168
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
jmp 0x35304
jmp 0x35338
jmp 0x35338
movq %rax, %rbx
pushq $0x20
popq %rsi
movq %r15, %rdi
callq 0x63c0
jmp 0x3533b
jmp 0x35338
jmp 0x35338
jmp 0x35338
jmp 0x35338
movq %rax, %rbx
leaq 0xb8(%rsp), %rdi
callq 0x990e
jmp 0x35355
jmp 0x35338
jmp 0x35338
movq %rax, %rbx
jmp 0x35355
jmp 0x35338
jmp 0x35338
jmp 0x35338
movq %rax, %rbx
jmp 0x3531b
jmp 0x35338
movq %rax, %rbx
leaq 0xf0(%rsp), %rdi
callq 0x61d0
leaq 0x18(%rsp), %rdi
callq 0x61d0
jmp 0x3533b
movq %rax, %rbx
leaq 0x18(%rsp), %rdi
callq 0x990e
jmp 0x3533b
jmp 0x35338
movq %rax, %rbx
leaq 0x130(%rsp), %rdi
callq 0x35522
leaq 0xb8(%rsp), %rdi
callq 0x22af2
leaq 0x110(%rsp), %rdi
callq 0x990e
leaq 0x60(%rsp), %rdi
callq 0x61d0
leaq 0x80(%rsp), %rdi
callq 0x990e
movq 0x38(%rsp), %rdi
callq 0x990e
movq %rbx, %rdi
callq 0x6650
nop