// File: /workspace/github_cpp_makefile_star2_O2/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, %r12d
movl %r8d, %ebx
movq %rdx, %r15
movq %rsi, %rbp
movq %rdi, 0x8(%rsp)
movq %rcx, 0x38(%rsp)
cmpl %r8d, %r9d
jle 0x388d2
movq %rcx, %r14
movslq (%r15), %rcx
movq 0x88(%rbp), %rax
cmpl %r12d, (%rax,%rcx,4)
jge 0x388d2
movl %r12d, 0x4(%rsp)
movq 0x70(%rbp), %rax
movq %rcx, 0x60(%rsp)
imulq $0x18, %rcx, %rcx
movslq %ebx, %rdx
movq (%rax,%rcx), %rax
movq (%rax,%rdx,8), %r15
xorps %xmm0, %xmm0
movq 0x8(%rsp), %rax
andq $0x0, 0x10(%rax)
movups %xmm0, (%rax)
andq $0x0, 0x20(%rsp)
andq $0x0, 0x80(%rsp)
leaq 0xd8(%rsp), %rax
movq %rax, -0x10(%rax)
andq $0x0, -0x8(%rax)
movaps %xmm0, 0x10(%rsp)
movaps %xmm0, 0x70(%rsp)
movb $0x0, (%rax)
leaq 0xb8(%rsp), %rax
movaps %xmm0, -0x18(%rax)
andq $0x0, -0x8(%rax)
movq %rax, (%rax)
movq %rax, 0x8(%rax)
leaq 0x118(%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 0x150(%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, %r12
movq %r15, (%rax)
movl %ebx, 0x8(%rax)
movq %rbp, %rdi
movq %r15, %rsi
movq %r14, %rdx
callq 0x32cd4
movl %eax, 0xc(%r12)
andq $0x0, 0x10(%r12)
leaq 0x40(%rsp), %rsi
movq %r12, (%rsi)
leaq 0xa0(%rsp), %rdi
callq 0x1705a
movq %rbp, 0x30(%rsp)
movq %rax, 0x98(%rsp)
leaq 0x40(%rsp), %rdi
movq %r12, %rsi
callq 0x32fe8
leaq 0xc8(%rsp), %rbx
leaq 0x40(%rsp), %r14
movq %rbx, %rdi
movq %r14, %rsi
callq 0x6470
movq %r14, %rdi
callq 0x61d0
leaq 0xe8(%rsp), %rdi
leaq 0x98(%rsp), %rdx
movq %rbx, %rsi
callq 0x23fea
movslq 0x4(%rsp), %rax
movq %rax, 0x68(%rsp)
leaq 0x10(%rsp), %r13
cmpq $0x0, 0xa0(%rsp)
je 0x38873
leaq 0xa0(%rsp), %rdi
callq 0x170d2
movq 0x30(%rsp), %rbx
movq (%rax), %r12
movl 0x4(%rsp), %eax
cmpl %eax, 0x8(%r12)
leaq 0x40(%rsp), %r15
jge 0x3885d
movq 0x38(%rbx), %rsi
movq (%r12), %rdx
movq 0x38(%rsp), %rcx
movq (%rsi), %rax
movq %r15, %rdi
callq *0x18(%rax)
movq %r13, %rdi
movq %r15, %rsi
callq 0x18466
movq %r15, %rdi
callq 0x990e
movslq 0x8(%r12), %rcx
movq 0x10(%rsp), %rbp
movq 0x18(%rsp), %r13
movq %r13, %r15
subq %rbp, %r15
movq %r15, %rax
sarq $0x3, %rax
addq %rcx, %rax
decq %rax
cmpq 0x68(%rsp), %rax
jae 0x385be
leaq 0x10(%rsp), %rdi
leaq 0x38(%rsp), %rsi
callq 0x171da
jmp 0x38584
movq %rcx, %rax
decq %rax
movq %r15, %rdx
sarq $0x3, %rdx
addq %rax, %rdx
cmpq 0x68(%rsp), %rdx
jbe 0x385e4
addq $-0x8, %r13
movq %r13, 0x18(%rsp)
addq $-0x8, %r15
jmp 0x385c4
movq %rbx, %rdi
movq 0x60(%rsp), %rsi
leaq 0x10(%rsp), %rdx
movl 0x4(%rsp), %r8d
callq 0x38b10
testb %al, %al
jne 0x387ee
leaq 0xa0(%rsp), %rdi
callq 0x17102
leaq 0x10(%rsp), %r13
leaq 0x40(%rsp), %r15
movq %r15, %rdi
movq %r12, %rsi
callq 0x32fe8
leaq 0x120(%rsp), %rdi
movq %r15, %rsi
callq 0x243dc
movq %r15, %rdi
callq 0x61d0
movq 0x38(%rbx), %rsi
movq (%r12), %rdx
movq %r15, %rdi
callq 0x164ca
leaq 0x70(%rsp), %rbx
movq %rbx, %rdi
movq %r15, %rsi
callq 0x18466
movq %r15, %rdi
callq 0x990e
movq %rbx, %rdi
movq %r12, %rsi
callq 0x171da
movq 0x70(%rsp), %rbx
movq 0x78(%rsp), %rax
movq %rax, 0x88(%rsp)
cmpq 0x88(%rsp), %rbx
je 0x38523
movq (%rbx), %r15
movl 0x8(%r12), %ebp
incl %ebp
leaq 0x40(%rsp), %r13
movq %r13, %rdi
movl %ebp, %esi
movq %r15, %rdx
callq 0x32f4c
movq %r12, %r14
leaq 0xc8(%rsp), %r12
movq %r12, %rdi
movq %r13, %rsi
callq 0x6470
movq %r13, %rdi
callq 0x61d0
leaq 0x120(%rsp), %rdi
movq %r12, %rsi
callq 0x2477c
testq %rax, %rax
leaq 0x10(%rsp), %r13
movq %r14, %r12
jne 0x387e5
movq (%r12), %rax
movq %rax, 0x40(%rsp)
movq %r15, 0x48(%rsp)
movq %r13, %rdi
leaq 0x40(%rsp), %rsi
leaq 0x50(%rsp), %rdx
callq 0x31e94
movl 0x8(%r12), %ecx
movq 0x30(%rsp), %rdi
movq 0x60(%rsp), %rsi
movq %r13, %rdx
movl 0x4(%rsp), %r8d
callq 0x38b10
testb %al, %al
je 0x387e5
movq 0x38(%rsp), %rdx
movq 0x30(%rsp), %rdi
movq %r15, %rsi
callq 0x32cd4
movl %eax, %r13d
leaq 0xe8(%rsp), %rdi
leaq 0xc8(%rsp), %rsi
callq 0x247aa
addl %ebp, %r13d
testq %rax, %rax
je 0x3878e
movq 0x28(%rax), %rsi
movq 0x10(%rsi), %rax
cmpl %r13d, 0xc(%rax)
jle 0x3877a
movl %ebp, 0x8(%rax)
movl %r13d, 0xc(%rax)
movq %r12, 0x10(%rax)
leaq 0xa0(%rsp), %rdi
callq 0x1717c
leaq 0x10(%rsp), %r13
jmp 0x387e5
pushq $0x18
popq %rdi
callq 0x63b0
movq %r15, (%rax)
movl %ebp, 0x8(%rax)
movl %r13d, 0xc(%rax)
movq %r12, 0x10(%rax)
movq %rax, 0x90(%rsp)
leaq 0xa0(%rsp), %rdi
leaq 0x90(%rsp), %rsi
callq 0x1705a
movq %rax, 0x40(%rsp)
leaq 0xe8(%rsp), %rdi
leaq 0xc8(%rsp), %rsi
leaq 0x40(%rsp), %rdx
callq 0x23fea
leaq 0x10(%rsp), %r13
addq $0x8, %rbx
jmp 0x38682
leaq 0x8(%rbp), %rsi
cmpq %rsi, %r13
je 0x38810
addq $-0x8, %r15
movq %rbp, %rdi
movq %r15, %rdx
callq 0x65c0
movq 0x10(%rsp), %rbp
movq 0x18(%rsp), %r13
addq $-0x8, %r13
movq %r13, 0x18(%rsp)
pushq $0x18
popq %rbx
cmpq %r13, %rbp
je 0x3885d
movq (%rbp), %r14
movq %rbx, %rdi
callq 0x63b0
movq %r14, (%rax)
movl 0x8(%r12), %ecx
incl %ecx
movl %ecx, 0x8(%rax)
andl $0x0, 0xc(%rax)
movq %r12, 0x10(%rax)
addq $0x8, %rbp
movq %rax, %r12
jmp 0x3881c
movq 0x8(%rsp), %rdi
movq %r12, %rsi
callq 0x171da
movq 0x10(%r12), %r12
testq %r12, %r12
jne 0x3884b
movq 0x8(%rsp), %rax
movq (%rax), %rdi
movq 0x8(%rax), %rsi
callq 0x18697
leaq 0x120(%rsp), %rdi
callq 0x22af2
leaq 0xe8(%rsp), %rdi
callq 0x22bb0
leaq 0xa0(%rsp), %rdi
callq 0x171f8
leaq 0xc8(%rsp), %rdi
callq 0x61d0
leaq 0x70(%rsp), %rdi
callq 0x990e
leaq 0x10(%rsp), %rdi
callq 0x990e
movq 0x8(%rsp), %rax
addq $0x158, %rsp            # imm = 0x158
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
movq 0x216bf(%rip), %rdi     # 0x59f98
leaq 0x5a8f(%rip), %rsi      # 0x3e36f
callq 0x63a0
leaq 0x5a9a(%rip), %rsi      # 0x3e386
movq %rax, %rdi
callq 0x63a0
movslq (%r15), %rcx
movq 0x88(%rbp), %rdx
movl (%rdx,%rcx,4), %esi
movq %rax, %rdi
callq 0x6630
leaq 0x5a7f(%rip), %rsi      # 0x3e38f
movq %rax, %rdi
callq 0x63a0
movq %rax, %rdi
movl %ebx, %esi
callq 0x6630
leaq 0x5a6e(%rip), %rsi      # 0x3e397
movq %rax, %rdi
callq 0x63a0
movq %rax, %rdi
movl %r12d, %esi
callq 0x6630
leaq 0x1c20(%rip), %rsi      # 0x3a563
movq %rax, %rdi
callq 0x63a0
pushq $0x1
popq %rdi
callq 0x6450
jmp 0x38998
jmp 0x38998
movq %rax, %rbx
pushq $0x18
popq %rsi
movq %r12, %rdi
callq 0x63c0
jmp 0x3899b
jmp 0x38998
jmp 0x38998
jmp 0x38998
jmp 0x38998
movq %rax, %rbx
leaq 0x40(%rsp), %rdi
callq 0x61d0
jmp 0x3899b
jmp 0x38998
jmp 0x38998
jmp 0x38998
jmp 0x38998
jmp 0x38998
jmp 0x38998
jmp 0x38998
jmp 0x38998
jmp 0x38998
jmp 0x38998
jmp 0x38998
jmp 0x38998
jmp 0x38998
movq %rax, %rbx
leaq 0x120(%rsp), %rdi
callq 0x22af2
leaq 0xe8(%rsp), %rdi
callq 0x22bb0
leaq 0xa0(%rsp), %rdi
callq 0x171f8
leaq 0xc8(%rsp), %rdi
callq 0x61d0
leaq 0x70(%rsp), %rdi
callq 0x990e
leaq 0x10(%rsp), %rdi
callq 0x990e
movq 0x8(%rsp), %rdi
callq 0x990e
movq %rbx, %rdi
callq 0x6650
nop