// File: Kei18[P]pibt/./src/solver/cbs.cpp
// Function: CBS::solvePart(std::vector<std::vector<Node*, std::allocator<Node*>>, std::allocator<std::vector<Node*, std::allocator<Node*>>>>&, std::vector<Agent*, std::allocator<Agent*>>&)
bool CBS::solvePart(Paths& paths, Agents& block) {
  CTNode* node;
  Constraints constraints;
  std::vector<CTNode*> OPEN, ALL;
  bool status = true;

  CTNode* root = new CTNode { {}, {}, 0, nullptr, true, {}, 0 };
  invoke(root, block);
  OPEN.push_back(root);
  ALL.push_back(root);
  bool CAT = std::any_of(paths.begin(), paths.end(),
                         [](Nodes p) { return !p.empty(); });

  while (!OPEN.empty()) {
    auto itr = std::min_element(OPEN.begin(), OPEN.end(),
                                [CAT, this, &paths]
                                (CTNode* n1, CTNode* n2) {
                                  if (CAT && n1->cost == n2->cost) {
                                    int c1 = this->countCollisions(n1, paths);
                                    int c2 = this->countCollisions(n2, paths);
                                    return c1 < c2;
                                  }
                                  return n1->cost < n2->cost;
                                });
    node = *itr;
    constraints = valid(node, block);
    if (constraints.empty()) break;
    OPEN.erase(itr);

    for (auto constraint : constraints) {
      CTNode* newNode = new CTNode { constraint, node->paths,
                                     0, node, true,
                                     {}, 0 };

      // it works, but it is late
      // if (isDuplicatedCTNode(newNode, ALL)) continue;

      invoke(newNode, block);
      if (newNode->valid) {
        OPEN.push_back(newNode);
        ALL.push_back(newNode);
      }
    }
    constraints.clear();
  }

  if (!OPEN.empty()) {  // sucssess
    for (int i = 0; i < paths.size(); ++i) {
      if (!node->paths[i].empty()) {
        paths[i] = node->paths[i];
      }
    }
    status = status && true;
  } else {
    status = false;
  }

  return status;
}

// Assembly:
pushq %rbp
movq %rsp, %rbp
subq $0x1c0, %rsp            # imm = 0x1C0
movq %rdi, -0x8(%rbp)
movq %rsi, -0x10(%rbp)
movq %rdx, -0x18(%rbp)
movq -0x8(%rbp), %rax
movq %rax, -0x148(%rbp)
leaq -0x38(%rbp), %rdi
callq 0x628f0
leaq -0x50(%rbp), %rdi
callq 0x62910
leaq -0x68(%rbp), %rdi
callq 0x62910
movb $0x1, -0x69(%rbp)
movl $0x68, %edi
callq 0x84e0
movq %rax, -0x140(%rbp)
jmp 0x58b64
movq -0x140(%rbp), %rdi
xorps %xmm0, %xmm0
movaps %xmm0, -0x160(%rbp)
movaps %xmm0, (%rdi)
movq $0x0, 0x10(%rdi)
callq 0x62930
movaps -0x160(%rbp), %xmm0
movq -0x140(%rbp), %rax
movq %rax, %rdi
addq $0x18, %rdi
movups %xmm0, 0x18(%rax)
movq $0x0, 0x28(%rax)
callq 0xd9c0
movaps -0x160(%rbp), %xmm0
movq -0x140(%rbp), %rax
movl $0x0, 0x30(%rax)
movq $0x0, 0x38(%rax)
movb $0x1, 0x40(%rax)
movq %rax, %rdi
addq $0x48, %rdi
movups %xmm0, 0x48(%rax)
movq $0x0, 0x58(%rax)
callq 0x4e800
movq -0x140(%rbp), %rax
movq -0x148(%rbp), %rdi
movl $0x0, 0x60(%rax)
movq %rax, -0x78(%rbp)
movq -0x78(%rbp), %rsi
movq -0x18(%rbp), %rdx
movq (%rdi), %rax
movq 0x20(%rax), %rax
callq *%rax
jmp 0x58c10
leaq -0x50(%rbp), %rdi
leaq -0x78(%rbp), %rsi
callq 0x62950
jmp 0x58c1f
leaq -0x68(%rbp), %rdi
leaq -0x78(%rbp), %rsi
callq 0x62950
jmp 0x58c2e
movq -0x10(%rbp), %rdi
callq 0x377e0
movq %rax, -0x90(%rbp)
movq -0x10(%rbp), %rdi
callq 0x37500
movq %rax, -0x98(%rbp)
movq -0x90(%rbp), %rdi
movq -0x98(%rbp), %rsi
callq 0x590f0
movb %al, -0x161(%rbp)
jmp 0x58c69
movb -0x161(%rbp), %al
andb $0x1, %al
movb %al, -0x85(%rbp)
leaq -0x50(%rbp), %rdi
callq 0x629c0
xorb $-0x1, %al
testb $0x1, %al
jne 0x58c8b
jmp 0x58fac
leaq -0x50(%rbp), %rdi
movq %rdi, -0x178(%rbp)
callq 0x62a00
movq -0x178(%rbp), %rdi
movq %rax, -0xb0(%rbp)
callq 0x62a30
movq %rax, %rcx
movq -0x148(%rbp), %rax
movq %rcx, -0xb8(%rbp)
movb -0x85(%rbp), %cl
andb $0x1, %cl
movb %cl, -0xd0(%rbp)
movq %rax, -0xc8(%rbp)
movq -0x10(%rbp), %rax
movq %rax, -0xc0(%rbp)
movq -0xb0(%rbp), %rdi
movq -0xb8(%rbp), %rsi
movq -0xc0(%rbp), %rcx
movq %rsp, %rax
movq %rcx, 0x10(%rax)
movups -0xd0(%rbp), %xmm0
movups %xmm0, (%rax)
callq 0x59130
movq %rax, -0x170(%rbp)
jmp 0x58d14
movq -0x170(%rbp), %rax
movq %rax, -0xa8(%rbp)
leaq -0xa8(%rbp), %rdi
callq 0x62a60
movq -0x148(%rbp), %rsi
movq (%rax), %rax
movq %rax, -0x20(%rbp)
movq -0x20(%rbp), %rdx
movq -0x18(%rbp), %rcx
leaq -0xe8(%rbp), %rdi
callq 0x591d0
jmp 0x58d52
leaq -0x38(%rbp), %rdi
leaq -0xe8(%rbp), %rsi
callq 0x62a80
leaq -0xe8(%rbp), %rdi
callq 0x62ab0
leaq -0x38(%rbp), %rdi
callq 0x62b10
testb $0x1, %al
jne 0x58d7d
jmp 0x58d96
jmp 0x58fac
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x80(%rbp)
movl %eax, -0x84(%rbp)
jmp 0x590bf
leaq -0xf0(%rbp), %rdi
leaq -0xa8(%rbp), %rsi
callq 0x62bc0
movq -0xf0(%rbp), %rsi
leaq -0x50(%rbp), %rdi
callq 0x62b50
movq %rax, -0x180(%rbp)
jmp 0x58dc2
movq -0x180(%rbp), %rax
movq %rax, -0xf8(%rbp)
leaq -0x38(%rbp), %rax
movq %rax, -0x100(%rbp)
movq -0x100(%rbp), %rdi
callq 0x62c00
movq %rax, -0x108(%rbp)
movq -0x100(%rbp), %rdi
callq 0x62c30
movq %rax, -0x110(%rbp)
leaq -0x108(%rbp), %rdi
leaq -0x110(%rbp), %rsi
callq 0x62c60
testb $0x1, %al
jne 0x58e1d
jmp 0x58f9e
leaq -0x108(%rbp), %rdi
callq 0x62ca0
movq %rax, %rsi
leaq -0x128(%rbp), %rdi
callq 0x62cc0
jmp 0x58e3a
movl $0x68, %edi
callq 0x84e0
movq %rax, -0x188(%rbp)
jmp 0x58e4d
movq -0x188(%rbp), %rdi
movq %rdi, %rax
movq %rax, -0x190(%rbp)
leaq -0x128(%rbp), %rsi
callq 0x62cc0
jmp 0x58e6c
movq -0x188(%rbp), %rdi
addq $0x18, %rdi
movq -0x20(%rbp), %rsi
addq $0x18, %rsi
callq 0x62db0
jmp 0x58e86
movq -0x188(%rbp), %rax
movl $0x0, 0x30(%rax)
movq -0x20(%rbp), %rcx
movq %rcx, 0x38(%rax)
movb $0x1, 0x40(%rax)
movq %rax, %rdi
addq $0x48, %rdi
xorps %xmm0, %xmm0
movups %xmm0, 0x48(%rax)
movq $0x0, 0x58(%rax)
callq 0x4e800
movq -0x188(%rbp), %rax
movq -0x148(%rbp), %rdi
movl $0x0, 0x60(%rax)
movq %rax, -0x130(%rbp)
movq -0x130(%rbp), %rsi
movq -0x18(%rbp), %rdx
movq (%rdi), %rax
movq 0x20(%rax), %rax
callq *%rax
jmp 0x58eed
movq -0x130(%rbp), %rax
testb $0x1, 0x40(%rax)
je 0x58f70
leaq -0x50(%rbp), %rdi
leaq -0x130(%rbp), %rsi
callq 0x62950
jmp 0x58f0c
leaq -0x68(%rbp), %rdi
leaq -0x130(%rbp), %rsi
callq 0x62950
jmp 0x58f1e
jmp 0x58f70
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x80(%rbp)
movl %eax, -0x84(%rbp)
jmp 0x58f8d
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x80(%rbp)
movl %eax, -0x84(%rbp)
jmp 0x58f5d
movq -0x190(%rbp), %rdi
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x80(%rbp)
movl %eax, -0x84(%rbp)
callq 0x62ea0
movq -0x188(%rbp), %rdi
movl $0x68, %esi
callq 0x84f0
jmp 0x58f8d
leaq -0x128(%rbp), %rdi
callq 0x62ea0
leaq -0x108(%rbp), %rdi
callq 0x62f00
jmp 0x58e01
leaq -0x128(%rbp), %rdi
callq 0x62ea0
jmp 0x590bf
leaq -0x38(%rbp), %rdi
callq 0x62f20
jmp 0x58c77
leaq -0x50(%rbp), %rdi
callq 0x629c0
testb $0x1, %al
jne 0x59086
movl $0x0, -0x134(%rbp)
movslq -0x134(%rbp), %rax
movq %rax, -0x198(%rbp)
movq -0x10(%rbp), %rdi
callq 0x37930
movq %rax, %rcx
movq -0x198(%rbp), %rax
cmpq %rcx, %rax
jae 0x59061
movq -0x20(%rbp), %rdi
addq $0x18, %rdi
movslq -0x134(%rbp), %rsi
callq 0xda70
movq %rax, %rdi
callq 0x3ec00
testb $0x1, %al
jne 0x5904b
movq -0x20(%rbp), %rdi
addq $0x18, %rdi
movslq -0x134(%rbp), %rsi
callq 0xda70
movq %rax, -0x1a0(%rbp)
movq -0x10(%rbp), %rdi
movslq -0x134(%rbp), %rsi
callq 0xda70
movq -0x1a0(%rbp), %rsi
movq %rax, %rdi
callq 0x3eca0
jmp 0x59049
jmp 0x5904b
jmp 0x5904d
movl -0x134(%rbp), %eax
addl $0x1, %eax
movl %eax, -0x134(%rbp)
jmp 0x58fc7
xorl %eax, %eax
testb $0x1, -0x69(%rbp)
movb %al, -0x1a1(%rbp)
je 0x59079
movb $0x1, %al
movb %al, -0x1a1(%rbp)
jmp 0x59079
movb -0x1a1(%rbp), %al
andb $0x1, %al
movb %al, -0x69(%rbp)
jmp 0x5908a
movb $0x0, -0x69(%rbp)
movb -0x69(%rbp), %al
movb %al, -0x1a2(%rbp)
leaq -0x68(%rbp), %rdi
callq 0x62f40
leaq -0x50(%rbp), %rdi
callq 0x62f40
leaq -0x38(%rbp), %rdi
callq 0x62ab0
movb -0x1a2(%rbp), %al
andb $0x1, %al
addq $0x1c0, %rsp            # imm = 0x1C0
popq %rbp
retq
leaq -0x68(%rbp), %rdi
callq 0x62f40
leaq -0x50(%rbp), %rdi
callq 0x62f40
leaq -0x38(%rbp), %rdi
callq 0x62ab0
movq -0x80(%rbp), %rdi
callq 0x8880
nopw %cs:(%rax,%rax)