// File: /workspace/github_cpp_makefile_star2_O2/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 $0xd8, %rsp
movq %rdx, %r13
movq %rsi, %r14
movq %rdi, -0x70(%rbp)
callq 0x2110
movq %rax, -0x68(%rbp)
leaq 0x68(%r14), %r12
andl $0x0, 0x68(%r14)
leaq 0x88(%r14), %rdi
leaq -0x100(%rbp), %rbx
orl $-0x1, (%rbx)
movq %rdi, -0x88(%rbp)
movq %rbx, %rsi
callq 0x5474
leaq 0xa0(%r14), %rdi
movq %rdi, -0x80(%rbp)
movq %r12, %rsi
callq 0x3dd6
leaq 0xb8(%r14), %rdi
movq %rdi, -0x78(%rbp)
movq %r12, %rsi
callq 0x3dd6
xorpd %xmm0, %xmm0
leaq -0x40(%rbp), %rax
movapd %xmm0, (%rax)
andq $0x0, 0x10(%rax)
movapd %xmm0, 0x60(%rbx)
movapd %xmm0, 0x50(%rbx)
movapd %xmm0, 0x40(%rbx)
movapd %xmm0, 0x30(%rbx)
movapd %xmm0, 0x20(%rbx)
movapd %xmm0, 0x10(%rbx)
movapd %xmm0, (%rbx)
leaq -0xb8(%rbp), %rdi
leaq -0x60(%rbp), %rbx
movapd %xmm0, (%rbx)
andq $0x0, 0x10(%rbx)
movq %rbx, %rsi
callq 0x56f6
movq %rbx, %rdi
callq 0x47fe
xorpd %xmm0, %xmm0
movapd %xmm0, (%rbx)
andq $0x0, 0x10(%rbx)
leaq -0x100(%rbp), %rdi
movq %rbx, %rsi
callq 0x5762
movq %rbx, %rdi
callq 0x48c2
leaq -0xe8(%rbp), %rdi
xorpd %xmm0, %xmm0
movapd %xmm0, (%rbx)
andq $0x0, 0x10(%rbx)
movq %rbx, %rsi
callq 0x5762
movq %rbx, %rdi
callq 0x48c2
movq (%r13), %rbx
cmpq 0x8(%r13), %rbx
je 0x2e68
movss 0x10(%rbx), %xmm0
addss 0x429a(%rip), %xmm0     # 0x7088
cvtss2sd %xmm0, %xmm0
divsd 0x421e(%rip), %xmm0     # 0x7018
andpd 0x423e(%rip), %xmm0     # 0x7040
cvttsd2si %xmm0, %esi
movslq %esi, %r15
movq -0x40(%rbp), %rcx
movq -0x38(%rbp), %rax
subq %rcx, %rax
cqto
pushq $0x70
popq %rdi
idivq %rdi
cmpq %r15, %rax
ja 0x2e4b
incl %esi
movslq %esi, %rsi
leaq -0x40(%rbp), %rdi
callq 0x3dfc
imulq $0x70, %r15, %rsi
addq -0x40(%rbp), %rsi
leaq -0x40(%rbp), %rdi
leaq -0x100(%rbp), %rdx
callq 0x3e3e
movq -0x40(%rbp), %rcx
imulq $0x70, %r15, %rax
leaq (%rcx,%rax), %rdi
addq $0x48, %rdi
movq %rbx, %rsi
callq 0x3ca0
addq $0x1c, %rbx
jmp 0x2dd7
leaq -0x40(%rbp), %rax
movq (%rax), %r13
cmpq -0x38(%rbp), %r13
je 0x2f05
movq 0x48(%r13), %rdi
movq 0x50(%r13), %rsi
cmpq %rdi, %rsi
je 0x2eec
leaq -0xa4f(%rip), %rdx      # 0x243e
callq 0x4ae2
movq 0x50(%r13), %rax
cmpq 0x48(%r13), %rax
je 0x2efc
pushq $0x20
popq %rdi
callq 0x21a0
movq %rax, %rbx
leaq 0x48(%r13), %rsi
movq %rax, %rdi
callq 0x3eee
movl $0x98, %edi
callq 0x21a0
movq %rax, %r15
movq $0xa, -0x60(%rbp)
movq %rax, %rdi
pushq $0x3
popq %rsi
movq %rbx, %rdx
leaq -0x60(%rbp), %rcx
callq 0x3fa8
movq %r15, %rdi
callq 0x4038
movq %rbx, 0x60(%r13)
movq %r15, 0x68(%r13)
jmp 0x2efc
leaq -0x40(%rbp), %rdi
movq %r13, %rsi
callq 0x3ee4
addq $-0x70, %r13
addq $0x70, %r13
jmp 0x2e6f
movq 0x80bc(%rip), %rdi      # 0xafc8
leaq 0x41b1(%rip), %rsi      # 0x70c4
callq 0x2190
movq %rax, %rdi
movq -0x38(%rbp), %rax
subq -0x40(%rbp), %rax
cqto
pushq $0x70
popq %rcx
idivq %rcx
movq %rax, %rsi
callq 0x2120
movq %rax, %rdi
callq 0x20b0
movq -0x40(%rbp), %r15
movq %r14, %rdi
movq %r15, %rsi
callq 0x30d8
xorl %r13d, %r13d
leaq -0x60(%rbp), %rbx
movq (%r15), %rax
movq 0x8(%r15), %rcx
subq %rax, %rcx
sarq $0x2, %rcx
cmpq %r13, %rcx
jbe 0x2fb0
movl (%rax,%r13,4), %edx
movq 0x18(%r15), %rax
movl (%rax,%r13,4), %ecx
movl (%r12), %r8d
movq %r15, %rsi
callq 0x32a0
incl (%r12)
orl $-0x1, -0x60(%rbp)
movq -0x88(%rbp), %rdi
movq %rbx, %rsi
callq 0x5474
movq -0x80(%rbp), %rdi
movq %r12, %rsi
callq 0x3dd6
movq -0x78(%rbp), %rdi
movq %r12, %rsi
callq 0x3dd6
incq %r13
jmp 0x2f51
pushq $0x1
popq %r12
pushq $0x70
popq %r13
movq -0x40(%rbp), %rbx
movq -0x38(%rbp), %rax
subq %rbx, %rax
cqto
pushq $0x70
popq %rcx
idivq %rcx
cmpq %r12, %rax
jbe 0x2ff8
addq %r13, %rbx
movq %r14, %rdi
movq %rbx, %rsi
callq 0x30d8
movq %r14, %rdi
movq %rbx, %rsi
movq %r15, %rdx
callq 0x32d2
incq %r12
addq $0x70, %r13
movq %rbx, %r15
jmp 0x2fb8
callq 0x2110
movq %rax, %rbx
movq 0x7fc1(%rip), %rdi      # 0xafc8
leaq 0x40c6(%rip), %rsi      # 0x70d4
callq 0x2190
movq -0x70(%rbp), %r15
subl -0x68(%rbp), %ebx
cvtsi2sd %ebx, %xmm0
divsd 0x3fe2(%rip), %xmm0     # 0x7008
mulsd 0x3fe2(%rip), %xmm0     # 0x7010
movq %rax, %rdi
callq 0x2240
leaq 0x4083(%rip), %rsi      # 0x70c0
movq %rax, %rdi
callq 0x2190
movq %rax, %rdi
callq 0x20b0
leaq -0x40(%rbp), %rdx
movq %r15, %rdi
movq %r14, %rsi
callq 0x34fe
leaq -0x100(%rbp), %rdi
callq 0x4086
leaq -0x40(%rbp), %rdi
callq 0x40f2
movq %r15, %rax
addq $0xd8, %rsp
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
jmp 0x30b7
jmp 0x30b7
jmp 0x30b7
movq %rax, %r14
movl $0x98, %esi
movq %r15, %rdi
jmp 0x30a2
movq %rax, %r14
pushq $0x20
popq %rsi
movq %rbx, %rdi
callq 0x21b0
jmp 0x30ba
jmp 0x30b7
jmp 0x30b7
jmp 0x30b7
jmp 0x30b7
jmp 0x30b7
jmp 0x30b7
jmp 0x30b7
movq %rax, %r14
leaq -0x100(%rbp), %rdi
callq 0x4086
leaq -0x40(%rbp), %rdi
callq 0x40f2
movq %r14, %rdi
callq 0x2290
nop