// File: /workspace/github_cpp_makefile_star2_O2/tomfunkhouser[P]gaps/pkgs/R3Surfels/R3SurfelPointGraph.cpp
// Function: gaps::R3SurfelPointGraph::RemoveOutlierEdges(double)
void R3SurfelPointGraph::
RemoveOutlierEdges(RNScalar max_zscore)
{
  // Allocate temporary memory for edge length statistics
  RNScalar *means = new RNScalar [ NPoints() ];
  RNScalar *stddevs = new RNScalar [ NPoints() ];

  // Compute edge length statistics
  for (int i = 0; i < NPoints(); i++) {
    // Compute sum
    RNLength sum = 0;
    R3SurfelPoint *point0 = Point(i);
    R3Point position0 = point0->Position();
    for (int j = 0; j < NNeighbors(i); j++) {
      R3SurfelPoint *point1 = Neighbor(i,j);
      R3Point position1 = point1->Position();
      RNLength edge_length = R3Distance(position0, position1);
      sum += edge_length;
    }
    
    // Compute mean
    means[i] = (NNeighbors(i)) ? sum / NNeighbors(i) : 0;

    // Compute sum of squared residuals
    RNLength ssd = 0;
    for (int j = 0; j < NNeighbors(i); j++) {
      R3SurfelPoint *point1 = Neighbor(i,j);
      R3Point position1 = point1->Position();
      RNLength edge_length = R3Distance(position0, position1);
      RNLength delta = edge_length - means[i];
      ssd += delta * delta;
    }

    // Compute standard deviation
    RNScalar variance = (NNeighbors(i)) ? ssd / NNeighbors(i) : 0; 
    stddevs[i] = sqrt(variance);
  }

  // Remove outlier edges
  for (int i = 0; i < NPoints(); i++) {
    if (stddevs[i] == 0) continue;
    R3SurfelPoint *point0 = Point(i);
    R3Point position0 = point0->Position();
    for (int j = 0; j < neighbors[i].NEntries(); j++) {
      R3SurfelPoint *point1 = neighbors[i].Kth(j);
      R3Point position1 = point1->Position();
      RNLength edge_length = R3Distance(position0, position1);
      RNScalar zscore = (edge_length - means[i]) / stddevs[i];
      if (zscore > max_zscore) {
        // Remove edge
        RNArrayEntry *entry = neighbors[i].KthEntry(j);
        neighbors[i].EntryContents(entry) = neighbors[i].Tail();
        neighbors[i].RemoveTail();
        j--;
      }
    }
  }    

  // Delete temporary memory for edge length statistics
  delete [] means;
  delete [] stddevs;
}

// Assembly:
pushq %rbp
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0x58, %rsp
movsd %xmm0, 0x18(%rsp)
movq %rdi, %rbx
movslq 0x18(%rdi), %rbp
leaq (,%rbp,8), %rax
testq %rbp, %rbp
pushq $-0x1
popq %r15
cmovnsq %rax, %r15
movq %r15, %rdi
callq 0x13030
movq %rax, 0x10(%rsp)
movq %r15, %rdi
callq 0x13030
movq %rax, 0x8(%rsp)
xorl %r15d, %r15d
leaq 0x28(%rsp), %r12
leaq 0x40(%rsp), %r13
movslq %ebp, %rax
cmpq %rax, %r15
jge 0x332a7
movq %r15, %rbp
shlq $0x4, %rbp
movq 0x10(%rbx), %rsi
addq %rbp, %rsi
movq %r12, %rdi
callq 0x16310
xorpd %xmm1, %xmm1
xorl %r14d, %r14d
movq 0x68(%rbx), %rcx
movslq 0xc(%rcx,%rbp), %rax
cmpq %rax, %r14
jge 0x331e6
movq (%rcx,%rbp), %rax
movq (%rax,%r14,8), %rsi
movq %r13, %rdi
movsd %xmm1, (%rsp)
callq 0x16310
movq %r12, %rdi
movq %r13, %rsi
callq 0x495ca
movsd (%rsp), %xmm1
addsd %xmm0, %xmm1
incq %r14
jmp 0x331aa
xorpd %xmm0, %xmm0
movsd %xmm0, (%rsp)
xorpd %xmm2, %xmm2
testl %eax, %eax
je 0x33203
cvtsi2sd %eax, %xmm0
divsd %xmm0, %xmm1
movapd %xmm1, %xmm2
movq 0x10(%rsp), %rdx
movsd %xmm2, (%rdx,%r15,8)
xorl %r14d, %r14d
movsd %xmm2, 0x20(%rsp)
movslq %eax, %rdx
cmpq %rdx, %r14
jge 0x33263
movq (%rcx,%rbp), %rax
movq (%rax,%r14,8), %rsi
movq %r13, %rdi
callq 0x16310
movq %r12, %rdi
movq %r13, %rsi
callq 0x495ca
movsd 0x20(%rsp), %xmm2
subsd %xmm2, %xmm0
mulsd %xmm0, %xmm0
movsd (%rsp), %xmm1
addsd %xmm0, %xmm1
movsd %xmm1, (%rsp)
incq %r14
movq 0x68(%rbx), %rcx
movl 0xc(%rcx,%rbp), %eax
jmp 0x33217
xorpd %xmm1, %xmm1
xorpd %xmm0, %xmm0
testl %eax, %eax
je 0x33280
cvtsi2sd %eax, %xmm0
movsd (%rsp), %xmm2
divsd %xmm0, %xmm2
movapd %xmm2, %xmm0
ucomisd %xmm1, %xmm0
jb 0x3328c
sqrtsd %xmm0, %xmm0
jmp 0x33291
callq 0x133d0
movq 0x8(%rsp), %rax
movsd %xmm0, (%rax,%r15,8)
incq %r15
movl 0x18(%rbx), %ebp
jmp 0x33181
xorl %r15d, %r15d
leaq 0x40(%rsp), %r13
movslq %ebp, %rax
cmpq %rax, %r15
jge 0x33370
movq 0x8(%rsp), %rax
movsd (%rax,%r15,8), %xmm0
ucomisd 0x950b2(%rip), %xmm0    # 0xc8380
jne 0x332d6
jnp 0x33368
movsd %xmm0, (%rsp)
movq %r15, %rbp
shlq $0x4, %rbp
movq 0x10(%rbx), %rsi
addq %rbp, %rsi
leaq 0x28(%rsp), %rdi
callq 0x16310
xorl %r14d, %r14d
movq 0x68(%rbx), %rax
cmpl 0xc(%rax,%rbp), %r14d
jge 0x33365
addq %rbp, %rax
movq (%rax), %rax
movslq %r14d, %r12
movq (%rax,%r12,8), %rsi
movq %r13, %rdi
callq 0x16310
leaq 0x28(%rsp), %rdi
movq %r13, %rsi
callq 0x495ca
movq 0x10(%rsp), %rax
subsd (%rax,%r15,8), %xmm0
divsd (%rsp), %xmm0
ucomisd 0x18(%rsp), %xmm0
jbe 0x33360
movq 0x68(%rbx), %rax
movq (%rax,%rbp), %rcx
movslq 0xc(%rax,%rbp), %rax
movq -0x8(%rcx,%rax,8), %rax
movq %rax, (%rcx,%r12,8)
movq 0x68(%rbx), %rdi
addq %rbp, %rdi
callq 0x17820
decl %r14d
incl %r14d
jmp 0x332f6
movl 0x18(%rbx), %ebp
incq %r15
jmp 0x332af
movq 0x10(%rsp), %rdi
callq 0x136a0
movq 0x8(%rsp), %rdi
callq 0x136a0
addq $0x58, %rsp
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
nop