// File: /workspace/github_cpp_makefile_star2_O2/Kei18[P]pibt/./src/solver/cbs.cpp
// Function: CBS::valid(CTNode*, std::vector<Agent*, std::allocator<Agent*>>&)
Constraints CBS::valid(CTNode* node, Agents& block) {
  Paths paths = node->paths;
  int maxLength = node->paths[0].size();
  Constraints constraints = {};
  std::vector<int> ids;
  Node* v;
  Agent *a, *b;
  int i, j;

  for (int t = 1; t < maxLength; ++t) {
    for (int _i = 0; _i < block.size(); ++_i) {
      a = block[_i];
      auto itr1 = std::find_if(A.begin(), A.end(),
                               [a](Agent* _a)
                               { return a->getId() == _a->getId(); });
      i = std::distance(A.begin(), itr1);
      v = paths[i][t];

      // check collision
      ids.clear();
      for (int _j = _i + 1; _j < block.size(); ++_j) {
        b = block[_j];
        auto itr2 = std::find_if(A.begin(), A.end(),
                                 [b](Agent* _b) { return b == _b; });
        j = std::distance(A.begin(), itr2);
        if (paths[j][t] == v) ids.push_back(j);
      }

      if (!ids.empty()) {  // detect collision
        ids.push_back(i);
        for (int k = 0; k < ids.size(); ++k) {
          Constraint constraint;
          for (int l = 0; l < ids.size(); ++l) {
            if (k == l) continue;
            Conflict* c = new Conflict { A[ids[l]], t, v, v, true, "" };
            setCKey(c);
            constraint.push_back(c);
          }
          constraints.push_back(constraint);
        }
        return constraints;
      }

      // check intersection
      auto itr3 = paths.begin() + i;
      auto itr4 = std::find_if(itr3 + 1, paths.end(),
                               [t, itr3](Nodes path) {
                                 if (path.empty()) return false;
                                 return (path[t] == (*itr3)[t-1])
                                   && (path[t-1] == (*itr3)[t]); });
      if (itr4 != paths.end()) {  // detect intersection

        Constraint knownConstraint = getConstraints(node);

        j = std::distance(paths.begin(), itr4);
        Conflict* c1 = new Conflict { A[i], t, (*itr3)[t], (*itr3)[t-1], false, "" };
        Conflict* c2 = new Conflict { A[j], t, (*itr4)[t], (*itr4)[t-1], false, "" };
        setCKey(c1);
        setCKey(c2);
        constraints.push_back({ c1 });
        constraints.push_back({ c2 });
        return constraints;
      }
    }
  }

  return constraints;
}

// Assembly:
pushq %rbp
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0xd8, %rsp
movq %rcx, 0x90(%rsp)
movq %rdx, %r15
movq %rsi, 0x98(%rsp)
movq %rdi, %rbx
leaq 0x18(%rdx), %rsi
leaq 0xa8(%rsp), %rdi
callq 0x227f6
movq %r15, 0xc0(%rsp)
movq 0x18(%r15), %rax
movq 0x8(%rax), %rcx
subq (%rax), %rcx
xorps %xmm0, %xmm0
movups %xmm0, (%rbx)
movq %rbx, 0x10(%rsp)
andq $0x0, 0x10(%rbx)
leaq 0x40(%rsp), %rax
andq $0x0, 0x10(%rax)
movaps %xmm0, (%rax)
shrq $0x3, %rcx
movslq %ecx, %rax
pushq $0x1
popq %rcx
movq %rcx, %r12
movq %rax, 0xc8(%rsp)
movq 0x98(%rsp), %r14
movq %r12, 0x58(%rsp)
cmpq %rax, %r12
jge 0x20223
movq $0x0, 0x88(%rsp)
movq %rcx, 0x80(%rsp)
movq 0x58(%rsp), %r12
movq 0x90(%rsp), %rcx
movq (%rcx), %rax
movq 0x8(%rcx), %rcx
movq %rcx, %rdx
subq %rax, %rdx
sarq $0x3, %rdx
movq 0x88(%rsp), %rsi
cmpq %rsi, %rdx
jbe 0x1ffa0
movq (%rax,%rsi,8), %rdi
movq 0x20(%r14), %rdx
movq 0x28(%r14), %rbx
movq %rbx, %r8
subq %rdx, %r8
movq %r8, %r9
sarq $0x5, %r9
movq %rdx, %rsi
testq %r9, %r9
jle 0x1fcb0
movq (%rsi), %r11
movl (%rdi), %r10d
cmpl (%r11), %r10d
je 0x1fce5
movq 0x8(%rsi), %r11
cmpl (%r11), %r10d
je 0x1fcd5
movq 0x10(%rsi), %r11
cmpl (%r11), %r10d
je 0x1fcdb
movq 0x18(%rsi), %r11
cmpl (%r11), %r10d
je 0x1fce1
addq $0x20, %rsi
decq %r9
addq $-0x20, %r8
jmp 0x1fc78
sarq $0x3, %r8
cmpq $0x3, %r8
je 0x1ff11
cmpq $0x2, %r8
je 0x1ff0d
cmpq $0x1, %r8
jne 0x1fce8
movl (%rdi), %edi
jmp 0x1ff33
addq $0x8, %rsi
jmp 0x1fce5
addq $0x10, %rsi
jmp 0x1fce5
addq $0x18, %rsi
movq %rsi, %rbx
subq %rdx, %rbx
shrq $0x3, %rbx
movl %ebx, 0x1c(%rsp)
movslq %ebx, %rdx
movq 0xa8(%rsp), %rsi
imulq $0x18, %rdx, %rdx
movq (%rsi,%rdx), %rdx
movq (%rdx,%r12,8), %r15
movq 0x40(%rsp), %rdx
cmpq %rdx, 0x48(%rsp)
je 0x1fd1b
movq %rdx, 0x48(%rsp)
incq 0x88(%rsp)
movq 0x80(%rsp), %r13
movq %rcx, %rdx
subq %rax, %rdx
sarq $0x3, %rdx
cmpq %r13, %rdx
jbe 0x1fe11
movq (%rax,%r13,8), %r8
movq 0x20(%r14), %rdx
movq 0x28(%r14), %rsi
movq %rsi, %r9
subq %rdx, %r9
movq %r9, %r10
sarq $0x5, %r10
movq %rdx, %rdi
testq %r10, %r10
jle 0x1fd83
cmpq %r8, (%rdi)
je 0x1fdc4
cmpq %r8, 0x8(%rdi)
je 0x1fdb4
cmpq %r8, 0x10(%rdi)
je 0x1fdba
cmpq %r8, 0x18(%rdi)
je 0x1fdc0
addq $0x20, %rdi
decq %r10
addq $-0x20, %r9
jmp 0x1fd5a
sarq $0x3, %r9
cmpq $0x1, %r9
je 0x1fdab
cmpq $0x2, %r9
je 0x1fda2
cmpq $0x3, %r9
jne 0x1fdc7
cmpq %r8, (%rdi)
je 0x1fdc4
addq $0x8, %rdi
cmpq %r8, (%rdi)
je 0x1fdc4
addq $0x8, %rdi
cmpq %r8, (%rdi)
cmoveq %rdi, %rsi
jmp 0x1fdc7
addq $0x8, %rdi
jmp 0x1fdc4
addq $0x10, %rdi
jmp 0x1fdc4
addq $0x18, %rdi
movq %rdi, %rsi
subq %rdx, %rsi
shrq $0x3, %rsi
movl %esi, 0xc(%rsp)
movslq %esi, %rdx
movq 0xa8(%rsp), %rsi
imulq $0x18, %rdx, %rdx
movq (%rsi,%rdx), %rdx
cmpq %r15, (%rdx,%r12,8)
jne 0x1fe09
leaq 0x40(%rsp), %rdi
leaq 0xc(%rsp), %rsi
callq 0x1be2c
movq 0x90(%rsp), %rcx
movq (%rcx), %rax
movq 0x8(%rcx), %rcx
incq %r13
jmp 0x1fd2b
movq 0x40(%rsp), %rax
cmpq 0x48(%rsp), %rax
jne 0x1ffb3
movslq %ebx, %rax
imulq $0x18, %rax, %rcx
movq 0xa8(%rsp), %rsi
movq 0xb0(%rsp), %rbp
leaq (%rsi,%rcx), %rax
movl %r12d, 0x20(%rsp)
movq %rax, 0x28(%rsp)
movq %rbp, 0x78(%rsp)
movq %rax, 0xd0(%rsp)
subq %rax, %rbp
addq $-0x18, %rbp
movq %rbp, %rax
cqto
pushq $0x18
popq %rdi
idivq %rdi
movq %rax, %r15
sarq $0x2, %r15
leaq (%rsi,%rcx), %r13
addq $0x30, %r13
leaq -0x18(%r13), %rbx
testq %r15, %r15
jle 0x1fee6
leaq 0x20(%rsp), %rdi
movq %rbx, %rsi
callq 0x21d52
testb %al, %al
jne 0x1ff85
leaq 0x20(%rsp), %rdi
movq %r13, %rsi
callq 0x21d52
testb %al, %al
jne 0x1ff64
leaq 0x18(%r13), %rbx
leaq 0x20(%rsp), %rdi
movq %rbx, %rsi
callq 0x21d52
testb %al, %al
jne 0x1ff85
leaq 0x30(%r13), %rbx
leaq 0x20(%rsp), %rdi
movq %rbx, %rsi
callq 0x21d52
testb %al, %al
jne 0x1ff85
decq %r15
addq $0x60, %r13
addq $-0x60, %rbp
jmp 0x1fe74
movq %rbp, %rax
cqto
pushq $0x18
popq %rcx
idivq %rcx
cmpq $0x1, %rax
je 0x1ff70
cmpq $0x3, %rax
je 0x1ff42
movq %rbx, %r13
cmpq $0x2, %rax
je 0x1ff53
movq 0x78(%rsp), %rbx
jmp 0x1ff85
movl (%rdi), %edi
jmp 0x1ff23
movq (%rsi), %r8
movl (%rdi), %edi
cmpl (%r8), %edi
je 0x1fce5
addq $0x8, %rsi
movq (%rsi), %r8
cmpl (%r8), %edi
je 0x1fce5
addq $0x8, %rsi
movq (%rsi), %r8
cmpl (%r8), %edi
cmoveq %rsi, %rbx
jmp 0x1fce8
leaq 0x20(%rsp), %rdi
movq %rbx, %rsi
callq 0x21d52
testb %al, %al
jne 0x1ff85
leaq 0x20(%rsp), %rdi
movq %r13, %rsi
callq 0x21d52
testb %al, %al
je 0x1ff69
movq %r13, %rbx
jmp 0x1ff85
addq $0x18, %r13
movq %r13, %rbx
leaq 0x20(%rsp), %rdi
movq %rbx, %rsi
callq 0x21d52
testb %al, %al
cmoveq 0x78(%rsp), %rbx
incq 0x80(%rsp)
cmpq 0xb0(%rsp), %rbx
je 0x1fc32
jmp 0x200a9
incq %r12
movq 0xc8(%rsp), %rax
pushq $0x1
popq %rcx
jmp 0x1fc0b
leaq 0x40(%rsp), %rdi
leaq 0x1c(%rsp), %rsi
callq 0x1be2c
xorl %ebp, %ebp
leaq 0x20(%rsp), %r14
movq 0x40(%rsp), %r12
movq 0x48(%rsp), %rax
movq %rax, %rcx
subq %r12, %rcx
sarq $0x2, %rcx
cmpq %rbp, %rcx
jbe 0x20223
xorps %xmm0, %xmm0
movaps %xmm0, 0x20(%rsp)
andq $0x0, 0x30(%rsp)
xorl %r13d, %r13d
movq %rax, %rcx
subq %r12, %rcx
sarq $0x2, %rcx
cmpq %r13, %rcx
jbe 0x2008c
cmpq %r13, %rbp
je 0x20084
pushq $0x48
popq %rdi
callq 0x63b0
movq %rax, %rbx
movslq (%r12,%r13,4), %rax
movq 0x98(%rsp), %rcx
movq 0x20(%rcx), %rcx
movq (%rcx,%rax,8), %rax
movq %rax, (%rbx)
movq 0x58(%rsp), %rax
movl %eax, 0x8(%rbx)
movq %r15, 0x10(%rbx)
movq %r15, 0x18(%rbx)
movb $0x1, 0x20(%rbx)
movq %rbx, %rdi
addq $0x28, %rdi
leaq 0x1a511(%rip), %rsi     # 0x3a564
leaq 0xa0(%rsp), %rdx
callq 0x92b0
movq %rbx, 0x60(%rsp)
movq %rbx, %rsi
callq 0x209a4
movq %r14, %rdi
leaq 0x60(%rsp), %rsi
callq 0x22936
movq 0x40(%rsp), %r12
movq 0x48(%rsp), %rax
incq %r13
jmp 0x1fff7
movq 0x10(%rsp), %rdi
movq %r14, %rsi
callq 0x22954
movq %r14, %rdi
callq 0x235f6
incq %rbp
jmp 0x1ffc9
leaq 0x20(%rsp), %rdi
movq 0xc0(%rsp), %rdx
callq 0x20b8e
movq %rbx, %rax
subq 0xa8(%rsp), %rax
cqto
pushq $0x18
popq %rcx
idivq %rcx
movl %eax, 0xc(%rsp)
pushq $0x48
popq %rdi
callq 0x63b0
movq %rax, %r15
movslq 0x1c(%rsp), %rax
movq 0x20(%r14), %rcx
movq (%rcx,%rax,8), %rax
movq %rax, (%r15)
movl %r12d, 0x8(%r15)
movq 0xd0(%rsp), %rax
movq (%rax), %rax
movq (%rax,%r12,8), %rcx
movq %rcx, 0x10(%r15)
movq %r12, %rcx
shlq $0x20, %rcx
movabsq $-0x100000000, %r12     # imm = 0xFFFFFFFF00000000
addq %rcx, %r12
movq %r12, %rcx
sarq $0x1d, %rcx
movq (%rax,%rcx), %rax
movq %rax, 0x18(%r15)
movb $0x0, 0x20(%r15)
movq %r15, %rdi
addq $0x28, %rdi
leaq 0x1a42a(%rip), %rsi     # 0x3a564
leaq 0x60(%rsp), %rdx
callq 0x92b0
pushq $0x48
popq %rdi
callq 0x63b0
movq %rax, %r13
sarq $0x20, %r12
movslq 0xc(%rsp), %rax
movq 0x20(%r14), %rcx
movq (%rcx,%rax,8), %rax
movq %rax, (%r13)
movq 0x58(%rsp), %rcx
movl %ecx, 0x8(%r13)
movq (%rbx), %rax
movq (%rax,%rcx,8), %rcx
movq %rcx, 0x10(%r13)
movq (%rax,%r12,8), %rax
movq %rax, 0x18(%r13)
movb $0x0, 0x20(%r13)
movq %r13, %rdi
addq $0x28, %rdi
leaq 0x1a3d1(%rip), %rsi     # 0x3a564
leaq 0x60(%rsp), %rdx
callq 0x92b0
movq %r15, %rsi
callq 0x209a4
movq %r13, %rsi
callq 0x209a4
leaq 0xa0(%rsp), %rsi
movq %r15, (%rsi)
leaq 0x60(%rsp), %rdi
pushq $0x1
popq %rdx
leaq 0xb(%rsp), %rcx
callq 0x22980
leaq 0x60(%rsp), %rsi
movq 0x10(%rsp), %rdi
callq 0x23d88
leaq 0x60(%rsp), %rdi
callq 0x235f6
leaq 0xa0(%rsp), %rsi
movq %r13, (%rsi)
leaq 0x60(%rsp), %rdi
pushq $0x1
popq %rdx
leaq 0xb(%rsp), %rcx
callq 0x22980
leaq 0x60(%rsp), %rsi
movq 0x10(%rsp), %rdi
callq 0x23d88
leaq 0x60(%rsp), %rdi
callq 0x235f6
leaq 0x20(%rsp), %rdi
callq 0x235f6
leaq 0x40(%rsp), %rdi
callq 0x1c0a0
leaq 0xa8(%rsp), %rdi
callq 0x92ec
movq 0x10(%rsp), %rax
addq $0xd8, %rsp
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
jmp 0x20255
jmp 0x2029f
movq %rax, %r14
leaq 0x60(%rsp), %rdi
callq 0x235f6
jmp 0x202a2
jmp 0x2029f
movq %rax, %r14
pushq $0x48
popq %rsi
movq %r13, %rdi
jmp 0x20293
movq %rax, %r14
pushq $0x48
popq %rsi
movq %r15, %rdi
jmp 0x20293
jmp 0x2029f
jmp 0x2029a
jmp 0x2029a
jmp 0x2029f
jmp 0x2029a
jmp 0x2029f
jmp 0x2029a
movq %rax, %r14
pushq $0x48
popq %rsi
movq %rbx, %rdi
callq 0x63c0
jmp 0x202a2
movq %rax, %r14
jmp 0x202ac
movq %rax, %r14
leaq 0x20(%rsp), %rdi
callq 0x235f6
leaq 0x40(%rsp), %rdi
callq 0x1c0a0
movq 0x10(%rsp), %rdi
callq 0x22786
leaq 0xa8(%rsp), %rdi
callq 0x92ec
movq %r14, %rdi
callq 0x6650
nop