// File: /workspace/github_cpp_makefile_star2_O3/Kei18[P]pibt/./src/solver/pibt.cpp
// Function: PIBT::priorityInheritance(Agent*, std::vector<Node*, std::allocator<Node*>>, std::vector<Node*, std::allocator<Node*>>&, std::vector<Agent*, std::allocator<Agent*>>&, std::vector<float, std::allocator<float>>&)
bool PIBT::priorityInheritance(Agent* a,
                               Nodes C,
                               Nodes& CLOSE_NODE,
                               Agents& OPEN_AGENT,
                               std::vector<float>& PL)
{
  // remove agent from OPEN_AGENT, Priority List
  auto itr = std::find(OPEN_AGENT.begin(), OPEN_AGENT.end(), a);
  PL.erase(PL.begin() + std::distance(OPEN_AGENT.begin(), itr));
  OPEN_AGENT.erase(itr);

  Node* target;

  // main loop
  while (!C.empty()) {

    // choose target
    target = chooseNode(a, C);
    CLOSE_NODE.push_back(target);

    bool isAgent = false;
    for (auto b : OPEN_AGENT) {
      if (target == b->getNode()) {  // If there is an agent
        if (priorityInheritance(b, a, CLOSE_NODE, OPEN_AGENT, PL)) {
          // priority inheritance success
          a->setNode(target);
          return true;
        } else {
          // priority inheritance fail
          updateC(C, target, CLOSE_NODE);
          isAgent = true;
          break;
        }
      }
    }

    // If there is an agent
    if (!isAgent) {
      a->setNode(target);
      return true;
    }
  }

  // failed
  a->setNode(a->getNode());
  return false;
}

// Assembly:
pushq %rbp
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0x48, %rsp
movq %r9, %rbx
movq %r8, %r14
movq %rcx, %r15
movq %rdx, %r12
movq %rdi, 0x10(%rsp)
movq %rsp, %rdx
movq %rsi, (%rdx)
movq (%r8), %rdi
movq 0x8(%r8), %rsi
callq 0x388d5
movq %rax, %rbp
subq (%r14), %rax
sarq %rax
movq (%rbx), %rdi
movq 0x8(%rbx), %rdx
leaq (%rdi,%rax), %rsi
addq $0x4, %rsi
cmpq %rdx, %rsi
je 0x38021
addq %rax, %rdi
subq %rsi, %rdx
callq 0x65b0
movq 0x8(%rbx), %rdx
addq $-0x4, %rdx
movq %rdx, 0x8(%rbx)
leaq 0x8(%rbp), %rsi
movq 0x8(%r14), %rdx
cmpq %rdx, %rsi
je 0x38045
subq %rsi, %rdx
movq %rbp, %rdi
callq 0x65b0
movq 0x8(%r14), %rdx
addq $-0x8, %rdx
movq %rdx, 0x8(%r14)
movq (%r12), %rax
cmpq 0x8(%r12), %rax
je 0x3814c
leaq 0x18(%rsp), %r13
movq (%rsp), %rbp
movq %r13, %rdi
movq %r12, %rsi
callq 0x19aae
movq 0x10(%rsp), %rdi
movq (%rdi), %rax
movq %rbp, %rsi
movq %r13, %rbp
movq %r13, %rdx
callq *0x30(%rax)
movq %rax, 0x8(%rsp)
movq 0x18(%rsp), %rdi
testq %rdi, %rdi
je 0x380a0
movq 0x28(%rsp), %rsi
subq %rdi, %rsi
callq 0x6380
movq 0x8(%r15), %rsi
cmpq 0x10(%r15), %rsi
je 0x380b9
movq 0x8(%rsp), %rax
movq %rax, (%rsi)
addq $0x8, 0x8(%r15)
jmp 0x380c6
movq %r15, %rdi
leaq 0x8(%rsp), %rdx
callq 0x1c5cc
movq (%r14), %rcx
movq 0x8(%r14), %rdx
movq 0x8(%rsp), %rsi
cmpq %rdx, %rcx
je 0x3815d
movq (%rcx), %rax
cmpq 0x8(%rax), %rsi
je 0x380ea
addq $0x8, %rcx
jmp 0x380d2
movq (%rsp), %rdx
movq 0x10(%rsp), %rdi
movq %rax, %rsi
movq %r15, %rcx
movq %r14, %r8
movq %rbx, %r9
callq 0x37dd6
testb %al, %al
jne 0x38177
leaq 0x30(%rsp), %r13
movq %r13, %rdi
movq %r15, %rsi
callq 0x19aae
movq %r12, %rsi
movq %r13, %rcx
callq 0x381a4
movq 0x30(%rsp), %rdi
testq %rdi, %rdi
je 0x3813a
movq 0x40(%rsp), %rsi
subq %rdi, %rsi
callq 0x6380
movq (%r12), %rax
cmpq 0x8(%r12), %rax
movq %rbp, %r13
jne 0x38061
movq (%rsp), %rdi
movq 0x8(%rdi), %rsi
callq 0x191f2
xorl %eax, %eax
jmp 0x38168
movq (%rsp), %rdi
callq 0x191f2
movb $0x1, %al
addq $0x48, %rsp
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
movq (%rsp), %rdi
movq 0x8(%rsp), %rsi
jmp 0x38161
movq %rax, %rbx
movq 0x18(%rsp), %rdi
testq %rdi, %rdi
je 0x3819c
movq 0x28(%rsp), %rsi
subq %rdi, %rsi
callq 0x6380
movq %rbx, %rdi
callq 0x6620