// File: /workspace/github_cpp_makefile_star2_O3/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, %r15
movl $0x0, 0x68(%r14)
leaq 0x88(%r14), %r13
movl $0xffffffff, -0xf0(%rbp) # imm = 0xFFFFFFFF
movq 0x90(%r14), %rsi
cmpq 0x98(%r14), %rsi
je 0x3289
movl $0xffffffff, (%rsi)     # imm = 0xFFFFFFFF
addq $0x4, %rsi
movq %rsi, 0x90(%r14)
jmp 0x3298
leaq -0xf0(%rbp), %rdx
movq %r13, %rdi
callq 0x606c
leaq 0x68(%r14), %rdx
leaq 0xa0(%r14), %rdi
movq 0xa8(%r14), %rsi
cmpq 0xb0(%r14), %rsi
movq %rdx, -0x48(%rbp)
movq %rdi, -0x78(%rbp)
je 0x32ce
movl 0x68(%r14), %eax
movl %eax, (%rsi)
addq $0x4, %rsi
movq %rsi, 0xa8(%r14)
jmp 0x32d3
callq 0x618e
leaq 0xb8(%r14), %rdi
movq 0xc0(%r14), %rsi
cmpq 0xc8(%r14), %rsi
movq %rbx, -0x68(%rbp)
movq %r15, -0x60(%rbp)
movq %rdi, -0x70(%rbp)
je 0x3309
movl 0x68(%r14), %eax
movl %eax, (%rsi)
addq $0x4, %rsi
movq %rsi, 0xc0(%r14)
jmp 0x3312
movq -0x48(%rbp), %rdx
callq 0x618e
xorpd %xmm0, %xmm0
movapd %xmm0, -0x40(%rbp)
movq $0x0, -0x30(%rbp)
movapd %xmm0, -0x90(%rbp)
movapd %xmm0, -0xa0(%rbp)
movapd %xmm0, -0xb0(%rbp)
movapd %xmm0, -0xc0(%rbp)
movapd %xmm0, -0xd0(%rbp)
movapd %xmm0, -0xe0(%rbp)
movapd %xmm0, -0xf0(%rbp)
movq (%r12), %r15
cmpq 0x8(%r12), %r15
je 0x3517
movq %r13, -0x58(%rbp)
leaq -0x40(%rbp), %rbx
movss 0x10(%r15), %xmm0
addss 0x4d00(%rip), %xmm0     # 0x8080
cvtss2sd %xmm0, %xmm0
divsd 0x4cd4(%rip), %xmm0     # 0x8060
andpd 0x4c8c(%rip), %xmm0     # 0x8020
cvttsd2si %xmm0, %ecx
movslq %ecx, %r13
movq -0x40(%rbp), %rax
movq -0x38(%rbp), %rdx
subq %rax, %rdx
sarq $0x4, %rdx
movabsq $0x6db6db6db6db6db7, %rsi # imm = 0x6DB6DB6DB6DB6DB7
imulq %rsi, %rdx
cmpq %r13, %rdx
ja 0x33e5
incl %ecx
movslq %ecx, %rsi
movq %rbx, %rdi
callq 0x4a24
imulq $0x70, %r13, %rsi
addq -0x40(%rbp), %rsi
movq %rbx, %rdi
leaq -0xf0(%rbp), %rdx
callq 0x4a9c
movq -0x40(%rbp), %rax
imulq $0x70, %r13, %rcx
leaq (%rax,%rcx), %rdi
addq $0x48, %rdi
movq 0x8(%rdi), %rsi
cmpq 0x10(%rdi), %rsi
je 0x3414
movupd (%r15), %xmm0
movups 0xc(%r15), %xmm1
movups %xmm1, 0xc(%rsi)
movupd %xmm0, (%rsi)
addq $0x1c, 0x8(%rdi)
jmp 0x341c
movq %r15, %rdx
callq 0x5594
addq $0x1c, %r15
cmpq 0x8(%r12), %r15
jne 0x3372
movq -0x40(%rbp), %r12
cmpq -0x38(%rbp), %r12
movq -0x58(%rbp), %r13
je 0x3517
movq 0x48(%r12), %r15
movq 0x50(%r12), %rbx
movq %rbx, %rax
subq %r15, %rax
je 0x34f9
sarq $0x2, %rax
movabsq $0x6db6db6db6db6db7, %rcx # imm = 0x6DB6DB6DB6DB6DB7
imulq %rcx, %rax
bsrq %rax, %rdx
xorl $0x3f, %edx
addl %edx, %edx
xorq $0x7e, %rdx
movq %r15, %rdi
movq %rbx, %rsi
leaq -0xf81(%rip), %rcx      # 0x24fe
callq 0x5714
movq %r15, %rdi
movq %rbx, %rsi
leaq -0xf93(%rip), %rdx      # 0x24fe
callq 0x5886
movq 0x50(%r12), %rax
cmpq 0x48(%r12), %rax
je 0x3509
movl $0x20, %edi
callq 0x21a0
movq %rax, %rbx
leaq 0x48(%r12), %rsi
movq %rax, %rdi
callq 0x4b42
movl $0x98, %edi
callq 0x21a0
movq %rax, %r15
movq $0xa, -0x50(%rbp)
movq %rax, %rdi
movl $0x3, %esi
movq %rbx, %rdx
leaq -0x50(%rbp), %rcx
callq 0x4c5c
movq %r15, %rdi
callq 0x4d02
movq %rbx, 0x60(%r12)
movq %r15, 0x68(%r12)
jmp 0x3509
leaq -0x40(%rbp), %rdi
movq %r12, %rsi
callq 0x6948
addq $-0x70, %r12
addq $0x70, %r12
cmpq -0x38(%rbp), %r12
jne 0x343d
movq 0x7aaa(%rip), %rdi      # 0xafc8
leaq 0x4b97(%rip), %rsi      # 0x80bc
movl $0xf, %edx
callq 0x21c0
movq -0x38(%rbp), %rsi
subq -0x40(%rbp), %rsi
sarq $0x4, %rsi
movabsq $0x6db6db6db6db6db7, %rax # imm = 0x6DB6DB6DB6DB6DB7
imulq %rax, %rsi
movq 0x7a78(%rip), %rdi      # 0xafc8
callq 0x2110
movq %rax, %rbx
movq (%rax), %rax
movq -0x18(%rax), %rdi
addq %rbx, %rdi
movl $0xa, %esi
callq 0x2190
movsbl %al, %esi
movq %rbx, %rdi
callq 0x2030
movq %rax, %rdi
callq 0x2120
movq -0x40(%rbp), %r12
movq %r14, %rdi
movq %r12, %rsi
callq 0x382e
movq (%r12), %rax
cmpq %rax, 0x8(%r12)
je 0x369f
xorl %ebx, %ebx
leaq -0x50(%rbp), %r15
movslq (%rax,%rbx,4), %rdx
movq 0x18(%r12), %rax
movl (%rax,%rbx,4), %ecx
movq -0x48(%rbp), %rax
movl (%rax), %eax
cmpl %ecx, %edx
jg 0x35e8
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 0x35dc
incl %eax
movl %eax, 0x68(%r14)
movl $0xffffffff, -0x50(%rbp) # imm = 0xFFFFFFFF
movq 0x90(%r14), %rsi
cmpq 0x98(%r14), %rsi
je 0x3618
movl $0xffffffff, (%rsi)     # imm = 0xFFFFFFFF
addq $0x4, %rsi
movq %rsi, 0x90(%r14)
jmp 0x3623
movq %r13, %rdi
movq %r15, %rdx
callq 0x606c
movq 0xa8(%r14), %rsi
cmpq 0xb0(%r14), %rsi
je 0x3646
movl 0x68(%r14), %eax
movl %eax, (%rsi)
addq $0x4, %rsi
movq %rsi, 0xa8(%r14)
jmp 0x3653
movq -0x78(%rbp), %rdi
movq -0x48(%rbp), %rdx
callq 0x618e
movq 0xc0(%r14), %rsi
cmpq 0xc8(%r14), %rsi
je 0x3676
movl 0x68(%r14), %eax
movl %eax, (%rsi)
addq $0x4, %rsi
movq %rsi, 0xc0(%r14)
jmp 0x3683
movq -0x70(%rbp), %rdi
movq -0x48(%rbp), %rdx
callq 0x618e
incq %rbx
movq (%r12), %rax
movq 0x8(%r12), %rcx
subq %rax, %rcx
sarq $0x2, %rcx
cmpq %rbx, %rcx
ja 0x35a3
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 0x3719
movl $0x1, %r15d
movl $0x70, %r13d
movq %rax, %rbx
addq %r13, %rbx
movq %r14, %rdi
movq %rbx, %rsi
callq 0x382e
movq %r14, %rdi
movq %rbx, %rsi
movq %r12, %rdx
callq 0x3b58
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 0x36ce
callq 0x2100
movq %rax, %rbx
movq 0x78a0(%rip), %rdi      # 0xafc8
leaq 0x499d(%rip), %rsi      # 0x80cc
movl $0xd, %edx
callq 0x21c0
movq -0x68(%rbp), %r15
subl -0x60(%rbp), %ebx
xorps %xmm0, %xmm0
cvtsi2sd %ebx, %xmm0
divsd 0x4901(%rip), %xmm0     # 0x8050
mulsd 0x4901(%rip), %xmm0     # 0x8058
movq 0x786a(%rip), %rdi      # 0xafc8
callq 0x2250
movq %rax, %rbx
leaq 0x494b(%rip), %rsi      # 0x80b8
movl $0x3, %edx
movq %rax, %rdi
callq 0x21c0
movq (%rbx), %rax
movq -0x18(%rax), %rdi
addq %rbx, %rdi
movl $0xa, %esi
callq 0x2190
movsbl %al, %esi
movq %rbx, %rdi
callq 0x2030
movq %rax, %rdi
callq 0x2120
leaq -0x40(%rbp), %rdx
movq %r15, %rdi
movq %r14, %rsi
callq 0x3f30
leaq -0xf0(%rbp), %rdi
callq 0x4d50
leaq -0x40(%rbp), %rdi
callq 0x4e00
movq %r15, %rax
addq $0xc8, %rsp
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
jmp 0x380d
jmp 0x380d
movq %rax, %r14
movl $0x98, %esi
movq %r15, %rdi
jmp 0x37f6
movq %rax, %r14
movl $0x20, %esi
movq %rbx, %rdi
callq 0x21b0
jmp 0x3810
jmp 0x380d
jmp 0x380d
jmp 0x380d
jmp 0x380d
jmp 0x380d
jmp 0x380d
jmp 0x380d
jmp 0x380d
movq %rax, %r14
leaq -0xf0(%rbp), %rdi
callq 0x4d50
leaq -0x40(%rbp), %rdi
callq 0x4e00
movq %r14, %rdi
callq 0x22a0
nop