// File: /workspace/github_cpp_makefile_star2_O1/Kei18[P]pibt/./src/solver/ecbs.cpp
// Function: ECBS::solvePart(std::vector<std::vector<Node*, std::allocator<Node*>>, std::allocator<std::vector<Node*, std::allocator<Node*>>>>&, std::vector<Agent*, std::allocator<Agent*>>&)
bool ECBS::solvePart(Paths& paths, Agents& block) {
  CTNode* node;
  Constraints constraints;

  int uuid = 0;
  int key, keyF;
  std::unordered_set<int> OPEN;
  std::vector<int> FOCUL;  // for FOCUL search
  float ub;

  bool status = true;
  bool updateMin = true;

  std::vector<CTNode*> table;
  std::vector<int> table_conflict;

  CTNode* root = new CTNode { {}, {}, 0, nullptr, true, {}, 0 };
  invoke(root, block);
  OPEN.insert(uuid);
  table.push_back(root);
  table_conflict.push_back(h3(root->paths));
  ++uuid;

  bool CAT = std::any_of(paths.begin(), paths.end(),
                         [](Nodes p) { return !p.empty(); });

  auto itrO = OPEN.begin();

  while (!OPEN.empty()) {

    if (updateMin) {
      itrO = std::min_element(OPEN.begin(), OPEN.end(),
                              [CAT, this, &paths, &table]
                              (int a, int b) {
                                CTNode* nA = table[a];
                                CTNode* nB = table[b];
                                if (CAT && nA->cost == nB->cost) {
                                  return this->countCollisions(nA, paths)
                                    < this->countCollisions(nB, paths);
                                }
                                return nA->cost < nB->cost;
                              });
    }

    key = *itrO;
    node = table[key];

    ub = node->LB * w;
    // update focul
    FOCUL.clear();
    for (auto keyO : OPEN) {
      if ((float)table[keyO]->cost <= ub) FOCUL.push_back(keyO);
    }
    auto itrF = std::min_element(FOCUL.begin(), FOCUL.end(),
                                 [&table, &table_conflict]
                                 (int a, int b) {
                                   int sA = table_conflict[a];
                                   int sB = table_conflict[b];
                                   if (sA == sB) {
                                     return table[a]->cost < table[b]->cost;
                                   }
                                   return sA < sB; });

    keyF = *itrF;
    node = table[keyF];

    constraints = valid(node, block);
    if (constraints.empty()) break;

    updateMin = (key == keyF);
    auto itrP = std::find(OPEN.begin(), OPEN.end(), keyF);
    OPEN.erase(itrP);

    for (auto constraint : constraints) {
      CTNode* newNode = new CTNode { constraint, node->paths,
                                     0, node, true, {}, 0 };
      // formating
      Node* g;
      Nodes p;
      for (int i = 0; i < node->paths.size(); ++i) {
        p = newNode->paths[i];
        if (p.empty()) continue;
        g = *(p.end() - 1);
        while (*(p.end() - 1) == g) p.erase(p.end() - 1);
        p.push_back(g);
        node->paths[i] = p;
      }
      newNode->fmins = node->fmins;
      invoke(newNode, block);
      if (newNode->valid) {
        OPEN.insert(uuid);
        table.push_back(newNode);
        table_conflict.push_back(h3(newNode->paths));
        ++uuid;
      }
    }
    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
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0x168, %rsp            # imm = 0x168
movq %rdx, 0xe8(%rsp)
movq %rsi, %r15
movq %rdi, %r12
xorps %xmm0, %xmm0
movaps %xmm0, 0xc0(%rsp)
xorl %eax, %eax
movq %rax, 0xd0(%rsp)
movl $0x0, 0xc(%rsp)
leaq 0x160(%rsp), %rcx
movq %rcx, -0x30(%rcx)
movq $0x1, -0x28(%rcx)
movups %xmm0, -0x20(%rcx)
movl $0x3f800000, -0x10(%rcx) # imm = 0x3F800000
movups %xmm0, -0x8(%rcx)
movq %rax, 0xb0(%rsp)
movaps %xmm0, 0xa0(%rsp)
movq %rax, 0x30(%rsp)
movaps %xmm0, 0x20(%rsp)
movq %rax, 0x90(%rsp)
movaps %xmm0, 0x80(%rsp)
movl $0x68, %edi
callq 0x6370
movq $0x0, 0x38(%rax)
xorps %xmm0, %xmm0
movups %xmm0, (%rax)
movups %xmm0, 0x10(%rax)
movups %xmm0, 0x20(%rax)
movl $0x0, 0x30(%rax)
movb $0x1, 0x40(%rax)
movups %xmm0, 0x48(%rax)
movups %xmm0, 0x54(%rax)
movq %rax, 0xf8(%rsp)
movq (%r12), %rcx
movq %r12, %rdi
movq %rax, %rsi
movq 0xe8(%rsp), %rdx
callq *0x20(%rcx)
leaq 0x130(%rsp), %rdi
leaq 0x60(%rsp), %rdx
movq %rdi, (%rdx)
leaq 0xc(%rsp), %rsi
callq 0x30250
movq 0x28(%rsp), %rsi
cmpq 0x30(%rsp), %rsi
je 0x2d166
movq 0xf8(%rsp), %rax
movq %rax, (%rsi)
addq $0x8, 0x28(%rsp)
jmp 0x2d178
leaq 0x20(%rsp), %rdi
leaq 0xf8(%rsp), %rdx
callq 0x2b9e2
movq 0xf8(%rsp), %rsi
addq $0x18, %rsi
movq %r12, %rdi
callq 0x2da80
movl %eax, 0x60(%rsp)
movq 0x88(%rsp), %rsi
cmpq 0x90(%rsp), %rsi
je 0x2d1b2
movl %eax, (%rsi)
addq $0x4, %rsi
movq %rsi, 0x88(%rsp)
jmp 0x2d1c4
leaq 0x80(%rsp), %rdi
leaq 0x60(%rsp), %rdx
callq 0x233f8
incl 0xc(%rsp)
movq (%r15), %rbx
movq 0x8(%r15), %r14
movq %r14, 0xd8(%rsp)
subq %rbx, %r14
sarq $0x3, %r14
movabsq $-0x5555555555555555, %rax # imm = 0xAAAAAAAAAAAAAAAB
imulq %rax, %r14
sarq $0x2, %r14
testq %r14, %r14
jle 0x2d257
incq %r14
movq %rbx, %rdi
callq 0x2fab2
testb %al, %al
jne 0x2d2c6
leaq 0x18(%rbx), %rdi
movq %rdi, 0x10(%rsp)
callq 0x2fab2
testb %al, %al
jne 0x2d2cb
leaq 0x30(%rbx), %rdi
movq %rdi, 0x10(%rsp)
callq 0x2fab2
testb %al, %al
jne 0x2d2cb
leaq 0x48(%rbx), %rdi
movq %rdi, 0x10(%rsp)
callq 0x2fab2
testb %al, %al
jne 0x2d2cb
addq $0x60, %rbx
decq %r14
cmpq $0x1, %r14
jg 0x2d1f8
movq 0xd8(%rsp), %rax
subq %rbx, %rax
sarq $0x3, %rax
movabsq $-0x5555555555555555, %rcx # imm = 0xAAAAAAAAAAAAAAAB
imulq %rcx, %rax
cmpq $0x1, %rax
je 0x2d2b3
cmpq $0x2, %rax
je 0x2d2a3
movq 0xd8(%rsp), %rcx
movq %rcx, 0x10(%rsp)
cmpq $0x3, %rax
jne 0x2d2cb
movq %rbx, %rdi
callq 0x2fab2
testb %al, %al
jne 0x2d2c6
addq $0x18, %rbx
movq %rbx, %rdi
callq 0x2fab2
testb %al, %al
jne 0x2d2c6
addq $0x18, %rbx
movq %rbx, %rdi
callq 0x2fab2
testb %al, %al
cmoveq 0xd8(%rsp), %rbx
movq %rbx, 0x10(%rsp)
movq %r15, 0xe0(%rsp)
cmpq $0x0, 0x148(%rsp)
je 0x2d8a3
movb $0x1, 0xb(%rsp)
movq 0x140(%rsp), %rcx
leaq 0x40(%rsp), %rbp
movq %r12, 0xf0(%rsp)
testb $0x1, 0xb(%rsp)
movq 0xe0(%rsp), %r14
je 0x2d3a4
movq 0x140(%rsp), %r15
testq %r15, %r15
je 0x2d3a1
movq (%r15), %r12
testq %r12, %r12
je 0x2d3a1
movslq 0x8(%r12), %rax
movslq 0x8(%r15), %rcx
movq 0x20(%rsp), %rdi
movq (%rdi,%rax,8), %rsi
movq (%rdi,%rcx,8), %rbx
movq 0x10(%rsp), %rax
cmpq 0xd8(%rsp), %rax
je 0x2d380
movl 0x30(%rsi), %eax
cmpl 0x30(%rbx), %eax
jne 0x2d380
movq 0xf0(%rsp), %r13
movq %r13, %rdi
movq %r14, %rdx
callq 0x27638
movl %eax, 0x58(%rsp)
movq %r13, %rdi
movq %rbx, %rsi
movq %r14, %rdx
callq 0x27638
cmpl %eax, 0x58(%rsp)
jmp 0x2d386
movl 0x30(%rsi), %eax
cmpl 0x30(%rbx), %eax
setl %al
movq %r12, %rcx
testb %al, %al
jne 0x2d393
movq %r15, %rcx
movq (%r12), %r12
movq %rcx, %r15
testq %r12, %r12
jne 0x2d328
jmp 0x2d3a4
movq %r15, %rcx
movq %rcx, 0x110(%rsp)
movslq 0x8(%rcx), %rbx
movq 0x20(%rsp), %rax
movq (%rax,%rbx,8), %rax
movq 0xa0(%rsp), %rcx
cmpq %rcx, 0xa8(%rsp)
je 0x2d3d3
movq %rcx, 0xa8(%rsp)
movq 0x140(%rsp), %r14
testq %r14, %r14
movq 0xf0(%rsp), %r12
leaq 0xa0(%rsp), %r15
je 0x2d463
xorps %xmm0, %xmm0
cvtsi2ssl 0x60(%rax), %xmm0
mulss 0xa8(%r12), %xmm0
movss %xmm0, 0x58(%rsp)
movslq 0x8(%r14), %rax
movl %eax, 0x60(%rsp)
movq 0x20(%rsp), %rcx
movq (%rcx,%rax,8), %rcx
xorps %xmm0, %xmm0
cvtsi2ssl 0x30(%rcx), %xmm0
movss 0x58(%rsp), %xmm1
ucomiss %xmm0, %xmm1
jb 0x2d45b
movq 0xa8(%rsp), %rsi
cmpq 0xb0(%rsp), %rsi
je 0x2d44e
movl %eax, (%rsi)
addq $0x4, %rsi
movq %rsi, 0xa8(%rsp)
jmp 0x2d45b
movq %r15, %rdi
leaq 0x60(%rsp), %rdx
callq 0x2171e
movq (%r14), %r14
testq %r14, %r14
jne 0x2d408
movq 0xa0(%rsp), %rcx
movq 0xa8(%rsp), %rax
cmpq %rax, %rcx
je 0x2d4cb
leaq 0x4(%rcx), %rdx
cmpq %rax, %rdx
je 0x2d4cb
movq 0x80(%rsp), %rsi
movq 0x20(%rsp), %rdi
movslq (%rdx), %r9
movslq (%rcx), %r8
movl (%rsi,%r8,4), %r10d
cmpl %r10d, (%rsi,%r9,4)
jne 0x2d4ae
movq (%rdi,%r9,8), %r9
movl 0x30(%r9), %r9d
movq (%rdi,%r8,8), %r8
cmpl 0x30(%r8), %r9d
setl %r9b
movq %rdx, %r8
testb %r9b, %r9b
jne 0x2d4bd
movq %rcx, %r8
addq $0x4, %rdx
movq %r8, %rcx
cmpq %rax, %rdx
jne 0x2d48e
jmp 0x2d4ce
movq %rcx, %r8
movslq (%r8), %r14
movq 0x20(%rsp), %rax
movq (%rax,%r14,8), %r13
movq %rbp, %rdi
movq %r12, %rsi
movq %r13, %rdx
movq 0xe8(%rsp), %rcx
callq 0x26d1e
xorps %xmm0, %xmm0
movaps %xmm0, 0x60(%rsp)
movaps 0xc0(%rsp), %xmm0
movq 0xd0(%rsp), %rax
movups 0x40(%rsp), %xmm1
movaps %xmm1, 0xc0(%rsp)
movq 0x50(%rsp), %rcx
movq %rcx, 0xd0(%rsp)
movaps 0x60(%rsp), %xmm1
movaps %xmm0, 0x60(%rsp)
movq %rax, 0x70(%rsp)
movups %xmm1, 0x40(%rsp)
movq $0x0, 0x50(%rsp)
leaq 0x60(%rsp), %rdi
callq 0x2ab2e
movq %rbp, %rdi
callq 0x2ab2e
movq 0xc0(%rsp), %rax
movq 0xc8(%rsp), %rcx
movq %rax, 0x120(%rsp)
movq %rcx, 0x118(%rsp)
cmpq %rcx, %rax
je 0x2d878
cmpl %r14d, %ebx
sete 0xb(%rsp)
movq 0x140(%rsp), %rsi
testq %rsi, %rsi
je 0x2d59a
cmpl %r14d, 0x8(%rsi)
je 0x2d59a
movq (%rsi), %rsi
jmp 0x2d58a
leaq 0x130(%rsp), %rdi
callq 0x302f4
movq 0xc0(%rsp), %r14
movq 0xc8(%rsp), %rax
movq %rax, 0x58(%rsp)
cmpq %rax, %r14
je 0x2d83b
leaq 0x18(%r13), %rbx
leaq 0x48(%r13), %rax
movq %rax, 0x128(%rsp)
leaq 0x60(%rsp), %rdi
movq %r14, %rsi
callq 0x2ab7c
movl $0x68, %edi
callq 0x6370
movq %rax, %r15
movq %rax, %rdi
leaq 0x60(%rsp), %rsi
callq 0x2ab7c
movq %r15, %rdi
addq $0x18, %rdi
movq %rbx, %rsi
callq 0x2abfc
movl $0x0, 0x30(%r15)
movq %r13, 0x38(%r15)
movb $0x1, 0x40(%r15)
xorps %xmm0, %xmm0
movups %xmm0, 0x48(%r15)
movups %xmm0, 0x54(%r15)
movq %r15, 0x18(%rsp)
movaps %xmm0, 0x40(%rsp)
movq $0x0, 0x50(%rsp)
movq 0x20(%r13), %rax
cmpq 0x18(%r13), %rax
je 0x2d71a
xorl %r12d, %r12d
movq 0x18(%rsp), %rax
leaq (%r12,%r12,2), %r15
shlq $0x3, %r15
movq 0x18(%rax), %rsi
addq %r15, %rsi
movq %rbp, %rdi
callq 0x1b2e2
movq 0x48(%rsp), %rax
cmpq %rax, 0x40(%rsp)
je 0x2d6f4
movq -0x8(%rax), %rax
movq %rax, 0x108(%rsp)
movq 0x48(%rsp), %rdx
cmpq %rax, -0x8(%rdx)
jne 0x2d6c1
leaq -0x8(%rdx), %rdi
leaq 0x8(%rdi), %rsi
cmpq %rdx, %rsi
je 0x2d6a1
subq %rsi, %rdx
callq 0x65b0
movq 0x48(%rsp), %rcx
leaq -0x8(%rcx), %rdx
movq %rdx, 0x48(%rsp)
leaq -0x10(%rcx), %rdi
movq 0x108(%rsp), %rax
cmpq %rax, -0x10(%rcx)
je 0x2d690
cmpq 0x50(%rsp), %rdx
je 0x2d6d3
movq %rax, (%rdx)
addq $0x8, 0x48(%rsp)
jmp 0x2d6e6
movq %rbp, %rdi
movq %rdx, %rsi
leaq 0x108(%rsp), %rdx
callq 0x1c69a
addq (%rbx), %r15
movq %r15, %rdi
movq %rbp, %rsi
callq 0x1b2e2
movq 0x20(%r13), %rax
subq 0x18(%r13), %rax
incq %r12
sarq $0x3, %rax
movabsq $-0x5555555555555555, %rcx # imm = 0xAAAAAAAAAAAAAAAB
imulq %rcx, %rax
cmpq %r12, %rax
ja 0x2d64d
movq 0x18(%rsp), %rdi
addq $0x48, %rdi
movq 0x128(%rsp), %rsi
callq 0x2faf2
movq 0xf0(%rsp), %r12
movq 0x18(%rsp), %rsi
movq (%r12), %rax
movq %r12, %rdi
movq 0xe8(%rsp), %rdx
callq *0x20(%rax)
movq 0x18(%rsp), %rax
cmpb $0x1, 0x40(%rax)
jne 0x2d7fe
leaq 0x130(%rsp), %rdi
movq %rdi, 0x100(%rsp)
leaq 0xc(%rsp), %rsi
leaq 0x100(%rsp), %rdx
callq 0x30250
movq 0x28(%rsp), %rsi
cmpq 0x30(%rsp), %rsi
je 0x2d79c
movq 0x18(%rsp), %rax
movq %rax, (%rsi)
addq $0x8, 0x28(%rsp)
jmp 0x2d7ab
leaq 0x20(%rsp), %rdi
leaq 0x18(%rsp), %rdx
callq 0x2b9e2
movq 0x18(%rsp), %rsi
addq $0x18, %rsi
movq %r12, %rdi
callq 0x2da80
movl %eax, 0x100(%rsp)
movq 0x88(%rsp), %rsi
cmpq 0x90(%rsp), %rsi
je 0x2d7e5
movl %eax, (%rsi)
addq $0x4, %rsi
movq %rsi, 0x88(%rsp)
jmp 0x2d7fa
leaq 0x80(%rsp), %rdi
leaq 0x100(%rsp), %rdx
callq 0x233f8
incl 0xc(%rsp)
movq 0x40(%rsp), %rdi
testq %rdi, %rdi
je 0x2d815
movq 0x50(%rsp), %rsi
subq %rdi, %rsi
callq 0x6380
movq 0x60(%rsp), %rdi
testq %rdi, %rdi
je 0x2d82c
movq 0x70(%rsp), %rsi
subq %rdi, %rsi
callq 0x6380
addq $0x18, %r14
cmpq 0x58(%rsp), %r14
jne 0x2d5d5
movq 0xc0(%rsp), %rbx
movq 0xc8(%rsp), %r14
cmpq %rbx, %r14
je 0x2d878
movq %rbx, %r15
movq (%r15), %rdi
testq %rdi, %rdi
je 0x2d867
movq 0x10(%r15), %rsi
subq %rdi, %rsi
callq 0x6380
addq $0x18, %r15
cmpq %r14, %r15
jne 0x2d853
movq %rbx, 0xc8(%rsp)
movq 0x118(%rsp), %rax
cmpq %rax, 0x120(%rsp)
je 0x2d8a3
cmpq $0x0, 0x148(%rsp)
movq 0x110(%rsp), %rcx
jne 0x2d2fc
jmp 0x2d8a3
movq 0x148(%rsp), %rbx
testq %rbx, %rbx
je 0x2d911
movq 0xe0(%rsp), %rax
movq (%rax), %rdi
cmpq %rdi, 0x8(%rax)
je 0x2d911
xorl %r14d, %r14d
xorl %r15d, %r15d
movq 0x18(%r13), %rsi
movq (%rsi,%r14), %rax
cmpq 0x8(%rsi,%r14), %rax
je 0x2d8e1
addq %r14, %rsi
addq %r14, %rdi
callq 0x1b2e2
incq %r15
movq 0xe0(%rsp), %rax
movq (%rax), %rdi
movq 0x8(%rax), %rax
subq %rdi, %rax
sarq $0x3, %rax
movabsq $-0x5555555555555555, %rcx # imm = 0xAAAAAAAAAAAAAAAB
imulq %rcx, %rax
addq $0x18, %r14
cmpq %r15, %rax
ja 0x2d8c7
movq 0x80(%rsp), %rdi
testq %rdi, %rdi
je 0x2d92e
movq 0x90(%rsp), %rsi
subq %rdi, %rsi
callq 0x6380
movq 0x20(%rsp), %rdi
testq %rdi, %rdi
je 0x2d945
movq 0x30(%rsp), %rsi
subq %rdi, %rsi
callq 0x6380
movq 0xa0(%rsp), %rdi
testq %rdi, %rdi
je 0x2d962
movq 0xb0(%rsp), %rsi
subq %rdi, %rsi
callq 0x6380
testq %rbx, %rbx
setne %bl
leaq 0x130(%rsp), %rdi
callq 0x1b8ca
leaq 0xc0(%rsp), %rdi
callq 0x2ab2e
movl %ebx, %eax
addq $0x168, %rsp            # imm = 0x168
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
jmp 0x2d9aa
jmp 0x2d9aa
jmp 0x2d9aa
jmp 0x2d9aa
jmp 0x2d9aa
jmp 0x2d9aa
jmp 0x2d9aa
jmp 0x2d9aa
jmp 0x2d9aa
jmp 0x2d9dc
movq %rax, %rbx
jmp 0x2da0d
movq %rax, %rbx
jmp 0x2d9d0
movq %rax, %rbx
jmp 0x2d9f6
movq %rax, %rbx
movq (%r15), %rdi
testq %rdi, %rdi
je 0x2d9d0
movq 0x10(%r15), %rsi
subq %rdi, %rsi
callq 0x6380
movl $0x68, %esi
movq %r15, %rdi
jmp 0x2d9f1
jmp 0x2d9dc
movq %rax, %rbx
movq 0x40(%rsp), %rdi
testq %rdi, %rdi
je 0x2d9f6
movq 0x50(%rsp), %rsi
subq %rdi, %rsi
callq 0x6380
movq 0x60(%rsp), %rdi
testq %rdi, %rdi
je 0x2da0d
movq 0x70(%rsp), %rsi
subq %rdi, %rsi
callq 0x6380
movq 0x80(%rsp), %rdi
testq %rdi, %rdi
je 0x2da2a
movq 0x90(%rsp), %rsi
subq %rdi, %rsi
callq 0x6380
movq 0x20(%rsp), %rdi
testq %rdi, %rdi
je 0x2da41
movq 0x30(%rsp), %rsi
subq %rdi, %rsi
callq 0x6380
movq 0xa0(%rsp), %rdi
testq %rdi, %rdi
je 0x2da5e
movq 0xb0(%rsp), %rsi
subq %rdi, %rsi
callq 0x6380
leaq 0x130(%rsp), %rdi
callq 0x1b8ca
leaq 0xc0(%rsp), %rdi
callq 0x2ab2e
movq %rbx, %rdi
callq 0x6620