// File: 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
movq %rsp, %rbp
subq $0x320, %rsp            # imm = 0x320
movq %rdx, -0x2b0(%rbp)
movq %rsi, -0x2a8(%rbp)
movq %rdi, -0x2a0(%rbp)
movq %rdi, %rax
movq %rax, -0x298(%rbp)
movq %rdi, -0x8(%rbp)
movq %rsi, -0x10(%rbp)
movq %rdx, -0x18(%rbp)
movl $0x5, -0x1c(%rbp)
movabsq $0x415854a640000000, %rax # imm = 0x415854A640000000
movq %rax, -0x28(%rbp)
movabsq $0x3f6b775a84f3e128, %rax # imm = 0x3F6B775A84F3E128
movq %rax, -0x30(%rbp)
movsd -0x28(%rbp), %xmm0
movsd -0x30(%rbp), %xmm2
movsd 0x5385(%rip), %xmm1     # 0x52040
subsd %xmm2, %xmm1
mulsd %xmm1, %xmm0
movsd %xmm0, -0x38(%rbp)
movsd -0x28(%rbp), %xmm1
movsd -0x38(%rbp), %xmm2
mulsd %xmm2, %xmm2
movaps %xmm1, %xmm0
mulsd %xmm0, %xmm0
subsd %xmm2, %xmm0
divsd %xmm1, %xmm0
divsd %xmm1, %xmm0
movsd %xmm0, -0x40(%rbp)
movabsq $0x42f6a8665bda5400, %rax # imm = 0x42F6A8665BDA5400
movq %rax, -0x48(%rbp)
movabsq $0x3f131da7d7cb8d5b, %rax # imm = 0x3F131DA7D7CB8D5B
movq %rax, -0x50(%rbp)
leaq -0x68(%rbp), %rdi
movq %rdi, -0x290(%rbp)
callq 0x1d670
movq -0x290(%rbp), %rsi
movq $0x0, -0x90(%rbp)
leaq -0x88(%rbp), %rdi
movq %rdi, -0x288(%rbp)
leaq -0x90(%rbp), %rdx
callq 0x1e1e0
movq -0x288(%rbp), %rdi
movq $0x0, -0x98(%rbp)
leaq -0x98(%rbp), %rsi
callq 0x1e220
movq %rax, -0x280(%rbp)
jmp 0x4cd6d
movq -0x280(%rbp), %rdi
leaq -0x50(%rbp), %rsi
callq 0x1e220
jmp 0x4cd7f
leaq -0x88(%rbp), %rdi
callq 0x1e390
movq -0x2a8(%rbp), %rsi
movsd -0x38(%rbp), %xmm0
movsd %xmm0, -0xb0(%rbp)
leaq -0xc8(%rbp), %rdi
callq 0x1f0e0
leaq -0xc8(%rbp), %rdi
callq 0x205d0
movsd %xmm0, -0xd0(%rbp)
movsd -0xd0(%rbp), %xmm1
movsd -0xb0(%rbp), %xmm0
ucomisd %xmm1, %xmm0
jbe 0x4ce1c
leaq -0xe0(%rbp), %rdi
callq 0x4e330
movq -0x2a0(%rbp), %rdi
leaq -0xe0(%rbp), %rsi
callq 0x4e360
jmp 0x4d615
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0xa0(%rbp)
movl %eax, -0xa4(%rbp)
leaq -0x88(%rbp), %rdi
callq 0x1e390
jmp 0x4d625
movq -0x2a8(%rbp), %rdx
leaq -0x128(%rbp), %rdi
leaq -0x68(%rbp), %rsi
callq 0x49c30
movq -0x2b0(%rbp), %rsi
leaq -0x110(%rbp), %rdi
leaq -0x128(%rbp), %rdx
callq 0x20d10
leaq -0xf8(%rbp), %rdi
leaq -0x110(%rbp), %rsi
callq 0x37ed0
leaq -0xf8(%rbp), %rdi
callq 0x205d0
movsd %xmm0, -0x130(%rbp)
movsd -0xd0(%rbp), %xmm0
mulsd -0x130(%rbp), %xmm0
mulsd -0x130(%rbp), %xmm0
divsd -0x48(%rbp), %xmm0
movsd 0x51a7(%rip), %xmm1     # 0x52040
subsd %xmm1, %xmm0
movsd %xmm0, -0x138(%rbp)
movsd -0x138(%rbp), %xmm0
movsd 0x518b(%rip), %xmm1     # 0x52040
ucomisd %xmm1, %xmm0
jb 0x4cedf
leaq -0x148(%rbp), %rdi
callq 0x4e330
movq -0x2a0(%rbp), %rdi
leaq -0x148(%rbp), %rsi
callq 0x4e360
jmp 0x4d615
movsd -0xd0(%rbp), %xmm0
movsd 0x5151(%rip), %xmm1     # 0x52040
subsd -0x138(%rbp), %xmm1
divsd %xmm1, %xmm0
movsd %xmm0, -0x150(%rbp)
leaq -0xc8(%rbp), %rdi
leaq -0xf8(%rbp), %rsi
callq 0x4e2c0
movsd %xmm0, -0x2b8(%rbp)
movsd -0x48(%rbp), %xmm0
mulsd -0x150(%rbp), %xmm0
callq 0x6390
movaps %xmm0, %xmm1
movsd -0x2b8(%rbp), %xmm0
divsd %xmm1, %xmm0
movsd %xmm0, -0x158(%rbp)
movsd -0x138(%rbp), %xmm0
movsd -0x138(%rbp), %xmm2
movsd -0x158(%rbp), %xmm1
mulsd -0x158(%rbp), %xmm1
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, -0x160(%rbp)
movsd -0x160(%rbp), %xmm0
callq 0x6390
movaps %xmm0, %xmm1
movsd 0x50b1(%rip), %xmm0     # 0x52040
ucomisd %xmm1, %xmm0
jb 0x4d01e
movsd -0x150(%rbp), %xmm0
movsd %xmm0, -0x2c0(%rbp)
movsd -0x160(%rbp), %xmm0
callq 0x6390
movaps %xmm0, %xmm1
movsd -0x2c0(%rbp), %xmm0
movsd 0x5077(%rip), %xmm2     # 0x52040
subsd %xmm1, %xmm2
movsd -0x28(%rbp), %xmm1
movq %xmm1, %rax
movabsq $-0x8000000000000000, %rcx # imm = 0x8000000000000000
xorq %rcx, %rax
movq %rax, %xmm1
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
xorps %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jb 0x4d01e
leaq -0x170(%rbp), %rdi
callq 0x4e330
movq -0x2a0(%rbp), %rdi
leaq -0x170(%rbp), %rsi
callq 0x4e360
jmp 0x4d615
movl $0x0, -0x1cc(%rbp)
movl -0x1cc(%rbp), %eax
cmpl -0x1c(%rbp), %eax
jge 0x4d435
movsd -0x150(%rbp), %xmm0
subsd -0xb0(%rbp), %xmm0
divsd -0x150(%rbp), %xmm0
movsd %xmm0, -0x178(%rbp)
movsd -0x160(%rbp), %xmm0
movsd -0x178(%rbp), %xmm1
movsd -0x178(%rbp), %xmm2
movq %xmm1, %rax
movabsq $-0x8000000000000000, %rcx # imm = 0x8000000000000000
xorq %rcx, %rax
movq %rax, %xmm1
mulsd %xmm2, %xmm1
addsd %xmm0, %xmm1
xorps %xmm0, %xmm0
ucomisd %xmm1, %xmm0
jbe 0x4d0bb
leaq -0x1e0(%rbp), %rdi
callq 0x4e330
movq -0x2a0(%rbp), %rdi
leaq -0x1e0(%rbp), %rsi
callq 0x4e360
jmp 0x4d615
movsd -0x160(%rbp), %xmm1
movsd -0x178(%rbp), %xmm0
movsd -0x178(%rbp), %xmm2
movq %xmm0, %rax
movabsq $-0x8000000000000000, %rcx # imm = 0x8000000000000000
xorq %rcx, %rax
movq %rax, %xmm0
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
callq 0x6390
movq %xmm0, %rax
movabsq $-0x8000000000000000, %rcx # imm = 0x8000000000000000
xorq %rcx, %rax
movq %rax, %xmm0
movsd %xmm0, -0x180(%rbp)
movsd -0x178(%rbp), %xmm0
movsd -0x138(%rbp), %xmm2
movsd -0x180(%rbp), %xmm1
mulsd -0x158(%rbp), %xmm1
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
divsd -0x160(%rbp), %xmm0
movsd %xmm0, -0x188(%rbp)
movsd -0x180(%rbp), %xmm0
movsd -0x138(%rbp), %xmm2
movsd -0x178(%rbp), %xmm1
mulsd -0x158(%rbp), %xmm1
movq %xmm1, %rax
movabsq $-0x8000000000000000, %rcx # imm = 0x8000000000000000
xorq %rcx, %rax
movq %rax, %xmm1
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
divsd -0x160(%rbp), %xmm0
movsd %xmm0, -0x190(%rbp)
movsd -0x188(%rbp), %xmm0
subsd -0x138(%rbp), %xmm0
movsd 0x4e8b(%rip), %xmm1     # 0x52040
subsd -0x138(%rbp), %xmm1
divsd %xmm1, %xmm0
movsd %xmm0, -0x198(%rbp)
movsd -0x190(%rbp), %xmm0
addsd -0x158(%rbp), %xmm0
subsd -0x180(%rbp), %xmm0
movsd %xmm0, -0x318(%rbp)
movsd -0x150(%rbp), %xmm0
mulsd -0x150(%rbp), %xmm0
mulsd -0x150(%rbp), %xmm0
divsd -0x48(%rbp), %xmm0
callq 0x6390
movaps %xmm0, %xmm1
movsd -0x318(%rbp), %xmm0
mulsd %xmm1, %xmm0
movsd %xmm0, -0x1a0(%rbp)
movsd -0x198(%rbp), %xmm0
movsd %xmm0, -0x308(%rbp)
leaq -0xc8(%rbp), %rdi
xorl %eax, %eax
movl %eax, %esi
callq 0x1de40
movsd (%rax), %xmm0
movsd %xmm0, -0x300(%rbp)
movsd -0x1a0(%rbp), %xmm0
movsd %xmm0, -0x310(%rbp)
leaq -0xf8(%rbp), %rdi
xorl %eax, %eax
movl %eax, %esi
callq 0x1de40
movsd -0x310(%rbp), %xmm1
movsd -0x308(%rbp), %xmm0
movsd -0x300(%rbp), %xmm2
mulsd (%rax), %xmm1
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, -0x1a8(%rbp)
movsd -0x198(%rbp), %xmm0
movsd %xmm0, -0x2f0(%rbp)
leaq -0xc8(%rbp), %rdi
movl $0x1, %esi
callq 0x1de40
movsd (%rax), %xmm0
movsd %xmm0, -0x2e8(%rbp)
movsd -0x1a0(%rbp), %xmm0
movsd %xmm0, -0x2f8(%rbp)
leaq -0xf8(%rbp), %rdi
movl $0x1, %esi
callq 0x1de40
movsd -0x2f8(%rbp), %xmm1
movsd -0x2f0(%rbp), %xmm0
movsd -0x2e8(%rbp), %xmm2
mulsd (%rax), %xmm1
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, -0x1b0(%rbp)
movsd -0x198(%rbp), %xmm0
movsd %xmm0, -0x2d8(%rbp)
leaq -0xc8(%rbp), %rdi
movl $0x2, %esi
callq 0x1de40
movsd (%rax), %xmm0
movsd %xmm0, -0x2d0(%rbp)
movsd -0x1a0(%rbp), %xmm0
movsd %xmm0, -0x2e0(%rbp)
leaq -0xf8(%rbp), %rdi
movl $0x2, %esi
callq 0x1de40
movsd -0x2e0(%rbp), %xmm1
movsd -0x2d8(%rbp), %xmm0
movsd -0x2d0(%rbp), %xmm2
mulsd (%rax), %xmm1
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, -0x1b8(%rbp)
movsd -0x28(%rbp), %xmm0
movsd %xmm0, -0x2c8(%rbp)
movsd -0x40(%rbp), %xmm0
movsd 0x4c98(%rip), %xmm1     # 0x52040
subsd -0x40(%rbp), %xmm1
divsd %xmm1, %xmm0
movsd -0x1b8(%rbp), %xmm1
divsd -0xb0(%rbp), %xmm1
mulsd %xmm1, %xmm0
movsd -0x1b8(%rbp), %xmm1
divsd -0xb0(%rbp), %xmm1
mulsd %xmm1, %xmm0
movsd 0x4c5f(%rip), %xmm1     # 0x52040
addsd %xmm1, %xmm0
callq 0x6390
movaps %xmm0, %xmm1
movsd -0x2c8(%rbp), %xmm0
divsd %xmm1, %xmm0
movsd %xmm0, -0x1c0(%rbp)
movsd -0xb0(%rbp), %xmm0
movsd %xmm0, -0x1c8(%rbp)
movsd -0x1c0(%rbp), %xmm0
movsd %xmm0, -0xb0(%rbp)
movl -0x1cc(%rbp), %eax
addl $0x1, %eax
movl %eax, -0x1cc(%rbp)
jmp 0x4d028
movsd -0x1c8(%rbp), %xmm0
subsd -0x1c0(%rbp), %xmm0
callq 0x8450
movsd 0x4bee(%rip), %xmm1     # 0x52040
ucomisd %xmm1, %xmm0
jbe 0x4d47c
leaq -0x1f0(%rbp), %rdi
callq 0x4e330
movq -0x2a0(%rbp), %rdi
leaq -0x1f0(%rbp), %rsi
callq 0x4e360
jmp 0x4d615
movsd -0x190(%rbp), %xmm0
movsd -0x188(%rbp), %xmm1
callq 0x60c0
movsd %xmm0, -0x1f8(%rbp)
movsd -0x1f8(%rbp), %xmm0
addsd -0x158(%rbp), %xmm0
subsd -0x180(%rbp), %xmm0
movsd %xmm0, -0x320(%rbp)
movsd -0x150(%rbp), %xmm0
mulsd -0x150(%rbp), %xmm0
mulsd -0x150(%rbp), %xmm0
divsd -0x48(%rbp), %xmm0
callq 0x6390
movaps %xmm0, %xmm1
movsd -0x320(%rbp), %xmm0
mulsd %xmm1, %xmm0
movsd %xmm0, -0x200(%rbp)
movsd -0x1b8(%rbp), %xmm0
divsd -0x1c0(%rbp), %xmm0
callq 0x6710
movsd %xmm0, -0x208(%rbp)
movsd -0x208(%rbp), %xmm0
callq 0x62b0
movsd 0x4b1c(%rip), %xmm1     # 0x52040
subsd -0x40(%rbp), %xmm1
callq 0x60c0
movsd %xmm0, -0x210(%rbp)
movsd -0x1b0(%rbp), %xmm0
movsd -0x1a8(%rbp), %xmm1
callq 0x60c0
movaps %xmm0, %xmm1
movsd -0x50(%rbp), %xmm0
movsd -0x200(%rbp), %xmm2
movq %xmm0, %rax
movabsq $-0x8000000000000000, %rcx # imm = 0x8000000000000000
xorq %rcx, %rax
movq %rax, %xmm0
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, -0x218(%rbp)
xorps %xmm0, %xmm0
movsd %xmm0, -0x238(%rbp)
leaq -0x230(%rbp), %rdi
leaq -0x210(%rbp), %rsi
leaq -0x218(%rbp), %rdx
leaq -0x238(%rbp), %rcx
callq 0x4a220
movsd 0x588a(%rip), %xmm0     # 0x52e40
movsd %xmm0, -0x270(%rbp)
leaq -0x268(%rbp), %rdi
leaq -0x230(%rbp), %rsi
leaq -0x270(%rbp), %rdx
callq 0x35e60
movsd 0x5858(%rip), %xmm0     # 0x52e38
movsd %xmm0, -0x278(%rbp)
leaq -0x258(%rbp), %rdi
leaq -0x268(%rbp), %rsi
leaq -0x278(%rbp), %rdx
callq 0x4e3e0
movq -0x2a0(%rbp), %rdi
leaq -0x258(%rbp), %rsi
callq 0x4e440
movq -0x298(%rbp), %rax
addq $0x320, %rsp            # imm = 0x320
popq %rbp
retq
movq -0xa0(%rbp), %rdi
callq 0x67a0
nopw %cs:(%rax,%rax)
nopl (%rax,%rax)