isdefined(Base, :__precompile__) && __precompile__()

module SIUnits

    using Compat; import Compat.String

    import Base: ==, +, -, *, /, .+, .-, .*, ./, //, ^
    import Base: promote_rule, promote_type, convert, show, mod

    immutable SIQuantity{T<:Number,m,kg,s,A,K,mol,cd,rad,sr} <: Number
        val::T
    end

    typealias UnitQuantity{T} SIQuantity{T,0,0,0,0,0,0,0,0,0}

    SIQuantity{T<:Number}(x::T) = UnitQuantity{T}(x)

    immutable SIUnit{m,kg,s,A,K,mol,cd,rad,sr} <: Number
    end

    if !isdefined(Base, :UnitRange)
        const Range = Ranges # Deprecations introduced early in the 0.3 cycle
        const UnitRange = Range1
    end

    abstract SIRanges{T,m,kg,s,A,K,mol,cd,rad,sr} <: Range{SIQuantity{T,m,kg,s,A,K,mol,cd,rad,sr}}

    immutable SIRange{R<:Range,T<:Real,m,kg,s,A,K,mol,cd,rad,sr} <: SIRanges{T,m,kg,s,A,K,mol,cd,rad,sr}
        val::R
    end

    typealias UnitTuple NTuple{9,Int}

    unit{T,m,kg,s,A,K,mol,cd,rad,sr}(x::SIRanges{T,m,kg,s,A,K,mol,cd,rad,sr}) = SIUnit{m,kg,s,A,K,mol,cd,rad,sr}()
    quantity{T,m,kg,s,A,K,mol,cd,rad,sr}(x::SIRanges{T,m,kg,s,A,K,mol,cd,rad,sr}) = SIQuantity{T,m,kg,s,A,K,mol,cd,rad,sr}

    import Base: length, getindex, unsafe_getindex, next, float, show, start, step, last, done, first, eltype, one, zero

    one(x::SIQuantity) = one(x.val)
    one{T,m,kg,s,A,K,mol,cd,rad,sr}(::Type{SIQuantity{T,m,kg,s,A,K,mol,cd,rad,sr}}) = one(T)
    zero(x::SIQuantity) = zero(x.val) * unit(x)
    zero{T,m,kg,s,A,K,mol,cd,rad,sr}(::Type{SIQuantity{T,m,kg,s,A,K,mol,cd,rad,sr}}) = zero(T) * SIUnit{m,kg,s,A,K,mol,cd,rad,sr}()

    # This is all nessecary because SIQuanity{T<:Real} !<: Real
    show(io::IO, x::SIRanges) = (show(io, x.val); show(io,unit(x)))
    function show(io::IO, r::SIRange)
        if step(r) == zero(quantity(r))
            print(io, "SIRange(",first(r),",",step(r),",",length(r),")")
        else
            print(io, first(r),':',step(r),':',last(r))
        end
    end
    show{T<:UnitRange}(io::IO, r::SIRange{T}) = print(io, first(r),':',last(r))
    getindex(r::SIRanges,i::Integer) = (quantity(r)(getindex(r.val,i)))
    getindex{T<:SIRanges}(r::T,i::Range) = T(getindex(r.val,i))
    unsafe_getindex(r::SIRanges,i::Integer) = (quantity(r)(unsafe_getindex(r.val,i)))
    unsafe_getindex{T<:SIRanges}(r::T,i::Range) = T(unsafe_getindex(r.val,i))
    function next(r::SIRanges, i)
        v, j = next(r.val,i)
        to_q(quantity(r),v), j
    end
    length(r::SIRanges) = length(r.val)
    start(r::SIRanges) = start(r.val)
    done(r::SIRanges,i) = done(r.val,i)
    eltype(r::SIRanges) = quantity(r)

    for func in (:first,:step,:last)
        @eval $(func)(r::SIRanges) = to_q(quantity(r),$(func)(r.val))
    end
    # Forward some linear range transformations to the wrapped range
    rangequantity{R<:Range}(::Type{R},tup::UnitTuple) = SIRange{R,eltype(R),tup[1],tup[2],tup[3],tup[4],tup[5],tup[6],tup[7],tup[8],tup[9]}
    for func in (VERSION < v"0.3-" ? (:+, :-) : (:.+, :.-)) # version 0.3 swaps fallbacks
        @eval $(func){T,S,m,kg,s,A,K,mol,cd,rad,sr}(x::SIRanges{T,m,kg,s,A,K,mol,cd,rad,sr}, y::SIQuantity{S,m,kg,s,A,K,mol,cd,rad,sr}) = (val = $(func)(x.val, y.val); SIRange{typeof(val),eltype(val),m,kg,s,A,K,mol,cd,rad,sr}(val))
        @eval $(func){T,S,m,kg,s,A,K,mol,cd,rad,sr}(x::SIQuantity{T,m,kg,s,A,K,mol,cd,rad,sr}, y::SIRanges{S,m,kg,s,A,K,mol,cd,rad,sr}) = (val = $(func)(x.val, y.val); SIRange{typeof(val),eltype(val),m,kg,s,A,K,mol,cd,rad,sr}(val))
    end
    ./(x::SIRanges, y::SIQuantity) = (val = ./(x.val, y.val); rangequantity(typeof(val),tup(x)-tup(y))(val))
    .*(x::SIRanges, y::SIQuantity) = (val = .*(x.val, y.val); rangequantity(typeof(val),tup(x)+tup(y))(val))
    .*(x::SIQuantity, y::SIRanges) = (val = .*(x.val, y.val); rangequantity(typeof(val),tup(x)+tup(y))(val))
    # Version 0.2 assumes all Ranges have start and len fields in ==, and
    # the fallback in 0.3 needlessly iterates through all values
    ==(r::SIRanges, s::SIRanges) = r.val == s.val && tup(r) == tup(s)
    ==(s::SIRanges, r::Range) = s.val == r && tup(s) == (0,0,0,0,0,0,0,0,0)
    ==(r::Range, s::SIRanges) = r == s.val && tup(s) == (0,0,0,0,0,0,0,0,0)

    tup2u(tup) = SIUnit{tup[1],tup[2],tup[3],tup[4],tup[5],tup[6],tup[7],tup[8],tup[9]}
    quantity(T::Type,tup::UnitTuple) = quantity(T,tup2u(tup)())
    -(tup::UnitTuple) = (-tup[1],-tup[2],-tup[3],-tup[4],-tup[5],-tup[6],-tup[7],-tup[8],-tup[9])

    for op in (:-,:*,:+)
        @eval function $(op)(tup1::UnitTuple,tup2::UnitTuple)
            ($(op)(tup1[1],tup2[1]),$(op)(tup1[2],tup2[2]),$(op)(tup1[3],tup2[3]),$(op)(tup1[4],tup2[4]),$(op)(tup1[5],tup2[5]),
                $(op)(tup1[6],tup2[6]),$(op)(tup1[7],tup2[7]),$(op)(tup1[8],tup2[8]),$(op)(tup1[9],tup2[9]))
        end
    end

    export quantity, @quantity

    function quantity{S}(T,quant::SIQuantity{S})
        quant.val == one(S) || error("Quantity value must be unity!")
        quantity(T,unit(quant))
    end
    quantity{m,kg,s,A,K,mol,cd,rad,sr}(T::(@compat Union{Type,TypeVar}),unit::SIUnit{m,kg,s,A,K,mol,cd,rad,sr}) = SIQuantity{T,m,kg,s,A,K,mol,cd,rad,sr}

    tup{m,kg,s,A,K,mol,cd,rad,sr}(u::SIUnit{m,kg,s,A,K,mol,cd,rad,sr}) = (m,kg,s,A,K,mol,cd,rad,sr)
    tup{T,m,kg,s,A,K,mol,cd,rad,sr}(u::SIQuantity{T,m,kg,s,A,K,mol,cd,rad,sr}) = (m,kg,s,A,K,mol,cd,rad,sr)
    tup{T,m,kg,s,A,K,mol,cd,rad,sr}(u::SIRanges{T,m,kg,s,A,K,mol,cd,rad,sr}) = (m,kg,s,A,K,mol,cd,rad,sr)

    macro quantity(expr,unit)
        esc(:(SIUnits.SIQuantity{$expr,SIUnits.tup($unit)...}))
    end

    # Irrationals propagate through units. Fancy!!
    promote_rule{sym,m,kg,s,A,K,mol,cd,rad,sr}(x::Type{Irrational{sym}},y::Type{SIUnit{m,kg,s,A,K,mol,cd,rad,sr}}) =
        SIQuantity{Irrational{sym},m,kg,s,A,K,mol,cd,rad,sr}
    promote_rule{sym,T,m,kg,s,A,K,mol,cd,rad,sr}(x::Type{Irrational{sym}},y::Type{SIQuantity{T,m,kg,s,A,K,mol,cd,rad,sr}}) =
        SIQuantity{promote_type(Irrational{sym},T)}

    promote_rule{T,S,mS,kgS,sS,AS,KS,molS,cdS,radS,srS,mT,kgT,sT,AT,KT,molT,cdT,radT,srT}(
        A::Type{SIQuantity{T,mT,kgT,sT,AT,KT,molT,cdT,radT,srT}},B::Type{SIQuantity{S,mS,kgS,sS,AS,KS,molS,cdS,radS,srS}}) = SIQuantity{promote_type(T,S)}
    promote_rule{T,mS,kgS,sS,AS,KS,molS,cdS,radS,srS,mT,kgT,sT,AT,KT,molT,cdT,radT,srT}(
        A::Type{SIQuantity{T,mT,kgT,sT,AT,KT,molT,cdT,radT,srT}},B::Type{SIUnit{mS,kgS,sS,AS,KS,molS,cdS,radS,srS}}) = SIQuantity{T}
    promote_rule{S,m,kg,s,A,K,mol,cd,rad,sr}(x::Type{Bool},y::Type{SIQuantity{S,m,kg,s,A,K,mol,cd,rad,sr}}) = SIQuantity{promote_type(Bool,S)}
    promote_rule{m,kg,s,A,K,mol,cd,rad,sr}(x::Type{Bool},y::Type{SIUnit{m,kg,s,A,K,mol,cd,rad,sr}}) = SIQuantity{Bool}
    promote_rule{T<:Number,S,m,kg,s,A,K,mol,cd,rad,sr}(x::Type{T},y::Type{SIQuantity{S,m,kg,s,A,K,mol,cd,rad,sr}}) = SIQuantity{promote_type(T,S)}
    promote_rule{T<:Number,m,kg,s,A,K,mol,cd,rad,sr}(x::Type{T},y::Type{SIUnit{m,kg,s,A,K,mol,cd,rad,sr}}) = SIQuantity{T}

    # One unspecified, units, one concrete (unspecified occurs as the promotion result from the rules above)
    promote_rule{T,S,m,kg,s,A,K,mol,cd,rad,sr}(x::Type{SIQuantity{T}},y::Type{SIQuantity{S,m,kg,s,A,K,mol,cd,rad,sr}}) = SIQuantity{promote_type(T,S)}

    # Unlike most other types, the promotion of two identitical SIQuantities is
    # not that type itself. As such, the promote_type behavior itself must be
    # overridden. C.f. https://github.com/Keno/SIUnits.jl/issues/27
    promote_type{T,m,kg,s,A,K,mol,cd,rad,sr}(::Type{SIQuantity{T,m,kg,s,A,K,mol,cd,rad,sr}}, ::Type{SIQuantity{T,m,kg,s,A,K,mol,cd,rad,sr}}) = SIQuantity{T}

    if VERSION >= v"0.4-dev"
        eval(quote
            convert{T}(::Type{SIQuantity{T}},x::Dates.Period) = error("Conversion from Period to SIQuantity not defined")
        end)
    end
    convert{T<:Number,m,kg,s,A,K,mol,cd,rad,sr}(::Type{SIQuantity{T}},x::SIUnit{m,kg,s,A,K,mol,cd,rad,sr}) = SIQuantity{T,m,kg,s,A,K,mol,cd,rad,sr}(one(T))
    convert{T<:Number}(::Type{SIQuantity{T}},x::T) = UnitQuantity{T}(x)
    convert{T<:Number,S<:Number}(::Type{SIQuantity{T}},x::S) = convert(SIQuantity{T},convert(T,x))
    convert{T<:Number}(::Type{SIQuantity{T}},x::SIQuantity{T}) = x
    convert{T<:Number,S,m,kg,s,A,K,mol,cd,rad,sr}(::Type{SIQuantity{T}},x::SIQuantity{S,m,kg,s,A,K,mol,cd,rad,sr}) = SIQuantity{T,m,kg,s,A,K,mol,cd,rad,sr}(convert(T,x.val))

    to_q{T,m,kg,s,A,K,mol,cd,rad,sr}(::Type{SIQuantity{T,m,kg,s,A,K,mol,cd,rad,sr}},val::T) = (0 == m == kg == s == A == K == mol == cd == rad == sr) ? val : SIQuantity{T,m,kg,s,A,K,mol,cd,rad,sr}(val)
    convert{T,m,kg,s,A,K,mol,cd,rad,sr}(::Type{SIQuantity{T,m,kg,s,A,K,mol,cd,rad,sr}},val::Number) = (SIQuantity{T,m,kg,s,A,K,mol,cd,rad,sr}(convert(T,val)))
    function convert{T,S,mS,kgS,sS,AS,KS,molS,cdS,radS,srS,mT,kgT,sT,AT,KT,molT,cdT,radT,srT}(::Type{SIQuantity{T,mT,kgT,sT,AT,KT,molT,cdT,radT,srT}},val::SIQuantity{S,mS,kgS,sS,AS,KS,molS,cdS,radS,srS})
        if mS != mT || kgS != kgT || sS != sT || AS != AT || KS != KT || molS != molT || cdS != cdT || radS != radT || srS != srT
            error("Dimension mismatch in convert. Attempted to convert a ($(repr(SIUnit{mS,kgS,sS,AS,KS,molS,cdS,radS,srS}))) to ($(repr(SIUnit{mT,kgT,sT,AT,KT,molT,cdT,radT,srT})))")
        end
        SIQuantity{T,mT,kgT,sT,AT,KT,molT,cdT,radT,srT}(convert(T,val.val))
    end

    for op in (:/,://)

        @eval function ($op){T}(x::Number,y::SIQuantity{T})
            val = ($op)(x,y.val)
            to_q(quantity(typeof(val),-tup(y)),val)
        end

        @eval function ($op)(x::SIQuantity,y::SIQuantity)
            val = $(op)(x.val,y.val)
            to_q(quantity(typeof(val),tup(x)-tup(y)),val)
        end

        @eval $(op)(x::SIUnit,y::SIUnit) = tup2u(tup(x)-tup(y))()
        @eval $(op){T}(x::SIQuantity{T},y::SIUnit) = to_q(quantity(T,tup(unit(x))-tup(y)),x.val)
        @eval $(op){T}(x::SIUnit,y::SIQuantity{T}) = to_q(quantity(T,tup(x)-tup(unit(y))),($op)(1,y.val))

        @eval $(op)(x::Number,y::SIUnit) = x*tup2u(-tup(y))()
    end

    inv(y::SIUnit) = tup2u(-tup(y))()

    function +{T,S,m,kg,s,A,K,mol,cd,rad,sr}(
        x::SIQuantity{T,m,kg,s,A,K,mol,cd,rad,sr},y::SIQuantity{S,m,kg,s,A,K,mol,cd,rad,sr})
        val = x.val+y.val
        SIQuantity{typeof(val),m,kg,s,A,K,mol,cd,rad,sr}(val)
    end

    function -{T,S,m,kg,s,A,K,mol,cd,rad,sr}(
        x::SIQuantity{T,m,kg,s,A,K,mol,cd,rad,sr},y::SIQuantity{S,m,kg,s,A,K,mol,cd,rad,sr})
        val = x.val-y.val
        SIQuantity{typeof(val),m,kg,s,A,K,mol,cd,rad,sr}(val)
    end

    function -{T,S,mS,kgS,sS,AS,KS,molS,cdS,radS,srS,mT,kgT,sT,AT,KT,molT,cdT,radT,srT}(
        x::SIQuantity{T,mT,kgT,sT,AT,KT,molT,cdT,radT,srT},y::SIQuantity{S,mS,kgS,sS,AS,KS,molS,cdS,radS,srS})
        error("Unit mismatch. Got ($(repr(unit(x)))) - ($(repr(unit(y))))")
    end

    function +{T,S,mS,kgS,sS,AS,KS,molS,cdS,radS,srS,mT,kgT,sT,AT,KT,molT,cdT,radT,srT}(
        x::SIQuantity{T,mT,kgT,sT,AT,KT,molT,cdT,radT,srT},y::SIQuantity{S,mS,kgS,sS,AS,KS,molS,cdS,radS,srS})
        error("Unit mismatch. Got ($(repr(unit(x)))) + ($(repr(unit(y))))")
    end

    function -{T,m,kg,s,A,K,mol,cd,rad,sr}(x::SIQuantity{T,m,kg,s,A,K,mol,cd,rad,sr})
        val = -(x.val)
        SIQuantity{typeof(val),m,kg,s,A,K,mol,cd,rad,sr}(val)
    end

    function ^{T,m,kg,s,A,K,mol,cd,rad,sr}(
        x::SIQuantity{T,m,kg,s,A,K,mol,cd,rad,sr},i::Integer)
        if i == 0
            return one(T)
        end
        val = x.val^i
        SIQuantity{typeof(val),m*i,kg*i,s*i,A*i,K*i,mol*i,cd*i,rad*i,sr*i}(val)
    end

    function ^{T,m,kg,s,A,K,mol,cd,rad,sr}(
        x::SIQuantity{T,m,kg,s,A,K,mol,cd,rad,sr},r::Rational)
        if r == 0
            return one(T)
        end
        val = x.val^r
        SIQuantity{typeof(val),convert(Int,m*r),convert(Int,kg*r),convert(Int,s*r),convert(Int,A*r),
        convert(Int,K*r),convert(Int,mol*r),convert(Int,cd*r),convert(Int,rad*r),convert(Int,sr*r)}(val)
    end

    ^{T,m,kg,s,A,K,mol,cd,rad,sr}(x::SIQuantity{T,m,kg,s,A,K,mol,cd,rad,sr},r::AbstractFloat) = x^rationalize(r)

    function ^{T,S,mS,kgS,sS,AS,KS,molS,cdS,radS,srS,mT,kgT,sT,AT,KT,molT,cdT,radT,srT}(
        x::SIQuantity{T,mT,kgT,sT,AT,KT,molT,cdT,radT,srT},y::SIQuantity{S,mS,kgS,sS,AS,KS,molS,cdS,radS,srS})
        error("Can not raise a number to a unitful quantity. Got ($(repr(unit(x))))^($(repr(unit(y))))")
    end

    ^{T,S,m,kg,s,A,K,mol,cd,rad,sr}(x::SIQuantity{T,m,kg,s,A,K,mol,cd,rad,sr},y::SIQuantity{S,0,0,0,0,0,0,0,0}) = x.val^(y.val)

    ==(x::SIQuantity,y::SIQuantity) = (tup(x) == tup(y)) && (x.val == y.val)
    =={T}(x::SIQuantity{T},y::SIUnit) = (tup(x) == tup(y)) && (x.val == one(T))
    =={T}(x::SIUnit,y::SIQuantity{T}) = (tup(x) == tup(y)) && (one(T) == y.val)
    ==(x::SIUnit,y::SIUnit) = tup(x) == tup(y)

    import Base: sqrt, abs, abs2, colon, isless, isfinite, isreal, real, imag, isnan

    function colon{T,S,X,m,kg,s,A,K,mol,cd,rad,sr}(start::SIQuantity{T,m,kg,s,A,K,mol,cd,rad,sr},step::SIQuantity{S,m,kg,s,A,K,mol,cd,rad,sr},stop::SIQuantity{X,m,kg,s,A,K,mol,cd,rad,sr})
        val = colon(start.val,step.val,stop.val)
        SIRange{typeof(val),eltype(val),m,kg,s,A,K,mol,cd,rad,sr}(val)
    end

    function colon{T,S,m,kg,s,A,K,mol,cd,rad,sr}(start::SIQuantity{T,m,kg,s,A,K,mol,cd,rad,sr},stop::SIQuantity{S,m,kg,s,A,K,mol,cd,rad,sr})
        val = colon(start.val,stop.val)
        SIRange{typeof(val),eltype(val),m,kg,s,A,K,mol,cd,rad,sr}(val)
    end

    function sqrt{T,m,kg,s,A,K,mol,cd,rad,sr}(x::SIQuantity{T,m,kg,s,A,K,mol,cd,rad,sr})
        val = sqrt(x.val)
        SIQuantity{typeof(val),convert(Int,m/2),convert(Int,kg/2),convert(Int,s/2),convert(Int,A/2),
        convert(Int,K/2),convert(Int,mol/2),convert(Int,cd/2),convert(Int,rad/2),convert(Int,rad/2)}(val)
    end

    function abs{T,m,kg,s,A,K,mol,cd,rad,sr}(x::SIQuantity{T,m,kg,s,A,K,mol,cd,rad,sr})
        SIQuantity{T,m,kg,s,A,K,mol,cd,rad,sr}(abs(x.val))
    end

    function abs2{T,m,kg,s,A,K,mol,cd,rad,sr}(x::SIQuantity{T,m,kg,s,A,K,mol,cd,rad,sr})
        val = abs2(x.val)
        SIQuantity{typeof(val),2*m,2*kg,2*s,2*A,2*K,2*mol,2*cd,2*rad,2*sr}(val)
    end

    function isfinite{T,m,kg,s,A,K,mol,cd,rad,sr}(x::SIQuantity{T,m,kg,s,A,K,mol,cd,rad,sr})
        isfinite(x.val)
    end

    isnan(x::SIQuantity) = isnan(x.val)
    isreal(x::SIQuantity) = isreal(x.val)
    real(x::SIQuantity) = typeof(x)(real(x.val))
    imag(x::SIQuantity) = typeof(x)(imag(x.val))

    function isless{T}(x::SIQuantity{T,0,0,0,0,0,0,0,0,0}, y::SIQuantity{T,0,0,0,0,0,0,0,0,0})
        return isless(x.val,y.val)
    end
    function isless{T,S}(x::SIQuantity{T,0,0,0,0,0,0,0,0,0}, y::SIQuantity{S,0,0,0,0,0,0,0,0,0})
        return isless(x.val,y.val)
    end
    function isless{T}(x::SIQuantity{T,0,0,0,0,0,0,0,0,0}, y::Real)
        return isless(x.val,y)
    end
    function isless{T}(x::Real, y::SIQuantity{T,0,0,0,0,0,0,0,0,0})
        return isless(x,y.val)
    end
    function isless{T,S,mT,kgT,sT,AT,KT,molT,cdT,radT,srT}(
        x::SIQuantity{T,mT,kgT,sT,AT,KT,molT,cdT,radT,srT},y::SIQuantity{S,mT,kgT,sT,AT,KT,molT,cdT,radT,srT})
        return isless(x.val,y.val)
    end

    function mod{T,S,mS,kgS,sS,AS,KS,molS,cdS,radS,srS,mT,kgT,sT,AT,KT,molT,cdT,radT,srT}(
        x::SIQuantity{T,mT,kgT,sT,AT,KT,molT,cdT,radT,srT},y::SIQuantity{S,mS,kgS,sS,AS,KS,molS,cdS,radS,srS})
        error("Unit mismatch. Got mod($(repr(unit(x))),$(repr(unit(y))))")
    end

    function mod{T,S,m,kg,s,A,K,mol,cd,rad,sr}(x::SIQuantity{T,m,kg,s,A,K,mol,cd,rad,sr},y::SIQuantity{S,m,kg,s,A,K,mol,cd,rad,sr})
        val = mod(x.val,y.val)
        SIQuantity{typeof(val),m,kg,s,A,K,mol,cd,rad,sr}(val)
    end

    import Base: sin, cos, tan, cot, sec, csc
    for func in (:sin,:cos,:tan,:cot,:sec,:csc)
        @eval $func{T}(θ::SIQuantity{T,0,0,0,0,0,0,0,1,0}) = $func(θ.val)
    end

    # Forwarding methods that do not affect units
    import Base: conj
    conj(x::SIQuantity) = typeof(x)(conj(x.val))

    float(x::SIQuantity) = float(x.val)
    if VERSION >= v"0.4.0"
        Base.Float64(x::SIQuantity) = Float64(x.val)
        Base.Int(x::SIQuantity) = Int(x.val)
    else
        Base.float64(x::SIQuantity) = float64(x.val)
        Base.int(x::SIQuantity) = int(x.val)
    end

    *(x::SIUnit,y::SIUnit) = tup2u(tup(x)+tup(y))()
    *{T}(x::SIUnit,y::SIQuantity{T}) = to_q(quantity(T,tup(y)+tup(x)),y.val)
    *{T}(x::SIQuantity{T},y::SIUnit) = to_q(quantity(T,tup(y)+tup(x)),x.val)
    *(x::Irrational,y::SIUnit) = quantity(typeof(x),y)(x)
    function *(x::SIQuantity,y::SIQuantity)
        ret = x.val * y.val
        to_q(quantity(typeof(ret),tup(x)+tup(y)),ret)
    end

    function ^{m,kg,s,A,K,mol,cd,rad,sr}(
        x::SIUnit{m,kg,s,A,K,mol,cd,rad,sr},i::Integer)
        SIUnit{m*i,kg*i,s*i,A*i,K*i,mol*i,cd*i,rad*i,sr*i}()
    end

    unit{T,m,kg,s,A,K,mol,cd,rad,sr}(x::SIQuantity{T,m,kg,s,A,K,mol,cd,rad,sr}) = SIUnit{m,kg,s,A,K,mol,cd,rad,sr}()

    export SIPrefix, Meter, KiloGram, Second, Ampere, Kelvin, Mole, Candela, Radian, Steradian, Kilo, Mega, Giga,
        Tera, Peta, Exa, Zetta, Centi, Milli, Micro, Nano, Pico, Femto, Atto, Zepto, Yocto,
        Gram, Joule, Coulomb, Volt, Farad, Newton, Ohm, CentiMeter, Siemens, Hertz, Watt, Pascal

    const SIPrefix  = SIUnit{0,0,0,0,0,0,0,0,0}()
    const Meter     = SIUnit{1,0,0,0,0,0,0,0,0}()
    const KiloGram  = SIUnit{0,1,0,0,0,0,0,0,0}()
    const Second    = SIUnit{0,0,1,0,0,0,0,0,0}()
    const Ampere    = SIUnit{0,0,0,1,0,0,0,0,0}()
    const Kelvin    = SIUnit{0,0,0,0,1,0,0,0,0}()
    const Mole      = SIUnit{0,0,0,0,0,1,0,0,0}()
    const Candela   = SIUnit{0,0,0,0,0,0,1,0,0}()
    const Radian    = SIUnit{0,0,0,0,0,0,0,1,0}()
    const Steradian = SIUnit{0,0,0,0,0,0,0,0,1}()

    const Kilo       = (1000)SIPrefix
    const Mega       = (10^6)SIPrefix
    const Giga       = (10^9)SIPrefix
    const Tera       = (10^12)SIPrefix
    const Peta       = (10^15)SIPrefix
    const Exa        = (10^18)SIPrefix
    const Zetta      = (10^21)SIPrefix
    const Yotta      = (10^24)SIPrefix
    const Centi      = (1//100)SIPrefix
    const Milli      = (1//1000)SIPrefix
    const Micro      = (1//10^6)SIPrefix
    const Nano       = (1//10^9)SIPrefix
    const Pico       = (1//10^12)SIPrefix
    const Femto      = (1//10^15)SIPrefix
    const Atto       = (1//10^18)SIPrefix
    const Zepto      = (1//10^21)SIPrefix
    const Yocto      = (1//10^24)SIPrefix

    const Gram       = (1//1000)KiloGram
    const Joule      = KiloGram*Meter^2/Second^2
    const Coulomb    = Ampere*Second
    const Volt       = Joule/Coulomb
    const Farad      = Coulomb^2/Joule
    const Newton     = KiloGram*Meter/Second^2
    const Ohm        = Volt/Ampere
    const Hertz      = inv(Second)
    const Siemens    = inv(Ohm)
    const Watt       = Joule/Second
    const Pascal     = Newton/Meter^2

    const CentiMeter = Centi*Meter

# Pretty Printing - Text
    superscript(i) = map(repr(i)) do c
        c   ==  '-' ? '\u207b' :
        c   ==  '1' ? '\u00b9' :
        c   ==  '2' ? '\u00b2' :
        c   ==  '3' ? '\u00b3' :
        c   ==  '4' ? '\u2074' :
        c   ==  '5' ? '\u2075' :
        c   ==  '6' ? '\u2076' :
        c   ==  '7' ? '\u2077' :
        c   ==  '8' ? '\u2078' :
        c   ==  '9' ? '\u2079' :
        c   ==  '0' ? '\u2070' :
        error("Unexpected Chatacter")
    end

    function spacing(idx::Int, x::SIUnit)
        # Only print a space if there are nonzero units coming after this one
        tup(x)[idx+1:end] == ntuple((i)->0, 9-idx) ? "" : " "
    end
    function show{m,kg,s,A,K,mol,cd,rad,sr}(io::IO,x::SIUnit{m,kg,s,A,K,mol,cd,rad,sr})
        kg  != 0 && print(io, "kg",  (kg  == 1 ? spacing(1,x) : superscript(kg)))
        m   != 0 && print(io, "m",   (m   == 1 ? spacing(2,x) : superscript(m)))
        s   != 0 && print(io, "s",   (s   == 1 ? spacing(3,x) : superscript(s)))
        A   != 0 && print(io, "A",   (A   == 1 ? spacing(4,x) : superscript(A)))
        K   != 0 && print(io, "K",   (K   == 1 ? spacing(5,x) : superscript(K)))
        mol != 0 && print(io, "mol", (mol == 1 ? spacing(6,x) : superscript(mol)))
        cd  != 0 && print(io, "cd",  (cd  == 1 ? spacing(7,x) : superscript(cd)))
        rad != 0 && print(io, "rad", (rad == 1 ? spacing(8,x) : superscript(rad)))
        sr  != 0 && print(io, "sr",  (sr  == 1 ? ""           : superscript(sr)))
        nothing
    end

    function show{T,m,kg,s,A,K,mol,cd,rad,sr}(io::IO,x::SIQuantity{T,m,kg,s,A,K,mol,cd,rad,sr})
        show(io,x.val)
        print(io," ")
        show(io,unit(x))
    end

    function sidims{m,kg,s,A,K,mol,cd,rad,sr}(::SIUnit{m,kg,s,A,K,mol,cd,rad,sr})
        (m,kg,s,A,K,mol,cd,rad,sr)
    end

    function sidims{T,m,kg,s,A,K,mol,cd,rad,sr}(::SIQuantity{T,m,kg,s,A,K,mol,cd,rad,sr})
        (m,kg,s,A,K,mol,cd,rad,sr)
    end

    export @prettyshow

    macro prettyshow(unit,string)
        esc(quote function Base.show(io::IO,::SIUnits.SIUnit{SIUnits.sidims($(unit))...})
            print(io,$(string))
        end
        @compat function Base.show(io::IO,::MIME"text/mathtex+latex",::SIUnits.SIUnit{SIUnits.sidims($(unit))...})
            show(io,MIME("text/mathtex+latex"),$(string))
        end
        end)
    end

# Pretty Printing - LaTeX

    using TexExtensions

    @compat import Base: show

    macro l(x)
        esc(quote
            $x != 0 && push!($x>0?num:den,string("\\text{",$(string(x)),"\}",abs($x) == 1 ? " " : string("^{",abs($x),"}")))
        end)
    end

    @compat function show{m,kg,s,A,K,mol,cd,rad,sr}(io::IO,::MIME"text/mathtex+latex",x::SIUnit{m,kg,s,A,K,mol,cd,rad,sr})
        num = String[]
        den = String[]
        @l kg
        @l m
        @l s
        @l A
        @l K
        @l mol
        @l cd
        @l rad
        @l sr
        if !isempty(den)
            if isempty(num)
                write(io,"\\frac{1}{",join(den,"\\;"),"}")
            else
                write(io,"\\frac{",join(num,"\\;"),"}{",join(den,"\\;"),"}")
            end
        else
            write(io,join(num,"\\;"))
        end
    end

    @compat function show{T,m,kg,s,A,K,mol,cd,rad,sr}(io::IO,::MIME"text/mathtex+latex",x::SIQuantity{T,m,kg,s,A,K,mol,cd,rad,sr})
        show(io,MIME("text/mathtex+latex"),x.val)
        write(io,"\\;")
        show(io,MIME("text/mathtex+latex"),unit(x))
    end

# Non-SI Units
immutable NonSIUnit{BaseUnit<:SIUnit,Unit}; end
immutable NonSIQuantity{T<:Number,Unit<:NonSIUnit} <: Number
    val::T
end

# Non-SI promote rules
promote_rule(x::Type{Irrational},y::Type{NonSIUnit}) =
    NonSIQuantity{x,y}
promote_rule{sym,T<:Number,Unit}(x::Type{Irrational{sym}},y::Type{NonSIQuantity{T,Unit}}) =
    NonSIQuantity{promote_type(Irrational{sym},T),Unit}

promote_rule{T<:Number,S<:Number,U1,U2}(
    A::Type{NonSIQuantity{T,U1}},B::Type{SIQuantity{S,U2}}) = NonSIQuantity{promote_type(T,S)}
promote_rule{T<:Number,U1}(
    A::Type{NonSIQuantity{T,U1}},U2::Type{NonSIUnit}) = NonSIQuantity{T}
promote_rule{S<:Number,U}(x::Type{Bool},y::Type{NonSIQuantity{S,U}}) = NonSIQuantity{promote_type(Bool,S),U}
promote_rule(x::Type{Bool},U::Type{NonSIUnit}) = NonSIQuantity{Bool,U}
promote_rule{T<:Number,S<:Number,U}(x::Type{T},y::Type{NonSIQuantity{S,U}}) = NonSIQuantity{promote_type(T,S),U}
promote_rule{T<:Number}(x::Type{T},U::Type{NonSIUnit}) = NonSIQuantity{T,U}

# Interaction between SI and non-SI quantities
promote_rule{S<:Number,T<:Number,U,m,kg,s,A,K,mol,cd,rad,sr}(x::Type{NonSIQuantity{S,U}},y::Type{SIQuantity{T,m,kg,s,A,K,mol,cd,rad,sr}}) =
    SIQuantity{promote_type(S,T)}
promote_rule{S<:Number,T<:Number,U,m,kg,s,A,K,mol,cd,rad,sr}(x::Type{SIQuantity{T,m,kg,s,A,K,mol,cd,rad,sr}},y::Type{NonSIQuantity{S,U}}) =
    SIQuantity{promote_type(S,T)}

siquantity{B}(T,U::NonSIUnit{B}) = quantity(T,B())
siquantity{B}(T,U::Type{NonSIUnit{B}}) = quantity(T,B())
#convert{T,S,U}(::Type{SIQuantity{T}},x::NonSIQuantity{S,U}) = (siquantity(promote_type(T,S),U())(x.val))

*{T<:NonSIUnit}(x::Number,t::T) = NonSIQuantity{typeof(x),T}(x)

baseunit{BaseUnit}(x::NonSIUnit{BaseUnit}) = BaseUnit()
baseunit{T,Unit}(x::NonSIQuantity{T,Unit}) = baseunit(unit(x))
unit{T,Unit}(x::NonSIQuantity{T,Unit}) = Unit()
quantity(T::(@compat Union{Type,TypeVar}),x::NonSIUnit) = NonSIQuantity{T,typeof(x)}
quantity(T::(@compat Union{Type,TypeVar}),U::Type{NonSIUnit}) = NonSIQuantity{T,U}

/{T,U}(x::NonSIQuantity{T,U},y::SIQuantity) = convert(SIQuantity,x)/y
/(x::NonSIUnit,y::SIUnit) = convert(SIQuantity,x)/y
/(x::SIUnit,y::NonSIUnit) = x/convert(SIQuantity,y)

/(x::SIQuantity,y::NonSIUnit) = x/convert(SIQuantity,y)
/(x::NonSIUnit,y::SIQuantity) = convert(SIQuantity,x)/y
-{T,U}(x::NonSIQuantity{T,U}) = NonSIQuantity{T,U}(-x.val)

^(x::(@compat Union{NonSIQuantity,NonSIUnit}),i::Integer) = convert(SIQuantity,x)^i

show{BaseUnit,Unit}(io::IO,x::NonSIUnit{BaseUnit,Unit}) = write(io,string(Unit))
function show(io::IO,x::NonSIQuantity)
    show(io,x.val)
    print(io," ")
    show(io,unit(x))
end

@compat function show{BaseUnit,Unit}(io::IO,::MIME"text/mathtex+latex",x::NonSIUnit{BaseUnit,Unit})
    write(io,"\\text{",string(Unit),"}")
end

@compat function show(io::IO,::MIME"text/mathtex+latex",x::NonSIQuantity)
    show(io,MIME("text/mathtex+latex"),x.val)
    write(io,"\\;")
    show(io,MIME("text/mathtex+latex"),unit(x))
end

# No, these two are not the same. Sometimes we get SIQuantities that are not specialized
# on the type out of promotion, hence the first method, but we still need the second method
# to be more specific that the convert methods of plain SIUnits above.
convert(::Type{SIQuantity},x::NonSIQuantity) = x.val * convert(SIQuantity,unit(x))
convert{T<:Number}(::Type{SIQuantity{T}},x::NonSIQuantity) = x.val * convert(SIQuantity,unit(x))

convert{T}(::Type{NonSIQuantity{T}},U::NonSIUnit) = NonSIQuantity{T,U}(one(T))
convert{T<:Number,U}(::Type{NonSIQuantity{T,U}},x::T) = UnitQuantity{T}(x)
#convert{T}(::Type{NonSIQuantity{T}},x::T) = UnitQuantity{T}(x)
#convert{T,S}(::Type{NonSIQuantity{T}},x::S) = convert(NonSIQuantity{T},convert(T,x))
if VERSION >= v"0.4-dev"
    eval(quote
        convert{T,U}(::Type{NonSIQuantity{T,U}},x::Dates.Period) = error("Conversion from Period to NonSIQuantity not defined")
    end)
end
convert{T,U}(::Type{NonSIQuantity{T,U}},x::Number) = convert(NonSIQuantity{T,U},convert(T,x))
convert{T,U}(::Type{NonSIQuantity{T,U}},x::NonSIQuantity{T,U}) = x
convert{T<:Number,S,U1,U2}(::Type{NonSIQuantity{T,U1}},x::NonSIQuantity{S,U2}) = NonSIQuantity{T,U2}(convert(T,x.val))

export as

function as{U<:NonSIUnit}(x::SIQuantity,y::U)
    val = x/y
    @assert !(typeof(val)<:SIQuantity)
    NonSIQuantity{typeof(val),U}(val)
end

function as{U<:NonSIUnit,Q<:SIQuantity}(X::AbstractArray{Q},y::U)
    val = [x/y for x in X]
    @assert !(typeof(eltype(val))<:SIQuantity)
    NonSIQuantity{typeof(val),U}(val)
end

function as(x::NonSIQuantity,y::SIUnit)
    @assert baseunit(x) == y
    convert(SIQuantity,x)
end

as(x::NonSIQuantity,y::SIQuantity) = as(x,unit(y))

include("nonsiunits.jl")
include("shortunits.jl")

end # module


function Base.map!(
    op::UnaryUnion, C::GBArray, A::GBArray;
    mask = nothing, accum = nothing, desc = nothing
)
    mask, accum = _handlenothings(mask, accum)
    desc === nothing && (desc = DEFAULTDESC)
    op = getoperator(op, eltype(A))
    accum = getaccum(accum, eltype(C))
    A, desc = _handletranspose(A, desc)
    if C isa GBVector && A isa GBVector
        libgb.GrB_Vector_apply(C, mask, accum, op, A, desc)
    elseif C isa GBMatrix && A isa GBMatrix
        libgb.GrB_Matrix_apply(C, mask, accum, op, A, desc)
    end
    return C
end

Base.map!(op::Function, C::GBArray, A::GBArray; mask = nothing, accum = nothing, desc = nothing) =
    map!(UnaryOp(op), C, A; mask, accum, desc)

function Base.map!(
    op::UnaryUnion, A::GBArray;
    mask = nothing, accum = nothing, desc = nothing
)
    return map!(op, A, A; mask, accum, desc)
end

Base.map!(op::Function, A::GBArray; mask = nothing, accum = nothing, desc = nothing) =
    map!(UnaryOp(op), A; mask, accum, desc)

function Base.map(
    op::UnaryUnion, A::GBArray;
    mask = nothing, accum = nothing, desc = nothing
)
    t = inferoutputtype(A, op)
    return map!(op, similar(A, t), A; mask, accum, desc)
end
Base.map(op::Function, A::GBArray; mask = nothing, accum = nothing, desc = nothing) =
    map(UnaryOp(op), A; mask, accum, desc)

function Base.map!(
    op::BinaryUnion, C::GBArray, x, A::GBArray;
    mask = nothing, accum = nothing, desc = nothing
)
    mask, accum = _handlenothings(mask, accum)
    desc === nothing && (desc = DEFAULTDESC)
    op = getoperator(op, optype(eltype(A), typeof(x)))
    accum = getaccum(accum, eltype(C))
    _, desc, A = _handletranspose(nothing, desc, A)
    if C isa GBVector && A isa GBVector
        libgb.scalarvecapply1st[optype(typeof(x), eltype(A))](C, mask, accum, op, x, A, desc)
    elseif C isa GBMatrix && A isa GBMatrix
        libgb.scalarmatapply1st[optype(typeof(x), eltype(A))](C, mask, accum, op, x, A, desc)
    end
    return C
end

function Base.map!(
    op::Function, C::GBArray, x, A::GBArray;
    mask = nothing, accum = nothing, desc = nothing
)
    map!(BinaryOps.BinaryOp(op), C, x, A; mask, accum, desc)
end

function Base.map!(
    op::BinaryUnion, x, A::GBArray;
    mask = nothing, accum = nothing, desc = nothing
)
    return map!(op, A, x, A; mask, accum, desc)
end

function Base.map!(
    op::Function, x, A::GBArray;
    mask = nothing, accum = nothing, desc = nothing
)
    map!(BinaryOps.BinaryOp(op), x, A; mask, accum, desc)
end

function Base.map(
    op::BinaryUnion, x, A::GBArray;
    mask = nothing, accum = nothing, desc = nothing
)
    t = inferoutputtype(A, op)
    return map!(op, similar(A, t), x, A; mask, accum, desc)
end

function Base.map(
    op::Function, x, A::GBArray;
    mask = nothing, accum = nothing, desc = nothing
)
    map(BinaryOps.BinaryOp(op), x, A; mask, accum, desc)
end

function Base.map!(
    op::BinaryUnion, C::GBArray, A::GBArray, x;
    mask = nothing, accum = nothing, desc = nothing
)
    mask, accum = _handlenothings(mask, accum)
    desc === nothing && (desc = DEFAULTDESC)
    op = getoperator(op, optype(eltype(A), typeof(x)))
    accum = getaccum(accum, eltype(C))
    A, desc, _ = _handletranspose(A, desc)
    if C isa GBVector && A isa GBVector
        libgb.scalarvecapply2nd[optype(typeof(x), eltype(A))](C, mask, accum, op, A, x, desc)
    elseif C isa GBMatrix && A isa GBMatrix
        libgb.scalarmatapply2nd[optype(typeof(x), eltype(A))](C, mask, accum, op, A, x, desc)
    end
    return C
end

function Base.map!(
    op::Function, C::GBArray, A::GBArray, x;
    mask = nothing, accum = nothing, desc = nothing
)
    map!(BinaryOps.BinaryOp(op), C, A, x; mask, accum, desc)
end

function Base.map!(
    op::BinaryUnion, A::GBArray, x;
    mask = nothing, accum = nothing, desc = nothing
)
    return map!(op, A, A, x; mask, accum, desc)
end

function Base.map!(
    op::Function, A::GBArray, x;
    mask = nothing, accum = nothing, desc = nothing
)
    map!(BinaryOps.BinaryOp(op), A, A, x; mask, accum, desc)
end

function Base.map(
    op::BinaryUnion, A::GBArray, x;
    mask = nothing, accum = nothing, desc = nothing
)
    t = inferoutputtype(A, op)
    return map!(op, similar(A, t), A, x; mask, accum, desc)
end

function Base.map(
    op::Function, A::GBArray, x;
    mask = nothing, accum = nothing, desc = nothing
)
    map(BinaryOps.BinaryOp(op), A, x; mask, accum, desc)
end

Base.:+(x::valid_union, u::GBArray; mask = nothing, accum = nothing, desc = nothing) =
    map(BinaryOps.PLUS, x, u; mask, accum, desc)
Base.:+(u::GBArray, x::valid_union; mask = nothing, accum = nothing, desc = nothing) =
    map(BinaryOps.PLUS, u, x; mask, accum, desc)

Base.:*(x::valid_union, u::GBArray; mask = nothing, accum = nothing, desc = nothing) =
    map(BinaryOps.TIMES, x, u; mask, accum, desc)
Base.:*(u::GBArray, x::valid_union; mask = nothing, accum = nothing, desc = nothing) =
    map(BinaryOps.TIMES, u, x; mask, accum, desc)

Base.:-(x::valid_union, u::GBArray; mask = nothing, accum = nothing, desc = nothing) =
    map(BinaryOps.MINUS, x, u; mask, accum, desc)
Base.:-(u::GBArray, x::valid_union; mask = nothing, accum = nothing, desc = nothing) =
    map(BinaryOps.MINUS, u, x; mask, accum, desc)

"""
    map(op::UnaryOp, A::GBArray; kwargs...)::GBArray
    map(op::BinaryOp, A::GBArray, x; kwargs...)::GBArray
    map(op::BinaryOp, x, A::GBArray, kwargs...)::GBArray

Transform a GBArray by applying `op` to each element.

UnaryOps apply elementwise in the usual fashion.
BinaryOps require the additional argument `x` which is substituted as the first or second
argument of `op` depending on its position.

# Arguments
- `op::MonoidBinaryOrRig = BinaryOps.PLUS`: the binary operation which is applied such that
    `C[i,j] = op(A[i,j], B[i,j])` for all `i,j` present in either `A` and/or `B`.
- `A::GBArray`: `GBVector` or optionally transposed `GBMatrix`.
- `x`: Position dependent argument to binary operators.

# Keywords
- `mask::Union{Nothing, GBMatrix} = nothing`: optional mask.
- `accum::Union{Nothing, AbstractBinaryOp} = nothing`: binary accumulator operation
    where `C[i,j] = accum(C[i,j], T[i,j])` where T is the result of this function before accum is applied.
- `desc = nothing`
"""
function Base.map(
    op::AbstractOp, A::GBArray, x = nothing;
    mask = nothing, accum = nothing, desc = nothing
)
end

"""
    mask!(C::GBArray, A::GBArray, mask::GBArray)

Apply a mask to matrix `A`, storing the results in C.

"""
function mask!(C::GBArray, A::GBArray, mask::GBArray; structural = false, complement = false)
    desc = DEFAULTDESC
    structural && (desc = desc + S)
    complement && (desc = desc + C)
    map!(identity, C, A; mask, desc)
    return C
end

"""
    mask(A::GBArray, mask::GBArray)

Apply a mask to matrix `A`.
"""
function mask(A::GBArray, mask::GBArray; structural = false, complement = false)
    return mask!(similar(A), A, mask; structural, complement)
end


#=
pfile.jl
read GE MRI k-space data from .p file
based on https://gitlab.com/fMRI/toppe/+toppe/+utils/loadpfile.m
=#

export loadpfile

# using MIRT: read_rdb_hdr

"""
`(dat, rdb_hdr) = loadpfile(pfile ; ...)`

Load data for one or more echoes from GE MRI scan Pfile.

in
- `pfile::String` filename

option
- `coils::AbstractVector{<:Integer}`
	only get data for these coils; default: all coils
- `echoes::AbstractVector{<:Integer}`
	only get data for these echoes; default: all echoes
- `slices::AbstractVector{<:Integer}`
	only get data for these slices; default: `2:nslices` (NB!)
	because first slice (dabslice=0 slot) may contain corrupt data.
- `views::AbstractVector{<:Integer}`
	only get data for these views; default: all views
- `quiet::Bool`	non-verbosity, default `false`

out
- `dat::Array{Complex{Int16}}` `[ndat, ncoil, nslice, necho, nview]`
- `rdb_hdr::NamedTuple`	header information

Note that to save memory the output type is complex-valued Int16.

Comments from Matlab version at
https://gitlab.com/fMRI/toppe/+toppe/+utils/loadpfile.m

This file was derived from part of the TOPPE development environment
for platform-independent MR pulse programming.

TOPPE is free software: you can redistribute it and/or modify
it under the terms of the GNU Library General Public License as published by
the Free Software Foundation version 2.0 of the License.

TOPPE is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
See the GNU Library General Public License for more details.

You should have received a copy of the GNU Library General Public License
along with TOPPE.
If not, see <http://www.gnu.org/licenses/old-licenses/lgpl-2.0.html>.

(c) 2016 The Regents of the University of Michigan
Jon-Fredrik Nielsen, jfnielse@umich.edu

2019-05-22 Julia version by Jeff Fessler
"""
function loadpfile(pfile::String ;
		coils::AbstractVector{<:Integer} = empty([], Integer),
		echoes::AbstractVector{<:Integer} = empty([], Integer),
		slices::AbstractVector{<:Integer} = empty([], Integer),
		views::AbstractVector{<:Integer} = empty([], Integer),
		quiet::Bool = false,
	)

	fid = open(pfile, "r") # open pfile
	rdb_hdr = read_rdb_hdr(fid) # read header

	# Header parameters
	ndat = rdb_hdr.frame_size
	nslices = rdb_hdr.nslices
	ptsize = rdb_hdr.point_size # 2: data stored in short int format, int16)
	if ptsize != 2 # 4 (extended precision) unsupported here
		throw("ptsize = $ptsize unsupported")
	end
	nechoes = rdb_hdr.nechoes
	nviews = rdb_hdr.nframes
	ncoils = rdb_hdr.dab[2] - rdb_hdr.dab[1] + 1

	# Calculate size of data chunks
	# See pfilestruct.jpg, and rhrawsize calculation in .e file.
	# number of data points per 'echo' loaddab slot. Includes baseline (0) view:
	echores  = ndat * (nviews+1)
	sliceres = nechoes * echores # number of data points per 'slice'
	coilres  = nslices * sliceres # number of data points per receive coil

	# This size should match the Pfile size exactly:
	pfilesize = rdb_hdr.off_data +
		2 * ptsize * ncoils * nslices * nechoes * (nviews+1) * ndat

	# File size check
	if pfilesize != filesize(pfile)
		throw("Expected $(pfilesize/1e6) MB but see $(filesize(pfile)/1e6) MB file")
	#	fprintf('Press enter to continue anyway...');
	#	input('');
	end

	# Check if second view is empty
	# This happens when there's only one view, but the scanner sets nviews = 2
	if nviews == 2
		# Seek to view slice 2, echo 1, view 2, coil 1:
		seek(fid, rdb_hdr.off_data + (2 * ptsize * (sliceres + 2*ndat)))
		view2tmp = Array{Int16}(undef, 2*ndat)
		view2tmp = read!(fid, view2tmp)
		if all(view2tmp .== Int16(0))
			nviews = 1 # set nviews to 1 so we don't read in all the empty data
			!quiet && @warn("View 2 appears to be empty, only loading view 1.")
		end
	end

	# Determine which portions to load
	# NB: default is to skip first slice (sometimes contains corrupted data)
	isempty(coils) && (coils = 1:ncoils)
	isempty(echoes) && (echoes = 1:nechoes)
	isempty(slices) && (slices = 2:nslices)
	isempty(views) && (views = 1:nviews)

	# output dimensions:
	dims = (ndat, length(coils), length(slices), length(echoes), length(views))

	# Warn if output size will exceed system memory.
	# Exceeding free RAM may cause a system freeze due to attempted mem swap
	memneeded = 4 * prod(dims) # 4 bytes per complex value because Int16
	memfree = Sys.total_memory() # system total memory
	mempercentuse = 100 * memneeded / memfree
	if mempercentuse > 100
		@warn("Loading data ($(memneeded/1e9) GB) may exceed available RAM")
		@warn("This could freeze computer!")
	#	fprintf('Press enter to continue anyway...');
	#	input('');
	elseif mempercentuse > 90 # Warn if we will we use 90% of memory
		@warn("Loading data ($(memneeded/1e9) GB) will use $mempercentuse % of your available RAM. Proceed with caution!")
	end

	if !quiet
		@info("ndat = $ndat, memory = $(memneeded/1e9) GB")
		@info("slices: $(slices[1])-$(slices[end]) / $nslices")
		@info("echoes: $(echoes[1])-$(echoes[end]) / $nechoes")
		@info("views: $(views[1])-$(views[end]) / $nviews")
		@info("coils: $(coils[1])-$(coils[end]) / $ncoils")
	end

	# Read data from file
	# Julia stores complex data as real1,imag1, real2,imag2, ... unlike matlab!
	# This simplifies the IO here!
	data = Array{Complex{Int16}}(undef, dims)
	dtmp = Array{Complex{Int16}}(undef, ndat) # one readout
#	!quiet && textprogressbar('Loading data: ')
	for icoil = coils
		@show icoil
	#	!quiet && textprogressbar(icoil/ncoils*100)
		for islice = slices
			for iecho = echoes
				for iview = views
					offsetres = (icoil-1)*coilres + (islice-1)*sliceres + (iecho-1)*echores + iview*ndat
					offsetbytes = 2 * ptsize * offsetres
					seek(fid, rdb_hdr.off_data + offsetbytes)
					read!(fid, dtmp)
					data[:, icoil-coils[1]+1, islice-slices[1]+1,
						iecho-echoes[1]+1, iview-views[1]+1] .= dtmp
				end
			end
		end
	end

	close(fid)
#	!quiet && textprogressbar(' done.')

	return (data, rdb_hdr)
end

"""
`loadpfile(file, echo::Integer ; ...)`

load a single echo
"""
function loadpfile(pfile::String, echo::Integer ; kwarg...)
	return loadpfile(pfile, echoes=[echo], kwarg...)
end

"""
`loadpfile(:test)`

does nothing now because tests require huge data files
"""
function loadpfile(test::Symbol)
	true
end


module Dungalyze

# package code goes here
# using StaticArrays, Dierckx, CSV, DataFrames, MAT, MATLAB, CoordinateTransformations, Plots, Unitful, Rematch, Distributions, Missings
# include(joinpath(Pkg.dir("Dungalyze"), "src", "calibrations.jl"))
# include(joinpath(Pkg.dir("Dungalyze"), "src", "turning_point.jl"))

using LinearAlgebra, SparseArrays
include("/home/yakir/.juliaold/v0.6/Dungalyze/src/types.jl")
include("/home/yakir/.juliaold/v0.6/Dungalyze/src/plots.jl")
include("/home/yakir/.juliaold/v0.6/Dungalyze/src/checks.jl")

# include(joinpath(Pkg.dir("Dungalyze"), "src", "types.jl"))
# include(joinpath(Pkg.dir("Dungalyze"), "src", "plots.jl"))
# include(joinpath(Pkg.dir("Dungalyze"), "src", "checks.jl"))

home = "/home/yakir/google-drive/.shared/Coffee beetle experiments"
figfolder = "dungalyze figures"
if !isdir(figfolder)
    mkdir(figfolder)
end

const CHECK = true
for experiment in readdir(home)
    # if !r"elin"i(experiment)
        path = joinpath(home, experiment)
        a = Experiment(path)
        plot(a)
    # end
end

#=df = DataFrame(Experiment = "", Run = "", Feeder_y = 0.0, Turning_point_x = 0.0, Turning_point_y = 0.0, Pellet_x = 0.0, Pellet_y = 0.0, Track_end_x = 0.0, Track_end_y = 0.0, ORG_nest_x = 0.0, ORG_nest_y = 0.0, Gravity_turn2pellet_x = 0.0, Gravity_turn2pellet_y = 0.0, Gravity_pellet2end_x = 0.0, Gravity_pellet2end_y = 0.0, Gravity_x = 0.0, Gravity_y = 0.0, Dist2turn = 0.0, Dis2pellet = 0.0, Dis2end = 0.0, Dis2org_nest = 0.0, Dis2grav_turn2pellet = 0.0, Dis2grav_pellet2end = 0.0, Dis2gravity = 0.0)
allowmissing!(df)
function doall(experiment::String; bad::Vector{String} = String[])
    _, _experiment = splitdir(experiment)
    mkdir(_experiment)
    cd(_experiment)
    ds = Final[]
    runs = get_runs(experiment, bad=bad)
    for r in runs
        d = Final(r)
        push!(df, row(d))
        push!(ds, d)
    end
    plot(ds)
    cd("..")
end
for experiment in readdir(home)
    bad = problems[problems[:Experiment] .== experiment, :Run]
    doall(joinpath(home, experiment), bad = bad)
end
deleterows!(df, 1)
CSV.write("all.csv", df)

experiment = "/home/yakir/google-drive/.shared/Coffee beetle experiments/Transfer pellet moved Therese"

for experiment in readdir(home)
    a = Experiment(joinpath(home, experiment))
    plot(a)
end

for experiment in readdir(home)
    if r"elin"i(experiment)
        f = joinpath(home, experiment)
        # println(f)
        a = Experiment(f)
        plot(a)
    end
end

runs = get_runs(experiment)
experiment = get_experiment(runs[1])
ds = Final{typeof(experiment), <:FinalPellet}[]

for r in runs
    d = Final(r)
    push!(ds, d)
end

plot(ds)

=##=function getpois(path::String)
    b = CSV.read(joinpath(path, poi_filename), header=["File", "Number", "POI"], types=[String, Int, String])
    b[:POI] .= strip.(b[:POI])
    pois = Dict{String, Dict{String, Int}}()
    by(b, :File) do df
        pois[df[1, :File]] = Dict(df[i,:POI] => df[i,:Number] for i in 1:nrow(df))
        end
        return pois
    end=##=
    =##=function extract_raw!(data::Dict{Symbol,Tuple{Matrix{Float64},Vector{Float64}}}, pois::Dict{String, Dict{String, Int}}, path::String, file::String)
        v = pois[file]
        matopen(joinpath(path, file), "r") do i
            X, Y, T, tmp = read(i, "xdata", "ydata", "tdata", "status")
            fr = tmp["FrameRate"]
            for (poi, column) in v
                data[Symbol(poi)] = _extract_raw(X[:, column], Y[:, column], T[:, column], fr)
            end
        end
    end=##=
    function doall(experiment::String; bad::Vector{String} = String[])
        runs = get_runs(experiment, bad=bad)
        ds = Final[]
        for r in runs
            a = sort_experiment(r)
            # d = Final(a)
            b = Calibrated(a)
            # c = Standardized(b)
            d = get_final(b)
            # p1 = plot(d)
            # p2 = plotsmooth(c, d)
            m = match(r"/.shared/(.*)$", r)
            path = joinpath("/home/yakir/tmp", m.captures[1])
            # mkpath(path)
            # savefig(p1, joinpath(path, "summary.pdf"))
            # savefig(p2, joinpath(path, "smooth.pdf"))
            push!(ds, d)
        end
        =##=if length(ds) > 1
            p = plot(ds)
            m = match(r"/.shared/(.*)$", experiment)
            path = joinpath("/home/yakir/tmp", m.captures[1])
            savefig(p, joinpath(path, "summary.pdf"))
        end=##=
    end

    # end # module

    home = "/home/yakir/google-drive/.shared/Coffee beetle experiments"
    a = Dict("Awayd_closed_nest_Belén" => ["64"],
             "Closed nest_Belén" => ["24"],
             "Daway_Belén" => [""],
             "Dright_Belén" => [""],
             "Dleft_closednest_Belén" => [""],
             "Transfer_Belén" => [""])
    for (experiment, bad) in a
        println(experiment)
        doall(joinpath(home, experiment), bad = bad)
    end

    doall.(joinpath.(home, readdir(home)))

    doall(joinpath(home, "Transfer pellet moved Therese"))

    experiments = filter(x -> !(r"^Transfer pellet moved Therese$"(x)), readdir(home))
    doall.(joinpath.(home, experiments))

    home = "/home/yakir/google-drive/.shared/Coffee beetle experiments"
    doall(joinpath(home, "Transfer_Belén"))

    home = "/home/yakir/google-drive/.shared/Coffee beetle experiments"
    doall(joinpath(home, "Transfer nest Therese"))

    experiments = filter(r"Bel[e,é]n", readdir(home))
    doall.(joinpath.(home, experiments))

    experiments = filter(r"therese"i, readdir(home))
    doall.(joinpath.(home, experiments))

    experiment = joinpath(home, "Closed nest_Belén")

    home = "/home/yakir/google-drive/.shared/Coffee beetle experiments"
    experiment = joinpath(home, filter(r"therese"i, readdir(home))[6])
    runs = get_runs(experiment)
    r = runs[1]
    a = sort_experiment(r)

    b = Calibrated(a)
    d = Final(b)

    home = "/home/yakir/google-drive/.shared/Coffee beetle experiments"
    include(joinpath(Pkg.dir("Dungalyze"), "src", "checks.jl"))

    for experiment in readdir(home)
        bad = problems[problems[:Experiment] .== experiment, :Run]
        doall(joinpath(home, experiment), bad = bad)
    end

    home = "/home/yakir/google-drive/.shared/Coffee beetle experiments"
    experiment = joinpath(home, "Closed nest Therese")
    bad = [""]

    experiment_path = joinpath(home, experiment)
    all_runs = get_runs(experiment_path)
    bad_runs = joinpath.(experiment_path, problems[problems[:Experiment] .== experiment, :Run])
    runs = setdiff(all_runs, bad_runs)
    for r in runs
        a = sort_experiment(r)
        b = Calibrated(a)
        d = Final(b)=#

end # module


"""
    struct ParamNamesUnit{N1,N2}
        var::NTuple{N1,Symbol}
        var_aux::Vector{Tuple{Vararg{Symbol,_N} where _N}}
        updt::NTuple{N2,Pair{Int64,Symbol}}
        updt_aux::Vector{Tuple{Vararg{Pair{Int64,Symbol},_N} where _N}}
        updt_obs::Vector{Tuple{Vararg{Pair{Int64,Int64},_N} where _N}}
    end

Smallest unit storing information about the names of the parameters that are
supposed to be updated at a given step of the MCMC algorithm. It is pertinent
to a single block of a single recording only, and in particular, only to a
single collection of laws, out of:
- `PP`, i.e. those relevant to a given block
- `P_last`, i.e. the artificial law of the block associated with the last obs
- `P_excl`, i.e. the last, missed out `PP`, missed out due to use of `P_last`
- `PPb`, i.e. the collection of all remaining artificial laws that are not
         `P_last`
It stores lists of parameters that are supposed to be updated at various stages
of the call to a function `set_proposal_law!`, which sets the proposal parameter
`θ°` inside the proposal laws.

# Fields
- `var`: names of `P_target` that are checked and—if need be—equalized between
         proposal and accepted laws
- `var_aux`: names of `P_aux` that are checked and—if need be—equalized between
             proposal and accepted laws
- `updt`: list of params of `P_target` that are being updated at a given MCMC
          step; listed as pairs: `idx-of-θ-to-relevant-value` =>
          `name-of-param-inside-P_target-struct`
- `updt_aux`: as above, but for `P_aux`
- `updt_obs`: list of params of `obs` (i.e. terminal observation for each guid
              prop) that are being updated at a given MCMC step; listed as
              pairs: `idx-of-θ-to-relevant-value` => `idx-of-obs-inside-obs.θ`

    ParamNamesUnit(
        PP::AbstractArray{<:GuidProp}, θnames::Vector{Symbol}, pdep, odeps
    )

Base constructor.

# Arguments
- `PP`:
- `θnames`:
- `pdep`:
- `odeps`:
"""
struct ParamNamesUnit{N1,N2}
    var::NTuple{N1,Symbol}
    var_aux::Vector{Tuple{Vararg{Symbol,_N} where _N}}
    updt::NTuple{N2,Pair{Int64,Symbol}}
    updt_aux::Vector{Tuple{Vararg{Pair{Int64,Symbol},_N} where _N}}
    updt_obs::Vector{Tuple{Vararg{Pair{Int64,Int64},_N} where _N}}

    function ParamNamesUnit(
            PP::AbstractArray{<:GuidProp},
            θnames::Vector{Symbol},
            pdep,
            odeps,
        )
        updt = find_θ_names_for_MCMC_update(θnames, pdep)
        updt_aux = find_θ_aux_names_for_MCMC_update(updt, PP)
        updt_obs = find_θ_obs_idx_for_MCMC_update(θnames, odeps)

        var = find_var_names_not_in_MCMC_update(updt, PP)
        var_aux = find_var_aux_names_not_in_MCMC_update(updt_aux, PP)

        new{length(var), length(updt)}(var, var_aux, updt, updt_aux, updt_obs)
    end

    RecordingParamNames() = new{0,0}()
end

tuple_lengths(::ParamNamesUnit{N1,N2}) where {N1,N2} = (N1, N2)

"""
    find_θ_names_for_MCMC_update(θnames, pdep)

Parse through `θnames` i.e. a list of all parameter names that are relevant for
a given MCMC update and pick out only those that are relevant for a given
diffusion law. All information about the relevant parameters of the law should
be stored inside `pdep`. Return a list of relevant parameter names in a format:
`idx-of-θ-to-relevant-value` => `name-of-param-inside-P_target-struct`.
"""
find_θ_names_for_MCMC_update(θnames, pdep) = Tuple(
    map(
        gl_p_n->(
            findfirst(
                n->(n==gl_p_n[1]),
                θnames
            ) => gl_p_n[2]
        ), # substitute a global param name (gl_p_n[1]) with its index in θnames
        filter(
            p->(p[1] in θnames),
            pdep
        ) # remove all parameter names not featured in θnames
    )
)

"""
    find_θ_aux_names_for_MCMC_update(θ_names_for_MCMC_update, PP)

Parse through `PP` and for each `GuidProp` struct enter the auxiliary law
`P_aux` and pick out names from `θ_names_for_MCMC_update` that are relevant to
this `P_aux` law.
"""
find_θ_aux_names_for_MCMC_update(θ_names_for_MCMC_update, PP) = map(PP) do P
    filter(
        p->(p[2] in DD.var_parameter_names(P.P_aux)),
        θ_names_for_MCMC_update
    )
end

"""
    find_θ_obs_idx_for_MCMC_update(θnames, odeps)

Parse through `θnames` i.e. a list of all parameter names that are relevant for
a given MCMC update and for each observation pick out only those that are
relevant for it. All information about the relevant parameters of the
observations should be stored inside `odeps`. Return a list of relevant
parameter names in a format: `idx-of-θ-to-relevant-value` =>
`idx-of-obs-inside-obs.θ`.
"""
find_θ_obs_idx_for_MCMC_update(θnames, odeps) = map(
    odep->Tuple(
        map(
            gl_p_n->(
                findfirst(
                    n->(n==gl_p_n[1]),
                    θnames
                ) => gl_p_n[2]
            ), # substitute a global parameter name with its index in θnames
            filter(
                p->(p[1] in θnames),
                odep
            ) # remove all observation params not featured in θnames
        )
    ),
    odeps
)

"""
    find_var_names_not_in_MCMC_update(composite_names_in_updt, PP)

Parse through all variable names of `P_target` and remove all those names that
already feature in `composite_names_in_updt`.
"""
function find_var_names_not_in_MCMC_update(composite_names_in_updt, PP)
    names_in_updt = getindex.(composite_names_in_updt, 2)
    length(PP) > 0 || return tuple()
    filter(pn->!(pn in names_in_updt), DD.var_parameter_names(PP))
end

"""
    find_var_aux_names_not_in_MCMC_update(composite_names_in_updt, PP)

Parse through all `PP` and for each element look through variable names of
`P_aux` and remove all those names that already feature in
`composite_names_in_updt`.
"""
function find_var_aux_names_not_in_MCMC_update(composite_names_in_updt, PP)
    names_in_updt = map(n->getindex.(n, 2), composite_names_in_updt)
    #println(names_in_updt)
    [
        filter(
            pn->!(pn in n),
            DD.var_parameter_names(P.P_aux)
        ) for (n, P) in zip(names_in_updt, PP)
    ]
end

"""
    struct ParamNamesBlock{N1,N2}
        PP::ParamNamesUnit{N1,N2}
        P_last::ParamNamesUnit{N1,N2}
        P_excl::ParamNamesUnit{N1,N2}
        Pb_excl::ParamNamesUnit{N1,N2}
    end

Stores information relevant for a `Block` about the names of the parameters that
are supposed to be updated at a given step of the MCMC algorithm. It stores
lists of parameters that are supposed to be updated at various stages of the
call to a function `set_proposal_law!`, which sets the proposal parameter `θ°`
inside the proposal laws.

# Fields
- `PP`, i.e. those relevant to a given block
- `P_last`, i.e. the artificial law of the block associated with the last obs
- `P_excl`, i.e. the last, missed out `PP`, missed out due to use of `P_last`
- `Pb_excl`, i.e. the collection of all remaining artificial laws that are not
             `P_last`

    ParamNamesBlock(b::Block, θnames, pdep, odeps)

Base constructor.

# Arguments
- `b::Block`:
- `θnames`:
- `pdep`:
- `odeps`:
"""
struct ParamNamesBlock{N1,N2}
    PP::ParamNamesUnit{N1,N2}
    P_last::ParamNamesUnit{N1,N2}
    P_excl::ParamNamesUnit{N1,N2}
    Pb_excl::ParamNamesUnit{N1,N2}

    function ParamNamesBlock(b::Block, θnames, pdep, odeps)
        i1, i2 = _idx_split(b)

        PP = ParamNamesUnit(b.PP, θnames, pdep, odeps[i1])
        P_last = ParamNamesUnit(b.P_last, θnames, pdep, [tuple() for _ in i2])
        P_excl = ParamNamesUnit(b.P_excl, θnames, pdep, odeps[i2])
        Pb_excl = ParamNamesUnit(b.Pb_excl, θnames, pdep, [tuple() for _ in i1])
        N1, N2 = tuple_lengths(PP)
        new{N1,N2}(PP, P_last, P_excl, Pb_excl)
    end
end

function _idx_split(b::Block{false})
    bidx = first(b.PP.indices) # PP is a subarray
    bidx, [bidx[end]+1]
end

function _idx_split(b::Block{true})
    bidx = first(b.PP.indices) # PP is a subarray
    bidx, Int64[]
end

tuple_lengths(::ParamNamesBlock{N1,N2}) where {N1,N2} = (N1, N2)

"""
    struct ParamNamesRecording{N1,N2}
        blocks::Vector{ParamNamesBlock{N1,N2}}
    end

Stores information relevant for an entire, single recording about the names of
the parameters that are supposed to be updated at a given step of the MCMC
algorithm. It stores lists of parameters that are supposed to be updated at
various stages of the call to a function `set_proposal_law!`, which sets the
proposal parameter `θ°` inside the proposal laws.

    ParamNamesRecording(rb::BlockCollection, θnames, pdep, odeps)

Base constructor.
"""
struct ParamNamesRecording{N1,N2}
    blocks::Vector{ParamNamesBlock{N1,N2}}

    function ParamNamesRecording(bc::BlockCollection, θnames, pdep, odeps)
        blocks = map(bb->ParamNamesBlock(bb.b, θnames, pdep, odeps), bc.blocks)
        N1, N2 = tuple_lengths(first(blocks))
        new{N1,N2}(blocks)
    end
end

"""
    struct ParamNamesAllObs{T}
        recordings::Vector{T}
    end

Stores information relevant for multiple recordings about the names of the
parameters that are supposed to be updated at a given step of the MCMC
algorithm. It stores lists of parameters that are supposed to be updated at
various stages of the call to a function `set_proposal_law!`, which sets the
proposal parameter `θ°` inside the proposal laws.

    ParamNamesAllObs(be::BlockEnsemble, θnames, all_obs)

Base constructor.
"""
struct ParamNamesAllObs{T}
    recordings::Vector{T}

    function ParamNamesAllObs(be::BlockEnsemble, θnames, all_obs)
        recordings = map(1:length(be.recordings)) do i
            ParamNamesRecording(
                be.recordings[i],
                θnames,
                all_obs.param_depend_rev[i],
                all_obs.obs_depend_rev[i]
            )
        end
        new{eltype(recordings)}(recordings)
    end
end


module XNAColors

macro colornames(s::Symbol, expr::Expr)
    return Expr(:block, esc(:($s = $expr)), [esc(:($(Meta.parse(k)) = $v)) for (k, v) in eval(expr)]...)
end

@colornames(colors, Dict{String, Tuple}(
    "Transparent" => (0, 0, 0, 0) ./ 255,
    "AliceBlue" => (240, 248, 255, 255) ./ 255,
    "AntiqueWhite" => (250, 235, 215, 255) ./ 255,
    "Aqua" => (0, 255, 255, 255) ./ 255,
    "Aquamarine" => (127, 255, 212, 255) ./ 255,
    "Azure" => (240, 255, 255, 255) ./ 255,
    "Beige" => (245, 245, 220, 255) ./ 255,
    "Bisque" => (255, 228, 196, 255) ./ 255,
    "Black" => (0, 0, 0, 255) ./ 255,
    "BlanchedAlmond" => (255, 235, 205, 255) ./ 255,
    "Blue" => (0, 0, 255, 255) ./ 255,
    "BlueViolet" => (138, 43, 226, 255) ./ 255,
    "Brown" => (165, 42, 42, 255) ./ 255,
    "BurlyWood" => (222, 184, 135, 255) ./ 255,
    "CadetBlue" => (95, 158, 160, 255) ./ 255,
    "Chartreuse" => (127, 255, 0, 255) ./ 255,
    "Chocolate" => (210, 105, 30, 255) ./ 255,
    "Coral" => (255, 127, 80, 255) ./ 255,
    "CornflowerBlue" => (100, 149, 237, 255) ./ 255,
    "Cornsilk" => (255, 248, 220, 255) ./ 255,
    "Crimson" => (220, 20, 60, 255) ./ 255,
    "Cyan" => (0, 255, 255, 255) ./ 255,
    "DarkBlue" => (0, 0, 139, 255) ./ 255,
    "DarkCyan" => (0, 139, 139, 255) ./ 255,
    "DarkGoldenrod" => (184, 134, 11, 255) ./ 255,
    "DarkGray" => (169, 169, 169, 255) ./ 255,
    "DarkGreen" => (0, 100, 0, 255) ./ 255,
    "DarkKhaki" => (189, 183, 107, 255) ./ 255,
    "DarkMagenta" => (139, 0, 139, 255) ./ 255,
    "DarkOliveGreen" => (85, 107, 47, 255) ./ 255,
    "DarkOrange" => (255, 140, 0, 255) ./ 255,
    "DarkOrchid" => (153, 50, 204, 255) ./ 255,
    "DarkRed" => (139, 0, 0, 255) ./ 255,
    "DarkSalmon" => (233, 150, 122, 255) ./ 255,
    "DarkSeaGreen" => (143, 188, 139, 255) ./ 255,
    "DarkSlateBlue" => (72, 61, 139, 255) ./ 255,
    "DarkSlateGray" => (47, 79, 79, 255) ./ 255,
    "DarkTurquoise" => (0, 206, 209, 255) ./ 255,
    "DarkViolet" => (148, 0, 211, 255) ./ 255,
    "DeepPink" => (255, 20, 147, 255) ./ 255,
    "DeepSkyBlue" => (0, 191, 255, 255) ./ 255,
    "DimGray" => (105, 105, 105, 255) ./ 255,
    "DodgerBlue" => (30, 144, 255, 255) ./ 255,
    "Firebrick" => (178, 34, 34, 255) ./ 255,
    "FloralWhite" => (255, 250, 240, 255) ./ 255,
    "ForestGreen" => (34, 139, 34, 255) ./ 255,
    "Fuchsia" => (255, 0, 255, 255) ./ 255,
    "Gainsboro" => (220, 220, 220, 255) ./ 255,
    "GhostWhite" => (248, 248, 255, 255) ./ 255,
    "Gold" => (255, 215, 0, 255) ./ 255,
    "Goldenrod" => (218, 165, 32, 255) ./ 255,
    "Gray" => (128, 128, 128, 255) ./ 255,
    "Green" => (0, 128, 0, 255) ./ 255,
    "GreenYellow" => (173, 255, 47, 255) ./ 255,
    "Honeydew" => (240, 255, 240, 255) ./ 255,
    "HotPink" => (255, 105, 180, 255) ./ 255,
    "IndianRed" => (205, 92, 92, 255) ./ 255,
    "Indigo" => (75, 0, 130, 255) ./ 255,
    "Ivory" => (255, 255, 240, 255) ./ 255,
    "Khaki" => (240, 230, 140, 255) ./ 255,
    "Lavender" => (230, 230, 250, 255) ./ 255,
    "LavenderBlush" => (255, 240, 245, 255) ./ 255,
    "LawnGreen" => (124, 252, 0, 255) ./ 255,
    "LemonChiffon" => (255, 250, 205, 255) ./ 255,
    "LightBlue" => (173, 216, 230, 255) ./ 255,
    "LightCoral" => (240, 128, 128, 255) ./ 255,
    "LightCyan" => (224, 255, 255, 255) ./ 255,
    "LightGoldenrodYellow" => (250, 250, 210, 255) ./ 255,
    "LightGray" => (211, 211, 211, 255) ./ 255,
    "LightGreen" => (144, 238, 144, 255) ./ 255,
    "LightPink" => (255, 182, 193, 255) ./ 255,
    "LightSalmon" => (255, 160, 122, 255) ./ 255,
    "LightSeaGreen" => (32, 178, 170, 255) ./ 255,
    "LightSkyBlue" => (135, 206, 250, 255) ./ 255,
    "LightSlateGray" => (119, 136, 153, 255) ./ 255,
    "LightSteelBlue" => (176, 196, 222, 255) ./ 255,
    "LightYellow" => (255, 255, 224, 255) ./ 255,
    "Lime" => (0, 255, 0, 255) ./ 255,
    "LimeGreen" => (50, 205, 50, 255) ./ 255,
    "Linen" => (250, 240, 230, 255) ./ 255,
    "Magenta" => (255, 0, 255, 255) ./ 255,
    "Maroon" => (128, 0, 0, 255) ./ 255,
    "MediumAquamarine" => (102, 205, 170, 255) ./ 255,
    "MediumBlue" => (0, 0, 205, 255) ./ 255,
    "MediumOrchid" => (186, 85, 211, 255) ./ 255,
    "MediumPurple" => (147, 112, 219, 255) ./ 255,
    "MediumSeaGreen" => (60, 179, 113, 255) ./ 255,
    "MediumSlateBlue" => (123, 104, 238, 255) ./ 255,
    "MediumSpringGreen" => (0, 250, 154, 255) ./ 255,
    "MediumTurquoise" => (72, 209, 204, 255) ./ 255,
    "MediumVioletRed" => (199, 21, 133, 255) ./ 255,
    "MidnightBlue" => (25, 25, 112, 255) ./ 255,
    "MintCream" => (245, 255, 250, 255) ./ 255,
    "MistyRose" => (255, 228, 225, 255) ./ 255,
    "Moccasin" => (255, 228, 181, 255) ./ 255,
    "NavajoWhite" => (255, 222, 173, 255) ./ 255,
    "Navy" => (0, 0, 128, 255) ./ 255,
    "OldLace" => (253, 245, 230, 255) ./ 255,
    "Olive" => (128, 128, 0, 255) ./ 255,
    "OliveDrab" => (107, 142, 35, 255) ./ 255,
    "Orange" => (255, 165, 0, 255) ./ 255,
    "OrangeRed" => (255, 69, 0, 255) ./ 255,
    "Orchid" => (218, 112, 214, 255) ./ 255,
    "PaleGoldenrod" => (238, 232, 170, 255) ./ 255,
    "PaleGreen" => (152, 251, 152, 255) ./ 255,
    "PaleTurquoise" => (175, 238, 238, 255) ./ 255,
    "PaleVioletRed" => (219, 112, 147, 255) ./ 255,
    "PapayaWhip" => (255, 239, 213, 255) ./ 255,
    "PeachPuff" => (255, 218, 185, 255) ./ 255,
    "Peru" => (205, 133, 63, 255) ./ 255,
    "Pink" => (255, 192, 203, 255) ./ 255,
    "Plum" => (221, 160, 221, 255) ./ 255,
    "PowderBlue" => (176, 224, 230, 255) ./ 255,
    "Purple" => (128, 0, 128, 255) ./ 255,
    "Red" => (255, 0, 0, 255) ./ 255,
    "RosyBrown" => (188, 143, 143, 255) ./ 255,
    "RoyalBlue" => (65, 105, 225, 255) ./ 255,
    "SaddleBrown" => (139, 69, 19, 255) ./ 255,
    "Salmon" => (250, 128, 114, 255) ./ 255,
    "SandyBrown" => (244, 164, 96, 255) ./ 255,
    "SeaGreen" => (46, 139, 87, 255) ./ 255,
    "SeaShell" => (255, 245, 238, 255) ./ 255,
    "Sienna" => (160, 82, 45, 255) ./ 255,
    "Silver" => (192, 192, 192, 255) ./ 255,
    "SkyBlue" => (135, 206, 235, 255) ./ 255,
    "SlateBlue" => (106, 90, 205, 255) ./ 255,
    "SlateGray" => (112, 128, 144, 255) ./ 255,
    "Snow" => (255, 250, 250, 255) ./ 255,
    "SpringGreen" => (0, 255, 127, 255) ./ 255,
    "SteelBlue" => (70, 130, 180, 255) ./ 255,
    "Tan" => (210, 180, 140, 255) ./ 255,
    "Teal" => (0, 128, 128, 255) ./ 255,
    "Thistle" => (216, 191, 216, 255) ./ 255,
    "Tomato" => (255, 99, 71, 255) ./ 255,
    "Turquoise" => (64, 224, 208, 255) ./ 255,
    "Violet" => (238, 130, 238, 255) ./ 255,
    "Wheat" => (245, 222, 179, 255) ./ 255,
    "White" => (255, 255, 255, 255) ./ 255,
    "WhiteSmoke" => (245, 245, 245, 255) ./ 255,
    "Yellow" => (255, 255, 0, 255) ./ 255,
    "YellowGreen" => (154, 205, 50, 255) ./ 255,
))

end


_clamp(x) = Point2f0(clamp(x[1], 0, 1), clamp(x[2], 0, 1))
function widget{T<:RGBA}(
        color::Signal{T}, window;
        area=(30, 30),
        kw_args...
    )
    @materialize mouse_buttons_pressed, mouseposition = window.inputs
    color_button = visualize(
        (ROUNDED_RECTANGLE, zeros(Point2f0, 1));
        scale=const_lift(x->Vec2f0(x[2]), area),
        offset=Vec2f0(0),
        color=color,
        stroke_width=1f0,
        stroke_color=RGBA{Float32}(0.9, 0.9, 0.9, 1.0),
        kw_args...
    )
    color_robj = color_button.children[]
    m2id = GLWindow.mouse2id(window)
    isoverpoint = const_lift(is_same_id, m2id, color_robj)
    key_pressed = map(mouse_buttons_pressed) do mbuttons
        if length(mbuttons) == 1
            b = first(mbuttons)
            return GLFW.MOUSE_BUTTON_LEFT == b || b == GLFW.MOUSE_BUTTON_RIGHT
        end
        false
    end
    # dragg while key_pressed. Drag only starts if isoverpoint is true
    mousedragg = GLAbstraction.dragged(mouseposition, key_pressed, isoverpoint)

    preserve(foldp((value(m2id)..., value(color)), mousedragg) do v0, dragg
        id, index, p0 = v0
        if dragg == Vec2f0(0) # if drag just started. Not the best way, maybe dragged should return a tuple of (draggvalue, started)
            id, index = value(m2id)
            if id==color_robj.id
                p0 = value(color)
            end
        else
            if id==color_robj.id
                mbutton = first(value(mouse_buttons_pressed))
                if mbutton == GLFW.MOUSE_BUTTON_RIGHT # with the right button we control red and green
                    r, g = _clamp(Point2f0(p0.r, p0.g) + Point2f0(dragg/50f0))
                    push!(color, RGBA{eltype(T)}(r, g, p0.b, p0.alpha))
                elseif mbutton == GLFW.MOUSE_BUTTON_LEFT # with the left button we control blue and alpha
                    b, a = _clamp(Point2f0(p0.b, p0.alpha) + Point2f0(dragg/50f0))
                    push!(color, RGBA{eltype(T)}(p0.r, p0.g, b, a))
                end
            end
        end
        return id, index, p0
    end)
    return color, color_button
end


module ReverseDiffExpressionsBase

# using StackPointers, SIMDPirates, VectorizationBase, LinearAlgebra
using LoopVectorization, OffsetArrays, VectorizationBase, ChainRules, StackPointers
using LoopVectorization: second, third, fourth, fifth, sixth, seventh, eighth, ninth

# import SIMDPirates: vadd, vsum, vifelse

# export uninitialized, isinitialized, tadd

# export Zero, One

# Base.Broadcast.materialize to be used for instantiating lazy objects.
# @inline grad
function constrain end
function constrain_pullback end
function constrain_reverse! end
function logpdf end
function ∂logpdf! end

@inline StackPointers.stack_pointer_call(::typeof(ChainRules.rrule), sp::StackPointer, f::F, args::Vararg{<:Any,N}) where {F, N} = (sp, rrule(f, args...))

include("derivativerules.jl")
# include("initialized_variable_tracker.jl")
include("identities.jl")
# include("uninitialized_references.jl")
include("target.jl")
# include("seed_increments.jl")
# include("fallback.jl")

adj(out, a) = Symbol("##∂", out, "/∂", a, "##")
adj(a) = adj(:target, a)

# RESERVED_INCREMENT_SEED_RESERVED! ∂getindex uninitialized
# @def_stackpointer_fallback alloc_adjoint
function __init__()
    # @add_stackpointer_method alloc_adjoint
end

end # module


# See ?extrap_prep for documentation for all these methods

extrap_prep{T}(g::Val{:gradient}, ::Type{T}, n::Val{1}) = extrap_prep(g, T, n, Val{1}())
extrap_prep{T}(g::Val{:gradient}, ::Type{Tuple{T}}, n::Val{1}) = extrap_prep(g, T, n)
extrap_prep{T}(g::Val{:gradient}, ::Type{Tuple{T,T}}, n::Val{1}) = extrap_prep(g, T, n)
extrap_prep{T}(g::Val{:gradient}, ::Type{Tuple{Tuple{T,T}}}, n::Val{1}) = extrap_prep(g, T, n)
function extrap_prep{S,T}(g::Val{:gradient}, ::Type{Tuple{S,T}}, ::Val{1})
    quote
        $(extrap_prep(g, S, n, Val{1}(), Val{:lo}()))
        $(extrap_prep(g, T, n, Val{1}(), Val{:hi}()))
    end
end
extrap_prep{S,T}(g::Val{:gradient}, ::Type{Tuple{Tuple{S,T}}}, n::Val{1}) = extrap_prep(g, Tuple{S,T}, n)
# needed for ambiguity resolution
extrap_prep{T<:Tuple}(::Val{:gradient}, ::Type{T}, ::Val{1}) = :(throw(ArgumentError("The 1-dimensional extrap configuration $T is not supported")))

function extrap_prep{T,N}(g::Val{:gradient}, ::Type{T}, n::Val{N})
    Expr(:block, [extrap_prep(g, T, n, Val{d}()) for d in 1:N]...)
end

function extrap_prep{T<:Tuple,N}(g::Val{:gradient}, ::Type{T}, n::Val{N})
    length(T.parameters) == N || return :(throw(ArgumentError("The $N-dimensional extrap configuration $T is not supported")))
    exprs = Expr[]
    for d in 1:N
        Tdim = T.parameters[d]
        if Tdim <: Tuple
            length(Tdim.parameters) == 2 || return :(throw(ArgumentError("The extrap configuration $Tdim for dimension $d is not supported - must be a tuple of length 2 or a simple configuration type"))
                )
            if Tdim.parameters[1] != Tdim.parameters[2]
                push!(exprs, extrap_prep(g, Tdim, n, Val{d}()))
            else
                push!(exprs, extrap_prep(g, Tdim.parameters[1], n, Val{d}()))
            end
        else
            push!(exprs, extrap_prep(g, Tdim, n, Val{d}()))
        end
    end
    return Expr(:block, exprs...)
end

function extrap_prep{S,T,N,d}(g::Val{:gradient}, ::Type{Tuple{S,T}}, n::Val{N}, dim::Val{d})
    quote
        $(extrap_prep(g, S, n, dim, Val{:lo}()))
        $(extrap_prep(g, T, n, dim, Val{:hi}()))
    end
end

extrap_prep{T,N,d}(g::Val{:gradient}, ::Type{T}, n::Val{N}, dim::Val{d}) = extrap_prep(g, Tuple{T,T}, n, dim)
extrap_prep(g::Val{:gradient}, args...) = extrap_prep(args...)


module ASEPotential

using Parameters
using PyCall
using Unitful

export ASECalculatorParameters, DFTKCalculatorParameters, LAMMPSCalculatorParameters, get_potential_energy, get_forces
include("atoms.jl")
export atom, atoms, read_atoms, write_atoms
include("calculators.jl")

end


@enum GitMode mode_dir=0o040000 mode_normal=0o100644 mode_executable=0o100755 mode_symlink=0o120000 mode_submodule=0o160000
Base.string(mode::GitMode) = string(UInt32(mode); base=8)
Base.print(io::IO, mode::GitMode) = print(io, string(mode))

struct GitTreeEntry
    mode::GitMode
    name::String
    hash::SHA1
end

struct GitTree <: GitObject
    entries::Vector{GitTreeEntry}
end

Base.sizeof(entry::GitTreeEntry) =
    ndigits(UInt32(entry.mode); base=8) + 1 + sizeof(entry.name) + 1 + 20
Base.sizeof(tree::GitTree) = sum(sizeof, tree.entries)

function Base.read(io::IO, ::Type{GitTreeEntry})
    mode = GitMode(parse(UInt32,readuntil(io, ' '),base=8))
    name = readuntil(io,'\0')
    hash = read(io, SHA1)
    return GitTreeEntry(mode, name, hash)
end

function GitTree(data::Vector{UInt8})
    entries = GitTreeEntry[]
    io = IOBuffer(data)
    while bytesavailable(io) > 0
        entry = read(io, GitTreeEntry)
        push!(entries, entry)
    end
    GitTree(entries)
end

function oid(tree::GitTree)
    ctx = SHA.SHA1_CTX()
    SHA.update!(ctx, Vector{UInt8}("tree $(sizeof(tree))\0"))
    for entry in tree.entries
        SHA.update!(ctx, Vector{UInt8}("$(entry.mode) $(entry.name)\0"))
        SHA.update!(ctx, entry.hash.bytes)
    end
    return SHA1(SHA.digest!(ctx))
end

entryorder(entry::GitTreeEntry) = entry.mode == mode_dir ? entry.name*"/" : entry.name

"""
    GitTreeEntry(fpath[, name])

Construct a `GitTreeEntry` object based on `path`. If no `name` is provided, it is
assumed to be the `basename(path)`.
"""
function GitTreeEntry(path::AbstractString, name::AbstractString=basename(filepath))
    mode = isdir(path) ? mode_dir : mode_normal
    hash = oid(path)
    GitTreeEntry(mode, name, hash)
end

function GitTree(path::AbstractString, ignore::Glob.FilenameMatch=fn"")
    names = filter(name -> !occursin(ignore,name), readdir(path))
    entries = map(names) do name
        GitTreeEntry(joinpath(path, name), name)
    end
    sort!(entries, by=entryorder)
    GitTree(entries)
end

function oid(path::AbstractString, ignore::Glob.FilenameMatch=fn".git")
    if isdir(path)
        oid(GitTree(path, ignore))
    else
        oid(GitBlob(read(path)))
    end
end


# Set of standard plotting functions for Kayaks / SAS
# For mix and match purposes these functions return Gadfly layers except for default plot

function plotSASDefault(fg, expID::String, posData::Array, iGTtemp::Array, dposData::Array ; datadir::String=joinpath(ENV["HOME"],"data", "kayaks", "08_10_parsed") , savedir::String="/tmp/test.pdf")
    pltemp = [];
    push!(pltemp,plotBeaconGT(iGTtemp));
    push!(pltemp,plotBeaconMax(fg));
    push!(pltemp,plotBeaconMean(fg));
    push!(pltemp,plotBeaconHist(fg));
    push!(pltemp,Gadfly.Theme(key_position = :none));
    plotPriors!(pltemp,fg);
    plotKDEMaxs!(pltemp,fg);
    push!(pltemp,plotPath(posData));
    push!(pltemp,plotPath(dposData,colorIn=colorant"steelblue1"));
    plotBeaconContours!(pltemp,fg);

    if expID == "dock"
        push!(pltemp, Coord.cartesian(xmin=-40, xmax=140, ymin=-140, ymax=30,fixed=true))
    elseif expID == "drift"
        push!(pltemp, Coord.cartesian(xmin=20, xmax=200, ymin=-260, ymax=0,fixed=true))
    end
    # push!(pltemp,Gadfly.Guide.xlabel("X (m)"),Gadfly.Guide.ylabel("Y (m)"));
    pltempplot = Gadfly.plot(pltemp...); pltempplot |> PDF(savedir)
    return pltempplot
end

function plotPriors!(plHolderIn,fg)
    for mysym in ls(fg)
        for myfac in ls(fg,mysym)
            if occursin(r"^x\df",string(myfac))
                xData = getKDEMax(getBelief(fg,mysym))
                push!(plHolderIn,plotPoint(xData,colorIn=colorant"green2",orderIn=2))
            end
        end
    end
end

function pltoPDF(plHolderIn ; savedir::String="/tmp/test.pdf")
    Gadfly.plot(plHolderIn...) |> PDF(savedir)
end

function plotBeaconMax(fg)
    L1 = getKDEMax(getBelief(fg, :l1))
    return layer(x=[L1[1];],y=[L1[2];], label=String["Beacon Max";],Geom.point,Geom.label(hide_overlaps=false), order=2, Theme(default_color=colorant"darkorange",highlight_width = 0pt));
end

function plotBeaconMean(fg)
    L1 = getKDEMean(getBelief(fg, :l1))
    return layer(x=[L1[1];],y=[L1[2];], label=String["Beacon Mean";],Geom.point,Geom.label(hide_overlaps=false), order=2, Theme(default_color=colorant"orangered",highlight_width = 0pt));
end

function plotBeaconHist(fg)
    L1 = getVal(getVariable(fg, :l1))
    return layer(x=L1[1,:],y=L1[2,:],Geom.histogram2d(xbincount=300, ybincount=300))
end

function plotBeaconContours!(plHolderIn, fg)
    K1 = plotKDEContour(getBelief(fg,:l1),xlbl="X (m)", ylbl="Y (m)",levels=5,layers=true);
    push!(plHolderIn,K1...)
    push!(plHolderIn,Gadfly.Theme(key_position = :none));
end

function plotKDEMaxs!(plHolderIn,fg;regx::Regex=r"x")
    for sym in ls(fg)
        if occursin(regx,string(sym))
            xData = getKDEMax(getBelief(fg,sym))
            push!(plHolderIn, plotPoint(xData))
        end
    end
end

function plotKDEMeans!(plHolderIn,fg;regx::Regex=r"x")
    for sym in ls(fg)
        if occursin(regx,string(sym))
            xData = getKDEMean(getBelief(fg,sym))
            push!(plHolderIn, plotPoint(xData))
        end
    end
end

function plotPoint(pointIn::Array; colorIn = colorant"blue", orderIn::Int=0)
    return layer(x=[pointIn[1];],y=[pointIn[2];],Geom.point,order=orderIn, Theme(default_color=colorIn,point_size = 1pt,highlight_width = 0pt))
end

function plotPoints(pointsIn::Array;colorIn::String="blue")
        return layer(x=pointsIn[:,1],y=pointsIn[:,2],Geom.point,Theme(default_color=colorant"blue",point_size = 1pt,highlight_width = 0pt))
end

function plotPath(pathIn::Array; colorIn = colorant"green")
    return layer(x=pathIn[:,1],y=pathIn[:,2], Geom.path,Theme(default_color=colorIn))
end

function plotBeaconGT(iGTtemp::Array)
    if size(iGTtemp,2) ==  1
        return layer(x=[iGTtemp[1];],y=[iGTtemp[2];], label=String["Beacon GT";],Geom.point,Geom.label(hide_overlaps=false), order=2, Theme(default_color=colorant"red",highlight_width = 0pt));
    else
        return layer(x=iGTtemp[:,1],y=iGTtemp[:,2], label=String["Beacon GT";],Geom.path,Geom.label(hide_overlaps=false), order=2, Theme(default_color=colorant"red",highlight_width = 0pt));
    end
end


function GRGSFormatReader(arg0::JString, arg1::jboolean)
    return GRGSFormatReader((JString, jboolean), arg0, arg1)
end

function GRGSFormatReader(arg0::JString, arg1::jboolean, arg2::TimeScale)
    return GRGSFormatReader((JString, jboolean, TimeScale), arg0, arg1, arg2)
end

function get_provider(obj::GRGSFormatReader, arg0::jboolean, arg1::jint, arg2::jint)
    return jcall(obj, "getProvider", RawSphericalHarmonicsProvider, (jboolean, jint, jint), arg0, arg1, arg2)
end

function load_data(obj::GRGSFormatReader, arg0::InputStream, arg1::JString)
    return jcall(obj, "loadData", void, (InputStream, JString), arg0, arg1)
end


export Task_TestTask

mutable struct Task_TestTask <: Task
end

function init_task(myt::Task_TestTask,rhd::RHD2000,han,fpga)
    setTtlOut(rhd,ones(Int32,16))
    nothing
end

function do_task(myt::Task_TestTask,rhd::RHD2000,myread,han,fpga)

    for i=1:SAMPLES_PER_DATA_BLOCK
        if rhd.adc[i,1]>10000
            setTtlOut(rhd,zeros(Int32,16))
            break
        end
    end

    nothing
end

function save_task(myt::Task_TestTask,rhd::RHD2000)

    f=open("adc1.bin","a+")
    for i=1:SAMPLES_PER_DATA_BLOCK
        write(f,rhd.adc[i,1])
    end
    close(f)
    nothing
end


@rosimport std_msgs.msg: ColorRGBA
@rosimport geometry_msgs.msg: Point
@rosimport visualization_msgs.msg: Marker
@rosimport safe_traffic_weaving.msg: VehicleTrajectory, path, XYThV
@rosimport auto_messages.msg: from_autobox, to_autobox
rostypegen(@__MODULE__)
import .std_msgs.msg: ColorRGBA
import .geometry_msgs.msg: Point
import .visualization_msgs.msg: Marker
import .safe_traffic_weaving.msg: VehicleTrajectory, path, XYThV
import .auto_messages.msg: from_autobox, to_autobox

TrajectoryTube(p::path) = TrajectoryTube{Float64}(
    invcumtrapz(p.Ux_des_mps, p.s_m), p.s_m, p.Ux_des_mps, p.Ax_des_mps2,
    p.posE_m, p.posN_m, p.Psi_rad, p.k_1pm, p.grade_rad, 0*p.grade_rad, p.edge_L_m, p.edge_R_m
)
TrajectoryTube(p::VehicleTrajectory) = TrajectoryTube{Float64}(
    p.t, p.s, p.V, p.A, p.E, p.N, p.heading, p.curvature, p.grade, p.bank, p.edge_L, p.edge_R
)

### DEBUG
const show_loginfo = fill(false)

### HJI visualization
include("rviz.jl")

### /des_path and /des_traj
const latest_trajectory = fill(straight_trajectory(30., 5.))
const tracking_mode = fill(:path)
function nominal_trajectory_callback(msg::path, mpc=X1DMPC)                 # /des_traj
    latest_trajectory[] = TrajectoryTube(msg)
    tracking_mode[] = :path
    mpc.time_offset = NaN
    mpc.solved = false
end
function nominal_trajectory_callback(msg::VehicleTrajectory, mpc=X1CMPC)    # /des_path
    latest_trajectory[] = TrajectoryTube(msg)
    tracking_mode[] = :traj
    mpc.time_offset = convert(Float64, msg.header.stamp)
    mpc.solved = false
end

### /to_autobox
const to_autobox_msg = to_autobox()

### /from_autobox
const use_HJI_policy = fill(false)
function from_autobox_callback(msg::from_autobox, to_autobox_pub, HJI_values_pub, HJI_contour_pub, path_mpc=X1DMPC, traj_mpc=X1CMPC)
    mpc = (tracking_mode[] == :path ? path_mpc : traj_mpc)
    mpc.current_state = BicycleState(msg.E_m, msg.N_m, msg.psi_rad, msg.ux_mps, msg.uy_mps, msg.r_radps)
    # mpc.current_control = BicycleControl(msg.delta_rad, msg.fxf_N, msg.fxr_N)
    mpc.current_control = BicycleControl(to_autobox_msg.delta_cmd_rad, to_autobox_msg.fxf_cmd_N, to_autobox_msg.fxr_cmd_N)
    mpc.trajectory = latest_trajectory[]

    ###### TODO: very messy
    relative_state = HJIRelativeState(mpc.current_state, traj_mpc.other_car_state)
    V, ∇V = mpc.HJI_cache[relative_state]
    show_loginfo[] && RobotOS.loginfo("Pigeon MPC: HJI value function = $V")
    # show_loginfo[] && RobotOS.loginfo("Pigeon MPC: HJI relative state = $(relative_state)")
    try
        update_HJI_values_marker!(HJI_values_marker, relative_state)
        update_HJI_contour_marker!(HJI_contour_marker, relative_state)
        publish(HJI_values_pub, HJI_values_marker)
        publish(HJI_contour_pub, HJI_contour_marker)
    catch err
        RobotOS.logwarn("HJI Visualization Error: $err\n$(stacktrace(catch_backtrace()))")
    end
    ######

    if msg.pre_flag == 0
        show_loginfo[] && RobotOS.loginfo("Pigeon MPC: /from_autobox pre_flag == 0, MPC inactive")
        return
    end
    if isnan(mpc.time_offset)
        show_loginfo[] && RobotOS.loginfo("Pigeon MPC: time_offset not set, running in path tracking mode")
        _, _, t = path_coordinates(mpc.trajectory, mpc.current_state)
    else
        t = convert(Float64, msg.header.stamp) - mpc.time_offset
        if t < 0 || t > mpc.trajectory.t[end]
            show_loginfo[] && RobotOS.loginfo("Pigeon MPC: current time $t outside trajectory interval [0, $(mpc.trajectory.t[end])]")
            return
        end
    end
    if mpc.current_state.Ux < 1
        show_loginfo[] && RobotOS.loginfo("Pigeon MPC: current speed < 1, pausing MPC while X1 is stopped")
        return
    end
    MPC_steps_missed = msg.header.seq - (mpc.heartbeat + 1)
    if MPC_steps_missed != 0
        RobotOS.logwarn("Pigeon MPC: $(MPC_steps_missed) from_autobox messages lost")
        mpc.heartbeat = msg.header.seq - 1
    end

    t_elapsed = @elapsed begin
        try
            compute_time_steps!(mpc, t)
            compute_linearization_nodes!(mpc)
            update_QP!(mpc)
            solve!(mpc)
        catch err
            RobotOS.logwarn("Pigeon MPC Error: $err\n$(stacktrace(catch_backtrace()))")
        end
    end

    if t_elapsed > 0.01
        RobotOS.logwarn("Pigeon MPC: OSQP took $(1000*t_elapsed) ms at heartbeat $(mpc.heartbeat)")
    else
        show_loginfo[] && RobotOS.loginfo("Pigeon MPC: OSQP took $(1000*t_elapsed) ms at heartbeat $(mpc.heartbeat)")
    end
    # show_loginfo[] && RobotOS.loginfo("Pigeon MPC: $(mpc.model.optimizer.results.info)")
    # show_loginfo[] && RobotOS.loginfo("deltas: $(value.(mpc.model, mpc.variables.δ))")
    mpc.heartbeat += 1

    s, e, _ = path_coordinates(mpc.trajectory, mpc.current_state)
    if tracking_mode[] == :traj && use_HJI_policy[] && V <= mpc.HJI_ϵ
        u_next = BicycleControl(mpc.dynamics.longitudinal_params, optimal_control(mpc.dynamics, relative_state, ∇V))
        RobotOS.logwarn("Pigeon MPC: HJI stepping in to save the day (with a hammer)")
        RobotOS.logwarn("Pigeon MPC: HJI value function = $V")
    else
        if tracking_mode[] == :traj && V <= mpc.HJI_ϵ
            RobotOS.logwarn("Pigeon MPC: HJI stepping in to save the day (with a feather)")
            RobotOS.logwarn("Pigeon MPC: HJI value function = $V")
        end
        u_next = get_next_control(mpc)
    end
    to_autobox_msg.header.stamp  = RobotOS.now()
    to_autobox_msg.post_flag     = 1    # TODO: check for OSQP failure
    to_autobox_msg.heartbeat     = mpc.heartbeat
    to_autobox_msg.s_m           = s
    to_autobox_msg.e_m           = e
    to_autobox_msg.delta_cmd_rad = u_next.δ
    to_autobox_msg.fxf_cmd_N     = u_next.Fxf
    to_autobox_msg.fxr_cmd_N     = u_next.Fxr
    if isnan(to_autobox_msg.delta_cmd_rad) || isnan(to_autobox_msg.fxf_cmd_N) || isnan(to_autobox_msg.fxr_cmd_N)
        show_loginfo[] && RobotOS.loginfo("Pigeon MPC: OSQP returned NaNs " *
                                          "($(to_autobox_msg.delta_cmd_rad), $(to_autobox_msg.fxf_cmd_N), $(to_autobox_msg.fxr_cmd_N))" *
                                          "; using previous control "*
                                          "($(mpc.current_control.δ), $(mpc.current_control.Fxf), $(mpc.current_control.Fxr))")
        to_autobox_msg.delta_cmd_rad = mpc.current_control.δ
        to_autobox_msg.fxf_cmd_N     = mpc.current_control.Fxf
        to_autobox_msg.fxr_cmd_N     = mpc.current_control.Fxr
        publish(to_autobox_pub, to_autobox_msg)
        to_autobox_msg.delta_cmd_rad = 0    # ensures that if we get 2 NaNs in a row, we return to 0 control
        to_autobox_msg.fxf_cmd_N     = 0
        to_autobox_msg.fxr_cmd_N     = 0
        Parametron.initialize!(mpc.model)   # reinitialize model (otherwise NaNs might stick)
        mpc.solved = false    # any warm starting should ignore the previous solution
    else
        publish(to_autobox_pub, to_autobox_msg)
    end
end

function other_car_callback(msg::XYThV, mpc=X1CMPC)
    mpc.other_car_state = SimpleCarState(msg.x, msg.y, msg.th - pi/2, msg.v)
end

### ROS node init
function start_ROS_node()
    init_node("pigeon", anonymous=false)
    other_car = RobotOS.get_param("human", "/xbox_car")
    to_autobox_pub = Publisher{to_autobox}("/to_autobox", queue_size=10)
    HJI_values_pub = Publisher{Marker}("/HJI_values", queue_size=1)
    HJI_contour_pub = Publisher{Marker}("/HJI_contour", queue_size=1)
    Subscriber{path}("/des_path", nominal_trajectory_callback, queue_size=1)
    Subscriber{VehicleTrajectory}("/des_traj", nominal_trajectory_callback, queue_size=1)
    Subscriber{from_autobox}("/from_autobox", from_autobox_callback, (to_autobox_pub, HJI_values_pub, HJI_contour_pub), queue_size=1)
    Subscriber{XYThV}("$(other_car)/xythv", other_car_callback, queue_size=1)    # TODO: abstract with a republisher
    @spawn spin()
end


include("preamble.jl")

using InteractiveUtils: code_llvm, code_warntype
using Statistics: mean

using Transducers: Reduction, start, __foldl__, __simple_foldl__

"""
    llvm_ir(f, args) :: String

Get LLVM IR of `f(args...)` as a string.
"""
llvm_ir(f, args) = sprint(code_llvm, f, Base.typesof(args...))

"""
    julia_ir(f, args) :: String

Get Julia IR of `f(args...)` as a string.
"""
julia_ir(f, args) = sprint(code_warntype, f, Base.typesof(args...))

matchedlines(r, s) = [m.match for m in eachmatch(r, s)]
anyunions(s) = matchedlines(r".*UNION.*", s)
nmatches(r, s) = count(_ -> true, eachmatch(r, s))

@testset "map!" begin
    xf = Filter(x -> -0.5 < x < 0.5) |> Map(x -> 2x)
    xs = Float64[]
    ys = Float64[]
    ir = llvm_ir(map!, (xf, ys, xs))
    @test nmatches(r"fmul <4 x double>", ir) >= 4
    @test nmatches(r"fcmp [a-z]* <4 x double>", ir) >= 4
end

@testset "PartitionBy" begin
    xf = PartitionBy(x -> x > 0) |>
        Filter(xs -> mean(abs, xs) < 1.0) |>
        Map(prod)

    coll = Float64[]
    rf = Reduction(xf, +, eltype(coll))
    val = start(rf, 0.0)
    @test anyunions(julia_ir(__foldl__, (rf, val, coll))) == []

    # If Julia becomes clever enough to make `__simple_foldl__`
    # type-stable, there is no need to maintain current complex code:
    simple_ir = julia_ir(__simple_foldl__, (rf, val, coll))
    @test !isempty(anyunions(simple_ir))
end


#unit-ubound.jl
#test that ubound constructors work

#test that the resolve_as_utype! directive works.

x = Unum{2,2}(0x000000000000000C, 0x0000000000000000, 0x0001, 0x0003, 0x0003)
y = Unum{2,2}(0x000000000000000C, 0x1000000000000000, 0x0001, 0x0003, 0x0003)
z = Unum{2,2}(0x000000000000000C, 0x0000000000000000, 0x0001, 0x0003, 0x0002)
@test Unums.resolve_as_utype!(x, y) == z

x = Unum{2,2}(0x000000000000000C, 0x2000000000000000, 0x0001, 0x0003, 0x0003)
y = Unum{2,2}(0x000000000000000C, 0x3000000000000000, 0x0001, 0x0003, 0x0003)
z = Unum{2,2}(0x000000000000000C, 0x2000000000000000, 0x0001, 0x0003, 0x0002)
@test Unums.resolve_as_utype!(x, y) == z

#disable this test for now.
#=
x = sss(Unum{3,5})
y = Unum{3,5}(0x0000000000000000, 0xFFFFFFFF00000000, 0x0001, 0x0007, 0x001F)
q = Unums.resolve_as_utype!(x, y)
@test q.lower == Unum{3,5}(0x0000000000000000, 0x0000000000000000, 0x0001, 0x0007, 0x0000)
@test q.upper == Unum{3,5}(0x0000000000000000, 0x8000000000000000, 0x0001, 0x0007, 0x0000)
=#

#x = Unum{3,5}(0x0000000000000000, 0x8000000000000000, 0x0001, 0x0007, 0x001F)
#y = Unum{3,5}(0x0000000000000000, 0xF000000000000000, 0x0001, 0x0007, 0x0004)
#q = Unums.resolve_as_utype!(x, y)
#@test q.lower == Unum{3,5}(0x0000000000000000, 0x8000000000000000, 0x0001, 0x0007, 0x0001)
#@test q.upper == Unum{3,5}(0x0000000000000000, 0xF000000000000000, 0x0001, 0x0007, 0x0004)

#test comparisons against similar unums

x = Unum{2,2}(0x000000000000000C, 0x2000000000000000, 0x0001, 0x0003, 0x0002)
b = Ubound(Unum{2,2}(0x000000000000000C, 0x2000000000000000, 0x0001, 0x0003, 0x0003),Unum{2,2}(0x000000000000000C, 0x3000000000000000, 0x0001, 0x0003, 0x0003))

@test x == b

################################################################################
## open-ubound-helper:  A short function which returns the open interval version
## of a bounding unum for a ubound.
#=
# first test warlpiri one as a lower bound should yield walpiri some.
@test Unums.__open_ubound_helper(one(Unum{0,0}), true) == Unum{0,0}(z16, z16, o16, t64, z64)
# as an upper bound should yield walpiri few
@test Unums.__open_ubound_helper(one(Unum{0,0}), false) == Unum{0,0}(z16, z16, o16, z64, z64)
# check to see that warlpiri few is unchanged.
@test Unums.__open_ubound_helper(pos_sss(Unum{0,0}), true) == pos_sss(Unum{0,0})
@test Unums.__open_ubound_helper(pos_sss(Unum{0,0}), false) == pos_sss(Unum{0,0})
# let's do some awkward zero tests.
@test Unums.__open_ubound_helper(zero(Unum{0,0}), true) == pos_sss(Unum{0,0})
@test Unums.__open_ubound_helper(zero(Unum{0,0}), false) == neg_sss(Unum{0,0})
@test Unums.__open_ubound_helper(-zero(Unum{0,0}), true) == pos_sss(Unum{0,0})
@test Unums.__open_ubound_helper(-zero(Unum{0,0}), false) == neg_sss(Unum{0,0})
#and finally, test wierd infinities
#@test_throws ArgumentError Unums.__open_ubound_helper(pos_inf(Unum{0,0}), true)
#@test_throws ArgumentError Unums.__open_ubound_helper(neg_inf(Unum{0,0}), false)
@test Unums.__open_ubound_helper(pos_inf(Unum{0,0}), false) == pos_mmr(Unum{0,0})
@test Unums.__open_ubound_helper(neg_inf(Unum{0,0}), true) == neg_mmr(Unum{0,0})

################################################################################
## open-ubound:  Do we generate open ubounds correctly?

#first on warlpiri ubounds, are we correctly generating open ubounds?  Start
#doubly-closed end input.
uresult = Ubound(neg_sss(Unum{0,0}), pos_sss(Unum{0,0}))
@test open_ubound(zero(Unum{0,0}), one(Unum{0,0})) == pos_sss(Unum{0,0})
@test open_ubound(neg_one(Unum{0,0}), one(Unum{0,0})) == uresult
#half-open inputs
@test open_ubound(neg_sss(Unum{0,0}), one(Unum{0,0})) == uresult
@test open_ubound(neg_one(Unum{0,0}), pos_sss(Unum{0,0})) == uresult
#open inputs
@test open_ubound(neg_sss(Unum{0,0}), pos_sss(Unum{0,0})) == uresult
#in unum{1,1} test oddly overlapping ubounds. This is a ubound pair that looks
#as follows:
# (a)
# ( b )
# and should appropriately generate b as the ubound.
olu_b = Unum{1,1}(z16, z16, o16, z64, o64)
olu_a = Unum{1,1}(o16, z16, o16, z64, o64)
println(olu_b)
println(olu_a)
#println(open_ubound(olu_a, olu_b))
println(Unums.ubound_resolve(Ubound(olu_a, olu_b)))
@test open_ubound(olu_a, olu_b) == olu_b
=#