// File: /workspace/github_cpp_makefile_star2_O3/istellartech[P]OpenTsiolkovsky/src/Orbit.cpp
// Function: elementECI2Orbit(Eigen::Matrix<double, 3, 1, 0, 3, 1>, Eigen::Matrix<double, 3, 1, 0, 3, 1>)
Orbit elementECI2Orbit(Vector3d posECI_, Vector3d velECI_){
    //    output
    //    a     Semi-major Axis(km)         長半径(km)
    //    e     Eccentricity(-)             離心率(-)
    //    i     Inclination(rad)            軌道傾斜角(rad)
    //    Omega RAAN(rad)                   昇交点赤経(rad)
    //    omega Argument of Perigee(rad)    近地点引数(rad)
    //    nu    true Anomaly                真近地点離角(rad)
    //    M     Mean Anomaly                平均近地点(rad)
    // cf. http://ccar.colorado.edu/asen5070/handouts/cart2kep2002.pdf
    Orbit element = Orbit();
    double a, e, i, Omega, omega, nu;
    double mu = 3.986004418e5;        // earth gravitational constant 地球の重力定数(km3/s2)
    Vector3d momentumECI_;
    double momentum_abs_;
    double posECI_abs_;
    double velECI_abs_;
    posECI_ = posECI_ / 1000; //m -> km
    velECI_ = velECI_ / 1000;
    momentumECI_ = posECI_.cross(velECI_);
    momentum_abs_ = momentumECI_.norm();
    posECI_abs_ = posECI_.norm();
    velECI_abs_ = velECI_.norm();
    double specific_energy = velECI_abs_ * velECI_abs_ / 2 - mu / posECI_abs_;
    a = - mu / 2 / specific_energy;
    e = pow(1 - momentum_abs_ * momentum_abs_ / a / mu, 0.5);
    i = acos(momentumECI_[2] / momentum_abs_);
    Omega = atan2(momentumECI_[0], -momentumECI_[1]);
    double argument_of_latitude = atan2(posECI_[2] / sin(i), posECI_[0] * cos(Omega) + posECI_[1] * sin(Omega));
    nu = acos((a * (1 - e*e) - posECI_abs_) / e / posECI_abs_);
    omega = argument_of_latitude - nu;
    element.semi_major = a;
    element.eccentricity = e;
    element.inclination = i;
    element.raan = Omega;
    element.arg_perigee = omega;
    element.true_anomaly = nu;
    return element;
}

// Assembly:
pushq %r14
pushq %rbx
subq $0x58, %rsp
movq %rsi, %r14
movq %rdi, %rbx
movabsq $0x41185421c467381d, %rax # imm = 0x41185421C467381D
movq %rax, (%rdi)
movabsq $0x3ff0000000000000, %rax # imm = 0x3FF0000000000000
movq %rax, 0x50(%rdi)
movupd (%rsi), %xmm0
movapd 0x6764(%rip), %xmm1     # 0x28220
divpd %xmm1, %xmm0
movupd %xmm0, (%rsi)
movsd 0x10(%rsi), %xmm0
movsd 0x675f(%rip), %xmm2     # 0x28230
divsd %xmm2, %xmm0
movsd %xmm0, 0x10(%rsi)
movupd (%rdx), %xmm0
divpd %xmm1, %xmm0
movupd %xmm0, (%rdx)
movsd 0x10(%rdx), %xmm1
divsd %xmm2, %xmm1
movsd %xmm1, 0x10(%rdx)
movsd (%rsi), %xmm3
movsd 0x8(%rsi), %xmm5
movsd 0x10(%rsi), %xmm4
movapd %xmm0, %xmm2
unpckhpd %xmm0, %xmm2            # xmm2 = xmm2[1],xmm0[1]
movapd %xmm2, %xmm6
mulsd %xmm4, %xmm6
movapd %xmm5, %xmm7
mulsd %xmm1, %xmm7
subsd %xmm6, %xmm7
movsd %xmm7, (%rsp)
movapd %xmm1, %xmm6
mulsd %xmm3, %xmm6
movapd %xmm4, %xmm8
mulsd %xmm0, %xmm8
subsd %xmm6, %xmm8
movapd %xmm8, 0x10(%rsp)
movapd %xmm0, %xmm6
mulsd %xmm5, %xmm6
movapd %xmm3, %xmm9
mulsd %xmm2, %xmm9
subsd %xmm6, %xmm9
movsd %xmm9, 0x8(%rsp)
movapd %xmm7, %xmm6
mulsd %xmm7, %xmm6
movapd %xmm8, %xmm7
mulsd %xmm8, %xmm7
movapd %xmm9, %xmm8
mulsd %xmm9, %xmm8
addsd %xmm7, %xmm8
addsd %xmm6, %xmm8
xorps %xmm6, %xmm6
sqrtsd %xmm8, %xmm6
movsd %xmm6, 0x28(%rsp)
mulsd %xmm3, %xmm3
mulsd %xmm5, %xmm5
mulsd %xmm4, %xmm4
addsd %xmm5, %xmm4
addsd %xmm3, %xmm4
sqrtsd %xmm4, %xmm4
movsd %xmm4, 0x50(%rsp)
mulsd %xmm0, %xmm0
mulsd %xmm2, %xmm2
mulsd %xmm1, %xmm1
addsd %xmm2, %xmm1
addsd %xmm0, %xmm1
xorps %xmm0, %xmm0
sqrtsd %xmm1, %xmm0
mulsd %xmm0, %xmm0
movsd 0x4148(%rip), %xmm1     # 0x25d20
mulsd %xmm1, %xmm0
movsd 0x6654(%rip), %xmm2     # 0x28238
movapd %xmm2, %xmm3
divsd %xmm4, %xmm3
subsd %xmm3, %xmm0
movsd 0x6648(%rip), %xmm4     # 0x28240
divsd %xmm0, %xmm4
movsd %xmm4, 0x38(%rsp)
movapd %xmm6, %xmm3
mulsd %xmm6, %xmm3
divsd %xmm4, %xmm3
divsd %xmm2, %xmm3
movsd 0x3456(%rip), %xmm0     # 0x25070
subsd %xmm3, %xmm0
callq 0x5640
movsd %xmm0, 0x30(%rsp)
movsd 0x8(%rsp), %xmm0
divsd 0x28(%rsp), %xmm0
callq 0x5360
movsd %xmm0, 0x8(%rsp)
movaps 0x10(%rsp), %xmm1
xorps 0x4304(%rip), %xmm1     # 0x25f50
movsd (%rsp), %xmm0
callq 0x50a0
movsd %xmm0, (%rsp)
movsd 0x10(%r14), %xmm0
movsd %xmm0, 0x10(%rsp)
movsd 0x8(%rsp), %xmm0
callq 0x50f0
movsd 0x10(%rsp), %xmm1
divsd %xmm0, %xmm1
movsd %xmm1, 0x10(%rsp)
movsd (%r14), %xmm0
movsd %xmm0, 0x28(%rsp)
movsd (%rsp), %xmm0
callq 0x5690
movsd %xmm0, 0x48(%rsp)
movsd 0x8(%r14), %xmm0
movsd %xmm0, 0x40(%rsp)
movsd (%rsp), %xmm0
callq 0x50f0
movsd 0x40(%rsp), %xmm2
mulsd %xmm0, %xmm2
movsd 0x48(%rsp), %xmm1
mulsd 0x28(%rsp), %xmm1
addsd %xmm2, %xmm1
movsd 0x10(%rsp), %xmm0
callq 0x50a0
movsd %xmm0, 0x10(%rsp)
movsd 0x30(%rsp), %xmm2
movapd %xmm2, %xmm0
mulsd %xmm2, %xmm0
movsd 0x337c(%rip), %xmm1     # 0x25070
subsd %xmm0, %xmm1
mulsd 0x38(%rsp), %xmm1
movsd 0x50(%rsp), %xmm0
subsd %xmm0, %xmm1
divsd %xmm2, %xmm1
divsd %xmm0, %xmm1
movapd %xmm1, %xmm0
callq 0x5360
movsd 0x10(%rsp), %xmm2
subsd %xmm0, %xmm2
movsd 0x38(%rsp), %xmm1
movsd %xmm1, 0x58(%rbx)
movsd 0x30(%rsp), %xmm1
movsd %xmm1, 0x10(%rbx)
movsd 0x8(%rsp), %xmm1
movsd %xmm1, 0x8(%rbx)
movsd (%rsp), %xmm1
movsd %xmm1, 0x18(%rbx)
movsd %xmm2, 0x20(%rbx)
movsd %xmm0, 0x48(%rbx)
movq %rbx, %rax
addq $0x58, %rsp
popq %rbx
popq %r14
retq