// File: /workspace/github_cpp_makefile_star2_O1/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:
pushq %rbx
subq $0x20, %rsp
movq %rdi, %rbx
movsd (%rsi), %xmm11
movsd 0x8(%rsi), %xmm10
movsd 0x10(%rsi), %xmm9
movapd %xmm10, %xmm0
mulsd %xmm10, %xmm0
movapd %xmm11, %xmm1
mulsd %xmm11, %xmm1
addsd %xmm0, %xmm1
movapd %xmm9, %xmm0
mulsd %xmm9, %xmm0
addsd %xmm1, %xmm0
xorpd %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jb 0x2381f
sqrtsd %xmm0, %xmm0
jmp 0x23852
movsd %xmm9, 0x18(%rsp)
movsd %xmm10, 0x10(%rsp)
movsd %xmm11, 0x8(%rsp)
callq 0x52e0
xorpd %xmm1, %xmm1
movsd 0x8(%rsp), %xmm11
movsd 0x10(%rsp), %xmm10
movsd 0x18(%rsp), %xmm9
ucomisd %xmm1, %xmm0
divsd %xmm0, %xmm9
jne 0x23864
jp 0x23864
xorpd %xmm9, %xmm9
divsd %xmm0, %xmm10
jne 0x23872
jp 0x23872
xorpd %xmm10, %xmm10
divsd %xmm0, %xmm11
jne 0x23880
jp 0x23880
xorpd %xmm11, %xmm11
movsd 0x6820(%rip), %xmm2     # 0x2a0a8
maxsd %xmm0, %xmm2
movsd 0x681c(%rip), %xmm1     # 0x2a0b0
mulsd %xmm9, %xmm1
movsd 0x454f(%rip), %xmm3     # 0x27df0
movapd %xmm9, %xmm0
mulsd %xmm3, %xmm0
mulsd %xmm9, %xmm0
addsd 0x4531(%rip), %xmm0     # 0x27de8
mulsd 0x67f9(%rip), %xmm0     # 0x2a0b8
mulsd 0x4429(%rip), %xmm0     # 0x27cf0
movsd 0x67f9(%rip), %xmm4     # 0x2a0c8
divsd %xmm2, %xmm4
mulsd %xmm2, %xmm2
movsd 0x67e1(%rip), %xmm5     # 0x2a0c0
movsd 0x67e9(%rip), %xmm6     # 0x2a0d0
movapd %xmm4, %xmm7
mulsd %xmm6, %xmm7
mulsd %xmm4, %xmm7
movapd %xmm9, %xmm8
mulsd %xmm1, %xmm8
mulsd %xmm3, %xmm0
addsd %xmm8, %xmm0
mulsd %xmm7, %xmm0
addsd 0x36fe(%rip), %xmm0     # 0x27010
divsd %xmm2, %xmm5
mulsd %xmm5, %xmm0
movsd 0x67b6(%rip), %xmm3     # 0x2a0d8
divsd %xmm2, %xmm3
mulsd %xmm4, %xmm3
mulsd %xmm4, %xmm3
mulsd %xmm6, %xmm3
mulsd %xmm1, %xmm3
mulsd %xmm0, %xmm11
movsd %xmm11, (%rbx)
mulsd %xmm0, %xmm10
movsd %xmm10, 0x8(%rbx)
mulsd %xmm9, %xmm0
addsd %xmm3, %xmm0
movsd %xmm0, 0x10(%rbx)
movq %rbx, %rax
addq $0x20, %rsp
popq %rbx
retq
nop