// File: 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
movq %rsp, %rbp
subq $0x150, %rsp            # imm = 0x150
movq %rdx, -0x118(%rbp)
movq %rdi, -0x10(%rbp)
movq %rsi, -0x18(%rbp)
movq %rdx, -0x20(%rbp)
movq %rcx, -0x28(%rbp)
movq %r8, -0x30(%rbp)
movq %r9, -0x38(%rbp)
movq -0x10(%rbp), %rax
movq %rax, -0x110(%rbp)
movq -0x30(%rbp), %rdi
callq 0x39b60
movq %rax, -0x48(%rbp)
movq -0x30(%rbp), %rdi
callq 0x399f0
movq %rax, -0x50(%rbp)
movq -0x48(%rbp), %rdi
movq -0x50(%rbp), %rsi
leaq -0x18(%rbp), %rdx
callq 0x8be30
movq %rax, -0x40(%rbp)
movq -0x38(%rbp), %rax
movq %rax, -0x108(%rbp)
movq -0x38(%rbp), %rdi
callq 0x8bc80
movq %rax, -0x68(%rbp)
movq -0x30(%rbp), %rdi
callq 0x39b60
movq %rax, -0x70(%rbp)
movq -0x40(%rbp), %rax
movq %rax, -0x78(%rbp)
movq -0x70(%rbp), %rdi
movq -0x78(%rbp), %rsi
callq 0x62750
movq %rax, %rsi
leaq -0x68(%rbp), %rdi
callq 0x8bef0
movq %rax, -0x60(%rbp)
leaq -0x58(%rbp), %rdi
leaq -0x60(%rbp), %rsi
callq 0x8bf30
movq -0x108(%rbp), %rdi
movq -0x58(%rbp), %rsi
callq 0x8be80
movq %rax, -0x80(%rbp)
movq -0x30(%rbp), %rax
movq %rax, -0x100(%rbp)
leaq -0x88(%rbp), %rdi
leaq -0x40(%rbp), %rsi
callq 0x628b0
movq -0x100(%rbp), %rdi
movq -0x88(%rbp), %rsi
callq 0x8bf70
movq %rax, -0x90(%rbp)
movq -0x118(%rbp), %rdi
callq 0x3ec00
xorb $-0x1, %al
testb $0x1, %al
jne 0x8a71b
jmp 0x8a941
movq -0x118(%rbp), %rsi
movq -0x18(%rbp), %rax
movq %rax, -0x130(%rbp)
leaq -0xb0(%rbp), %rdi
movq %rdi, -0x128(%rbp)
callq 0x3aff0
movq -0x110(%rbp), %rdi
movq -0x130(%rbp), %rsi
movq -0x128(%rbp), %rdx
movq (%rdi), %rax
movq 0x30(%rax), %rax
callq *%rax
movq %rax, -0x120(%rbp)
jmp 0x8a767
movq -0x120(%rbp), %rax
movq %rax, -0x98(%rbp)
leaq -0xb0(%rbp), %rdi
callq 0xd7d0
movq -0x28(%rbp), %rdi
leaq -0x98(%rbp), %rsi
callq 0x3f490
movb $0x0, -0xbd(%rbp)
movq -0x30(%rbp), %rax
movq %rax, -0xc8(%rbp)
movq -0xc8(%rbp), %rdi
callq 0x39b60
movq %rax, -0xd0(%rbp)
movq -0xc8(%rbp), %rdi
callq 0x399f0
movq %rax, -0xd8(%rbp)
leaq -0xd0(%rbp), %rdi
leaq -0xd8(%rbp), %rsi
callq 0x51bd0
testb $0x1, %al
jne 0x8a7e5
jmp 0x8a91d
leaq -0xd0(%rbp), %rdi
callq 0x51c10
movq (%rax), %rax
movq %rax, -0xe0(%rbp)
movq -0x98(%rbp), %rax
movq %rax, -0x138(%rbp)
movq -0xe0(%rbp), %rdi
callq 0x51cb0
movq %rax, %rcx
movq -0x138(%rbp), %rax
cmpq %rcx, %rax
jne 0x8a90a
movq -0x110(%rbp), %rdi
movq -0xe0(%rbp), %rsi
movq -0x18(%rbp), %rdx
movq -0x28(%rbp), %rcx
movq -0x30(%rbp), %r8
movq -0x38(%rbp), %r9
callq 0x8a350
testb $0x1, %al
jne 0x8a851
jmp 0x8a88d
movq -0x18(%rbp), %rdi
movq -0x98(%rbp), %rsi
callq 0x3a480
movb $0x1, -0x1(%rbp)
jmp 0x8a968
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0xb8(%rbp)
movl %eax, -0xbc(%rbp)
leaq -0xb0(%rbp), %rdi
callq 0xd7d0
jmp 0x8a976
movq -0x98(%rbp), %rax
movq %rax, -0x148(%rbp)
movq -0x28(%rbp), %rsi
leaq -0xf8(%rbp), %rdi
movq %rdi, -0x140(%rbp)
callq 0x3aff0
movq -0x110(%rbp), %rdi
movq -0x118(%rbp), %rsi
movq -0x148(%rbp), %rdx
movq -0x140(%rbp), %rcx
callq 0x8a990
jmp 0x8a8d5
leaq -0xf8(%rbp), %rdi
callq 0xd7d0
movb $0x1, -0xbd(%rbp)
jmp 0x8a91d
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0xb8(%rbp)
movl %eax, -0xbc(%rbp)
leaq -0xf8(%rbp), %rdi
callq 0xd7d0
jmp 0x8a976
jmp 0x8a90c
leaq -0xd0(%rbp), %rdi
callq 0x51c30
jmp 0x8a7c9
testb $0x1, -0xbd(%rbp)
jne 0x8a93c
movq -0x18(%rbp), %rdi
movq -0x98(%rbp), %rsi
callq 0x3a480
movb $0x1, -0x1(%rbp)
jmp 0x8a968
jmp 0x8a704
movq -0x18(%rbp), %rax
movq %rax, -0x150(%rbp)
movq -0x18(%rbp), %rdi
callq 0x51cb0
movq -0x150(%rbp), %rdi
movq %rax, %rsi
callq 0x3a480
movb $0x0, -0x1(%rbp)
movb -0x1(%rbp), %al
andb $0x1, %al
addq $0x150, %rsp            # imm = 0x150
popq %rbp
retq
movq -0xb8(%rbp), %rdi
callq 0x8880
nopw %cs:(%rax,%rax)