// File: /workspace/github_cpp_makefile_star2_O3/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 $0x158, %rsp            # imm = 0x158
movq %rdx, 0xf0(%rsp)
movq %rsi, %r14
movq %rdi, %r12
xorps %xmm0, %xmm0
movaps %xmm0, 0xa0(%rsp)
xorl %eax, %eax
movq %rax, 0xb0(%rsp)
movl $0x0, 0xc(%rsp)
leaq 0x150(%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, 0x90(%rsp)
movaps %xmm0, 0x80(%rsp)
movq %rax, 0x30(%rsp)
movaps %xmm0, 0x20(%rsp)
movq %rax, 0x70(%rsp)
movaps %xmm0, 0x60(%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 0xf0(%rsp), %rdx
callq *0x20(%rcx)
leaq 0x120(%rsp), %rdi
leaq 0xc0(%rsp), %rdx
movq %rdi, (%rdx)
leaq 0xc(%rsp), %rsi
callq 0x300d0
movq 0x28(%rsp), %rax
cmpq 0x30(%rsp), %rax
je 0x2cfcf
movq 0xf8(%rsp), %rsi
movq %rsi, (%rax)
addq $0x8, 0x28(%rsp)
jmp 0x2cfec
leaq 0x20(%rsp), %rdi
leaq 0xf8(%rsp), %rdx
movq %rax, %rsi
callq 0x2b89c
movq 0xf8(%rsp), %rsi
addq $0x18, %rsi
movq %r12, %rdi
callq 0x2d8cc
movl %eax, 0xc0(%rsp)
movq 0x68(%rsp), %rsi
cmpq 0x70(%rsp), %rsi
je 0x2d018
movl %eax, (%rsi)
addq $0x4, %rsi
movq %rsi, 0x68(%rsp)
jmp 0x2d02a
leaq 0x60(%rsp), %rdi
leaq 0xc0(%rsp), %rdx
callq 0x23392
incl 0xc(%rsp)
movq (%r14), %rbx
movq %r14, %r13
movq 0x8(%r14), %r14
movq %r14, 0xe0(%rsp)
subq %rbx, %r14
movq %r14, %r15
sarq $0x3, %r15
movabsq $-0x5555555555555555, %rax # imm = 0xAAAAAAAAAAAAAAAB
imulq %rax, %r15
sarq $0x2, %r15
testq %r15, %r15
jle 0x2d0bf
incq %r15
movq %rbx, %rdi
callq 0x2f962
testb %al, %al
jne 0x2d105
leaq 0x18(%rbx), %rdi
movq %rdi, 0x10(%rsp)
callq 0x2f962
testb %al, %al
jne 0x2d10a
leaq 0x30(%rbx), %rdi
movq %rdi, 0x10(%rsp)
callq 0x2f962
testb %al, %al
jne 0x2d10a
leaq 0x48(%rbx), %rdi
movq %rdi, 0x10(%rsp)
callq 0x2f962
testb %al, %al
jne 0x2d10a
addq $0x60, %rbx
decq %r15
addq $-0x60, %r14
cmpq $0x1, %r15
jg 0x2d064
sarq $0x3, %r14
movabsq $-0x5555555555555555, %rax # imm = 0xAAAAAAAAAAAAAAAB
imulq %rax, %r14
cmpq $0x1, %r14
je 0x2d124
cmpq $0x2, %r14
je 0x2d10f
movq 0xe0(%rsp), %rax
movq %rax, 0x10(%rsp)
cmpq $0x3, %r14
movq %r13, %r14
jne 0x2d13f
movq %rbx, %rdi
callq 0x2f962
testb %al, %al
jne 0x2d13a
addq $0x18, %rbx
jmp 0x2d112
movq %rbx, 0x10(%rsp)
movq %r13, %r14
jmp 0x2d13f
movq %r13, %r14
movq %rbx, %rdi
callq 0x2f962
testb %al, %al
jne 0x2d13a
addq $0x18, %rbx
jmp 0x2d127
movq %r13, %r14
movq %rbx, %rdi
callq 0x2f962
testb %al, %al
cmoveq 0xe0(%rsp), %rbx
movq %rbx, 0x10(%rsp)
cmpq $0x0, 0x138(%rsp)
je 0x2d766
movb $0x1, %al
movq 0x130(%rsp), %r13
leaq 0x40(%rsp), %rbp
movq %r14, 0xd8(%rsp)
movq %r12, 0xe8(%rsp)
testb $0x1, %al
je 0x2d205
movq 0x130(%rsp), %rax
movq (%rax), %r15
testq %r15, %r15
je 0x2d202
movq %rax, %rcx
movslq 0x8(%r15), %rax
movq %rcx, 0x38(%rsp)
movslq 0x8(%rcx), %rcx
movq 0x20(%rsp), %rdx
movq (%rdx,%rax,8), %rsi
movq (%rdx,%rcx,8), %rbx
movl 0x30(%rsi), %eax
movl 0x30(%rbx), %ecx
movq 0x10(%rsp), %rdx
cmpq 0xe0(%rsp), %rdx
je 0x2d1e4
cmpl %ecx, %eax
jne 0x2d1e4
movq 0xe8(%rsp), %r13
movq %r13, %rdi
movq %r14, %rdx
callq 0x27512
movl %eax, %r12d
movq %r13, %rdi
movq %rbx, %rsi
movq %r14, %rdx
callq 0x27512
cmpl %eax, %r12d
jmp 0x2d1e6
cmpl %ecx, %eax
setl %al
movq %r15, %r13
testb %al, %al
jne 0x2d1f5
movq 0x38(%rsp), %r13
movq (%r15), %r15
movq %r13, %rax
testq %r15, %r15
jne 0x2d185
jmp 0x2d205
movq %rax, %r13
movslq 0x8(%r13), %rbx
movq 0x20(%rsp), %rax
movq (%rax,%rbx,8), %rcx
movq 0x80(%rsp), %rax
cmpq %rax, 0x88(%rsp)
je 0x2d22c
movq %rax, 0x88(%rsp)
movq 0x130(%rsp), %r14
testq %r14, %r14
movq 0xe8(%rsp), %r12
movq %r13, 0x110(%rsp)
je 0x2d333
xorps %xmm0, %xmm0
cvtsi2ssl 0x60(%rcx), %xmm0
mulss 0xa8(%r12), %xmm0
movss %xmm0, 0x38(%rsp)
movslq 0x8(%r14), %rax
movl %eax, 0xc0(%rsp)
movq 0x20(%rsp), %rcx
movq (%rcx,%rax,8), %rcx
xorps %xmm0, %xmm0
cvtsi2ssl 0x30(%rcx), %xmm0
movss 0x38(%rsp), %xmm1
ucomiss %xmm0, %xmm1
jb 0x2d2c3
movq 0x88(%rsp), %rsi
cmpq 0x90(%rsp), %rsi
je 0x2d2ae
movl %eax, (%rsi)
addq $0x4, %rsi
movq %rsi, 0x88(%rsp)
jmp 0x2d2c3
leaq 0x80(%rsp), %rdi
leaq 0xc0(%rsp), %rdx
callq 0x21698
movq (%r14), %r14
testq %r14, %r14
jne 0x2d265
movq 0x80(%rsp), %rax
movq 0x88(%rsp), %rdx
cmpq %rdx, %rax
je 0x2d333
leaq 0x4(%rax), %rsi
movq 0x20(%rsp), %rcx
cmpq %rdx, %rsi
je 0x2d338
movq 0x60(%rsp), %rdi
movslq (%rsi), %r9
movslq (%rax), %r8
movl (%rdi,%r8,4), %r10d
cmpl %r10d, (%rdi,%r9,4)
jne 0x2d313
movq (%rcx,%r9,8), %r9
movl 0x30(%r9), %r9d
movq (%rcx,%r8,8), %r8
cmpl 0x30(%r8), %r9d
setl %r9b
movq %rsi, %r8
testb %r9b, %r9b
jne 0x2d322
movq %rax, %r8
addq $0x4, %rsi
movq %r8, %rax
cmpq %rdx, %rsi
jne 0x2d2f3
movq %r8, %rax
jmp 0x2d338
movq 0x20(%rsp), %rcx
movslq (%rax), %r14
movq (%rcx,%r14,8), %r13
movq %rbp, %rdi
movq %r12, %rsi
movq %r13, %rdx
movq 0xf0(%rsp), %rcx
callq 0x26c5e
movq 0xb0(%rsp), %rax
movaps 0x40(%rsp), %xmm0
movq 0x50(%rsp), %rcx
movq %rcx, 0xb0(%rsp)
movaps 0xa0(%rsp), %xmm1
movaps %xmm0, 0xa0(%rsp)
movaps %xmm1, 0xc0(%rsp)
movq %rax, 0xd0(%rsp)
xorps %xmm0, %xmm0
movaps %xmm0, 0x40(%rsp)
movq $0x0, 0x50(%rsp)
leaq 0xc0(%rsp), %rdi
callq 0x2aa2e
movq %rbp, %rdi
callq 0x2aa2e
movq 0xa0(%rsp), %rax
cmpq 0xa8(%rsp), %rax
je 0x2d6f0
cmpl %r14d, %ebx
sete %al
movq 0x130(%rsp), %rsi
testq %rsi, %rsi
je 0x2d3e9
cmpl %r14d, 0x8(%rsi)
je 0x2d3eb
movq (%rsi), %rsi
jmp 0x2d3d9
xorl %esi, %esi
movb %al, 0xb(%rsp)
leaq 0x120(%rsp), %rdi
callq 0x30182
movq 0xa0(%rsp), %r14
movq 0xa8(%rsp), %rax
movq %rax, 0x38(%rsp)
cmpq %rax, %r14
je 0x2d6cb
leaq 0x18(%r13), %rbx
leaq 0x48(%r13), %rax
movq %rax, 0x118(%rsp)
leaq 0xc0(%rsp), %rdi
movq %r14, %rsi
callq 0x2aa82
movl $0x68, %edi
callq 0x6370
movq %rax, %r15
movq %rax, %rdi
leaq 0xc0(%rsp), %rsi
callq 0x2aa82
movq %r15, %rdi
addq $0x18, %rdi
movq %rbx, %rsi
callq 0x2ab02
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 0x2d57a
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 0x1b2e0
movq 0x48(%rsp), %rdx
cmpq %rdx, 0x40(%rsp)
je 0x2d547
leaq -0x8(%rdx), %rdi
movq -0x8(%rdx), %rax
movq %rax, 0x108(%rsp)
leaq 0x8(%rdi), %rsi
cmpq %rdx, %rsi
je 0x2d4fe
subq %rsi, %rdx
callq 0x65b0
movq 0x48(%rsp), %rdx
movq 0x108(%rsp), %rax
movq %rdx, %rcx
addq $-0x8, %rdx
movq %rdx, 0x48(%rsp)
leaq -0x10(%rcx), %rdi
cmpq %rax, -0x10(%rcx)
je 0x2d4e0
cmpq 0x50(%rsp), %rdx
je 0x2d526
movq %rax, (%rdx)
addq $0x8, 0x48(%rsp)
jmp 0x2d539
movq %rbp, %rdi
movq %rdx, %rsi
leaq 0x108(%rsp), %rdx
callq 0x1c5cc
addq (%rbx), %r15
movq %r15, %rdi
movq %rbp, %rsi
callq 0x1b2e0
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 0x2d4a8
movq 0x18(%rsp), %r15
movq 0xe8(%rsp), %r12
addq $0x48, %r15
movq %r15, %rdi
movq 0x118(%rsp), %rsi
callq 0x2f9a2
movq 0x18(%rsp), %rsi
movq (%r12), %rax
movq %r12, %rdi
movq 0xf0(%rsp), %rdx
callq *0x20(%rax)
movq 0x18(%rsp), %rax
cmpb $0x1, 0x40(%rax)
jne 0x2d64b
leaq 0x120(%rsp), %rdi
movq %rdi, 0x100(%rsp)
leaq 0xc(%rsp), %rsi
leaq 0x100(%rsp), %rdx
callq 0x300d0
movq 0x28(%rsp), %rax
cmpq 0x30(%rsp), %rax
je 0x2d5f2
movq 0x18(%rsp), %rsi
movq %rsi, (%rax)
addq $0x8, 0x28(%rsp)
jmp 0x2d609
leaq 0x20(%rsp), %rdi
movq %rax, %rsi
leaq 0x18(%rsp), %rdx
callq 0x2b89c
movq 0x18(%rsp), %rsi
addq $0x18, %rsi
movq %r12, %rdi
callq 0x2d8cc
movl %eax, 0x100(%rsp)
movq 0x68(%rsp), %rsi
cmpq 0x70(%rsp), %rsi
je 0x2d635
movl %eax, (%rsi)
addq $0x4, %rsi
movq %rsi, 0x68(%rsp)
jmp 0x2d647
leaq 0x60(%rsp), %rdi
leaq 0x100(%rsp), %rdx
callq 0x23392
incl 0xc(%rsp)
movq 0x40(%rsp), %rdi
testq %rdi, %rdi
je 0x2d662
movq 0x50(%rsp), %rsi
subq %rdi, %rsi
callq 0x6380
movq 0xc0(%rsp), %rdi
testq %rdi, %rdi
je 0x2d67f
movq 0xd0(%rsp), %rsi
subq %rdi, %rsi
callq 0x6380
addq $0x18, %r14
cmpq 0x38(%rsp), %r14
jne 0x2d42a
movq 0xa0(%rsp), %rbx
movq 0xa8(%rsp), %r14
cmpq %rbx, %r14
je 0x2d6cb
movq %rbx, %r15
movq (%r15), %rdi
testq %rdi, %rdi
je 0x2d6ba
movq 0x10(%r15), %rsi
subq %rdi, %rsi
callq 0x6380
addq $0x18, %r15
cmpq %r14, %r15
jne 0x2d6a6
movq %rbx, 0xa8(%rsp)
cmpq $0x0, 0x138(%rsp)
movq 0xd8(%rsp), %r14
movq 0x110(%rsp), %r13
movb 0xb(%rsp), %al
jne 0x2d16d
jmp 0x2d766
cmpq $0x0, 0x138(%rsp)
je 0x2d766
movq 0xd8(%rsp), %rax
movq (%rax), %rdi
movq 0x8(%rax), %rax
movb $0x1, %bl
cmpq %rdi, %rax
je 0x2d768
xorl %r14d, %r14d
xorl %r15d, %r15d
movq 0x18(%r13), %rsi
movq (%rsi,%r14), %rcx
cmpq 0x8(%rsi,%r14), %rcx
je 0x2d740
addq %r14, %rsi
addq %r14, %rdi
callq 0x1b2e0
movq 0xd8(%rsp), %rax
movq (%rax), %rdi
movq 0x8(%rax), %rax
incq %r15
movq %rax, %rcx
subq %rdi, %rcx
sarq $0x3, %rcx
movabsq $-0x5555555555555555, %rdx # imm = 0xAAAAAAAAAAAAAAAB
imulq %rdx, %rcx
addq $0x18, %r14
cmpq %r15, %rcx
ja 0x2d717
jmp 0x2d768
xorl %ebx, %ebx
movq 0x60(%rsp), %rdi
testq %rdi, %rdi
je 0x2d77f
movq 0x70(%rsp), %rsi
subq %rdi, %rsi
callq 0x6380
movq 0x20(%rsp), %rdi
testq %rdi, %rdi
je 0x2d796
movq 0x30(%rsp), %rsi
subq %rdi, %rsi
callq 0x6380
movq 0x80(%rsp), %rdi
testq %rdi, %rdi
je 0x2d7b3
movq 0x90(%rsp), %rsi
subq %rdi, %rsi
callq 0x6380
leaq 0x120(%rsp), %rdi
callq 0x1b8ec
leaq 0xa0(%rsp), %rdi
callq 0x2aa2e
movl %ebx, %eax
addq $0x158, %rsp            # imm = 0x158
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
jmp 0x2d7f5
jmp 0x2d7f5
jmp 0x2d7f5
jmp 0x2d7f5
jmp 0x2d7f5
jmp 0x2d7f5
jmp 0x2d7f5
jmp 0x2d7f5
jmp 0x2d7f5
jmp 0x2d827
movq %rax, %rbx
jmp 0x2d85e
movq %rax, %rbx
jmp 0x2d816
movq %rax, %rbx
movq (%r15), %rdi
testq %rdi, %rdi
je 0x2d816
movq 0x10(%r15), %rsi
subq %rdi, %rsi
callq 0x6380
movl $0x68, %esi
movq %r15, %rdi
jmp 0x2d83c
movq %rax, %rbx
jmp 0x2d841
jmp 0x2d827
movq %rax, %rbx
movq 0x40(%rsp), %rdi
testq %rdi, %rdi
je 0x2d841
movq 0x50(%rsp), %rsi
subq %rdi, %rsi
callq 0x6380
movq 0xc0(%rsp), %rdi
testq %rdi, %rdi
je 0x2d85e
movq 0xd0(%rsp), %rsi
subq %rdi, %rsi
callq 0x6380
movq 0x60(%rsp), %rdi
testq %rdi, %rdi
je 0x2d875
movq 0x70(%rsp), %rsi
subq %rdi, %rsi
callq 0x6380
movq 0x20(%rsp), %rdi
testq %rdi, %rdi
je 0x2d88c
movq 0x30(%rsp), %rsi
subq %rdi, %rsi
callq 0x6380
movq 0x80(%rsp), %rdi
testq %rdi, %rdi
je 0x2d8a9
movq 0x90(%rsp), %rsi
subq %rdi, %rsi
callq 0x6380
leaq 0x120(%rsp), %rdi
callq 0x1b8ec
leaq 0xa0(%rsp), %rdi
callq 0x2aa2e
movq %rbx, %rdi
callq 0x6620
nop