// File: /workspace/github_cpp_makefile_star2_O1/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 %r15
pushq %r14
pushq %rbx
subq $0x60, %rsp
movq %rdx, %r15
movq %rsi, %r14
movq %rdi, %rbx
movabsq $0x41185421c467381d, %rax # imm = 0x41185421C467381D
movq %rax, (%rdi)
movabsq $0x3ff0000000000000, %rax # imm = 0x3FF0000000000000
movq %rax, 0x58(%rdi)
xorl %ecx, %ecx
movq %rcx, 0x48(%rdi)
xorps %xmm0, %xmm0
movups %xmm0, 0x10(%rdi)
movq %rcx, 0x20(%rdi)
movq %rax, 0x50(%rdi)
movsd (%rsi), %xmm0
movsd 0x8(%rsi), %xmm1
movsd 0x6729(%rip), %xmm2     # 0x2a0e0
divsd %xmm2, %xmm0
movsd %xmm0, (%rsi)
divsd %xmm2, %xmm1
movsd %xmm1, 0x8(%rsi)
movsd 0x10(%rsi), %xmm0
divsd %xmm2, %xmm0
movsd %xmm0, 0x10(%rsi)
movsd (%rdx), %xmm0
movsd 0x8(%rdx), %xmm5
divsd %xmm2, %xmm0
movsd %xmm0, (%rdx)
divsd %xmm2, %xmm5
movsd %xmm5, 0x8(%rdx)
movsd 0x10(%rdx), %xmm1
divsd %xmm2, %xmm1
movsd %xmm1, 0x10(%rdx)
movsd (%rsi), %xmm2
movsd 0x8(%rsi), %xmm3
movsd 0x10(%rsi), %xmm6
movapd %xmm5, %xmm4
mulsd %xmm6, %xmm4
mulsd %xmm0, %xmm6
mulsd %xmm3, %xmm0
mulsd %xmm1, %xmm3
subsd %xmm4, %xmm3
mulsd %xmm2, %xmm1
subsd %xmm1, %xmm6
mulsd %xmm2, %xmm5
subsd %xmm0, %xmm5
movsd %xmm3, 0x18(%rsp)
movapd %xmm3, %xmm1
mulsd %xmm3, %xmm1
movapd %xmm6, 0x40(%rsp)
movapd %xmm6, %xmm2
mulsd %xmm6, %xmm2
movsd %xmm5, 0x8(%rsp)
movapd %xmm5, %xmm0
mulsd %xmm5, %xmm0
addsd %xmm2, %xmm0
addsd %xmm1, %xmm0
xorpd %xmm3, %xmm3
ucomisd %xmm3, %xmm0
jb 0x23a76
sqrtsd %xmm0, %xmm0
jmp 0x23a7f
callq 0x52e0
xorpd %xmm3, %xmm3
movsd %xmm0, 0x10(%rsp)
movsd (%r14), %xmm1
movsd 0x8(%r14), %xmm2
mulsd %xmm1, %xmm1
mulsd %xmm2, %xmm2
movsd 0x10(%r14), %xmm0
mulsd %xmm0, %xmm0
addsd %xmm2, %xmm0
addsd %xmm1, %xmm0
ucomisd %xmm3, %xmm0
jb 0x23ab6
sqrtsd %xmm0, %xmm0
jmp 0x23abb
callq 0x52e0
movsd %xmm0, 0x20(%rsp)
movsd (%r15), %xmm1
movsd 0x8(%r15), %xmm2
mulsd %xmm1, %xmm1
mulsd %xmm2, %xmm2
movsd 0x10(%r15), %xmm0
mulsd %xmm0, %xmm0
addsd %xmm2, %xmm0
addsd %xmm1, %xmm0
xorpd %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jb 0x23af6
sqrtsd %xmm0, %xmm0
jmp 0x23afb
callq 0x52e0
mulsd %xmm0, %xmm0
movsd 0x41e9(%rip), %xmm1     # 0x27cf0
mulsd %xmm1, %xmm0
movsd 0x65d5(%rip), %xmm2     # 0x2a0e8
movapd %xmm2, %xmm3
divsd 0x20(%rsp), %xmm3
subsd %xmm3, %xmm0
movsd 0x65c7(%rip), %xmm4     # 0x2a0f0
divsd %xmm0, %xmm4
movsd %xmm4, 0x38(%rsp)
movsd 0x10(%rsp), %xmm3
mulsd %xmm3, %xmm3
divsd %xmm4, %xmm3
divsd %xmm2, %xmm3
movsd 0x34c3(%rip), %xmm0     # 0x27010
subsd %xmm3, %xmm0
callq 0x5640
movsd %xmm0, 0x28(%rsp)
movsd 0x8(%rsp), %xmm0
divsd 0x10(%rsp), %xmm0
callq 0x5360
movsd %xmm0, 0x30(%rsp)
movaps 0x40(%rsp), %xmm1
xorps 0x34a1(%rip), %xmm1     # 0x27020
movsd 0x18(%rsp), %xmm0
callq 0x50a0
movsd %xmm0, 0x10(%rsp)
movsd 0x10(%r14), %xmm0
movsd %xmm0, 0x8(%rsp)
movsd 0x30(%rsp), %xmm0
callq 0x50f0
movsd 0x8(%rsp), %xmm1
divsd %xmm0, %xmm1
movsd %xmm1, 0x8(%rsp)
movsd (%r14), %xmm0
movsd %xmm0, 0x40(%rsp)
movsd 0x10(%rsp), %xmm0
callq 0x5690
movsd %xmm0, 0x18(%rsp)
movsd 0x8(%r14), %xmm0
movsd %xmm0, 0x58(%rsp)
movsd 0x10(%rsp), %xmm0
callq 0x50f0
movsd 0x58(%rsp), %xmm2
mulsd %xmm0, %xmm2
movsd 0x18(%rsp), %xmm1
mulsd 0x40(%rsp), %xmm1
addsd %xmm2, %xmm1
movsd 0x8(%rsp), %xmm0
callq 0x50a0
movsd %xmm0, 0x8(%rsp)
movsd 0x28(%rsp), %xmm2
movapd %xmm2, %xmm0
mulsd %xmm2, %xmm0
movsd 0x33e5(%rip), %xmm1     # 0x27010
subsd %xmm0, %xmm1
mulsd 0x38(%rsp), %xmm1
movsd 0x20(%rsp), %xmm0
subsd %xmm0, %xmm1
divsd %xmm2, %xmm1
divsd %xmm0, %xmm1
movapd %xmm1, %xmm0
callq 0x5360
movsd 0x8(%rsp), %xmm2
subsd %xmm0, %xmm2
movsd 0x38(%rsp), %xmm1
movsd %xmm1, 0x58(%rbx)
movsd 0x28(%rsp), %xmm1
movsd %xmm1, 0x10(%rbx)
movsd 0x30(%rsp), %xmm1
movsd %xmm1, 0x8(%rbx)
movsd 0x10(%rsp), %xmm1
movsd %xmm1, 0x18(%rbx)
movsd %xmm2, 0x20(%rbx)
movsd %xmm0, 0x48(%rbx)
movq %rbx, %rax
addq $0x60, %rsp
popq %rbx
popq %r14
popq %r15
retq