// File: /workspace/github_cpp_makefile_star2_O2/sus304[P]ForRocket/src/dynamics/noniterative_iip.cpp
// Function: forrocket::IIP(Eigen::Matrix<double, 3, 1, 0, 3, 1>&, Eigen::Matrix<double, 3, 1, 0, 3, 1>&)
std::pair<double, Eigen::Vector3d> forrocket::IIP(Eigen::Vector3d& pos_ECI, Eigen::Vector3d& vel_ECI) {
        // arg: Current ECI, Current ECI Vel
        // return IIP LLH [lat, lon, height]
        WGS84 wgs84;

        Eigen::Vector3d ir0;  // initial posint unit vector
        Eigen::Vector3d iv0;  // initial velocity unit vector
        double r0;  // initial position norm
        double v0;  // initial velocity norm
        double gamma0;  // inertia flight path angle

        Eigen::Vector3d ip;
        double rp;  // 地球中心とIIP間距離
        double rp_init;  // 収束計算用

        double vc;  // circular orbital velocity
        double lambda;  // squared speed ratio of current to circular orbital

        double c[3];
        double c_squr[3];

        double sin_phi;
        double cos_phi;
        double phi;
        
        double k[2];

        double lat_IIP_ECI;
        double lon_IIP_ECI;

        double lat_IIP_ECEF;
        double lon_IIP_ECEF;

        double tf1, tf1h, tf1l1, tf1l2;
        double tf2, tf21, tf2h, tf2l;
        double tf;

        Eigen::Vector3d IIP_LLH;

        // =================

        r0 = pos_ECI.norm();
        v0 = vel_ECI.norm();
        ir0 = pos_ECI.normalized();
        iv0 = vel_ECI.normalized();

        vc = std::sqrt(wgs84.GM / r0);
        gamma0 = std::asin(ir0.dot(iv0));  // [rad]
        lambda = std::pow(v0 / vc, 2);

        const double eps = 1e-6;
        const double relaxation = 0.1;
        rp_init = (wgs84.a + wgs84.b) * 0.5;
        // rp_init = wgs84.b;
        unsigned int counter = 0;
        while (std::abs(rp - rp_init) > eps) {
            rp = rp_init;

            c[0] = -std::tan(gamma0);
            c[1] = 1.0 - 1.0 / (lambda * std::pow(std::cos(gamma0), 2));
            c[2] = r0 / rp - 1.0 / (lambda * std::pow(std::cos(gamma0), 2));  // rp
            for (int i=0; i < 3; i++) {
                c_squr[i] = c[i] * c[i];
            }

            sin_phi = (c[0] * c[2]
                    + std::sqrt(c_squr[0] * c_squr[2] - (c_squr[0] + c_squr[1]) * (c_squr[2] - c_squr[1])))
                    / (c_squr[0] + c_squr[1]);
            cos_phi = std::sqrt(1.0 - sin_phi);
            phi = std::asin(sin_phi);  // [rad]

            k[0] = std::cos(gamma0 + phi) / std::cos(gamma0);
            k[1] = sin_phi / std::cos(gamma0);

            ip = k[0] * ir0 + k[1] * iv0;

            lat_IIP_ECI = std::asin(ip[2]);
            lon_IIP_ECI = std::atan2(ip[1], ip[0]);

            tf1h = std::tan(gamma0) * (1.0 - cos_phi) + (1.0 - lambda) * sin_phi;
            tf1l1 = (1.0 - cos_phi) / (lambda * std::pow(std::cos(gamma0), 2));
            tf1l2 = std::cos(gamma0 + phi) / std::cos(gamma0);
            tf1 = tf1h / ((2.0 - lambda) * (tf1l1 + tf1l2));

            tf21 = 2.0 * std::cos(gamma0) / (lambda * std::pow(2.0 / lambda - 1.0, 1.5));
            tf2h = std::sqrt(2.0 / lambda - 1.0);
            tf2l = std::cos(gamma0) * (1.0 / std::tan(phi * 0.5)) - std::sin(gamma0);
            tf2 = tf21 * std::atan2(tf2h , tf2l);

            tf = r0 / (v0 * std::cos(gamma0)) * (tf1 + tf2);

            lat_IIP_ECEF = lat_IIP_ECI;
            lon_IIP_ECEF = lon_IIP_ECI - wgs84.omega * tf;

            rp = wgs84.a * std::sqrt(1.0 - std::pow(wgs84.e * std::sin(lat_IIP_ECI), 2));
            
            counter++;
            double delta = rp - rp_init;
            if (rp > rp_init) {
                rp_init = rp_init + delta * relaxation;
            }
            else {
                rp_init = rp_init - delta * relaxation;
            }

        }
            c[2] = r0 / rp - 1.0 / (lambda * std::pow(std::cos(gamma0), 2));  // rp

        IIP_LLH[0] = rad2deg(lat_IIP_ECEF);
        IIP_LLH[1] = rad2deg(lon_IIP_ECEF);
        IIP_LLH[2] = 0.0;
        
        return std::make_pair(tf, IIP_LLH);
    }

// Assembly:
pushq %rbp
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0x278, %rsp            # imm = 0x278
movq %rdx, %rbx
movq %rsi, %r14
movq %rdi, 0xa8(%rsp)
movq %rsi, %rdi
callq 0x1c7d2
movaps %xmm0, 0x80(%rsp)
movq %rbx, %rdi
callq 0x1c7d2
movaps %xmm0, 0x10(%rsp)
leaq 0x218(%rsp), %r15
movq %r15, %rdi
movq %r14, %rsi
callq 0x21968
leaq 0x1b0(%rsp), %rdi
movq %r15, %rsi
callq 0x149bd
leaq 0x218(%rsp), %r14
movq %r14, %rdi
movq %rbx, %rsi
callq 0x21968
leaq 0x198(%rsp), %rdi
movq %r14, %rsi
callq 0x149bd
movsd 0x1344e(%rip), %xmm0    # 0x34778
divsd 0x80(%rsp), %xmm0
xorpd %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jb 0x21343
sqrtsd %xmm0, %xmm0
jmp 0x21348
callq 0x52a0
movsd %xmm0, 0x50(%rsp)
leaq 0x1b0(%rsp), %rdi
leaq 0x198(%rsp), %rsi
callq 0x21a28
callq 0x5550
movsd %xmm0, (%rsp)
movapd 0x10(%rsp), %xmm3
movapd %xmm3, %xmm0
divsd 0x50(%rsp), %xmm0
mulsd %xmm0, %xmm0
leaq 0x98(%rsp), %r12
movsd 0x105af(%rip), %xmm1    # 0x31940
subsd %xmm0, %xmm1
movsd %xmm1, 0xb8(%rsp)
movsd 0x13daa(%rip), %xmm1    # 0x35150
movapd %xmm1, %xmm2
subsd %xmm0, %xmm2
divsd %xmm0, %xmm1
addsd 0x10f16(%rip), %xmm1    # 0x322d0
movapd %xmm0, 0x130(%rsp)
unpcklpd %xmm0, %xmm0            # xmm0 = xmm0[0,0]
movapd %xmm0, 0x110(%rsp)
unpcklpd %xmm3, %xmm2            # xmm2 = xmm2[0],xmm3[0]
movapd %xmm2, 0x120(%rsp)
movapd 0x13d8b(%rip), %xmm2    # 0x35170
leaq 0x1f0(%rsp), %r13
leaq 0x90(%rsp), %rbp
leaq 0x1c8(%rsp), %rbx
leaq 0xf8(%rsp), %r14
leaq 0x218(%rsp), %r15
movsd %xmm1, 0x38(%rsp)
sqrtsd %xmm1, %xmm0
movsd %xmm0, 0xb0(%rsp)
movapd %xmm2, 0xd0(%rsp)
unpckhpd %xmm2, %xmm2            # xmm2 = xmm2[1,1]
subsd %xmm2, %xmm0
andpd 0x114c7(%rip), %xmm0    # 0x32900
ucomisd 0x13d17(%rip), %xmm0    # 0x35158
jbe 0x21905
movsd (%rsp), %xmm0
movapd %xmm2, 0x50(%rsp)
callq 0x5200
xorps 0x104d2(%rip), %xmm0    # 0x31930
movaps %xmm0, 0x20(%rsp)
movlps %xmm0, 0x180(%rsp)
movsd (%rsp), %xmm0
callq 0x5610
movaps %xmm0, 0x40(%rsp)
movapd 0x80(%rsp), %xmm0
divsd 0x50(%rsp), %xmm0
movapd 0x114bf(%rip), %xmm1    # 0x32950
unpcklpd %xmm0, %xmm1            # xmm1 = xmm1[0],xmm0[0]
movapd %xmm1, 0x10(%rsp)
movsd (%rsp), %xmm0
callq 0x5610
movapd 0x40(%rsp), %xmm1
unpcklpd %xmm0, %xmm1            # xmm1 = xmm1[0],xmm0[0]
mulpd %xmm1, %xmm1
mulpd 0x110(%rsp), %xmm1
movapd 0x13cbc(%rip), %xmm0    # 0x35180
divpd %xmm1, %xmm0
addpd 0x10(%rsp), %xmm0
movapd %xmm0, 0x10(%rsp)
movupd %xmm0, 0x188(%rsp)
xorl %eax, %eax
cmpq $0x3, %rax
je 0x21500
movsd 0x180(%rsp,%rax,8), %xmm0
mulsd %xmm0, %xmm0
movsd %xmm0, 0xe0(%rsp,%rax,8)
incq %rax
jmp 0x214df
movsd 0xf0(%rsp), %xmm1
movsd 0xe0(%rsp), %xmm0
movsd 0xe8(%rsp), %xmm2
movapd %xmm0, %xmm3
addsd %xmm2, %xmm3
mulsd %xmm1, %xmm0
subsd %xmm2, %xmm1
mulsd %xmm3, %xmm1
subsd %xmm1, %xmm0
ucomisd 0x113b5(%rip), %xmm0    # 0x328f0
jb 0x21543
sqrtsd %xmm0, %xmm0
jmp 0x21554
movsd %xmm3, 0x40(%rsp)
callq 0x52a0
movsd 0x40(%rsp), %xmm3
movapd 0x10(%rsp), %xmm1
unpckhpd %xmm1, %xmm1            # xmm1 = xmm1[1,1]
mulsd 0x20(%rsp), %xmm1
addsd %xmm0, %xmm1
divsd %xmm3, %xmm1
movsd 0x103cc(%rip), %xmm0    # 0x31940
movapd %xmm1, 0x10(%rsp)
subsd %xmm1, %xmm0
ucomisd 0x1136a(%rip), %xmm0    # 0x328f0
jb 0x2158e
sqrtsd %xmm0, %xmm0
jmp 0x21593
callq 0x52a0
movsd %xmm0, 0x68(%rsp)
movapd 0x10(%rsp), %xmm0
callq 0x5550
movsd (%rsp), %xmm1
movsd %xmm0, 0x8(%rsp)
addsd %xmm0, %xmm1
movsd %xmm1, 0x30(%rsp)
movapd %xmm1, %xmm0
callq 0x5610
movsd %xmm0, 0x20(%rsp)
movsd (%rsp), %xmm0
callq 0x5610
movsd 0x20(%rsp), %xmm1
divsd %xmm0, %xmm1
movsd %xmm1, 0x90(%rsp)
movsd (%rsp), %xmm0
callq 0x5610
movapd 0x10(%rsp), %xmm1
divsd %xmm0, %xmm1
movsd %xmm1, 0x98(%rsp)
movq %r13, %rdi
movq %rbp, %rsi
leaq 0x1b0(%rsp), %rdx
callq 0x1c651
movq %rbx, %rdi
movq %r12, %rsi
leaq 0x198(%rsp), %rdx
callq 0x1c651
movsd 0x200(%rsp), %xmm0
movsd %xmm0, 0x230(%rsp)
movq 0x208(%rsp), %rax
movq %rax, 0x238(%rsp)
movsd 0x1d8(%rsp), %xmm0
movsd %xmm0, 0x258(%rsp)
movq 0x1e0(%rsp), %rax
movq %rax, 0x260(%rsp)
movq %r14, %rdi
movq %r15, %rsi
callq 0x21aab
movsd 0x108(%rsp), %xmm0
callq 0x5550
movsd %xmm0, 0x40(%rsp)
movsd 0xf8(%rsp), %xmm1
movsd 0x100(%rsp), %xmm0
callq 0x5070
movaps %xmm0, 0x170(%rsp)
movsd (%rsp), %xmm0
callq 0x5200
movaps %xmm0, 0x20(%rsp)
movsd (%rsp), %xmm0
callq 0x5610
movaps %xmm0, 0x160(%rsp)
movsd 0x30(%rsp), %xmm0
callq 0x5610
movaps %xmm0, 0x150(%rsp)
movsd (%rsp), %xmm0
callq 0x5610
movaps %xmm0, 0x140(%rsp)
movsd (%rsp), %xmm0
callq 0x5610
movsd %xmm0, 0x30(%rsp)
movsd 0x38(%rsp), %xmm0
movsd 0x13a52(%rip), %xmm1    # 0x35160
callq 0x55a0
movsd %xmm0, 0xc8(%rsp)
movsd 0x38(%rsp), %xmm0
ucomisd 0x111c6(%rip), %xmm0    # 0x328f0
movsd 0xb0(%rsp), %xmm0
jae 0x21740
movsd 0x38(%rsp), %xmm0
callq 0x52a0
movsd %xmm0, 0x70(%rsp)
movsd (%rsp), %xmm0
callq 0x5610
movsd %xmm0, 0xc0(%rsp)
movsd 0x8(%rsp), %xmm0
mulsd 0x101e1(%rip), %xmm0    # 0x31948
callq 0x5200
movsd 0x101cc(%rip), %xmm1    # 0x31940
divsd %xmm0, %xmm1
movsd %xmm1, 0x8(%rsp)
movsd (%rsp), %xmm0
callq 0x50e0
movsd 0x8(%rsp), %xmm1
mulsd 0xc0(%rsp), %xmm1
subsd %xmm0, %xmm1
movsd 0x70(%rsp), %xmm0
callq 0x5070
movsd %xmm0, 0x8(%rsp)
movsd (%rsp), %xmm0
callq 0x5610
movaps %xmm0, 0x70(%rsp)
movsd 0x40(%rsp), %xmm0
callq 0x50e0
movapd %xmm0, %xmm1
mulsd 0x13996(%rip), %xmm1    # 0x35168
mulsd %xmm1, %xmm1
movsd 0x10162(%rip), %xmm0    # 0x31940
subsd %xmm1, %xmm0
ucomisd 0x11106(%rip), %xmm0    # 0x328f0
jb 0x217f2
sqrtsd %xmm0, %xmm0
jmp 0x217f7
callq 0x52a0
movapd 0x20(%rsp), %xmm4
movsd 0x1013b(%rip), %xmm1    # 0x31940
subsd 0x68(%rsp), %xmm1
movapd 0x10(%rsp), %xmm2
mulsd 0xb8(%rsp), %xmm2
mulsd %xmm1, %xmm4
addsd %xmm2, %xmm4
unpcklpd 0x150(%rsp), %xmm1      # xmm1 = xmm1[0],mem[0]
movapd 0x160(%rsp), %xmm2
mulsd %xmm2, %xmm2
movapd 0x130(%rsp), %xmm3
mulsd %xmm3, %xmm2
unpcklpd 0x140(%rsp), %xmm2      # xmm2 = xmm2[0],mem[0]
divpd %xmm2, %xmm1
movapd %xmm1, %xmm2
unpckhpd %xmm1, %xmm2            # xmm2 = xmm2[1],xmm1[1]
addpd %xmm1, %xmm2
movsd 0x30(%rsp), %xmm1
addsd %xmm1, %xmm1
movsd 0xc8(%rsp), %xmm5
mulsd %xmm3, %xmm5
divsd %xmm5, %xmm1
mulsd 0x8(%rsp), %xmm1
unpcklpd 0x70(%rsp), %xmm2       # xmm2 = xmm2[0],mem[0]
mulpd 0x120(%rsp), %xmm2
unpcklpd 0x80(%rsp), %xmm4       # xmm4 = xmm4[0],mem[0]
divpd %xmm2, %xmm4
addsd %xmm4, %xmm1
unpckhpd %xmm4, %xmm4            # xmm4 = xmm4[1,1]
mulsd %xmm1, %xmm4
mulsd 0x12ec1(%rip), %xmm0    # 0x34770
movapd %xmm0, %xmm1
movapd 0x50(%rsp), %xmm3
subsd %xmm3, %xmm1
cmpltsd %xmm0, %xmm3
movapd %xmm3, %xmm2
andpd %xmm1, %xmm2
xorpd 0x1005e(%rip), %xmm1    # 0x31930
andnpd %xmm1, %xmm3
orpd %xmm2, %xmm3
movapd %xmm4, %xmm1
unpcklpd %xmm3, %xmm1            # xmm1 = xmm1[0],xmm3[0]
movapd 0xd0(%rsp), %xmm2
movlpd 0x170(%rsp), %xmm2      # xmm2 = mem[0],xmm2[1]
mulpd 0x13894(%rip), %xmm1    # 0x35190
addpd %xmm1, %xmm2
jmp 0x21420
movsd (%rsp), %xmm0
movaps %xmm4, 0x20(%rsp)
callq 0x5610
movsd 0x40(%rsp), %xmm0
callq 0x5a7d
movsd %xmm0, (%rsp)
movaps 0xd0(%rsp), %xmm0
callq 0x5a7d
movq 0xa8(%rsp), %rax
movaps 0x20(%rsp), %xmm1
movsd %xmm1, (%rax)
movsd (%rsp), %xmm1
movsd %xmm1, 0x8(%rax)
movsd %xmm0, 0x10(%rax)
andq $0x0, 0x18(%rax)
addq $0x278, %rsp            # imm = 0x278
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq