// File: /workspace/github_cpp_makefile_star2_O2/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, 0x40(%rsp)
movq %rsi, %r12
andq $0x0, 0xa0(%rsp)
movq %rdi, %r13
andl $0x0, 0x4(%rsp)
xorps %xmm0, %xmm0
movaps %xmm0, 0x90(%rsp)
leaq 0x150(%rsp), %rax
movq %rax, -0x30(%rax)
movq $0x1, -0x28(%rax)
movups %xmm0, -0x20(%rax)
movl $0x3f800000, -0x10(%rax) # imm = 0x3F800000
movups %xmm0, -0x8(%rax)
movaps %xmm0, 0x70(%rsp)
andq $0x0, 0x80(%rsp)
movaps %xmm0, 0x50(%rsp)
andq $0x0, 0x60(%rsp)
movaps %xmm0, 0xf0(%rsp)
andq $0x0, 0x100(%rsp)
pushq $0x68
popq %rdi
callq 0x63b0
andq $0x0, 0x38(%rax)
xorps %xmm0, %xmm0
movups %xmm0, (%rax)
movups %xmm0, 0x10(%rax)
movups %xmm0, 0x20(%rax)
andl $0x0, 0x30(%rax)
movb $0x1, 0x40(%rax)
movups %xmm0, 0x48(%rax)
movups %xmm0, 0x54(%rax)
movq %rax, 0xb8(%rsp)
movq (%r13), %rcx
movq %r13, %rdi
movq %rax, %rsi
movq 0x40(%rsp), %rdx
callq *0x20(%rcx)
leaq 0x120(%rsp), %rdi
leaq 0x4(%rsp), %rsi
callq 0x27478
leaq 0x50(%rsp), %rdi
leaq 0xb8(%rsp), %rsi
callq 0x22768
movq 0xb8(%rsp), %rsi
addq $0x18, %rsi
movq %r13, %rdi
callq 0x254ce
leaq 0xd8(%rsp), %rsi
movl %eax, (%rsi)
leaq 0xf0(%rsp), %rdi
callq 0x1d402
incl 0x4(%rsp)
movq (%r12), %rbx
movq 0x8(%r12), %r14
movq %r14, 0x30(%rsp)
subq %rbx, %r14
pushq $0x18
popq %rcx
movq %r14, %rax
cqto
idivq %rcx
movq %rax, %r15
sarq $0x2, %r15
testq %r15, %r15
jle 0x24ee3
movq %rbx, %rdi
callq 0x26ba4
testb %al, %al
jne 0x24f3a
leaq 0x18(%rbx), %rdi
movq %rdi, 0x8(%rsp)
callq 0x26ba4
testb %al, %al
jne 0x24f3f
leaq 0x30(%rbx), %rdi
movq %rdi, 0x8(%rsp)
callq 0x26ba4
testb %al, %al
jne 0x24f3f
leaq 0x48(%rbx), %rdi
movq %rdi, 0x8(%rsp)
callq 0x26ba4
testb %al, %al
jne 0x24f3f
addq $0x60, %rbx
decq %r15
addq $-0x60, %r14
jmp 0x24e87
movq %r14, %rax
cqto
pushq $0x18
popq %rcx
idivq %rcx
cmpq $0x1, %rax
je 0x24f2a
cmpq $0x2, %rax
je 0x24f1a
movq 0x30(%rsp), %rcx
movq %rcx, 0x8(%rsp)
cmpq $0x3, %rax
jne 0x24f3f
movq %rbx, %rdi
callq 0x26ba4
testb %al, %al
jne 0x24f3a
addq $0x18, %rbx
movq %rbx, %rdi
callq 0x26ba4
testb %al, %al
jne 0x24f3a
addq $0x18, %rbx
movq %rbx, %rdi
callq 0x26ba4
testb %al, %al
cmoveq 0x30(%rsp), %rbx
movq %rbx, 0x8(%rsp)
movq %r13, 0x10(%rsp)
movq %r12, 0xa8(%rsp)
movb $0x1, %r13b
leaq 0x120(%rsp), %rax
movq 0x10(%rax), %rcx
leaq 0xc0(%rsp), %rbx
movb $0x1, %al
cmpq $0x0, 0x138(%rsp)
je 0x253db
testb $0x1, %al
je 0x25003
movq 0x130(%rsp), %r15
movq %r15, %rcx
movq (%r15), %r15
testq %r15, %r15
je 0x25003
movslq 0x8(%r15), %rax
movq %rcx, 0x38(%rsp)
movslq 0x8(%rcx), %rcx
movq 0x50(%rsp), %rdx
movq (%rdx,%rax,8), %rsi
movq (%rdx,%rcx,8), %r14
movl 0x30(%rsi), %eax
movl 0x30(%r14), %ecx
movq 0x8(%rsp), %rdx
cmpq 0x30(%rsp), %rdx
je 0x24ff0
cmpl %ecx, %eax
jne 0x24ff0
movq 0x10(%rsp), %rbp
movq %rbp, %rdi
movq 0xa8(%rsp), %r12
movq %r12, %rdx
callq 0x202d6
movl %eax, 0x18(%rsp)
movq %rbp, %rdi
movq %r14, %rsi
movq %r12, %rdx
callq 0x202d6
cmpl %eax, 0x18(%rsp)
jmp 0x24ff2
cmpl %ecx, %eax
setl %al
movq %r15, %rcx
testb %al, %al
jne 0x24f87
movq 0x38(%rsp), %rcx
jmp 0x24f87
movq %rcx, 0x38(%rsp)
movslq 0x8(%rcx), %r14
movq 0x50(%rsp), %rax
movq (%rax,%r14,8), %rax
movq 0x70(%rsp), %rcx
cmpq %rcx, 0x78(%rsp)
je 0x25026
movq %rcx, 0x78(%rsp)
cvtsi2ssl 0x60(%rax), %xmm0
movq 0x10(%rsp), %rax
mulss 0xa8(%rax), %xmm0
movss %xmm0, 0x18(%rsp)
leaq 0x130(%rsp), %r15
movq (%r15), %r15
testq %r15, %r15
je 0x25086
movslq 0x8(%r15), %rax
movl %eax, 0xd8(%rsp)
movq 0x50(%rsp), %rcx
movq (%rcx,%rax,8), %rax
cvtsi2ssl 0x30(%rax), %xmm0
movss 0x18(%rsp), %xmm1
ucomiss %xmm0, %xmm1
jb 0x25046
leaq 0x70(%rsp), %rdi
leaq 0xd8(%rsp), %rsi
callq 0x1be2c
jmp 0x25046
movq 0x70(%rsp), %rcx
movq 0x78(%rsp), %rdx
movq 0x50(%rsp), %rax
cmpq %rdx, %rcx
je 0x250e3
movq 0xf0(%rsp), %rsi
leaq 0x4(%rcx), %rdi
cmpq %rdx, %rdi
je 0x250e3
movslq (%rdi), %r9
movslq (%rcx), %r8
movl (%rsi,%r8,4), %r10d
cmpl %r10d, (%rsi,%r9,4)
jne 0x250cb
movq (%rax,%r9,8), %r9
movl 0x30(%r9), %r9d
movq (%rax,%r8,8), %r8
cmpl 0x30(%r8), %r9d
setl %r8b
movq %rdi, %r9
testb %r8b, %r8b
jne 0x250da
movq %rcx, %r9
addq $0x4, %rdi
movq %r9, %rcx
jmp 0x250a6
movslq (%rcx), %rcx
movl %ecx, 0x2c(%rsp)
movq (%rax,%rcx,8), %r12
leaq 0xd8(%rsp), %r15
movq %r15, %rdi
movq 0x10(%rsp), %rsi
movq %r12, %rdx
movq 0x40(%rsp), %rcx
callq 0x1fb86
leaq 0x90(%rsp), %rdi
movq %r15, %rsi
callq 0x2376e
movq %r15, %rdi
callq 0x22786
movq 0x90(%rsp), %rax
cmpq 0x98(%rsp), %rax
je 0x25388
cmpl 0x2c(%rsp), %r14d
sete 0x3(%rsp)
movq 0x130(%rsp), %rdi
xorl %esi, %esi
leaq 0x2c(%rsp), %rdx
callq 0x27551
leaq 0x120(%rsp), %rdi
movq %rax, %rsi
callq 0x27566
movq 0x90(%rsp), %r14
movq 0x98(%rsp), %rax
movq %rax, 0x118(%rsp)
leaq 0x18(%r12), %rbp
leaq 0x48(%r12), %rax
movq %rax, 0x110(%rsp)
cmpq 0x118(%rsp), %r14
je 0x2536d
leaq 0xd8(%rsp), %rdi
movq %r14, %rsi
callq 0x227a8
pushq $0x68
popq %rdi
callq 0x63b0
movq %rax, %r15
movq %rax, %rdi
leaq 0xd8(%rsp), %rsi
callq 0x227a8
movq %r15, %rdi
addq $0x18, %rdi
movq %rbp, %rsi
callq 0x227f6
movq %r14, 0x18(%rsp)
andl $0x0, 0x30(%r15)
movq %r12, 0x38(%r15)
movb $0x1, 0x40(%r15)
xorps %xmm0, %xmm0
movups %xmm0, 0x48(%r15)
movups %xmm0, 0x54(%r15)
movq %r15, 0x20(%rsp)
movaps %xmm0, 0xc0(%rsp)
andq $0x0, 0xd0(%rsp)
xorl %r14d, %r14d
movq 0x20(%r12), %rax
subq 0x18(%r12), %rax
cqto
pushq $0x18
popq %rcx
idivq %rcx
movq 0x20(%rsp), %rdi
cmpq %r14, %rax
jbe 0x252cb
imulq $0x18, %r14, %r15
movq 0x18(%rdi), %rsi
addq %r15, %rsi
movq %rbx, %rdi
callq 0x16f90
movq 0xc8(%rsp), %rsi
cmpq %rsi, 0xc0(%rsp)
je 0x252c3
movq -0x8(%rsi), %rax
movq %rax, 0xb0(%rsp)
movq %rax, %rcx
cmpq %rax, %rcx
jne 0x252a4
testb %r13b, %r13b
jne 0x2528f
leaq -0x8(%rsi), %rdi
movq %rsi, %rdx
subq %rsi, %rdx
callq 0x65c0
movq 0xc8(%rsp), %rsi
movq 0xb0(%rsp), %rax
leaq -0x8(%rsi), %rdx
movq %rdx, 0xc8(%rsp)
movq -0x10(%rsi), %rcx
movq %rdx, %rsi
jmp 0x25266
movq %rbx, %rdi
leaq 0xb0(%rsp), %rsi
callq 0x171da
addq (%rbp), %r15
movq %r15, %rdi
movq %rbx, %rsi
callq 0x16f90
incq %r14
jmp 0x25212
addq $0x48, %rdi
movq 0x110(%rsp), %rsi
callq 0x26c18
movq 0x18(%rsp), %r14
movq 0x20(%rsp), %rsi
movq 0x10(%rsp), %rdi
movq (%rdi), %rax
movq 0x40(%rsp), %rdx
callq *0x20(%rax)
movq 0x20(%rsp), %rax
cmpb $0x1, 0x40(%rax)
jne 0x2534f
leaq 0x120(%rsp), %rdi
leaq 0x4(%rsp), %rsi
callq 0x27478
leaq 0x50(%rsp), %rdi
leaq 0x20(%rsp), %rsi
callq 0x22768
movq 0x20(%rsp), %rsi
addq $0x18, %rsi
movq 0x10(%rsp), %rdi
callq 0x254ce
movl %eax, 0x4c(%rsp)
leaq 0xf0(%rsp), %rdi
leaq 0x4c(%rsp), %rsi
callq 0x1d402
incl 0x4(%rsp)
movq %rbx, %rdi
callq 0x990e
leaq 0xd8(%rsp), %rdi
callq 0x235f6
addq $0x18, %r14
jmp 0x25191
leaq 0x90(%rsp), %rdi
callq 0x22850
movq 0x38(%rsp), %rcx
movb 0x3(%rsp), %al
jmp 0x24f65
cmpq $0x0, 0x138(%rsp)
je 0x253db
xorl %ebx, %ebx
xorl %r14d, %r14d
movq 0xa8(%rsp), %r15
movq (%r15), %rdi
movq 0x8(%r15), %rax
subq %rdi, %rax
cqto
pushq $0x18
popq %rcx
idivq %rcx
cmpq %r14, %rax
jbe 0x253df
movq 0x18(%r12), %rsi
movq (%rsi,%rbx), %rax
cmpq 0x8(%rsi,%rbx), %rax
je 0x253d2
addq %rbx, %rsi
addq %rbx, %rdi
callq 0x16f90
incq %r14
addq $0x18, %rbx
jmp 0x253a0
xorl %ebx, %ebx
jmp 0x253e1
movb $0x1, %bl
leaq 0xf0(%rsp), %rdi
callq 0x1c0a0
leaq 0x50(%rsp), %rdi
callq 0x235c2
leaq 0x70(%rsp), %rdi
callq 0x1c0a0
leaq 0x120(%rsp), %rdi
callq 0x174fe
leaq 0x90(%rsp), %rdi
callq 0x22786
movl %ebx, %eax
addq $0x158, %rsp            # imm = 0x158
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
jmp 0x25444
jmp 0x25444
jmp 0x25444
jmp 0x25444
jmp 0x25444
jmp 0x25444
jmp 0x25444
jmp 0x25444
jmp 0x25444
jmp 0x2546d
movq %rax, %rbx
jmp 0x2548a
movq %rax, %rbx
jmp 0x2547d
movq %rax, %rbx
jmp 0x2545e
movq %rax, %rbx
movq %r15, %rdi
callq 0x235f6
pushq $0x68
popq %rsi
movq %r15, %rdi
callq 0x63c0
jmp 0x2547d
jmp 0x2546d
movq %rax, %rbx
leaq 0xc0(%rsp), %rdi
callq 0x990e
leaq 0xd8(%rsp), %rdi
callq 0x235f6
leaq 0xf0(%rsp), %rdi
callq 0x1c0a0
leaq 0x50(%rsp), %rdi
callq 0x235c2
leaq 0x70(%rsp), %rdi
callq 0x1c0a0
leaq 0x120(%rsp), %rdi
callq 0x174fe
leaq 0x90(%rsp), %rdi
callq 0x22786
movq %rbx, %rdi
callq 0x6650
nop