// File: /workspace/github_cpp_makefile_star2_O1/mitchellsayer[P]FastPointCloudSegmentation/src/./point_cloud_segmenter.cc
// Function: PointCloudSegmenter::GroundPlaneFitting(std::vector<Vec3, std::allocator<Vec3>>&)
void  PointCloudSegmenter::GroundPlaneFitting(std::vector<Vec3>& cloud) {
  int start_s = clock(); //Take start timestamp

  int seg_size = (cloud.size() / n_segs);
  std::vector<Vec3> cloud_segs[n_segs];

  std::vector<Vec3>::iterator it;
  int i = 0, cur_seg = 0;

  for (it = cloud.begin(); it != cloud.end(); it++, i++) {
    if (i == seg_size) {
      if ((cur_seg + 1) != n_segs) {
        cur_seg++;
        i = 0;
      }
    }

    cloud_segs[cur_seg].push_back(*it);
  }

  std::vector<Vec3> cur_p_gnd;  //Pts belonging to ground surface in current segment iteration
  std::vector<Vec3> cur_p_ngnd; //Pts not belonging to ground surface in current segment iteration
  std::vector<Vec3> cur_p_list;
  std::vector<Vec3>& cur_cloud_seg = cloud_segs[0];

  //Loop through each segment and apply GPF
  for (i = 0; i < n_segs; i++) {

    cur_cloud_seg = cloud_segs[i];

    //Sort points in current segment by x value for segmentation
    std::sort(cur_cloud_seg.begin(), cur_cloud_seg.end(), CompareX);

    //Get initial ground points
    ExtractInitialSeeds(cur_cloud_seg, cur_p_gnd);

    for(int i = 0; i < n_iter; i++) {

      Vec3 normal = CalculatePlaneNormal(cur_p_gnd);

      double d = -((normal.x * cloud_centroid.x) + (normal.y * cloud_centroid.y) + (normal.z * cloud_centroid.z));
      double normal_mag = sqrt(normal.x * normal.x +normal.y * normal.y + normal.z * normal.z);

      if (normal_mag != 0) {

        //Clear gnd points in between iterations
        cur_p_gnd.clear();
        cur_p_ngnd.clear();
        cur_p_list.clear();

        //Calculate distance from point to orthogonal projection
        //and compare to th_dist
        double dist;
        std::vector<Vec3>::iterator seg_it;
        for (seg_it = cur_cloud_seg.begin(); seg_it != cur_cloud_seg.end(); seg_it++) {

          dist = abs(normal.x * seg_it->x + normal.y * seg_it->y + normal.z * seg_it->z + d) / normal_mag;

          if (dist < th_dist) {
            seg_it->label = -3;
            cur_p_gnd.push_back(*seg_it);
            cur_p_list.push_back(*seg_it);
          } else {
            cur_p_ngnd.push_back(*seg_it);
            cur_p_list.push_back(*seg_it);
          }

        }
      }
    }

    //Add results from current segment to main sets
    this->p_gnd.insert(p_gnd.end(), cur_p_gnd.begin(), cur_p_gnd.end());
    this->p_ngnd.insert(p_ngnd.end(), cur_p_ngnd.begin(), cur_p_ngnd.end());
    this->p_all.insert(p_all.end(), cur_p_list.begin(), cur_p_list.end());

    cur_p_gnd.clear();
    cur_p_ngnd.clear();
    cur_p_list.clear();

  }

  int stop_s = clock(); //Take end timestamp
  std::cout << "GPF runtime: " << (stop_s - start_s) / double(CLOCKS_PER_SEC) * 1000 << " ms" << std::endl;
}

// Assembly:
pushq %rbp
movq %rsp, %rbp
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0xd8, %rsp
movq %rsi, %rbx
movq %rdi, %r15
callq 0x2100
movq %rax, -0xb8(%rbp)
movq (%rbx), %r12
movq %rbx, -0x88(%rbp)
movq 0x8(%rbx), %rax
subq %r12, %rax
sarq $0x2, %rax
movabsq $0x6db6db6db6db6db7, %rcx # imm = 0x6DB6DB6DB6DB6DB7
imulq %rcx, %rax
movl 0x60(%r15), %ecx
movslq %ecx, %rcx
xorl %edx, %edx
divq %rcx
movq %rax, -0xa0(%rbp)
movl %ecx, %eax
movq %rsp, %rbx
movq %rax, -0xb0(%rbp)
leaq (%rax,%rax,2), %rax
leaq 0xf(,%rax,8), %rdx
andq $-0x10, %rdx
subq %rdx, %rbx
movq %rbx, %rsp
testl %ecx, %ecx
je 0x2675
leaq -0x18(,%rax,8), %rax
movabsq $-0x5555555555555555, %rcx # imm = 0xAAAAAAAAAAAAAAAB
mulq %rcx
shrq $0x4, %rdx
leaq (%rdx,%rdx,2), %rax
leaq 0x18(,%rax,8), %rdx
movq %rbx, %rdi
xorl %esi, %esi
callq 0x20d0
movq -0x88(%rbp), %rax
cmpq 0x8(%rax), %r12
je 0x26eb
xorl %r14d, %r14d
xorl %r13d, %r13d
cmpl -0xa0(%rbp), %r14d
jne 0x26a1
leal 0x1(%r13), %eax
cmpl 0x60(%r15), %eax
je 0x26a1
xorl %r14d, %r14d
movl %eax, %r13d
movslq %r13d, %rax
leaq (%rax,%rax,2), %rax
leaq (%rbx,%rax,8), %rdi
movq 0x8(%rdi), %rsi
cmpq 0x10(%rdi), %rsi
je 0x26cf
movups (%r12), %xmm0
movups 0xc(%r12), %xmm1
movups %xmm1, 0xc(%rsi)
movups %xmm0, (%rsi)
addq $0x1c, 0x8(%rdi)
jmp 0x26d7
movq %r12, %rdx
callq 0x56b8
addq $0x1c, %r12
incl %r14d
movq -0x88(%rbp), %rax
cmpq 0x8(%rax), %r12
jne 0x2688
xorps %xmm0, %xmm0
movaps %xmm0, -0x60(%rbp)
xorl %eax, %eax
movq %rax, -0x50(%rbp)
movq %rax, -0x70(%rbp)
movaps %xmm0, -0x80(%rbp)
movq %rax, -0x30(%rbp)
movaps %xmm0, -0x40(%rbp)
cmpl $0x0, 0x60(%r15)
jle 0x2a8d
leaq 0x18(%r15), %rax
movq %rax, -0xc8(%rbp)
leaq 0x30(%r15), %rax
movq %rax, -0xc0(%rbp)
xorl %ecx, %ecx
leaq -0x40(%rbp), %r13
movq %rcx, -0xd0(%rbp)
leaq (%rcx,%rcx,2), %rax
leaq (%rbx,%rax,8), %rsi
movq %rbx, %rdi
callq 0x4a2a
movq (%rbx), %r12
movq 0x8(%rbx), %r14
cmpq %r14, %r12
je 0x279b
movq %r14, %rax
subq %r12, %rax
sarq $0x2, %rax
movabsq $0x6db6db6db6db6db7, %rcx # imm = 0x6DB6DB6DB6DB6DB7
imulq %rcx, %rax
bsrq %rax, %rdx
xorl $0x3f, %edx
addl %edx, %edx
xorq $0x7e, %rdx
movq %r12, %rdi
movq %r14, %rsi
leaq -0x276(%rip), %rcx      # 0x250e
callq 0x5838
movq %r12, %rdi
movq %r14, %rsi
leaq -0x288(%rip), %rdx      # 0x250e
callq 0x59aa
movq %r15, %rdi
movq %rbx, %rsi
leaq -0x60(%rbp), %rdx
callq 0x2c20
cmpl $0x0, 0x58(%r15)
jle 0x2a08
xorl %r14d, %r14d
leaq -0xec(%rbp), %rdi
leaq -0x60(%rbp), %rdx
callq 0x2f52
movss -0xec(%rbp), %xmm1
movss -0xe8(%rbp), %xmm0
movss 0x89c4(%rip), %xmm3     # 0xb1a4
mulss %xmm0, %xmm3
movss 0x89b4(%rip), %xmm4     # 0xb1a0
mulss %xmm1, %xmm4
movss -0xe4(%rbp), %xmm2
movss 0x89a8(%rip), %xmm5     # 0xb1a8
mulss %xmm2, %xmm5
movss %xmm0, -0xa0(%rbp)
mulss %xmm0, %xmm0
movss %xmm1, -0x88(%rbp)
mulss %xmm1, %xmm1
addss %xmm0, %xmm1
movss %xmm2, -0xa4(%rbp)
movaps %xmm2, %xmm0
mulss %xmm2, %xmm0
addss %xmm1, %xmm0
ucomiss 0x583e(%rip), %xmm0     # 0x8078
jb 0x2842
sqrtss %xmm0, %xmm0
jmp 0x2875
movss %xmm3, -0x98(%rbp)
movss %xmm4, -0x90(%rbp)
movaps %xmm5, -0x100(%rbp)
callq 0x22d0
movaps -0x100(%rbp), %xmm5
movss -0x90(%rbp), %xmm4
movss -0x98(%rbp), %xmm3
ucomiss 0x57fc(%rip), %xmm0     # 0x8078
jne 0x2884
jnp 0x29fb
movq -0x60(%rbp), %rax
cmpq %rax, -0x58(%rbp)
je 0x2892
movq %rax, -0x58(%rbp)
movq -0x80(%rbp), %rax
cmpq %rax, -0x78(%rbp)
je 0x28a0
movq %rax, -0x78(%rbp)
movq -0x40(%rbp), %rax
cmpq %rax, -0x38(%rbp)
je 0x28ae
movq %rax, -0x38(%rbp)
movq (%rbx), %r12
cmpq 0x8(%rbx), %r12
je 0x29fb
addss %xmm3, %xmm4
addss %xmm4, %xmm5
xorps 0x5746(%rip), %xmm5     # 0x8010
xorps %xmm1, %xmm1
cvtss2sd %xmm5, %xmm1
movsd %xmm1, -0x98(%rbp)
cvtss2sd %xmm0, %xmm0
movsd %xmm0, -0x90(%rbp)
movss 0x4(%r12), %xmm0
mulss -0xa0(%rbp), %xmm0
movss (%r12), %xmm1
mulss -0x88(%rbp), %xmm1
addss %xmm0, %xmm1
movss 0x8(%r12), %xmm0
mulss -0xa4(%rbp), %xmm0
addss %xmm1, %xmm0
cvtss2sd %xmm0, %xmm0
addsd -0x98(%rbp), %xmm0
andpd 0x56f3(%rip), %xmm0     # 0x8020
divsd -0x90(%rbp), %xmm0
movsd 0x50(%r15), %xmm1
ucomisd %xmm0, %xmm1
jbe 0x2974
movl $0xc0400000, 0x18(%r12) # imm = 0xC0400000
movq -0x58(%rbp), %rsi
cmpq -0x50(%rbp), %rsi
je 0x299e
movupd (%r12), %xmm0
movupd 0xc(%r12), %xmm1
movupd %xmm1, 0xc(%rsi)
movupd %xmm0, (%rsi)
addq $0x1c, %rsi
movq %rsi, -0x58(%rbp)
jmp 0x29b8
movq -0x78(%rbp), %rsi
cmpq -0x70(%rbp), %rsi
je 0x29ac
movupd (%r12), %xmm0
movupd 0xc(%r12), %xmm1
movupd %xmm1, 0xc(%rsi)
movupd %xmm0, (%rsi)
addq $0x1c, %rsi
movq %rsi, -0x78(%rbp)
jmp 0x29b8
leaq -0x60(%rbp), %rdi
movq %r12, %rdx
callq 0x56b8
jmp 0x29b8
leaq -0x80(%rbp), %rdi
movq %r12, %rdx
callq 0x56b8
movq -0x38(%rbp), %rsi
cmpq -0x30(%rbp), %rsi
je 0x29e2
movupd (%r12), %xmm0
movupd 0xc(%r12), %xmm1
movupd %xmm1, 0xc(%rsi)
movupd %xmm0, (%rsi)
addq $0x1c, %rsi
movq %rsi, -0x38(%rbp)
jmp 0x29ed
movq %r13, %rdi
movq %r12, %rdx
callq 0x56b8
addq $0x1c, %r12
cmpq 0x8(%rbx), %r12
jne 0x28e5
incl %r14d
cmpl 0x58(%r15), %r14d
jl 0x27b8
movq 0x8(%r15), %rsi
movq -0x60(%rbp), %rdx
movq -0x58(%rbp), %rcx
movq %r15, %rdi
callq 0x5f70
movq 0x20(%r15), %rsi
movq -0x80(%rbp), %rdx
movq -0x78(%rbp), %rcx
movq -0xc8(%rbp), %rdi
callq 0x5f70
movq 0x38(%r15), %rsi
movq -0x40(%rbp), %rdx
movq -0x38(%rbp), %rcx
movq -0xc0(%rbp), %rdi
callq 0x5f70
movq -0x60(%rbp), %rax
cmpq %rax, -0x58(%rbp)
movq -0xd0(%rbp), %rcx
je 0x2a61
movq %rax, -0x58(%rbp)
movq -0x80(%rbp), %rax
cmpq %rax, -0x78(%rbp)
je 0x2a6f
movq %rax, -0x78(%rbp)
movq -0x40(%rbp), %rax
cmpq %rax, -0x38(%rbp)
je 0x2a7d
movq %rax, -0x38(%rbp)
incq %rcx
movslq 0x60(%r15), %rax
cmpq %rax, %rcx
jl 0x272f
callq 0x2100
movq %rax, %r14
movq 0x852c(%rip), %rdi      # 0xafc8
leaq 0x5607(%rip), %rsi      # 0x80aa
movl $0xd, %edx
callq 0x21d0
subl -0xb8(%rbp), %r14d
xorps %xmm0, %xmm0
cvtsi2sd %r14d, %xmm0
divsd 0x558c(%rip), %xmm0     # 0x8050
mulsd 0x558c(%rip), %xmm0     # 0x8058
movq 0x84f5(%rip), %rdi      # 0xafc8
callq 0x2260
movq %rax, %r14
leaq 0x55d6(%rip), %rsi      # 0x80b8
movl $0x3, %edx
movq %rax, %rdi
callq 0x21d0
movq (%r14), %rax
movq -0x18(%rax), %rdi
addq %r14, %rdi
movl $0xa, %esi
callq 0x21a0
movsbl %al, %esi
movq %r14, %rdi
callq 0x2030
movq %rax, %rdi
callq 0x2120
movq -0x40(%rbp), %rdi
testq %rdi, %rdi
movq -0xb0(%rbp), %r14
je 0x2b32
movq -0x30(%rbp), %rsi
subq %rdi, %rsi
callq 0x21c0
movq -0x80(%rbp), %rdi
testq %rdi, %rdi
je 0x2b47
movq -0x70(%rbp), %rsi
subq %rdi, %rsi
callq 0x21c0
movq -0x60(%rbp), %rdi
testq %rdi, %rdi
je 0x2b5c
movq -0x50(%rbp), %rsi
subq %rdi, %rsi
callq 0x21c0
testq %r14, %r14
je 0x2b86
shlq $0x3, %r14
leaq (%r14,%r14,2), %r14
movq -0x18(%rbx,%r14), %rdi
testq %rdi, %rdi
je 0x2b80
movq -0x8(%rbx,%r14), %rsi
subq %rdi, %rsi
callq 0x21c0
addq $-0x18, %r14
jne 0x2b69
leaq -0x28(%rbp), %rsp
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
jmp 0x2ba4
movq %rax, %r14
jmp 0x2be6
jmp 0x2ba4
jmp 0x2ba4
jmp 0x2ba4
jmp 0x2ba4
movq %rax, %r14
movq -0x40(%rbp), %rdi
testq %rdi, %rdi
je 0x2bbc
movq -0x30(%rbp), %rsi
subq %rdi, %rsi
callq 0x21c0
movq -0x80(%rbp), %rdi
testq %rdi, %rdi
je 0x2bd1
movq -0x70(%rbp), %rsi
subq %rdi, %rsi
callq 0x21c0
movq -0x60(%rbp), %rdi
testq %rdi, %rdi
je 0x2be6
movq -0x50(%rbp), %rsi
subq %rdi, %rsi
callq 0x21c0
movq -0xb0(%rbp), %rax
testq %rax, %rax
je 0x2c17
shlq $0x3, %rax
leaq (%rax,%rax,2), %r15
movq -0x18(%rbx,%r15), %rdi
testq %rdi, %rdi
je 0x2c11
movq -0x8(%rbx,%r15), %rsi
subq %rdi, %rsi
callq 0x21c0
addq $-0x18, %r15
jne 0x2bfa
movq %r14, %rdi
callq 0x22b0
nop