// File: /workspace/github_cpp_makefile_star2_O1/istellartech[P]OpenTsiolkovsky/src/coordinate_transform.cpp
// Function: posLLH_IIP(Eigen::Matrix<double, 3, 1, 0, 3, 1>, Eigen::Matrix<double, 3, 1, 0, 3, 1>)
Vector3d posLLH_IIP(Vector3d posECEF_, Vector3d vel_ECEF_ECEFframe_){

    int n_iter = 5;
    double Ra = 6378137.0;
    double f = 1.0 / 298.257223563;
    double Rb = Ra * (1.0 - f);
    double e2 = (Ra * Ra - Rb *Rb) / Ra / Ra;
    double mu = 3.986004418e14;
    double omega = 7.2921151467e-5;
    Vector3d omegaVec_;
    omegaVec_ << 0.0, 0.0, omega;

    // (v)-(A): The distance frome the center of the Earth ellipsoid to the launch point (the initial approximation of r_k1, k=1)
    double r_k1 = Rb;
   
    // (v)-(B): The radial distance from the geocenter to the launch vehicle position
    Vector3d posECI_init_ = posECEF_;
    double r0 = posECI_init_.norm();
    if (r0 < r_k1) // then tha launch vehicle position is below the Earth's surface and an impact point cannot be computed
        return Vector3d::Zero();      // no solution
    
    // (v)-(C): The inertial velocity compoents
    Vector3d velECI_init_ = vel_ECEF_ECEFframe_ + omegaVec_.cross(posECEF_);
    // (v)-(D): The magnitude of the inertial velocity vector
    double v0 = velECI_init_.norm();
    
    // (v)-(E): The eccentricity of the trajectory ellipse multiplied by the cosine of the eccentric anomaly at epoch
    double eps_cos = (r0 * v0 * v0 / mu) - 1.0;
    
    if (eps_cos >= 1.0) // then the trajectory orbit is not elliptical, but is hyperbolic or parabolic, and an impact point cannot be computed
        return Vector3d::Zero();      // no solution

    // (v)-(F): The semi-major axis of the trajectory ellipse
    double a_t = r0 / (1 - eps_cos);
    
    // (v)-(G): The eccentricity of the trajectory ellipse multiplied by the sine of the eccentric anomaly at epoch
    double eps_sin = posECI_init_.dot(velECI_init_) / sqrt(mu * a_t);
    
    // (v)-(H): The eccentricity of the trajectory ellipse squared 
    double eps2 = eps_cos * eps_cos + eps_sin * eps_sin;
    if (sqrt(eps2) <= 1.0 && a_t * (1 - sqrt(eps2)) - Ra >= 0.0) // then the trajectory perigee height is positive and an impact point cannot be computed
        return Vector3d::Zero();      // no solution
    
    double eps_k_cos, eps_k_sin, delta_eps_k_cos, delta_eps_k_sin;
    double fseries_2, gseries_2, Ek, Fk, Gk, r_k2, r_k1_tmp;
    
    for (int i=0; i<n_iter; i++){
        // (v)-(I): The eccentricity of the trajectory ellipse multiplied by the cosine of the eccentric anomaly at impact
        eps_k_cos = (a_t - r_k1) / a_t;
        
        // (v)-(J): The eccentricity of the trajectory ellipse multiplied by the sine of the eccentric anomaly at impact
        if ((eps2 - eps_k_cos * eps_k_cos) < 0) // then the trajectory orbit does not intersect the Earth's surface and an impact point cannot be computed
            return Vector3d::Zero();      // no solution
        eps_k_sin = -sqrt(eps2 - eps_k_cos * eps_k_cos);
        
        // (v)-(K): The cosine of the difference between the eccentric anomaly at impact and epoch
        delta_eps_k_cos = (eps_k_cos*eps_cos + eps_k_sin*eps_sin) / eps2;
        
        // (v)-(L): The sine of the difference between the eccentric anomaly at impact and epoch
        delta_eps_k_sin = (eps_k_sin*eps_cos - eps_k_cos*eps_sin) / eps2;
        
        // (v)-(M): The f-series expansion of Kepler's equations
        fseries_2 = (delta_eps_k_cos - eps_cos) / (1 - eps_cos);
        // (v)-(N): The g-series expansion of Kepler's equations
        gseries_2 = (delta_eps_k_sin + eps_sin - eps_k_sin) * sqrt(a_t*a_t*a_t / mu);
        
        // (v)-(O): The E,F,G coordinates at impact
        Ek = fseries_2*posECI_init_[0] + gseries_2*velECI_init_[0];
        Fk = fseries_2*posECI_init_[1] + gseries_2*velECI_init_[1];
        Gk = fseries_2*posECI_init_[2] + gseries_2*velECI_init_[2];
        
        // (v)-(P): The approximated distance from the geocenter to the launch vehicle position at impact
        r_k2 = Ra / sqrt((e2/(1 - e2))*(Gk/r_k1)*(Gk/r_k1) + 1);
        
        // (v)-(Q): Substituting and repeating
        r_k1_tmp = r_k1;
        r_k1 = r_k2;
    }

    // (v)-(Q): check convergence
    if (abs(r_k1_tmp - r_k2) > 1.0) // then the iterative solution does not converge and an impact point does not meet the accuracy tolerance
        return Vector3d::Zero();      // no solution
    
    // (v)-(R): The difference between the eccentric anomaly at impact and epoch
    double delta_eps = atan2(delta_eps_k_sin, delta_eps_k_cos);
    
    // (v)-(S): The time of flight from epoch to impact
    double time_sec = (delta_eps + eps_sin - eps_k_sin) * sqrt(a_t*a_t*a_t / mu);
    
    // (v)-(T): The geocentric latitude at impact
    double phi_impact_tmp = asin(Gk / r_k2);
    // (v)-(U): The geodetic latitude at impact
    double phi_impact = atan2(tan(phi_impact_tmp), 1.0 - e2);
    // (v)-(V): The East longitude at impact
    double lambda_impact = atan2(Fk, Ek) - omega*time_sec;
    
    // finish: convert to posECEF_IIP_
    // posECEF_IIP_ = coord_utils.CoordUtils([rad2deg(phi_impact), rad2deg(lammda_impact), 0]).ecef_origin
    
    Vector3d posIIP(phi_impact, lambda_impact, 0.0);
    return posIIP * 180.0 / M_PI;

}

// Assembly:
pushq %rbp
pushq %r15
pushq %r14
pushq %rbx
subq $0x118, %rsp            # imm = 0x118
movq %rdx, %r14
movq %rsi, %r15
movq %rdi, %rbx
movabsq $0x3f131da7d7cb8d5b, %rax # imm = 0x3F131DA7D7CB8D5B
movq %rax, 0x78(%rsp)
leaq 0xc8(%rsp), %rax
leaq 0xa8(%rsp), %rdi
movq %rax, (%rdi)
xorl %ecx, %ecx
movq %rcx, 0x8(%rdi)
movl $0x1, %edx
movq %rdx, 0x10(%rdi)
movq %rdx, 0x18(%rdi)
movq %rcx, (%rax)
leaq 0x110(%rsp), %rsi
movq %rcx, (%rsi)
callq 0x16f1a
leaq 0x78(%rsp), %rsi
movq %rax, %rdi
callq 0x16f1a
movq 0xc0(%rsp), %rax
addq 0xb0(%rsp), %rax
cmpq $0x3, %rax
jne 0x26cf1
cmpq $0x1, 0xb8(%rsp)
jne 0x26cf1
movsd (%r15), %xmm1
movsd 0x8(%r15), %xmm2
movsd 0x10(%r15), %xmm0
movsd %xmm1, 0x20(%rsp)
mulsd %xmm1, %xmm1
movsd %xmm2, 0x18(%rsp)
mulsd %xmm2, %xmm2
movsd %xmm0, 0x70(%rsp)
mulsd %xmm0, %xmm0
addsd %xmm2, %xmm0
addsd %xmm1, %xmm0
xorpd %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jb 0x2676a
sqrtsd %xmm0, %xmm4
jmp 0x26773
callq 0x52e0
movapd %xmm0, %xmm4
movsd 0x392d(%rip), %xmm0     # 0x2a0a8
ucomisd %xmm4, %xmm0
ja 0x26bb7
movsd 0xc8(%rsp), %xmm7
movsd 0xd0(%rsp), %xmm0
movsd 0x10(%r15), %xmm1
movsd 0xd8(%rsp), %xmm6
movsd (%r15), %xmm2
movsd 0x8(%r15), %xmm3
movapd %xmm0, %xmm5
mulsd %xmm1, %xmm5
mulsd %xmm7, %xmm1
mulsd %xmm3, %xmm7
mulsd %xmm6, %xmm3
subsd %xmm3, %xmm5
mulsd %xmm2, %xmm6
subsd %xmm1, %xmm6
mulsd %xmm0, %xmm2
subsd %xmm2, %xmm7
addsd (%r14), %xmm5
addsd 0x8(%r14), %xmm6
addsd 0x10(%r14), %xmm7
movsd %xmm5, 0x58(%rsp)
movapd %xmm5, %xmm1
mulsd %xmm5, %xmm1
movsd %xmm6, 0x50(%rsp)
movapd %xmm6, %xmm2
mulsd %xmm6, %xmm2
movsd %xmm7, 0x68(%rsp)
movapd %xmm7, %xmm0
mulsd %xmm7, %xmm0
addsd %xmm2, %xmm0
addsd %xmm1, %xmm0
xorpd %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jb 0x2682c
sqrtsd %xmm0, %xmm0
jmp 0x2683b
movsd %xmm4, (%rsp)
callq 0x52e0
movsd (%rsp), %xmm4
movapd %xmm4, %xmm1
mulsd %xmm0, %xmm1
mulsd %xmm0, %xmm1
divsd 0x3889(%rip), %xmm1     # 0x2a0d8
addsd 0x1591(%rip), %xmm1     # 0x27de8
ucomisd 0x7b1(%rip), %xmm1      # 0x27010
jae 0x26bb7
movsd 0x7a3(%rip), %xmm0      # 0x27010
movsd %xmm1, 0x60(%rsp)
subsd %xmm1, %xmm0
movsd %xmm0, 0xa0(%rsp)
divsd %xmm0, %xmm4
movsd %xmm4, (%rsp)
movsd 0x20(%rsp), %xmm0
mulsd 0x58(%rsp), %xmm0
movsd 0x18(%rsp), %xmm1
mulsd 0x50(%rsp), %xmm1
movsd 0x70(%rsp), %xmm2
mulsd 0x68(%rsp), %xmm2
addsd %xmm1, %xmm2
addsd %xmm0, %xmm2
movsd %xmm2, 0x8(%rsp)
movsd 0x3815(%rip), %xmm0     # 0x2a0d8
mulsd %xmm4, %xmm0
xorpd %xmm2, %xmm2
ucomisd %xmm2, %xmm0
jb 0x268d7
sqrtsd %xmm0, %xmm0
jmp 0x268e0
callq 0x52e0
xorpd %xmm2, %xmm2
movsd 0x8(%rsp), %xmm1
divsd %xmm0, %xmm1
movsd %xmm1, 0x8(%rsp)
movapd %xmm1, %xmm0
mulsd %xmm1, %xmm0
movsd 0x60(%rsp), %xmm4
mulsd %xmm4, %xmm4
addsd %xmm0, %xmm4
ucomisd %xmm2, %xmm4
movsd %xmm4, 0x10(%rsp)
jb 0x2691b
xorps %xmm0, %xmm0
sqrtsd %xmm4, %xmm0
jmp 0x2692a
movapd %xmm4, %xmm0
callq 0x52e0
movsd 0x10(%rsp), %xmm4
movsd (%rsp), %xmm2
movsd 0x3771(%rip), %xmm3     # 0x2a0a8
movsd 0x6d1(%rip), %xmm1      # 0x27010
ucomisd %xmm0, %xmm1
jb 0x2699a
xorpd %xmm5, %xmm5
ucomisd %xmm5, %xmm4
jb 0x26958
xorps %xmm0, %xmm0
sqrtsd %xmm4, %xmm0
jmp 0x26978
movapd %xmm4, %xmm0
callq 0x52e0
xorpd %xmm5, %xmm5
movsd 0x10(%rsp), %xmm4
movsd 0x3735(%rip), %xmm3     # 0x2a0a8
movsd (%rsp), %xmm2
movsd 0x690(%rip), %xmm1      # 0x27010
subsd %xmm0, %xmm1
mulsd %xmm2, %xmm1
addsd 0x38d8(%rip), %xmm1     # 0x2a268
ucomisd %xmm5, %xmm1
jae 0x26bb7
movapd %xmm2, %xmm5
mulsd %xmm2, %xmm5
mulsd %xmm2, %xmm5
divsd 0x372a(%rip), %xmm5     # 0x2a0d8
movl $0x5, %ebp
xorpd %xmm8, %xmm8
xorps %xmm0, %xmm0
sqrtsd %xmm5, %xmm0
movsd %xmm0, 0x98(%rsp)
movsd %xmm5, 0x28(%rsp)
movapd %xmm2, %xmm1
movapd %xmm3, 0x80(%rsp)
subsd %xmm3, %xmm1
divsd %xmm2, %xmm1
movapd %xmm1, %xmm2
mulsd %xmm1, %xmm2
movapd %xmm4, %xmm0
subsd %xmm2, %xmm0
ucomisd %xmm0, %xmm8
ja 0x26bb7
ucomisd %xmm8, %xmm0
movapd %xmm1, 0x30(%rsp)
jb 0x26a11
sqrtsd %xmm0, %xmm0
jmp 0x26a2d
callq 0x52e0
xorpd %xmm8, %xmm8
movapd 0x30(%rsp), %xmm1
movsd 0x28(%rsp), %xmm5
movsd 0x10(%rsp), %xmm4
movsd %xmm0, 0x48(%rsp)
ucomisd %xmm8, %xmm5
movsd 0x98(%rsp), %xmm0
jae 0x26a63
movapd %xmm5, %xmm0
callq 0x52e0
xorpd %xmm8, %xmm8
movapd 0x30(%rsp), %xmm1
movsd 0x28(%rsp), %xmm5
movsd 0x10(%rsp), %xmm4
movapd %xmm1, %xmm9
xorpd 0x5af(%rip), %xmm9      # 0x27020
movsd 0x8(%rsp), %xmm2
movapd %xmm2, %xmm6
movsd 0x48(%rsp), %xmm7
mulsd %xmm7, %xmm6
movsd 0x60(%rsp), %xmm3
mulsd %xmm3, %xmm1
subsd %xmm6, %xmm1
divsd %xmm4, %xmm1
mulsd %xmm2, %xmm9
movapd %xmm7, %xmm6
mulsd %xmm3, %xmm6
subsd %xmm6, %xmm9
divsd %xmm4, %xmm9
movapd %xmm1, %xmm6
subsd %xmm3, %xmm6
divsd 0xa0(%rsp), %xmm6
movapd %xmm2, %xmm10
addsd %xmm9, %xmm10
addsd %xmm7, %xmm10
mulsd %xmm0, %xmm10
movsd 0x68(%rsp), %xmm0
mulsd %xmm10, %xmm0
movapd %xmm6, %xmm2
mulsd 0x70(%rsp), %xmm2
addsd %xmm0, %xmm2
movapd %xmm2, 0x100(%rsp)
divsd 0x80(%rsp), %xmm2
movapd %xmm2, %xmm0
mulsd 0x379e(%rip), %xmm0     # 0x2a2a8
mulsd %xmm2, %xmm0
addsd 0x4fa(%rip), %xmm0      # 0x27010
ucomisd %xmm8, %xmm0
jb 0x26b23
sqrtsd %xmm0, %xmm0
jmp 0x26b7f
movapd %xmm1, 0x30(%rsp)
movapd %xmm9, 0xf0(%rsp)
movapd %xmm6, 0xe0(%rsp)
movsd %xmm10, 0x90(%rsp)
callq 0x52e0
movsd 0x90(%rsp), %xmm10
movapd 0xe0(%rsp), %xmm6
movapd 0xf0(%rsp), %xmm9
xorpd %xmm8, %xmm8
movapd 0x30(%rsp), %xmm1
movsd 0x28(%rsp), %xmm5
movsd 0x10(%rsp), %xmm4
movsd (%rsp), %xmm2
movsd 0x353c(%rip), %xmm3     # 0x2a0c8
divsd %xmm0, %xmm3
decl %ebp
jne 0x269ce
movapd 0x80(%rsp), %xmm0
subsd %xmm3, %xmm0
andpd 0x483(%rip), %xmm0      # 0x27030
ucomisd 0x45b(%rip), %xmm0      # 0x27010
jbe 0x26bd8
xorpd %xmm0, %xmm0
movupd %xmm0, (%rbx)
movq $0x0, 0x10(%rbx)
movq %rbx, %rax
addq $0x118, %rsp            # imm = 0x118
popq %rbx
popq %r14
popq %r15
popq %rbp
retq
movsd %xmm3, 0x30(%rsp)
movsd 0x50(%rsp), %xmm2
mulsd %xmm10, %xmm2
movsd 0x18(%rsp), %xmm0
mulsd %xmm6, %xmm0
addsd %xmm2, %xmm0
movsd %xmm0, 0x18(%rsp)
movsd 0x58(%rsp), %xmm2
mulsd %xmm10, %xmm2
movsd 0x20(%rsp), %xmm0
mulsd %xmm6, %xmm0
addsd %xmm2, %xmm0
movsd %xmm0, 0x20(%rsp)
movapd %xmm9, %xmm0
callq 0x50a0
movsd 0x28(%rsp), %xmm2
movsd 0x8(%rsp), %xmm1
addsd %xmm0, %xmm1
addsd 0x48(%rsp), %xmm1
movsd %xmm1, 0x8(%rsp)
xorpd %xmm0, %xmm0
ucomisd %xmm0, %xmm2
jb 0x26c55
xorps %xmm0, %xmm0
sqrtsd %xmm2, %xmm0
jmp 0x26c5e
movapd %xmm2, %xmm0
callq 0x52e0
mulsd 0x8(%rsp), %xmm0
movsd %xmm0, 0x80(%rsp)
movapd 0x100(%rsp), %xmm0
divsd 0x30(%rsp), %xmm0
callq 0x55d0
callq 0x5240
movsd 0x3612(%rip), %xmm1     # 0x2a2a0
callq 0x50a0
movsd %xmm0, 0x30(%rsp)
movsd 0x18(%rsp), %xmm0
movsd 0x20(%rsp), %xmm1
callq 0x50a0
movsd 0x80(%rsp), %xmm1
mulsd 0x78(%rsp), %xmm1
subsd %xmm1, %xmm0
movsd 0x101b(%rip), %xmm1     # 0x27ce0
movsd 0x30(%rsp), %xmm3
mulsd %xmm1, %xmm3
movsd 0x1011(%rip), %xmm2     # 0x27ce8
divsd %xmm2, %xmm3
movsd %xmm3, (%rbx)
mulsd %xmm1, %xmm0
divsd %xmm2, %xmm0
movsd %xmm0, 0x8(%rbx)
jmp 0x26bbf
leaq 0x24dc(%rip), %rdi      # 0x291d4
leaq 0x255c(%rip), %rsi      # 0x2925b
leaq 0x2582(%rip), %rcx      # 0x29288
movl $0x6a, %edx
callq 0x5250
movq 0xc0(%rsp), %rcx
addq 0xb0(%rsp), %rcx
cmpq $0x3, %rcx
jne 0x26d39
cmpq $0x1, 0xb8(%rsp)
jne 0x26d39
movq %rax, %rdi
callq 0x5650
callq 0x589d