// File: /workspace/github_cpp_makefile_star2_O2/istellartech[P]OpenTsiolkovsky/src/gravity.cpp
// Function: gravityECI(Eigen::Matrix<double, 3, 1, 0, 3, 1>)
Vector3d gravityECI(Vector3d posECI_){
    Vector3d gravityECI_;               // Output of this function

    /* WGS84 EGM96 */
    double a = 6378137.0;               // Equatorial radius [m]
    double one_f = 298.257223563;       // Inverse flattening [-]
    double mu = 3.986004418e14;         // Gravitational constant [m3/s2]
    double omega = 7.292115e-5;         // Angular velocity [rad/s]
    double barC20 = -0.484165371736e-3;    // Normalized Gravitational Coefficient \bar{C}_{20}

    double f = 1.0 / one_f;             // Flattening [-]
    double b = a * (1.0 - f);           // Polar radius [m]

    double x = posECI_[0];
    double y = posECI_[1];
    double z = posECI_[2];
    double r = sqrt(x * x + y * y + z * z);

    double irx, iry, irz;           // Unit position vector
    double barP20, barP20d;         // Normalized associated Legendre function \bar{P}_{20} and the derivative;
    double g_ir, g_iz;              // Gravities of r direction and z direction

    if (r == 0.0) {
        irx = iry = irz = 0;
    } else {
        irx = x / r;
        iry = y / r;
        irz = z / r;
    }

    barP20  = sqrt(5.0) * (3.0 * irz * irz - 1.0) * 0.5;
    barP20d = sqrt(5.0) * 3.0 * irz;

    if (r < b) {    // If position is under the ground
        r = b;      // Override the radius
    }

    g_ir = - mu / (r * r) * (1.0 + barC20 * (a/r) * (a/r) * (3.0 * barP20 + irz * barP20d));
    g_iz =   mu / (r * r) * (a/r) * (a/r) * barC20 * barP20d;

    gravityECI_[0] = g_ir * irx;
    gravityECI_[1] = g_ir * iry;
    gravityECI_[2] = g_ir * irz + g_iz;
    return gravityECI_;
}

// Assembly:
movq %rdi, %rax
movsd 0x10(%rsi), %xmm0
movupd (%rsi), %xmm2
movapd %xmm2, %xmm1
mulpd %xmm2, %xmm1
unpckhpd %xmm1, %xmm1            # xmm1 = xmm1[1,1]
movapd %xmm2, %xmm3
mulsd %xmm2, %xmm3
addsd %xmm1, %xmm3
movapd %xmm0, %xmm1
mulsd %xmm0, %xmm1
addsd %xmm3, %xmm1
sqrtsd %xmm1, %xmm3
xorpd %xmm1, %xmm1
movapd %xmm3, %xmm4
cmpeqsd %xmm1, %xmm4
movq %xmm4, %rcx
andl $0x1, %ecx
ucomisd %xmm1, %xmm3
movapd %xmm3, %xmm1
unpcklpd %xmm3, %xmm1            # xmm1 = xmm1[0],xmm3[0]
divpd %xmm1, %xmm2
divsd %xmm3, %xmm0
jne 0x1bee1
jp 0x1bee1
xorpd %xmm0, %xmm0
movd %ecx, %xmm1
pshufd $0x0, %xmm1, %xmm1      # xmm1 = xmm1[0,0,0,0]
pslld $0x1f, %xmm1
psrad $0x1f, %xmm1
pandn %xmm2, %xmm1
movsd 0x6f78(%rip), %xmm4     # 0x22e78
maxsd %xmm3, %xmm4
movsd 0x6f74(%rip), %xmm3     # 0x22e80
mulsd %xmm0, %xmm3
movsd 0x3ff0(%rip), %xmm5     # 0x1ff08
movapd %xmm0, %xmm2
mulsd %xmm5, %xmm2
mulsd %xmm0, %xmm2
addsd 0x3fd4(%rip), %xmm2     # 0x1ff00
mulsd 0x6f54(%rip), %xmm2     # 0x22e88
mulsd 0x3efc(%rip), %xmm2     # 0x1fe38
movsd 0x6f54(%rip), %xmm6     # 0x22e98
divsd %xmm4, %xmm6
mulsd %xmm4, %xmm4
movsd 0x6f3c(%rip), %xmm7     # 0x22e90
movsd 0x6f43(%rip), %xmm8     # 0x22ea0
movapd %xmm6, %xmm9
mulsd %xmm8, %xmm9
mulsd %xmm6, %xmm9
movapd %xmm0, %xmm10
mulsd %xmm3, %xmm10
mulsd %xmm5, %xmm2
addsd %xmm10, %xmm2
mulsd %xmm9, %xmm2
addsd 0x30d4(%rip), %xmm2     # 0x1f060
divsd %xmm4, %xmm7
mulsd %xmm7, %xmm2
movsd 0x6f0c(%rip), %xmm5     # 0x22ea8
divsd %xmm4, %xmm5
mulsd %xmm6, %xmm5
mulsd %xmm6, %xmm5
mulsd %xmm8, %xmm5
mulsd %xmm3, %xmm5
movapd %xmm2, %xmm3
unpcklpd %xmm2, %xmm3            # xmm3 = xmm3[0],xmm2[0]
mulpd %xmm1, %xmm3
movupd %xmm3, (%rax)
mulsd %xmm0, %xmm2
addsd %xmm5, %xmm2
movsd %xmm2, 0x10(%rax)
retq
nop