// File: /workspace/github_cpp_makefile_star2_O2/mitchellsayer[P]FastPointCloudSegmentation/src/./point_cloud_segmenter.cc
// Function: PointCloudSegmenter::FindRuns(Scanline&)
void PointCloudSegmenter::FindRuns(Scanline& cur_scanline) {
  int i = 0;

  std::vector<Vec3>::iterator it;

  while (cur_scanline.points[i].label == -3) {
    i++;
  }

  if (i < cur_scanline.points.size() ) {
    cur_scanline.s_queue.push_back(i);

    if (i == cur_scanline.points.size() - 1){
      cur_scanline.e_queue.push_back(i);
      return;
    }

    i += 1;
  } else {
    return;
  }

  for (; i < cur_scanline.points.size(); i++) {
    Vec3* cur_p = &cur_scanline.points[i];
    Vec3* prev_p = &cur_scanline.points[i-1];

    if (cur_p->label != -3) {

      if (prev_p->label == -3) {
        //Start new run if immediately after gnd pt
        cur_scanline.s_queue.push_back(i);
        if (i == cur_scanline.points.size() - 1) {
          cur_scanline.e_queue.push_back(i);
        }
      } else {
        //check distance to prev point
        if ( (cur_scanline.points[i]).distance(cur_scanline.points[i-1]) > this->th_run ) {
          cur_scanline.e_queue.push_back(i - 1);
          cur_scanline.s_queue.push_back(i);
        }

        if (i == cur_scanline.points.size() - 1) {
            cur_scanline.e_queue.push_back(i);
        }
      }
    } else {
      //Current point is ground & previous point is not
      //then end that run
      if (prev_p->label != -3) {
        cur_scanline.e_queue.push_back(i-1);
      }
    }
  }
}

// Assembly:
pushq %rbp
movq %rsp, %rbp
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0x18, %rsp
movq %rsi, %rbx
movq %rdi, %r14
movq 0x48(%rsi), %rdx
leaq 0x18(%rdx), %rax
pushq $-0x1
popq %rcx
movss (%rax), %xmm0
incq %rcx
addq $0x1c, %rax
ucomiss 0x3f7c(%rip), %xmm0     # 0x7088
jne 0x3110
jnp 0x30fa
movl %ecx, -0x2c(%rbp)
movq 0x50(%rbx), %rax
subq %rdx, %rax
pushq $0x1c
popq %r13
cqto
idivq %r13
cmpq %rcx, %rax
jbe 0x3165
leaq -0x2c(%rbp), %r15
movq %rbx, %rdi
movq %r15, %rsi
callq 0x3dd6
movslq (%r15), %rsi
movq 0x48(%rbx), %r8
movq 0x50(%rbx), %rcx
movq %rcx, %rax
subq %r8, %rax
cqto
idivq %r13
decq %rax
leaq 0x18(%rbx), %r15
cmpq %rsi, %rax
jne 0x3174
leaq -0x2c(%rbp), %rsi
movq %r15, %rdi
callq 0x3dd6
addq $0x18, %rsp
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
leaq -0x2c(%rbp), %r12
leal 0x1(%rsi), %eax
movl %eax, -0x2c(%rbp)
movslq %eax, %rdi
subq %r8, %rcx
movq %rcx, %rax
cqto
idivq %r13
cmpq %rdi, %rax
movss 0x3ef1(%rip), %xmm1     # 0x7088
jbe 0x3165
imulq $0x1c, %rdi, %rcx
movslq %esi, %rdx
imulq $0x1c, %rdx, %rdx
movss 0x18(%r8,%rcx), %xmm0
ucomiss %xmm1, %xmm0
movss 0x18(%r8,%rdx), %xmm0
jne 0x31b9
jnp 0x31ed
ucomiss %xmm1, %xmm0
jne 0x320c
jp 0x320c
movq %rbx, %rdi
movq %r12, %rsi
callq 0x3dd6
movslq -0x2c(%rbp), %rcx
movq 0x50(%rbx), %rax
subq 0x48(%rbx), %rax
cqto
idivq %r13
decq %rax
cmpq %rcx, %rax
je 0x3285
jmp 0x3290
ucomiss %xmm1, %xmm0
jne 0x31f8
jnp 0x3290
movl %esi, -0x30(%rbp)
movq %r15, %rdi
leaq -0x30(%rbp), %rsi
callq 0x5474
jmp 0x3290
addq %r8, %rcx
addq %r8, %rdx
movss (%rcx), %xmm0
subss (%rdx), %xmm0
movss 0x4(%rcx), %xmm1
subss 0x4(%rdx), %xmm1
movss 0x8(%rcx), %xmm2
subss 0x8(%rdx), %xmm2
mulss %xmm1, %xmm1
mulss %xmm0, %xmm0
addss %xmm1, %xmm0
mulss %xmm2, %xmm2
addss %xmm0, %xmm2
sqrtss %xmm2, %xmm0
cvtss2sd %xmm0, %xmm0
ucomisd 0x70(%r14), %xmm0
jbe 0x327d
movl %esi, -0x34(%rbp)
movq %r15, %rdi
leaq -0x34(%rbp), %rsi
callq 0x5474
movq %rbx, %rdi
movq %r12, %rsi
callq 0x3dd6
movslq -0x2c(%rbp), %rdi
movq 0x50(%rbx), %rax
subq 0x48(%rbx), %rax
cqto
idivq %r13
decq %rax
cmpq %rdi, %rax
jne 0x3290
movq %r15, %rdi
movq %r12, %rsi
callq 0x3dd6
movl -0x2c(%rbp), %esi
movq 0x48(%rbx), %r8
movq 0x50(%rbx), %rcx
jmp 0x3178