// File: 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
subq $0x280, %rsp            # imm = 0x280
movq %rdi, -0x8(%rbp)
movq %rsi, -0x10(%rbp)
movq -0x8(%rbp), %rax
movq %rax, -0x1b0(%rbp)
callq 0x2110
movl %eax, -0x14(%rbp)
movq -0x10(%rbp), %rdi
callq 0x5780
movq -0x1b0(%rbp), %rcx
movslq 0x60(%rcx), %rcx
xorl %edx, %edx
divq %rcx
movq %rax, %rcx
movq -0x1b0(%rbp), %rax
movl %ecx, -0x18(%rbp)
movl 0x60(%rax), %eax
movq %rax, -0x1a8(%rbp)
movq %rsp, %rcx
movq %rcx, -0x20(%rbp)
leaq (%rax,%rax,2), %rcx
leaq 0xf(,%rcx,8), %rdx
andq $-0x10, %rdx
movq %rsp, %rcx
subq %rdx, %rcx
movq %rcx, -0x1a0(%rbp)
movq %rcx, %rsp
movq %rax, -0x28(%rbp)
cmpq $0x0, %rax
je 0x273e
movq -0x1a0(%rbp), %rax
movq -0x1a8(%rbp), %rcx
imulq $0x18, %rcx, %rdx
movq %rax, %rcx
addq %rdx, %rcx
movq %rcx, -0x1c0(%rbp)
movq %rax, -0x1b8(%rbp)
movq -0x1b8(%rbp), %rdi
movq %rdi, -0x1c8(%rbp)
callq 0x57b0
movq -0x1c8(%rbp), %rax
movq -0x1c0(%rbp), %rcx
addq $0x18, %rax
cmpq %rcx, %rax
movq %rax, -0x1b8(%rbp)
jne 0x270d
leaq -0x30(%rbp), %rdi
callq 0x57d0
movl $0x0, -0x34(%rbp)
movl $0x0, -0x38(%rbp)
movq -0x10(%rbp), %rdi
callq 0x57f0
movq %rax, -0x40(%rbp)
movq -0x40(%rbp), %rax
movq %rax, -0x30(%rbp)
movq -0x10(%rbp), %rdi
callq 0x5860
movq %rax, -0x48(%rbp)
leaq -0x30(%rbp), %rdi
leaq -0x48(%rbp), %rsi
callq 0x5820
testb $0x1, %al
jne 0x278d
jmp 0x281d
movl -0x34(%rbp), %eax
cmpl -0x18(%rbp), %eax
jne 0x27b9
movq -0x1b0(%rbp), %rcx
movl -0x38(%rbp), %eax
addl $0x1, %eax
cmpl 0x60(%rcx), %eax
je 0x27b7
movl -0x38(%rbp), %eax
addl $0x1, %eax
movl %eax, -0x38(%rbp)
movl $0x0, -0x34(%rbp)
jmp 0x27b9
movq -0x1a0(%rbp), %rax
movslq -0x38(%rbp), %rcx
leaq (%rcx,%rcx,2), %rcx
leaq (%rax,%rcx,8), %rax
movq %rax, -0x1d0(%rbp)
leaq -0x30(%rbp), %rdi
callq 0x5900
movq -0x1d0(%rbp), %rdi
movq %rax, %rsi
callq 0x5890
jmp 0x27ed
jmp 0x27ef
leaq -0x30(%rbp), %rdi
xorl %esi, %esi
callq 0x5920
movq %rax, -0x60(%rbp)
movl -0x34(%rbp), %eax
addl $0x1, %eax
movl %eax, -0x34(%rbp)
jmp 0x276a
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x50(%rbp)
movl %eax, -0x54(%rbp)
jmp 0x2f52
leaq -0x78(%rbp), %rdi
callq 0x57b0
leaq -0x90(%rbp), %rdi
callq 0x57b0
leaq -0xa8(%rbp), %rdi
callq 0x57b0
movq -0x1a0(%rbp), %rax
movq %rax, -0xb0(%rbp)
movl $0x0, -0x34(%rbp)
movq -0x1b0(%rbp), %rcx
movl -0x34(%rbp), %eax
cmpl 0x60(%rcx), %eax
jge 0x2e3c
movq -0x1a0(%rbp), %rax
movslq -0x34(%rbp), %rcx
leaq (%rcx,%rcx,2), %rcx
leaq (%rax,%rcx,8), %rsi
movq -0xb0(%rbp), %rdi
callq 0x5960
jmp 0x2887
movq -0xb0(%rbp), %rdi
callq 0x57f0
movq %rax, -0xb8(%rbp)
movq -0xb0(%rbp), %rdi
callq 0x5860
movq %rax, -0xc0(%rbp)
movq -0xb8(%rbp), %rdi
movq -0xc0(%rbp), %rsi
leaq -0x352(%rip), %rdx      # 0x2570
callq 0x5bb0
jmp 0x28c9
movq -0x1b0(%rbp), %rdi
movq -0xb0(%rbp), %rsi
leaq -0x78(%rbp), %rdx
callq 0x2fc0
jmp 0x28e2
movl $0x0, -0xc4(%rbp)
movq -0x1b0(%rbp), %rcx
movl -0xc4(%rbp), %eax
cmpl 0x58(%rcx), %eax
jge 0x2c59
movq -0x1b0(%rbp), %rsi
leaq -0xe0(%rbp), %rdi
leaq -0x78(%rbp), %rdx
callq 0x31f0
jmp 0x291b
movss 0x178ed(%rip), %xmm2    # 0x1a210
movss -0xe0(%rbp), %xmm1
movss -0xdc(%rbp), %xmm0
movss 0x178d9(%rip), %xmm3    # 0x1a214
mulss %xmm3, %xmm0
mulss %xmm2, %xmm1
addss %xmm0, %xmm1
movss -0xd8(%rbp), %xmm0
movss 0x178c1(%rip), %xmm2    # 0x1a218
mulss %xmm2, %xmm0
addss %xmm1, %xmm0
movaps 0xf6fa(%rip), %xmm1     # 0x12060
pxor %xmm1, %xmm0
cvtss2sd %xmm0, %xmm0
movsd %xmm0, -0xe8(%rbp)
movss -0xe0(%rbp), %xmm1
movss -0xdc(%rbp), %xmm0
mulss %xmm0, %xmm0
mulss %xmm1, %xmm1
addss %xmm0, %xmm1
movss -0xd8(%rbp), %xmm0
mulss %xmm0, %xmm0
addss %xmm1, %xmm0
callq 0x5c00
movss %xmm0, -0x1d4(%rbp)
jmp 0x29b1
movss -0x1d4(%rbp), %xmm0
cvtss2sd %xmm0, %xmm0
movsd %xmm0, -0xf0(%rbp)
movsd -0xf0(%rbp), %xmm0
xorps %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jne 0x29dd
jp 0x29dd
jmp 0x2c43
leaq -0x78(%rbp), %rdi
callq 0x5c20
leaq -0x90(%rbp), %rdi
callq 0x5c20
leaq -0xa8(%rbp), %rdi
callq 0x5c20
leaq -0x100(%rbp), %rdi
callq 0x57d0
movq -0xb0(%rbp), %rdi
callq 0x57f0
movq %rax, -0x108(%rbp)
movq -0x108(%rbp), %rax
movq %rax, -0x100(%rbp)
movq -0xb0(%rbp), %rdi
callq 0x5860
movq %rax, -0x110(%rbp)
leaq -0x100(%rbp), %rdi
leaq -0x110(%rbp), %rsi
callq 0x5820
testb $0x1, %al
jne 0x2a5a
jmp 0x2c41
movss -0xe0(%rbp), %xmm0
movss %xmm0, -0x1f8(%rbp)
leaq -0x100(%rbp), %rdi
movq %rdi, -0x1f0(%rbp)
callq 0x5c60
movq -0x1f0(%rbp), %rdi
movss (%rax), %xmm0
movss %xmm0, -0x1f4(%rbp)
movss -0xdc(%rbp), %xmm0
movss %xmm0, -0x1fc(%rbp)
callq 0x5c60
movss -0x1fc(%rbp), %xmm1
movss -0x1f8(%rbp), %xmm0
movss -0x1f4(%rbp), %xmm2
movq -0x1f0(%rbp), %rdi
movss 0x4(%rax), %xmm3
mulss %xmm3, %xmm1
mulss %xmm2, %xmm0
addss %xmm1, %xmm0
movss %xmm0, -0x1e4(%rbp)
movss -0xd8(%rbp), %xmm0
movss %xmm0, -0x1e8(%rbp)
callq 0x5c60
movss -0x1e8(%rbp), %xmm0
movss -0x1e4(%rbp), %xmm1
movss 0x8(%rax), %xmm2
mulss %xmm2, %xmm0
addss %xmm1, %xmm0
cvtss2sd %xmm0, %xmm0
movsd -0xe8(%rbp), %xmm1
addsd %xmm1, %xmm0
callq 0x5c40
movsd %xmm0, -0x1e0(%rbp)
jmp 0x2b2e
movq -0x1b0(%rbp), %rax
movsd -0x1e0(%rbp), %xmm0
divsd -0xf0(%rbp), %xmm0
movsd %xmm0, -0xf8(%rbp)
movsd -0xf8(%rbp), %xmm1
movsd 0x50(%rax), %xmm0
ucomisd %xmm1, %xmm0
jbe 0x2be9
leaq -0x100(%rbp), %rdi
movq %rdi, -0x208(%rbp)
callq 0x5c60
movq -0x208(%rbp), %rdi
movl $0xc0400000, 0x18(%rax) # imm = 0xC0400000
callq 0x5900
movq %rax, %rsi
leaq -0x78(%rbp), %rdi
callq 0x5890
jmp 0x2b98
leaq -0x100(%rbp), %rdi
callq 0x5900
movq %rax, %rsi
leaq -0xa8(%rbp), %rdi
callq 0x5890
jmp 0x2bb5
jmp 0x2c25
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x50(%rbp)
movl %eax, -0x54(%rbp)
leaq -0xa8(%rbp), %rdi
callq 0x5d70
leaq -0x90(%rbp), %rdi
callq 0x5d70
leaq -0x78(%rbp), %rdi
callq 0x5d70
jmp 0x2f52
leaq -0x100(%rbp), %rdi
callq 0x5900
movq %rax, %rsi
leaq -0x90(%rbp), %rdi
callq 0x5890
jmp 0x2c06
leaq -0x100(%rbp), %rdi
callq 0x5900
movq %rax, %rsi
leaq -0xa8(%rbp), %rdi
callq 0x5890
jmp 0x2c23
jmp 0x2c25
jmp 0x2c27
leaq -0x100(%rbp), %rdi
xorl %esi, %esi
callq 0x5920
movq %rax, -0x118(%rbp)
jmp 0x2a2b
jmp 0x2c43
jmp 0x2c45
movl -0xc4(%rbp), %eax
addl $0x1, %eax
movl %eax, -0xc4(%rbp)
jmp 0x28ec
movq -0x1b0(%rbp), %rdi
callq 0x5860
movq %rax, -0x128(%rbp)
leaq -0x120(%rbp), %rdi
leaq -0x128(%rbp), %rsi
callq 0x5d30
leaq -0x78(%rbp), %rdi
movq %rdi, -0x218(%rbp)
callq 0x57f0
movq -0x218(%rbp), %rdi
movq %rax, -0x130(%rbp)
callq 0x5860
movq -0x1b0(%rbp), %rdi
movq %rax, -0x138(%rbp)
movq -0x120(%rbp), %rsi
movq -0x130(%rbp), %rdx
movq -0x138(%rbp), %rcx
callq 0x5c80
movq %rax, -0x210(%rbp)
jmp 0x2cd3
movq -0x1b0(%rbp), %rdi
movq -0x210(%rbp), %rax
movq %rax, -0x140(%rbp)
addq $0x18, %rdi
movq %rdi, -0x228(%rbp)
callq 0x5860
movq %rax, -0x150(%rbp)
leaq -0x148(%rbp), %rdi
leaq -0x150(%rbp), %rsi
callq 0x5d30
leaq -0x90(%rbp), %rdi
movq %rdi, -0x230(%rbp)
callq 0x57f0
movq -0x230(%rbp), %rdi
movq %rax, -0x158(%rbp)
callq 0x5860
movq -0x228(%rbp), %rdi
movq %rax, -0x160(%rbp)
movq -0x148(%rbp), %rsi
movq -0x158(%rbp), %rdx
movq -0x160(%rbp), %rcx
callq 0x5c80
movq %rax, -0x220(%rbp)
jmp 0x2d69
movq -0x1b0(%rbp), %rdi
movq -0x220(%rbp), %rax
movq %rax, -0x168(%rbp)
addq $0x30, %rdi
movq %rdi, -0x240(%rbp)
callq 0x5860
movq %rax, -0x178(%rbp)
leaq -0x170(%rbp), %rdi
leaq -0x178(%rbp), %rsi
callq 0x5d30
leaq -0xa8(%rbp), %rdi
movq %rdi, -0x248(%rbp)
callq 0x57f0
movq -0x248(%rbp), %rdi
movq %rax, -0x180(%rbp)
callq 0x5860
movq -0x240(%rbp), %rdi
movq %rax, -0x188(%rbp)
movq -0x170(%rbp), %rsi
movq -0x180(%rbp), %rdx
movq -0x188(%rbp), %rcx
callq 0x5c80
movq %rax, -0x238(%rbp)
jmp 0x2dff
movq -0x238(%rbp), %rax
movq %rax, -0x190(%rbp)
leaq -0x78(%rbp), %rdi
callq 0x5c20
leaq -0x90(%rbp), %rdi
callq 0x5c20
leaq -0xa8(%rbp), %rdi
callq 0x5c20
movl -0x34(%rbp), %eax
addl $0x1, %eax
movl %eax, -0x34(%rbp)
jmp 0x2853
callq 0x2110
movl %eax, -0x194(%rbp)
movq 0x17172(%rip), %rdi     # 0x19fc0
leaq 0xf25d(%rip), %rsi      # 0x120b2
callq 0x21d0
movq %rax, -0x250(%rbp)
jmp 0x2e63
movq -0x250(%rbp), %rdi
movl -0x194(%rbp), %eax
movl -0x14(%rbp), %ecx
subl %ecx, %eax
cvtsi2sd %eax, %xmm0
movsd 0xf1b7(%rip), %xmm1     # 0x12038
divsd %xmm1, %xmm0
movsd 0xf1b3(%rip), %xmm1     # 0x12040
mulsd %xmm1, %xmm0
callq 0x23c0
movq %rax, -0x258(%rbp)
jmp 0x2e9f
movq -0x258(%rbp), %rdi
leaq 0xf213(%rip), %rsi      # 0x120c0
callq 0x21d0
movq %rax, -0x260(%rbp)
jmp 0x2ebb
movq -0x260(%rbp), %rdi
movq 0x170d7(%rip), %rsi     # 0x19fa0
callq 0x2200
jmp 0x2ed0
leaq -0xa8(%rbp), %rdi
callq 0x5d70
leaq -0x90(%rbp), %rdi
callq 0x5d70
leaq -0x78(%rbp), %rdi
callq 0x5d70
movq -0x1a8(%rbp), %rax
movq -0x1a0(%rbp), %rcx
imulq $0x18, %rax, %rdx
movq %rcx, %rax
addq %rdx, %rax
cmpq %rax, %rcx
movq %rax, -0x268(%rbp)
je 0x2f46
movq -0x268(%rbp), %rdi
addq $-0x18, %rdi
movq %rdi, -0x270(%rbp)
callq 0x5d70
movq -0x1a0(%rbp), %rcx
movq -0x270(%rbp), %rax
cmpq %rcx, %rax
movq %rax, -0x268(%rbp)
jne 0x2f15
movq -0x20(%rbp), %rax
movq %rax, %rsp
movq %rbp, %rsp
popq %rbp
retq
movq -0x1a0(%rbp), %rcx
movq -0x1a8(%rbp), %rax
imulq $0x18, %rax, %rdx
movq %rcx, %rax
addq %rdx, %rax
cmpq %rax, %rcx
movq %rax, -0x278(%rbp)
je 0x2fa7
movq -0x278(%rbp), %rdi
addq $-0x18, %rdi
movq %rdi, -0x280(%rbp)
callq 0x5d70
movq -0x1a0(%rbp), %rcx
movq -0x280(%rbp), %rax
cmpq %rcx, %rax
movq %rax, -0x278(%rbp)
jne 0x2f76
jmp 0x2fa9
movq -0x50(%rbp), %rdi
callq 0x2370
nopw %cs:(%rax,%rax)