// File: 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
movq %rsp, %rbp
subq $0x430, %rsp            # imm = 0x430
movq %rdi, -0x318(%rbp)
movq %rdi, %rax
movq %rax, -0x310(%rbp)
movq %rdi, -0x8(%rbp)
movq %rsi, -0x10(%rbp)
movq %rdx, -0x18(%rbp)
movq %rcx, -0x20(%rbp)
movl %r8d, -0x24(%rbp)
movl %r9d, -0x28(%rbp)
movq -0x10(%rbp), %rax
movq %rax, -0x308(%rbp)
movl -0x28(%rbp), %eax
cmpl -0x24(%rbp), %eax
jle 0xae760
movq -0x308(%rbp), %rdi
movl -0x28(%rbp), %eax
movl %eax, -0x31c(%rbp)
movq -0x18(%rbp), %rsi
callq 0xadcd0
movl %eax, %ecx
movl -0x31c(%rbp), %eax
cmpl %ecx, %eax
jg 0xae7f4
movq 0x52831(%rip), %rdi     # 0x100f98
leaq 0x91a7(%rip), %rsi      # 0xb7915
callq 0x84d0
movq %rax, %rdi
leaq 0x91af(%rip), %rsi      # 0xb792c
callq 0x84d0
movq -0x308(%rbp), %rdi
movq %rax, -0x328(%rbp)
movq -0x18(%rbp), %rsi
callq 0xadcd0
movq -0x328(%rbp), %rdi
movl %eax, %esi
callq 0x8860
movq %rax, %rdi
leaq 0x9184(%rip), %rsi      # 0xb7935
callq 0x84d0
movq %rax, %rdi
movl -0x24(%rbp), %esi
callq 0x8860
movq %rax, %rdi
leaq 0x9172(%rip), %rsi      # 0xb793d
callq 0x84d0
movq %rax, %rdi
movl -0x28(%rbp), %esi
callq 0x8860
movq %rax, %rdi
leaq 0x4dd2(%rip), %rsi      # 0xb35b7
callq 0x84d0
movl $0x1, %edi
callq 0x85d0
movq -0x18(%rbp), %rdi
callq 0x63b50
movl %eax, %ecx
movq -0x308(%rbp), %rax
movl %ecx, -0x2c(%rbp)
addq $0x70, %rax
movq %rax, -0x330(%rbp)
movq -0x18(%rbp), %rdi
callq 0x63b50
movq -0x330(%rbp), %rdi
movslq %eax, %rsi
callq 0xda70
movq %rax, %rdi
movslq -0x24(%rbp), %rsi
callq 0xda90
movq -0x318(%rbp), %rdi
movq (%rax), %rax
movq %rax, -0x38(%rbp)
movb $0x0, -0x39(%rbp)
callq 0x3e880
leaq -0x58(%rbp), %rdi
callq 0x3e880
leaq -0x70(%rbp), %rdi
callq 0x3e880
leaq -0x98(%rbp), %rdi
callq 0x86b0
movb $0x1, -0x99(%rbp)
leaq -0xc8(%rbp), %rdi
leaq -0xc9(%rbp), %rsi
callq 0x3eef0
jmp 0xae889
leaq -0x118(%rbp), %rdi
callq 0x636c0
leaq -0x150(%rbp), %rdi
callq 0x636e0
movl $0x18, %edi
callq 0x84e0
movq %rax, -0x338(%rbp)
jmp 0xae8b4
movq -0x308(%rbp), %rdi
movq -0x338(%rbp), %rax
movq -0x38(%rbp), %rcx
movq %rcx, (%rax)
movl -0x24(%rbp), %ecx
movl %ecx, 0x8(%rax)
addq $0xc, %rax
movq %rax, -0x348(%rbp)
movq -0x38(%rbp), %rsi
movq -0x20(%rbp), %rdx
callq 0x9d410
movl %eax, -0x33c(%rbp)
jmp 0xae8ef
movq -0x338(%rbp), %rax
movq -0x348(%rbp), %rcx
movl -0x33c(%rbp), %edx
movl %edx, (%rcx)
movq $0x0, 0x10(%rax)
movq %rax, -0x158(%rbp)
movq -0x158(%rbp), %rsi
leaq -0x168(%rbp), %rdi
callq 0x3f050
jmp 0xae929
leaq -0xc8(%rbp), %rdi
leaq -0x168(%rbp), %rsi
callq 0x3ef90
movq %rax, -0x350(%rbp)
jmp 0xae945
movq -0x350(%rbp), %rax
movq %rax, -0x160(%rbp)
movq -0x158(%rbp), %rsi
leaq -0x188(%rbp), %rdi
callq 0x9da10
jmp 0xae968
leaq -0x98(%rbp), %rdi
movq %rdi, -0x368(%rbp)
leaq -0x188(%rbp), %rsi
movq %rsi, -0x370(%rbp)
callq 0x85f0
movq -0x370(%rbp), %rdi
callq 0x8330
movq -0x368(%rbp), %rsi
leaq -0x118(%rbp), %rdi
leaq -0x160(%rbp), %rdx
callq 0x63700
movb %dl, -0x359(%rbp)
movq %rax, -0x358(%rbp)
jmp 0xae9be
movb -0x359(%rbp), %al
movq -0x358(%rbp), %rcx
movq %rcx, -0x198(%rbp)
movb %al, -0x190(%rbp)
leaq -0xc8(%rbp), %rdi
callq 0x3f0c0
movb %al, -0x371(%rbp)
jmp 0xae9ec
movb -0x371(%rbp), %al
xorb $-0x1, %al
testb $0x1, %al
jne 0xae9fd
jmp 0xaf156
leaq -0xc8(%rbp), %rdi
callq 0x3f0f0
movq %rax, -0x380(%rbp)
jmp 0xaea12
movq -0x380(%rbp), %rax
movq (%rax), %rax
movq %rax, -0x158(%rbp)
movq -0x158(%rbp), %rax
movl 0x8(%rax), %eax
cmpl -0x28(%rbp), %eax
jl 0xaea94
movb $0x0, -0x99(%rbp)
jmp 0xaf156
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0xd8(%rbp)
movl %eax, -0xdc(%rbp)
jmp 0xaf243
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0xd8(%rbp)
movl %eax, -0xdc(%rbp)
jmp 0xaf21f
movq -0x338(%rbp), %rdi
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0xd8(%rbp)
movl %eax, -0xdc(%rbp)
movl $0x18, %esi
callq 0x84f0
jmp 0xaf21f
movq -0x308(%rbp), %rax
movq 0x38(%rax), %rsi
movq -0x158(%rbp), %rax
movq (%rax), %rdx
movq -0x20(%rbp), %rcx
movq (%rsi), %rax
movq 0x18(%rax), %rax
leaq -0x1b0(%rbp), %rdi
callq *%rax
jmp 0xaeabf
leaq -0x58(%rbp), %rdi
leaq -0x1b0(%rbp), %rsi
callq 0x3f230
leaq -0x1b0(%rbp), %rdi
callq 0xd7d0
movq -0x158(%rbp), %rax
movslq 0x8(%rax), %rax
movq %rax, -0x388(%rbp)
leaq -0x58(%rbp), %rdi
callq 0x3b0e0
movq %rax, %rcx
movq -0x388(%rbp), %rax
addq %rcx, %rax
subq $0x1, %rax
movslq -0x28(%rbp), %rcx
cmpq %rcx, %rax
jae 0xaeb21
leaq -0x58(%rbp), %rdi
leaq -0x20(%rbp), %rsi
callq 0x3f490
jmp 0xaeb1f
jmp 0xaeadb
jmp 0xaeb23
movq -0x158(%rbp), %rax
movslq 0x8(%rax), %rax
movq %rax, -0x390(%rbp)
leaq -0x58(%rbp), %rdi
callq 0x3b0e0
movq %rax, %rcx
movq -0x390(%rbp), %rax
addq %rcx, %rax
subq $0x1, %rax
movslq -0x28(%rbp), %rcx
cmpq %rcx, %rax
jbe 0xaeb63
leaq -0x58(%rbp), %rdi
callq 0xac5f0
jmp 0xaeb23
movq -0x308(%rbp), %rdi
movl -0x2c(%rbp), %esi
movq -0x158(%rbp), %rax
movl 0x8(%rax), %ecx
movl -0x28(%rbp), %r8d
leaq -0x58(%rbp), %rdx
callq 0xaf410
movb %al, -0x391(%rbp)
jmp 0xaeb8c
movb -0x391(%rbp), %al
testb $0x1, %al
jne 0xaeb9b
jmp 0xaecb9
leaq -0x58(%rbp), %rdi
movq %rdi, -0x3a8(%rbp)
callq 0x3b730
movq %rax, -0x1c0(%rbp)
leaq -0x1b8(%rbp), %rdi
leaq -0x1c0(%rbp), %rsi
callq 0x3f680
movq -0x3a8(%rbp), %rdi
movq -0x1b8(%rbp), %rsi
callq 0x3f610
movq %rax, -0x3a0(%rbp)
jmp 0xaebe1
movq -0x3a0(%rbp), %rax
movq %rax, -0x1c8(%rbp)
leaq -0x58(%rbp), %rax
movq %rax, -0x1d0(%rbp)
movq -0x1d0(%rbp), %rdi
callq 0x3b730
movq %rax, -0x1d8(%rbp)
movq -0x1d0(%rbp), %rdi
callq 0x3b760
movq %rax, -0x1e0(%rbp)
leaq -0x1d8(%rbp), %rdi
leaq -0x1e0(%rbp), %rsi
callq 0x3b790
testb $0x1, %al
jne 0xaec39
jmp 0xaecad
leaq -0x1d8(%rbp), %rdi
callq 0x3bb00
movq (%rax), %rax
movq %rax, -0x1e8(%rbp)
movl $0x18, %edi
callq 0x84e0
movq %rax, -0x3b0(%rbp)
jmp 0xaec62
movq -0x3b0(%rbp), %rax
movq -0x1e8(%rbp), %rcx
movq %rcx, (%rax)
movq -0x158(%rbp), %rcx
movl 0x8(%rcx), %ecx
addl $0x1, %ecx
movl %ecx, 0x8(%rax)
movl $0x0, 0xc(%rax)
movq -0x158(%rbp), %rcx
movq %rcx, 0x10(%rax)
movq %rax, -0x158(%rbp)
leaq -0x1d8(%rbp), %rdi
callq 0x3bad0
jmp 0xaec20
movb $0x0, -0x99(%rbp)
jmp 0xaf156
leaq -0xc8(%rbp), %rdi
callq 0x3f150
jmp 0xaecc7
movq -0x158(%rbp), %rsi
leaq -0x208(%rbp), %rdi
callq 0x9da10
jmp 0xaecdc
leaq -0x150(%rbp), %rdi
leaq -0x208(%rbp), %rsi
callq 0x63750
movb %dl, -0x3b9(%rbp)
movq %rax, -0x3b8(%rbp)
jmp 0xaecfe
movb -0x3b9(%rbp), %al
movq -0x3b8(%rbp), %rcx
movq %rcx, -0x218(%rbp)
movb %al, -0x210(%rbp)
leaq -0x208(%rbp), %rdi
callq 0x8330
movq -0x308(%rbp), %rax
movq 0x38(%rax), %rsi
movq -0x158(%rbp), %rax
movq (%rax), %rdx
leaq -0x230(%rbp), %rdi
callq 0x3c8d0
jmp 0xaed47
leaq -0x70(%rbp), %rdi
movq %rdi, -0x3c8(%rbp)
leaq -0x230(%rbp), %rsi
movq %rsi, -0x3d0(%rbp)
callq 0x3f230
movq -0x3d0(%rbp), %rdi
callq 0xd7d0
movq -0x3c8(%rbp), %rdi
movq -0x158(%rbp), %rsi
callq 0x3f490
jmp 0xaed86
leaq -0x70(%rbp), %rax
movq %rax, -0x238(%rbp)
movq -0x238(%rbp), %rdi
callq 0x3b730
movq %rax, -0x240(%rbp)
movq -0x238(%rbp), %rdi
callq 0x3b760
movq %rax, -0x248(%rbp)
leaq -0x240(%rbp), %rdi
leaq -0x248(%rbp), %rsi
callq 0x3b790
testb $0x1, %al
jne 0xaedd3
jmp 0xaf151
leaq -0x240(%rbp), %rdi
callq 0x3bb00
movq (%rax), %rax
movq %rax, -0x250(%rbp)
movq -0x158(%rbp), %rax
movl 0x8(%rax), %eax
incl %eax
movl %eax, -0x78(%rbp)
movl -0x78(%rbp), %esi
movq -0x250(%rbp), %rdx
leaq -0x270(%rbp), %rdi
callq 0x9d8c0
jmp 0xaee10
leaq -0x98(%rbp), %rdi
movq %rdi, -0x3e0(%rbp)
leaq -0x270(%rbp), %rsi
movq %rsi, -0x3e8(%rbp)
callq 0x85f0
movq -0x3e8(%rbp), %rdi
callq 0x8330
movq -0x3e0(%rbp), %rsi
leaq -0x150(%rbp), %rdi
callq 0x637c0
movq %rax, -0x3d8(%rbp)
jmp 0xaee59
movq -0x3d8(%rbp), %rax
movq %rax, -0x278(%rbp)
leaq -0x150(%rbp), %rdi
callq 0x637f0
movq %rax, -0x280(%rbp)
leaq -0x278(%rbp), %rdi
leaq -0x280(%rbp), %rsi
callq 0x63790
testb $0x1, %al
jne 0xaee93
jmp 0xaeebb
jmp 0xaf140
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0xd8(%rbp)
movl %eax, -0xdc(%rbp)
leaq -0x208(%rbp), %rdi
callq 0x8330
jmp 0xaf21f
movq -0x158(%rbp), %rax
movq (%rax), %rax
movq %rax, -0x2a0(%rbp)
movq -0x250(%rbp), %rax
movq %rax, -0x298(%rbp)
leaq -0x2a0(%rbp), %rax
movq %rax, -0x290(%rbp)
movq $0x2, -0x288(%rbp)
movq -0x290(%rbp), %rsi
movq -0x288(%rbp), %rdx
leaq -0x58(%rbp), %rdi
callq 0x97100
jmp 0xaef0c
movq -0x308(%rbp), %rdi
movl -0x2c(%rbp), %esi
movq -0x158(%rbp), %rax
movl 0x8(%rax), %ecx
movl -0x28(%rbp), %r8d
leaq -0x58(%rbp), %rdx
callq 0xaf410
movb %al, -0x3e9(%rbp)
jmp 0xaef35
movb -0x3e9(%rbp), %al
testb $0x1, %al
jne 0xaef46
jmp 0xaef41
jmp 0xaf140
movq -0x308(%rbp), %rdi
movl -0x78(%rbp), %eax
movl %eax, -0x3f4(%rbp)
movq -0x250(%rbp), %rsi
movq -0x20(%rbp), %rdx
callq 0x9d410
movl %eax, -0x3f0(%rbp)
jmp 0xaef6e
movl -0x3f0(%rbp), %ecx
movl -0x3f4(%rbp), %eax
addl %ecx, %eax
movl %eax, -0x74(%rbp)
leaq -0x118(%rbp), %rdi
leaq -0x98(%rbp), %rsi
callq 0x63820
movq %rax, -0x400(%rbp)
jmp 0xaef9b
movq -0x400(%rbp), %rax
movq %rax, -0x2a8(%rbp)
leaq -0x118(%rbp), %rdi
callq 0x63880
movq %rax, -0x2b0(%rbp)
leaq -0x2a8(%rbp), %rdi
leaq -0x2b0(%rbp), %rsi
callq 0x63850
testb $0x1, %al
jne 0xaefd8
jmp 0xaf0a1
movl $0x18, %edi
callq 0x84e0
movq %rax, -0x408(%rbp)
jmp 0xaefeb
movq -0x408(%rbp), %rax
movq -0x250(%rbp), %rcx
movq %rcx, (%rax)
movl -0x78(%rbp), %ecx
movl %ecx, 0x8(%rax)
movl -0x74(%rbp), %ecx
movl %ecx, 0xc(%rax)
movq -0x158(%rbp), %rcx
movq %rcx, 0x10(%rax)
movq %rax, -0x2b8(%rbp)
movq -0x2b8(%rbp), %rsi
leaq -0x2c8(%rbp), %rdi
callq 0x3f050
jmp 0xaf02f
leaq -0xc8(%rbp), %rdi
leaq -0x2c8(%rbp), %rsi
callq 0x3ef90
movq %rax, -0x410(%rbp)
jmp 0xaf04b
movq -0x410(%rbp), %rax
movq %rax, -0x2c0(%rbp)
leaq -0x118(%rbp), %rdi
leaq -0x98(%rbp), %rsi
leaq -0x2c0(%rbp), %rdx
callq 0x63700
movb %dl, -0x419(%rbp)
movq %rax, -0x418(%rbp)
jmp 0xaf082
movb -0x419(%rbp), %al
movq -0x418(%rbp), %rcx
movq %rcx, -0x2d8(%rbp)
movb %al, -0x2d0(%rbp)
jmp 0xaf13e
leaq -0x2a8(%rbp), %rdi
callq 0x638b0
movq 0x20(%rax), %rax
movq %rax, -0x2e0(%rbp)
leaq -0x2e0(%rbp), %rdi
callq 0x3f3b0
movq %rax, -0x428(%rbp)
jmp 0xaf0cd
movq -0x428(%rbp), %rax
movq (%rax), %rax
movq %rax, -0x2e8(%rbp)
movq -0x2e8(%rbp), %rax
movl 0xc(%rax), %eax
cmpl -0x74(%rbp), %eax
jle 0xaf119
movl -0x78(%rbp), %ecx
movq -0x2e8(%rbp), %rax
movl %ecx, 0x8(%rax)
movl -0x74(%rbp), %ecx
movq -0x2e8(%rbp), %rax
movl %ecx, 0xc(%rax)
movq -0x158(%rbp), %rcx
movq -0x2e8(%rbp), %rax
movq %rcx, 0x10(%rax)
movq -0x2e0(%rbp), %rax
movq %rax, -0x2f0(%rbp)
movq -0x2f0(%rbp), %rsi
leaq -0xc8(%rbp), %rdi
callq 0x3f3e0
jmp 0xaf13c
jmp 0xaf13e
jmp 0xaf140
leaq -0x240(%rbp), %rdi
callq 0x3bad0
jmp 0xaedb7
jmp 0xae9d8
testb $0x1, -0x99(%rbp)
jne 0xaf1d1
jmp 0xaf161
cmpq $0x0, -0x158(%rbp)
je 0xaf194
movq -0x318(%rbp), %rdi
movq -0x158(%rbp), %rsi
callq 0x3f490
jmp 0xaf180
movq -0x158(%rbp), %rax
movq 0x10(%rax), %rax
movq %rax, -0x158(%rbp)
jmp 0xaf161
movq -0x318(%rbp), %rdi
callq 0x3b730
movq -0x318(%rbp), %rdi
movq %rax, -0x2f8(%rbp)
callq 0x3b760
movq %rax, -0x300(%rbp)
movq -0x2f8(%rbp), %rdi
movq -0x300(%rbp), %rsi
callq 0x3f500
jmp 0xaf1cf
jmp 0xaf1d1
movb $0x1, -0x39(%rbp)
leaq -0x150(%rbp), %rdi
callq 0x63930
leaq -0x118(%rbp), %rdi
callq 0x63950
leaq -0xc8(%rbp), %rdi
callq 0x3f580
leaq -0x98(%rbp), %rdi
callq 0x8330
leaq -0x70(%rbp), %rdi
callq 0xd7d0
leaq -0x58(%rbp), %rdi
callq 0xd7d0
testb $0x1, -0x39(%rbp)
jne 0xaf27b
jmp 0xaf26f
leaq -0x150(%rbp), %rdi
callq 0x63930
leaq -0x118(%rbp), %rdi
callq 0x63950
leaq -0xc8(%rbp), %rdi
callq 0x3f580
leaq -0x98(%rbp), %rdi
callq 0x8330
leaq -0x70(%rbp), %rdi
callq 0xd7d0
leaq -0x58(%rbp), %rdi
callq 0xd7d0
movq -0x318(%rbp), %rdi
callq 0xd7d0
jmp 0xaf28b
movq -0x318(%rbp), %rdi
callq 0xd7d0
movq -0x310(%rbp), %rax
addq $0x430, %rsp            # imm = 0x430
popq %rbp
retq
movq -0xd8(%rbp), %rdi
callq 0x8880
nopw (%rax,%rax)