// File: tomfunkhouser[P]gaps/apps/conf2sfl/conf2sfl.cpp
// Function: LoadSurfels(gaps::R3SurfelScene*, gaps::RNArray<gaps::R3SegmentationPoint*> const&, gaps::R3SurfelObject*, gaps::R3SurfelNode*, char const*, gaps::R2Grid const&, gaps::R3Point const&)
static R3SurfelBlock *
LoadSurfels(R3SurfelScene *scene, const RNArray<R3SegmentationPoint *>& points,
  R3SurfelObject *parent_object, R3SurfelNode *parent_node,
  const char *node_name, const R2Grid& depth_image, const R3Point& origin)
{
  // Get convenient variables
  R3SurfelTree *tree = scene->Tree();
  R3SurfelDatabase *database = tree->Database();
  int cx = depth_image.XResolution()/2;
  int cy = depth_image.YResolution()/2;
  double rx = cx;
  double ry = cy;
  
  // Allocate array of surfels
  int nsurfels = 0;
  R3Surfel *surfels = new R3Surfel [ points.NEntries() ];
  if (!surfels) {
    RNFail("Unable to allocate surfels for %s\n", node_name);
    return NULL;
  }

  // Load surfels into array
  for (int i = 0; i < points.NEntries(); i++) {
    R3SegmentationPoint *point = points.Kth(i);
        
    // Check depth
    if (RNIsNegativeOrZero(point->depth)) continue;
    if ((max_depth > 0) && (point->depth > max_depth)) continue;

    // Get/check grid index
    int ix, iy;
    if (point->data_index < 0) continue;
    depth_image.IndexToIndices(point->data_index, ix, iy);

    // Check pixel stride
    if (ix % pixel_stride != 0) continue;
    if (iy % pixel_stride != 0) continue;

    // Check if outside elipse centered in image
    if (omit_corners) {
      int dx = (ix - cx) / rx;
      int dy = (iy - cy) / ry;
      if (dx*dx + dy*dy > 1) continue;
    }

    // Set position
    float px = (float) (point->position.X() - origin.X());
    float py = (float) (point->position.Y() - origin.Y());
    float pz = (float) (point->position.Z() - origin.Z());
    surfels[nsurfels].SetPosition(px, py, pz);

    // Set normal
    float nx = (float) point->normal.X();
    float ny = (float) point->normal.Y();
    float nz = (float) point->normal.Z();
    surfels[nsurfels].SetNormal(nx, ny, nz);

    // Set tangent
    float tx = (float) point->tangent.X();
    float ty = (float) point->tangent.Y();
    float tz = (float) point->tangent.Z();
    surfels[nsurfels].SetTangent(tx, ty, tz);

    // Set radius
    surfels[nsurfels].SetRadius(0, (float) point->radius1);
    surfels[nsurfels].SetRadius(1, (float) point->radius2);

    // Set depth
    surfels[nsurfels].SetDepth(point->depth);
    
    // Set color
    surfels[nsurfels].SetColor(point->color);

    // Set identifier
    unsigned int identifier = database->NSurfels() + nsurfels + 1;    
    surfels[nsurfels].SetIdentifier(identifier);

    // Set boundary flags
    if (point->boundary == R3_SURFEL_BORDER_BOUNDARY_FLAG) surfels[nsurfels].SetBorderBoundary(TRUE);
    else if (point->boundary == R3_SURFEL_SILHOUETTE_BOUNDARY_FLAG) surfels[nsurfels].SetSilhouetteBoundary(TRUE);
    else if (point->boundary == R3_SURFEL_SHADOW_BOUNDARY_FLAG) surfels[nsurfels].SetShadowBoundary(TRUE);

    // Increment number of surfels
    nsurfels++;
  }

  // Create block from array of surfels
  R3SurfelBlock *block = new R3SurfelBlock(surfels, nsurfels, origin);
  if (!block) {
    RNFail("Unable to allocate block\n");
    delete [] surfels;
    return NULL;
  }

  // Delete array of surfels
  delete [] surfels;

  // Insert block into database
  block->UpdateProperties();
  database->InsertBlock(block);

  // Create node
  R3SurfelNode *node = NULL;
  if (block && parent_node) {
    // Create node
    node = new R3SurfelNode(node_name);
    if (!node) {
      RNFail("Unable to allocate node for %s\n", node_name);
      delete block;
      return NULL;
    }
            
    // Insert node into tree
    tree->InsertNode(node, parent_node);
    node->InsertBlock(block);
    node->UpdateProperties();
  }

  // Creat object
  R3SurfelObject *object = NULL;
  if (node && parent_object) {
    // Create object
    object = new R3SurfelObject(node_name);
    if (!object) {
      RNFail("Unable to allocate object for %s\n", node_name);
      delete block;
      delete node;
      return NULL;
    }

    // Insert object
    scene->InsertObject(object, parent_object);
    object->InsertNode(node);
    object->UpdateProperties();

    // Create PCA object property
    R3SurfelObjectProperty *pca = new R3SurfelObjectProperty(R3_SURFEL_OBJECT_PCA_PROPERTY, object);
    scene->InsertObjectProperty(pca);
  }

  // Release block
  database->ReleaseBlock(block);

  // Return block
  return block;
}

// Assembly:
subq $0x178, %rsp            # imm = 0x178
movq 0x180(%rsp), %rax
movq %rdi, 0x168(%rsp)
movq %rsi, 0x160(%rsp)
movq %rdx, 0x158(%rsp)
movq %rcx, 0x150(%rsp)
movq %r8, 0x148(%rsp)
movq %r9, 0x140(%rsp)
movq 0x168(%rsp), %rdi
callq 0x1edc0
movq %rax, 0x138(%rsp)
movq 0x138(%rsp), %rdi
callq 0x1edf0
movq %rax, 0x130(%rsp)
movq 0x140(%rsp), %rdi
callq 0x1f1a0
movl $0x2, %ecx
cltd
idivl %ecx
movl %eax, 0x12c(%rsp)
movq 0x140(%rsp), %rdi
callq 0x1f1c0
movl $0x2, %ecx
cltd
idivl %ecx
movl %eax, 0x128(%rsp)
cvtsi2sdl 0x12c(%rsp), %xmm0
movsd %xmm0, 0x120(%rsp)
cvtsi2sdl 0x128(%rsp), %xmm0
movsd %xmm0, 0x118(%rsp)
movl $0x0, 0x114(%rsp)
movq 0x160(%rsp), %rdi
callq 0x1ecf0
cltq
movq %rax, 0x88(%rsp)
movl $0x30, %ecx
mulq %rcx
movq %rax, %rdi
seto %cl
movq $-0x1, %rax
testb $0x1, %cl
cmovneq %rax, %rdi
movq %rdi, 0x80(%rsp)
callq 0x16040
movq %rax, %rcx
movq 0x88(%rsp), %rax
movq %rcx, 0x90(%rsp)
cmpq $0x0, %rax
je 0x1e236
movq 0x90(%rsp), %rax
movq 0x88(%rsp), %rcx
imulq $0x30, %rcx, %rdx
movq %rax, %rcx
addq %rdx, %rcx
movq %rcx, 0x70(%rsp)
movq %rax, 0x78(%rsp)
movq 0x78(%rsp), %rdi
movq %rdi, 0x68(%rsp)
callq 0x4a880
jmp 0x1e21e
movq 0x70(%rsp), %rcx
movq 0x68(%rsp), %rax
addq $0x30, %rax
cmpq %rcx, %rax
movq %rax, 0x78(%rsp)
jne 0x1e20d
movq 0x90(%rsp), %rax
movq %rax, 0x108(%rsp)
cmpq $0x0, 0x108(%rsp)
jne 0x1e2a6
movq 0x148(%rsp), %rsi
leaq 0x1df2c0(%rip), %rdi    # 0x1fd520
movb $0x0, %al
callq 0x1779b0
movq $0x0, 0x170(%rsp)
jmp 0x1ebfe
movq 0x80(%rsp), %rsi
movq 0x90(%rsp), %rdi
movq %rax, %rcx
movl %edx, %eax
movq %rcx, 0x100(%rsp)
movl %eax, 0xfc(%rsp)
callq 0x16e20
jmp 0x1ec0e
movl $0x0, 0xf8(%rsp)
movl 0xf8(%rsp), %eax
movl %eax, 0x64(%rsp)
movq 0x160(%rsp), %rdi
callq 0x1ecf0
movl %eax, %ecx
movl 0x64(%rsp), %eax
cmpl %ecx, %eax
jge 0x1e80a
movq 0x160(%rsp), %rdi
movl 0xf8(%rsp), %esi
callq 0x1f6a0
movq %rax, 0xf0(%rsp)
movq 0xf0(%rsp), %rax
movsd (%rax), %xmm0
leaq 0x24a442(%rip), %rax    # 0x268748
movsd (%rax), %xmm1
callq 0x1f320
cmpl $0x0, %eax
je 0x1e319
jmp 0x1e7f4
movsd 0x24a50f(%rip), %xmm0   # 0x268830
xorps %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jbe 0x1e345
movq 0xf0(%rsp), %rax
movsd (%rax), %xmm0
ucomisd 0x24a4f2(%rip), %xmm0   # 0x268830
jbe 0x1e345
jmp 0x1e7f4
movq 0xf0(%rsp), %rax
cmpl $0x0, 0xd4(%rax)
jge 0x1e35b
jmp 0x1e7f4
movq 0x140(%rsp), %rdi
movq 0xf0(%rsp), %rax
movl 0xd4(%rax), %esi
leaq 0xec(%rsp), %rdx
leaq 0xe8(%rsp), %rcx
callq 0x1f6c0
movl 0xec(%rsp), %eax
cltd
idivl 0x2413cc(%rip)          # 0x25f760
cmpl $0x0, %edx
je 0x1e39e
jmp 0x1e7f4
movl 0xe8(%rsp), %eax
cltd
idivl 0x2413b4(%rip)          # 0x25f760
cmpl $0x0, %edx
je 0x1e3b6
jmp 0x1e7f4
cmpl $0x0, 0x24a47b(%rip)    # 0x268838
je 0x1e437
movl 0xec(%rsp), %eax
subl 0x12c(%rsp), %eax
cvtsi2sd %eax, %xmm0
divsd 0x120(%rsp), %xmm0
cvttsd2si %xmm0, %eax
movl %eax, 0xe4(%rsp)
movl 0xe8(%rsp), %eax
subl 0x128(%rsp), %eax
cvtsi2sd %eax, %xmm0
divsd 0x118(%rsp), %xmm0
cvttsd2si %xmm0, %eax
movl %eax, 0xe0(%rsp)
movl 0xe4(%rsp), %eax
imull 0xe4(%rsp), %eax
movl 0xe0(%rsp), %ecx
imull 0xe0(%rsp), %ecx
addl %ecx, %eax
cmpl $0x1, %eax
jle 0x1e435
jmp 0x1e7f4
jmp 0x1e437
movq 0xf0(%rsp), %rdi
addq $0x8, %rdi
callq 0x1efb0
movsd %xmm0, 0x48(%rsp)
movq 0x180(%rsp), %rdi
callq 0x1efb0
movaps %xmm0, %xmm1
movsd 0x48(%rsp), %xmm0
subsd %xmm1, %xmm0
cvtsd2ss %xmm0, %xmm0
movss %xmm0, 0xdc(%rsp)
movq 0xf0(%rsp), %rdi
addq $0x8, %rdi
callq 0x1efc0
movsd %xmm0, 0x50(%rsp)
movq 0x180(%rsp), %rdi
callq 0x1efc0
movaps %xmm0, %xmm1
movsd 0x50(%rsp), %xmm0
subsd %xmm1, %xmm0
cvtsd2ss %xmm0, %xmm0
movss %xmm0, 0xd8(%rsp)
movq 0xf0(%rsp), %rdi
addq $0x8, %rdi
callq 0x1efd0
movsd %xmm0, 0x58(%rsp)
movq 0x180(%rsp), %rdi
callq 0x1efd0
movaps %xmm0, %xmm1
movsd 0x58(%rsp), %xmm0
subsd %xmm1, %xmm0
cvtsd2ss %xmm0, %xmm0
movss %xmm0, 0xd4(%rsp)
movq 0x108(%rsp), %rdi
movslq 0x114(%rsp), %rax
imulq $0x30, %rax, %rax
addq %rax, %rdi
movss 0xdc(%rsp), %xmm0
movss 0xd8(%rsp), %xmm1
movss 0xd4(%rsp), %xmm2
callq 0x202d0
movq 0xf0(%rsp), %rdi
addq $0x20, %rdi
callq 0x20310
cvtsd2ss %xmm0, %xmm0
movss %xmm0, 0xd0(%rsp)
movq 0xf0(%rsp), %rdi
addq $0x20, %rdi
callq 0x20320
cvtsd2ss %xmm0, %xmm0
movss %xmm0, 0xcc(%rsp)
movq 0xf0(%rsp), %rdi
addq $0x20, %rdi
callq 0x20330
cvtsd2ss %xmm0, %xmm0
movss %xmm0, 0xc8(%rsp)
movq 0x108(%rsp), %rdi
movslq 0x114(%rsp), %rax
imulq $0x30, %rax, %rax
addq %rax, %rdi
movss 0xd0(%rsp), %xmm0
movss 0xcc(%rsp), %xmm1
movss 0xc8(%rsp), %xmm2
callq 0x20340
movq 0xf0(%rsp), %rdi
addq $0x38, %rdi
callq 0x20310
cvtsd2ss %xmm0, %xmm0
movss %xmm0, 0xc4(%rsp)
movq 0xf0(%rsp), %rdi
addq $0x38, %rdi
callq 0x20320
cvtsd2ss %xmm0, %xmm0
movss %xmm0, 0xc0(%rsp)
movq 0xf0(%rsp), %rdi
addq $0x38, %rdi
callq 0x20330
cvtsd2ss %xmm0, %xmm0
movss %xmm0, 0xbc(%rsp)
movq 0x108(%rsp), %rdi
movslq 0x114(%rsp), %rax
imulq $0x30, %rax, %rax
addq %rax, %rdi
movss 0xc4(%rsp), %xmm0
movss 0xc0(%rsp), %xmm1
movss 0xbc(%rsp), %xmm2
callq 0x20410
movq 0x108(%rsp), %rdi
movslq 0x114(%rsp), %rax
imulq $0x30, %rax, %rax
addq %rax, %rdi
movq 0xf0(%rsp), %rax
movsd 0x50(%rax), %xmm0
cvtsd2ss %xmm0, %xmm0
xorl %esi, %esi
callq 0x204e0
movq 0x108(%rsp), %rdi
movslq 0x114(%rsp), %rax
imulq $0x30, %rax, %rax
addq %rax, %rdi
movq 0xf0(%rsp), %rax
movsd 0x58(%rax), %xmm0
cvtsd2ss %xmm0, %xmm0
movl $0x1, %esi
callq 0x204e0
movq 0x108(%rsp), %rdi
movslq 0x114(%rsp), %rax
imulq $0x30, %rax, %rax
addq %rax, %rdi
movq 0xf0(%rsp), %rax
movsd (%rax), %xmm0
cvtsd2ss %xmm0, %xmm0
callq 0x20560
movq 0x108(%rsp), %rdi
movslq 0x114(%rsp), %rax
imulq $0x30, %rax, %rax
addq %rax, %rdi
movq 0xf0(%rsp), %rsi
addq $0x78, %rsi
callq 0x205e0
movq 0x130(%rsp), %rdi
callq 0x1ee20
movslq 0x114(%rsp), %rcx
addq %rcx, %rax
addq $0x1, %rax
movl %eax, 0xb8(%rsp)
movq 0x108(%rsp), %rdi
movslq 0x114(%rsp), %rax
imulq $0x30, %rax, %rax
addq %rax, %rdi
movl 0xb8(%rsp), %esi
callq 0x20670
movq 0xf0(%rsp), %rax
cmpl $0x30, 0xa4(%rax)
jne 0x1e779
movq 0x108(%rsp), %rdi
movslq 0x114(%rsp), %rax
imulq $0x30, %rax, %rax
addq %rax, %rdi
movl $0x1, %esi
callq 0x20690
jmp 0x1e7e3
movq 0xf0(%rsp), %rax
cmpl $0x10, 0xa4(%rax)
jne 0x1e7ad
movq 0x108(%rsp), %rdi
movslq 0x114(%rsp), %rax
imulq $0x30, %rax, %rax
addq %rax, %rdi
movl $0x1, %esi
callq 0x206d0
jmp 0x1e7e1
movq 0xf0(%rsp), %rax
cmpl $0x20, 0xa4(%rax)
jne 0x1e7df
movq 0x108(%rsp), %rdi
movslq 0x114(%rsp), %rax
imulq $0x30, %rax, %rax
addq %rax, %rdi
movl $0x1, %esi
callq 0x20710
jmp 0x1e7e1
jmp 0x1e7e3
movl 0x114(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x114(%rsp)
movl 0xf8(%rsp), %eax
addl $0x1, %eax
movl %eax, 0xf8(%rsp)
jmp 0x1e2b1
movl $0xc8, %edi
callq 0x16740
movq %rax, %rdi
movq %rdi, %rax
movq %rax, 0x40(%rsp)
movq 0x108(%rsp), %rsi
movl 0x114(%rsp), %edx
movq 0x180(%rsp), %rcx
xorps %xmm0, %xmm0
callq 0x4c620
jmp 0x1e840
movq 0x40(%rsp), %rax
movq %rax, 0xb0(%rsp)
cmpq $0x0, 0xb0(%rsp)
jne 0x1e8bc
leaq 0x1dece4(%rip), %rdi    # 0x1fd543
movb $0x0, %al
callq 0x1779b0
movq 0x108(%rsp), %rax
movq %rax, 0x38(%rsp)
cmpq $0x0, %rax
je 0x1e883
movq 0x38(%rsp), %rdi
callq 0x169a0
movq $0x0, 0x170(%rsp)
jmp 0x1ebfe
movq 0x40(%rsp), %rdi
movq %rax, %rcx
movl %edx, %eax
movq %rcx, 0x100(%rsp)
movl %eax, 0xfc(%rsp)
movl $0xc8, %esi
callq 0x16750
jmp 0x1ec0e
movq 0x108(%rsp), %rax
movq %rax, 0x30(%rsp)
cmpq $0x0, %rax
je 0x1e8d9
movq 0x30(%rsp), %rdi
callq 0x169a0
movq 0xb0(%rsp), %rdi
callq 0x4f690
movq 0x130(%rsp), %rdi
movq 0xb0(%rsp), %rsi
movq (%rdi), %rax
callq *0x10(%rax)
movq $0x0, 0xa8(%rsp)
cmpq $0x0, 0xb0(%rsp)
je 0x1ea1d
cmpq $0x0, 0x150(%rsp)
je 0x1ea1d
movl $0xd0, %edi
callq 0x16740
movq %rax, %rdi
movq %rdi, %rax
movq %rax, 0x28(%rsp)
movq 0x148(%rsp), %rsi
callq 0x5acf0
jmp 0x1e94a
movq 0x28(%rsp), %rax
movq %rax, 0xa8(%rsp)
cmpq $0x0, 0xa8(%rsp)
jne 0x1e9dd
movq 0x148(%rsp), %rsi
leaq 0x1deb3d(%rip), %rdi    # 0x1fd4ae
movb $0x0, %al
callq 0x1779b0
movq 0xb0(%rsp), %rax
movq %rax, 0x20(%rsp)
cmpq $0x0, %rax
je 0x1e9a4
movq 0x20(%rsp), %rdi
callq 0x4d4d0
movq 0x20(%rsp), %rdi
movl $0xc8, %esi
callq 0x16750
movq $0x0, 0x170(%rsp)
jmp 0x1ebfe
movq 0x28(%rsp), %rdi
movq %rax, %rcx
movl %edx, %eax
movq %rcx, 0x100(%rsp)
movl %eax, 0xfc(%rsp)
movl $0xd0, %esi
callq 0x16750
jmp 0x1ec0e
movq 0x138(%rsp), %rdi
movq 0xa8(%rsp), %rsi
movq 0x150(%rsp), %rdx
callq 0x5d9f0
movq 0xa8(%rsp), %rdi
movq 0xb0(%rsp), %rsi
movq (%rdi), %rax
callq *0x28(%rax)
movq 0xa8(%rsp), %rdi
callq 0x5cc70
movq $0x0, 0xa0(%rsp)
cmpq $0x0, 0xa8(%rsp)
je 0x1ebd9
cmpq $0x0, 0x158(%rsp)
je 0x1ebd9
movl $0xf8, %edi
callq 0x16740
movq %rax, %rdi
movq %rdi, %rax
movq %rax, 0x18(%rsp)
movq 0x148(%rsp), %rsi
callq 0x692c0
jmp 0x1ea6b
movq 0x18(%rsp), %rax
movq %rax, 0xa0(%rsp)
cmpq $0x0, 0xa0(%rsp)
jne 0x1eb20
movq 0x148(%rsp), %rsi
leaq 0x1deac7(%rip), %rdi    # 0x1fd55d
movb $0x0, %al
callq 0x1779b0
movq 0xb0(%rsp), %rax
movq %rax, 0x10(%rsp)
cmpq $0x0, %rax
je 0x1eac9
movq 0x10(%rsp), %rdi
callq 0x4d4d0
movq 0x10(%rsp), %rdi
movl $0xc8, %esi
callq 0x16750
movq 0xa8(%rsp), %rax
movq %rax, 0x8(%rsp)
cmpq $0x0, %rax
je 0x1eae7
movq 0x8(%rsp), %rdi
movq (%rdi), %rax
callq *0x8(%rax)
movq $0x0, 0x170(%rsp)
jmp 0x1ebfe
movq 0x18(%rsp), %rdi
movq %rax, %rcx
movl %edx, %eax
movq %rcx, 0x100(%rsp)
movl %eax, 0xfc(%rsp)
movl $0xf8, %esi
callq 0x16750
jmp 0x1ec0e
movq 0x168(%rsp), %rdi
movq 0xa0(%rsp), %rsi
movq 0x158(%rsp), %rdx
movq (%rdi), %rax
movq 0x18(%rax), %rax
callq *%rax
movq 0xa0(%rsp), %rdi
movq 0xa8(%rsp), %rsi
callq 0x6b4e0
movq 0xa0(%rsp), %rdi
callq 0x6c150
movl $0x30, %edi
callq 0x16740
movq %rax, %rdi
movq %rdi, %rax
movq %rax, (%rsp)
movq 0xa0(%rsp), %rdx
xorl %r8d, %r8d
movl %r8d, %ecx
movl $0x1, %esi
callq 0x6c7a0
jmp 0x1eb91
movq (%rsp), %rax
movq %rax, 0x98(%rsp)
movq 0x168(%rsp), %rdi
movq 0x98(%rsp), %rsi
movq (%rdi), %rax
callq *0x40(%rax)
jmp 0x1ebd9
movq (%rsp), %rdi
movq %rax, %rcx
movl %edx, %eax
movq %rcx, 0x100(%rsp)
movl %eax, 0xfc(%rsp)
movl $0x30, %esi
callq 0x16750
jmp 0x1ec0e
movq 0x130(%rsp), %rdi
movq 0xb0(%rsp), %rsi
callq 0x20750
movq 0xb0(%rsp), %rax
movq %rax, 0x170(%rsp)
movq 0x170(%rsp), %rax
addq $0x178, %rsp            # imm = 0x178
retq
movq 0x100(%rsp), %rdi
callq 0x16ca0
nopl (%rax,%rax)