// File: /workspace/github_cpp_makefile_star2_O2/tomfunkhouser[P]gaps/pkgs/GSV/GSVRun.cpp
// Function: gaps::GSVRun::ReadLaserObjFile(int, gaps::RNArray<gaps::GSVScanline*>&, gaps::RNArray<gaps::GSVScan*>&, int)
int GSVRun::
ReadLaserObjFile(int laser_index, RNArray<GSVScanline *>& scanlines, RNArray<GSVScan *>& scanline_scans, RNBoolean read_points)
{
  // Create filename
  char filename[4096];
  sprintf(filename, "%s/%s/laser_point_cloud_%d.obj", scene->RawDataDirectoryName(), Name(), laser_index+1);

  // Open file
  FILE *fp = fopen(filename, "r");
  if (!fp) {
    RNFail("Unable to open laser pose file: %s\n", filename);
    return 0;
  }

  // Create laser
  GSVLaser *laser = NULL;
  while (NLasers() <= laser_index) 
    InsertLaser(new GSVLaser(GSV_SICK_LASER));
  laser = Laser(laser_index);

  // Read points for each scanline
  int line_count = 0;
  const int buffer_size = 16*1024;
  char buffer[buffer_size];
  char keyword[1024];
  GSVScanline *scanline  = NULL;
  int num_scanline_points = 0;
  const int max_scanline_points = 1024;
  GSVPoint scanline_points[max_scanline_points];
  while (fgets(buffer, buffer_size, fp)) {
    line_count++;
    if (buffer[0] == '\0') continue;
    if (buffer[0] == '#') continue;
    if (buffer[0] == 'v') {
      // Parse point
      double x, y, z; 
      if (sscanf(buffer, "%s%lf%lf%lf", keyword, &x, &y, &z) != 4) {
        RNFail("Error reading line %d from %s\n", line_count, filename);
        return 0;
      }

      // Insert point
      if (scanline) {
        if (num_scanline_points < max_scanline_points) {
          scanline_points[num_scanline_points].SetPosition(R3Point(x, y, z));
          num_scanline_points++;
        }
      }
    }
    else if (buffer[0] == 'g') {
      // Set points in previous scanline
      if (scanline) {
        if (read_points) {
          // Project car points onto ground
          if (num_scanline_points > 25) {
            for (int i = num_scanline_points - 25; i < num_scanline_points; i++) {
              R3Point position =  scanline_points[i].Position();
              position[2] = scanline_points[num_scanline_points-25].Position()[2];
              scanline_points[i].SetPosition(position);
            }
          }
          scanline->SetPoints(scanline_points, num_scanline_points);
        }
        num_scanline_points = 0;
        scanline = NULL;
      }

      // Get scanline string
      char scanline_string[1024];
      if (sscanf(buffer, "%s%s", keyword, scanline_string) != 2) {
        RNFail("Error reading line %d from %s\n", line_count, filename);
        return 0;
      }

      // Parse scanline string
      int segment_index = 0;
      // int scanline_index = 0;
      char *bufferp = strtok(scanline_string, "_ \n\t");
      if (bufferp) { 
        assert(!strcmp(bufferp, "seg"));
        bufferp = strtok(NULL, "_ \n\t"); 
        if (bufferp) {
          segment_index = atoi(bufferp);
          bufferp = strtok(NULL, "_ \n\t"); 
          if (bufferp) {
            assert(!strcmp(bufferp, "scanline"));
            bufferp = strtok(NULL, "_ \n\t"); 
            if (bufferp) {
              // scanline_index = atoi(bufferp);
            }
          }
        }
      }

      // Get/create segment
      GSVSegment *segment = NULL;
      while (NSegments() <= segment_index) 
        InsertSegment(new GSVSegment());
      segment = Segment(segment_index);
        
      // Get/create survey
      GSVSurvey *survey = NULL;
      while (segment->NSurveys() <= laser_index) 
        segment->InsertSurvey(new GSVSurvey());
      survey = segment->Survey(laser_index);

      // Insert survey into laser
      if (survey->Laser() == NULL) laser->InsertSurvey(survey);
      assert(survey->Laser() == laser);

      // Get/create scan
      GSVScan *scan = new GSVScan();
      survey->InsertScan(scan);

      // Create scanline
      scanline = new GSVScanline();

      // Insert scanline into array to be returned
      scanlines.Insert(scanline);

      // Insert scan into array to be returned (so that can insert scanlines conditionally based on pose)
      scanline_scans.Insert(scan);
    }
  }

  // Set points in last scanline
  if (scanline) {
    if (read_points) {
      // Project car points onto ground
      if (num_scanline_points > 25) {
        for (int i = num_scanline_points - 25; i < num_scanline_points; i++) {
          R3Point position =  scanline_points[i].Position();
          position[2] = scanline_points[num_scanline_points-25].Position()[2];
          scanline_points[i].SetPosition(position);
        }
      }
      scanline->SetPoints(scanline_points, num_scanline_points);
    }
    num_scanline_points = 0;
    scanline = NULL;
  }

  // Close file
  fclose(fp);

  // Return success
  return 1;
}

// Assembly:
pushq %rbp
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0x19878, %rsp          # imm = 0x19878
movl %r8d, 0x14(%rsp)
movq %rcx, %r14
movq %rdx, 0x38(%rsp)
movl %esi, %r12d
movq %rdi, %r13
movq 0x8(%rdi), %rax
movq 0x48(%rdi), %rcx
movq 0x20(%rax), %rdx
leal 0x1(%r12), %r8d
leaq 0xf4e84(%rip), %rsi     # 0x12be2a
xorl %ebp, %ebp
leaq 0x870(%rsp), %rbx
movq %rbx, %rdi
xorl %eax, %eax
callq 0x27140
leaq 0x1160e2(%rip), %rsi    # 0x14d0a3
movq %rbx, %rdi
callq 0x27b20
testq %rax, %rax
je 0x37397
movq %r14, 0x30(%rsp)
movq %rax, 0x18(%rsp)
pushq $0x78
popq %r14
pushq $0x2
popq %rbp
cmpl %r12d, 0x44(%r13)
jg 0x3700b
movq %r14, %rdi
callq 0x27940
movq %rax, %r15
movq %rax, %rdi
movl %ebp, %esi
callq 0x3c210
movq %r13, %rdi
movq %r15, %rsi
callq 0x363c8
jmp 0x36fe3
movq 0x38(%r13), %rax
movslq %r12d, %rcx
movq %rcx, 0x28(%rsp)
movq (%rax,%rcx,8), %rax
movq %rax, 0x20(%rsp)
xorl %ebx, %ebx
leaq (%rsp,%rbx), %rdi
addq $0x5870, %rdi           # imm = 0x5870
callq 0x42260
addq $0x50, %rbx
cmpq $0x14000, %rbx          # imm = 0x14000
jne 0x37022
movq $0x0, 0x8(%rsp)
xorl %ebp, %ebp
xorl %r15d, %r15d
movq 0x18(%rsp), %r14
incl %r15d
leaq 0x1870(%rsp), %rdi
movl $0x4000, %esi           # imm = 0x4000
movq %r14, %rdx
callq 0x27fe0
movl %ebp, %ebx
testq %rax, %rax
je 0x37316
movzbl 0x1870(%rsp), %eax
testl %eax, %eax
je 0x37052
cmpl $0x23, %eax
je 0x37052
cmpl $0x67, %eax
je 0x37127
cmpl $0x76, %eax
jne 0x37052
leaq 0x1870(%rsp), %rdi
leaq 0xf4dca(%rip), %rsi     # 0x12be6d
leaq 0x470(%rsp), %rdx
leaq 0x48(%rsp), %rcx
leaq 0x68(%rsp), %r8
leaq 0x60(%rsp), %r9
xorl %eax, %eax
callq 0x27900
cmpl $0x4, %eax
jne 0x373e7
cmpq $0x0, 0x8(%rsp)
je 0x37052
cmpl $0x3ff, %ebp            # imm = 0x3FF
jg 0x37052
movslq %ebp, %rax
imulq $0x50, %rax, %rbx
movsd 0x48(%rsp), %xmm0
movsd 0x68(%rsp), %xmm1
movsd 0x60(%rsp), %xmm2
leaq 0x70(%rsp), %rdi
callq 0xc0dfc
movaps 0x70(%rsp), %xmm0
movaps %xmm0, 0x5870(%rsp,%rbx)
movsd 0x80(%rsp), %xmm0
movsd %xmm0, 0x5880(%rsp,%rbx)
incl %ebp
jmp 0x37052
cmpl $0x0, 0x14(%rsp)
sete %al
cmpq $0x0, 0x8(%rsp)
sete %cl
movl $0x0, %r14d
cmovel %ebp, %r14d
orb %al, %cl
jne 0x371c2
cmpl $0x1a, %ebp
jl 0x371ab
leal -0x19(%rbp), %eax
imulq $0x50, %rax, %rcx
leaq (%rsp,%rcx), %r14
addq $0x5870, %r14           # imm = 0x5870
subq %rax, %rbx
movq %r14, 0x40(%rsp)
subq $0x1, %rbx
jb 0x371ab
leaq 0x70(%rsp), %rdi
movq %r14, %rsi
callq 0x310c8
leaq 0x48(%rsp), %rdi
movq 0x40(%rsp), %rsi
callq 0x310c8
movsd 0x58(%rsp), %xmm0
movsd %xmm0, 0x80(%rsp)
movaps 0x70(%rsp), %xmm1
movaps %xmm1, (%r14)
movsd %xmm0, 0x10(%r14)
addq $0x50, %r14
jmp 0x37165
movq 0x8(%rsp), %rdi
leaq 0x5870(%rsp), %rsi
movl %ebp, %edx
callq 0x3e3b2
xorl %r14d, %r14d
leaq 0x1870(%rsp), %rdi
leaq 0xf88c9(%rip), %rsi     # 0x12fa9a
leaq 0x470(%rsp), %rdx
leaq 0x70(%rsp), %rbx
movq %rbx, %rcx
xorl %eax, %eax
callq 0x27900
cmpl $0x2, %eax
jne 0x373e7
movq %rbx, %rdi
leaq 0xf4c7e(%rip), %rbx     # 0x12be79
movq %rbx, %rsi
callq 0x27f90
xorl %ebp, %ebp
testq %rax, %rax
je 0x3723c
xorl %edi, %edi
movq %rbx, %rsi
callq 0x27f90
testq %rax, %rax
je 0x3723c
movq %rax, %rdi
callq 0x27f80
movl %eax, %ebp
xorl %edi, %edi
movq %rbx, %rsi
callq 0x27f90
testq %rax, %rax
je 0x3723c
xorl %edi, %edi
movq %rbx, %rsi
callq 0x27f90
movl %r14d, %ebx
cmpl %ebp, 0x24(%r13)
jg 0x37269
movl $0x98, %edi
callq 0x27940
movq %rax, %r14
movq %rax, %rdi
xorl %esi, %esi
callq 0x37710
movq %r13, %rdi
movq %r14, %rsi
callq 0x36272
jmp 0x3723f
movq 0x18(%r13), %rax
movslq %ebp, %rcx
movq (%rax,%rcx,8), %r14
cmpl %r12d, 0x2c(%r14)
jg 0x3729c
movl $0x80, %edi
callq 0x27940
movq %rax, %rbp
movq %rax, %rdi
callq 0x3c590
movq %r14, %rdi
movq %rbp, %rsi
callq 0x37e76
jmp 0x37274
movq 0x20(%r14), %rax
movq 0x28(%rsp), %rcx
movq (%rax,%rcx,8), %r14
cmpq $0x0, 0x18(%r14)
jne 0x372bd
movq 0x20(%rsp), %rdi
movq %r14, %rsi
callq 0x3c3a8
pushq $0x78
popq %rdi
callq 0x27940
movq %rax, %rbp
movq %rax, %rdi
callq 0x3ccdc
movq %r14, %rdi
movq %rbp, %rsi
callq 0x3c966
movl $0xc8, %edi
callq 0x27940
movq %rax, %r14
movq %rax, %rdi
callq 0x3ddd8
movq 0x38(%rsp), %rdi
movq %r14, 0x8(%rsp)
movq %r14, %rsi
callq 0x3110a
movq 0x30(%rsp), %rdi
movq %rbp, %rsi
callq 0x3110a
movl %ebx, %ebp
jmp 0x3704d
cmpl $0x0, 0x14(%rsp)
sete %al
cmpq $0x0, 0x8(%rsp)
sete %cl
orb %al, %cl
jne 0x373c8
cmpl $0x1a, %ebp
jl 0x373af
leal -0x19(%rbp), %eax
imulq $0x50, %rax, %rcx
leaq (%rsp,%rcx), %r14
addq $0x5870, %r14           # imm = 0x5870
subq %rax, %rbx
leaq 0x70(%rsp), %r15
leaq 0x48(%rsp), %r12
movq %r14, %r13
subq $0x1, %rbx
jb 0x373af
movq %r15, %rdi
movq %r13, %rsi
callq 0x310c8
movq %r12, %rdi
movq %r14, %rsi
callq 0x310c8
movsd 0x58(%rsp), %xmm0
movsd %xmm0, 0x80(%rsp)
movaps 0x70(%rsp), %xmm1
movaps %xmm1, (%r13)
movsd %xmm0, 0x10(%r13)
addq $0x50, %r13
jmp 0x37356
leaq 0xf4aab(%rip), %rdi     # 0x12be49
leaq 0x870(%rsp), %rsi
xorl %eax, %eax
callq 0xe434a
jmp 0x373d3
leaq 0x5870(%rsp), %rsi
movq 0x8(%rsp), %rdi
movl %ebp, %edx
callq 0x3e3b2
movq 0x18(%rsp), %r14
movq %r14, %rdi
callq 0x27930
pushq $0x1
popq %rbp
movl %ebp, %eax
addq $0x19878, %rsp          # imm = 0x19878
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
leaq 0xf4a1d(%rip), %rdi     # 0x12be0b
xorl %ebp, %ebp
leaq 0x870(%rsp), %rdx
movl %r15d, %esi
xorl %eax, %eax
callq 0xe434a
jmp 0x373d3
movq %rax, %rbx
movl $0xc8, %esi
jmp 0x3742b
movq %rax, %rbx
pushq $0x78
popq %rsi
jmp 0x3741e
movq %rax, %rbx
movl $0x80, %esi
movq %rbp, %rdi
jmp 0x37439
movq %rax, %rbx
movl $0x98, %esi
movq %r14, %rdi
jmp 0x37439
movq %rax, %rbx
pushq $0x78
popq %rsi
movq %r15, %rdi
callq 0x27960
movq %rbx, %rdi
callq 0x28170