// File: /workspace/github_cpp_makefile_star2_O2/Kei18[P]pibt/./src/solver/whca.cpp
// Function: WHCA::getPath(Agent*, int, std::vector<std::vector<Node*, std::allocator<Node*>>, std::allocator<std::vector<Node*, std::allocator<Node*>>>>&)
Nodes WHCA::getPath(Agent* a, int startTime, Paths& paths) {

  Node* _s = a->getNode();
  Node* _g = a->getGoal();

  Nodes path, tmpPath, C;

  int f, g;
  std::string key;
  bool prohibited = false;
  int maxLength = getMaxLengthPaths(paths);

  // ==== fast implementation ====
  tmpPath = G->getPath(_s, _g);
  if (hasWindow) {
    while (tmpPath.size() <= window + 1) tmpPath.push_back(_g);
    while (tmpPath.size() != window + 1) tmpPath.pop_back();
  } else {
    while (tmpPath.size() <= maxLength) tmpPath.push_back(_g);
  }
  // check result of fast implementation
  // first, check goals
  if (!hasWindow) {
    for (auto cg : CGOAL) {
      if (cg->a == a) continue;
      for (auto v : tmpPath) {
        if (cg->v == v) {
          prohibited = true;
          break;
        }
      }
      if (prohibited) break;
    }
  }
  // second, check collision and intersection
  if (!prohibited) {
    int limit;
    for (auto p : paths) {
      limit = p.size();
      if (hasWindow) limit = startTime + window + 1;
      for (int t = startTime + 1; t < limit; ++t) {
        if (t >= p.size()) break;
        if (p[t] == tmpPath[t-startTime]) { // collision
          prohibited = true;
          break;
        }
        if (t > startTime) {  // intersection
          if (p[t] == tmpPath[t-startTime-1] && p[t-1] == tmpPath[t-startTime]) {
            prohibited = true;
            break;
          }
        }
      }
      if (prohibited) break;
    }
  }
  // success of fast implementation
  if (!prohibited) return tmpPath;
  tmpPath.clear();
  // =============================

  // normal path finding
  bool invalid = true;

  boost::heap::fibonacci_heap<Fib_AN> OPEN;
  std::unordered_map<std::string, boost::heap::fibonacci_heap<Fib_AN>::handle_type> SEARCHED;
  std::unordered_set<std::string> CLOSE;  // key
  AN* n = new AN { _s, startTime, pathDist(_s, _g), nullptr };
  auto handle = OPEN.push(Fib_AN(n));
  key = getKey(n);
  SEARCHED.emplace(key, handle);

  while (!OPEN.empty()) {
    // argmin
    n = OPEN.top().node;

    // check termination condition
    if (!hasWindow) {  // non-window
      if (n->v == _g && n->g >= maxLength - 1) {
        invalid = false;
        break;
      }
    } else {  // windowed
      if (n->g >= startTime + window) {
        invalid = false;
        break;
      }
    }

    // update list
    OPEN.pop();
    CLOSE.emplace(getKey(n));

    // search neighbor
    C = G->neighbor(n->v);
    C.push_back(n->v);

    for (auto m : C) {
      // check other paths
      g = n->g + 1;
      key = getKey(g, m);
      if (CLOSE.find(key) != CLOSE.end()) continue;

      // check collision
      prohibited = false;
      for (auto p : paths) {
        if (g >= p.size()) continue;
        if (p[g] == m) {  // vertex collision
          prohibited = true;
          break;
        }
        if (p[g] == n->v && p[g-1] == m) {  // swap collision
          prohibited = true;
          break;
        }
      }
      if (prohibited) continue;

      // collsiion at goal
      if (!hasWindow) {
        for (auto cg : CGOAL) {  // collection of goals
          if (a != cg->a && g >= cg->t && m == cg->v) {
            prohibited = true;
            break;
          }
        }
        if (prohibited) continue;
      }

      f = g + pathDist(m, _g);

      auto itrS = SEARCHED.find(key);
      if (itrS == SEARCHED.end()) {  // new node
        AN* l = new AN { m, g, f, n };
        auto handle = OPEN.push(Fib_AN(l));
        SEARCHED.emplace(key, handle);
      } else {
        auto handle = itrS->second;
        AN* l = (*handle).node;
        if (l->f > f) {
          l->g = g;
          l->f = f;
          l->p = n;
        }
        OPEN.increase(handle);
      }
    }
  }

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

  return path;
}

// Assembly:
pushq %rbp
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0x198, %rsp            # imm = 0x198
movq %rcx, 0x48(%rsp)
movq %rsi, %r13
movq %rdi, %r12
movq 0x8(%rdx), %rbp
movq %rdx, 0x88(%rsp)
movq 0x10(%rdx), %r14
movq %r14, 0x40(%rsp)
andq $0x0, 0x70(%rsp)
xorps %xmm0, %xmm0
andq $0x0, 0x30(%rsp)
movaps %xmm0, 0x60(%rsp)
andq $0x0, 0xa0(%rsp)
movaps %xmm0, 0x20(%rsp)
movaps %xmm0, 0x90(%rsp)
leaq 0x118(%rsp), %rax
movq %rax, -0x10(%rax)
andq $0x0, -0x8(%rax)
movb $0x0, (%rax)
movq %rsi, %rdi
movq %r8, 0x80(%rsp)
movq %r8, %rsi
callq 0x32bac
movl %eax, 0x14(%rsp)
movq 0x38(%r13), %rsi
movq (%rsi), %rax
leaq 0x128(%rsp), %rdi
movq %rbp, %rdx
movq %r14, %rcx
callq *0x18(%rax)
leaq 0x20(%rsp), %rdi
leaq 0x128(%rsp), %r14
movq %r14, %rsi
callq 0x18466
movq %r14, %rdi
callq 0x990e
cmpb $0x0, 0x68(%r13)
je 0x36e25
leaq 0x20(%rsp), %r14
leaq 0x40(%rsp), %r15
movq 0x20(%rsp), %rax
movq 0x28(%rsp), %rcx
movq %rcx, %rdx
subq %rax, %rdx
movq %rdx, %rdi
sarq $0x3, %rdi
movslq 0x6c(%r13), %rsi
incq %rsi
cmpq %rsi, %rdi
ja 0x36d29
movq %r14, %rdi
movq %r15, %rsi
callq 0x171da
jmp 0x36cec
addq $-0x8, %rcx
movq %rcx, 0x28(%rsp)
addq $-0x8, %rdx
movq %rdx, %rdi
sarq $0x3, %rdi
cmpq %rsi, %rdi
jne 0x36d1c
cmpb $0x0, 0x68(%r13)
movq %r12, 0x78(%rsp)
movq %r13, 0x18(%rsp)
je 0x36e5e
movq %rbp, 0x50(%rsp)
movq 0x80(%rsp), %rax
movq (%rax), %r14
movq 0x8(%rax), %rax
movq %rax, 0x8(%rsp)
movq 0x48(%rsp), %rax
leal 0x1(%rax), %ecx
movl %ecx, 0x58(%rsp)
movslq %eax, %r13
leaq 0x1(%r13), %rbx
xorl %ebp, %ebp
leaq 0x128(%rsp), %r15
cmpq 0x8(%rsp), %r14
je 0x36e9f
movq %r15, %rdi
movq %r14, %rsi
callq 0x15d3a
movq 0x128(%rsp), %rax
movq 0x130(%rsp), %rcx
subq %rax, %rcx
sarq $0x3, %rcx
movq 0x18(%rsp), %rdx
movl 0x6c(%rdx), %esi
addl 0x58(%rsp), %esi
cmpb $0x0, 0x68(%rdx)
cmovel %ecx, %esi
movq 0x20(%rsp), %rdx
movslq %esi, %rsi
addq $0x8, %rdx
movq %rbx, %rdi
cmpq %rsi, %rdi
jge 0x36e07
cmpq %rdi, %rcx
jbe 0x36e07
movq (%rax,%rdi,8), %r9
movq (%rdx), %r8
movb $0x1, %r12b
cmpq %r8, %r9
je 0x36e0a
leaq -0x1(%rdi), %r10
cmpq %r13, %r10
jl 0x36dfe
cmpq -0x8(%rdx), %r9
jne 0x36dfe
cmpq %r8, -0x8(%rax,%rdi,8)
je 0x36e0a
incq %rdi
addq $0x8, %rdx
jmp 0x36dcf
movl %ebp, %r12d
movq %r15, %rdi
callq 0x990e
addq $0x18, %r14
movl %r12d, %ebp
testb $0x1, %r12b
je 0x36d80
jmp 0x36ea2
movslq 0x14(%rsp), %rbx
leaq 0x20(%rsp), %r14
leaq 0x40(%rsp), %r15
movq 0x20(%rsp), %rax
movq 0x28(%rsp), %rcx
movq %rcx, %rdx
subq %rax, %rdx
sarq $0x3, %rdx
cmpq %rbx, %rdx
ja 0x36d35
movq %r14, %rdi
movq %r15, %rsi
callq 0x171da
jmp 0x36e34
movq 0x70(%r13), %rdx
movq 0x78(%r13), %rsi
cmpq %rsi, %rdx
je 0x36d4a
movq (%rdx), %rdi
movq 0x88(%rsp), %r8
cmpq %r8, (%rdi)
je 0x36e99
movq %rax, %r8
cmpq %rcx, %r8
je 0x36e99
movq 0x10(%rdi), %r9
leaq 0x8(%r8), %r10
cmpq (%r8), %r9
movq %r10, %r8
jne 0x36e82
jmp 0x36ebb
addq $0x8, %rdx
jmp 0x36e66
movl %ebp, %r12d
testb $0x1, %r12b
movq 0x78(%rsp), %rbx
movq 0x18(%rsp), %r13
movq 0x50(%rsp), %rbp
je 0x37301
movq 0x20(%rsp), %rax
cmpq %rax, 0x28(%rsp)
je 0x36ecc
movq %rax, 0x28(%rsp)
leaq 0xf8(%rsp), %rax
xorps %xmm0, %xmm0
movaps %xmm0, -0x18(%rax)
andq $0x0, -0x8(%rax)
movq %rax, (%rax)
movq %rax, 0x8(%rax)
leaq 0x158(%rsp), %rax
movq %rax, -0x30(%rax)
pushq $0x1
popq %rcx
movq %rcx, -0x28(%rax)
movups %xmm0, -0x20(%rax)
movl $0x3f800000, %edx       # imm = 0x3F800000
movl %edx, -0x10(%rax)
movups %xmm0, -0x8(%rax)
leaq 0x190(%rsp), %rax
movq %rax, -0x30(%rax)
movq %rcx, -0x28(%rax)
movups %xmm0, -0x20(%rax)
movl %edx, -0x10(%rax)
movups %xmm0, -0x8(%rax)
pushq $0x18
popq %rdi
callq 0x63b0
movq %rax, %r14
movq %rbp, (%rax)
movq 0x48(%rsp), %rax
movl %eax, 0x8(%r14)
movq 0x40(%rsp), %rdx
movq %r13, %rdi
movq %rbp, %rsi
callq 0x32cd4
movl %eax, 0xc(%r14)
andq $0x0, 0x10(%r14)
leaq 0xb8(%rsp), %rsi
movq %r14, (%rsi)
leaq 0xe0(%rsp), %rdi
callq 0x1705a
movq %rax, 0xd8(%rsp)
leaq 0xb8(%rsp), %rdi
movq %r14, %rsi
callq 0x32fe8
leaq 0x108(%rsp), %rbx
leaq 0xb8(%rsp), %r14
movq %rbx, %rdi
movq %r14, %rsi
callq 0x6470
movq %r14, %rdi
callq 0x61d0
leaq 0x128(%rsp), %rdi
leaq 0xd8(%rsp), %rdx
movq %rbx, %rsi
callq 0x23fea
decl 0x14(%rsp)
leaq 0xb8(%rsp), %r15
cmpq $0x0, 0xe0(%rsp)
movq 0x78(%rsp), %rbx
je 0x372b9
leaq 0xe0(%rsp), %rdi
callq 0x170d2
movq (%rax), %rcx
cmpb $0x0, 0x68(%r13)
movq %rcx, 0x8(%rsp)
je 0x37006
movl 0x6c(%r13), %eax
addl 0x48(%rsp), %eax
jmp 0x37019
movq (%rcx), %rax
cmpq 0x40(%rsp), %rax
jne 0x37022
movl 0x14(%rsp), %eax
movq 0x8(%rsp), %rcx
cmpl %eax, 0x8(%rcx)
jge 0x3728d
leaq 0xe0(%rsp), %rdi
callq 0x17102
movq %r15, %rdi
movq 0x8(%rsp), %rsi
callq 0x32fe8
leaq 0x160(%rsp), %rdi
movq %r15, %rsi
callq 0x243dc
movq %r15, %rdi
callq 0x61d0
movq 0x38(%r13), %rsi
movq 0x8(%rsp), %rbx
movq (%rbx), %rdx
movq %r15, %rdi
callq 0x164ca
leaq 0x90(%rsp), %r14
movq %r14, %rdi
movq %r15, %rsi
callq 0x18466
movq %r15, %rdi
callq 0x990e
movq %r14, %rdi
movq %rbx, %rsi
callq 0x171da
movq 0x90(%rsp), %rax
movq 0x98(%rsp), %rcx
movq %rcx, 0x50(%rsp)
cmpq 0x50(%rsp), %rax
je 0x36fcc
movq %rax, 0x58(%rsp)
movq (%rax), %rbp
movq 0x8(%rsp), %rax
movslq 0x8(%rax), %rbx
leaq 0x1(%rbx), %r13
movq %r15, %rdi
movl %r13d, %esi
movq %rbp, %rdx
callq 0x32f4c
leaq 0x108(%rsp), %r14
movq %r14, %rdi
movq %r15, %rsi
callq 0x6470
movq %r15, %rdi
callq 0x61d0
leaq 0x160(%rsp), %rdi
movq %r14, %rsi
callq 0x2477c
testq %rax, %rax
je 0x37111
movq 0x58(%rsp), %rax
addq $0x8, %rax
movq 0x18(%rsp), %r13
jmp 0x370a3
movq 0x80(%rsp), %rax
movq (%rax), %r14
movq 0x8(%rax), %r12
cmpq %r12, %r14
je 0x37178
movq %r15, %rdi
movq %r14, %rsi
callq 0x15d3a
movq 0xb8(%rsp), %rax
movq 0xc0(%rsp), %rcx
subq %rax, %rcx
sarq $0x3, %rcx
cmpq %r13, %rcx
jbe 0x3716a
movq 0x8(%rax,%rbx,8), %rcx
cmpq %rbp, %rcx
je 0x371e6
movq 0x8(%rsp), %rdx
cmpq (%rdx), %rcx
jne 0x3716a
cmpq %rbp, (%rax,%rbx,8)
je 0x371e6
movq %r15, %rdi
callq 0x990e
addq $0x18, %r14
jmp 0x37120
movq 0x18(%rsp), %rax
cmpb $0x0, 0x68(%rax)
je 0x371f3
movq 0x40(%rsp), %rdx
movq 0x18(%rsp), %rdi
movq %rbp, %rsi
callq 0x32cd4
movl %eax, %r14d
leaq 0x128(%rsp), %rdi
leaq 0x108(%rsp), %rsi
callq 0x247aa
addl %r13d, %r14d
testq %rax, %rax
je 0x3722f
movq 0x28(%rax), %rsi
movq 0x10(%rsi), %rax
cmpl %r14d, 0xc(%rax)
jle 0x371d4
movl %r13d, 0x8(%rax)
movl %r14d, 0xc(%rax)
movq 0x8(%rsp), %rcx
movq %rcx, 0x10(%rax)
leaq 0xe0(%rsp), %rdi
callq 0x1717c
jmp 0x37101
movq %r15, %rdi
callq 0x990e
jmp 0x37101
movq 0x18(%rsp), %rcx
movq 0x70(%rcx), %rax
movq 0x78(%rcx), %rcx
cmpq %rcx, %rax
je 0x37183
movq (%rax), %rdx
movq 0x88(%rsp), %rsi
cmpq %rsi, (%rdx)
je 0x37229
cmpl 0x8(%rdx), %r13d
jl 0x37229
cmpq 0x10(%rdx), %rbp
je 0x37101
addq $0x8, %rax
jmp 0x37200
pushq $0x18
popq %rdi
callq 0x63b0
movq %rbp, (%rax)
movl %r13d, 0x8(%rax)
movl %r14d, 0xc(%rax)
movq 0x8(%rsp), %rcx
movq %rcx, 0x10(%rax)
movq %rax, 0xb0(%rsp)
leaq 0xe0(%rsp), %rdi
leaq 0xb0(%rsp), %rsi
callq 0x1705a
movq %rax, 0xb8(%rsp)
leaq 0x128(%rsp), %rdi
leaq 0x108(%rsp), %rsi
movq %r15, %rdx
callq 0x23fea
jmp 0x37101
movq 0x8(%rsp), %r14
testq %r14, %r14
je 0x372aa
leaq 0x60(%rsp), %rdi
movq %r14, %rsi
callq 0x171da
movq 0x10(%r14), %r14
jmp 0x37292
movq 0x60(%rsp), %rdi
movq 0x68(%rsp), %rsi
callq 0x18697
movaps 0x60(%rsp), %xmm0
movups %xmm0, (%rbx)
movq 0x70(%rsp), %rax
movq %rax, 0x10(%rbx)
xorps %xmm0, %xmm0
movaps %xmm0, 0x60(%rsp)
andq $0x0, 0x70(%rsp)
leaq 0x160(%rsp), %rdi
callq 0x22af2
leaq 0x128(%rsp), %rdi
callq 0x22bb0
leaq 0xe0(%rsp), %rdi
callq 0x171f8
jmp 0x37320
movaps 0x20(%rsp), %xmm0
movups %xmm0, (%rbx)
movq 0x30(%rsp), %rax
movq %rax, 0x10(%rbx)
xorps %xmm0, %xmm0
movaps %xmm0, 0x20(%rsp)
andq $0x0, 0x30(%rsp)
leaq 0x108(%rsp), %rdi
callq 0x61d0
leaq 0x90(%rsp), %rdi
callq 0x990e
leaq 0x20(%rsp), %rdi
callq 0x990e
leaq 0x60(%rsp), %rdi
callq 0x990e
movq %rbx, %rax
addq $0x198, %rsp            # imm = 0x198
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
jmp 0x373b2
jmp 0x373b2
movq %rax, %rbx
pushq $0x18
popq %rsi
movq %r14, %rdi
callq 0x63c0
jmp 0x373b5
jmp 0x373b2
jmp 0x3739b
jmp 0x3739b
jmp 0x373b2
jmp 0x3739b
jmp 0x373b2
jmp 0x373b2
movq %rax, %rbx
leaq 0xb8(%rsp), %rdi
callq 0x61d0
jmp 0x373b5
jmp 0x373b2
jmp 0x3739b
movq %rax, %rbx
jmp 0x373dc
jmp 0x373b2
jmp 0x373b2
jmp 0x373b2
jmp 0x373b2
jmp 0x373b2
jmp 0x373b2
jmp 0x373b2
jmp 0x373b2
jmp 0x373b2
movq %rax, %rbx
leaq 0x160(%rsp), %rdi
callq 0x22af2
leaq 0x128(%rsp), %rdi
callq 0x22bb0
leaq 0xe0(%rsp), %rdi
callq 0x171f8
leaq 0x108(%rsp), %rdi
callq 0x61d0
leaq 0x90(%rsp), %rdi
callq 0x990e
leaq 0x20(%rsp), %rdi
callq 0x990e
leaq 0x60(%rsp), %rdi
callq 0x990e
movq %rbx, %rdi
callq 0x6650