// File: 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 %rbp
movq %rsp, %rbp
subq $0xd0, %rsp
movq %rsi, -0xa8(%rbp)
movq %rdi, -0xb8(%rbp)
movq %rdi, %rax
movq %rax, -0xb0(%rbp)
movq %rdi, -0x8(%rbp)
movq %rsi, -0x10(%rbp)
callq 0x1d670
movq -0xa8(%rbp), %rdi
movsd 0x10ca1(%rip), %xmm0    # 0x59750
movsd %xmm0, -0x18(%rbp)
movsd 0x10c8c(%rip), %xmm0    # 0x59748
movsd %xmm0, -0x20(%rbp)
movsd 0x10c77(%rip), %xmm0    # 0x59740
movsd %xmm0, -0x28(%rbp)
movsd 0x10c62(%rip), %xmm0    # 0x59738
movsd %xmm0, -0x30(%rbp)
movsd 0x10c4d(%rip), %xmm0    # 0x59730
movsd %xmm0, -0x38(%rbp)
movsd 0x9550(%rip), %xmm0     # 0x52040
divsd -0x20(%rbp), %xmm0
movsd %xmm0, -0x40(%rbp)
movsd -0x18(%rbp), %xmm0
movsd 0x9539(%rip), %xmm1     # 0x52040
subsd -0x40(%rbp), %xmm1
mulsd %xmm1, %xmm0
movsd %xmm0, -0x48(%rbp)
xorl %eax, %eax
movl %eax, %esi
callq 0x1de40
movq -0xa8(%rbp), %rdi
movsd (%rax), %xmm0
movsd %xmm0, -0x50(%rbp)
movl $0x1, %esi
callq 0x1de40
movq -0xa8(%rbp), %rdi
movsd (%rax), %xmm0
movsd %xmm0, -0x58(%rbp)
movl $0x2, %esi
callq 0x1de40
movsd (%rax), %xmm0
movsd %xmm0, -0x60(%rbp)
movsd -0x50(%rbp), %xmm1
movsd -0x50(%rbp), %xmm2
movsd -0x58(%rbp), %xmm0
mulsd -0x58(%rbp), %xmm0
mulsd %xmm2, %xmm1
addsd %xmm0, %xmm1
movsd -0x60(%rbp), %xmm0
movsd -0x60(%rbp), %xmm2
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
callq 0x6390
movsd %xmm0, -0x68(%rbp)
movsd -0x68(%rbp), %xmm0
xorps %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jne 0x48bbd
jp 0x48bbd
xorps %xmm0, %xmm0
movsd %xmm0, -0x80(%rbp)
xorps %xmm0, %xmm0
movsd %xmm0, -0x78(%rbp)
xorps %xmm0, %xmm0
movsd %xmm0, -0x70(%rbp)
jmp 0x48bea
movsd -0x50(%rbp), %xmm0
divsd -0x68(%rbp), %xmm0
movsd %xmm0, -0x70(%rbp)
movsd -0x58(%rbp), %xmm0
divsd -0x68(%rbp), %xmm0
movsd %xmm0, -0x78(%rbp)
movsd -0x60(%rbp), %xmm0
divsd -0x68(%rbp), %xmm0
movsd %xmm0, -0x80(%rbp)
movsd 0xa376(%rip), %xmm0     # 0x52f68
callq 0x6390
movsd 0xa2c9(%rip), %xmm1     # 0x52ec8
mulsd -0x80(%rbp), %xmm1
movsd -0x80(%rbp), %xmm2
mulsd %xmm2, %xmm1
movsd 0xa253(%rip), %xmm2     # 0x52e68
addsd %xmm2, %xmm1
mulsd %xmm1, %xmm0
movsd 0xa23b(%rip), %xmm1     # 0x52e60
mulsd %xmm1, %xmm0
movsd %xmm0, -0x88(%rbp)
movsd 0xa32f(%rip), %xmm0     # 0x52f68
callq 0x6390
movsd 0xa282(%rip), %xmm1     # 0x52ec8
mulsd %xmm1, %xmm0
mulsd -0x80(%rbp), %xmm0
movsd %xmm0, -0x90(%rbp)
movsd -0x68(%rbp), %xmm1
movsd -0x48(%rbp), %xmm0
ucomisd %xmm1, %xmm0
jbe 0x48c71
movsd -0x48(%rbp), %xmm0
movsd %xmm0, -0x68(%rbp)
movq -0xb8(%rbp), %rdi
movsd -0x28(%rbp), %xmm0
movq %xmm0, %rax
movabsq $-0x8000000000000000, %rcx # imm = 0x8000000000000000
xorq %rcx, %rax
movq %rax, %xmm0
movsd -0x68(%rbp), %xmm1
mulsd -0x68(%rbp), %xmm1
divsd %xmm1, %xmm0
movsd -0x38(%rbp), %xmm1
movsd -0x18(%rbp), %xmm2
divsd -0x68(%rbp), %xmm2
mulsd %xmm2, %xmm1
movsd -0x18(%rbp), %xmm2
divsd -0x68(%rbp), %xmm2
mulsd %xmm2, %xmm1
movsd -0x88(%rbp), %xmm2
movsd -0x80(%rbp), %xmm3
mulsd -0x90(%rbp), %xmm3
movsd 0xa1e8(%rip), %xmm4     # 0x52ec8
mulsd %xmm4, %xmm2
addsd %xmm3, %xmm2
mulsd %xmm2, %xmm1
movsd 0x934c(%rip), %xmm2     # 0x52040
addsd %xmm2, %xmm1
mulsd %xmm1, %xmm0
movsd %xmm0, -0x98(%rbp)
movsd -0x28(%rbp), %xmm0
movsd -0x68(%rbp), %xmm1
mulsd -0x68(%rbp), %xmm1
divsd %xmm1, %xmm0
movsd -0x18(%rbp), %xmm1
divsd -0x68(%rbp), %xmm1
mulsd %xmm1, %xmm0
movsd -0x18(%rbp), %xmm1
divsd -0x68(%rbp), %xmm1
mulsd %xmm1, %xmm0
mulsd -0x38(%rbp), %xmm0
mulsd -0x90(%rbp), %xmm0
movsd %xmm0, -0xa0(%rbp)
movsd -0x98(%rbp), %xmm0
mulsd -0x70(%rbp), %xmm0
movsd %xmm0, -0xd0(%rbp)
xorl %eax, %eax
movl %eax, %esi
callq 0x1de40
movsd -0xd0(%rbp), %xmm0
movq -0xb8(%rbp), %rdi
movsd %xmm0, (%rax)
movsd -0x98(%rbp), %xmm0
mulsd -0x78(%rbp), %xmm0
movsd %xmm0, -0xc8(%rbp)
movl $0x1, %esi
callq 0x1de40
movsd -0xc8(%rbp), %xmm0
movq -0xb8(%rbp), %rdi
movsd %xmm0, (%rax)
movsd -0x98(%rbp), %xmm0
movsd -0x80(%rbp), %xmm2
movsd -0xa0(%rbp), %xmm1
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, -0xc0(%rbp)
movl $0x2, %esi
callq 0x1de40
movsd -0xc0(%rbp), %xmm0
movq %rax, %rcx
movq -0xb0(%rbp), %rax
movsd %xmm0, (%rcx)
addq $0xd0, %rsp
popq %rbp
retq
nopl (%rax)