// File: /workspace/github_cpp_makefile_star2_O2/Kei18[P]pibt/./src/solver/cbs.cpp
// Function: CBS::AstarSearch(Agent*, CTNode*)
Nodes CBS::AstarSearch(Agent* a, CTNode* node) {
  Constraint constraints = getConstraintsForAgent(node, a);

  Node* _s = a->getNode();
  Node* _g = a->getGoal();

  // ==== fast implementation ====
  // constraint free
  if (constraints.empty()) return G->getPath(_s, _g);

  // know path
  std::string cKey = getCKey(_s, _g, constraints);
  auto itrK = knownPaths.find(cKey);
  if (itrK != knownPaths.end()) {
    if (itrK->second.empty()) node->valid = false;
    return itrK->second;
  }

  // previously computed path
  std::string dKey = cKey;
  if (!constraints.empty()) {
    while (*(dKey.end()-1) != '-') dKey.erase(dKey.end()-1);
    dKey.erase(dKey.end()-1);
  }
  Nodes dPath;
  itrK = knownPaths.find(dKey);
  if (itrK != knownPaths.end()) {
    dPath = itrK->second;
  }

  // goal condition
  bool existGoalConstraint = false;
  int timeGoalConstraint = 0;
  for (auto c: constraints) {
    if (c->onNode && c->v == _g && c->t > timeGoalConstraint) {
      existGoalConstraint = true;
      timeGoalConstraint = c->t;
    }
  }
  // =============================

  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, 0, 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 condtion
    if (n->v == _g) {
      if (!existGoalConstraint || timeGoalConstraint < n->g) {
        invalid = false;
        break;
      }
    }

    // ==== fast implementation ====
    tmpPath = G->getPath(n->v, _g);
    if (validShorcut(a, n, _g, constraints, tmpPath)) {
      for (int i = 1; i < tmpPath.size(); ++i) {
        n = new AN { tmpPath[i], 0, 0, n };
      }
      invalid = false;
      break;
    }
    tmpPath.clear();

    // encount previously explored path
    if (!dPath.empty() && n->g <= dPath.size() - 1 && n->v == dPath[n->g]) {
      tmpPath = dPath;
      for (int i = 0; i < n->g; ++i) tmpPath.erase(tmpPath.begin());
      if (validShorcut(a, n, _g, constraints, tmpPath)) {
        for (int i = 1; i < tmpPath.size(); ++i) {
          n = new AN { tmpPath[i], 0, 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;

      // check constraints
      auto constraint = std::find_if(constraints.begin(), constraints.end(),
                                     [a, g, m, n] (Conflict* c) {
                                       if (c->a != a) return false;
                                       if (c->t != g) return false;
                                       if (c->onNode) return c->v == m;
                                       return c->v == m && c->u == n->v;
                                     });
      if (constraint != constraints.end()) continue;
      f = g + pathDist(m, _g);

      // ==== fast implementation ====
      if (!dPath.empty() && g <= dPath.size() - 1 && m == dPath[g]) {
        f = dPath.size() - 1;
      }
      // if (existGoalConstraint) {
      //   f = pathDist(m, _g) + timeGoalConstraint;
      // }
      // =============================

      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());
  } else {
    node->valid = false;
  }
  knownPaths.emplace(cKey, path);

  return path;
}

// Assembly:
pushq %rbp
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0x208, %rsp            # imm = 0x208
movq %rdx, %r15
movq %rsi, %r12
movq %rdi, %rbp
leaq 0x58(%rsp), %r14
movq %r14, %rdi
movq %rcx, 0xb0(%rsp)
movq %rcx, %rdx
movq %r15, %rcx
callq 0x21686
movq 0x8(%r15), %rbx
movq %r15, 0x50(%rsp)
movq 0x10(%r15), %r15
movq (%r14), %rax
cmpq 0x8(%r14), %rax
je 0x20cb5
leaq 0x1b0(%rsp), %rdi
leaq 0x58(%rsp), %r8
movq %rbx, %rdx
movq %r15, %rcx
callq 0x21702
leaq 0x70(%r12), %rdi
leaq 0x1b0(%rsp), %rsi
movq %rdi, 0xa8(%rsp)
callq 0x23f1a
testq %rax, %rax
je 0x20cce
movq %rax, %rcx
addq $0x28, %rax
movq 0x28(%rcx), %rdx
cmpq 0x30(%rcx), %rdx
jne 0x20ca5
movq 0xb0(%rsp), %rcx
movb $0x0, 0x40(%rcx)
movq %rbp, %rdi
movq %rax, %rsi
callq 0x15d3a
jmp 0x2155b
movq 0x38(%r12), %rsi
movq (%rsi), %rax
movq %rbp, %rdi
movq %rbx, %rdx
movq %r15, %rcx
callq *0x18(%rax)
jmp 0x21568
leaq 0x130(%rsp), %rdi
leaq 0x1b0(%rsp), %rsi
callq 0x64e0
movq 0x58(%rsp), %rax
cmpq 0x60(%rsp), %rax
je 0x20d2a
leaq 0x130(%rsp), %r14
movq 0x130(%rsp), %rax
movq 0x138(%rsp), %rcx
leaq (%rax,%rcx), %rsi
decq %rsi
cmpb $0x2d, (%rsi)
je 0x20d1d
movq %r14, %rdi
callq 0x6480
jmp 0x20cf7
leaq 0x130(%rsp), %rdi
callq 0x6480
xorps %xmm0, %xmm0
movaps %xmm0, 0x70(%rsp)
andq $0x0, 0x80(%rsp)
leaq 0x130(%rsp), %rsi
movq 0xa8(%rsp), %rdi
callq 0x23f1a
testq %rax, %rax
je 0x20d66
addq $0x28, %rax
leaq 0x70(%rsp), %rdi
movq %rax, %rsi
callq 0x16f90
movq 0x58(%rsp), %rax
movq 0x60(%rsp), %rcx
movl $0x0, 0x18(%rsp)
movl $0x0, 0x1c(%rsp)
cmpq %rcx, %rax
je 0x20db9
movq (%rax), %rdx
cmpb $0x1, 0x20(%rdx)
jne 0x20db3
cmpq %r15, 0x10(%rdx)
jne 0x20db3
movl 0x8(%rdx), %edx
movl 0x18(%rsp), %edi
cmpl %edi, %edx
setg %sil
cmovgl %edx, %edi
movl %edi, 0x18(%rsp)
movl 0x1c(%rsp), %edx
orb %sil, %dl
movl %edx, 0x1c(%rsp)
addq $0x8, %rax
jmp 0x20d80
xorps %xmm0, %xmm0
andq $0x0, 0x10(%rbp)
movups %xmm0, (%rbp)
andq $0x0, 0x30(%rsp)
andq $0x0, 0x100(%rsp)
leaq 0x98(%rsp), %rax
movq %rax, -0x10(%rax)
andq $0x0, -0x8(%rax)
movaps %xmm0, 0x20(%rsp)
movaps %xmm0, 0xf0(%rsp)
movb $0x0, (%rax)
leaq 0x168(%rsp), %rax
movaps %xmm0, -0x18(%rax)
andq $0x0, -0x8(%rax)
movq %rax, (%rax)
movq %rax, 0x8(%rax)
leaq 0x200(%rsp), %rax
movq %rax, -0x30(%rax)
pushq $0x1
popq %rdx
movq %rdx, -0x28(%rax)
movups %xmm0, -0x20(%rax)
movl $0x3f800000, %ecx       # imm = 0x3F800000
movl %ecx, -0x10(%rax)
movups %xmm0, -0x8(%rax)
leaq 0x1a8(%rsp), %rax
movq %rax, -0x30(%rax)
movq %rdx, -0x28(%rax)
movups %xmm0, -0x20(%rax)
movl %ecx, -0x10(%rax)
movups %xmm0, -0x8(%rax)
pushq $0x18
popq %rdi
callq 0x63b0
movq %rax, %r14
movq %rbx, (%rax)
andl $0x0, 0x8(%rax)
movq %r12, %rdi
movq %rbx, %rsi
movq %r15, %rdx
callq 0x32cd4
movl %eax, 0xc(%r14)
andq $0x0, 0x10(%r14)
leaq 0xd0(%rsp), %rsi
movq %r14, (%rsi)
leaq 0x150(%rsp), %rdi
callq 0x1705a
movq %rax, 0x128(%rsp)
leaq 0xd0(%rsp), %rdi
movq %r14, %rsi
callq 0x32fe8
leaq 0x88(%rsp), %rbx
leaq 0xd0(%rsp), %r14
movq %rbx, %rdi
movq %r14, %rsi
callq 0x6470
movq %r14, %rdi
callq 0x61d0
leaq 0x1d0(%rsp), %rdi
leaq 0x128(%rsp), %rdx
movq %rbx, %rsi
callq 0x23fea
leaq 0xd0(%rsp), %r14
leaq 0x178(%rsp), %r13
movq %rbp, 0x10(%rsp)
movq %r12, 0xb8(%rsp)
movq %r15, 0xc8(%rsp)
cmpq $0x0, 0x150(%rsp)
je 0x2141b
leaq 0x150(%rsp), %rdi
callq 0x170d2
movq (%rax), %rax
movq %rax, 0x8(%rsp)
movq (%rax), %rdx
cmpq %r15, %rdx
jne 0x20f4d
testb $0x1, 0x1c(%rsp)
je 0x214b9
movl 0x18(%rsp), %eax
movq 0x8(%rsp), %rcx
cmpl 0x8(%rcx), %eax
jl 0x214b9
movq 0x38(%r12), %rsi
movq (%rsi), %rax
movq %r14, %rdi
movq %r15, %rcx
callq *0x18(%rax)
leaq 0x20(%rsp), %rbx
movq %rbx, %rdi
movq %r14, %rsi
callq 0x18466
movq %r14, %rdi
callq 0x990e
movq 0x50(%rsp), %rsi
movq 0x8(%rsp), %rdx
movq %r15, %rcx
leaq 0x58(%rsp), %r8
movq %rbx, %r9
callq 0x2180c
testb %al, %al
jne 0x2142c
movq 0x20(%rsp), %rax
cmpq %rax, 0x28(%rsp)
je 0x20fa9
movq %rax, 0x28(%rsp)
movq 0x70(%rsp), %rax
movq 0x78(%rsp), %rdx
cmpq %rdx, %rax
je 0x2104e
movq 0x8(%rsp), %rcx
movslq 0x8(%rcx), %rcx
subq %rax, %rdx
sarq $0x3, %rdx
decq %rdx
cmpq %rcx, %rdx
jb 0x2104e
movq 0x8(%rsp), %rdx
movq (%rdx), %rdx
cmpq (%rax,%rcx,8), %rdx
jne 0x2104e
leaq 0x20(%rsp), %rdi
leaq 0x70(%rsp), %rsi
callq 0x16f90
xorl %ebx, %ebx
movq 0x8(%rsp), %rax
cmpl 0x8(%rax), %ebx
jge 0x2102a
movq 0x20(%rsp), %rdi
movq 0x28(%rsp), %rdx
leaq 0x8(%rdi), %rsi
cmpq %rdx, %rsi
je 0x2101d
subq %rsi, %rdx
callq 0x65c0
movq 0x28(%rsp), %rdx
addq $-0x8, %rdx
movq %rdx, 0x28(%rsp)
incl %ebx
jmp 0x20ff3
movq 0x50(%rsp), %rsi
movq 0x8(%rsp), %rdx
movq %r15, %rcx
leaq 0x58(%rsp), %r8
leaq 0x20(%rsp), %r9
callq 0x2180c
testb %al, %al
jne 0x21470
leaq 0x150(%rsp), %rdi
callq 0x17102
movq %r14, %rdi
movq 0x8(%rsp), %rsi
callq 0x32fe8
movq %r13, %rdi
movq %r14, %rsi
callq 0x243dc
movq %r14, %rdi
callq 0x61d0
movq 0x38(%r12), %rsi
movq 0x8(%rsp), %rbx
movq (%rbx), %rdx
movq %r14, %rdi
callq 0x164ca
leaq 0xf0(%rsp), %rbp
movq %rbp, %rdi
movq %r14, %rsi
callq 0x18466
movq %r14, %rdi
callq 0x990e
movq %rbp, %rdi
movq %rbx, %rsi
movq 0x10(%rsp), %rbp
callq 0x171da
leaq 0x88(%rsp), %rbx
movq 0xf0(%rsp), %rax
movq 0xf8(%rsp), %rcx
movq %rcx, 0x110(%rsp)
cmpq 0x110(%rsp), %rax
je 0x20f04
movq %rax, 0x118(%rsp)
movq (%rax), %rdx
movq 0x8(%rsp), %rax
movslq 0x8(%rax), %rcx
movq %rcx, 0x108(%rsp)
leaq 0x1(%rcx), %rsi
movq %r14, %rdi
movq %rsi, 0x40(%rsp)
movq %rdx, 0x48(%rsp)
callq 0x32f4c
movq %rbx, %rdi
movq %r14, %rsi
callq 0x6470
movq %r14, %rdi
callq 0x61d0
movq %r13, %rdi
movq %rbx, %rsi
callq 0x2477c
testq %rax, %rax
jne 0x21382
movq %r14, %r13
movq 0x58(%rsp), %r12
movq 0x60(%rsp), %rbx
movq 0x50(%rsp), %rax
movq %rax, 0xd0(%rsp)
movq 0x40(%rsp), %rax
movl %eax, 0xd8(%rsp)
movq 0x48(%rsp), %rax
movq %rax, 0xe0(%rsp)
movq 0x8(%rsp), %rax
movq %rax, 0xe8(%rsp)
movq %rbx, 0xc0(%rsp)
subq %r12, %rbx
movq %rbx, %rbp
sarq $0x5, %rbp
testq %rbp, %rbp
jle 0x211fc
movq %r13, %r14
movq %r13, %rdi
movq %r12, %rsi
callq 0x21da8
testb %al, %al
jne 0x2124e
leaq 0x8(%r12), %r15
movq %r14, %rdi
movq %r15, %rsi
callq 0x21da8
testb %al, %al
jne 0x21251
leaq 0x10(%r12), %r15
movq %r14, %rdi
movq %r15, %rsi
callq 0x21da8
testb %al, %al
jne 0x21251
leaq 0x18(%r12), %r15
movq %r14, %rdi
movq %r15, %rsi
callq 0x21da8
testb %al, %al
jne 0x21258
addq $0x20, %r12
decq %rbp
addq $-0x20, %rbx
jmp 0x21194
sarq $0x3, %rbx
cmpq $0x1, %rbx
je 0x213ad
cmpq $0x2, %rbx
movq %r13, %r14
je 0x2139b
cmpq $0x3, %rbx
leaq 0x178(%rsp), %r13
jne 0x213c7
movq %r14, %rdi
movq %r12, %rsi
callq 0x21da8
testb %al, %al
leaq 0x88(%rsp), %rbx
je 0x213e1
movq %r12, %r15
movq 0x10(%rsp), %rbp
jmp 0x21270
movq %r12, %r15
movq 0x10(%rsp), %rbp
jmp 0x21260
movq 0x10(%rsp), %rbp
movq %r13, %r14
leaq 0x178(%rsp), %r13
leaq 0x88(%rsp), %rbx
cmpq 0xc0(%rsp), %r15
movq 0xb8(%rsp), %r12
jne 0x21382
movq %r12, %rdi
movq 0x48(%rsp), %rsi
movq 0xc8(%rsp), %rdx
callq 0x32cd4
movl %eax, %r15d
movq 0x40(%rsp), %rsi
addl %esi, %r15d
movq 0x70(%rsp), %rcx
movq 0x78(%rsp), %rax
cmpq %rax, %rcx
je 0x212de
subq %rcx, %rax
sarq $0x3, %rax
leaq -0x1(%rax), %rdx
cmpq %rsi, %rdx
jb 0x212de
movq 0x48(%rsp), %rdx
movq 0x108(%rsp), %rsi
cmpq 0x8(%rcx,%rsi,8), %rdx
jne 0x212de
decl %eax
movl %eax, %r15d
leaq 0x1d0(%rsp), %rdi
movq %rbx, %rsi
callq 0x247aa
testq %rax, %rax
je 0x21325
movq 0x28(%rax), %rsi
movq 0x10(%rsi), %rax
cmpl %r15d, 0xc(%rax)
jle 0x21316
movq 0x40(%rsp), %rcx
movl %ecx, 0x8(%rax)
movl %r15d, 0xc(%rax)
movq 0x8(%rsp), %rcx
movq %rcx, 0x10(%rax)
leaq 0x150(%rsp), %rdi
callq 0x1717c
jmp 0x21382
pushq $0x18
popq %rdi
callq 0x63b0
movq 0x48(%rsp), %rcx
movq %rcx, (%rax)
movq 0x40(%rsp), %rcx
movl %ecx, 0x8(%rax)
movl %r15d, 0xc(%rax)
movq 0x8(%rsp), %rcx
movq %rcx, 0x10(%rax)
movq %rax, 0x120(%rsp)
leaq 0x150(%rsp), %rdi
leaq 0x120(%rsp), %rsi
callq 0x1705a
movq %rax, 0xd0(%rsp)
leaq 0x1d0(%rsp), %rdi
movq %rbx, %rsi
movq %r14, %rdx
callq 0x23fea
movq 0x118(%rsp), %rax
addq $0x8, %rax
movq 0xc8(%rsp), %r15
jmp 0x210db
leaq 0x178(%rsp), %r13
leaq 0x88(%rsp), %rbx
jmp 0x213e5
movq 0x10(%rsp), %rbp
movq %r13, %r14
leaq 0x178(%rsp), %r13
leaq 0x88(%rsp), %rbx
jmp 0x213fd
movq 0x10(%rsp), %rbp
movq 0xb8(%rsp), %r12
leaq 0x88(%rsp), %rbx
jmp 0x21286
addq $0x8, %r12
movq %r14, %rdi
movq %r12, %rsi
callq 0x21da8
testb %al, %al
movq 0x10(%rsp), %rbp
jne 0x21413
addq $0x8, %r12
movq %r14, %rdi
movq %r12, %rsi
callq 0x21da8
testb %al, %al
cmoveq 0xc0(%rsp), %r12
movq %r12, %r15
jmp 0x21270
movq 0xb0(%rsp), %rax
movb $0x0, 0x40(%rax)
jmp 0x214e1
movq 0x20(%rsp), %r14
movq 0x28(%rsp), %r15
subq %r14, %r15
sarq $0x3, %r15
pushq $0x1
popq %r12
pushq $0x18
popq %rbx
cmpq %r12, %r15
jbe 0x214b9
movq %rbx, %rdi
callq 0x63b0
movq (%r14,%r12,8), %rcx
movq %rcx, (%rax)
andq $0x0, 0x8(%rax)
movq 0x8(%rsp), %rcx
movq %rcx, 0x10(%rax)
incq %r12
movq %rax, 0x8(%rsp)
jmp 0x21444
movq 0x20(%rsp), %r14
movq 0x28(%rsp), %r15
subq %r14, %r15
sarq $0x3, %r15
pushq $0x18
popq %rbx
pushq $0x1
popq %rdx
cmpq %rdx, %r15
jbe 0x214b9
movq %rdx, %r12
movq %rbx, %rdi
callq 0x63b0
movq %r12, %rdx
movq (%r14,%r12,8), %rcx
movq %rcx, (%rax)
andq $0x0, 0x8(%rax)
movq 0x8(%rsp), %rcx
movq %rcx, 0x10(%rax)
incq %rdx
movq %rax, 0x8(%rsp)
jmp 0x21487
movq 0x8(%rsp), %rbx
testq %rbx, %rbx
je 0x214d4
movq %rbp, %rdi
movq %rbx, %rsi
callq 0x171da
movq 0x10(%rbx), %rbx
jmp 0x214be
movq (%rbp), %rdi
movq 0x8(%rbp), %rsi
callq 0x18697
leaq 0x1b0(%rsp), %rsi
movq 0xa8(%rsp), %rdi
movq %rbp, %rdx
callq 0x247d8
leaq 0x178(%rsp), %rdi
callq 0x22af2
leaq 0x1d0(%rsp), %rdi
callq 0x22bb0
leaq 0x150(%rsp), %rdi
callq 0x171f8
leaq 0x88(%rsp), %rdi
callq 0x61d0
leaq 0xf0(%rsp), %rdi
callq 0x990e
leaq 0x20(%rsp), %rdi
callq 0x990e
leaq 0x70(%rsp), %rdi
callq 0x990e
leaq 0x130(%rsp), %rdi
callq 0x61d0
leaq 0x1b0(%rsp), %rdi
callq 0x61d0
leaq 0x58(%rsp), %rdi
callq 0x235f6
movq %rbp, %rax
addq $0x208, %rsp            # imm = 0x208
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
jmp 0x215e9
jmp 0x215a1
jmp 0x215fa
jmp 0x215fa
movq %rax, %rbx
pushq $0x18
popq %rsi
movq %r14, %rdi
callq 0x63c0
jmp 0x215fd
jmp 0x215fa
movq %rax, %rbx
jmp 0x21650
jmp 0x215b9
jmp 0x215fa
jmp 0x215b1
jmp 0x215fa
movq %rax, %rbx
jmp 0x21674
movq %rax, %rbx
jmp 0x21667
jmp 0x215fa
movq %rax, %rbx
movq 0x10(%rsp), %rbp
jmp 0x215fd
movq %rax, %rbx
leaq 0xd0(%rsp), %rdi
callq 0x61d0
jmp 0x215fd
jmp 0x215fa
jmp 0x215fa
jmp 0x215fa
jmp 0x215fa
jmp 0x215fa
movq %rax, %rbx
jmp 0x2165a
jmp 0x215fa
jmp 0x215fa
jmp 0x215fa
jmp 0x215fa
jmp 0x215fa
jmp 0x215fa
movq %rax, %rbx
leaq 0x178(%rsp), %rdi
callq 0x22af2
leaq 0x1d0(%rsp), %rdi
callq 0x22bb0
leaq 0x150(%rsp), %rdi
callq 0x171f8
leaq 0x88(%rsp), %rdi
callq 0x61d0
leaq 0xf0(%rsp), %rdi
callq 0x990e
leaq 0x20(%rsp), %rdi
callq 0x990e
movq %rbp, %rdi
callq 0x990e
leaq 0x70(%rsp), %rdi
callq 0x990e
leaq 0x130(%rsp), %rdi
callq 0x61d0
leaq 0x1b0(%rsp), %rdi
callq 0x61d0
leaq 0x58(%rsp), %rdi
callq 0x235f6
movq %rbx, %rdi
callq 0x6650