// File: /workspace/github_cpp_makefile_star2_O1/Kei18[P]pibt/./src/solver/tp.cpp
// Function: TP::getPickDelivPath(Agent*, Node*, Node*, Node*, int, bool)
Nodes TP::getPickDelivPath(Agent* a,
                           Node* s,
                           Node* g1,
                           Node* g2,
                           int startTime,
                           bool futureCollision)
{
  Nodes path;
  Nodes C;
  AN2 *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);
  }
  int cost_g1tog2 = pathDist(g1, g2, pathends);

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

  t = startTime - 1;
  l = new AN2 { s, g1, true, startTime - 1,
                pathDist(s, g1) + cost_g1tog2,
                nullptr };
  key = getKey(t, s);
  table.emplace(key, l);
  OPEN = { key };

  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 == n->g && n->g == g2) {
      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) break;

    } else if (n->v == n->g && n->g == g1) {
      n->g = g2;  // next target
    }

    // 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 AN2 { m, n->g, true, t, 100000, n });
      }
      l = table.at(key);
      if (!l->open) continue;

      // cost = pathDist(m, n->g, pathends);
      cost = pathDist(m, n->g);
      f = t + cost;
      if (n->g == g1) f += cost_g1tog2;

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

      OPEN.insert(key);
    }
  }

  // back tracking
  if (n->v == g2) {  // 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 $0x198, %rsp            # imm = 0x198
movq %r9, 0x60(%rsp)
movq %r8, 0x68(%rsp)
movq %rcx, 0x50(%rsp)
xorl %eax, %eax
movq %rax, 0x10(%rdi)
xorps %xmm0, %xmm0
movq %rdi, 0x48(%rsp)
movups %xmm0, (%rdi)
movq %rax, 0xa0(%rsp)
movaps %xmm0, 0x90(%rsp)
leaq 0xc8(%rsp), %rcx
movq %rcx, -0x10(%rcx)
movq %rax, -0x8(%rcx)
movb $0x0, (%rcx)
movq %rax, 0x80(%rsp)
movaps %xmm0, 0x70(%rsp)
movq 0x88(%rsi), %r14
movq %rsi, 0x40(%rsp)
movq 0x90(%rsi), %rbp
cmpq %rbp, %r14
je 0x41270
movq %rdx, %rbx
leaq 0x108(%rsp), %r15
leaq 0x70(%rsp), %r12
leaq 0x158(%rsp), %r13
movq %r15, %rdi
movq %r14, %rsi
callq 0x19a68
movq 0x110(%rsp), %rax
movq -0x8(%rax), %rax
movq %rax, 0x158(%rsp)
cmpq 0x50(%rsp), %rax
je 0x4124a
cmpq 0x8(%rbx), %rax
je 0x4124a
movq 0x78(%rsp), %rsi
cmpq 0x80(%rsp), %rsi
je 0x4123f
movq %rax, (%rsi)
addq $0x8, 0x78(%rsp)
jmp 0x4124a
movq %r12, %rdi
movq %r13, %rdx
callq 0x1c69a
movq 0x108(%rsp), %rdi
testq %rdi, %rdi
je 0x41267
movq 0x118(%rsp), %rsi
subq %rdi, %rsi
callq 0x6380
addq $0x18, %r14
cmpq %rbp, %r14
jne 0x411f9
leaq 0x70(%rsp), %rcx
movq 0x40(%rsp), %rdi
movq 0x68(%rsp), %rsi
movq 0x60(%rsp), %rdx
callq 0x3f52a
movl %eax, 0x5c(%rsp)
leaq 0x138(%rsp), %rax
movq %rax, -0x30(%rax)
movl $0x1, %ecx
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 0x190(%rsp), %rax
movq %rax, -0x30(%rax)
movq %rcx, -0x28(%rax)
movups %xmm0, -0x20(%rax)
movl %edx, -0x10(%rax)
movups %xmm0, -0x8(%rax)
movl $0x28, %edi
callq 0x6370
movq %rax, %r14
movl 0x1d0(%rsp), %ebx
decl %ebx
movq 0x50(%rsp), %rsi
movq %rsi, (%rax)
movq 0x68(%rsp), %rdx
movq %rdx, 0x8(%rax)
movb $0x1, 0x10(%rax)
movl %ebx, 0x14(%rax)
movq 0x40(%rsp), %rdi
callq 0x3f248
addl 0x5c(%rsp), %eax
movl %eax, 0x18(%r14)
movq $0x0, 0x20(%r14)
movq %r14, 0xb0(%rsp)
movq 0x50(%rsp), %rdx
leaq 0x10(%rsp), %rdi
movl %ebx, %esi
callq 0x3f57e
leaq 0xb8(%rsp), %rdi
leaq 0x10(%rsp), %rbx
movq %rbx, %rsi
callq 0x6460
movq (%rbx), %rdi
leaq 0x20(%rsp), %r13
cmpq %r13, %rdi
je 0x41360
movq 0x20(%rsp), %rsi
incq %rsi
callq 0x6380
leaq 0x160(%rsp), %rdi
leaq 0xb8(%rsp), %rsi
leaq 0xb0(%rsp), %rdx
callq 0x436ce
leaq 0x10(%rsp), %rdi
movq %r13, (%rdi)
movq 0xb8(%rsp), %rsi
movq 0xc0(%rsp), %rdx
addq %rsi, %rdx
callq 0xb7d0
leaq 0x108(%rsp), %rdi
leaq 0x10(%rsp), %rsi
movl $0x1, %edx
callq 0x43046
movq 0x10(%rsp), %rdi
cmpq %r13, %rdi
je 0x413cb
movq 0x20(%rsp), %rsi
incq %rsi
callq 0x6380
cmpq $0x0, 0x120(%rsp)
je 0x419b9
movq 0x118(%rsp), %rbx
testq %rbx, %rbx
je 0x414ea
movq (%rbx), %r14
testq %r14, %r14
je 0x414ea
movq %r13, 0x10(%rsp)
movq 0x8(%r14), %rsi
movq 0x10(%r14), %rdx
addq %rsi, %rdx
leaq 0x10(%rsp), %rdi
callq 0xb7d0
leaq 0x160(%rsp), %r12
leaq 0xf8(%rsp), %rax
movq %rax, 0xe8(%rsp)
movq 0x8(%rbx), %rsi
movq 0x10(%rbx), %rdx
addq %rsi, %rdx
leaq 0xe8(%rsp), %rdi
callq 0xb7d0
movq %r12, %rdi
leaq 0x10(%rsp), %rsi
callq 0x43abe
testq %rax, %rax
je 0x41b03
movq 0x28(%rax), %r15
movq %r12, %rdi
leaq 0xe8(%rsp), %rsi
callq 0x43abe
testq %rax, %rax
je 0x41b03
movq 0x28(%rax), %rax
movl 0x18(%rax), %ecx
cmpl %ecx, 0x18(%r15)
jne 0x4148e
movl 0x14(%r15), %ecx
cmpl 0x14(%rax), %ecx
setg %bpl
jmp 0x41492
setl %bpl
movq 0xe8(%rsp), %rdi
leaq 0xf8(%rsp), %rax
cmpq %rax, %rdi
je 0x414b7
movq 0xf8(%rsp), %rsi
incq %rsi
callq 0x6380
movq 0x10(%rsp), %rdi
cmpq %r13, %rdi
je 0x414ce
movq 0x20(%rsp), %rsi
incq %rsi
callq 0x6380
movq %r14, %r12
testb %bpl, %bpl
jne 0x414d9
movq %rbx, %r12
movq (%r14), %r14
movq %r12, %rbx
testq %r14, %r14
jne 0x413f7
jmp 0x414ed
movq %rbx, %r12
leaq 0x8(%r12), %rsi
leaq 0xb8(%rsp), %rdi
callq 0x6200
leaq 0x10(%rsp), %rbx
leaq 0x160(%rsp), %rdi
leaq 0xb8(%rsp), %rsi
callq 0x43abe
testq %rax, %rax
je 0x41b1b
movq 0x28(%rax), %rcx
movq 0x8(%rcx), %rax
movq %rcx, 0x8(%rsp)
movq (%rcx), %rcx
xorq %rax, %rcx
movq %rax, %rdx
xorq 0x60(%rsp), %rdx
orq %rcx, %rdx
jne 0x4162c
cmpb $0x0, 0x1d8(%rsp)
je 0x419b9
movq 0x40(%rsp), %rax
movq 0x88(%rax), %rbp
movq 0x90(%rax), %rbx
cmpq %rbx, %rbp
je 0x419b9
movq 0x8(%rsp), %rax
movslq 0x14(%rax), %rax
leaq 0x1(%rax), %r15
shlq $0x3, %rax
movq %rax, 0x30(%rsp)
movb $0x1, %r14b
leaq 0x10(%rsp), %rdi
movq %rbp, %rsi
callq 0x19a68
movq 0x10(%rsp), %rdi
movq 0x18(%rsp), %rax
movq %rax, %rcx
subq %rdi, %rcx
sarq $0x3, %rcx
movl $0x7, %r13d
cmpq %r15, %rcx
jbe 0x415ed
movq 0x30(%rsp), %rcx
addq %rdi, %rcx
addq $0x8, %rcx
movq 0x8(%rsp), %rsi
cmpq %rax, %rcx
jae 0x415da
movq (%rcx), %rdx
addq $0x8, %rcx
cmpq (%rsi), %rdx
jne 0x415c6
xorl %r14d, %r14d
testb $0x1, %r14b
movl $0x6, %r13d
movl $0x0, %eax
cmovnel %eax, %r13d
testq %rdi, %rdi
je 0x415ff
movq 0x20(%rsp), %rsi
subq %rdi, %rsi
callq 0x6380
cmpl $0x7, %r13d
je 0x4160a
testl %r13d, %r13d
jne 0x41617
addq $0x18, %rbp
cmpq %rbx, %rbp
jne 0x41589
testb $0x1, %r14b
leaq 0x20(%rsp), %r13
leaq 0x10(%rsp), %rbx
je 0x41644
jmp 0x419b9
xorq 0x68(%rsp), %rax
orq %rax, %rcx
jne 0x41644
movq 0x60(%rsp), %rax
movq 0x8(%rsp), %rcx
movq %rax, 0x8(%rcx)
movq 0x8(%rsp), %r14
movb $0x0, 0x10(%r14)
leaq 0x108(%rsp), %rdi
movq %r12, %rsi
callq 0x4341c
movq (%r14), %rax
movq %rax, 0x10(%rsp)
leaq 0x90(%rsp), %rdi
movq %rbx, %rsi
leaq 0x18(%rsp), %rdx
callq 0x3e02c
movq 0x40(%rsp), %rax
movq 0x38(%rax), %rsi
movq (%r14), %rdx
movq %rbx, %rdi
callq 0x1a320
leaq 0xb8(%rsp), %r15
leaq 0x160(%rsp), %r12
movq 0x10(%rsp), %rbx
movq 0x18(%rsp), %r14
cmpq %r14, %rbx
je 0x41722
movq (%rbx), %rax
movq %rax, 0xe0(%rsp)
movq %rax, 0xe8(%rsp)
movq 0x70(%rsp), %rdi
movq 0x78(%rsp), %rsi
leaq 0xe8(%rsp), %rdx
callq 0x19ae8
cmpq 0x78(%rsp), %rax
jne 0x4171c
movq 0x98(%rsp), %rsi
cmpq 0xa0(%rsp), %rsi
je 0x41707
movq 0xe0(%rsp), %rax
movq %rax, (%rsi)
addq $0x8, 0x98(%rsp)
jmp 0x4171c
leaq 0x90(%rsp), %rdi
leaq 0xe0(%rsp), %rdx
callq 0x1c69a
addq $0x8, %rbx
jmp 0x416a9
movq 0x10(%rsp), %rdi
testq %rdi, %rdi
je 0x41739
movq 0x20(%rsp), %rsi
subq %rdi, %rsi
callq 0x6380
movq 0x90(%rsp), %rax
movq 0x98(%rsp), %rcx
movq %rcx, 0x140(%rsp)
cmpq %rcx, %rax
leaq 0x10(%rsp), %rbx
je 0x419a8
movq %rax, 0x148(%rsp)
movq (%rax), %rax
movq %rax, 0x30(%rsp)
movq 0x8(%rsp), %rax
movl 0x14(%rax), %ecx
leal 0x1(%rcx), %eax
movl %eax, 0x3c(%rsp)
movq %rcx, 0xd8(%rsp)
testl %ecx, %ecx
js 0x41873
movq 0x40(%rsp), %rax
movq 0x88(%rax), %r14
movq 0x90(%rax), %rbp
cmpq %rbp, %r14
je 0x41873
movq 0xd8(%rsp), %rax
leaq 0x2(%rax), %r13
movl 0x3c(%rsp), %eax
movq %rax, 0x150(%rsp)
xorl %r12d, %r12d
movq %rbx, %rdi
movq %r14, %rsi
callq 0x19a68
movq 0x10(%rsp), %rdi
movq 0x18(%rsp), %rax
subq %rdi, %rax
sarq $0x3, %rax
movl $0x10, %ebx
cmpq %r13, %rax
jb 0x41825
movq 0x150(%rsp), %rax
movq (%rdi,%rax,8), %rax
movb $0x1, %r15b
movl $0xf, %ebx
cmpq 0x30(%rsp), %rax
je 0x41828
movq 0x8(%rsp), %rcx
cmpq (%rcx), %rax
jne 0x41823
movq 0x30(%rsp), %rax
movq 0xd8(%rsp), %rcx
cmpq %rax, (%rdi,%rcx,8)
je 0x41828
xorl %ebx, %ebx
movl %r12d, %r15d
testq %rdi, %rdi
je 0x4183a
movq 0x20(%rsp), %rsi
subq %rdi, %rsi
callq 0x6380
testb $0xf, %bl
leaq 0x10(%rsp), %rbx
jne 0x41854
addq $0x18, %r14
movl %r15d, %r12d
cmpq %rbp, %r14
jne 0x417c5
testb $0x1, %r15b
leaq 0x20(%rsp), %r13
leaq 0xb8(%rsp), %r15
leaq 0x160(%rsp), %r12
jne 0x4198e
movq %rbx, %rdi
movl 0x3c(%rsp), %ebp
movl %ebp, %esi
movq 0x30(%rsp), %rdx
callq 0x3f57e
movq %r15, %rdi
movq %rbx, %rsi
callq 0x6460
movq 0x10(%rsp), %rdi
cmpq %r13, %rdi
je 0x418a8
movq 0x20(%rsp), %rsi
incq %rsi
callq 0x6380
movq %r12, %rdi
movq %r15, %rsi
callq 0x43abe
testq %rax, %rax
jne 0x418fc
movl $0x28, %edi
callq 0x6370
movq 0x30(%rsp), %rcx
movq %rcx, (%rax)
movq 0x8(%rsp), %rdx
movq 0x8(%rdx), %rcx
movq %rcx, 0x8(%rax)
movb $0x1, 0x10(%rax)
movl %ebp, 0x14(%rax)
movl $0x186a0, 0x18(%rax)    # imm = 0x186A0
movq %rdx, 0x20(%rax)
movq %rax, 0x10(%rsp)
movq %r12, %rdi
movq %r15, %rsi
movq %rbx, %rdx
callq 0x43af8
movq %r12, %rdi
movq %r15, %rsi
callq 0x43abe
testq %rax, %rax
je 0x41b0f
movq 0x28(%rax), %rax
movq %rax, 0xb0(%rsp)
cmpb $0x1, 0x10(%rax)
jne 0x4198e
movq 0x8(%rsp), %rax
movq 0x8(%rax), %rdx
movq 0x40(%rsp), %rdi
movq 0x30(%rsp), %rsi
callq 0x3f248
movl 0x3c(%rsp), %esi
addl %esi, %eax
movq 0x68(%rsp), %rcx
movq 0x8(%rsp), %rdx
cmpq %rcx, 0x8(%rdx)
movl $0x0, %ecx
cmovel 0x5c(%rsp), %ecx
addl %eax, %ecx
movq 0xb0(%rsp), %rax
cmpl %ecx, 0x18(%rax)
jle 0x41976
movl %esi, 0x14(%rax)
movl %ecx, 0x18(%rax)
movq 0x8(%rsp), %rcx
movq %rcx, 0x20(%rax)
leaq 0x108(%rsp), %rdi
movq %rdi, 0x10(%rsp)
movq %r15, %rsi
movq %rbx, %rdx
callq 0x43616
movq 0x148(%rsp), %rax
addq $0x8, %rax
cmpq 0x140(%rsp), %rax
jne 0x4175f
cmpq $0x0, 0x120(%rsp)
jne 0x413da
jmp 0x419b9
movq 0x60(%rsp), %rax
movq 0x8(%rsp), %rdx
cmpq %rax, (%rdx)
jne 0x41a76
cmpq $0x0, 0x20(%rdx)
je 0x41a06
leaq 0x20(%rdx), %rbx
movq 0x48(%rsp), %rdi
movq 0x8(%rdi), %rsi
cmpq 0x10(%rdi), %rsi
je 0x419f3
movq (%rdx), %rax
movq %rax, (%rsi)
addq $0x8, 0x8(%rdi)
jmp 0x419f8
callq 0x1c69a
movq (%rbx), %rdx
leaq 0x20(%rdx), %rbx
cmpq $0x0, 0x20(%rdx)
jne 0x419d7
movq 0x48(%rsp), %rdi
movq 0x8(%rdi), %rsi
cmpq 0x10(%rdi), %rsi
je 0x41a24
movq 0x50(%rsp), %rax
movq %rax, (%rsi)
addq $0x8, 0x8(%rdi)
jmp 0x41a2e
leaq 0x50(%rsp), %rdx
callq 0x1c69a
movq 0x48(%rsp), %rcx
movq (%rcx), %rax
movq 0x8(%rcx), %rcx
cmpq %rcx, %rax
setne %dl
addq $-0x8, %rcx
cmpq %rax, %rcx
seta %sil
andb %dl, %sil
cmpb $0x1, %sil
jne 0x41a76
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 0x41a58
leaq 0x160(%rsp), %rdi
callq 0x429c4
leaq 0x108(%rsp), %rdi
callq 0x2af52
movq 0x70(%rsp), %rdi
testq %rdi, %rdi
je 0x41aaa
movq 0x80(%rsp), %rsi
subq %rdi, %rsi
callq 0x6380
movq 0xb8(%rsp), %rdi
leaq 0xc8(%rsp), %rax
cmpq %rax, %rdi
je 0x41acf
movq 0xc8(%rsp), %rsi
incq %rsi
callq 0x6380
movq 0x90(%rsp), %rdi
testq %rdi, %rdi
je 0x41aec
movq 0xa0(%rsp), %rsi
subq %rdi, %rsi
callq 0x6380
movq 0x48(%rsp), %rax
addq $0x198, %rsp            # imm = 0x198
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
leaq 0x89d1(%rip), %rdi      # 0x4a4db
callq 0x6520
leaq 0x89c5(%rip), %rdi      # 0x4a4db
callq 0x6520
leaq 0x89b9(%rip), %rdi      # 0x4a4db
callq 0x6520
jmp 0x41b8e
jmp 0x41bf3
jmp 0x41bf3
movq %rax, %rbx
movl $0x28, %esi
movq %r14, %rdi
jmp 0x41bea
jmp 0x41b7e
jmp 0x41bf3
jmp 0x41bf3
movq %rax, %rbx
movq 0x108(%rsp), %rdi
testq %rdi, %rdi
je 0x41c10
movq 0x118(%rsp), %rsi
subq %rdi, %rsi
callq 0x6380
jmp 0x41c10
jmp 0x41bf3
jmp 0x41bf3
jmp 0x41bf3
movq %rax, %rbx
jmp 0x41c10
jmp 0x41bf3
jmp 0x41bf3
jmp 0x41bf3
jmp 0x41bf3
movq %rax, %rbx
jmp 0x41bd8
jmp 0x41bf3
jmp 0x41bf3
movq %rax, %rbx
movq 0x10(%rsp), %rdi
testq %rdi, %rdi
je 0x41bf6
movq 0x20(%rsp), %rsi
subq %rdi, %rsi
jmp 0x41bea
jmp 0x41bb0
movq %rax, %rbx
movq 0xe8(%rsp), %rdi
leaq 0xf8(%rsp), %rax
cmpq %rax, %rdi
je 0x41bd8
movq 0xf8(%rsp), %rsi
incq %rsi
callq 0x6380
movq 0x10(%rsp), %rdi
cmpq %r13, %rdi
je 0x41bf6
movq 0x20(%rsp), %rsi
incq %rsi
callq 0x6380
jmp 0x41bf6
jmp 0x41bf3
movq %rax, %rbx
leaq 0x160(%rsp), %rdi
callq 0x429c4
leaq 0x108(%rsp), %rdi
callq 0x2af52
movq 0x70(%rsp), %rdi
testq %rdi, %rdi
je 0x41c2a
movq 0x80(%rsp), %rsi
subq %rdi, %rsi
callq 0x6380
movq 0xb8(%rsp), %rdi
leaq 0xc8(%rsp), %rax
cmpq %rax, %rdi
je 0x41c4f
movq 0xc8(%rsp), %rsi
incq %rsi
callq 0x6380
movq 0x90(%rsp), %rdi
testq %rdi, %rdi
je 0x41c6c
movq 0xa0(%rsp), %rsi
subq %rdi, %rsi
callq 0x6380
movq 0x48(%rsp), %rax
movq (%rax), %rdi
testq %rdi, %rdi
je 0x41c85
movq 0x10(%rax), %rsi
subq %rdi, %rsi
callq 0x6380
movq %rbx, %rdi
callq 0x6620
nop