// File: /workspace/github_cpp_makefile_star2_O1/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 $0x78, %rsp
movq %r9, %rbx
movq %r8, %r14
movq %rcx, %r12
movq %rdx, %r15
movq %rdi, 0x30(%rsp)
leaq 0x10(%rsp), %rdx
movq %rsi, (%rdx)
movq (%r8), %rdi
movq 0x8(%r8), %rsi
callq 0x38b4d
movq %rax, %rbp
movq %r14, 0x8(%rsp)
subq (%r14), %rax
sarq %rax
movq (%rbx), %rdi
movq %rbx, 0x20(%rsp)
movq 0x8(%rbx), %rdx
leaq (%rdi,%rax), %rsi
addq $0x4, %rsi
cmpq %rdx, %rsi
je 0x38213
addq %rax, %rdi
subq %rsi, %rdx
callq 0x65b0
movq 0x20(%rsp), %rax
addq $-0x4, 0x8(%rax)
leaq 0x8(%rbp), %rsi
movq 0x8(%rsp), %rax
movq 0x8(%rax), %rdx
cmpq %rdx, %rsi
je 0x3823a
subq %rsi, %rdx
movq %rbp, %rdi
callq 0x65b0
movq 0x8(%rsp), %rax
addq $-0x8, 0x8(%rax)
movq %r12, 0x28(%rsp)
movq %r15, 0x18(%rsp)
movq (%r15), %rax
movq 0x8(%r15), %rcx
movq 0x10(%rsp), %rbp
movq %rax, 0x40(%rsp)
movq %rcx, 0x38(%rsp)
cmpq %rcx, %rax
je 0x383d5
leaq 0x48(%rsp), %rbx
movq %rbx, %rdi
movq %r15, %rsi
callq 0x19a68
movq 0x30(%rsp), %rdi
movq (%rdi), %rax
movq %rbp, %rsi
movq %rbx, %rdx
callq *0x30(%rax)
movq %rax, (%rsp)
movq 0x48(%rsp), %rdi
testq %rdi, %rdi
je 0x382a9
movq 0x58(%rsp), %rsi
subq %rdi, %rsi
callq 0x6380
movq 0x8(%r12), %rsi
cmpq 0x10(%r12), %rsi
je 0x382c4
movq (%rsp), %rax
movq %rax, (%rsi)
addq $0x8, 0x8(%r12)
jmp 0x382cf
movq %r12, %rdi
movq %rsp, %rdx
callq 0x1c69a
movq 0x8(%rsp), %rax
movq (%rax), %rbx
movq 0x8(%rax), %r12
cmpq %r12, %rbx
je 0x3838d
xorl %ebp, %ebp
movq (%rbx), %rsi
movq (%rsp), %r13
movq 0x8(%rsi), %r14
cmpq %r14, %r13
je 0x382fb
xorl %r15d, %r15d
jmp 0x38373
movq 0x10(%rsp), %rdx
movq 0x30(%rsp), %rdi
movq 0x28(%rsp), %r15
movq %r15, %rcx
movq 0x8(%rsp), %r8
movq 0x20(%rsp), %r9
callq 0x37fc6
testb %al, %al
je 0x38336
movq 0x10(%rsp), %rdi
movq (%rsp), %rsi
callq 0x191a6
movl $0x1, %r15d
jmp 0x38373
leaq 0x60(%rsp), %rbp
movq %rbp, %rdi
movq %r15, %rsi
callq 0x19a68
movq 0x18(%rsp), %rsi
movq %rbp, %rcx
callq 0x38420
movq 0x60(%rsp), %rdi
movl $0x4, %r15d
movb $0x1, %bpl
testq %rdi, %rdi
je 0x38373
movq 0x70(%rsp), %rsi
subq %rdi, %rsi
callq 0x6380
cmpq %r14, %r13
je 0x38395
addq $0x8, %rbx
cmpq %r12, %rbx
jne 0x382e6
movl $0x4, %r15d
jmp 0x38395
movl $0x4, %r15d
xorl %ebp, %ebp
cmpl $0x4, %r15d
setne %al
movl $0x0, %ecx
cmovel %ecx, %r15d
orb %bpl, %al
testb $0x1, %al
movq 0x28(%rsp), %r12
jne 0x383c5
movq 0x10(%rsp), %rdi
movq (%rsp), %rsi
callq 0x191a6
movl $0x1, %r15d
testl %r15d, %r15d
movq 0x18(%rsp), %r15
je 0x3824e
jmp 0x383e1
movq 0x8(%rbp), %rsi
movq %rbp, %rdi
callq 0x191a6
movq 0x38(%rsp), %rax
cmpq %rax, 0x40(%rsp)
setne %al
addq $0x78, %rsp
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
movq %rax, %rbx
movq 0x48(%rsp), %rdi
testq %rdi, %rdi
je 0x38417
movq 0x58(%rsp), %rsi
subq %rdi, %rsi
callq 0x6380
movq %rbx, %rdi
callq 0x6620
nop