// File: 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
movq %rsp, %rbp
subq $0x390, %rsp            # imm = 0x390
movq %rdi, -0x2d0(%rbp)
movq %rdi, %rax
movq %rax, -0x2c8(%rbp)
movq %rdi, -0x8(%rbp)
movq %rsi, -0x10(%rbp)
movq %rdx, -0x18(%rbp)
leaq -0x58(%rbp), %rdi
callq 0x2ee40
leaq -0x70(%rbp), %rdi
callq 0x2f9d0
leaq -0x88(%rbp), %rdi
callq 0x2f9d0
leaq -0xb8(%rbp), %rdi
callq 0x2f9d0
leaq -0x1c0(%rbp), %rdi
callq 0x2f9d0
movq -0x10(%rbp), %rdi
callq 0x55fb0
movsd %xmm0, -0x90(%rbp)
movq -0x18(%rbp), %rdi
callq 0x55fb0
movsd %xmm0, -0x98(%rbp)
movq -0x10(%rbp), %rsi
leaq -0x1d8(%rbp), %rdi
callq 0x704d0
leaq -0x70(%rbp), %rdi
leaq -0x1d8(%rbp), %rsi
callq 0x37a90
movq -0x18(%rbp), %rsi
leaq -0x1f0(%rbp), %rdi
callq 0x704d0
leaq -0x88(%rbp), %rdi
leaq -0x1f0(%rbp), %rsi
callq 0x37a90
movsd -0x40(%rbp), %xmm0
divsd -0x90(%rbp), %xmm0
callq 0x6370
movsd %xmm0, -0xd0(%rbp)
leaq -0x70(%rbp), %rdi
leaq -0x88(%rbp), %rsi
callq 0x705d0
callq 0x66e0
movsd %xmm0, -0xa0(%rbp)
movsd -0x98(%rbp), %xmm0
divsd -0xd0(%rbp), %xmm0
movl $0x2, %edi
callq 0x283f0
movsd %xmm0, -0xd8(%rbp)
movsd 0x3503c(%rip), %xmm0    # 0xa4bc0
movsd %xmm0, -0x1f8(%rbp)
movsd 0x35024(%rip), %xmm0    # 0xa4bb8
movsd %xmm0, -0x200(%rbp)
movsd -0x58(%rbp), %xmm0
addsd -0x30(%rbp), %xmm0
movsd 0x25ab2(%rip), %xmm1    # 0x95660
mulsd %xmm1, %xmm0
movsd %xmm0, -0xc8(%rbp)
movl $0x0, -0x204(%rbp)
movsd -0xc0(%rbp), %xmm0
subsd -0xc8(%rbp), %xmm0
callq 0x373a0
movsd 0x34fdf(%rip), %xmm1    # 0xa4bc0
ucomisd %xmm1, %xmm0
jbe 0x703ae
movsd -0xc8(%rbp), %xmm0
movsd %xmm0, -0xc0(%rbp)
movsd -0xa0(%rbp), %xmm0
callq 0x6280
movq %xmm0, %rax
movabsq $-0x8000000000000000, %rcx # imm = 0x8000000000000000
xorq %rcx, %rax
movq %rax, %xmm0
movsd %xmm0, -0xf0(%rbp)
movsd -0xd8(%rbp), %xmm0
movsd %xmm0, -0x2e8(%rbp)
movsd -0xa0(%rbp), %xmm0
callq 0x67f0
movl $0x2, %edi
callq 0x283f0
movaps %xmm0, %xmm1
movsd -0x2e8(%rbp), %xmm0
mulsd %xmm1, %xmm0
movsd 0x259d3(%rip), %xmm1    # 0x95638
divsd %xmm0, %xmm1
movsd 0x259c7(%rip), %xmm0    # 0x95638
subsd %xmm1, %xmm0
movsd %xmm0, -0xe8(%rbp)
movsd -0x90(%rbp), %xmm0
divsd -0xc0(%rbp), %xmm0
movsd %xmm0, -0x2d8(%rbp)
movsd -0xd8(%rbp), %xmm0
movsd %xmm0, -0x2e0(%rbp)
movsd -0xa0(%rbp), %xmm0
callq 0x67f0
movl $0x2, %edi
callq 0x283f0
movsd -0x2e0(%rbp), %xmm2
movaps %xmm0, %xmm1
movsd -0x2d8(%rbp), %xmm0
mulsd %xmm1, %xmm2
movsd 0x2595d(%rip), %xmm1    # 0x95638
divsd %xmm2, %xmm1
subsd %xmm1, %xmm0
movsd %xmm0, -0xe0(%rbp)
movl $0x0, -0x208(%rbp)
cmpl $0x3, -0x208(%rbp)
jge 0x6fd3f
movslq -0x208(%rbp), %rax
movsd -0xf0(%rbp,%rax,8), %xmm0
movslq -0x208(%rbp), %rax
mulsd -0xf0(%rbp,%rax,8), %xmm0
movslq -0x208(%rbp), %rax
movsd %xmm0, -0x110(%rbp,%rax,8)
movl -0x208(%rbp), %eax
addl $0x1, %eax
movl %eax, -0x208(%rbp)
jmp 0x6fcf5
movsd -0xf0(%rbp), %xmm0
movsd %xmm0, -0x368(%rbp)
movsd -0xe0(%rbp), %xmm0
movsd %xmm0, -0x370(%rbp)
movsd -0x110(%rbp), %xmm0
movsd -0x100(%rbp), %xmm2
movsd -0x110(%rbp), %xmm1
addsd -0x108(%rbp), %xmm1
movsd -0x100(%rbp), %xmm3
subsd -0x108(%rbp), %xmm3
mulsd %xmm3, %xmm1
movq %xmm1, %rax
movabsq $-0x8000000000000000, %rcx # imm = 0x8000000000000000
xorq %rcx, %rax
movq %rax, %xmm1
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
callq 0x6370
movsd -0x370(%rbp), %xmm2
movaps %xmm0, %xmm1
movsd -0x368(%rbp), %xmm0
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd -0x110(%rbp), %xmm1
addsd -0x108(%rbp), %xmm1
divsd %xmm1, %xmm0
movsd %xmm0, -0x118(%rbp)
movsd 0x25842(%rip), %xmm0    # 0x95638
subsd -0x118(%rbp), %xmm0
callq 0x6370
movsd %xmm0, -0x120(%rbp)
movsd -0x118(%rbp), %xmm0
callq 0x66e0
movsd %xmm0, -0x128(%rbp)
movsd -0xa0(%rbp), %xmm0
addsd -0x128(%rbp), %xmm0
callq 0x67f0
movsd %xmm0, -0x360(%rbp)
movsd -0xa0(%rbp), %xmm0
callq 0x67f0
movaps %xmm0, %xmm1
movsd -0x360(%rbp), %xmm0
divsd %xmm1, %xmm0
movsd %xmm0, -0x140(%rbp)
movsd -0x118(%rbp), %xmm0
movsd %xmm0, -0x358(%rbp)
movsd -0xa0(%rbp), %xmm0
callq 0x67f0
movaps %xmm0, %xmm1
movsd -0x358(%rbp), %xmm0
divsd %xmm1, %xmm0
movsd %xmm0, -0x138(%rbp)
leaq -0x140(%rbp), %rsi
leaq -0x290(%rbp), %rdi
leaq -0x70(%rbp), %rdx
callq 0x55cc0
leaq -0x140(%rbp), %rsi
addq $0x8, %rsi
leaq -0x2b8(%rbp), %rdi
leaq -0x88(%rbp), %rdx
callq 0x55cc0
leaq -0x268(%rbp), %rdi
leaq -0x290(%rbp), %rsi
leaq -0x2b8(%rbp), %rdx
callq 0x70640
leaq -0xb8(%rbp), %rdi
leaq -0x268(%rbp), %rsi
callq 0x706b0
leaq -0xb8(%rbp), %rdi
movl $0x2, %esi
callq 0x52b00
movsd (%rax), %xmm0
callq 0x66e0
movsd %xmm0, -0x148(%rbp)
leaq -0xb8(%rbp), %rdi
movl $0x1, %esi
callq 0x52b00
movsd (%rax), %xmm0
movsd %xmm0, -0x350(%rbp)
leaq -0xb8(%rbp), %rdi
xorl %eax, %eax
movl %eax, %esi
callq 0x52b00
movsd -0x350(%rbp), %xmm0
movsd (%rax), %xmm1
callq 0x60a0
movsd %xmm0, -0x150(%rbp)
movsd -0xa0(%rbp), %xmm0
callq 0x6280
movsd 0x256c4(%rip), %xmm2    # 0x95638
subsd -0x120(%rbp), %xmm2
movsd 0x256b4(%rip), %xmm1    # 0x95638
subsd -0xd8(%rbp), %xmm1
mulsd -0x118(%rbp), %xmm1
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, -0x170(%rbp)
movsd 0x2568c(%rip), %xmm0    # 0x95638
subsd -0x120(%rbp), %xmm0
movsd %xmm0, -0x340(%rbp)
movsd -0xd8(%rbp), %xmm0
movsd %xmm0, -0x348(%rbp)
movsd -0xa0(%rbp), %xmm0
callq 0x67f0
movl $0x2, %edi
callq 0x283f0
movsd -0x348(%rbp), %xmm1
movaps %xmm0, %xmm2
movsd -0x340(%rbp), %xmm0
mulsd %xmm2, %xmm1
divsd %xmm1, %xmm0
movsd %xmm0, -0x178(%rbp)
movsd -0xa0(%rbp), %xmm0
addsd -0x128(%rbp), %xmm0
callq 0x67f0
movsd %xmm0, -0x338(%rbp)
movsd -0xa0(%rbp), %xmm0
callq 0x67f0
movaps %xmm0, %xmm1
movsd -0x338(%rbp), %xmm0
divsd %xmm1, %xmm0
movsd %xmm0, -0x180(%rbp)
movsd -0x170(%rbp), %xmm0
movsd 0x27609(%rip), %xmm1    # 0x97660
subsd -0xd8(%rbp), %xmm1
movsd -0x178(%rbp), %xmm2
addsd -0x180(%rbp), %xmm2
mulsd %xmm2, %xmm1
divsd %xmm1, %xmm0
movsd %xmm0, -0x168(%rbp)
movsd -0xa0(%rbp), %xmm0
callq 0x67f0
movaps %xmm0, %xmm1
movsd 0x275c9(%rip), %xmm0    # 0x97660
mulsd %xmm1, %xmm0
movsd %xmm0, -0x328(%rbp)
movsd -0xd8(%rbp), %xmm0
movsd %xmm0, -0x330(%rbp)
movsd 0x275a5(%rip), %xmm0    # 0x97660
divsd -0xd8(%rbp), %xmm0
movsd 0x2556d(%rip), %xmm1    # 0x95638
subsd %xmm1, %xmm0
movsd 0x34af1(%rip), %xmm1    # 0xa4bc8
callq 0x6750
movsd -0x330(%rbp), %xmm1
movaps %xmm0, %xmm2
movsd -0x328(%rbp), %xmm0
mulsd %xmm2, %xmm1
divsd %xmm1, %xmm0
movsd %xmm0, -0x190(%rbp)
movsd 0x27559(%rip), %xmm0    # 0x97660
divsd -0xd8(%rbp), %xmm0
movsd 0x25521(%rip), %xmm1    # 0x95638
subsd %xmm1, %xmm0
callq 0x6370
movsd %xmm0, -0x198(%rbp)
movsd -0xa0(%rbp), %xmm0
callq 0x67f0
movsd %xmm0, -0x318(%rbp)
movsd 0x2551b(%rip), %xmm0    # 0x95660
mulsd -0x128(%rbp), %xmm0
callq 0x6280
movaps %xmm0, %xmm1
movsd 0x254db(%rip), %xmm0    # 0x95638
divsd %xmm1, %xmm0
movsd %xmm0, -0x320(%rbp)
movsd -0xa0(%rbp), %xmm0
callq 0x6120
movsd -0x320(%rbp), %xmm2
movaps %xmm0, %xmm1
movsd -0x318(%rbp), %xmm0
movq %xmm1, %rax
movabsq $-0x8000000000000000, %rcx # imm = 0x8000000000000000
xorq %rcx, %rax
movq %rax, %xmm1
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, -0x1a0(%rbp)
movsd -0x190(%rbp), %xmm0
movsd %xmm0, -0x310(%rbp)
movsd -0x198(%rbp), %xmm0
movsd -0x1a0(%rbp), %xmm1
callq 0x60a0
movaps %xmm0, %xmm1
movsd -0x310(%rbp), %xmm0
mulsd %xmm1, %xmm0
movsd %xmm0, -0x188(%rbp)
movsd -0x90(%rbp), %xmm0
movsd %xmm0, -0x300(%rbp)
movsd -0x98(%rbp), %xmm0
movsd %xmm0, -0x308(%rbp)
movsd -0xa0(%rbp), %xmm0
callq 0x67f0
movsd -0x308(%rbp), %xmm1
movaps %xmm0, %xmm2
movsd -0x300(%rbp), %xmm0
mulsd %xmm2, %xmm1
divsd %xmm1, %xmm0
movsd -0x168(%rbp), %xmm1
addsd -0x188(%rbp), %xmm1
mulsd %xmm1, %xmm0
movsd %xmm0, -0x1a8(%rbp)
movsd -0x148(%rbp), %xmm0
movsd %xmm0, -0x158(%rbp)
movsd -0x150(%rbp), %xmm1
movsd -0x48(%rbp), %xmm0
movsd -0x1a8(%rbp), %xmm2
movq %xmm0, %rax
movabsq $-0x8000000000000000, %rcx # imm = 0x8000000000000000
xorq %rcx, %rax
movq %rax, %xmm0
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, -0x160(%rbp)
movsd -0x58(%rbp), %xmm0
movsd %xmm0, -0x2f0(%rbp)
movsd -0x20(%rbp), %xmm0
movsd %xmm0, -0x2f8(%rbp)
movsd -0x148(%rbp), %xmm0
callq 0x6120
movaps %xmm0, %xmm1
movsd -0x2f8(%rbp), %xmm0
mulsd %xmm1, %xmm0
movl $0x2, %edi
callq 0x283f0
movaps %xmm0, %xmm1
movsd 0x25351(%rip), %xmm0    # 0x95638
subsd %xmm1, %xmm0
callq 0x6370
movaps %xmm0, %xmm1
movsd -0x2f0(%rbp), %xmm0
mulsd %xmm1, %xmm0
movsd %xmm0, -0xc0(%rbp)
movl -0x204(%rbp), %eax
addl $0x1, %eax
movl %eax, -0x204(%rbp)
movsd -0xc0(%rbp), %xmm0
subsd -0xc8(%rbp), %xmm0
movsd %xmm0, -0x2c0(%rbp)
movsd -0xc0(%rbp), %xmm0
ucomisd -0xc8(%rbp), %xmm0
jbe 0x7036a
movsd -0xc8(%rbp), %xmm1
movsd -0x2c0(%rbp), %xmm0
movsd 0x34860(%rip), %xmm2    # 0xa4bb8
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, -0xc8(%rbp)
jmp 0x703a9
movsd -0xc8(%rbp), %xmm1
movsd -0x2c0(%rbp), %xmm0
movq %xmm0, %rax
movabsq $-0x8000000000000000, %rcx # imm = 0x8000000000000000
xorq %rcx, %rax
movq %rax, %xmm0
movsd 0x3481f(%rip), %xmm2    # 0xa4bb8
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, -0xc8(%rbp)
jmp 0x6fbc4
movsd -0x90(%rbp), %xmm0
divsd -0xc0(%rbp), %xmm0
movsd %xmm0, -0x388(%rbp)
movsd -0xd8(%rbp), %xmm0
movsd %xmm0, -0x390(%rbp)
movsd -0xa0(%rbp), %xmm0
callq 0x67f0
movl $0x2, %edi
callq 0x283f0
movsd -0x390(%rbp), %xmm2
movaps %xmm0, %xmm1
movsd -0x388(%rbp), %xmm0
mulsd %xmm1, %xmm2
movsd 0x2522c(%rip), %xmm1    # 0x95638
divsd %xmm2, %xmm1
subsd %xmm1, %xmm0
movsd %xmm0, -0xe0(%rbp)
movsd -0x158(%rbp), %xmm0
callq 0x70c0
movsd %xmm0, -0x380(%rbp)
leaq -0x1c0(%rbp), %rdi
xorl %eax, %eax
movl %eax, %esi
callq 0x52b00
movsd -0x380(%rbp), %xmm0
movsd %xmm0, (%rax)
movsd -0x160(%rbp), %xmm0
callq 0x70c0
movsd %xmm0, -0x378(%rbp)
leaq -0x1c0(%rbp), %rdi
movl $0x1, %esi
callq 0x52b00
movsd -0x378(%rbp), %xmm0
movsd %xmm0, (%rax)
leaq -0x1c0(%rbp), %rdi
movl $0x2, %esi
callq 0x52b00
movq -0x2d0(%rbp), %rdi
xorps %xmm0, %xmm0
movsd %xmm0, (%rax)
leaq -0x1a8(%rbp), %rsi
leaq -0x1c0(%rbp), %rdx
callq 0x706e0
movq -0x2c8(%rbp), %rax
addq $0x390, %rsp            # imm = 0x390
popq %rbp
retq
nopw %cs:(%rax,%rax)
nopl (%rax,%rax)