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

  this->new_label = 0;
  this->next.push_back(-1);
  this->tail.push_back(this->new_label);
  this->rtable.push_back(this->new_label);

  std::vector<Scanline> scanlines;

  std::vector<Vec3>::iterator v_it;
  std::vector<Scanline>::iterator s_it;
  std::vector<int>::iterator r_it;
  int i;

  //Organize points into scanlines
  Scanline scanline;
  scanline.points = std::vector<Vec3>();
  scanline.s_queue = std::vector<int>();
  scanline.e_queue = std::vector<int>();
  i = 0;

  for (v_it = cloud.begin(); v_it != cloud.end(); v_it++, i++) {
    int scan_index;

    //if (v_it->theta <= 2 && v_it->theta > -8.333) {
    scan_index = (int)abs((v_it->theta - 3) / (0.6));
    // } else if (v_it->theta <= -8.333 && v_it->theta >= -24.333 ) {
    //   scan_index = 31 + (int)round(abs((v_it->theta + 8.333) / (0.5)));
    // }

    if (scan_index >= scanlines.size()) {
      scanlines.resize(scan_index + 1);
      scanlines.insert(scanlines.begin() + scan_index, scanline);
    }
    scanlines[scan_index].points.push_back(*v_it);
  }

  //Index scanlines by theta
  for (s_it = scanlines.begin(); s_it != scanlines.end(); s_it++) {
    if (s_it->points.size() == 0) {
      //(s_it - 1)->points.insert((s_it - 1)->points.end(), s_it->points.begin(), s_it->points.end());
      scanlines.erase(s_it);
      s_it--;
    } else {
      std::sort(s_it->points.begin(), s_it->points.end(), CompareTheta);

      if (s_it->points.size() > 0) {
        ScanlinePointCloud * tree_point_cloud = new ScanlinePointCloud(s_it->points);
        ScanlineKDTree * tree = new ScanlineKDTree(3, *tree_point_cloud, nanoflann::KDTreeSingleIndexAdaptorParams(10));
        tree->buildIndex();

        s_it->tree_point_cloud = tree_point_cloud;
        s_it->tree = tree;
      }
    }
  }

  std::cout << "Num Scanlines: " << scanlines.size() << std::endl;

  //Assign inital labels
  Scanline* scan_above = &scanlines[0];
  Scanline* scan_current = nullptr;
  FindRuns(*scan_above);
  int start_index;
  int end_index;

  for (int i = 0; i < scan_above->s_queue.size(); i++) {
    start_index = scan_above->s_queue[i];
    end_index = scan_above->e_queue[i];

    SetRunLabel(*scan_above, start_index, end_index, this->new_label);
    this->new_label += 1;
    this->next.push_back(-1);
    this->tail.push_back(this->new_label);
    this->rtable.push_back(this->new_label);
  }

  //Find runs for all subsequent scanlines and propogate labels
  for (int i = 1; i < scanlines.size(); i++) {
    scan_current = &(scanlines[i]);
    FindRuns(*scan_current);
    UpdateLabels(*scan_current, *scan_above);
    scan_above = scan_current;
  }

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

  //Extract and return clusters
  return ExtractClusters(scanlines);
}

// Assembly:
pushq %rbp
movq %rsp, %rbp
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0xc8, %rsp
movq %rdx, %r12
movq %rsi, %r14
movq %rdi, %rbx
callq 0x2100
movq %rax, -0x58(%rbp)
movl $0x0, 0x68(%r14)
leaq 0x88(%r14), %r15
movl $0xffffffff, -0xf0(%rbp) # imm = 0xFFFFFFFF
movq 0x90(%r14), %rsi
cmpq 0x98(%r14), %rsi
je 0x3332
movl $0xffffffff, (%rsi)     # imm = 0xFFFFFFFF
addq $0x4, %rsi
movq %rsi, 0x90(%r14)
jmp 0x3341
leaq -0xf0(%rbp), %rdx
movq %r15, %rdi
callq 0x61b0
leaq 0x68(%r14), %rdx
leaq 0xa0(%r14), %rdi
movq 0xa8(%r14), %rsi
cmpq 0xb0(%r14), %rsi
movq %rdx, -0x48(%rbp)
movq %rdi, -0x70(%rbp)
je 0x3377
movl 0x68(%r14), %eax
movl %eax, (%rsi)
addq $0x4, %rsi
movq %rsi, 0xa8(%r14)
jmp 0x337c
callq 0x62d2
leaq 0xb8(%r14), %rdi
movq 0xc0(%r14), %rsi
cmpq 0xc8(%r14), %rsi
movq %rdi, -0x68(%rbp)
je 0x33aa
movl 0x68(%r14), %eax
movl %eax, (%rsi)
addq $0x4, %rsi
movq %rsi, 0xc0(%r14)
jmp 0x33b3
movq -0x48(%rbp), %rdx
callq 0x62d2
xorpd %xmm0, %xmm0
movapd %xmm0, -0x40(%rbp)
xorl %eax, %eax
movq %rax, -0x30(%rbp)
movapd %xmm0, -0xb0(%rbp)
movapd %xmm0, -0xa0(%rbp)
movapd %xmm0, -0x90(%rbp)
movapd %xmm0, -0xc0(%rbp)
movapd %xmm0, -0xd0(%rbp)
movapd %xmm0, -0xe0(%rbp)
movapd %xmm0, -0xf0(%rbp)
movq -0xa8(%rbp), %rdi
movq -0x98(%rbp), %rsi
movupd %xmm0, -0xa8(%rbp)
movq %rax, -0x98(%rbp)
testq %rdi, %rdi
je 0x3428
subq %rdi, %rsi
callq 0x21c0
xorpd %xmm0, %xmm0
movq -0xf0(%rbp), %rdi
movq -0xe0(%rbp), %rsi
movapd %xmm0, -0xf0(%rbp)
movq $0x0, -0xe0(%rbp)
testq %rdi, %rdi
je 0x3456
subq %rdi, %rsi
callq 0x21c0
movq %r15, -0x78(%rbp)
movq %rbx, -0x60(%rbp)
movq -0xd8(%rbp), %rdi
movq -0xc8(%rbp), %rsi
xorpd %xmm0, %xmm0
movupd %xmm0, -0xd8(%rbp)
movq $0x0, -0xc8(%rbp)
testq %rdi, %rdi
je 0x3490
subq %rdi, %rsi
callq 0x21c0
movq (%r12), %rbx
cmpq 0x8(%r12), %rbx
je 0x3556
leaq -0x40(%rbp), %r15
movss 0x10(%rbx), %xmm0
addss 0x4bd0(%rip), %xmm0     # 0x8080
cvtss2sd %xmm0, %xmm0
divsd 0x4ba4(%rip), %xmm0     # 0x8060
andpd 0x4b5c(%rip), %xmm0     # 0x8020
cvttsd2si %xmm0, %eax
movq -0x38(%rbp), %rcx
subq -0x40(%rbp), %rcx
movslq %eax, %r13
sarq $0x4, %rcx
movabsq $0x6db6db6db6db6db7, %rdx # imm = 0x6DB6DB6DB6DB6DB7
imulq %rdx, %rcx
cmpq %r13, %rcx
ja 0x350e
incl %eax
movslq %eax, %rsi
movq %r15, %rdi
callq 0x4b40
imulq $0x70, %r13, %rsi
addq -0x40(%rbp), %rsi
movq %r15, %rdi
leaq -0xf0(%rbp), %rdx
callq 0x4bb8
movq -0x40(%rbp), %rax
imulq $0x70, %r13, %rcx
leaq (%rax,%rcx), %rdi
addq $0x48, %rdi
movq 0x8(%rdi), %rsi
cmpq 0x10(%rdi), %rsi
je 0x353f
movupd (%rbx), %xmm0
movups 0xc(%rbx), %xmm1
movups %xmm1, 0xc(%rsi)
movupd %xmm0, (%rsi)
addq $0x1c, 0x8(%rdi)
jmp 0x3547
movq %rbx, %rdx
callq 0x56b8
addq $0x1c, %rbx
cmpq 0x8(%r12), %rbx
jne 0x34a3
movq -0x40(%rbp), %r12
cmpq -0x38(%rbp), %r12
je 0x363d
leaq -0x104d(%rip), %rbx     # 0x251e
movq 0x48(%r12), %r13
movq 0x50(%r12), %r15
movq %r15, %rax
subq %r13, %rax
je 0x361f
sarq $0x2, %rax
movabsq $0x6db6db6db6db6db7, %rcx # imm = 0x6DB6DB6DB6DB6DB7
imulq %rcx, %rax
bsrq %rax, %rdx
xorl $0x3f, %edx
addl %edx, %edx
xorq $0x7e, %rdx
movq %r13, %rdi
movq %r15, %rsi
movq %rbx, %rcx
callq 0x5838
movq %r13, %rdi
movq %r15, %rsi
movq %rbx, %rdx
callq 0x59aa
movq 0x50(%r12), %rax
cmpq 0x48(%r12), %rax
je 0x362f
movl $0x20, %edi
callq 0x21b0
movq %rax, %r15
leaq 0x48(%r12), %rsi
movq %rax, %rdi
callq 0x4c5e
movl $0x98, %edi
callq 0x21b0
movq %rax, %r13
movq $0xa, -0x50(%rbp)
movq %rax, %rdi
movl $0x3, %esi
movq %r15, %rdx
leaq -0x50(%rbp), %rcx
callq 0x4d72
movq %r13, %rdi
callq 0x4e18
movq %r15, 0x60(%r12)
movq %r13, 0x68(%r12)
jmp 0x362f
leaq -0x40(%rbp), %rdi
movq %r12, %rsi
callq 0x6a64
addq $-0x70, %r12
addq $0x70, %r12
cmpq -0x38(%rbp), %r12
jne 0x356b
movq 0x7984(%rip), %rdi      # 0xafc8
leaq 0x4a71(%rip), %rsi      # 0x80bc
movl $0xf, %edx
callq 0x21d0
movq -0x38(%rbp), %rsi
subq -0x40(%rbp), %rsi
sarq $0x4, %rsi
movabsq $0x6db6db6db6db6db7, %rax # imm = 0x6DB6DB6DB6DB6DB7
imulq %rax, %rsi
movq 0x7952(%rip), %rdi      # 0xafc8
callq 0x2110
movq %rax, %rbx
movq (%rax), %rax
movq -0x18(%rax), %rdi
addq %rbx, %rdi
movl $0xa, %esi
callq 0x21a0
movsbl %al, %esi
movq %rbx, %rdi
callq 0x2030
movq %rax, %rdi
callq 0x2120
movq -0x40(%rbp), %r12
movq %r14, %rdi
movq %r12, %rsi
callq 0x3956
movq (%r12), %rax
cmpq %rax, 0x8(%r12)
je 0x37c8
xorl %r15d, %r15d
leaq -0x50(%rbp), %rbx
movslq (%rax,%r15,4), %rdx
movq 0x18(%r12), %rax
movl (%rax,%r15,4), %ecx
movq -0x48(%rbp), %rax
movl (%rax), %eax
cmpl %ecx, %edx
jg 0x3710
xorps %xmm0, %xmm0
cvtsi2ss %eax, %xmm0
movq 0x48(%r12), %rsi
subl %edx, %ecx
incl %ecx
leaq (%rdx,%rdx,8), %rdi
leaq (%rdi,%rdi,2), %rdi
addq %rdx, %rdi
leaq (%rsi,%rdi), %rdx
addq $0x18, %rdx
movss %xmm0, (%rdx)
addq $0x1c, %rdx
decl %ecx
jne 0x3704
incl %eax
movl %eax, 0x68(%r14)
movl $0xffffffff, -0x50(%rbp) # imm = 0xFFFFFFFF
movq 0x90(%r14), %rsi
cmpq 0x98(%r14), %rsi
je 0x3740
movl $0xffffffff, (%rsi)     # imm = 0xFFFFFFFF
addq $0x4, %rsi
movq %rsi, 0x90(%r14)
jmp 0x374c
movq -0x78(%rbp), %rdi
movq %rbx, %rdx
callq 0x61b0
movq 0xa8(%r14), %rsi
cmpq 0xb0(%r14), %rsi
je 0x376f
movl 0x68(%r14), %eax
movl %eax, (%rsi)
addq $0x4, %rsi
movq %rsi, 0xa8(%r14)
jmp 0x377c
movq -0x70(%rbp), %rdi
movq -0x48(%rbp), %rdx
callq 0x62d2
movq 0xc0(%r14), %rsi
cmpq 0xc8(%r14), %rsi
je 0x379f
movl 0x68(%r14), %eax
movl %eax, (%rsi)
addq $0x4, %rsi
movq %rsi, 0xc0(%r14)
jmp 0x37ac
movq -0x68(%rbp), %rdi
movq -0x48(%rbp), %rdx
callq 0x62d2
incq %r15
movq (%r12), %rax
movq 0x8(%r12), %rcx
subq %rax, %rcx
sarq $0x2, %rcx
cmpq %r15, %rcx
ja 0x36ca
movq -0x40(%rbp), %rax
movq -0x38(%rbp), %rcx
subq %rax, %rcx
sarq $0x4, %rcx
movabsq $0x6db6db6db6db6db7, %rdx # imm = 0x6DB6DB6DB6DB6DB7
imulq %rdx, %rcx
cmpq $0x2, %rcx
jb 0x3842
movl $0x1, %r15d
movl $0x70, %r13d
movq %rax, %rbx
addq %r13, %rbx
movq %r14, %rdi
movq %rbx, %rsi
callq 0x3956
movq %r14, %rdi
movq %rbx, %rsi
movq %r12, %rdx
callq 0x3c46
incq %r15
movq -0x40(%rbp), %rax
movq -0x38(%rbp), %rcx
subq %rax, %rcx
sarq $0x4, %rcx
movabsq $0x6db6db6db6db6db7, %rdx # imm = 0x6DB6DB6DB6DB6DB7
imulq %rdx, %rcx
addq $0x70, %r13
movq %rbx, %r12
cmpq %r15, %rcx
ja 0x37f7
callq 0x2100
movq %rax, %rbx
movq 0x7777(%rip), %rdi      # 0xafc8
leaq 0x4874(%rip), %rsi      # 0x80cc
movl $0xd, %edx
callq 0x21d0
movq -0x60(%rbp), %r15
subl -0x58(%rbp), %ebx
xorps %xmm0, %xmm0
cvtsi2sd %ebx, %xmm0
divsd 0x47d8(%rip), %xmm0     # 0x8050
mulsd 0x47d8(%rip), %xmm0     # 0x8058
movq 0x7741(%rip), %rdi      # 0xafc8
callq 0x2260
movq %rax, %rbx
leaq 0x4822(%rip), %rsi      # 0x80b8
movl $0x3, %edx
movq %rax, %rdi
callq 0x21d0
movq (%rbx), %rax
movq -0x18(%rax), %rdi
addq %rbx, %rdi
movl $0xa, %esi
callq 0x21a0
movsbl %al, %esi
movq %rbx, %rdi
callq 0x2030
movq %rax, %rdi
callq 0x2120
leaq -0x40(%rbp), %rdx
movq %r15, %rdi
movq %r14, %rsi
callq 0x4022
leaq -0xf0(%rbp), %rdi
callq 0x4e66
leaq -0x40(%rbp), %rdi
callq 0x4f16
movq %r15, %rax
addq $0xc8, %rsp
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
jmp 0x3936
jmp 0x3936
jmp 0x3936
jmp 0x3936
jmp 0x3936
movq %rax, %rbx
movl $0x98, %esi
movq %r13, %rdi
jmp 0x3925
movq %rax, %rbx
movl $0x20, %esi
movq %r15, %rdi
callq 0x21c0
jmp 0x3939
jmp 0x3936
jmp 0x3936
jmp 0x3936
jmp 0x3936
jmp 0x3936
movq %rax, %rbx
leaq -0xf0(%rbp), %rdi
callq 0x4e66
leaq -0x40(%rbp), %rdi
callq 0x4f16
movq %rbx, %rdi
callq 0x22b0