// File: mitchellsayer[P]FastPointCloudSegmentation/src/./point_cloud_segmenter.cc
// Function: PointCloudSegmenter::ExtractInitialSeeds(std::vector<Vec3, std::allocator<Vec3>>&, std::vector<Vec3, std::allocator<Vec3>>&)
void PointCloudSegmenter::ExtractInitialSeeds(std::vector<Vec3>& cloud_seg, std::vector<Vec3>& seeds) {
  if (!cloud_seg.empty()) {

    //Sort points in current segment by height
    std::sort(cloud_seg.begin(), cloud_seg.end(), CompareZ);

    Vec3 lpr;

    //Calc Lowest Point Representative (LPR)
    std::vector<Vec3>::iterator it;
    int i = 0;

    for (it = cloud_seg.begin(); it != cloud_seg.end(); it++) {
      if (it->z > -4) {
        if (i == n_lpr) break;

        lpr.x += it->x;
        lpr.y += it->y;
        lpr.z += it->z;

        i++;
      }
    }

    lpr.x /= n_lpr;
    lpr.y /= n_lpr;
    lpr.z /= n_lpr;

    //Add point as initial seed if its dist to lpr is < th_seeds
    for (it =  cloud_seg.begin(); it != cloud_seg.end(); it++) {
      if (it->z - lpr.z < this->th_seeds) {
        seeds.push_back(*it);
      }
    }
  }
}

// Assembly:
pushq %rbp
movq %rsp, %rbp
subq $0xa0, %rsp
movq %rdi, -0x8(%rbp)
movq %rsi, -0x10(%rbp)
movq %rdx, -0x18(%rbp)
movq -0x8(%rbp), %rax
movq %rax, -0x90(%rbp)
movq -0x10(%rbp), %rdi
callq 0x5dd0
testb $0x1, %al
jne 0x31e3
movq -0x10(%rbp), %rdi
callq 0x57f0
movq %rax, -0x20(%rbp)
movq -0x10(%rbp), %rdi
callq 0x5860
movq %rax, -0x28(%rbp)
movq -0x20(%rbp), %rdi
movq -0x28(%rbp), %rsi
leaq -0xadc(%rip), %rdx      # 0x2540
callq 0x5bb0
leaq -0x44(%rbp), %rdi
callq 0x5730
leaq -0x50(%rbp), %rdi
callq 0x57d0
movl $0x0, -0x54(%rbp)
movq -0x10(%rbp), %rdi
callq 0x57f0
movq %rax, -0x60(%rbp)
movq -0x60(%rbp), %rax
movq %rax, -0x50(%rbp)
movq -0x10(%rbp), %rdi
callq 0x5860
movq %rax, -0x68(%rbp)
leaq -0x50(%rbp), %rdi
leaq -0x68(%rbp), %rsi
callq 0x5820
testb $0x1, %al
jne 0x3072
jmp 0x3104
leaq -0x50(%rbp), %rdi
callq 0x5c60
movss 0x8(%rax), %xmm0
movss 0xef80(%rip), %xmm1     # 0x12008
ucomiss %xmm1, %xmm0
jbe 0x30ee
movq -0x90(%rbp), %rcx
movl -0x54(%rbp), %eax
cmpl 0x5c(%rcx), %eax
jne 0x309e
jmp 0x3104
leaq -0x50(%rbp), %rdi
callq 0x5c60
movss (%rax), %xmm0
addss -0x44(%rbp), %xmm0
movss %xmm0, -0x44(%rbp)
leaq -0x50(%rbp), %rdi
callq 0x5c60
movss 0x4(%rax), %xmm0
addss -0x40(%rbp), %xmm0
movss %xmm0, -0x40(%rbp)
leaq -0x50(%rbp), %rdi
callq 0x5c60
movss 0x8(%rax), %xmm0
addss -0x3c(%rbp), %xmm0
movss %xmm0, -0x3c(%rbp)
movl -0x54(%rbp), %eax
addl $0x1, %eax
movl %eax, -0x54(%rbp)
jmp 0x30f0
leaq -0x50(%rbp), %rdi
xorl %esi, %esi
callq 0x5920
movq %rax, -0x70(%rbp)
jmp 0x304f
movq -0x90(%rbp), %rax
cvtsi2ssl 0x5c(%rax), %xmm1
movss -0x44(%rbp), %xmm0
divss %xmm1, %xmm0
movss %xmm0, -0x44(%rbp)
cvtsi2ssl 0x5c(%rax), %xmm1
movss -0x40(%rbp), %xmm0
divss %xmm1, %xmm0
movss %xmm0, -0x40(%rbp)
cvtsi2ssl 0x5c(%rax), %xmm1
movss -0x3c(%rbp), %xmm0
divss %xmm1, %xmm0
movss %xmm0, -0x3c(%rbp)
movq -0x10(%rbp), %rdi
callq 0x57f0
movq %rax, -0x78(%rbp)
movq -0x78(%rbp), %rax
movq %rax, -0x50(%rbp)
movq -0x10(%rbp), %rdi
callq 0x5860
movq %rax, -0x80(%rbp)
leaq -0x50(%rbp), %rdi
leaq -0x80(%rbp), %rsi
callq 0x5820
testb $0x1, %al
jne 0x3179
jmp 0x31e1
leaq -0x50(%rbp), %rdi
callq 0x5c60
movq %rax, %rcx
movq -0x90(%rbp), %rax
movss 0x8(%rcx), %xmm0
subss -0x3c(%rbp), %xmm0
cvtss2sd %xmm0, %xmm1
movsd 0x48(%rax), %xmm0
ucomisd %xmm1, %xmm0
jbe 0x31c8
movq -0x18(%rbp), %rax
movq %rax, -0x98(%rbp)
leaq -0x50(%rbp), %rdi
callq 0x5900
movq -0x98(%rbp), %rdi
movq %rax, %rsi
callq 0x5890
jmp 0x31ca
leaq -0x50(%rbp), %rdi
xorl %esi, %esi
callq 0x5920
movq %rax, -0x88(%rbp)
jmp 0x3159
jmp 0x31e3
addq $0xa0, %rsp
popq %rbp
retq
nopl (%rax)