// File: /workspace/github_cpp_makefile_star2_O1/mitchellsayer[P]FastPointCloudSegmentation/src/./point_cloud_segmenter.cc
// Function: PointCloudSegmenter::CalculatePlaneNormal(std::vector<Vec3, std::allocator<Vec3>>&)
Vec3 PointCloudSegmenter::CalculatePlaneNormal(std::vector<Vec3>& cur_p_gnd) {
  int n = cur_p_gnd.size();

  if (n > 3) {
    //Calculate centroid
    Vec3 centroid;

    std::vector<Vec3>::iterator it;

    for (it = cur_p_gnd.begin(); it != cur_p_gnd.end(); it++) {
      centroid.x += it->x;
      centroid.y += it->y;
      centroid.z += it->z;
    }

    centroid.x /= n;
    centroid.y /= n;
    centroid.z /= n;

    cloud_centroid = centroid;

    //Calc covarience matrix
    double xx = 0, xy = 0, xz = 0,
           yy = 0, yz = 0, zz = 0;

    Vec3 r;

    for (it = cur_p_gnd.begin(); it != cur_p_gnd.end(); it++) {
      r.x = it->x - centroid.x;
      r.y = it->y - centroid.y;
      r.z = it->z - centroid.z;

      xx += r.x * r.x;
      xy += r.x * r.y;
      xz += r.x * r.z;
      yy += r.y * r.y;
      yz += r.y * r.z;
      zz += r.z * r.z;
    }

    double det_x = yy*zz - yz*yz;
    double det_y = xx*zz - xz*xz;
    double det_z = xx*yy - xy*xy;

    double det_max = std::max(det_x, std::max(det_y, det_z));

    Vec3 norm;

    // Pick path with best conditioning
    if (det_max == det_x) {
      norm.x = det_x;
      norm.y = xz*yz - xy*zz;
      norm.z = xy*yz - xz*yy;
    } else if (det_max == det_y) {
      norm.x = xz*yz - xy*zz;
      norm.y = det_y;
      norm.z = xy*xz - yz*xx;
    } else {
      norm.x = xy*yz - xz*yy;
      norm.y = xy*xz - yz*xx;
      norm.z = det_z;
    }

    double dir_mag = sqrt(pow(norm.x, 2) + pow(norm.y, 2) + pow(norm.z, 2));

    norm.x /= dir_mag;
    norm.y /= dir_mag;
    norm.z /= dir_mag;

    return norm;
  }

  return Vec3();
}

// Assembly:
pushq %rbp
movq %rsp, %rbp
pushq %rbx
subq $0x18, %rsp
movq %rdi, %rbx
movq (%rdx), %rcx
movq 0x8(%rdx), %rsi
movq %rsi, %rax
subq %rcx, %rax
shrq $0x2, %rax
imull $0xb6db6db7, %eax, %eax # imm = 0xB6DB6DB7
cmpl $0x4, %eax
jl 0x2fa3
cmpq %rsi, %rcx
je 0x2fbd
xorps %xmm7, %xmm7
xorps %xmm8, %xmm8
xorps %xmm6, %xmm6
addss (%rcx), %xmm7
addss 0x4(%rcx), %xmm8
addss 0x8(%rcx), %xmm6
addq $0x1c, %rcx
cmpq %rsi, %rcx
jne 0x2f89
jmp 0x2fc7
xorps %xmm0, %xmm0
movups %xmm0, (%rbx)
movq $0x0, 0x10(%rbx)
movl $0xbf800000, 0x18(%rbx) # imm = 0xBF800000
jmp 0x328c
xorps %xmm6, %xmm6
xorps %xmm8, %xmm8
xorps %xmm7, %xmm7
cvtsi2ss %eax, %xmm0
divss %xmm0, %xmm7
divss %xmm0, %xmm8
divss %xmm0, %xmm6
movss %xmm7, 0x81c0(%rip)     # 0xb1a0
movss %xmm8, 0x81bb(%rip)     # 0xb1a4
movss %xmm6, 0x81b7(%rip)     # 0xb1a8
movabsq $-0x4080000000000000, %rax # imm = 0xBF80000000000000
movq %rax, 0x81b2(%rip)      # 0xb1b4
movq $0x0, 0x819f(%rip) # 0xb1ac
movq (%rdx), %rax
movq 0x8(%rdx), %rcx
cmpq %rcx, %rax
je 0x30d1
xorpd %xmm2, %xmm2
xorps %xmm0, %xmm0
xorpd %xmm1, %xmm1
xorpd %xmm4, %xmm4
xorpd %xmm5, %xmm5
xorpd %xmm3, %xmm3
movss (%rax), %xmm9
movss 0x4(%rax), %xmm10
subss %xmm7, %xmm9
subss %xmm8, %xmm10
movss 0x8(%rax), %xmm11
subss %xmm6, %xmm11
movaps %xmm9, %xmm12
mulss %xmm9, %xmm12
cvtss2sd %xmm12, %xmm12
addsd %xmm12, %xmm2
movaps %xmm9, %xmm12
mulss %xmm10, %xmm12
cvtss2sd %xmm12, %xmm12
addsd %xmm12, %xmm0
mulss %xmm11, %xmm9
cvtss2sd %xmm9, %xmm9
addsd %xmm9, %xmm1
movaps %xmm10, %xmm9
mulss %xmm10, %xmm9
cvtss2sd %xmm9, %xmm9
addsd %xmm9, %xmm4
mulss %xmm11, %xmm10
xorps %xmm9, %xmm9
cvtss2sd %xmm10, %xmm9
addsd %xmm9, %xmm3
mulss %xmm11, %xmm11
xorps %xmm9, %xmm9
cvtss2sd %xmm11, %xmm9
addsd %xmm9, %xmm5
addq $0x1c, %rax
cmpq %rcx, %rax
jne 0x3034
jmp 0x30e8
xorpd %xmm3, %xmm3
xorpd %xmm5, %xmm5
xorpd %xmm4, %xmm4
xorpd %xmm1, %xmm1
xorps %xmm0, %xmm0
xorpd %xmm2, %xmm2
movapd 0x4f40(%rip), %xmm6     # 0x8030
movapd %xmm3, %xmm7
mulsd %xmm3, %xmm7
movapd %xmm4, %xmm11
mulsd %xmm5, %xmm11
subsd %xmm7, %xmm11
movapd %xmm1, %xmm7
xorpd %xmm6, %xmm7
movapd %xmm1, %xmm9
mulsd %xmm1, %xmm9
movapd %xmm2, %xmm8
mulsd %xmm5, %xmm8
subsd %xmm9, %xmm8
movapd %xmm0, %xmm10
xorpd %xmm6, %xmm10
movapd %xmm0, %xmm12
mulsd %xmm0, %xmm12
movapd %xmm2, %xmm9
mulsd %xmm4, %xmm9
subsd %xmm12, %xmm9
movapd %xmm9, %xmm12
maxsd %xmm8, %xmm12
maxsd %xmm11, %xmm12
xorps %xmm13, %xmm13
movups %xmm13, (%rbx)
movq $0x0, 0x10(%rbx)
movl $0xbf800000, 0x18(%rbx) # imm = 0xBF800000
ucomisd %xmm11, %xmm12
jne 0x31aa
jp 0x31aa
xorps %xmm2, %xmm2
cvtsd2ss %xmm11, %xmm2
mulsd %xmm10, %xmm5
mulsd %xmm3, %xmm1
addsd %xmm5, %xmm1
cvtsd2ss %xmm1, %xmm1
movss %xmm2, (%rbx)
movss %xmm1, 0x4(%rbx)
mulsd %xmm7, %xmm4
mulsd %xmm3, %xmm0
addsd %xmm4, %xmm0
jmp 0x31ed
xorpd %xmm3, %xmm6
ucomisd %xmm8, %xmm12
jne 0x3296
jp 0x3296
mulsd %xmm10, %xmm5
mulsd %xmm1, %xmm3
addsd %xmm5, %xmm3
cvtsd2ss %xmm3, %xmm3
xorps %xmm4, %xmm4
cvtsd2ss %xmm8, %xmm4
movss %xmm3, (%rbx)
movss %xmm4, 0x4(%rbx)
mulsd %xmm6, %xmm2
mulsd %xmm1, %xmm0
addsd %xmm2, %xmm0
cvtsd2ss %xmm0, %xmm0
movss %xmm0, 0x8(%rbx)
xorps %xmm3, %xmm3
cvtss2sd (%rbx), %xmm3
movaps %xmm3, %xmm0
xorps %xmm2, %xmm2
cvtss2sd 0x4(%rbx), %xmm2
mulsd %xmm3, %xmm0
movaps %xmm2, %xmm1
mulsd %xmm2, %xmm1
addsd %xmm0, %xmm1
xorps %xmm4, %xmm4
cvtss2sd 0x8(%rbx), %xmm4
movaps %xmm4, %xmm0
mulsd %xmm4, %xmm0
addsd %xmm1, %xmm0
xorpd %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jb 0x323a
sqrtsd %xmm0, %xmm0
jmp 0x325d
movsd %xmm2, -0x20(%rbp)
movsd %xmm3, -0x18(%rbp)
movsd %xmm4, -0x10(%rbp)
callq 0x2160
movsd -0x10(%rbp), %xmm4
movsd -0x18(%rbp), %xmm3
movsd -0x20(%rbp), %xmm2
divsd %xmm0, %xmm3
xorps %xmm1, %xmm1
cvtsd2ss %xmm3, %xmm1
movss %xmm1, (%rbx)
divsd %xmm0, %xmm2
xorps %xmm1, %xmm1
cvtsd2ss %xmm2, %xmm1
movss %xmm1, 0x4(%rbx)
divsd %xmm0, %xmm4
xorps %xmm0, %xmm0
cvtsd2ss %xmm4, %xmm0
movss %xmm0, 0x8(%rbx)
movq %rbx, %rax
addq $0x18, %rsp
popq %rbx
popq %rbp
retq
mulsd %xmm7, %xmm4
mulsd %xmm0, %xmm3
addsd %xmm4, %xmm3
cvtsd2ss %xmm3, %xmm3
movss %xmm3, (%rbx)
mulsd %xmm6, %xmm2
mulsd %xmm0, %xmm1
addsd %xmm2, %xmm1
xorps %xmm0, %xmm0
cvtsd2ss %xmm1, %xmm0
movss %xmm0, 0x4(%rbx)
xorps %xmm0, %xmm0
cvtsd2ss %xmm9, %xmm0
jmp 0x31f1
nop