// File: 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
subq $0x2a0, %rsp            # imm = 0x2A0
movq %rdi, -0x200(%rbp)
movq %rdi, %rax
movq %rax, -0x1f8(%rbp)
movq %rdi, -0x8(%rbp)
movq %rsi, -0x10(%rbp)
movq %rdx, -0x18(%rbp)
movq -0x10(%rbp), %rax
movq %rax, -0x1f0(%rbp)
callq 0x2110
movq -0x1f0(%rbp), %rdi
movl %eax, -0x1c(%rbp)
movq %rdi, %rax
addq $0x68, %rax
movq %rax, -0x1e8(%rbp)
movl $0x0, 0x68(%rdi)
addq $0x88, %rdi
movl $0xffffffff, -0x20(%rbp) # imm = 0xFFFFFFFF
leaq -0x20(%rbp), %rsi
callq 0x5e80
movq -0x1f0(%rbp), %rdi
movq -0x1e8(%rbp), %rsi
addq $0xa0, %rdi
callq 0x5eb0
movq -0x1f0(%rbp), %rdi
movq -0x1e8(%rbp), %rsi
addq $0xb8, %rdi
callq 0x5eb0
leaq -0x38(%rbp), %rdi
callq 0x5f20
leaq -0x40(%rbp), %rdi
callq 0x57d0
leaq -0x48(%rbp), %rdi
callq 0x5f40
leaq -0x50(%rbp), %rdi
callq 0x5f60
leaq -0xc8(%rbp), %rdi
callq 0x5f80
jmp 0x3974
leaq -0xf0(%rbp), %rdi
xorl %esi, %esi
movl $0x18, %edx
callq 0x20e0
leaq -0xf0(%rbp), %rdi
callq 0x57b0
leaq -0xc8(%rbp), %rdi
addq $0x48, %rdi
leaq -0xf0(%rbp), %rsi
callq 0x5fe0
leaq -0xf0(%rbp), %rdi
callq 0x5d70
leaq -0x108(%rbp), %rdi
xorl %esi, %esi
movl $0x18, %edx
callq 0x20e0
leaq -0x108(%rbp), %rdi
callq 0x6010
leaq -0xc8(%rbp), %rdi
leaq -0x108(%rbp), %rsi
callq 0x6030
leaq -0x108(%rbp), %rdi
callq 0x6060
leaq -0x120(%rbp), %rdi
xorl %esi, %esi
movl $0x18, %edx
callq 0x20e0
leaq -0x120(%rbp), %rdi
callq 0x6010
leaq -0xc8(%rbp), %rdi
addq $0x18, %rdi
leaq -0x120(%rbp), %rsi
callq 0x6030
leaq -0x120(%rbp), %rdi
callq 0x6060
movl $0x0, -0x54(%rbp)
movq -0x18(%rbp), %rdi
callq 0x57f0
movq %rax, -0x128(%rbp)
movq -0x128(%rbp), %rax
movq %rax, -0x40(%rbp)
movq -0x18(%rbp), %rdi
callq 0x5860
movq %rax, -0x130(%rbp)
leaq -0x40(%rbp), %rdi
leaq -0x130(%rbp), %rsi
callq 0x5820
testb $0x1, %al
jne 0x3a81
jmp 0x3c08
leaq -0x40(%rbp), %rdi
callq 0x5c60
movss 0x10(%rax), %xmm0
movss 0xe575(%rip), %xmm1     # 0x1200c
addss %xmm1, %xmm0
cvtss2sd %xmm0, %xmm0
movsd 0xe5a1(%rip), %xmm1     # 0x12048
divsd %xmm1, %xmm0
callq 0x5c40
movsd %xmm0, -0x208(%rbp)
jmp 0x3aba
movsd -0x208(%rbp), %xmm0
cvttsd2si %xmm0, %eax
movl %eax, -0x134(%rbp)
movslq -0x134(%rbp), %rax
movq %rax, -0x210(%rbp)
leaq -0x38(%rbp), %rdi
callq 0x60c0
movq %rax, %rcx
movq -0x210(%rbp), %rax
cmpq %rcx, %rax
jb 0x3bb1
movl -0x134(%rbp), %eax
incl %eax
movslq %eax, %rsi
leaq -0x38(%rbp), %rdi
callq 0x60f0
jmp 0x3b0c
leaq -0x38(%rbp), %rdi
movq %rdi, -0x220(%rbp)
callq 0x6390
movq %rax, -0x150(%rbp)
movslq -0x134(%rbp), %rsi
leaq -0x150(%rbp), %rdi
callq 0x63c0
movq %rax, -0x148(%rbp)
leaq -0x140(%rbp), %rdi
leaq -0x148(%rbp), %rsi
callq 0x6400
movq -0x220(%rbp), %rdi
movq -0x140(%rbp), %rsi
leaq -0xc8(%rbp), %rdx
callq 0x6180
movq %rax, -0x218(%rbp)
jmp 0x3b73
movq -0x218(%rbp), %rax
movq %rax, -0x158(%rbp)
jmp 0x3bb1
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0xd0(%rbp)
movl %eax, -0xd4(%rbp)
jmp 0x41a3
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0xd0(%rbp)
movl %eax, -0xd4(%rbp)
jmp 0x4197
movslq -0x134(%rbp), %rsi
leaq -0x38(%rbp), %rdi
callq 0x6440
addq $0x48, %rax
movq %rax, -0x228(%rbp)
leaq -0x40(%rbp), %rdi
callq 0x5900
movq -0x228(%rbp), %rdi
movq %rax, %rsi
callq 0x5890
jmp 0x3be6
jmp 0x3be8
leaq -0x40(%rbp), %rdi
xorl %esi, %esi
callq 0x5920
movq %rax, -0x160(%rbp)
movl -0x54(%rbp), %eax
addl $0x1, %eax
movl %eax, -0x54(%rbp)
jmp 0x3a58
leaq -0x38(%rbp), %rdi
callq 0x6390
movq %rax, -0x168(%rbp)
movq -0x168(%rbp), %rax
movq %rax, -0x48(%rbp)
leaq -0x38(%rbp), %rdi
callq 0x64a0
movq %rax, -0x170(%rbp)
leaq -0x48(%rbp), %rdi
leaq -0x170(%rbp), %rsi
callq 0x6460
testb $0x1, %al
jne 0x3c4c
jmp 0x3e93
leaq -0x48(%rbp), %rdi
callq 0x64d0
movq %rax, %rdi
addq $0x48, %rdi
callq 0x5780
cmpq $0x0, %rax
jne 0x3cb5
leaq -0x178(%rbp), %rdi
leaq -0x48(%rbp), %rsi
callq 0x6400
movq -0x178(%rbp), %rsi
leaq -0x38(%rbp), %rdi
callq 0x64f0
movq %rax, -0x230(%rbp)
jmp 0x3c90
movq -0x230(%rbp), %rax
movq %rax, -0x180(%rbp)
leaq -0x48(%rbp), %rdi
xorl %esi, %esi
callq 0x6560
movq %rax, -0x188(%rbp)
jmp 0x3e7a
leaq -0x48(%rbp), %rdi
movq %rdi, -0x238(%rbp)
callq 0x64d0
movq %rax, %rdi
addq $0x48, %rdi
callq 0x57f0
movq -0x238(%rbp), %rdi
movq %rax, -0x190(%rbp)
callq 0x64d0
movq %rax, %rdi
addq $0x48, %rdi
callq 0x5860
movq %rax, -0x198(%rbp)
movq -0x190(%rbp), %rdi
movq -0x198(%rbp), %rsi
leaq -0x176c(%rip), %rdx     # 0x25a0
callq 0x5bb0
jmp 0x3d13
leaq -0x48(%rbp), %rdi
callq 0x64d0
movq %rax, %rdi
addq $0x48, %rdi
callq 0x5780
cmpq $0x0, %rax
jbe 0x3e78
movl $0x20, %edi
callq 0x21e0
movq %rax, -0x240(%rbp)
jmp 0x3d45
leaq -0x48(%rbp), %rdi
callq 0x64d0
movq -0x240(%rbp), %rdi
movq %rax, %rsi
addq $0x48, %rsi
callq 0x65a0
jmp 0x3d63
movq -0x240(%rbp), %rax
movq %rax, -0x1a0(%rbp)
movl $0x98, %edi
callq 0x21e0
movq %rax, -0x248(%rbp)
jmp 0x3d84
movq -0x1a0(%rbp), %rax
movq %rax, -0x250(%rbp)
leaq -0x1b0(%rbp), %rdi
movl $0xa, %esi
callq 0x66e0
jmp 0x3da5
movq -0x250(%rbp), %rdx
movq -0x248(%rbp), %rdi
movl $0x3, %esi
leaq -0x1b0(%rbp), %rcx
callq 0x6700
jmp 0x3dc6
movq -0x248(%rbp), %rax
movq %rax, -0x1a8(%rbp)
movq -0x1a8(%rbp), %rdi
callq 0x67f0
jmp 0x3de2
movq -0x1a0(%rbp), %rax
movq %rax, -0x260(%rbp)
leaq -0x48(%rbp), %rdi
callq 0x64d0
movq -0x260(%rbp), %rcx
movq %rcx, 0x60(%rax)
movq -0x1a8(%rbp), %rax
movq %rax, -0x258(%rbp)
leaq -0x48(%rbp), %rdi
callq 0x64d0
movq -0x258(%rbp), %rcx
movq %rcx, 0x68(%rax)
jmp 0x3e78
movq -0x240(%rbp), %rdi
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0xd0(%rbp)
movl %eax, -0xd4(%rbp)
movl $0x20, %esi
callq 0x21f0
jmp 0x4197
movq -0x248(%rbp), %rdi
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0xd0(%rbp)
movl %eax, -0xd4(%rbp)
movl $0x98, %esi
callq 0x21f0
jmp 0x4197
jmp 0x3e7a
jmp 0x3e7c
leaq -0x48(%rbp), %rdi
xorl %esi, %esi
callq 0x6860
movq %rax, -0x1b8(%rbp)
jmp 0x3c23
movq 0x16126(%rip), %rdi     # 0x19fc0
leaq 0xe223(%rip), %rsi      # 0x120c4
callq 0x21d0
movq %rax, -0x268(%rbp)
jmp 0x3eaf
leaq -0x38(%rbp), %rdi
callq 0x60c0
movq -0x268(%rbp), %rdi
movq %rax, %rsi
callq 0x2040
movq %rax, -0x270(%rbp)
jmp 0x3ed0
movq -0x270(%rbp), %rdi
movq 0x160c2(%rip), %rsi     # 0x19fa0
callq 0x2200
jmp 0x3ee5
xorl %eax, %eax
movl %eax, %esi
leaq -0x38(%rbp), %rdi
callq 0x6440
movq -0x1f0(%rbp), %rdi
movq %rax, -0x1c0(%rbp)
movq $0x0, -0x1c8(%rbp)
movq -0x1c0(%rbp), %rsi
callq 0x41c0
jmp 0x3f19
movl $0x0, -0x1d4(%rbp)
movslq -0x1d4(%rbp), %rax
movq %rax, -0x278(%rbp)
movq -0x1c0(%rbp), %rdi
callq 0x68a0
movq %rax, %rcx
movq -0x278(%rbp), %rax
cmpq %rcx, %rax
jae 0x402b
movq -0x1c0(%rbp), %rdi
movslq -0x1d4(%rbp), %rsi
callq 0x68c0
movl (%rax), %eax
movl %eax, -0x1cc(%rbp)
movq -0x1c0(%rbp), %rdi
addq $0x18, %rdi
movslq -0x1d4(%rbp), %rsi
callq 0x68c0
movq -0x1f0(%rbp), %rdi
movl (%rax), %eax
movl %eax, -0x1d0(%rbp)
movq -0x1c0(%rbp), %rsi
movl -0x1cc(%rbp), %edx
movl -0x1d0(%rbp), %ecx
movl 0x68(%rdi), %r8d
callq 0x44b0
jmp 0x3faf
movq -0x1f0(%rbp), %rdi
movl 0x68(%rdi), %eax
incl %eax
movl %eax, 0x68(%rdi)
addq $0x88, %rdi
movl $0xffffffff, -0x1d8(%rbp) # imm = 0xFFFFFFFF
leaq -0x1d8(%rbp), %rsi
callq 0x5e80
jmp 0x3fdd
movq -0x1f0(%rbp), %rsi
movq %rsi, %rdi
addq $0xa0, %rdi
addq $0x68, %rsi
callq 0x5eb0
jmp 0x3ff9
movq -0x1f0(%rbp), %rsi
movq %rsi, %rdi
addq $0xb8, %rdi
addq $0x68, %rsi
callq 0x5eb0
jmp 0x4015
jmp 0x4017
movl -0x1d4(%rbp), %eax
addl $0x1, %eax
movl %eax, -0x1d4(%rbp)
jmp 0x3f23
movl $0x1, -0x1dc(%rbp)
movslq -0x1dc(%rbp), %rax
movq %rax, -0x280(%rbp)
leaq -0x38(%rbp), %rdi
callq 0x60c0
movq %rax, %rcx
movq -0x280(%rbp), %rax
cmpq %rcx, %rax
jae 0x40c5
movslq -0x1dc(%rbp), %rsi
leaq -0x38(%rbp), %rdi
callq 0x6440
movq -0x1f0(%rbp), %rdi
movq %rax, -0x1c8(%rbp)
movq -0x1c8(%rbp), %rsi
callq 0x41c0
jmp 0x4087
movq -0x1f0(%rbp), %rdi
movq -0x1c8(%rbp), %rsi
movq -0x1c0(%rbp), %rdx
callq 0x4510
jmp 0x40a3
movq -0x1c8(%rbp), %rax
movq %rax, -0x1c0(%rbp)
movl -0x1dc(%rbp), %eax
addl $0x1, %eax
movl %eax, -0x1dc(%rbp)
jmp 0x4035
callq 0x2110
movl %eax, -0x1e0(%rbp)
movq 0x15ee9(%rip), %rdi     # 0x19fc0
leaq 0xdff6(%rip), %rsi      # 0x120d4
callq 0x21d0
movq %rax, -0x288(%rbp)
jmp 0x40ec
movq -0x288(%rbp), %rdi
movl -0x1e0(%rbp), %eax
movl -0x1c(%rbp), %ecx
subl %ecx, %eax
cvtsi2sd %eax, %xmm0
movsd 0xdf2e(%rip), %xmm1     # 0x12038
divsd %xmm1, %xmm0
movsd 0xdf2a(%rip), %xmm1     # 0x12040
mulsd %xmm1, %xmm0
callq 0x23c0
movq %rax, -0x290(%rbp)
jmp 0x4128
movq -0x290(%rbp), %rdi
leaq 0xdf8a(%rip), %rsi      # 0x120c0
callq 0x21d0
movq %rax, -0x298(%rbp)
jmp 0x4144
movq -0x298(%rbp), %rdi
movq 0x15e4e(%rip), %rsi     # 0x19fa0
callq 0x2200
jmp 0x4159
movq -0x1f0(%rbp), %rsi
movq -0x200(%rbp), %rdi
leaq -0x38(%rbp), %rdx
callq 0x4910
jmp 0x4172
leaq -0xc8(%rbp), %rdi
callq 0x68e0
leaq -0x38(%rbp), %rdi
callq 0x6980
movq -0x1f8(%rbp), %rax
addq $0x2a0, %rsp            # imm = 0x2A0
popq %rbp
retq
leaq -0xc8(%rbp), %rdi
callq 0x68e0
leaq -0x38(%rbp), %rdi
callq 0x6980
movq -0xd0(%rbp), %rdi
callq 0x2370
nopl (%rax,%rax)