// File: 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
movq %rsp, %rbp
subq $0x390, %rsp            # imm = 0x390
movq %rdi, -0x8(%rbp)
movq %rsi, -0x10(%rbp)
movq %rdx, -0x18(%rbp)
movq -0x8(%rbp), %rax
movq %rax, -0x2a8(%rbp)
leaq -0x38(%rbp), %rdi
callq 0x628f0
movl $0x0, -0x3c(%rbp)
leaq -0x80(%rbp), %rdi
callq 0x3ef70
leaq -0x98(%rbp), %rdi
callq 0x4e800
movb $0x1, -0x9d(%rbp)
movb $0x1, -0x9e(%rbp)
leaq -0xb8(%rbp), %rdi
callq 0x62910
leaq -0xd0(%rbp), %rdi
callq 0x4e800
movl $0x68, %edi
callq 0x84e0
movq %rax, -0x2a0(%rbp)
jmp 0x6f750
movq -0x2a0(%rbp), %rdi
xorps %xmm0, %xmm0
movaps %xmm0, -0x2c0(%rbp)
movaps %xmm0, (%rdi)
movq $0x0, 0x10(%rdi)
callq 0x62930
movaps -0x2c0(%rbp), %xmm0
movq -0x2a0(%rbp), %rax
movq %rax, %rdi
addq $0x18, %rdi
movups %xmm0, 0x18(%rax)
movq $0x0, 0x28(%rax)
callq 0xd9c0
movaps -0x2c0(%rbp), %xmm0
movq -0x2a0(%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 -0x2a0(%rbp), %rax
movq -0x2a8(%rbp), %rdi
movl $0x0, 0x60(%rax)
movq %rax, -0xd8(%rbp)
movq -0xd8(%rbp), %rsi
movq -0x18(%rbp), %rdx
movq (%rdi), %rax
movq 0x20(%rax), %rax
callq *%rax
jmp 0x6f802
leaq -0x80(%rbp), %rdi
leaq -0x3c(%rbp), %rsi
callq 0x74250
movb %dl, -0x2c9(%rbp)
movq %rax, -0x2c8(%rbp)
jmp 0x6f81e
movb -0x2c9(%rbp), %al
movq -0x2c8(%rbp), %rcx
movq %rcx, -0xf8(%rbp)
movb %al, -0xf0(%rbp)
leaq -0xb8(%rbp), %rdi
leaq -0xd8(%rbp), %rsi
callq 0x62950
jmp 0x6f84d
movq -0x2a8(%rbp), %rdi
movq -0xd8(%rbp), %rsi
addq $0x18, %rsi
callq 0x70270
movl %eax, -0x2d0(%rbp)
jmp 0x6f86c
movl -0x2d0(%rbp), %eax
movl %eax, -0xfc(%rbp)
leaq -0xd0(%rbp), %rdi
leaq -0xfc(%rbp), %rsi
callq 0x536f0
jmp 0x6f88d
movl -0x3c(%rbp), %eax
incl %eax
movl %eax, -0x3c(%rbp)
movq -0x10(%rbp), %rdi
callq 0x377e0
movq %rax, -0x108(%rbp)
movq -0x10(%rbp), %rdi
callq 0x37500
movq %rax, -0x110(%rbp)
movq -0x108(%rbp), %rdi
movq -0x110(%rbp), %rsi
callq 0x70640
movb %al, -0x2d1(%rbp)
jmp 0x6f8d0
movb -0x2d1(%rbp), %al
andb $0x1, %al
movb %al, -0xfd(%rbp)
leaq -0x80(%rbp), %rdi
callq 0x74290
movq %rax, -0x120(%rbp)
leaq -0x80(%rbp), %rdi
callq 0x742c0
xorb $-0x1, %al
testb $0x1, %al
jne 0x6f902
jmp 0x700ed
testb $0x1, -0x9e(%rbp)
je 0x6f9db
leaq -0x80(%rbp), %rdi
movq %rdi, -0x2e8(%rbp)
callq 0x74290
movq -0x2e8(%rbp), %rdi
movq %rax, -0x130(%rbp)
callq 0x3f2c0
movq %rax, %rcx
movq -0x2a8(%rbp), %rax
movq %rcx, -0x138(%rbp)
movb -0xfd(%rbp), %cl
andb $0x1, %cl
movb %cl, -0x158(%rbp)
movq %rax, -0x150(%rbp)
movq -0x10(%rbp), %rax
movq %rax, -0x148(%rbp)
leaq -0xb8(%rbp), %rax
movq %rax, -0x140(%rbp)
movq -0x130(%rbp), %rdi
movq -0x138(%rbp), %rsi
movups -0x158(%rbp), %xmm0
movups -0x148(%rbp), %xmm1
movq %rsp, %rax
movups %xmm1, 0x10(%rax)
movups %xmm0, (%rax)
callq 0x70680
movq %rax, -0x2e0(%rbp)
jmp 0x6f9a6
movq -0x2e0(%rbp), %rax
movq %rax, -0x128(%rbp)
movq -0x128(%rbp), %rax
movq %rax, -0x120(%rbp)
jmp 0x6f9db
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0xe0(%rbp)
movl %eax, -0xe4(%rbp)
jmp 0x70227
leaq -0x120(%rbp), %rdi
callq 0x742e0
movl (%rax), %eax
movl %eax, -0x40(%rbp)
movslq -0x40(%rbp), %rsi
leaq -0xb8(%rbp), %rdi
callq 0x74310
movq %rax, %rcx
movq -0x2a8(%rbp), %rax
movq (%rcx), %rcx
movq %rcx, -0x20(%rbp)
movq -0x20(%rbp), %rcx
cvtsi2ssl 0x60(%rcx), %xmm0
mulss 0xa8(%rax), %xmm0
movss %xmm0, -0x9c(%rbp)
leaq -0x98(%rbp), %rdi
callq 0x63350
leaq -0x80(%rbp), %rax
movq %rax, -0x160(%rbp)
movq -0x160(%rbp), %rdi
callq 0x74290
movq %rax, -0x168(%rbp)
movq -0x160(%rbp), %rdi
callq 0x3f2c0
movq %rax, -0x170(%rbp)
leaq -0x168(%rbp), %rdi
leaq -0x170(%rbp), %rsi
callq 0x3f260
testb $0x1, %al
jne 0x6fa7c
jmp 0x6fadf
leaq -0x168(%rbp), %rdi
callq 0x742e0
movl (%rax), %eax
movl %eax, -0x174(%rbp)
movslq -0x174(%rbp), %rsi
leaq -0xb8(%rbp), %rdi
callq 0x74310
movq (%rax), %rax
cvtsi2ssl 0x30(%rax), %xmm1
movss -0x9c(%rbp), %xmm0
ucomiss %xmm1, %xmm0
jb 0x6facf
leaq -0x98(%rbp), %rdi
leaq -0x174(%rbp), %rsi
callq 0x4e820
jmp 0x6facd
jmp 0x6facf
jmp 0x6fad1
leaq -0x168(%rbp), %rdi
callq 0x74330
jmp 0x6fa63
leaq -0x98(%rbp), %rdi
movq %rdi, -0x2f8(%rbp)
callq 0x4f9b0
movq -0x2f8(%rbp), %rdi
movq %rax, -0x188(%rbp)
callq 0x4f840
movq %rax, -0x190(%rbp)
leaq -0xb8(%rbp), %rax
movq %rax, -0x1a0(%rbp)
leaq -0xd0(%rbp), %rax
movq %rax, -0x198(%rbp)
movq -0x188(%rbp), %rdi
movq -0x190(%rbp), %rsi
movq -0x1a0(%rbp), %rdx
movq -0x198(%rbp), %rcx
callq 0x70740
movq %rax, -0x2f0(%rbp)
jmp 0x6fb52
movq -0x2f0(%rbp), %rax
movq %rax, -0x180(%rbp)
leaq -0x180(%rbp), %rdi
callq 0x53a50
movl (%rax), %eax
movl %eax, -0x44(%rbp)
movslq -0x44(%rbp), %rsi
leaq -0xb8(%rbp), %rdi
callq 0x74310
movq -0x2a8(%rbp), %rsi
movq (%rax), %rax
movq %rax, -0x20(%rbp)
movq -0x20(%rbp), %rdx
movq -0x18(%rbp), %rcx
leaq -0x1b8(%rbp), %rdi
callq 0x591d0
jmp 0x6fba5
leaq -0x38(%rbp), %rdi
leaq -0x1b8(%rbp), %rsi
callq 0x62a80
leaq -0x1b8(%rbp), %rdi
callq 0x62ab0
leaq -0x38(%rbp), %rdi
callq 0x62b10
testb $0x1, %al
jne 0x6fbd0
jmp 0x6fbd5
jmp 0x700ed
movl -0x40(%rbp), %eax
movl -0x44(%rbp), %ecx
subl %ecx, %eax
sete -0x9e(%rbp)
leaq -0x80(%rbp), %rdi
movq %rdi, -0x308(%rbp)
callq 0x74290
movq -0x308(%rbp), %rdi
movq %rax, -0x1c8(%rbp)
callq 0x3f2c0
movq %rax, -0x1d0(%rbp)
movq -0x1c8(%rbp), %rdi
movq -0x1d0(%rbp), %rsi
leaq -0x44(%rbp), %rdx
callq 0x74360
movq %rax, -0x300(%rbp)
jmp 0x6fc2e
movq -0x300(%rbp), %rax
movq %rax, -0x1c0(%rbp)
movq -0x1c0(%rbp), %rax
movq %rax, -0x1d8(%rbp)
movq -0x1d8(%rbp), %rsi
leaq -0x80(%rbp), %rdi
callq 0x743b0
movq %rax, -0x310(%rbp)
jmp 0x6fc63
movq -0x310(%rbp), %rax
movq %rax, -0x1e0(%rbp)
leaq -0x38(%rbp), %rax
movq %rax, -0x1e8(%rbp)
movq -0x1e8(%rbp), %rdi
callq 0x62c00
movq %rax, -0x1f0(%rbp)
movq -0x1e8(%rbp), %rdi
callq 0x62c30
movq %rax, -0x1f8(%rbp)
leaq -0x1f0(%rbp), %rdi
leaq -0x1f8(%rbp), %rsi
callq 0x62c60
testb $0x1, %al
jne 0x6fcbe
jmp 0x700df
leaq -0x1f0(%rbp), %rdi
callq 0x62ca0
movq %rax, %rsi
leaq -0x210(%rbp), %rdi
callq 0x62cc0
jmp 0x6fcdb
movl $0x68, %edi
callq 0x84e0
movq %rax, -0x318(%rbp)
jmp 0x6fcee
movq -0x318(%rbp), %rdi
movq %rdi, %rax
movq %rax, -0x320(%rbp)
leaq -0x210(%rbp), %rsi
callq 0x62cc0
jmp 0x6fd0d
movq -0x318(%rbp), %rdi
addq $0x18, %rdi
movq -0x20(%rbp), %rsi
addq $0x18, %rsi
callq 0x62db0
jmp 0x6fd27
movq -0x318(%rbp), %rdi
movl $0x0, 0x30(%rdi)
movq -0x20(%rbp), %rax
movq %rax, 0x38(%rdi)
movb $0x1, 0x40(%rdi)
addq $0x48, %rdi
movq %rdi, -0x328(%rbp)
xorl %esi, %esi
movl $0x18, %edx
callq 0x82c0
movq -0x328(%rbp), %rdi
callq 0x4e800
movq -0x318(%rbp), %rax
movl $0x0, 0x60(%rax)
movq %rax, -0x218(%rbp)
leaq -0x238(%rbp), %rdi
callq 0x3e880
movl $0x0, -0x23c(%rbp)
movslq -0x23c(%rbp), %rax
movq %rax, -0x330(%rbp)
movq -0x20(%rbp), %rdi
addq $0x18, %rdi
callq 0x37930
movq %rax, %rcx
movq -0x330(%rbp), %rax
cmpq %rcx, %rax
jae 0x6ffc9
movq -0x218(%rbp), %rdi
addq $0x18, %rdi
movslq -0x23c(%rbp), %rsi
callq 0xda70
movq %rax, %rsi
leaq -0x238(%rbp), %rdi
callq 0x3eca0
jmp 0x6fde5
leaq -0x238(%rbp), %rdi
callq 0x3ec00
testb $0x1, %al
jne 0x6fdfa
jmp 0x6fe81
jmp 0x6ffb5
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0xe0(%rbp)
movl %eax, -0xe4(%rbp)
jmp 0x700ce
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0xe0(%rbp)
movl %eax, -0xe4(%rbp)
jmp 0x6fe48
movq -0x320(%rbp), %rdi
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0xe0(%rbp)
movl %eax, -0xe4(%rbp)
callq 0x62ea0
movq -0x318(%rbp), %rdi
movl $0x68, %esi
callq 0x84f0
jmp 0x700ce
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0xe0(%rbp)
movl %eax, -0xe4(%rbp)
leaq -0x238(%rbp), %rdi
callq 0xd7d0
jmp 0x700ce
leaq -0x238(%rbp), %rdi
callq 0x3b760
movq %rax, -0x250(%rbp)
leaq -0x250(%rbp), %rdi
movl $0x1, %esi
callq 0x4c520
movq %rax, -0x248(%rbp)
leaq -0x248(%rbp), %rdi
callq 0x3bb00
movq (%rax), %rax
movq %rax, -0x220(%rbp)
leaq -0x238(%rbp), %rdi
callq 0x3b760
movq %rax, -0x260(%rbp)
leaq -0x260(%rbp), %rdi
movl $0x1, %esi
callq 0x4c520
movq %rax, -0x258(%rbp)
leaq -0x258(%rbp), %rdi
callq 0x3bb00
movq (%rax), %rax
cmpq -0x220(%rbp), %rax
jne 0x6ff79
leaq -0x238(%rbp), %rdi
movq %rdi, -0x340(%rbp)
callq 0x3b760
movq %rax, -0x278(%rbp)
leaq -0x278(%rbp), %rdi
movl $0x1, %esi
callq 0x4c520
movq %rax, -0x270(%rbp)
leaq -0x268(%rbp), %rdi
leaq -0x270(%rbp), %rsi
callq 0x3f680
movq -0x340(%rbp), %rdi
movq -0x268(%rbp), %rsi
callq 0x3f610
movq %rax, -0x338(%rbp)
jmp 0x6ff66
movq -0x338(%rbp), %rax
movq %rax, -0x280(%rbp)
jmp 0x6fec2
leaq -0x238(%rbp), %rdi
leaq -0x220(%rbp), %rsi
callq 0x3f490
jmp 0x6ff8e
movq -0x20(%rbp), %rdi
addq $0x18, %rdi
movslq -0x23c(%rbp), %rsi
callq 0xda70
movq %rax, %rdi
leaq -0x238(%rbp), %rsi
callq 0x3eca0
jmp 0x6ffb3
jmp 0x6ffb5
movl -0x23c(%rbp), %eax
addl $0x1, %eax
movl %eax, -0x23c(%rbp)
jmp 0x6fd8f
movq -0x20(%rbp), %rsi
addq $0x48, %rsi
movq -0x218(%rbp), %rdi
addq $0x48, %rdi
callq 0x743f0
jmp 0x6ffe3
movq -0x2a8(%rbp), %rdi
movq -0x218(%rbp), %rsi
movq -0x18(%rbp), %rdx
movq (%rdi), %rax
movq 0x20(%rax), %rax
callq *%rax
jmp 0x70000
movq -0x218(%rbp), %rax
testb $0x1, 0x40(%rax)
je 0x700a5
leaq -0x80(%rbp), %rdi
leaq -0x3c(%rbp), %rsi
callq 0x74250
movb %dl, -0x349(%rbp)
movq %rax, -0x348(%rbp)
jmp 0x7002d
movb -0x349(%rbp), %al
movq -0x348(%rbp), %rcx
movq %rcx, -0x290(%rbp)
movb %al, -0x288(%rbp)
leaq -0xb8(%rbp), %rdi
leaq -0x218(%rbp), %rsi
callq 0x62950
jmp 0x7005c
movq -0x2a8(%rbp), %rdi
movq -0x218(%rbp), %rsi
addq $0x18, %rsi
callq 0x70270
movl %eax, -0x350(%rbp)
jmp 0x7007b
movl -0x350(%rbp), %eax
movl %eax, -0x294(%rbp)
leaq -0xd0(%rbp), %rdi
leaq -0x294(%rbp), %rsi
callq 0x536f0
jmp 0x7009c
movl -0x3c(%rbp), %eax
addl $0x1, %eax
movl %eax, -0x3c(%rbp)
leaq -0x238(%rbp), %rdi
callq 0xd7d0
leaq -0x210(%rbp), %rdi
callq 0x62ea0
leaq -0x1f0(%rbp), %rdi
callq 0x62f00
jmp 0x6fca2
leaq -0x210(%rbp), %rdi
callq 0x62ea0
jmp 0x70227
leaq -0x38(%rbp), %rdi
callq 0x62f20
jmp 0x6f8ee
leaq -0x80(%rbp), %rdi
callq 0x742c0
testb $0x1, %al
jne 0x701cd
movl $0x0, -0x298(%rbp)
movslq -0x298(%rbp), %rax
movq %rax, -0x358(%rbp)
movq -0x10(%rbp), %rdi
callq 0x37930
movq %rax, %rcx
movq -0x358(%rbp), %rax
cmpq %rcx, %rax
jae 0x701a2
movq -0x20(%rbp), %rdi
addq $0x18, %rdi
movslq -0x298(%rbp), %rsi
callq 0xda70
movq %rax, %rdi
callq 0x3ec00
testb $0x1, %al
jne 0x7018c
movq -0x20(%rbp), %rdi
addq $0x18, %rdi
movslq -0x298(%rbp), %rsi
callq 0xda70
movq %rax, -0x360(%rbp)
movq -0x10(%rbp), %rdi
movslq -0x298(%rbp), %rsi
callq 0xda70
movq -0x360(%rbp), %rsi
movq %rax, %rdi
callq 0x3eca0
jmp 0x7018a
jmp 0x7018c
jmp 0x7018e
movl -0x298(%rbp), %eax
addl $0x1, %eax
movl %eax, -0x298(%rbp)
jmp 0x70108
xorl %eax, %eax
testb $0x1, -0x9d(%rbp)
movb %al, -0x361(%rbp)
je 0x701bd
movb $0x1, %al
movb %al, -0x361(%rbp)
jmp 0x701bd
movb -0x361(%rbp), %al
andb $0x1, %al
movb %al, -0x9d(%rbp)
jmp 0x701d4
movb $0x0, -0x9d(%rbp)
movb -0x9d(%rbp), %al
movb %al, -0x362(%rbp)
leaq -0xd0(%rbp), %rdi
callq 0x4e960
leaq -0xb8(%rbp), %rdi
callq 0x62f40
leaq -0x98(%rbp), %rdi
callq 0x4e960
leaq -0x80(%rbp), %rdi
callq 0x3f540
leaq -0x38(%rbp), %rdi
callq 0x62ab0
movb -0x362(%rbp), %al
andb $0x1, %al
addq $0x390, %rsp            # imm = 0x390
popq %rbp
retq
leaq -0xd0(%rbp), %rdi
callq 0x4e960
leaq -0xb8(%rbp), %rdi
callq 0x62f40
leaq -0x98(%rbp), %rdi
callq 0x4e960
leaq -0x80(%rbp), %rdi
callq 0x3f540
leaq -0x38(%rbp), %rdi
callq 0x62ab0
movq -0xe0(%rbp), %rdi
callq 0x8880
nopl (%rax)