// File: /workspace/github_cpp_makefile_star2_O2/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:
pushq %rbp
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0x88, %rsp
movq %r8, 0x10(%rsp)
movq %rcx, 0x28(%rsp)
movq %rdx, 0x78(%rsp)
movq %rsi, %rbp
movq 0xc0(%rsp), %rax
movq %rax, 0x20(%rsp)
movq %rdi, 0x80(%rsp)
movq 0x8(%rdi), %rax
movq %rax, 0x70(%rsp)
movq 0x10(%rax), %rax
movq %rax, 0x18(%rsp)
movq %r9, (%rsp)
movq 0xc0(%r9), %xmm0
movdqa %xmm0, 0x30(%rsp)
movslq 0xc(%rsi), %r15
pushq $0x30
popq %rcx
movq %r15, %rax
mulq %rcx
pushq $-0x1
popq %r14
cmovnoq %rax, %r14
movq %r14, %rdi
callq 0x15040
movq %rax, %r12
testq %r15, %r15
je 0x19acb
imulq $0x30, %r15, %rbx
xorl %r15d, %r15d
leaq (%r12,%r15), %rdi
callq 0x2d5ac
addq $0x30, %r15
cmpq %r15, %rbx
jne 0x19ab9
movdqa 0x30(%rsp), %xmm0
movdqa %xmm0, %xmm2
psrld $0x1f, %xmm2
paddd %xmm0, %xmm2
psrad $0x1, %xmm2
cvtdq2pd %xmm2, %xmm3
xorl %ebx, %ebx
leaq 0x142278(%rip), %r14    # 0x15bd68
xorpd %xmm4, %xmm4
xorl %r15d, %r15d
movq (%rsp), %r8
movslq 0xc(%rbp), %rax
cmpq %rax, %rbx
jge 0x19d42
movq (%rbp), %rax
movq (%rax,%rbx,8), %r13
movsd (%r13), %xmm0
movsd (%r14), %xmm1
ucomisd %xmm0, %xmm1
jae 0x19d3a
movsd 0x142323(%rip), %xmm1   # 0x15be50
ucomisd %xmm4, %xmm1
jbe 0x19b3d
ucomisd %xmm1, %xmm0
ja 0x19d3a
movl 0xd4(%r13), %eax
testl %eax, %eax
js 0x19d3a
cltd
idivl 0xc8(%r8)
movl %eax, %ecx
movl %edx, %esi
movl 0x141b02(%rip), %edi    # 0x15b660
movl %edx, %eax
cltd
idivl %edi
testl %edx, %edx
jne 0x19d3a
movl %ecx, %eax
cltd
idivl %edi
testl %edx, %edx
jne 0x19d3a
cmpb $0x1, 0x1422d9(%rip)    # 0x15be58
jne 0x19bbb
movd %ecx, %xmm0
movd %esi, %xmm1
punpckldq %xmm0, %xmm1    # xmm1 = xmm1[0],xmm0[0],xmm1[1],xmm0[1]
psubd %xmm2, %xmm1
cvtdq2pd %xmm1, %xmm0
divpd %xmm3, %xmm0
cvttpd2dq %xmm0, %xmm0
pshufd $0xf5, %xmm0, %xmm1     # xmm1 = xmm0[1,1,3,3]
pmuludq %xmm0, %xmm0
pmuludq %xmm1, %xmm1
paddd %xmm0, %xmm1
movd %xmm1, %eax
cmpl $0x1, %eax
ja 0x19d3a
movapd %xmm3, 0x40(%rsp)
movdqa %xmm2, 0x50(%rsp)
movsd 0x18(%r13), %xmm0
movq 0x20(%rsp), %rcx
subsd 0x10(%rcx), %xmm0
cvtsd2ss %xmm0, %xmm0
movslq %r15d, %rax
movl %r15d, 0xc(%rsp)
imulq $0x30, %rax, %rax
movupd 0x8(%r13), %xmm1
movupd (%rcx), %xmm2
subpd %xmm2, %xmm1
cvtpd2ps %xmm1, %xmm1
movq %rax, 0x68(%rsp)
movlpd %xmm1, (%r12,%rax)
movss %xmm0, 0x8(%r12,%rax)
cvtsd2ss 0x20(%r13), %xmm0
cvtsd2ss 0x28(%r13), %xmm1
leaq (%r12,%rax), %r15
cvtsd2ss 0x30(%r13), %xmm2
movq %r15, %rdi
movq %r15, 0x30(%rsp)
callq 0x1a44e
cvtsd2ss 0x38(%r13), %xmm0
cvtsd2ss 0x40(%r13), %xmm1
cvtsd2ss 0x48(%r13), %xmm2
movq %r15, %rdi
callq 0x1a4b0
movupd 0x50(%r13), %xmm0
cvtpd2ps %xmm0, %xmm0
cvtps2pd %xmm0, %xmm0
mulpd 0xd4413(%rip), %xmm0    # 0xee070
addpd 0xd441b(%rip), %xmm0    # 0xee080
cvtpd2ps %xmm0, %xmm0
movaps 0xd4420(%rip), %xmm1    # 0xee090
minps %xmm0, %xmm1
cvttps2dq %xmm1, %xmm0
pshuflw $0xe8, %xmm0, %xmm0     # xmm0 = xmm0[0,2,2,3,4,5,6,7]
movq 0x68(%rsp), %rcx
movd %xmm0, 0x1c(%r12,%rcx)
cvtsd2ss (%r13), %xmm0
cvtss2sd %xmm0, %xmm0
mulsd 0xd4396(%rip), %xmm0    # 0xee030
addsd 0xd437e(%rip), %xmm0    # 0xee020
cvtsd2ss %xmm0, %xmm0
movss 0xd4402(%rip), %xmm1    # 0xee0b0
minss %xmm0, %xmm1
cvttss2si %xmm1, %eax
movw %ax, 0x28(%r12,%rcx)
movq %rcx, %r15
leaq 0x78(%r13), %rsi
movq 0x30(%rsp), %rdi
callq 0x1a512
movl 0xc(%rsp), %edx
incl %edx
movq 0x18(%rsp), %rax
movl 0x50(%rax), %eax
movl %edx, %ecx
addl %edx, %eax
movl %eax, 0x20(%r12,%r15)
movl 0xa4(%r13), %eax
cmpl $0x30, %eax
je 0x19d19
cmpl $0x20, %eax
je 0x19d15
cmpl $0x10, %eax
movq (%rsp), %r8
movdqa 0x50(%rsp), %xmm2
movl %ecx, %r15d
movapd 0x40(%rsp), %xmm3
xorpd %xmm4, %xmm4
jne 0x19d3a
movb $0x10, %al
jmp 0x19d32
movb $0x20, %al
jmp 0x19d1b
movb $0x30, %al
movq (%rsp), %r8
movdqa 0x50(%rsp), %xmm2
movl %ecx, %r15d
movapd 0x40(%rsp), %xmm3
xorpd %xmm4, %xmm4
movq 0x30(%rsp), %rcx
orb %al, 0x2f(%rcx)
incq %rbx
jmp 0x19afb
movl $0xc8, %edi
callq 0x15620
movq %rax, %r13
pxor %xmm0, %xmm0
movq %rax, %rdi
movq %r12, %rsi
movl %r15d, %edx
movq 0x20(%rsp), %rcx
callq 0x2e24e
movq %r12, %rdi
callq 0x15810
movq %r13, %rdi
callq 0x2f910
movq 0x18(%rsp), %rbx
movq (%rbx), %rax
movq %rbx, %rdi
movq %r13, %rsi
callq *0x10(%rax)
cmpq $0x0, 0x28(%rsp)
movq 0x80(%rsp), %r12
movq 0x78(%rsp), %rbp
je 0x19e4d
movl $0xd0, %edi
callq 0x15620
movq %rax, %r14
movq %rax, %rdi
movq 0x10(%rsp), %rsi
callq 0x34938
movq 0x70(%rsp), %rdi
movq %r14, %rsi
movq 0x28(%rsp), %rdx
callq 0x35d1a
movq (%r14), %rax
movq %r14, %rdi
movq %r13, %rsi
callq *0x28(%rax)
movq %r14, %rdi
callq 0x357ee
testq %rbp, %rbp
je 0x19e4d
movl $0xf8, %edi
callq 0x15620
movq %rax, %r15
movq %rax, %rdi
movq 0x10(%rsp), %rsi
callq 0x3af2c
movq (%r12), %rax
movq %r12, %rdi
movq %r15, %rsi
movq %rbp, %rdx
callq *0x18(%rax)
movq %r15, %rdi
movq %r14, %rsi
callq 0x3be40
movq %r15, %rdi
callq 0x3c326
pushq $0x30
popq %rdi
callq 0x15620
movq %rax, %r14
pushq $0x1
popq %rsi
movq %rax, %rdi
movq %r15, %rdx
xorl %ecx, %ecx
xorl %r8d, %r8d
callq 0x3c38c
movq (%r12), %rax
movq %r12, %rdi
movq %r14, %rsi
callq *0x40(%rax)
movq %rbx, %rdi
movq %r13, %rsi
addq $0x88, %rsp
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
jmp 0x1a546
movq %rax, %rbx
pushq $0x30
popq %rsi
jmp 0x19e86
movq %rax, %rbx
movl $0xf8, %esi
movq %r15, %rdi
jmp 0x19e96
movq %rax, %rbx
movl $0xd0, %esi
movq %r14, %rdi
jmp 0x19e96
movq %rax, %rbx
movl $0xc8, %esi
movq %r13, %rdi
callq 0x15630
jmp 0x19eab
movq %rax, %rbx
movq %r12, %rdi
movq %r14, %rsi
callq 0x15c50
movq %rbx, %rdi
callq 0x15af0
nop