// File: /workspace/github_cpp_makefile_star2_O3/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, -0xa8(%rbp)
movq 0x8(%rbx), %r14
movq %r14, %rax
subq %r12, %rax
sarq $0x2, %rax
movabsq $0x6db6db6db6db6db7, %rcx # imm = 0x6DB6DB6DB6DB6DB7
imulq %rcx, %rax
movl 0x60(%r15), %esi
movslq %esi, %rcx
xorl %edx, %edx
divq %rcx
movq %rax, %r13
movl %esi, %eax
movq %rsp, %rbx
movq %rax, -0xb0(%rbp)
leaq (%rax,%rax,2), %rax
leaq 0xf(,%rax,8), %rcx
andq $-0x10, %rcx
subq %rcx, %rbx
movq %rbx, %rsp
movl %esi, -0x2c(%rbp)
testl %esi, %esi
je 0x2657
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
cmpq %r14, %r12
je 0x26d8
movq %r13, -0xa0(%rbp)
xorl %r13d, %r13d
xorl %r14d, %r14d
movq -0xa0(%rbp), %rcx
cmpl %ecx, %r13d
jne 0x2688
leal 0x1(%r14), %eax
movl %ecx, %r13d
cmpl 0x60(%r15), %eax
je 0x2688
xorl %r13d, %r13d
movl %eax, %r14d
movslq %r14d, %rax
leaq (%rax,%rax,2), %rax
leaq (%rbx,%rax,8), %rdi
movq 0x8(%rdi), %rsi
cmpq 0x10(%rdi), %rsi
je 0x26b6
movups (%r12), %xmm0
movups 0xc(%r12), %xmm1
movups %xmm1, 0xc(%rsi)
movups %xmm0, (%rsi)
addq $0x1c, 0x8(%rdi)
jmp 0x26be
movq %r12, %rdx
callq 0x5594
addq $0x1c, %r12
incl %r13d
movq -0xa8(%rbp), %rax
cmpq 0x8(%rax), %r12
jne 0x2669
movl 0x60(%r15), %eax
jmp 0x26db
movl -0x2c(%rbp), %eax
xorps %xmm0, %xmm0
movaps %xmm0, -0x70(%rbp)
xorl %ecx, %ecx
movq %rcx, -0x60(%rbp)
movq %rcx, -0x80(%rbp)
movaps %xmm0, -0x90(%rbp)
movq %rcx, -0x40(%rbp)
movaps %xmm0, -0x50(%rbp)
testl %eax, %eax
jle 0x2a69
leaq 0x18(%r15), %rax
movq %rax, -0xc8(%rbp)
leaq 0x30(%r15), %rax
movq %rax, -0xc0(%rbp)
xorl %ecx, %ecx
leaq -0x50(%rbp), %r13
movq %rcx, -0xd0(%rbp)
leaq (%rcx,%rcx,2), %rax
leaq (%rbx,%rax,8), %rsi
movq %rbx, %rdi
callq 0x490c
movq (%rbx), %r12
movq 0x8(%rbx), %r14
cmpq %r14, %r12
je 0x278b
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 -0x286(%rip), %rcx      # 0x24ee
callq 0x5714
movq %r12, %rdi
movq %r14, %rsi
leaq -0x298(%rip), %rdx      # 0x24ee
callq 0x5886
movq %r15, %rdi
movq %rbx, %rsi
leaq -0x70(%rbp), %rdx
callq 0x2c04
cmpl $0x0, 0x58(%r15)
jle 0x29d5
xorl %r14d, %r14d
leaq -0xfc(%rbp), %rdi
leaq -0x70(%rbp), %rdx
callq 0x2f34
movss -0xfc(%rbp), %xmm1
movss -0xf8(%rbp), %xmm0
movss -0xf4(%rbp), %xmm2
movss %xmm0, -0xa0(%rbp)
mulss %xmm0, %xmm0
movss %xmm1, -0xa8(%rbp)
mulss %xmm1, %xmm1
addss %xmm0, %xmm1
movss %xmm2, -0x94(%rbp)
movaps %xmm2, %xmm0
mulss %xmm2, %xmm0
addss %xmm1, %xmm0
sqrtss %xmm0, %xmm0
ucomiss 0x586e(%rip), %xmm0     # 0x8078
jne 0x2812
jnp 0x29c8
movq -0x70(%rbp), %rax
cmpq %rax, -0x68(%rbp)
je 0x2820
movq %rax, -0x68(%rbp)
movq -0x90(%rbp), %rax
cmpq %rax, -0x88(%rbp)
je 0x2837
movq %rax, -0x88(%rbp)
movq -0x50(%rbp), %rax
cmpq %rax, -0x48(%rbp)
je 0x2845
movq %rax, -0x48(%rbp)
movq (%rbx), %r12
cmpq 0x8(%rbx), %r12
je 0x29c8
movss 0x893a(%rip), %xmm1     # 0xb194
mulss -0xa0(%rbp), %xmm1
movss 0x8926(%rip), %xmm2     # 0xb190
mulss -0xa8(%rbp), %xmm2
addss %xmm1, %xmm2
movss 0x891a(%rip), %xmm1     # 0xb198
mulss -0x94(%rbp), %xmm1
addss %xmm2, %xmm1
xorps 0x577f(%rip), %xmm1     # 0x8010
cvtss2sd %xmm1, %xmm1
movsd %xmm1, -0xe0(%rbp)
cvtss2sd %xmm0, %xmm0
movsd %xmm0, -0xd8(%rbp)
movss 0x4(%r12), %xmm0
mulss -0xa0(%rbp), %xmm0
movss (%r12), %xmm1
mulss -0xa8(%rbp), %xmm1
addss %xmm0, %xmm1
movss 0x8(%r12), %xmm0
mulss -0x94(%rbp), %xmm0
addss %xmm1, %xmm0
cvtss2sd %xmm0, %xmm0
addsd -0xe0(%rbp), %xmm0
andpd 0x572f(%rip), %xmm0     # 0x8020
divsd -0xd8(%rbp), %xmm0
movsd 0x50(%r15), %xmm1
ucomisd %xmm0, %xmm1
jbe 0x2938
movl $0xc0400000, 0x18(%r12) # imm = 0xC0400000
movq -0x68(%rbp), %rsi
cmpq -0x60(%rbp), %rsi
je 0x2968
movupd (%r12), %xmm0
movupd 0xc(%r12), %xmm1
movupd %xmm1, 0xc(%rsi)
movupd %xmm0, (%rsi)
addq $0x1c, %rsi
movq %rsi, -0x68(%rbp)
jmp 0x2985
movq -0x88(%rbp), %rsi
cmpq -0x80(%rbp), %rsi
je 0x2976
movupd (%r12), %xmm0
movupd 0xc(%r12), %xmm1
movupd %xmm1, 0xc(%rsi)
movupd %xmm0, (%rsi)
addq $0x1c, %rsi
movq %rsi, -0x88(%rbp)
jmp 0x2985
leaq -0x70(%rbp), %rdi
movq %r12, %rdx
callq 0x5594
jmp 0x2985
leaq -0x90(%rbp), %rdi
movq %r12, %rdx
callq 0x5594
movq -0x48(%rbp), %rsi
cmpq -0x40(%rbp), %rsi
je 0x29af
movupd (%r12), %xmm0
movupd 0xc(%r12), %xmm1
movupd %xmm1, 0xc(%rsi)
movupd %xmm0, (%rsi)
addq $0x1c, %rsi
movq %rsi, -0x48(%rbp)
jmp 0x29ba
movq %r13, %rdi
movq %r12, %rdx
callq 0x5594
addq $0x1c, %r12
cmpq 0x8(%rbx), %r12
jne 0x28a9
incl %r14d
cmpl 0x58(%r15), %r14d
jl 0x27a8
movq 0x8(%r15), %rsi
movq -0x70(%rbp), %rdx
movq -0x68(%rbp), %rcx
movq %r15, %rdi
callq 0x5e2e
movq 0x20(%r15), %rsi
movq -0x90(%rbp), %rdx
movq -0x88(%rbp), %rcx
movq -0xc8(%rbp), %rdi
callq 0x5e2e
movq 0x38(%r15), %rsi
movq -0x50(%rbp), %rdx
movq -0x48(%rbp), %rcx
movq -0xc0(%rbp), %rdi
callq 0x5e2e
movq -0x70(%rbp), %rax
cmpq %rax, -0x68(%rbp)
movq -0xd0(%rbp), %rcx
je 0x2a34
movq %rax, -0x68(%rbp)
movq -0x90(%rbp), %rax
cmpq %rax, -0x88(%rbp)
je 0x2a4b
movq %rax, -0x88(%rbp)
movq -0x50(%rbp), %rax
cmpq %rax, -0x48(%rbp)
je 0x2a59
movq %rax, -0x48(%rbp)
incq %rcx
movslq 0x60(%r15), %rax
cmpq %rax, %rcx
jl 0x271f
callq 0x2100
movq %rax, %r14
movq 0x8550(%rip), %rdi      # 0xafc8
leaq 0x562b(%rip), %rsi      # 0x80aa
movl $0xd, %edx
callq 0x21c0
subl -0xb8(%rbp), %r14d
xorps %xmm0, %xmm0
cvtsi2sd %r14d, %xmm0
divsd 0x55b0(%rip), %xmm0     # 0x8050
mulsd 0x55b0(%rip), %xmm0     # 0x8058
movq 0x8519(%rip), %rdi      # 0xafc8
callq 0x2250
movq %rax, %r14
leaq 0x55fa(%rip), %rsi      # 0x80b8
movl $0x3, %edx
movq %rax, %rdi
callq 0x21c0
movq (%r14), %rax
movq -0x18(%rax), %rdi
addq %r14, %rdi
movl $0xa, %esi
callq 0x2190
movsbl %al, %esi
movq %r14, %rdi
callq 0x2030
movq %rax, %rdi
callq 0x2120
movq -0x50(%rbp), %rdi
testq %rdi, %rdi
je 0x2b07
movq -0x40(%rbp), %rsi
subq %rdi, %rsi
callq 0x21b0
movq -0x90(%rbp), %rdi
testq %rdi, %rdi
je 0x2b1f
movq -0x80(%rbp), %rsi
subq %rdi, %rsi
callq 0x21b0
movq -0x70(%rbp), %rdi
testq %rdi, %rdi
je 0x2b34
movq -0x60(%rbp), %rsi
subq %rdi, %rsi
callq 0x21b0
cmpl $0x0, -0x2c(%rbp)
je 0x2b66
movq -0xb0(%rbp), %rax
shlq $0x3, %rax
leaq (%rax,%rax,2), %r14
movq -0x18(%rbx,%r14), %rdi
testq %rdi, %rdi
je 0x2b60
movq -0x8(%rbx,%r14), %rsi
subq %rdi, %rsi
callq 0x21b0
addq $-0x18, %r14
jne 0x2b49
leaq -0x28(%rbp), %rsp
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
jmp 0x2b84
movq %rax, %r14
jmp 0x2bc9
jmp 0x2b84
jmp 0x2b84
jmp 0x2b84
jmp 0x2b84
movq %rax, %r14
movq -0x50(%rbp), %rdi
testq %rdi, %rdi
je 0x2b9c
movq -0x40(%rbp), %rsi
subq %rdi, %rsi
callq 0x21b0
movq -0x90(%rbp), %rdi
testq %rdi, %rdi
je 0x2bb4
movq -0x80(%rbp), %rsi
subq %rdi, %rsi
callq 0x21b0
movq -0x70(%rbp), %rdi
testq %rdi, %rdi
je 0x2bc9
movq -0x60(%rbp), %rsi
subq %rdi, %rsi
callq 0x21b0
cmpl $0x0, -0x2c(%rbp)
je 0x2bfb
movq -0xb0(%rbp), %rax
shlq $0x3, %rax
leaq (%rax,%rax,2), %r15
movq -0x18(%rbx,%r15), %rdi
testq %rdi, %rdi
je 0x2bf5
movq -0x8(%rbx,%r15), %rsi
subq %rdi, %rsi
callq 0x21b0
addq $-0x18, %r15
jne 0x2bde
movq %r14, %rdi
callq 0x22a0
nop