// File: /workspace/github_cpp_makefile_star2_O2/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 0x2110
movq %rax, -0xb0(%rbp)
movq (%rbx), %r14
movq %rbx, -0x40(%rbp)
movq 0x8(%rbx), %rsi
movq %rsi, %rax
subq %r14, %rax
pushq $0x1c
popq %rcx
cqto
idivq %rcx
movl 0x60(%r15), %ecx
movslq %ecx, %rcx
movq $0x0, -0x30(%rbp)
xorl %edx, %edx
divq %rcx
movq %rax, %rdx
movl %ecx, %eax
movq %rsp, %rbx
movq %rax, -0xa8(%rbp)
imulq $0x18, %rax, %r12
leaq 0xf(%r12), %rax
andq $-0x10, %rax
subq %rax, %rbx
movq %rbx, %rsp
testl %ecx, %ecx
movq %rdx, -0x38(%rbp)
je 0x2565
leaq -0x18(%r12), %rcx
movq %rsi, %r13
pushq $0x18
popq %rsi
movq %rcx, %rax
xorl %edx, %edx
divq %rsi
subq %rdx, %rcx
addq $0x18, %rcx
movq %rbx, %rdi
xorl %esi, %esi
movq %rcx, %rdx
callq 0x20e0
movq %r13, %rsi
movq -0x38(%rbp), %rdx
xorl %r13d, %r13d
cmpq %rsi, %r14
je 0x25b0
cmpl %edx, %r13d
jne 0x2588
movq -0x30(%rbp), %rax
incl %eax
movl %edx, %r13d
cmpl 0x60(%r15), %eax
je 0x2588
xorl %r13d, %r13d
movq %rax, -0x30(%rbp)
movslq -0x30(%rbp), %rax
imulq $0x18, %rax, %rdi
addq %rbx, %rdi
movq %r14, %rsi
callq 0x3ca0
addq $0x1c, %r14
incl %r13d
movq -0x40(%rbp), %rax
movq 0x8(%rax), %rsi
movq -0x38(%rbp), %rdx
jmp 0x2568
leaq -0x60(%rbp), %rax
andq $0x0, 0x10(%rax)
xorpd %xmm0, %xmm0
movapd %xmm0, (%rax)
leaq -0x80(%rbp), %rax
movapd %xmm0, (%rax)
andq $0x0, 0x10(%rax)
leaq -0xa0(%rbp), %r13
movapd %xmm0, (%r13)
andq $0x0, 0x10(%r13)
leaq 0x18(%r15), %rax
movq %rax, -0xc0(%rbp)
leaq 0x30(%r15), %rax
movq %rax, -0xb8(%rbp)
xorl %ecx, %ecx
movslq 0x60(%r15), %rax
cmpq %rax, %rcx
jge 0x284c
movq %rcx, -0xc8(%rbp)
imulq $0x18, %rcx, %rsi
addq %rbx, %rsi
movq %rbx, %rdi
callq 0x3ccc
movq (%rbx), %rdi
movq 0x8(%rbx), %rsi
leaq -0x1fb(%rip), %rdx      # 0x242e
callq 0x4ae2
movq %r15, %rdi
movq %rbx, %rsi
leaq -0x60(%rbp), %rdx
callq 0x2948
xorl %eax, %eax
cmpl 0x58(%r15), %eax
jge 0x27c0
movl %eax, -0x84(%rbp)
leaq -0xf4(%rbp), %rdi
leaq -0x60(%rbp), %rdx
callq 0x2a0a
movss -0xf4(%rbp), %xmm1
movss -0xf0(%rbp), %xmm0
movss -0xec(%rbp), %xmm2
movss %xmm0, -0x38(%rbp)
mulss %xmm0, %xmm0
movss %xmm1, -0x30(%rbp)
mulss %xmm1, %xmm1
addss %xmm0, %xmm1
movss %xmm2, -0x40(%rbp)
movaps %xmm2, %xmm0
mulss %xmm2, %xmm0
addss %xmm1, %xmm0
sqrtss %xmm0, %xmm0
ucomiss 0x49d8(%rip), %xmm0     # 0x7080
jne 0x26b0
jnp 0x27b3
movq -0x60(%rbp), %rax
cmpq %rax, -0x58(%rbp)
je 0x26be
movq %rax, -0x58(%rbp)
movq -0x80(%rbp), %rax
cmpq %rax, -0x78(%rbp)
je 0x26cc
movq %rax, -0x78(%rbp)
movq -0xa0(%rbp), %rax
cmpq %rax, -0x98(%rbp)
je 0x26e3
movq %rax, -0x98(%rbp)
movss 0x8aa1(%rip), %xmm1     # 0xb18c
mulss -0x38(%rbp), %xmm1
movss 0x8a90(%rip), %xmm2     # 0xb188
mulss -0x30(%rbp), %xmm2
addss %xmm1, %xmm2
movss 0x8a87(%rip), %xmm1     # 0xb190
mulss -0x40(%rbp), %xmm1
addss %xmm2, %xmm1
xorps 0x4917(%rip), %xmm1     # 0x7030
cvtss2sd %xmm1, %xmm1
movsd %xmm1, -0xd8(%rbp)
cvtss2sd %xmm0, %xmm0
movsd %xmm0, -0xd0(%rbp)
movq (%rbx), %r14
cmpq 0x8(%rbx), %r14
je 0x27b3
movss 0x4(%r14), %xmm0
mulss -0x38(%rbp), %xmm0
movss (%r14), %xmm1
mulss -0x30(%rbp), %xmm1
addss %xmm0, %xmm1
movss 0x8(%r14), %xmm0
mulss -0x40(%rbp), %xmm0
addss %xmm1, %xmm0
cvtss2sd %xmm0, %xmm0
addsd -0xd8(%rbp), %xmm0
andpd 0x48ca(%rip), %xmm0     # 0x7040
divsd -0xd0(%rbp), %xmm0
movsd 0x50(%r15), %xmm1
ucomisd %xmm0, %xmm1
leaq -0x80(%rbp), %rdi
jbe 0x279a
movl $0xc0400000, 0x18(%r14) # imm = 0xC0400000
leaq -0x60(%rbp), %rdi
movq %r14, %rsi
callq 0x3ca0
movq %r13, %rdi
movq %r14, %rsi
callq 0x3ca0
addq $0x1c, %r14
jmp 0x2734
movl -0x84(%rbp), %eax
incl %eax
jmp 0x263f
movq 0x8(%r15), %rsi
movq -0x60(%rbp), %rdx
movq -0x58(%rbp), %rcx
movq %r15, %rdi
callq 0x3db6
movq 0x20(%r15), %rsi
movq -0x80(%rbp), %rdx
movq -0x78(%rbp), %rcx
movq -0xc0(%rbp), %rdi
callq 0x3db6
movq 0x38(%r15), %rsi
movq -0xa0(%rbp), %rdx
movq -0x98(%rbp), %rcx
movq -0xb8(%rbp), %rdi
callq 0x3db6
movq -0x60(%rbp), %rax
cmpq %rax, -0x58(%rbp)
movq -0xc8(%rbp), %rcx
je 0x281f
movq %rax, -0x58(%rbp)
movq -0x80(%rbp), %rax
cmpq %rax, -0x78(%rbp)
je 0x282d
movq %rax, -0x78(%rbp)
movq -0xa0(%rbp), %rax
cmpq %rax, -0x98(%rbp)
je 0x2844
movq %rax, -0x98(%rbp)
incq %rcx
jmp 0x25f8
callq 0x2110
movq %rax, %r14
movq 0x876d(%rip), %rdi      # 0xafc8
leaq 0x4850(%rip), %rsi      # 0x70b2
callq 0x2190
subl -0xb0(%rbp), %r14d
cvtsi2sd %r14d, %xmm0
divsd 0x478d(%rip), %xmm0     # 0x7008
mulsd 0x478d(%rip), %xmm0     # 0x7010
movq %rax, %rdi
callq 0x2240
leaq 0x482e(%rip), %rsi      # 0x70c0
movq %rax, %rdi
callq 0x2190
movq %rax, %rdi
callq 0x20b0
leaq -0xa0(%rbp), %rdi
callq 0x47fe
leaq -0x80(%rbp), %rdi
callq 0x47fe
leaq -0x60(%rbp), %rdi
callq 0x47fe
movq -0xa8(%rbp), %rax
testq %rax, %rax
je 0x28e3
addq $-0x18, %rbx
imulq $0x18, %rax, %r14
leaq (%rbx,%r14), %rdi
callq 0x47fe
addq $-0x18, %r14
jne 0x28d4
leaq -0x28(%rbp), %rsp
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
jmp 0x2901
jmp 0x2901
jmp 0x2901
jmp 0x2901
movq %rax, %r14
jmp 0x2922
jmp 0x2901
movq %rax, %r14
leaq -0xa0(%rbp), %rdi
callq 0x47fe
leaq -0x80(%rbp), %rdi
callq 0x47fe
leaq -0x60(%rbp), %rdi
callq 0x47fe
cmpq $0x0, -0xa8(%rbp)
je 0x293f
addq $-0x18, %rbx
leaq (%rbx,%r12), %rdi
callq 0x47fe
addq $-0x18, %r12
jne 0x2930
movq %r14, %rdi
callq 0x2290
nop