// File: /workspace/github_cpp_makefile_star2_O2/Kei18[P]pibt/./src/solver/iecbs.cpp
// Function: iECBS::highwayCost(Node*, Node*)
float iECBS::highwayCost(Node* _s, Node* _g) {
  // ==== fast implementation ====
  if (_s == _g) return 0;

  // registered
  int dist = wdists(_s->getIndex(), _g->getIndex());
  if (dist != 0) return dist;
  // =============================

  float f, w, d;
  bool invalid = true;
  Nodes C;
  std::string keyW;

  // prepare node open hashtable
  boost::heap::fibonacci_heap<Fib_ANF> OPEN;
  std::unordered_map<int, boost::heap::fibonacci_heap<Fib_ANF>::handle_type> SEARCHED;
  std::unordered_set<int> CLOSE;
  ANF* n = new ANF { _s, 0, static_cast<float>(pathDist(_s, _g)), nullptr };
  auto handle = OPEN.push(Fib_ANF(n));
  SEARCHED.emplace(n->v->getId(), handle);

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

    // check goal condition
    if (n->v == _g) {
      invalid = false;
      break;
    }

    // ==== fast implementation ====
    dist = wdists(n->v->getIndex(), _g->getIndex());
    if (dist != 0) {
      d = dist;
      // register
      while (n->p != nullptr) {
        dist = wdists(n->v->getIndex(), _g->getIndex());
        keyW = std::to_string(n->p->v->getId()) + "-" + std::to_string(n->v->getId());
        w = highway.at(keyW);
        wdists(n->p->v->getIndex(), _g->getIndex()) = dist + w;
        n = n->p;
      }
      return d;
    }
    // =============================

    // update list
    OPEN.pop();
    CLOSE.emplace(n->v->getId());

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

    for (auto m : C) {
      if (CLOSE.find(m->getId()) != CLOSE.end()) continue;
      keyW = std::to_string(n->v->getId()) + "-" + std::to_string(m->getId());
      w = highway.at(keyW);
      f = n->g + w + pathDist(m, _g);

      // ==== fast implementation ====
      dist = wdists(m->getIndex(), _g->getIndex());
      if (dist != 0) f = n->g + w + dist;
      // =============================

      auto itrS = SEARCHED.find(m->getId());
      if (itrS == SEARCHED.end()) {  // new node
        ANF* l = new ANF { m, n->g + w, f, n };
        auto handle = OPEN.push(Fib_ANF(l));
        SEARCHED.emplace(l->v->getId(), handle);
      } else {
        auto handle = itrS->second;
        ANF* l = (*handle).node;
        if (l->f > f) {
          l->g = n->g + w;
          l->f = f;
          l->p = n;
          OPEN.increase(handle);
        }
      }
    }
  }

  if (invalid) {
    std::cout << "error@iEPIBT::highwayCost, " << "cannot find a path : "
              << _s->getId() << " -> " << _g->getId() << "\n";
    std::exit(1);
  }

  // ==== fast implementation ====
  d = n->g;
  while (n != nullptr) {
    wdists(n->v->getIndex(), _g->getIndex()) = n->g;
    n = n->p;
  }
  // =============================
  return d;
}

// Assembly:
pushq %rbp
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0x1a8, %rsp            # imm = 0x1A8
cmpq %rdx, %rsi
je 0x2a0e1
movq %rdx, %rbx
movq %rsi, %r12
movq %rdi, %r13
leaq 0x148(%rdi), %r15
movslq 0x4(%rsi), %rsi
movslq 0x4(%rdx), %rdx
movq %r15, %rdi
callq 0x2abc6
cvttss2si (%rax), %eax
testl %eax, %eax
je 0x2a0e9
cvtsi2ss %eax, %xmm0
jmp 0x2a699
xorps %xmm0, %xmm0
jmp 0x2a699
andq $0x0, 0x50(%rsp)
leaq 0xe0(%rsp), %rax
movq %rax, -0x10(%rax)
andq $0x0, -0x8(%rax)
xorps %xmm0, %xmm0
movaps %xmm0, 0x40(%rsp)
movb $0x0, (%rax)
leaq 0xb8(%rsp), %rax
movaps %xmm0, -0x18(%rax)
andq $0x0, -0x8(%rax)
movq %rax, (%rax)
movq %rax, 0x8(%rax)
leaq 0x168(%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 0x1a0(%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 %r12, (%rax)
andl $0x0, 0x8(%rax)
movq %r13, %rdi
movq %r12, %rsi
movq %rbx, %rdx
callq 0x32cd4
cvtsi2ss %eax, %xmm0
movss %xmm0, 0xc(%r14)
andq $0x0, 0x10(%r14)
leaq 0x80(%rsp), %rsi
movq %r14, (%rsi)
leaq 0xa0(%rsp), %rdi
callq 0x2ac10
movq %r12, 0x58(%rsp)
movq %r15, 0x20(%rsp)
movq %rbx, 0x18(%rsp)
leaq 0x130(%rsp), %rdx
movq %rax, (%rdx)
movq (%r14), %rax
movl (%rax), %eax
leaq 0x80(%rsp), %rsi
movl %eax, (%rsi)
leaq 0x138(%rsp), %rdi
callq 0x2c1da
movq %r13, 0x38(%rsp)
leaq 0x110(%r13), %rax
movq %rax, 0x30(%rsp)
leaq 0x80(%rsp), %rbp
cmpq $0x0, 0xa0(%rsp)
je 0x2a6ab
leaq 0xa0(%rsp), %rdi
callq 0x2ac88
movq 0x18(%rsp), %rbx
movq 0x20(%rsp), %r15
movq (%rax), %r14
movq (%r14), %rax
cmpq %rbx, %rax
je 0x2a4ee
movslq 0x4(%rax), %rsi
movslq 0x4(%rbx), %rdx
movq %r15, %rdi
callq 0x2abc6
cvttss2si (%rax), %eax
testl %eax, %eax
jne 0x2a532
leaq 0xa0(%rsp), %rdi
callq 0x2acb8
movq (%r14), %rax
movl (%rax), %eax
movl %eax, 0x80(%rsp)
leaq 0x170(%rsp), %rdi
movq %rbp, %rsi
callq 0x18142
movq 0x38(%rsp), %rax
movq 0x38(%rax), %rsi
movq %r14, 0x10(%rsp)
movq (%r14), %rdx
movq %rbp, %rdi
callq 0x164ca
leaq 0x40(%rsp), %rdi
movq %rbp, %rsi
callq 0x18466
movq %rbp, %rdi
callq 0x990e
movq 0x40(%rsp), %r13
movq 0x48(%rsp), %rax
movq %rax, 0x28(%rsp)
cmpq 0x28(%rsp), %r13
je 0x2a1f7
movq (%r13), %rbx
movl (%rbx), %eax
movl %eax, 0x80(%rsp)
leaq 0x170(%rsp), %rdi
movq %rbp, %rsi
callq 0x184b8
testq %rax, %rax
je 0x2a2e1
addq $0x8, %r13
jmp 0x2a2ae
movq 0x10(%rsp), %rax
movq (%rax), %rax
movl (%rax), %esi
leaq 0xf0(%rsp), %r15
movq %r15, %rdi
callq 0x943e
movq %rbp, %r14
leaq 0x60(%rsp), %rdi
movq %r15, %rsi
leaq 0x1405a(%rip), %rdx     # 0x3e367
callq 0x93fb
movl (%rbx), %esi
leaq 0x110(%rsp), %rbp
movq %rbp, %rdi
callq 0x943e
movq %r14, %r12
movq %r14, %rdi
leaq 0x60(%rsp), %r15
movq %r15, %rsi
movq %rbp, %rdx
callq 0x229b6
leaq 0xd0(%rsp), %rdi
movq %r12, %rsi
callq 0x6470
movq %r12, %rdi
callq 0x61d0
movq %rbp, %rdi
callq 0x61d0
movq %r15, %rdi
callq 0x61d0
leaq 0xf0(%rsp), %rdi
callq 0x61d0
movq 0x30(%rsp), %rdi
leaq 0xd0(%rsp), %rsi
callq 0x2c51e
movss (%rax), %xmm0
movss %xmm0, 0x8(%rsp)
movq 0x10(%rsp), %rax
movss 0x8(%rax), %xmm0
movss %xmm0, 0xc(%rsp)
movq 0x38(%rsp), %rdi
movq %rbx, %rsi
movq 0x18(%rsp), %rdx
callq 0x32cd4
movl %eax, %r15d
movslq 0x4(%rbx), %rsi
movq 0x18(%rsp), %rax
movslq 0x4(%rax), %rdx
movq 0x20(%rsp), %rdi
callq 0x2abc6
cvttss2si (%rax), %eax
testl %eax, %eax
je 0x2a3ef
movq 0x10(%rsp), %rcx
movss 0x8(%rcx), %xmm0
addss 0x8(%rsp), %xmm0
cvtsi2ss %eax, %xmm1
addss %xmm0, %xmm1
movss %xmm1, 0xc(%rsp)
jmp 0x2a40a
movss 0xc(%rsp), %xmm1
addss 0x8(%rsp), %xmm1
cvtsi2ss %r15d, %xmm0
addss %xmm1, %xmm0
movss %xmm0, 0xc(%rsp)
movq %r14, %rbp
movl (%rbx), %eax
movl %eax, 0x80(%rsp)
leaq 0x138(%rsp), %rdi
movq %r14, %rsi
callq 0x2c7f8
testq %rax, %rax
je 0x2a477
movq 0x10(%rax), %rsi
movq 0x10(%rsi), %rax
movss 0xc(%rax), %xmm0
movss 0xc(%rsp), %xmm1
ucomiss %xmm1, %xmm0
jbe 0x2a2db
movq 0x10(%rsp), %rcx
movss 0x8(%rsp), %xmm0
addss 0x8(%rcx), %xmm0
movss %xmm0, 0x8(%rax)
movss %xmm1, 0xc(%rax)
movq %rcx, 0x10(%rax)
leaq 0xa0(%rsp), %rdi
callq 0x2ad32
jmp 0x2a2db
pushq $0x18
popq %rdi
callq 0x63b0
movq %rax, %r15
movq %rbx, (%rax)
movq 0x10(%rsp), %rax
movss 0x8(%rsp), %xmm0
addss 0x8(%rax), %xmm0
movss %xmm0, 0x8(%r15)
movss 0xc(%rsp), %xmm0
movss %xmm0, 0xc(%r15)
movq %rax, 0x10(%r15)
movq %r15, 0x60(%rsp)
leaq 0xa0(%rsp), %rdi
leaq 0x60(%rsp), %rbx
movq %rbx, %rsi
callq 0x2ac10
movq %rax, 0x80(%rsp)
movq (%r15), %rax
movl (%rax), %eax
movl %eax, 0x60(%rsp)
leaq 0x138(%rsp), %rdi
movq %rbx, %rsi
movq %rbp, %rdx
callq 0x2c1da
jmp 0x2a2db
movss 0x8(%r14), %xmm0
movss %xmm0, 0x8(%rsp)
testq %r14, %r14
je 0x2a655
movss 0x8(%r14), %xmm0
movss %xmm0, 0x10(%rsp)
movq (%r14), %rax
movslq 0x4(%rax), %rsi
movslq 0x4(%rbx), %rdx
movq %r15, %rdi
callq 0x2abc6
movss 0x10(%rsp), %xmm0
movss %xmm0, (%rax)
movq 0x10(%r14), %r14
jmp 0x2a4fa
cvtsi2ss %eax, %xmm0
leaq 0x60(%rsp), %r15
leaq 0x110(%rsp), %rbp
leaq 0x80(%rsp), %r13
movss %xmm0, 0x8(%rsp)
cmpq $0x0, 0x10(%r14)
je 0x2a655
movq (%r14), %rax
movslq 0x4(%rax), %rsi
movq 0x18(%rsp), %rax
movslq 0x4(%rax), %rdx
movq 0x20(%rsp), %rdi
callq 0x2abc6
cvttss2si (%rax), %eax
movl %eax, 0x10(%rsp)
movq %r14, %rbx
movq 0x10(%r14), %rax
movq (%rax), %rax
movl (%rax), %esi
leaq 0xf0(%rsp), %r14
movq %r14, %rdi
callq 0x943e
movq %r15, %rdi
movq %r14, %rsi
leaq 0x13dc0(%rip), %rdx     # 0x3e367
callq 0x93fb
movq (%rbx), %rax
movl (%rax), %esi
movq %rbp, %rdi
callq 0x943e
movq %r13, %rdi
movq %r15, %rsi
movq %rbp, %rdx
callq 0x229b6
movq %r15, %r12
leaq 0xd0(%rsp), %r15
movq %r15, %rdi
movq %r13, %rsi
callq 0x6470
movq %r13, %rdi
callq 0x61d0
movq %rbp, %rdi
callq 0x61d0
movq %r12, %rdi
callq 0x61d0
movq %r14, %rdi
callq 0x61d0
movq 0x30(%rsp), %rdi
movq %r15, %rsi
callq 0x2c51e
movq %rbx, %r14
movq %r12, %r15
movss (%rax), %xmm0
movss %xmm0, 0x28(%rsp)
movq 0x10(%rbx), %rax
movq (%rax), %rax
movslq 0x4(%rax), %rsi
movq 0x18(%rsp), %rax
movslq 0x4(%rax), %rdx
movq 0x20(%rsp), %rdi
callq 0x2abc6
cvtsi2ssl 0x10(%rsp), %xmm0
movss 0x28(%rsp), %xmm1
addss %xmm0, %xmm1
movss %xmm1, (%rax)
movq 0x10(%r14), %r14
jmp 0x2a551
leaq 0x170(%rsp), %rdi
callq 0x174fe
leaq 0x138(%rsp), %rdi
callq 0x2af32
leaq 0xa0(%rsp), %rdi
callq 0x2ad90
leaq 0xd0(%rsp), %rdi
callq 0x61d0
leaq 0x40(%rsp), %rdi
callq 0x990e
movss 0x8(%rsp), %xmm0
addq $0x1a8, %rsp            # imm = 0x1A8
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
movq 0x2f8e6(%rip), %rdi     # 0x59f98
leaq 0x121f3(%rip), %rsi     # 0x3c8ac
callq 0x63a0
leaq 0x12203(%rip), %rsi     # 0x3c8c8
movq %rax, %rdi
callq 0x63a0
movq 0x58(%rsp), %rcx
movl (%rcx), %esi
movq %rax, %rdi
callq 0x6630
leaq 0x121fb(%rip), %rsi     # 0x3c8de
movq %rax, %rdi
callq 0x63a0
movq 0x18(%rsp), %rcx
movl (%rcx), %esi
movq %rax, %rdi
callq 0x6630
leaq 0xfe62(%rip), %rsi      # 0x3a563
movq %rax, %rdi
callq 0x63a0
pushq $0x1
popq %rdi
callq 0x6450
jmp 0x2a77c
jmp 0x2a77c
movq %rax, %rbx
pushq $0x18
popq %rsi
movq %r14, %rdi
callq 0x63c0
jmp 0x2a77f
jmp 0x2a77c
jmp 0x2a74f
jmp 0x2a77c
jmp 0x2a743
jmp 0x2a748
jmp 0x2a77c
jmp 0x2a77c
jmp 0x2a77c
jmp 0x2a77c
jmp 0x2a77c
jmp 0x2a77c
jmp 0x2a77c
jmp 0x2a77c
jmp 0x2a77c
jmp 0x2a77c
movq %rax, %rbx
jmp 0x2a769
movq %rax, %rbx
jmp 0x2a75f
jmp 0x2a77c
movq %rax, %rbx
leaq 0x110(%rsp), %rdi
callq 0x61d0
leaq 0x60(%rsp), %rdi
callq 0x61d0
leaq 0xf0(%rsp), %rdi
callq 0x61d0
jmp 0x2a77f
jmp 0x2a77c
jmp 0x2a77c
movq %rax, %rbx
leaq 0x170(%rsp), %rdi
callq 0x174fe
leaq 0x138(%rsp), %rdi
callq 0x2af32
leaq 0xa0(%rsp), %rdi
callq 0x2ad90
leaq 0xd0(%rsp), %rdi
callq 0x61d0
leaq 0x40(%rsp), %rdi
callq 0x990e
movq %rbx, %rdi
callq 0x6650
nop