// File: /workspace/github_cpp_makefile_star2_O3/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, %ebp
movq %rcx, 0x40(%rsp)
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 0x114056(%rip), %rsi    # 0x14ce2a
xorl %r14d, %r14d
leaq 0x870(%rsp), %rbx
movq %rbx, %rdi
xorl %eax, %eax
callq 0x27140
leaq 0x135653(%rip), %rsi    # 0x16e443
movq %rbx, %rdi
callq 0x27b20
testq %rax, %rax
je 0x39318
movq %rax, %rbx
cmpl %r12d, 0x44(%r13)
jg 0x38e4d
leaq 0x38(%r13), %r14
movl $0x78, %edi
callq 0x27930
movq %rax, %r15
movq %rax, %rdi
movl $0x2, %esi
callq 0x3e9f4
movq %r13, 0x8(%r15)
movl 0x44(%r13), %edx
movl %edx, 0x10(%r15)
movq %r14, %rdi
movq %r15, %rsi
callq 0xf8c16
movq %r13, %rdi
callq 0x37ec6
cmpl %r12d, 0x44(%r13)
jle 0x38e0e
movq 0x38(%r13), %rax
movslq %r12d, %r15
movq (%rax,%r15,8), %rax
movq %rax, 0x30(%rsp)
xorl %r14d, %r14d
leaq (%rsp,%r14), %rdi
addq $0x5870, %rdi           # imm = 0x5870
callq 0x45524
addq $0x50, %r14
cmpq $0x14000, %r14          # imm = 0x14000
jne 0x38e60
leaq 0x1870(%rsp), %rdi
movl $0x4000, %esi           # imm = 0x4000
movq %rbx, %rdx
callq 0x27fc0
testq %rax, %rax
je 0x39308
movl %ebp, 0x14(%rsp)
leaq 0x18(%r13), %rax
movq %rax, 0x20(%rsp)
leaq 0x1870(%rsp), %r14
movq $0x0, (%rsp)
movq $0x0, 0x8(%rsp)
xorl %ebp, %ebp
incl %ebp
movzbl 0x1870(%rsp), %eax
cmpl $0x22, %eax
jle 0x38ed9
cmpl $0x23, %eax
je 0x38ee1
jmp 0x38efd
testl %eax, %eax
jne 0x39227
movq %r14, %rdi
movl $0x4000, %esi           # imm = 0x4000
movq %rbx, %rdx
callq 0x27fc0
incl %ebp
testq %rax, %rax
jne 0x38ec5
jmp 0x39240
cmpl $0x67, %eax
je 0x38fb7
cmpl $0x76, %eax
jne 0x39227
movq %r14, %rdi
leaq 0x113f54(%rip), %rsi    # 0x14ce6d
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 0x39343
cmpq $0x0, 0x8(%rsp)
je 0x3921f
movq (%rsp), %rax
cmpl $0x3ff, %eax            # imm = 0x3FF
jg 0x39223
movslq (%rsp), %rax
leaq (%rax,%rax,4), %r14
shlq $0x4, %r14
movsd 0x48(%rsp), %xmm0
movsd 0x68(%rsp), %xmm1
movsd 0x60(%rsp), %xmm2
leaq 0x70(%rsp), %rdi
callq 0xd43de
movq (%rsp), %rax
movaps 0x70(%rsp), %xmm0
movaps %xmm0, 0x5870(%rsp,%r14)
movsd 0x80(%rsp), %xmm0
movsd %xmm0, 0x5880(%rsp,%r14)
leaq 0x1870(%rsp), %r14
incl %eax
jmp 0x39223
cmpl $0x0, 0x14(%rsp)
sete %al
cmpq $0x0, 0x8(%rsp)
sete %cl
movl $0x0, %esi
movq (%rsp), %rdx
cmovel %edx, %esi
orb %al, %cl
movq %r15, 0x18(%rsp)
jne 0x3909d
movq %rbx, 0x28(%rsp)
cmpl $0x1a, %edx
leaq 0x70(%rsp), %rbx
jl 0x39078
movq (%rsp), %rax
addl $-0x19, %eax
leaq (%rax,%rax,4), %rax
shlq $0x4, %rax
leaq (%rsp,%rax), %r14
addq $0x5870, %r14           # imm = 0x5870
movl $0x10, %r15d
movsd -0x10(%r14,%r15), %xmm0
movsd -0x8(%r14,%r15), %xmm1
movsd (%r14,%r15), %xmm2
movq %rbx, %rdi
callq 0xd43de
movsd (%r14), %xmm0
movsd 0x8(%r14), %xmm1
movsd 0x10(%r14), %xmm2
leaq 0x48(%rsp), %rdi
callq 0xd43de
movsd 0x58(%rsp), %xmm0
movsd %xmm0, 0x80(%rsp)
movaps 0x70(%rsp), %xmm1
movaps %xmm1, -0x10(%r14,%r15)
movsd %xmm0, (%r14,%r15)
addq $0x50, %r15
cmpl $0x7e0, %r15d           # imm = 0x7E0
jne 0x39014
movq 0x8(%rsp), %rdi
leaq 0x5870(%rsp), %rsi
movq (%rsp), %rdx
callq 0x4109c
xorl %esi, %esi
leaq 0x1870(%rsp), %r14
movq 0x28(%rsp), %rbx
movl %esi, (%rsp)
movq %r14, %rdi
leaq 0x117ac0(%rip), %rsi    # 0x150b6a
leaq 0x470(%rsp), %rdx
leaq 0x70(%rsp), %r14
movq %r14, %rcx
xorl %eax, %eax
callq 0x27900
cmpl $0x2, %eax
jne 0x39343
movq %r14, %rdi
leaq 0x113da5(%rip), %r15    # 0x14ce79
movq %r15, %rsi
callq 0x27f80
xorl %r14d, %r14d
testq %rax, %rax
je 0x39151
xorl %edi, %edi
movq %r15, %rsi
callq 0x27f80
testq %rax, %rax
je 0x39151
movq %rax, %rdi
callq 0x27f70
movl %eax, %r14d
xorl %edi, %edi
movq %r15, %rsi
callq 0x27f80
testq %rax, %rax
je 0x39151
xorl %edi, %edi
movq %r15, %rsi
callq 0x27f80
jmp 0x39151
movl $0x98, %edi
callq 0x27930
movq %rax, %r15
movq %rax, %rdi
xorl %esi, %esi
callq 0x39658
movq %r13, 0x10(%r15)
movl 0x24(%r13), %edx
movl %edx, 0x18(%r15)
movq 0x20(%rsp), %rdi
movq %r15, %rsi
callq 0xf8c16
movq %r13, %rdi
callq 0x37ec6
cmpl %r14d, 0x24(%r13)
jle 0x39119
movq 0x20(%rsp), %rax
movq (%rax), %rax
movslq %r14d, %rcx
movq (%rax,%rcx,8), %r14
cmpl %r12d, 0x2c(%r14)
jg 0x3918e
movl $0x80, %edi
callq 0x27930
movq %rax, %r15
movq %rax, %rdi
callq 0x3eddc
movq %r14, %rdi
movq %r15, %rsi
callq 0x39f7c
jmp 0x39166
movq 0x20(%r14), %rax
movq 0x18(%rsp), %rcx
movq (%rax,%rcx,8), %r15
cmpq $0x0, 0x18(%r15)
jne 0x391af
movq 0x30(%rsp), %rdi
movq %r15, %rsi
callq 0x3eba8
movl $0x78, %edi
callq 0x27930
movq %rax, %r14
movq %rax, %rdi
callq 0x3f594
movq %r15, %rdi
movq %r14, %rsi
callq 0x3f1de
movl $0xc8, %edi
callq 0x27930
movq %rax, %r15
movq %rax, %rdi
callq 0x409a8
movq 0x38(%rsp), %rdi
movl 0xc(%rdi), %edx
movq %r15, 0x8(%rsp)
movq %r15, %rsi
callq 0xf8c16
movq 0x40(%rsp), %rdi
movl 0xc(%rdi), %edx
movq %r14, %rsi
callq 0xf8c16
movl (%rsp), %eax
movq %rax, (%rsp)
leaq 0x1870(%rsp), %r14
movq 0x18(%rsp), %r15
jmp 0x39227
movq (%rsp), %rax
movq %rax, (%rsp)
movq %r14, %rdi
movl $0x4000, %esi           # imm = 0x4000
movq %rbx, %rdx
callq 0x27fc0
testq %rax, %rax
jne 0x38ec3
movq 0x8(%rsp), %rbp
testq %rbp, %rbp
sete %al
cmpl $0x0, 0x14(%rsp)
sete %cl
orb %al, %cl
jne 0x39308
cmpl $0x1a, (%rsp)
jl 0x392f4
movq (%rsp), %rax
addl $-0x19, %eax
leaq (%rax,%rax,4), %rax
shlq $0x4, %rax
leaq (%rsp,%rax), %r12
addq $0x5870, %r12           # imm = 0x5870
movl $0x10, %r13d
leaq 0x70(%rsp), %r14
leaq 0x48(%rsp), %r15
movsd -0x10(%r12,%r13), %xmm0
movsd -0x8(%r12,%r13), %xmm1
movsd (%r12,%r13), %xmm2
movq %r14, %rdi
callq 0xd43de
movsd (%r12), %xmm0
movsd 0x8(%r12), %xmm1
movsd 0x10(%r12), %xmm2
movq %r15, %rdi
callq 0xd43de
movsd 0x58(%rsp), %xmm0
movsd %xmm0, 0x80(%rsp)
movaps 0x70(%rsp), %xmm1
movaps %xmm1, -0x10(%r12,%r13)
movsd %xmm0, (%r12,%r13)
addq $0x50, %r13
cmpl $0x7e0, %r13d           # imm = 0x7E0
jne 0x3928f
leaq 0x5870(%rsp), %rsi
movq %rbp, %rdi
movq (%rsp), %rdx
callq 0x4109c
movq %rbx, %rdi
callq 0x27920
movl $0x1, %r14d
jmp 0x3932e
leaq 0x113b2a(%rip), %rdi    # 0x14ce49
leaq 0x870(%rsp), %rsi
xorl %eax, %eax
callq 0xfb6f2
movl %r14d, %eax
addq $0x19878, %rsp          # imm = 0x19878
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
leaq 0x113ac1(%rip), %rdi    # 0x14ce0b
xorl %r14d, %r14d
leaq 0x870(%rsp), %rdx
movl %ebp, %esi
xorl %eax, %eax
callq 0xfb6f2
jmp 0x3932e
movq %rax, %rbx
movl $0xc8, %esi
jmp 0x39393
movq %rax, %rbx
movl $0x78, %esi
movq %r14, %rdi
jmp 0x39396
movq %rax, %rbx
movl $0x80, %esi
jmp 0x39393
movq %rax, %rbx
movl $0x98, %esi
jmp 0x39393
movq %rax, %rbx
movl $0x78, %esi
movq %r15, %rdi
callq 0x27950
movq %rbx, %rdi
callq 0x28150
nop