// File: /workspace/github_cpp_makefile_star2_O2/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 %r13
pushq %r12
pushq %rbx
subq $0x80, %rsp
movq %rdx, %r12
movq %rsi, %r14
movq %rdi, %rbx
movabsq $0x41185421c467381d, %rax # imm = 0x41185421C467381D
movq %rax, (%rdi)
movabsq $0x3ff0000000000000, %rax # imm = 0x3FF0000000000000
movq %rax, 0x50(%rdi)
leaq 0x50(%rsp), %r15
movq %rsi, (%r15)
movabsq $0x408f400000000000, %r13 # imm = 0x408F400000000000
movq %r13, 0x8(%r15)
movq %rsi, %rdi
movq %r15, %rsi
callq 0x155d4
movq %r12, (%r15)
movq %r13, 0x8(%r15)
leaq 0x50(%rsp), %rsi
movq %r12, %rdi
callq 0x155d4
leaq 0x68(%rsp), %r13
movq %r13, %rdi
movq %r14, %rsi
movq %r12, %rdx
callq 0x1c922
leaq 0x50(%rsp), %r15
movq %r13, %rdi
movq %r15, %rsi
callq 0x15314
movq %r15, %rdi
callq 0x10c94
movsd %xmm0, 0x8(%rsp)
movq %r14, %rdi
callq 0x10c94
movsd %xmm0, 0x30(%rsp)
movq %r12, %rdi
callq 0x10c94
mulsd %xmm0, %xmm0
movsd 0x3dac(%rip), %xmm1     # 0x1fe38
mulsd %xmm1, %xmm0
movsd 0x6e18(%rip), %xmm2     # 0x22eb0
movapd %xmm2, %xmm3
divsd 0x30(%rsp), %xmm3
subsd %xmm3, %xmm0
movsd 0x6e0a(%rip), %xmm4     # 0x22eb8
divsd %xmm0, %xmm4
movsd %xmm4, 0x28(%rsp)
movsd 0x8(%rsp), %xmm3
mulsd %xmm3, %xmm3
divsd %xmm4, %xmm3
divsd %xmm2, %xmm3
movsd 0x2f8e(%rip), %xmm0     # 0x1f060
subsd %xmm3, %xmm0
callq 0x5630
movsd %xmm0, 0x18(%rsp)
movsd 0x10(%r15), %xmm0
divsd 0x8(%rsp), %xmm0
callq 0x5360
movsd %xmm0, 0x20(%rsp)
movsd (%r15), %xmm0
movsd 0x8(%r15), %xmm1
xorps 0x2f06(%rip), %xmm1     # 0x1f010
callq 0x5090
movsd %xmm0, 0x8(%rsp)
movsd 0x10(%r14), %xmm0
movsd %xmm0, 0x10(%rsp)
movsd 0x20(%rsp), %xmm0
callq 0x50e0
movsd 0x10(%rsp), %xmm1
divsd %xmm0, %xmm1
movsd %xmm1, 0x10(%rsp)
movsd (%r14), %xmm0
movsd %xmm0, 0x48(%rsp)
movsd 0x8(%rsp), %xmm0
callq 0x5680
movsd %xmm0, 0x40(%rsp)
movsd 0x8(%r14), %xmm0
movsd %xmm0, 0x38(%rsp)
movsd 0x8(%rsp), %xmm0
callq 0x50e0
movsd 0x38(%rsp), %xmm2
mulsd %xmm0, %xmm2
movsd 0x40(%rsp), %xmm1
mulsd 0x48(%rsp), %xmm1
addsd %xmm2, %xmm1
movsd 0x10(%rsp), %xmm0
callq 0x5090
movsd %xmm0, 0x10(%rsp)
movsd 0x18(%rsp), %xmm2
movapd %xmm2, %xmm0
mulsd %xmm2, %xmm0
movsd 0x2eb0(%rip), %xmm1     # 0x1f060
subsd %xmm0, %xmm1
mulsd 0x28(%rsp), %xmm1
movsd 0x30(%rsp), %xmm0
subsd %xmm0, %xmm1
divsd %xmm2, %xmm1
divsd %xmm0, %xmm1
movapd %xmm1, %xmm0
callq 0x5360
movsd 0x10(%rsp), %xmm2
subsd %xmm0, %xmm2
movsd 0x28(%rsp), %xmm1
movsd %xmm1, 0x58(%rbx)
movsd 0x18(%rsp), %xmm1
movsd %xmm1, 0x10(%rbx)
movsd 0x20(%rsp), %xmm1
movsd %xmm1, 0x8(%rbx)
movsd 0x8(%rsp), %xmm1
movsd %xmm1, 0x18(%rbx)
movsd %xmm2, 0x20(%rbx)
movsd %xmm0, 0x48(%rbx)
movq %rbx, %rax
addq $0x80, %rsp
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
retq