// File: /workspace/github_cpp_makefile_star2_O3/Kei18[P]pibt/./src/solver/winpibt.cpp
// Function: winPIBT::getPath(Agent*, Node*, int, int)
Nodes winPIBT::getPath(Agent* a, Node* _g, int t1, int t2) {
  if (t2 <= t1 || t2 <= ell(a)) {
    std::cout << "error@winPIBT::getPath"
              << ", ell : " << ell(a)
              << ", t1 : " << t1
              << ", t2 : " << t2
              << "\n";
    std::exit(1);
  }

  int id = a->getId();
  Node* _s = PATHS[a->getId()][t1];  // start pos
  Nodes path, tmpPath, C;
  int f, g;
  std::string key;
  bool invalid = true;  // success or not

  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, t1, 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 goal
    if (n->g >= t2) {
      invalid = false;
      break;
    }

    // ==== fast implementation ====
    tmpPath = G->getPath(n->v, _g);
    while (n->g + tmpPath.size() - 1 < t2) tmpPath.push_back(_g);
    while (n->g + tmpPath.size() - 1 > t2) tmpPath.pop_back();
    if (checkValidPath(id, tmpPath, n->g, t2)) {
      tmpPath.erase(tmpPath.begin());
      for (auto v : tmpPath) n = new AN { v, n->g + 1, 0, n };
      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) {
      g = n->g + 1;
      key = getKey(g, m);
      if (CLOSE.find(key) != CLOSE.end()) continue;

      // collision check
      tmpPath = { n->v, m };
      if (!checkValidPath(id, tmpPath, n->g, t2)) 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 $0x158, %rsp            # imm = 0x158
movl %r9d, %ebp
movl %r8d, %ebx
movq %rdx, %r15
movq %rsi, %r14
movq %rcx, 0x48(%rsp)
movq %rcx, 0x70(%rsp)
cmpl %r8d, %r9d
jle 0x463da
movslq (%r15), %rcx
movq 0x88(%r14), %rax
cmpl %ebp, (%rax,%rcx,4)
jge 0x463da
movq %rdi, %r13
movl %ebp, 0xc(%rsp)
movq 0x70(%r14), %rax
movq %rcx, 0x98(%rsp)
leaq (%rcx,%rcx,2), %rcx
movslq %ebx, %rdx
movq (%rax,%rcx,8), %rax
movq (%rax,%rdx,8), %r15
xorl %eax, %eax
movq %rax, 0x10(%rdi)
xorps %xmm0, %xmm0
movups %xmm0, (%rdi)
movq %rax, 0x40(%rsp)
movaps %xmm0, 0x30(%rsp)
movq %rax, 0x60(%rsp)
movaps %xmm0, 0x50(%rsp)
leaq 0x88(%rsp), %rcx
movq %rcx, -0x10(%rcx)
movq %rax, -0x8(%rcx)
movb $0x0, (%rcx)
leaq 0xb8(%rsp), %rcx
movq %rax, -0x8(%rcx)
movaps %xmm0, -0x18(%rcx)
movq %rcx, (%rcx)
movq %rcx, 0x8(%rcx)
leaq 0x118(%rsp), %rax
movq %rax, -0x30(%rax)
movl $0x1, %ecx
movq %rcx, -0x28(%rax)
movups %xmm0, -0x20(%rax)
movl $0x3f800000, %edx       # imm = 0x3F800000
movl %edx, -0x10(%rax)
movups %xmm0, -0x8(%rax)
leaq 0x150(%rsp), %rax
movq %rax, -0x30(%rax)
movq %rcx, -0x28(%rax)
movups %xmm0, -0x20(%rax)
movl %edx, -0x10(%rax)
movups %xmm0, -0x8(%rax)
movl $0x18, %edi
callq 0x6370
movq %rax, %r12
movq %r14, %rbp
movq %r15, (%rax)
movl %ebx, 0x8(%rax)
movq %r14, %rdi
movq %r15, %rsi
movq 0x48(%rsp), %rdx
callq 0x3efbe
movl %eax, 0xc(%r12)
movq $0x0, 0x10(%r12)
leaq 0x10(%rsp), %rsi
movq %r12, (%rsi)
leaq 0xa0(%rsp), %rdi
callq 0x1b3c4
movq %rax, 0xe0(%rsp)
leaq 0x10(%rsp), %rdi
movq %r12, %rsi
callq 0x3f4d4
leaq 0x78(%rsp), %rdi
leaq 0x10(%rsp), %rbx
movq %rbx, %rsi
callq 0x6460
movq (%rbx), %rdi
leaq 0x20(%rsp), %rax
cmpq %rax, %rdi
je 0x45e51
movq 0x20(%rsp), %rsi
incq %rsi
callq 0x6380
leaq 0xe8(%rsp), %rdi
leaq 0x78(%rsp), %rsi
leaq 0xe0(%rsp), %rdx
callq 0x2c168
cmpq $0x0, 0xa0(%rsp)
je 0x4634e
movslq 0xc(%rsp), %rbx
leaq 0x10(%rsp), %rdi
leaq 0x30(%rsp), %r15
movq %r13, (%rsp)
movq %rbp, 0x48(%rsp)
movq %rbx, 0xd0(%rsp)
movq 0xa8(%rsp), %rax
movq 0x10(%rax), %r12
movl 0xc(%rsp), %eax
cmpl %eax, 0x8(%r12)
jge 0x462d8
movq 0x38(%rbp), %rsi
movq (%r12), %rdx
movq 0x70(%rsp), %rcx
movq (%rsi), %rax
callq *0x18(%rax)
movq 0x30(%rsp), %rdi
movq 0x40(%rsp), %rsi
movaps 0x10(%rsp), %xmm0
movaps %xmm0, 0x30(%rsp)
movq 0x20(%rsp), %rax
movq %rax, 0x40(%rsp)
xorps %xmm0, %xmm0
movaps %xmm0, 0x10(%rsp)
movq $0x0, 0x20(%rsp)
testq %rdi, %rdi
je 0x45f1b
subq %rdi, %rsi
callq 0x6380
movq 0x10(%rsp), %rdi
testq %rdi, %rdi
je 0x45f1b
movq 0x20(%rsp), %rsi
subq %rdi, %rsi
callq 0x6380
movslq 0x8(%r12), %rcx
movq 0x30(%rsp), %r13
movq 0x38(%rsp), %r14
movq %r14, %rax
subq %r13, %rax
sarq $0x3, %rax
leaq (%rcx,%rax), %rdx
decq %rdx
cmpq %rbx, %rdx
jae 0x45f98
cmpq 0x40(%rsp), %r14
je 0x45f5f
movq 0x70(%rsp), %rax
movq %rax, (%r14)
movq 0x38(%rsp), %r14
addq $0x8, %r14
movq %r14, 0x38(%rsp)
jmp 0x45f79
movq %r15, %rdi
movq %r14, %rsi
leaq 0x70(%rsp), %rdx
callq 0x1c5cc
movl 0x8(%r12), %ecx
movq 0x38(%rsp), %r14
movslq %ecx, %rax
movq 0x30(%rsp), %r13
movq %r14, %rdx
subq %r13, %rdx
sarq $0x3, %rdx
addq %rax, %rdx
decq %rdx
cmpq %rbx, %rdx
jb 0x45f40
jmp 0x45f9b
movq %rcx, %rax
decq %rax
movq %r14, %r15
cmpq %rbx, %rdx
jbe 0x45fd0
movq $-0x8, %rdx
subq %r13, %rdx
movq %r14, %r15
addq $-0x8, %r14
leaq (%rdx,%r15), %rsi
sarq $0x3, %rsi
addq %rax, %rsi
addq $-0x8, %r15
cmpq %rbx, %rsi
ja 0x45fb3
movq %r14, 0x38(%rsp)
movq %rbp, %rdi
movq 0x98(%rsp), %rsi
leaq 0x30(%rsp), %rdx
movl 0xc(%rsp), %r8d
callq 0x4670a
testb %al, %al
jne 0x46272
leaq 0xa0(%rsp), %rdi
callq 0x1b446
leaq 0x10(%rsp), %rbx
movq %rbx, %rdi
movq %r12, %rsi
callq 0x3f4d4
leaq 0x120(%rsp), %rdi
movq %rbx, %rsi
callq 0x2c554
movq 0x10(%rsp), %rdi
leaq 0x20(%rsp), %rax
cmpq %rax, %rdi
je 0x4603b
movq 0x20(%rsp), %rsi
incq %rsi
callq 0x6380
movq 0x38(%rbp), %rsi
movq (%r12), %rdx
movq %rbx, %rdi
callq 0x1a38a
movq 0x50(%rsp), %rdi
movq 0x60(%rsp), %rsi
movaps 0x10(%rsp), %xmm0
movaps %xmm0, 0x50(%rsp)
movq 0x20(%rsp), %rax
movq %rax, 0x60(%rsp)
xorps %xmm0, %xmm0
movaps %xmm0, 0x10(%rsp)
movq $0x0, 0x20(%rsp)
testq %rdi, %rdi
je 0x4609e
subq %rdi, %rsi
callq 0x6380
movq 0x10(%rsp), %rdi
testq %rdi, %rdi
je 0x4609e
movq 0x20(%rsp), %rsi
subq %rdi, %rsi
callq 0x6380
movq 0x58(%rsp), %rsi
cmpq 0x60(%rsp), %rsi
je 0x460c1
movq (%r12), %rax
movq %rax, (%rsi)
movq 0x58(%rsp), %rbx
addq $0x8, %rbx
movq %rbx, 0x58(%rsp)
jmp 0x460d3
leaq 0x50(%rsp), %rdi
movq %r12, %rdx
callq 0x1c5cc
movq 0x58(%rsp), %rbx
movq 0x50(%rsp), %rbp
cmpq %rbx, %rbp
je 0x46243
movq (%rbp), %r13
movl 0x8(%r12), %r15d
incl %r15d
leaq 0x10(%rsp), %r14
movq %r14, %rdi
movl %r15d, %esi
movq %r13, %rdx
callq 0x3f2d8
leaq 0x78(%rsp), %rdi
movq %r14, %rsi
callq 0x6460
movq 0x10(%rsp), %rdi
leaq 0x20(%rsp), %rax
cmpq %rax, %rdi
je 0x46129
movq 0x20(%rsp), %rsi
incq %rsi
callq 0x6380
leaq 0x120(%rsp), %rdi
leaq 0x78(%rsp), %rsi
callq 0x2c91a
testq %rax, %rax
jne 0x4623a
movq (%r12), %rax
movq %rax, 0x10(%rsp)
movq %r13, 0x18(%rsp)
leaq 0x30(%rsp), %r14
movq %r14, %rdi
leaq 0x10(%rsp), %rsi
leaq 0x20(%rsp), %rdx
callq 0x3dd40
movl 0x8(%r12), %ecx
movq 0x48(%rsp), %rdi
movq 0x98(%rsp), %rsi
movq %r14, %rdx
movl 0xc(%rsp), %r8d
callq 0x4670a
testb %al, %al
je 0x4623a
movq 0x70(%rsp), %rdx
movq 0x48(%rsp), %rdi
movq %r13, %rsi
callq 0x3efbe
movl %eax, %r14d
leaq 0xe8(%rsp), %rdi
leaq 0x78(%rsp), %rsi
callq 0x2c954
addl %r15d, %r14d
testq %rax, %rax
je 0x461e8
movq 0x28(%rax), %rsi
movq 0x10(%rsi), %rax
cmpl %r14d, 0xc(%rax)
jle 0x461d9
movl %r15d, 0x8(%rax)
movl %r14d, 0xc(%rax)
movq %r12, 0x10(%rax)
leaq 0xa0(%rsp), %rdi
callq 0x1b4b2
jmp 0x4623a
movl $0x18, %edi
callq 0x6370
movq %r13, (%rax)
movl %r15d, 0x8(%rax)
movl %r14d, 0xc(%rax)
movq %r12, 0x10(%rax)
movq %rax, 0xd8(%rsp)
leaq 0xa0(%rsp), %rdi
leaq 0xd8(%rsp), %rsi
callq 0x1b3c4
movq %rax, 0x10(%rsp)
leaq 0xe8(%rsp), %rdi
leaq 0x78(%rsp), %rsi
leaq 0x10(%rsp), %rdx
callq 0x2c168
addq $0x8, %rbp
jmp 0x460d8
cmpq $0x0, 0xa0(%rsp)
movq (%rsp), %r13
movq 0x48(%rsp), %rbp
movq 0xd0(%rsp), %rbx
leaq 0x10(%rsp), %rdi
leaq 0x30(%rsp), %r15
jne 0x45e9a
jmp 0x4634e
leaq 0x8(%r13), %rsi
cmpq %r14, %rsi
je 0x46293
subq %rsi, %r15
movq %r13, %rdi
movq %r15, %rdx
callq 0x65b0
movq 0x30(%rsp), %r13
movq 0x38(%rsp), %r14
addq $-0x8, %r14
movq %r14, 0x38(%rsp)
cmpq %r14, %r13
je 0x462d8
movq (%r13), %r15
movl $0x18, %edi
callq 0x6370
movq %rax, %rbx
movq %r15, (%rax)
movl 0x8(%r12), %eax
incl %eax
movl %eax, 0x8(%rbx)
movl $0x0, 0xc(%rbx)
movq %r12, 0x10(%rbx)
addq $0x8, %r13
movq %rbx, %r12
cmpq %r14, %r13
jne 0x462a1
jmp 0x462db
movq %r12, %rbx
movq (%rsp), %r13
movq 0x8(%r13), %rsi
cmpq 0x10(%r13), %rsi
je 0x462f6
movq (%rbx), %rax
movq %rax, (%rsi)
addq $0x8, 0x8(%r13)
jmp 0x46301
movq %r13, %rdi
movq %rbx, %rdx
callq 0x1c5cc
movq 0x10(%rbx), %rbx
testq %rbx, %rbx
jne 0x462df
movq (%r13), %rax
movq 0x8(%r13), %rcx
cmpq %rcx, %rax
setne %dl
addq $-0x8, %rcx
cmpq %rax, %rcx
seta %sil
andb %dl, %sil
cmpb $0x1, %sil
jne 0x4634e
addq $0x8, %rax
movq -0x8(%rax), %rdx
movq (%rcx), %rsi
movq %rsi, -0x8(%rax)
movq %rdx, (%rcx)
addq $-0x8, %rcx
leaq 0x8(%rax), %rdx
cmpq %rcx, %rax
movq %rdx, %rax
jb 0x46330
leaq 0x120(%rsp), %rdi
callq 0x2ae7e
leaq 0xe8(%rsp), %rdi
callq 0x2af1c
leaq 0xa0(%rsp), %rdi
callq 0x1b54e
movq 0x78(%rsp), %rdi
leaq 0x88(%rsp), %rax
cmpq %rax, %rdi
je 0x46397
movq 0x88(%rsp), %rsi
incq %rsi
callq 0x6380
movq 0x50(%rsp), %rdi
testq %rdi, %rdi
je 0x463ae
movq 0x60(%rsp), %rsi
subq %rdi, %rsi
callq 0x6380
movq 0x30(%rsp), %rdi
testq %rdi, %rdi
je 0x463c5
movq 0x40(%rsp), %rsi
subq %rdi, %rsi
callq 0x6380
movq %r13, %rax
addq $0x158, %rsp            # imm = 0x158
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
movq 0x1bbc7(%rip), %rdi     # 0x61fa8
leaq 0x5a77(%rip), %rsi      # 0x4be5f
callq 0x6360
leaq 0x5a82(%rip), %rsi      # 0x4be76
movq %rax, %rdi
callq 0x6360
movslq (%r15), %rcx
movq 0x88(%r14), %rdx
movl (%rdx,%rcx,4), %esi
movq %rax, %rdi
callq 0x6610
leaq 0x5a67(%rip), %rsi      # 0x4be7f
movq %rax, %rdi
callq 0x6360
movq %rax, %rdi
movl %ebx, %esi
callq 0x6610
leaq 0x5a56(%rip), %rsi      # 0x4be87
movq %rax, %rdi
callq 0x6360
movq %rax, %rdi
movl %ebp, %esi
callq 0x6610
leaq 0x2119(%rip), %rsi      # 0x48563
movq %rax, %rdi
callq 0x6360
movl $0x1, %edi
callq 0x6440
jmp 0x4649f
jmp 0x4649f
jmp 0x4649f
movq %rax, %rbx
movl $0x18, %esi
movq %r12, %rdi
callq 0x6380
jmp 0x464bf
jmp 0x4649f
jmp 0x464b8
jmp 0x4649f
jmp 0x464b8
jmp 0x464b8
movq %rax, %rbx
movq 0x10(%rsp), %rdi
leaq 0x20(%rsp), %rax
cmpq %rax, %rdi
je 0x464bb
movq 0x20(%rsp), %rsi
incq %rsi
callq 0x6380
jmp 0x464bb
movq %rax, %rbx
jmp 0x464bf
jmp 0x464b8
jmp 0x464b8
jmp 0x464b8
jmp 0x464b8
jmp 0x464b8
jmp 0x464b8
jmp 0x464b8
jmp 0x464b8
jmp 0x464b8
jmp 0x464b8
movq %rax, %rbx
movq (%rsp), %r13
leaq 0x120(%rsp), %rdi
callq 0x2ae7e
leaq 0xe8(%rsp), %rdi
callq 0x2af1c
leaq 0xa0(%rsp), %rdi
callq 0x1b54e
movq 0x78(%rsp), %rdi
leaq 0x88(%rsp), %rax
cmpq %rax, %rdi
je 0x46508
movq 0x88(%rsp), %rsi
incq %rsi
callq 0x6380
movq 0x50(%rsp), %rdi
testq %rdi, %rdi
je 0x4651f
movq 0x60(%rsp), %rsi
subq %rdi, %rsi
callq 0x6380
movq 0x30(%rsp), %rdi
testq %rdi, %rdi
je 0x46536
movq 0x40(%rsp), %rsi
subq %rdi, %rsi
callq 0x6380
movq (%r13), %rdi
testq %rdi, %rdi
je 0x4654b
movq 0x10(%r13), %rsi
subq %rdi, %rsi
callq 0x6380
movq %rbx, %rdi
callq 0x6620
nop