#!/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) = (
        %v(1 0 0),
        %v(0 1 0),
        %v(0 0 1),
    )
    func muladd(v1, x1, v2, x2) {
        (v1 * x1) + (v2 * 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)*r
    var speed = muladd(i, rprime*c - r*s, j, rprime*s + r*c)
    speed /= speed.abs
    speed *= sqrt(2/r - 1/semimajor_axis)
    struct Result { position, speed }
    Result(position, speed)
}
for args in ([
    [1, 0.1, 0, 355/(113*6), 0, 0],
    [1, 0.1, Num.pi/18, Num.pi/6, Num.pi/4, 0]
]) {
    var r = orbital_state_vectors(args...)
    say "Arguments: #{args}:"
    say "Position : #{r.position}"
    say "Speed    : #{r.speed}\n"
}
var r = orbital_state_vectors(1, 0.1, Num.pi/18, Num.pi/6, Num.pi/4, 0)
assert(r.position[0] =~= 0.23777128398220654779107184959165027147748809404)
assert(r.position[1] =~= 0.860960261697715834668966272382699039216399966872)
assert(r.position[2] =~= 0.110509023572075562109405412890808505271310143909)
assert(r.speed[0] =~= -1.06193301748006004757467368094494935655538772696)
assert(r.speed[1] =~= 0.275850020569249507846452830330085489348356659642)
assert(r.speed[2] =~= 0.135747024865598167166145512759280712986072818844)