#!/usr/bin/ruby

#
## https://rosettacode.org/wiki/Orbital_elements
#

func orbital_state_vectors(
    semimajor_axis,
    eccentricity,
    inclination,
    longitude_of_ascending_node,
    argument_of_periapsis,
    true_anomaly
) {
    var (i, j, k) = (Vec(1,0,0), Vec(0,1,0), Vec(0,0,1))

    func muladd(v1, x1, v2, x2) {
        v1.mul(x1).add(v2.mul(x2))
    }

    func rotate(Ref i, Ref j, α) {
        (*i, *j) = (
            muladd(*i, +cos(α), *j, sin(α)),
            muladd(*i, -sin(α), *j, cos(α)),
        )
    }

    rotate(\i, \j, longitude_of_ascending_node)
    rotate(\j, \k, inclination)
    rotate(\i, \j, argument_of_periapsis)

    var l = (eccentricity == 1 ? 2*semimajor_axis
                               : semimajor_axis*(1 - eccentricity**2))

    var (c, s) = with(true_anomaly) { (.cos, .sin) }

    var r = l/(1 + eccentricity*c)
    var rprime = (s * r**2 / l)
    var position = muladd(i, c, j, s).mul(r)

    var speed = muladd(i, rprime*c - r*s, j, rprime*s + r*c)
    speed.div!(abs(speed))
    speed.mul!(sqrt(2/r - 1/semimajor_axis))

    struct Result { position, speed }
    Result(position, speed)
}

var r = orbital_state_vectors(
    semimajor_axis: 1,
    eccentricity: 0.1,
    inclination: 0,
    longitude_of_ascending_node: 355/(113*6),
    argument_of_periapsis: 0,
    true_anomaly: 0,
)

say '['+r.position.join(', ')+']'
say '['+r.speed.join(', ')+']'

assert_eq(r.position.round(-15), [0.77942284339868f, 0.450000034653684f, 0])
assert_eq(r.speed.round(-15), [-0.552770840960444f, 0.957427083179762f, 0])

__END__
[0.779422843398679832042176328223663037464703527986, 0.450000034653684237432302249506712706822033851071, 0]
[-0.552770840960443759673279062314259546277084494097, 0.957427083179761535246200368614952095349966503287, 0]
