rv_to_orbital_elements Subroutine

    pure subroutine rv_to_orbital_elements(mu, r, v, p, ecc, inc, raan, aop, tru)

    implicit none

    real(wp),intent(in)              :: mu   !! gravitational parameter [\(km^{3}/s^{2}\)]
    real(wp),dimension(3),intent(in) :: r    !! position vector [km]
    real(wp),dimension(3),intent(in) :: v    !! velocity vector [km/s]
    real(wp),intent(out)             :: p    !! semiparameter \(a(1-e^{2})\) [km]
    real(wp),intent(out)             :: ecc  !! eccentricity [--]
    real(wp),intent(out)             :: inc  !! inclination [rad]
    real(wp),intent(out)             :: raan !! raan [rad]
    real(wp),intent(out)             :: aop  !! argument of peripsis [rad]
    real(wp),intent(out)             :: tru  !! true anomaly [rad]

    real(wp),dimension(3) :: h,n,e
    logical :: circular,equatorial
    reaL(wp) :: hmag,rmag,vmag

    rmag = norm2(r)
    vmag = norm2(v)
    h    = cross(r,v)
    hmag = norm2(h)
    n    = cross([zero,zero,one],h)/hmag
    e    = ((vmag**2-mu/rmag)*r-dot_product(r,v)*v)/mu
    ecc  = norm2(e)
    p    = hmag**2/mu
    inc  = atan2(norm2(h(1:2)),h(3))

    call orbit_check(ecc,inc,circular,equatorial)

    if (equatorial .and. .not. circular) then
        raan = zero
        aop  = wrap_angle(atan2(e(2),e(1))) ! Longitude of periapsis
        tru  = wrap_angle(atan2(dot_product(h,cross(e,r))/hmag,dot_product(r,e)))
    elseif ( .not. equatorial .and. circular) then
        raan = wrap_angle(atan2(n(2),n(1)))
        aop  = zero
        tru  = wrap_angle(atan2(dot_product(r,cross(h,n))/hmag,dot_product(r,n))) ! Argument of latitude
    elseif (equatorial .and. circular) then
        raan = zero
        aop  = zero
        tru  = wrap_angle(atan2(r(2),r(1))) ! True longitude
    else
        raan = wrap_angle(atan2(n(2),n(1)))
        aop  = wrap_angle(atan2(dot_product(e,cross(h,n))/hmag,dot_product(e,n)))
        tru  = wrap_angle(atan2(dot_product(r,cross(h,e))/hmag,dot_product(r,e)))
    endif

    end subroutine rv_to_orbital_elements