struct Quaternion
    i::Float64
    j::Float64
    k::Float64
    r::Float64

    # TODO: Determine an acceptable tolerance. ≈ default is too strict.
    Quaternion(i, j, k, r) =
        abs(norm([i, j, k, r]) - 1) < 0.1 ? new(i, j, k, r) :
        error("Magnitude not equal to 1: " * string(norm([i, j, k, r])))

    Quaternion(q) = Quaternion(q[1], q[2], q[3], q[4])
    Quaternion() = new(0, 0, 0, 1)

    Quaternion(yaw, pitch, roll) =
        Quaternion([0 0 sin(yaw / 2) cos(yaw / 2)]) *
        Quaternion([0 sin(pitch / 2) 0 cos(pitch / 2)]) *
        Quaternion([sin(roll / 2) 0 0 cos(roll / 2)])

    function Quaternion(ē, ϕ)
        if sum(ē) == 0
            return Quaternion([0 0 0 cos(ϕ / 2)])
        end
        dir = normalize(ē) * sin(ϕ / 2)
        return Quaternion(dir[1], dir[2], dir[3], cos(ϕ / 2))
    end

end

function QuaternionMultiplication(l::Quaternion, r::Quaternion)
    R = [
        r.r r.k r.j r.i
        -r.k r.r r.i r.j
        r.j -r.i r.r r.k
        -r.i -r.j -r.k r.r
    ]
    L = [l.i; l.j; l.k; l.r]
    return Quaternion(R * L)
end

Base.:*(l::Quaternion, r::Quaternion) =
    QuaternionMultiplication(l::Quaternion, r::Quaternion)

Base.iterate(q::Quaternion, state = 1) =
    state > 4 ? nothing : (collect(q)[state], state + 1)
Base.length(q::Quaternion) = 4
Base.collect(q::Quaternion) = [q.i, q.j, q.k, q.r]
Base.getindex(q::Quaternion, i) = collect(q)[i]
Base.isapprox(a::Quaternion, b::Quaternion) = isapprox(collect(a), collect(b))

LinearAlgebra.norm(q::Quaternion) = norm(collect(q))
LinearAlgebra.normalize(q::Quaternion) = collect(q) / norm(q)

# I dont think we need an actual type for this it might just be easier to keep it as a Vector or something.
# struct EulerAngles
#     roll::Float64
#     pitch::Float64
#     yaw::Float64

#     EulerAngles(r, p, y) = new(r, p, y)
#     EulerAngles() = new(0.0, 0.0, 0.0)
# end


function q_to_Euler(q::Quaternion)
    # https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Source_code_2

    # roll (x-axis rotation)
    sinr_cosp = 2 * (q.r * q.i + q.j * q.k)
    cosr_cosp = 1 - 2 * (q.i * q.i + q.j * q.j)
    roll = atan(sinr_cosp, cosr_cosp)

    # pitch (y-axis rotation)
    sinp = 2 * (q.r * q.j - q.k * q.i)
    if (abs(sinp) >= 1)
        pitch = copysign(π / 2, sinp) # use 90 degrees if out of range
    else
        pitch = asin(sinp)
    end

    # yaw (z-axis rotation)
    siny_cosp = 2 * (q.r * q.k + q.i * q.j)
    cosy_cosp = 1 - 2 * (q.j * q.j + q.k * q.k)
    yaw = atan(siny_cosp, cosy_cosp)


    # return EulerAngles(roll, pitch, yaw)
    return (roll = rad2deg(roll), pitch = rad2deg(pitch), yaw = rad2deg(yaw))
    # return (roll = roll, pitch = pitch, yaw = yaw)
end