# `_vscalef` for architectures without `vscalef`.
# magic rounding constant: 1.5*2^52 Adding, then subtracting it from a float rounds it to an Int.
MAGIC_ROUND_CONST(::Type{Float64}) = 6.755399441055744e15
MAGIC_ROUND_CONST(::Type{Float32}) = 1.2582912f7
@inline function vscalef(x::Union{T,AbstractSIMD{<:Any,T}}, y::Union{T,AbstractSIMD{<:Any,T}}, ::False) where {T<:Union{Float32,Float64}}
    _vscalef(x, floor(y))
end
@inline signif_bits(::Type{Float32}) = 0x00000017 # 23
@inline signif_bits(::Type{Float64}) = 0x0000000000000034 # 52
@inline function _vscalef(x::Union{T,AbstractSIMD{<:Any,T}}, y::Union{T,AbstractSIMD{<:Any,T}}) where {T<:Union{Float32,Float64}}
    N = reinterpret(Base.uinttype(T), y + MAGIC_ROUND_CONST(T))
    k = N# >>> 0x00000008

    small_part = reinterpret(Base.uinttype(T), x)
    twopk = (k % Base.uinttype(T)) << signif_bits(T)
    reinterpret(T, twopk + small_part)
end
@inline vscalef(m::AbstractMask, v1::AbstractSIMD, v2::AbstractSIMD, v3::AbstractSIMD, ::False) = vifelse(m, vscalef(v1, v2, False()), v3)
@inline vscalef(v1::T,v2::T) where {T<:AbstractSIMD} = vscalef(v1,v2,has_feature(Val(:x86_64_avx512f)))
@inline vscalef(m::AbstractMask, v1::T, v2::T, v3::T) where {T<:AbstractSIMD} = vscalef(m,v1,v2,v3,has_feature(Val(:x86_64_avx512f)))
@inline vscalef(v1::T,v2::T) where {T<:Union{Float32,Float64}} = vscalef(v1,v2,False())
@inline vscalef(b::Bool,v1::T,v2::T,v3::T) where {T<:Union{Float32,Float64}} = b ? vscalef(v1,v2,False()) : zero(T)
@inline vscalef(v1,v2) = ((v3,v4) = promote(v1,v2); vscalef(v3,v4))
@generated function vscalef(v1::Vec{W,T}, v2::Vec{W,T}, ::True) where {W,T<:Union{Float32,Float64}}
    bits = 8W*sizeof(T)
    bits ∈ (128,256,512) || throw(ArgumentError("Vectors are $bits bits, but only 128, 256, and 512 bits are supported."))
    ltyp = LLVM_TYPES[T]
    vtyp = "<$W x $ltyp>"
    dors = T === Float64 ? 'd' : 's'
    instr = "$vtyp @llvm.x86.avx512.mask.scalef.p$(dors).$bits"
    mtyp = W == 16 ? "i16" : "i8"
    if bits == 512
        decl = "declare $instr($vtyp, $vtyp, $vtyp, $mtyp, i32)"
        # instrs = "%res = call $instr($vtyp %0, $vtyp %1, $vtyp undef, $mtyp -1, i32 11)\nret $vtyp %res"
        instrs = "%res = call $instr($vtyp %0, $vtyp %1, $vtyp undef, $mtyp -1, i32 8)\nret $vtyp %res"
    else
        decl = "declare $instr($vtyp, $vtyp, $vtyp, $mtyp)"
        instrs = "%res = call $instr($vtyp %0, $vtyp %1, $vtyp undef, $mtyp -1)\nret $vtyp %res"
    end
    arg_syms = [:(data(v1)), :(data(v2))]
    llvmcall_expr(decl, instrs, :(_Vec{$W,$T}), :(Tuple{_Vec{$W,$T},_Vec{$W,$T}}), vtyp, fill(vtyp, 2), arg_syms)
end
@generated function vscalef(m::AbstractMask{W}, v1::Vec{W,T}, v2::Vec{W,T}, v3::Vec{W,T}, ::True) where {W,T<:Union{Float32,Float64}}
    bits = 8W*sizeof(T)
    bits ∈ (128,256,512) || throw(ArgumentError("Vectors are $bits bits, but only 128, 256, and 512 bits are supported."))
    ltyp = LLVM_TYPES[T]
    vtyp = "<$W x $ltyp>"
    dors = T === Float64 ? 'd' : 's'
    mtyp = W == 16 ? "i16" : "i8"
    mtypj = W == 16 ? :UInt16 : :UInt8
    instr = "$vtyp @llvm.x86.avx512.mask.scalef.p$(dors).$bits"
    if bits == 512
        decl = "declare $instr($vtyp, $vtyp, $vtyp, $mtyp, i32)"
        instrs = "%res = call $instr($vtyp %0, $vtyp %1, $vtyp %2, $mtyp %3, i32 11)\nret $vtyp %res"
        instrs = "%res = call $instr($vtyp %0, $vtyp %1, $vtyp %2, $mtyp %3, i32 8)\nret $vtyp %res"
    else
        decl = "declare $instr($vtyp, $vtyp, $vtyp, $mtyp)"
        instrs = "%res = call $instr($vtyp %0, $vtyp %1, $vtyp %2, $mtyp %3)\nret $vtyp %res"
    end
    arg_syms = [:(data(v1)), :(data(v2)), :(data(v3)), :(data(m))]
    llvmcall_expr(decl, instrs, :(_Vec{$W,$T}), :(Tuple{_Vec{$W,$T},_Vec{$W,$T},_Vec{$W,$T},$mtypj}), vtyp, [vtyp,vtyp,vtyp,mtyp], arg_syms)
end
@generated function vsreduce(v::Vec{W,T}, ::Val{M}) where {W,T<:Union{Float32,Float64}, M}
    bits = 8W*sizeof(T)
    bits ∈ (128,256,512) || throw(ArgumentError("Vectors are $bits bits, but only 128, 256, and 512 bits are supported."))
    M isa Integer || throw(ArgumentError("M must be an integer, but received $M of type $(typeof(M))."))
    ltyp = LLVM_TYPES[T]
    vtyp = "<$W x $ltyp>"
    dors = T === Float64 ? 'd' : 's'
    instr = "$vtyp @llvm.x86.avx512.mask.reduce.p$(dors).$bits"
    mtyp = W == 16 ? "i16" : "i8"
    if bits == 512
        decl = "declare $instr($vtyp, i32, $vtyp, $mtyp, i32)"
        instrs = "%res = call $instr($vtyp %0, i32 $M, $vtyp undef, $mtyp -1, i32 8)\nret $vtyp %res"
    else
        decl = "declare $instr($vtyp, i32, $vtyp, $mtyp)"
        instrs = "%res = call $instr($vtyp %0, i32 $M, $vtyp undef, $mtyp -1)\nret $vtyp %res"
    end
    arg_syms = [:(data(v))]
    llvmcall_expr(decl, instrs, :(_Vec{$W,$T}), :(Tuple{_Vec{$W,$T}}), vtyp, [vtyp], arg_syms)
end

@generated function vpermi2pd(c::Vec{8,UInt64}, v1::Vec{8,Float64}, v2::Vec{8,Float64}) #where {W,T<:Union{Float32,Float64}, M}
    W = 8; T = Float64
    bits = 8W*sizeof(T)
    # bits ∈ (128,256,512) || throw(ArgumentError("Vectors are $bits bits, but only 128, 256, and 512 bits are supported."))
    ityp = "i$(8sizeof(T))"
    vityp = "<$W x $ityp>"
    ltyp = LLVM_TYPES[T]
    vtyp = "<$W x $ltyp>"
    dors = T === Float64 ? 'd' : 's'
    instr = "$vtyp @llvm.x86.avx512.vpermi2var.p$(dors).$bits"
    decl = "declare $instr($vtyp, $vityp, $vtyp)"
    instrs = "%res = call $instr($vtyp %0, $vityp %1, $vtyp %2)\nret $vtyp %res"
    arg_syms = [:(data(v1)), :(data(c)), :(data(v2))]
    jityp = T === Float64 ? :UInt64 : :UInt32
    llvmcall_expr(decl, instrs, :(_Vec{$W,$T}), :(Tuple{_Vec{$W,$T},_Vec{$W,$jityp},_Vec{$W,$T}}), vtyp, [vtyp,vityp,vtyp], arg_syms)
end

@inline vscalef(v1::VecUnroll, v2::VecUnroll) = VecUnroll(fmap(vscalef, getfield(v1, :data), getfield(v2, :data)))
@inline vscalef(m::VecUnroll, v1::VecUnroll, v2::VecUnroll, v3::VecUnroll) = VecUnroll(fmap(vscalef, getfield(m, :data), getfield(v1, :data), getfield(v2, :data), getfield(v3, :data)))
@inline vsreduce(v::VecUnroll, ::Val{M}) where {M} = VecUnroll(fmap(vsreduce, getfield(v, :data), Val{M}()))
@inline vpermi2pd(v1::VecUnroll, v2::VecUnroll, v3::VecUnroll) = VecUnroll(fmap(vpermi2pd, getfield(v1, :data), getfield(v2, :data), getfield(v3, :data)))
@inline vpermi2pd(v1::VecUnroll, v2::Vec, v3::Vec) = VecUnroll(fmap(vpermi2pd, getfield(v1, :data), v2, v3))

# magic rounding constant: 1.5*2^52 Adding, then subtracting it from a float rounds it to an Int.

# min and max arguments by base and type
MAX_EXP(::Val{2},  ::Type{Float64}) =  1024.0                   # log2 2^1023*(2-2^-52)
MIN_EXP(::Val{2},  ::Type{Float64}) = -1022.0                   # log2(big(2)^-1023*(2-2^-52))
MAX_EXP(::Val{2},  ::Type{Float32}) =  128f0                    # log2 2^127*(2-2^-52)
MIN_EXP(::Val{2},  ::Type{Float32}) = -126f0                    # log2 2^-1075
MAX_EXP(::Val{ℯ},  ::Type{Float64}) =  709.782712893383996732   # log 2^1023*(2-2^-52)
MIN_EXP(::Val{ℯ},  ::Type{Float64}) = -708.396418532264106335  # log 2^-1075
MAX_EXP(::Val{ℯ},  ::Type{Float32}) =  88.72283905206835f0      # log 2^127 *(2-2^-23)
MIN_EXP(::Val{ℯ},  ::Type{Float32}) = -87.3365448101577555f0#-103.97207708f0           # log 2^-150
MAX_EXP(::Val{10}, ::Type{Float64}) =  308.25471555991675       # log10 2^1023*(2-2^-52)
MIN_EXP(::Val{10}, ::Type{Float64}) = -307.65260000       # log10 2^-1075
MAX_EXP(::Val{10}, ::Type{Float32}) =  38.531839419103626f0     # log10 2^127 *(2-2^-23)
MIN_EXP(::Val{10}, ::Type{Float32}) = -37.9297794795476f0      # log10 2^-127 *(2-2^-23)

# 256/log(base, 2) (For Float64 reductions)
LogBo256INV(::Val{2}, ::Type{Float64})    = 256.
LogBo256INV(::Val{ℯ}, ::Type{Float64})    = 369.3299304675746
LogBo256INV(::Val{10}, ::Type{Float64})   = 850.4135922911647
LogBo16INV(::Val{2}, ::Type{Float64})    = 16.
LogBo16INV(::Val{ℯ}, ::Type{Float64})    = 23.083120654223414
LogBo16INV(::Val{10}, ::Type{Float64})   = 53.150849518197795
# -log(base, 2)/256 in upper and lower bits
LogBo256U(::Val{2}, ::Type{Float64})      = -0.00390625
LogBo256U(::Val{ℯ}, ::Type{Float64})      = -0.0027076061740622863
LogBo256U(::Val{10}, ::Type{Float64})     = -0.0011758984205624266
LogBo256L(base::Val{2}, ::Type{Float64})  =  0.
LogBo256L(base::Val{ℯ}, ::Type{Float64})  = -9.058776616587108e-20
LogBo256L(base::Val{10}, ::Type{Float64}) = 1.0952062999160822e-20

LogBo16U(::Val{2}, ::Type{Float64})      = -0.0625
LogBo16U(::Val{ℯ}, ::Type{Float64})      = -0.04332169878499658183857700759113603550471875839751595338254250059333710137310597
LogBo16U(::Val{10}, ::Type{Float64})     = -0.01881437472899882470085868092028081417301186759138178383190171632044426182965149
LogBo16L(base::Val{2}, ::Type{Float64})  = Zero()
LogBo16L(base::Val{ℯ}, ::Type{Float64})  = -1.4494042586539372e-18
LogBo16L(base::Val{10}, ::Type{Float64}) = 1.7523300798657315e-19

# 1/log(base, 2) (For Float32 reductions)
LogBINV(::Val{2}, ::Type{Float32})    =  1f0
LogBINV(::Val{ℯ}, ::Type{Float32})    =  1.442695f0
LogBINV(::Val{10}, ::Type{Float32})   =  3.321928f0
# -log(base, 2) in upper and lower bits
LogBU(::Val{2}, ::Type{Float32})      = -1f0
LogBU(::Val{ℯ}, ::Type{Float32})      = -0.6931472f0
LogBU(::Val{10}, ::Type{Float32})     = -0.30103f0
LogBL(base::Val{2}, ::Type{Float32})  =  0f0
LogBL(base::Val{ℯ}, ::Type{Float32})  =  1.9046542f-9
LogBL(base::Val{10}, ::Type{Float32}) =  1.4320989f-8

const FloatType64 = Union{Float64,AbstractSIMD{<:Any,Float64}}
const FloatType32 = Union{Float32,AbstractSIMD{<:Any,Float32}}
# Range reduced kernels
@inline function expm1b_kernel(::Val{2}, x::FloatType64)
    # c6 = 0.6931471807284470571335252997834339128744539291358546258980326560263434831636494
    # c5 = 0.2402265119815758621794630410361025063296309075509484445159065872903725193960909
    # c4 = 0.05550410353447979823044149277158612685395896745775325243210607075766620053156177
    # c3 = 0.009618027253668450057706478612143223628979891379942570690446533010539871321541621
    # c2 = 0.001333392256353875413926876917141786686018234585146983223440727245459444740967253
    # c1 = 0.0001546929114168849728971327603158937595919966441732209337930866845915899223829891
    # c0 = 1.520192159457321441849564286267892034534060236471603225598783028117591315796835e-05
    # c5 = 0.6931472067096466099497350107329038640311532915014328403152023514862215769240471
    # c4 = 0.2402265150505520926831534797602254284855354178135282005410994764797061783074115
    # c3 = 0.05550327215766594452554739168596479012109775178907146059734348050730310383358696
    # c2 = 0.00961799451416147891836707565892019722415069604010430702590438969636015825337285
    # c1 = 0.001340043166700788064581996335332499076913713747844545061461125917537819216640726
    # c0 = 0.0001547802227945780278842074081393459334029013917349238368250485753166922523500416
    # x * muladd(muladd(muladd(muladd(muladd(muladd(muladd(muladd(c0,x,c1),x,c2),x,c3),x,c4),x,c5),x,c6),x,c7),x,c8)
    # x * muladd(muladd(muladd(muladd(muladd(muladd(c0,x,c1),x,c2),x,c3),x,c4),x,c5),x,c6)
    # x * muladd(muladd(muladd(muladd(muladd(c0,x,c1),x,c2),x,c3),x,c4),x,c5)
  x * muladd(muladd(muladd(0.009618130135925114, x, 0.055504115022757844), x, 0.2402265069590989), x, 0.6931471805599393)
end
@inline function expm1b_kernel(::Val{ℯ}, x::FloatType64)
  x * muladd(muladd(muladd(0.04166666762124105, x, 0.1666666704849642), x, 0.49999999999999983), x, 0.9999999999999998)
end
@inline function expm1b_kernel(::Val{10}, x::FloatType64)
  x * muladd(muladd(muladd(muladd(0.5393833837413015, x, 1.1712561359457612), x, 2.0346785922926713), x, 2.6509490552382577), x, 2.302585092994046)
end
@inline function expb_kernel(::Val{2}, x::FloatType32)
  muladd(muladd(muladd(muladd(muladd(muladd(muladd(1.5316464f-5, x, 0.00015478022f0), x, 0.0013400431f0), x, 0.009617995f0), x, 0.05550327f0), x, 0.24022652f0), x, 0.6931472f0), x, 1.0f0)
end
@inline function expb_kernel(::Val{ℯ}, x::FloatType32)
  muladd(muladd(muladd(muladd(muladd(muladd(muladd(0.00019924171f0, x, 0.0013956056f0), x, 0.008375129f0), x, 0.041666083f0), x, 0.16666415f0), x, 0.5f0), x, 1.0f0), x, 1.0f0)
end
@inline function expb_kernel(::Val{10}, x::FloatType32)
  muladd(muladd(muladd(muladd(muladd(muladd(muladd(0.06837386f0, x, 0.20799689f0), x, 0.54208815f0), x, 1.1712388f0), x, 2.034648f0), x, 2.6509492f0), x, 2.3025851f0), x, 1.0f0)
end

const J_TABLE= Float64[2.0^(big(j-1)/256) for j in 1:256];

@inline fast_fma(a, b, c, ::True) = fma(a, b, c)
@inline function fast_fma(a, b, c, ::False)
    d = dadd(dmul(Double(a),Double(b),False()),Double(c))
    add_ieee(d.hi, d.lo)
end

@static if (Sys.ARCH === :x86_64) | (Sys.ARCH === :i686)
const TABLE_EXP_64_0= Vec(ntuple(j -> Core.VecElement(Float64(2.0^(big(j-1)/16))), Val(8)))
const TABLE_EXP_64_1= Vec(ntuple(j -> Core.VecElement(Float64(2.0^(big(j+7)/16))), Val(8)))

@inline target_trunc(v, ::VectorizationBase.True) = v
@inline target_trunc(v, ::VectorizationBase.False) = v % UInt32
@inline target_trunc(v) = target_trunc(v, VectorizationBase.has_feature(Val(:x86_64_avx512dq)))

# @inline function vexp2_v1(x::AbstractSIMD{8,Float64})
#     x16 = x
#     # x16 = 16x
#     r =  vsreduce(x16, Val(4))
#     m = x16 - r
#     mfrac = m
#     inds = (reinterpret(UInt64, mfrac) >> 0x000000000000002d) & 0x000000000000000f
#     # @show r m mfrac reinterpret(UInt64, m) reinterpret(UInt64, mfrac)
#     # js = vpermi2pd(inds, TABLE_EXP_64_0, TABLE_EXP_64_1)
#     # @show m mfrac r
#     small_part = expm1b_kernel(Val(2), r) + 1.0
#     # js = 1.0
#     # small_part = vfmadd(js, expm1b_kernel(Val(2), r), js)
#     vscalef(small_part, mfrac)
# end

@inline function expm1b_kernel_16(::Val{2}, x)
    c5 = 0.6931471805599457533351827593325319924753473772859614915719486459595837313933663
    c4 = 0.2402265069591009431089489060897837825648676480621950809556237945205562267112511
    c3 = 0.05550410865663929372911461843767669974316894963735870580154522796380984673567634
    c2 = 0.00961812910613182376367867689426991348318504321185094738294343470767871628697879
    c1 = 0.001333378157683735211078403326604752238340853209789619792858391909299167771871147
    c0 = 0.0001540378851029625623114826398060979330719673345637296642237670082377277446583639

    x * vmuladd_fast(vmuladd_fast(vmuladd_fast(vmuladd_fast(vmuladd_fast(c0,x,c1),x,c2),x,c3),x,c4),x,c5)
end
@inline function expm1b_kernel_16(::Val{ℯ}, x)
    c5 = 1.000000000000000640438225946852982701258391604480638275588427888399057119915227
    c4 = 0.5000000000000004803287542332217715766116071510522583538834274474935075866898906
    c3 = 0.1666666666420970554527207281834431226735741940161719645985080160507073865025253
    c2 = 0.04166666666018301921665935823120024420659933010915524936059730214858712998209511
    c1 = 0.008333472974984405879148046293292753978131598285368755170712233472964279745777974
    c0 = 0.001388912162496018623851591184688043066476532389093553363939521815388727152058955
    x * vmuladd_fast(vmuladd_fast(vmuladd_fast(vmuladd_fast(vmuladd_fast(c0,x,c1),x,c2),x,c3),x,c4),x,c5)
end
@inline function expm1b_kernel_16(::Val{10}, x)
    c5 = 2.302585092994047158681503503460480873860999793973827515869365761962430703319599
    c4 = 2.650949055239201551934947671858339219424413336755573194223955910456821842421188
    c3 = 2.034678591993528625083054018110717593492391962926107057414972651492780932119609
    c2 = 1.171255148730010832148986815739840479496404930758611797062000621828706728750671
    c1 = 0.5393919676343165962473794696403862709895176169091768718271240564783020989456929
    c0 = 0.2069993173257113377172910397724414085027323868592924170210484263853229417141011
    x * vmuladd_fast(vmuladd_fast(vmuladd_fast(vmuladd_fast(vmuladd_fast(c0,x,c1),x,c2),x,c3),x,c4),x,c5)
end

# @inline function expm1b_kernel_6(::Val{2}, x)
#     c6 = 0.6931471805599453094157857128856867906777808839530204388709017060018142940776171
#     c5 = 0.2402265069591008469523412357777710806331204676211882505524452303635329805655358
#     c4 = 0.05550410866482161698117481145997657675888637052903928922860089114193615876343007
#     c3 = 0.009618129106525681209446910869436511674612664593938506025921861852732097374219765
#     c2 = 0.001333355814485111028814491629997422287837080138262113640339762815224264878390602
#     c1 = 0.0001540375624549508734984308915404920724081959405205139643458055193131059150331185
#     c0 = 1.525295744513115862409484203574886089068776710280414076942740530839031792795809e-05
#     x * vmuladd_fast(vmuladd_fast(vmuladd_fast(vmuladd_fast(vmuladd_fast(vmuladd_fast(c0,x,c1),x,c2),x,c3),x,c4),x,c5),x,c6)
# end
# # using Remez
# # N,D,E,X = ratfn_minimax(x -> (exp2(x) - big"1")/x, [big(nextfloat(-0.03125)),big(0.03125)], 4, 0); @show(E); N
# @inline function expm1b_kernel_4(::Val{2}, x)
#     c4 = 0.6931471805599461972549081995383434692316977327912755704234013405443109498729026
#     c3 = 0.2402265069131940842333497738928958607054740795078596615709864445611497846077303
#     c2 = 0.05550410865300270379171299778517151376504051870524903806295523325435530249981495
#     c1 = 0.009618317140648284298097106744730186251913149278152053357630395863210686828434175
#     c0 = 0.001333381881551676348461495248002642715422207072457864472267417920610122672570108
#     x * vmuladd_fast(vmuladd_fast(vmuladd_fast(vmuladd_fast(c0,x,c1),x,c2),x,c3),x,c4)
# end

# @inline function vexp2_v4(x::AbstractSIMD{W,Float64}) where {W}
#     # r - VectorizationBase.vroundscale(r, Val(16*(4)))
#     r = VectorizationBase.vsreduce(x,Val(0))
#     rscale = VectorizationBase.vroundscale(r, Val(64))
#     rs = r - rscale
#     inds = convert(UInt, vsreduce(rscale, Val(1))*16.0)
#     expr = expm1b_kernel_5(Val(2), rs)
#     N_float = x - rs
#     # @show inds rs N_float

#     js = vpermi2pd(inds, TABLE_EXP_64_0, TABLE_EXP_64_1)
#     small_part = vfmadd(js, expr, js)
#     res = vscalef(small_part, N_float)
#     return res

# end

####################################################################################################
################################## Non-AVX512 implementation #######################################
####################################################################################################

# With AVX512, we use a tiny look-up table of just 16 numbers for `Float64`,
# because we can perform the lookup using `vpermi2pd`, which is much faster than gather.
# To compensate, we need a larger polynomial.

# Because of the larger polynomial, this implementation works better on systems with 2 FMA units.

@inline function vexp2(x::AbstractSIMD{8,Float64}, ::True)
    r = vsreduce(16.0x, Val(0)) * 0.0625
    N_float = x - r
    expr = expm1b_kernel_16(Val(2), r)
    inds = convert(UInt64, vsreduce(N_float, Val(1))*16.0)
    # inds = ((trunc(Int64, 16.0*N_float)%UInt64)) & 0x000000000000000f
    js = vpermi2pd(inds, TABLE_EXP_64_0, TABLE_EXP_64_1)
    small_part = vfmadd(js, expr, js)
    res = vscalef(small_part, N_float)
    return res
end
# @inline function vexp2_v2(x::AbstractSIMD{8,Float64}, ::True)#, ::Val{N}) where {N}
#     r1 = vsreduce(x, Val(0))
#     m = x - r1
#     r = vfmsub(vsreduce(r1 * 16.0, Val(1)), 0.0625, 0.5)
#     j = r1 - r
#     js = vpermi2pd(convert(UInt, j), TABLE_EXP_64_0, TABLE_EXP_64_1)

#     expr = expm1b_kernel_5(Val(2), r) # 2^r - 1

#     small_part = vfmadd(js, expr, js)
#     # @show r m j
#     vscalef(small_part, m)
# end
# @inline function vexp_v3(x::AbstractSIMD{8,Float64}, ::True)#, ::Val{N}) where {N}
#     xl2e = mul_ieee(1.4426950408889634, x)
#     r1 = vsreduce(xl2e, Val(0))
#     m = xl2e - r1
#     r = vfmsub(vsreduce(r1 * 16.0, Val(1)), 0.0625, 0.5)
#     j = r1 - r
#     js = vpermi2pd(convert(UInt, j), TABLE_EXP_64_0, TABLE_EXP_64_1)
#     rs = vfnmadd(0.6931471805599453094172321214581765680755001343602552541206800094933936219696955, m+j, x)
#     expr = expm1b_kernel_5(Val(ℯ), r) # 2^r - 1

#     small_part = vfmadd(js, expr, js)
#     @show r m j
#     vscalef(small_part, m)
# end

# _log2(::Val{ℯ}) = 1.4426950408889634
# invlog2hi(::Val{ℯ}) = 0.6931471805599453094172321214581765680755001343602552541206800094933936219696955
# invlog2lo(::Val{ℯ}) = -2.319046813846299615494855463875478650412068000949339362196969553467383712860567e-17
# _log2(::Val{10}) = 3.321928094887362347870319429489390175864831393024580612054756395815934776608624
# invlog2hi(::Val{10}) = 0.3010299956639811952137388947244930267681898814621085413104274611271081892744238
# invlog2lo(::Val{10}) = 2.803728127785170339013117338996875833689572538872891810725576172209659522828247e-18

# Requires two more floating point μops, but 8 less loading μops than the default version.
# This thus microbenchmarks a little worse, but the theory is that using less cache than the
# 256 Float64 * 8 bytes/Float64 = 2 KiB table may improve real world performance / reduce
# random latency.
@inline function vexp_avx512(x::AbstractSIMD{8,Float64}, ::Val{B}) where {B}
    N_float = round(x*LogBo16INV(Val(B), Float64))
    r = muladd(N_float, LogBo16U(Val(B), Float64), x)
    r = muladd(N_float, LogBo16L(Val(B), Float64), r)
    inds = ((trunc(Int64, N_float)%UInt64)) & 0x000000000000000f
    expr = expm1b_kernel_16(Val(B), r)
    js = vpermi2pd(inds, TABLE_EXP_64_0, TABLE_EXP_64_1)
    small_part = vfmadd(js, expr, js)
    res = vscalef(small_part, 0.0625*N_float)
    return res
end

@inline function vexp_avx512(x::AbstractSIMD{W,Float64}, ::Val{B}) where {W,B}
    N_float = muladd(x, LogBo256INV(Val{B}(), Float64), MAGIC_ROUND_CONST(Float64))
    N = target_trunc(reinterpret(UInt64, N_float))
    N_float = N_float - MAGIC_ROUND_CONST(Float64)
    r = fma(N_float, LogBo256U(Val{B}(), Float64), x)
    r = fma(N_float, LogBo256L(Val{B}(), Float64), r)
    # @show (N & 0x000000ff) % Int
    # @show N N & 0x000000ff
    js = vload(VectorizationBase.zero_offsets(stridedpointer(J_TABLE)), (N & 0x000000ff,))
    # k = N >>> 0x00000008
    # small_part = reinterpret(UInt64, vfmadd(js, expm1b_kernel(Val{B}(), r), js))
    small_part = vfmadd(js, expm1b_kernel(Val{B}(), r), js)
    # return reinterpret(Float64, small_part), r, k, N_float, js
    res = vscalef(small_part, 0.00390625*N_float)
    # twopk = (k % UInt64) << 0x0000000000000034
    # res = reinterpret(Float64, twopk + small_part)
    return res
end
@inline function vexp_avx512(x::Union{Float32,AbstractSIMD{<:Any,Float32}}, ::Val{B}) where {B}
    N_float = vfmadd(x, LogBINV(Val{B}(), Float32), MAGIC_ROUND_CONST(Float32))
    N = reinterpret(UInt32, N_float)
    N_float = (N_float - MAGIC_ROUND_CONST(Float32))

    r = fast_fma(N_float, LogBU(Val{B}(), Float32), x, fma_fast())
    r = fast_fma(N_float, LogBL(Val{B}(), Float32), r, fma_fast())

    small_part = expb_kernel(Val{B}(), r)
    res = vscalef(small_part, N_float)
    # twopk = N << 0x00000017
    # res = reinterpret(Float32, twopk + small_part)
    return res
end

else# if !((Sys.ARCH === :x86_64) | (Sys.ARCH === :i686))
  const target_trunc = identity
end

# @inline function vexp_avx512(vu::VecUnroll{1,8,Float64,Vec{8,Float64}}, ::Val{B}) where {B}
#   x, y = data(vu)
#   N_float₁ = round(x*LogBo16INV(Val(B), Float64))
#   N_float₂ = muladd(y, LogBo256INV(Val{B}(), Float64), MAGIC_ROUND_CONST(Float64))
#   r₁ = muladd(N_float₁, LogBo16U(Val(B), Float64), x)
#   N₂ = target_trunc(reinterpret(UInt64, N_float₂))
#   r₁ = muladd(N_float₁, LogBo16L(Val(B), Float64), r₁)
#   js₂ = vload(VectorizationBase.zero_offsets(stridedpointer(J_TABLE)), (N₂ & 0x000000ff,))
#   N_float₂ = N_float₂ - MAGIC_ROUND_CONST(Float64)
#   inds₁ = ((trunc(Int64, N_float₁)%UInt64)) & 0x000000000000000f
#   r₂ = fma(N_float₂, LogBo256U(Val{B}(), Float64), y)
#   expr₁ = expm1b_kernel_16(Val(B), r₁)
#   r₂ = fma(N_float₂, LogBo256L(Val{B}(), Float64), r₂)
#   js₁ = vpermi2pd(inds₁, TABLE_EXP_64_0, TABLE_EXP_64_1)
#   small_part₁ = vfmadd(js₁, expr₁, js₁)
#   small_part₂ = vfmadd(js₂, expm1b_kernel(Val{B}(), r₂), js₂)
#   res₁ = vscalef(small_part₁, 0.0625*N_float₁)
#   res₂ = vscalef(small_part₂, 0.00390625*N_float₂)
#   return VecUnroll((res₁, res₂))
# end

# @inline _vexp(x, ::True) = vexp2( 1.4426950408889634 * x, True() )
# @inline _vexp(x, ::True) = vexp2( mul_ieee(1.4426950408889634, x), True() )
# @inline _vexp10(x, ::True) = vexp2( 3.321928094887362 * x, True() )
# @inline _vexp(x) = _vexp(x, has_feature(Val(:x86_64_avx512f)))
# @inline _vexp10(x) = _vexp10(x, has_feature(Val(:x86_64_avx512f)))

@inline vexp(x::AbstractSIMD, ::True) = vexp_avx512(x, Val(ℯ))
@inline vexp2(x::AbstractSIMD, ::True) = vexp_avx512(x, Val(2))
@inline vexp10(x::AbstractSIMD, ::True) = vexp_avx512(x, Val(10))
# @inline vexp(x::AbstractSIMD{W,Float32}, ::True) where {W} = vexp_generic(x, Val(ℯ))
# @inline vexp2(x::AbstractSIMD{W,Float32}, ::True) where {W} = vexp_generic(x, Val(2))
# @inline vexp10(x::AbstractSIMD{W,Float32}, ::True) where {W} = vexp_generic(x, Val(10))

@inline Base.exp(v::AbstractSIMD{W}) where {W} = vexp(float(v))
@inline Base.exp2(v::AbstractSIMD{W}) where {W} = vexp2(float(v))
@inline Base.exp10(v::AbstractSIMD{W}) where {W} = vexp10(float(v))
@static if (Base.libllvm_version ≥ v"11") & ((Sys.ARCH === :x86_64) | (Sys.ARCH === :i686))
    @inline vexp(v::AbstractSIMD) = vexp(v, has_feature(Val(:x86_64_avx512f)))
    @inline vexp2(v::AbstractSIMD) = vexp2(v, has_feature(Val(:x86_64_avx512f)))
    @inline vexp10(v::AbstractSIMD) = vexp10(v, has_feature(Val(:x86_64_avx512f)))
else
    @inline vexp(v::AbstractSIMD) = vexp(v, False())
    @inline vexp2(v::AbstractSIMD) = vexp2(v, False())
    @inline vexp10(v::AbstractSIMD) = vexp10(v, False())
end
@inline vexp(v::Union{Float32,Float64}) = vexp(v, False())
@inline vexp2(v::Union{Float32,Float64}) = vexp2(v, False())
@inline vexp10(v::Union{Float32,Float64}) = vexp10(v, False())
@inline vexp(v::AbstractSIMD{2,Float32}) = vexp(v, False())
@inline vexp2(v::AbstractSIMD{2,Float32}) = vexp2(v, False())
@inline vexp10(v::AbstractSIMD{2,Float32}) = vexp10(v, False())

####################################################################################################
################################## Non-AVX512 implementation #######################################
####################################################################################################

@inline function vexp_generic_core(x::Union{Float64,AbstractSIMD{<:Any,Float64}}, ::Val{B}) where {B}
    N_float = muladd(x, LogBo256INV(Val{B}(), Float64), MAGIC_ROUND_CONST(Float64))
    N = target_trunc(reinterpret(UInt64, N_float))
    N_float = N_float - MAGIC_ROUND_CONST(Float64)
    r = fast_fma(N_float, LogBo256U(Val{B}(), Float64), x, fma_fast())
    r = fast_fma(N_float, LogBo256L(Val{B}(), Float64), r, fma_fast())
    # @show (N & 0x000000ff) % Int
    js = vload(VectorizationBase.zero_offsets(stridedpointer(J_TABLE)), (N & 0x000000ff,))
    k = N >>> 0x00000008
    small_part = reinterpret(UInt64, vfmadd(js, expm1b_kernel(Val{B}(), r), js))
    # return reinterpret(Float64, small_part), r, k, N_float, js
    twopk = (k % UInt64) << 0x0000000000000034
    res = reinterpret(Float64, twopk + small_part)
    return res
end
@inline function vexp_generic(x::Union{Float64,AbstractSIMD{<:Any,Float64}}, ::Val{B}) where {B}
    res = vexp_generic_core(x, Val{B}())
    res = ifelse(x >= MAX_EXP(Val{B}(), Float64), Inf, res)
    res = ifelse(x <= MIN_EXP(Val{B}(), Float64), 0.0, res)
    res = ifelse(isnan(x), x, res)
    return res
end

@inline function vexp_generic_core(x::Union{Float32,AbstractSIMD{<:Any,Float32}}, ::Val{B}) where {B}
    N_float = vfmadd(x, LogBINV(Val{B}(), Float32), MAGIC_ROUND_CONST(Float32))
    N = reinterpret(UInt32, N_float)
    N_float = (N_float - MAGIC_ROUND_CONST(Float32))

    r = fast_fma(N_float, LogBU(Val{B}(), Float32), x, fma_fast())
    r = fast_fma(N_float, LogBL(Val{B}(), Float32), r, fma_fast())

    small_part = reinterpret(UInt32, expb_kernel(Val{B}(), r))
    twopk = N << 0x00000017
    res = reinterpret(Float32, twopk + small_part)
    return res
end
@inline function vexp_generic(x::Union{Float32,AbstractSIMD{<:Any,Float32}}, ::Val{B}) where {B}
    res = vexp_generic_core(x, Val{B}())
    res = ifelse(x >= MAX_EXP(Val{B}(), Float32), Inf32, res)
    res = ifelse(x <= MIN_EXP(Val{B}(), Float32), 0.0f0, res)
    res = ifelse(isnan(x), x, res)
    return res
end
for (func, base) in (:vexp2=>Val(2), :vexp=>Val(ℯ), :vexp10=>Val(10))
    @eval @inline $func(x, ::False) = vexp_generic(x, $base)
end

####################################################################################################
#################################### LOG HELPERS ###################################################
####################################################################################################

# TODO: move these back to log.jl when when the log implementations there are good & well tested enough to use.

@generated function vgetexp(v::Vec{W,T}) where {W,T<:Union{Float32,Float64}}
    bits = 8W*sizeof(T)
    bits ∈ (128,256,512) || throw(ArgumentError("Vectors are $bits bits, but only 128, 256, and 512 bits are supported."))
    ltyp = LLVM_TYPES[T]
    vtyp = "<$W x $ltyp>"
    dors = T === Float64 ? 'd' : 's'
    instr = "$vtyp @llvm.x86.avx512.mask.getexp.p$(dors).$bits"
    mtyp = W == 16 ? "i16" : "i8"
    if bits == 512
        decl = "declare $instr($vtyp, $vtyp, $mtyp, i32)"
        instrs = "%res = call $instr($vtyp %0, $vtyp undef, $mtyp -1, i32 12)\nret $vtyp %res"
    else
        decl = "declare $instr($vtyp, $vtyp, $mtyp)"
        instrs = "%res = call $instr($vtyp %0, $vtyp undef, $mtyp -1)\nret $vtyp %res"
    end
    arg_syms = [:(data(v))]
    llvmcall_expr(decl, instrs, :(_Vec{$W,$T}), :(Tuple{_Vec{$W,$T}}), vtyp, [vtyp], arg_syms)
end
@generated function vgetmant(v::Vec{W,T}) where {W,T<:Union{Float32,Float64}}
    bits = 8W*sizeof(T)
    bits ∈ (128,256,512) || throw(ArgumentError("Vectors are $bits bits, but only 128, 256, and 512 bits are supported."))
    ltyp = LLVM_TYPES[T]
    vtyp = "<$W x $ltyp>"
    dors = T === Float64 ? 'd' : 's'
    instr = "$vtyp @llvm.x86.avx512.mask.getmant.p$(dors).$bits"
    mtyp = W == 16 ? "i16" : "i8"
    if bits == 512
        decl = "declare $instr($vtyp, i32, $vtyp, $mtyp, i32)"
        instrs = "%res = call $instr($vtyp %0, i32 11, $vtyp undef, $mtyp -1, i32 12)\nret $vtyp %res"
    else
        decl = "declare $instr($vtyp, i32, $vtyp, $mtyp)"
        instrs = "%res = call $instr($vtyp %0, i32 11, $vtyp undef, $mtyp -1)\nret $vtyp %res"
    end
    arg_syms = [:(data(v))]
    llvmcall_expr(decl, instrs, :(_Vec{$W,$T}), :(Tuple{_Vec{$W,$T}}), vtyp, [vtyp], arg_syms)
end
@generated function vgetmant(v::Vec{W,T}, ::Union{StaticInt{N},Val{N}}) where {W,T<:Union{Float32,Float64},N}
    bits = 8W*sizeof(T)
    bits ∈ (128,256,512) || throw(ArgumentError("Vectors are $bits bits, but only 128, 256, and 512 bits are supported."))
    ltyp = LLVM_TYPES[T]
    vtyp = "<$W x $ltyp>"
    dors = T === Float64 ? 'd' : 's'
    instr = "$vtyp @llvm.x86.avx512.mask.getmant.p$(dors).$bits"
    mtyp = W == 16 ? "i16" : "i8"
    if bits == 512
        decl = "declare $instr($vtyp, i32, $vtyp, $mtyp, i32)"
        instrs = "%res = call $instr($vtyp %0, i32 $N, $vtyp undef, $mtyp -1, i32 12)\nret $vtyp %res"
    else
        decl = "declare $instr($vtyp, i32, $vtyp, $mtyp)"
        instrs = "%res = call $instr($vtyp %0, i32 $N, $vtyp undef, $mtyp -1)\nret $vtyp %res"
    end
    arg_syms = [:(data(v))]
    llvmcall_expr(decl, instrs, :(_Vec{$W,$T}), :(Tuple{_Vec{$W,$T}}), vtyp, [vtyp], arg_syms)
end
@generated function vgetmant12(v::Vec{W,T}) where {W,T<:Union{Float32,Float64}}
    bits = 8W*sizeof(T)
    bits ∈ (128,256,512) || throw(ArgumentError("Vectors are $bits bits, but only 128, 256, and 512 bits are supported."))
    ltyp = LLVM_TYPES[T]
    vtyp = "<$W x $ltyp>"
    dors = T === Float64 ? 'd' : 's'
    instr = "$vtyp @llvm.x86.avx512.mask.getmant.p$(dors).$bits"
    mtyp = W == 16 ? "i16" : "i8"
    if bits == 512
        decl = "declare $instr($vtyp, i32, $vtyp, $mtyp, i32)"
        instrs = "%res = call $instr($vtyp %0, i32 12, $vtyp undef, $mtyp -1, i32 8)\nret $vtyp %res"
    else
        decl = "declare $instr($vtyp, i32, $vtyp, $mtyp)"
        instrs = "%res = call $instr($vtyp %0, i32 12, $vtyp undef, $mtyp -1)\nret $vtyp %res"
    end
    arg_syms = [:(data(v))]
    llvmcall_expr(decl, instrs, :(_Vec{$W,$T}), :(Tuple{_Vec{$W,$T}}), vtyp, [vtyp], arg_syms)
end

@generated function vroundscale(v::Vec{W,T}, ::Union{Val{N},StaticInt{N}}) where {W,T,N}
    bits = 8W*sizeof(T)
    bits ∈ (128,256,512) || throw(ArgumentError("Vectors are $bits bits, but only 128, 256, and 512 bits are supported."))
    ltyp = LLVM_TYPES[T]
    vtyp = "<$W x $ltyp>"
    dors = T === Float64 ? 'd' : 's'
    instr = "$vtyp @llvm.x86.avx512.mask.rndscale.p$(dors).$bits"
    mtyp = W == 16 ? "i16" : "i8"
    if bits == 512
        decl = "declare $instr($vtyp, i32, $vtyp, $mtyp, i32)"
        instrs = "%res = call $instr($vtyp %0, i32 $N, $vtyp undef, $mtyp -1, i32 4)\nret $vtyp %res"
    else
        decl = "declare $instr($vtyp, i32, $vtyp, $mtyp)"
        instrs = "%res = call $instr($vtyp %0, i32 $N, $vtyp undef, $mtyp -1)\nret $vtyp %res"
    end
    arg_syms = [:(data(v))]
    llvmcall_expr(decl, instrs, :(_Vec{$W,$T}), :(Tuple{_Vec{$W,$T}}), vtyp, [vtyp], arg_syms)
end

@inline vgetexp(v::VecUnroll) = VecUnroll(fmap(vgetexp, getfield(v, :data)))
@inline vgetmant(v::VecUnroll) = VecUnroll(fmap(vgetmant, getfield(v, :data)))
@inline vgetmant(v::VecUnroll,::Union{StaticInt{N},Val{N}}) where {N} = VecUnroll(fmap(vgetmant, getfield(v, :data), StaticInt{N}()))
@inline Base.significand(v::VecUnroll, ::True) = VecUnroll(fmap(vgetmant12, getfield(v, :data)))
@inline vroundscale(v::VecUnroll,::Union{StaticInt{N},Val{N}}) where {N} = VecUnroll(fmap(vroundscale, getfield(v, :data), StaticInt{N}()))
@inline Base.significand(v::Vec, ::True) = vgetmant12(v)
@inline Base.significand(v::AbstractSIMDVector, ::True) = vgetmant12(Vec(v))

mask_exponent(::Val{Float64}) = 0x000f_ffff_ffff_ffff
set_exponent(::Val{Float64}) = 0x3ff0_0000_0000_0000

mask_exponent(::Val{Float32}) = 0x007fffff
set_exponent(::Val{Float32}) = 0x3f800000

mask_exponent(::Val{Float16}) = 0x07ff
set_exponent(::Val{Float16}) = 0x3c00

@inline function Base.significand(v::AbstractSIMD{W,T}, ::False) where {W,T}
  reinterpret(T, (reinterpret(Base.uinttype(T), v) & mask_exponent(Val(T))) | set_exponent(Val(T)))
end
@inline Base.exponent(v::Vec, ::True)= vgetexp(v)
@inline Base.exponent(v::AbstractSIMDVector, ::True)= vgetexp(Vec(v))
@inline Base.exponent(v::VecUnroll, ::True)= VecUnroll(fmap(vgetexp, getfield(v, :data)))
@static if VERSION ≥ v"1.7.0-beta"
  using Base: inttype
else
  @inline inttype(::Type{T}) where {T} = signed(Base.uinttype(T))
end
@inline function Base.exponent(v::AbstractSIMD{W,T}, ::False) where {W,T}
  I = inttype(T)
  vshift = reinterpret(I, v) >> (Base.significand_bits(T)%I)
  e = ((vshift % Int) & Base.exponent_raw_max(T)) - I(Base.exponent_bias(T))
  convert(T, e % Int32)
end

@inline Base.significand(v::AbstractSIMD{W,T}) where {W,T<:Union{Float32,Float64}} = significand(v, has_feature(Val(:x86_64_avx512f)))
@inline Base.exponent(v::AbstractSIMD{W,T}) where {W,T<:Union{Float32,Float64}} = exponent(v, has_feature(Val(:x86_64_avx512f)))
@inline Base.significand(v::AbstractSIMD{W,Float16}) where {W} = significand(v, False())
@inline Base.exponent(v::AbstractSIMD{W,Float16}) where {W} = exponent(v, False())
@inline Base.ldexp(v::AbstractSIMD, e::AbstractSIMD) = vscalef(v, e, has_feature(Val(:x86_64_avx512f)))

# @inline function vexp2_v2(x::AbstractSIMD{8,Float64})
#     x16 = 16.0*x
#     # x8 = 8x
#     r =  vsreduce(x16, Val(0)) * 0.0625
#     @fastmath  begin
#     m = x - r
#     # m + r = x16, r ∈ (-0.5,0.5]
#     # m/16 + r/16 = x, r ∈ (-1/32, 1/32]
#     # we must now vreduce `mfrac`
#     # return m
#     end
#     # expr = expm1b_kernel_4(Val(2), r)
#     expr = expm1b_kernel_5(Val(2), r)
#     inds = convert(UInt64, vsreduce(m, Val(1)) * 16.0)
#     # inds = (reinterpret(UInt64, mfrac) >> 0x000000000000002d) & 0x000000000000000f
#     # inds = (reinterpret(UInt64, mfrac) >> 0x0000000000000035) & 0x000000000000000f
#     # @show r m mfrac reinterpret(UInt64, m) reinterpret(UInt64, mfrac)
#     js = vpermi2pd(inds, TABLE_EXP_64_0, TABLE_EXP_64_1)
#     # return r, mfrac, js
#     # @show js m 16mfrac r mfrac inds%Int ((inds>>1)%Int)
#     # js = 1.0
#     small_part = vfmadd(js, expr, js)
#     vscalef(small_part, m)#, r, mfrac, js, inds
# end

# for (func, base) in (:vexp2=>Val(2), :vexp=>Val(ℯ), :vexp10=>Val(10))
#     @eval begin
#         @inline function $func(x::AbstractSIMD{W,Float64}) where {W}
#             N_float = muladd(x, LogBo256INV($base, Float64), MAGIC_ROUND_CONST(Float64))
#             N = target_trunc(reinterpret(UInt64, N_float))
#             N_float = N_float - MAGIC_ROUND_CONST(Float64)
#             r = fast_fma(N_float, LogBo256U($base, Float64), x, fma_fast())
#             r = fast_fma(N_float, LogBo256L($base, Float64), r, fma_fast())
#             js = vload(VectorizationBase.zero_offsets(stridedpointer(J_TABLE)), (N & 0x000000ff,))
#             k = N >>> 0x00000008
#             small_part = vfmadd(js, expm1b_kernel($base, r), js)
#             twopk = (k % UInt64) << 0x0000000000000034
#             @show N_float k twopk small_part
#             return small_part
#             # res = reinterpret(Float64, twopk + small_part)
#             # return res
#             vscalef(small_part, N_float)
#         end
#     end
# end


# [
#  [ MPIStateArray Name, Field Name, Maximum, Minimum, Mean, Standard Deviation ],
#  [         :                :          :        :      :          :           ],
# ]

#! format: off

parr = [
 ["state",     "ρ", 12, 12, 12, 12],
 ["state", "ρu[1]",  0,  0,  0,  0],
 ["state", "ρu[2]",  0,  0,  0,  0],
 ["state", "ρu[3]", 12,  8, 12, 12],
 ["state",    "ρθ", 12, 10, 12, 12],
]

second_order_flat = (
[
 [ "state",     "ρ",   9.95252314022507689195e-01,  9.99992856011554298590e-01,  9.98330419819817738158e-01,  1.48639562654353791886e-03 ],
 [ "state", "ρu[1]",  -6.36093867411581375298e-15,  8.28629359748938561747e-15,  5.16470747993988398929e-16,  1.17216775586980107502e-15 ],
 [ "state", "ρu[2]",  -3.70797682276813316617e-15,  9.42818503691220985643e-15,  6.07164354178963622023e-16,  1.23143801722553508699e-15 ],
 [ "state", "ρu[3]",  -1.65133743521589989450e-03,  5.29367075491514133403e-09, -8.40309050919211576736e-04,  4.66618546037470796790e-04 ],
 [ "state",    "ρθ",  -9.95249493245247940365e+00,  1.99973806108046075331e-05, -4.98740538889376860965e+00,  2.91965149708573168397e+00 ],
],
 parr,
)

second_order = (
[
 [ "state",     "ρ",   9.95252314022507689195e-01,  9.99992856011554298590e-01,  9.98330419819817738158e-01,  1.48639562654353791886e-03 ],
 [ "state", "ρu[1]",  -6.36142448250610767485e-15,  8.28656656189152269018e-15,  5.17120118507278235699e-16,  1.17302082244252352324e-15 ],
 [ "state", "ρu[2]",  -3.70772154095122499196e-15,  9.42925797553789671672e-15,  6.06575884435408245721e-16,  1.23026363135339100359e-15 ],
 [ "state", "ρu[3]",  -1.65133743521588883564e-03,  5.29367075398690075732e-09, -8.40309050919211468315e-04,  4.66618546037470417320e-04 ],
 [ "state",    "ρθ",  -9.95249493245247940365e+00,  1.99973806108054952236e-05, -4.98740538889376860965e+00,  2.91965149708573168397e+00 ],
],
 parr,
)

fourth_order_flat = (
[
 [ "state",     "ρ",   9.95377495534709000324e-01,  9.99992951378667060958e-01,  9.98321272635789513927e-01,  1.50722816639464090097e-03 ],
 [ "state", "ρu[1]",  -1.22866254290737312252e-14,  1.95946879007561421956e-14,  1.73741725325975513295e-15,  4.23083086870931785383e-15 ],
 [ "state", "ρu[2]",  -1.63058197687820133665e-14,  2.71134092821328864807e-14,  1.75302916450504673999e-15,  4.33921909208334743316e-15 ],
 [ "state", "ρu[3]",  -1.66252523985498286418e-03,  5.55021884647818839491e-08, -8.13842490777055664608e-04,  4.76414940515918383830e-04 ],
 [ "state",    "ρθ",  -9.95373884733410818626e+00, -4.05672848247591079102e-07, -4.98722877855243940104e+00,  2.97859288054288384728e+00 ],
],
 parr,
)

fourth_order = (
[
 [ "state",     "ρ",   9.95377495534709000324e-01,  9.99992951378667060958e-01,  9.98321272635789513927e-01,  1.50722816639464068413e-03 ],
 [ "state", "ρu[1]",  -1.22778750186973996879e-14,  1.95893093780676565332e-14,  1.73744809621522011733e-15,  4.23044883848200063451e-15 ],
 [ "state", "ρu[2]",  -1.62568523313266733927e-14,  2.71071963864711276060e-14,  1.75189805678757112174e-15,  4.33763908149213610479e-15 ],
 [ "state", "ρu[3]",  -1.66252523985503642377e-03,  5.55021884639224168229e-08, -8.13842490777055447768e-04,  4.76414940515918817511e-04 ],
 [ "state",    "ρθ",  -9.95373884733410818626e+00, -4.05672848142820462126e-07, -4.98722877855243940104e+00,  2.97859288054288384728e+00 ],
],
 parr,
)

#! format: on

refVals = (; second_order, fourth_order, second_order_flat, fourth_order_flat)


module Rivers

using Unitful
using Unitful: m, s, km, yr
import Unitful: uconvert
using Distances: Haversine
using NearestNeighbors: BruteTree, knn
using OceanGrids
import OceanGrids: regrid

# These river discharge volumetric flow rates and locations were taken from the Dai and Trenberth dataset
# located on UCAR's Research Data Archive (RDA)
# (URL: https://rda.ucar.edu/datasets/ds551.0/index.html#!description)
# and were converted to Julia/AIBECS with permission from Aiguo Dai.
#
# This module only contains the annual mean river discharge of the 200 largest rivers, taken specifically from files `runoff-table-A.txt` and `runoff-table2-top50r.txt` of the original dataset.
#
# Then I copied these files here and wrote them up into a Julia format by hand
# because it was the most straightforward approach for me here...

struct River{T}
    name::String
    lon::Float64
    lat::Float64
    VFR::T # volumetric flow rate
end

# Show method for rivers
function Base.show(io::IO, r::River)
    lat, lon, name, v = r.lat, r.lon, r.name, r.VFR
    print(io, name, " ", (lat,lon), " ", round(Int, ustrip(v)) * unit(v))
end
Base.show(io::IO, m::MIME"text/plain", r::River) = show(io, r)

const RIVERS = [
    River("Amazon",                          -55.5,  -2.0, 6642.0km^3/yr),
    River("Congo",                            15.3,  -4.3, 1308.0km^3/yr),
    River("Orinoco",                         -63.6,   8.1, 1129.0km^3/yr),
    River("Changjiang",                      117.6,  30.8,  944.0km^3/yr),
    River("Brahmaputra",                      89.7,  25.2,  628.0km^3/yr),
    River("Mississippi",                     -90.9,  32.3,  610.0km^3/yr),
    River("Yenisey",                          86.5,  67.4,  599.0km^3/yr),
    River("Paraná",                          -60.7, -32.7,  568.0km^3/yr),
    River("Lena",                            127.4,  70.7,  531.0km^3/yr),
    River("Mekong",                          105.8,  15.1,  525.0km^3/yr),
    River("Tocantins",                       -49.7,  -3.8,  511.0km^3/yr),
    River("Tapajos",                         -56.8,  -5.2,  415.0km^3/yr),
    River("Ob",                               66.6,  66.6,  412.0km^3/yr),
    River("Ganges",                           88.1,  24.5,  404.0km^3/yr),
    River("Irrawaddy",                        96.0,  21.9,  393.0km^3/yr),
    River("St Lawrence",                     -74.7,  45.0,  363.0km^3/yr),
    River("Amur",                            137.0,  50.5,  354.0km^3/yr),
    River("Xingu",                           -52.2,  -3.2,  302.0km^3/yr),
    River("Mackenzie",                      -133.7,  67.5,  290.0km^3/yr),
    River("Xijiang",                         111.3,  23.5,  270.0km^3/yr),
    River("Columbia",                       -121.2,  45.6,  252.0km^3/yr),
    River("Magdalena",                       -74.9,  10.2,  231.0km^3/yr),
    River("Uruguay",                         -58.0, -31.4,  228.0km^3/yr),
    River("Yukon",                          -162.9,  61.9,  212.0km^3/yr),
    River("Atrato",                          -76.7,   6.2,  204.0km^3/yr),
    River("Danube",                           28.7,  45.2,  202.0km^3/yr),
    River("Niger",                             3.5,  11.9,  193.0km^3/yr),
    River("Ogooué",                           10.2,  -0.7,  186.0km^3/yr),
    River("Essequibo",                       -58.6,   5.8,  154.0km^3/yr),
    River("Fraser",                         -121.4,  49.4,  144.0km^3/yr),
    River("Pechora",                          52.2,  67.6,  140.0km^3/yr),
    River("Nelson",                          -97.9,  54.8,  126.0km^3/yr),
    River("Khatanga",                        102.5,  72.0,  124.0km^3/yr),
    River("Sepik",                           142.2,  -4.2,  123.0km^3/yr),
    River("Kolyma",                          158.7,  68.7,  118.0km^3/yr),
    River("Zambeze",                          33.6, -16.1,  117.0km^3/yr),
    River("Severnaya Dvina",                  41.9,  64.1,  112.0km^3/yr),
    River("Indus",                            68.3,  25.4,  104.0km^3/yr),
    River("Sanaga",                           10.1,   3.8,   99.0km^3/yr),
    River("Godavari",                         81.8,  16.9,   97.0km^3/yr),
    River("Rajang",                          112.9,   2.0,   93.0km^3/yr),
    River("Sao Francisco",                   -37.0, -10.0,   90.0km^3/yr),
    River("Usumacinta",                      -91.5,  17.4,   89.0km^3/yr),
    River("Maroni",                          -54.5,   5.0,   86.0km^3/yr),
    River("Rhine",                             6.1,  51.8,   75.0km^3/yr),
    River("Purari",                          145.1,  -7.0,   74.0km^3/yr),
    River("Caniapiscau",                     -69.2,  57.4,   73.0km^3/yr),
    River("Mahanadi",                         84.0,  20.8,   73.0km^3/yr),
    River("Sacramento",                     -121.5,  38.6,   69.0km^3/yr),
    River("Jacui",                           -51.5, -30.0,   69.0km^3/yr),
    River("San Juan CO",                     -77.2,   4.2,   65.0km^3/yr),
    River("Bénoué",                           13.4,   9.3,   64.0km^3/yr),
    River("Kuskokwim",                      -158.1,  61.9,   57.0km^3/yr),
    River("Albany",                          -83.9,  51.3,   57.0km^3/yr),
    River("Huai",                            117.4,  32.9,   57.0km^3/yr),
    River("La Grande",                       -78.6,  53.7,   56.0km^3/yr),
    River("Tigris (Dicle)",                   44.4,  33.3,   56.0km^3/yr),
    River("Krishna",                          80.6,  16.5,   55.0km^3/yr),
    River("Ottawa",                          -76.2,  45.5,   55.0km^3/yr),
    River("Chindwin",                         95.7,  26.0,   55.0km^3/yr),
    River("Taz",                              82.3,  66.6,   55.0km^3/yr),
    River("Po",                               11.6,  44.9,   55.0km^3/yr),
    River("Courantyne",                      -57.7,   5.0,   54.0km^3/yr),
    River("Indigirka",                       147.5,  69.6,   54.0km^3/yr),
    River("Rhone",                             4.6,  43.8,   54.0km^3/yr),
    River("Saguenay",                        -71.6,  48.6,   53.0km^3/yr),
    River("Alabama",                         -87.5,  31.5,   51.0km^3/yr),
    River("Stikine",                        -132.1,  56.7,   51.0km^3/yr),
    River("Eastmain",                        -78.1,  52.2,   49.0km^3/yr),
    River("Brahmani",                         85.0,  21.5,   48.0km^3/yr),
    River("Dnepr",                            35.2,  47.9,   47.0km^3/yr),
    River("Huanghe",                         114.9,  35.2,   47.0km^3/yr),
    River("Jamanxim",                        -55.8,  -5.5,   47.0km^3/yr),
    River("Susquehanna",                     -76.9,  40.2,   46.0km^3/yr),
    River("Beijiang",                        112.9,  23.6,   46.0km^3/yr),
    River("Don",                              40.7,  47.5,   45.0km^3/yr),
    River("Susitna",                        -150.5,  61.5,   45.0km^3/yr),
    River("Narmada",                          73.7,  21.9,   44.0km^3/yr),
    River("Santa Cruz",                      -71.9, -50.3,   41.0km^3/yr),
    River("Oyapock (Oia)",                   -51.9,   3.8,   40.0km^3/yr),
    River("George",                          -65.8,  58.2,   40.0km^3/yr),
    River("Rufiji",                           37.9,  -7.8,   40.0km^3/yr),
    River("Nile",                             31.3,  29.7,   40.0km^3/yr),
    River("Nottaway",                        -77.4,  50.1,   39.0km^3/yr),
    River("Volta",                             0.1,   6.2,   37.0km^3/yr),
    River("Willamette",                     -123.0,  44.9,   37.0km^3/yr),
    River("Skeena",                         -128.4,  54.6,   36.0km^3/yr),
    River("Garonne",                           0.2,  44.4,   36.0km^3/yr),
    River("Red",                             -92.4,  31.3,   36.0km^3/yr),
    River("Pahang",                          102.4,   3.4,   35.0km^3/yr),
    River("Moose",                           -81.3,  50.8,   34.0km^3/yr),
    River("Chao Phraya",                     100.1,  15.3,   34.0km^3/yr),
    River("Cuyuni",                          -58.8,   6.4,   34.0km^3/yr),
    River("Vistula (Wisła)",                  18.8,  54.1,   34.0km^3/yr),
    River("Copper",                         -144.5,  61.5,   34.0km^3/yr),
    River("Saint John",                      -66.8,  46.0,   34.0km^3/yr),
    River("Olenek",                          123.7,  71.9,   34.0km^3/yr),
    River("Rupert",                          -76.9,  51.5,   33.0km^3/yr),
    River("Vuoksi",                           28.8,  61.2,   33.0km^3/yr),
    River("Kamchatka",                       161.6,  56.3,   33.0km^3/yr),
    River("Manicouagan",                     -68.3,  49.2,   33.0km^3/yr),
    River("Esmeraldas",                      -79.4,   0.5,   32.0km^3/yr),
    River("Doce",                            -40.1, -19.4,   32.0km^3/yr),
    River("Yana",                            136.1,  70.8,   32.0km^3/yr),
    River("Jari",                            -52.6,  -0.6,   32.0km^3/yr),
    River("Bío Bío",                         -73.1, -36.8,   32.0km^3/yr),
    River("Severn-CA",                       -88.3,  55.4,   32.0km^3/yr),
    River("Nushagak",                       -157.5,  59.3,   31.0km^3/yr),
    River("Tsiribihina",                      45.0, -19.7,   31.0km^3/yr),
    River("Baker",                           -72.9, -47.3,   30.0km^3/yr),
    River("Araguari",                        -51.4,   0.7,   30.0km^3/yr),
    River("Pur",                              78.2,  67.0,   29.0km^3/yr),
    River("Nass",                           -129.1,  55.2,   29.0km^3/yr),
    River("Douro (Duero)",                    -7.8,  41.2,   29.0km^3/yr),
    River("Negro-AR",                        -63.7, -40.5,   29.0km^3/yr),
    River("Kouilou",                          12.1,  -4.1,   29.0km^3/yr),
    River("Loire",                            -0.8,  47.4,   28.0km^3/yr),
    River("Puelo",                           -72.2, -41.5,   28.0km^3/yr),
    River("Dongjiang",                       114.3,  23.2,   27.0km^3/yr),
    River("Tista",                            89.5,  25.8,   27.0km^3/yr),
    River("Elbe (Labe)",                      11.8,  53.0,   27.0km^3/yr),
    River("Alsek",                          -138.1,  59.4,   27.0km^3/yr),
    River("Tombigbee",                       -87.9,  32.5,   27.0km^3/yr),
    River("Paraiba do Sul",                  -41.3, -21.8,   26.0km^3/yr),
    River("Parnaiba",                        -42.4,  -3.5,   26.0km^3/yr),
    River("Karun",                            48.7,  31.3,   25.0km^3/yr),
    River("Curua-2",                         -54.5,  -5.6,   25.0km^3/yr),
    River("Mezen",                            45.6,  65.0,   25.0km^3/yr),
    River("Kinabatangan",                    117.6,   5.3,   25.0km^3/yr),
    River("Saint-Maurice",                   -72.7,  46.6,   24.0km^3/yr),
    River("Nyanga",                           10.7,  -2.7,   23.0km^3/yr),
    River("Capim",                           -47.8,  -2.5,   23.0km^3/yr),
    River("Grijalva",                        -93.2,  18.0,   22.0km^3/yr),
    River("Senegal",                         -15.5,  16.5,   22.0km^3/yr),
    River("Papaloapan",                      -96.1,  18.2,   22.0km^3/yr),
    River("Euphrates (F",                     44.3,  32.7,   21.0km^3/yr),
    River("Glomma (Glåma)",                   11.1,  59.6,   21.0km^3/yr),
    River("Apalachicola",                    -84.9,  30.7,   20.0km^3/yr),
    River("Nadym",                            72.7,  65.6,   20.0km^3/yr),
    River("Odra (Oder)",                      14.3,  52.8,   20.0km^3/yr),
    River("Daugava (Západnaya Dviná)",        25.9,  56.5,   19.0km^3/yr),
    River("Hayes, MB",                       -92.8,  56.4,   19.0km^3/yr),
    River("Baleine (Great Whale River)",     -77.0,  55.2,   19.0km^3/yr),
    River("Mangoky",                          43.9, -21.8,   19.0km^3/yr),
    River("Feuilles (Rivière aux Feuilles)", -70.4,  58.6,   19.0km^3/yr),
    River("Melezes (Rivière aux Mélèzes)",   -69.6,  57.7,   19.0km^3/yr),
    River("Churchill",                       -94.6,  58.1,   18.0km^3/yr),
    River("Kelantan",                        102.2,   5.8,   18.0km^3/yr),
    River("Clutha",                          169.7, -46.2,   17.0km^3/yr),
    River("Cross",                             9.3,   5.8,   17.0km^3/yr),
    River("Kemi",                             24.7,  65.9,   17.0km^3/yr),
    River("Göta",                             12.4,  58.4,   17.0km^3/yr),
    River("Han",                             127.0,  37.5,   17.0km^3/yr),
    River("Back",                            -96.5,  66.1,   17.0km^3/yr),
    River("Onega",                            38.5,  63.8,   16.0km^3/yr),
    River("Tapi (Tapti)",                     72.9,  21.3,   16.0km^3/yr),
    River("Anabar",                          114.1,  72.0,   16.0km^3/yr),
    River("Jequitinhonh",                    -39.5, -15.9,   14.0km^3/yr),
    River("Kuban",                            38.2,  45.2,   14.0km^3/yr),
    River("Pánuco",                          -98.6,  22.0,   14.0km^3/yr),
    River("Limpopo",                          33.0, -24.5,   14.0km^3/yr),
    River("Winisk",                          -87.2,  54.5,   13.0km^3/yr),
    River("Ebro",                              0.5,  40.8,   13.0km^3/yr),
    River("Narva",                            28.2,  59.4,   12.0km^3/yr),
    River("Colorado-AR",                    -114.6,  32.7,   12.0km^3/yr),
    River("Seine",                             1.2,  49.3,   11.0km^3/yr),
    River("Coopermine",                     -115.4,  67.7,   11.0km^3/yr),
    River("Dnestr",                           29.5,  46.8,   11.0km^3/yr),
    River("Tagus (Tejo)",                     -8.4,  39.5,   11.0km^3/yr),
    River("Itapecuru",                       -44.4,  -3.6,    9.9km^3/yr),
    River("Burdekin",                        147.2, -19.8,   10.0km^3/yr),
    River("Murray",                          142.8, -34.6,    9.4km^3/yr),
    River("Bolshoy Anyuy",                   161.2,  68.2,    8.4km^3/yr),
    River("Grande de Santiago",             -105.1,  21.8,    8.4km^3/yr),
    River("Bandama",                          -4.8,   5.9,    8.3km^3/yr),
    River("Cauvery",                          78.8,  10.8,    7.7km^3/yr),
    River("Juba",                             42.5,   3.8,    7.5km^3/yr),
    River("Fitzroy-QX",                      150.1, -23.1,    7.4km^3/yr),
    River("Brazos",                          -95.8,  29.6,    7.1km^3/yr),
    River("Comoé (Komoé)",                    -3.7,   6.6,    6.8km^3/yr),
    River("Jaguaribe",                       -38.2,  -5.2,    6.6km^3/yr),
    River("Daly",                            130.7, -13.8,    6.3km^3/yr),
    River("Ouémé",                             2.5,   6.9,    5.4km^3/yr),
    River("Guadiana",                         -7.6,  37.8,    5.2km^3/yr),
    River("Anderson",                       -128.4,  68.6,    5.0km^3/yr),
    River("Victoria",                        130.9, -15.5,    4.9km^3/yr),
    River("Roper",                           134.4, -14.7,    4.8km^3/yr),
    River("Liao",                            123.5,  42.2,    4.6km^3/yr),
    River("Orange (Senqu)",                   17.6, -28.8,    4.6km^3/yr),
    River("Paraguacu",                       -39.0, -12.6,    4.3km^3/yr),
    River("Contas [de]",                     -39.3, -14.3,    3.4km^3/yr),
    River("Guadalquivir",                     -6.0,  37.5,    3.3km^3/yr),
    River("Yaqui",                          -109.5,  29.2,    3.3km^3/yr),
    River("Colorado-TX",                     -96.1,  29.3,    2.6km^3/yr),
    River("Rio Grande",                      -97.4,  25.9,    1.5km^3/yr),
    River("de Grey",                         119.2, -20.3,    1.4km^3/yr),
    River("Gascoyne",                        113.8, -24.8,    0.7km^3/yr),
    River("Avon",                            116.1, -31.8,    0.7km^3/yr),
    River("Fortescue",                       116.2, -21.3,    0.3km^3/yr),
    River("Murchison",                       114.5, -27.9,    0.2km^3/yr)
]

citation() = """
- For the dataset itself: Dai, A. 2017. Dai and Trenberth Global River Flow and Continental Discharge Dataset. Research Data Archive at the National Center for Atmospheric Research, Computational and Information Systems Laboratory. https://doi.org/10.5065/D6V69H1T.
- For the formal publication that describe the dataset: Dai, A., and K. E. Trenberth, 2002: Estimates of freshwater discharge from continents: Latitudinal and seasonal variations. J. Hydrometeorol., 3, 660–687.
- For descriptions of updates to the dataset:
    - Dai, A., T. Qian, K. E. Trenberth, and J. D Milliman, 2009: Changes in continental freshwater discharge from 1948-2004. J. Climate, 22, 2773–2791.
    - Dai, A., 2016: Historical and future changes in streamflow and continental runoff: A review, in Terrestrial Water Cycle and Climate Change: Natural and Human-Induced Impacts, Geophysical Monograph 221. Ed. Qiuhong Tang and Taikan Oki, AGU, John Wiley & Sons, 17–37 (DOI: 10.1002/9781118971772).
"""

uconvert(u, r::River) = River(r.name, r.lon, r.lat, uconvert(u, r.VFR))

function load(unit=m^3/s)
    @info """You are about to use the Dai and Trenberth river discharge dataset.
          If you use it for research, please cite:

          $(citation())

          You can find the corresponding BibTeX entries in the CITATION.bib file
          at the root of the AIBECS.jl package repository.
          (Look for the "Dai_2017", "Dai_Trenberth_2002", "Dai_etal_2009", and "Dai_2016" keys.)
          """
    return uconvert.(unit, RIVERS)
end

function regrid(R::Vector{River{T}}, grd) where T <: Quantity
    lats = [r.lat for r in R]
    lons = [r.lon for r in R]
    depths = zeros(length(R))
    vs = [r.VFR for r in R]
    return regrid(vs, lats, lons, depths, grd)
end
export regrid

end # module

export Rivers


export ferrite_dict

# Specific power loss data read off the charts
# frequency,(x1,y1),(x2,y2)
# frequency if Hz
# x1, x2 flux density in mT
# y1, y2 specific power loss kW/m^3
spldata_3f3 = SplInput((
  25e3,(100,14),(300,270),
  100e3,(62,20,),(240,750),
  200e3,(38,8),(190,1100),
  400e3,(20,17),(140,1700),
  700e3,(12,20),(80,1750)
  ))
spldata_3f35 = SplInput((
  500e3,(42,52),(130,1400),
  1e6,(20,70),(68,1400)
  ))
spldata_3f4 = SplInput((
  25e3,(82,19),(210,360),
  100e3,(48,19),(170,1070),
  200e3,(35,19),(150,1300),
  400e3,(25,16),(140,1700),
  1e6,(15,20),(78,1900),
  2e6,(5.5,20),(40,1850),
  3e6,(4,40),(26,2000)
  ))
spldata_3f45 = SplInput((
  500e3,(22,12),(120,1700),
  1e6,(14,10),(95,1750),
  2e6,(5.5,14),(50,2000),
  ))
spldata_3f5 = SplInput((
  1e6,(15,10),(95,1750),
  2e6,(6.8,13),(57,1600),
  3e6,(4,15),(40,1700)
  ))
spldata_4f1 = SplInput((
  3e6,(5.3,33),(28,1400),
  5e6,(4,40),(22,1500),
  10e6,(3.2,43),(13,1500)
  ))

spldata_r = SplInput((
  25e3,(110,10),(300,180),
  100e3,(51,10),(300,1100),
  200e3,(35,10),(240,2000),
  500e3,(30,45),(140,2000)
  ))

spldata_l = SplInput((
  500e3,(20,10),(110,1000),
  1e6,(15,10),(80,1000),
  2e6,(4.2,10),(36,1000),
  3e6,(2.5,10),(22,1000)
  ))

const ferrite_dict = Dict(
  "3f3" => FerriteProperties("3f3",0.2e6,0.5e6,25,100,
                             BHloop(15,.44,.15),
                             BHloop(10,.345,.120),
                             SpecificPowerLossData(spldata_3f3)),
  "3f35"=> FerriteProperties("3F35",0.5e6,1.0e6,25,100,
                              BHloop(40,.45,.2),
                              BHloop(25,.4,.15),
                              SpecificPowerLossData(spldata_3f35)),
  "3f4" => FerriteProperties("3f4",1e6,2e6,25,100,
                             BHloop(60,0.4,0.16),
                             BHloop(50,0.345,.14),
                             SpecificPowerLossData(spldata_3f4)),
  "3f45" => FerriteProperties("3f45",1e6,2e6,25,100,
                             BHloop(60,.375,.155),
                             BHloop(50,.360,.145),
                             SpecificPowerLossData(spldata_3f45)),
  "3f5" => FerriteProperties("3f5",2e6,4e6,25,100,
                             BHloop(60,.35,.15),
                             BHloop(50,.30,.12),
                             SpecificPowerLossData(spldata_3f5)),
  "4f1" => FerriteProperties("4f1",4e6,10e6,25,100,
                             BHloop(160,.275,.200),
                             BHloop(140,.250,.150),
                             SpecificPowerLossData(spldata_4f1)),
  "r" => FerriteProperties("r",0.5e6,1.8e6,25,100,
                            BHloop(NaN,NaN,NaN),
                            BHloop(NaN,NaN,NaN),
                            SpecificPowerLossData(spldata_r)),
  "l" => FerriteProperties("l",0.5e6,3e6,25,100,
                            BHloop(NaN,NaN,NaN),
                            BHloop(NaN,NaN,NaN),
                            SpecificPowerLossData(spldata_l))
)


using DataStructures
using Statistics

"""
PS with PF
----------

Prioritized Sweeping with: ParticleFilterJump for the Tsas' estimation
and Perfect Integration for Rsas' estimation.

See learner/prioritizedsweepinglearner.jl for the core implementation and more info.

Note 1
------
With particle filtering we need to run multiple times with different seeds.
As final LL we report the MEAN across seeds.
For signals/results we keep track of the ones of all seeds
and in the end report the one with the MAXIMUM LL.
Note 2
------
get_all_puzzles(), get_all_episodes_in_puzzle(), get_data_by_puzzle_episode()
are in rl_data_toolsforalgos.jl
"""
function getLL_Prio_Sweep_TParticleFilterJump(data::Array{Int,2},
                                                gamma::Float64, temp::Float64,
                                                surpriseprobability::Float64,
                                                stochasticity::Float64,
                                                nr_upd_cycles::Union{Int, Float64},
                                                nparticles::Union{Int, Float64};
                                                particlefilterseeds = [1,2,3],
                                                init_R = 0.,
                                                do_debug_print = false
                                                )
    nr_upd_cycles = converttoInt(nr_upd_cycles)
    nparticles = converttoInt(nparticles)
    data_stats, nr_states, nr_actions, nr_samples = get_data_stats_and_dimensions(data)
    if data_stats.nr_subjects != 1
        error("Invalid argument: getLL_Prio_Sweep_TParticleFilter supports only one subject")
    end
    all_puzzles = get_all_puzzles(data)

    LL_seeds = zeros(Float64, length(particlefilterseeds))
    action_selection_probs_seeds = Array{Array{Float64, 1}, 1}(undef, length(particlefilterseeds))
    [action_selection_probs_seeds[i_seed] = zeros(Float64, nr_samples) for i_seed in 1:length(particlefilterseeds)]
    # signals:
    Surprise_seeds =Array{Array{Float64,1}, 1}(undef, length(particlefilterseeds))
    [Surprise_seeds[i_seed] = zeros(Float64, nr_samples) for i_seed in 1:length(particlefilterseeds)]
    γ_surprise_seeds =Array{Array{Float64,1}, 1}(undef, length(particlefilterseeds))
    [γ_surprise_seeds[i_seed] = zeros(Float64, nr_samples) for i_seed in 1:length(particlefilterseeds)]

    QSA_seeds = Array{Any, 1}(undef, length(particlefilterseeds))
    Tsas_seeds = Array{Any, 1}(undef, length(particlefilterseeds))
    learner = []
    for i_seed in 1:length(particlefilterseeds)
        action_counter = 0
        for i_puzzle in all_puzzles
            # Puzzle = independent block of the experiment.
            # The code was made for general purposes, in this experiment we
            # have only ONE puzzle.
            learner = PrioritizedSweeping(nr_states = nr_states,
                                        nr_actions = nr_actions,
                                        γ = gamma, init_R = init_R,
                                        nr_upd_cycles = nr_upd_cycles,
                                        Testimatetype = TParticleFilterJump,
                                        nparticles = nparticles,
                                        surpriseprobability = surpriseprobability,
                                        stochasticity = stochasticity,
                                        seedlearner = particlefilterseeds[i_seed],
                                        do_debug_print = do_debug_print
                                        )
            all_epi_in_puzzle = get_all_episodes_in_puzzle(data, i_puzzle)
            for i_episode in all_epi_in_puzzle
                episode_data = get_data_by_puzzle_episode(data, i_puzzle, i_episode)
                path_length = size(episode_data)[1]  # = T
                if do_debug_print
                    println("i_puzzle:$i_puzzle | i_episode:$i_episode | path_length:$path_length")
                end
                for t in 1:path_length
                    action_counter += 1
                    state = episode_data[t, Int(c_state)]
                    action = episode_data[t, Int(c_action)]
                    reward = episode_data[t, Int(c_reward)]
                    next_state = episode_data[t, Int(c_nextState)]
                    done = reward>0 ? true : false
                    # println("t:$t | s:$state | a:$action | R:$reward | s':$next_state | done: $done")
                    if do_debug_print
                        println("t:$t | s:$state | a:$action | R:$reward | s':$next_state | done: $done")
                    end

                    p_actions = getSoftmaxProbs(learner.QSA[state,:], temp, [0., 0.])
                    action_selection_probs_seeds[i_seed][action_counter] = copy(p_actions[action])
                    LL_seeds[i_seed] += log(p_actions[action])
                    if do_debug_print
                        println("p(s,a)=$p_actions, action=$action")
                    end

                    # Prioritized Sweeping with Particle Filtering
                    update!(learner, state, action, next_state, reward, done)

                    Surprise_seeds[i_seed][action_counter] = copy(learner.Testimate.Sgm[1])
                    γ_surprise_seeds[i_seed][action_counter] = copy(learner.Testimate.γ_surprise[1])
                    QSA_seeds[i_seed] = copy(learner.QSA)
                    Tsas_seeds[i_seed]= deepcopy(learner.Testimate.Ps1a0s0)

                end # end t
            end # end i_episode
        end # end i_puzzle
    end # end i_seed

    LL = get_meanLL_acrossseeds(LL_seeds)

    action_selection_probs_cat = hcat(action_selection_probs_seeds...)
    action_selection_probs = mean(action_selection_probs_cat, dims=2)[:]
    trial_by_trial_p_sa = (action_selection_probs)

    # For signals: return the ones corresponding to the Max LL across seeds
    LL_max, seed_idx = findmax(LL_seeds)
    trial_by_trial_p_sa_max = (action_selection_probs_seeds[seed_idx])
    if do_debug_print
        println(finalQSA_record)
        println("action_selection_probs: $action_selection_probs")
        println("LL_mean: $LL_mean")
        println("LL_max: $LL_max")
        println("trial_by_trial_p_sa: $trial_by_trial_p_sa")
    end
    algo = "prio_sweep_particlefilter"
    param_v = [nr_upd_cycles, nparticles, gamma, temp, surpriseprobability, stochasticity]
    param_str = "nr_upd_cycles=$nr_upd_cycles, nparticles=$nparticles,
                gamma=$(round(gamma,digits=4)), temp=$(round(temp,digits=4)),
                surpriseprobability=$(round(surpriseprobability,digits=4)),
                stochasticity=$(round(stochasticity,digits=4))"
    results_dict = Dict{String,Any}("final_QSA" => QSA_seeds[seed_idx],
                                    "final_Tsas" => Tsas_seeds[seed_idx],
                                    "Surprise" => Surprise_seeds[seed_idx],
                                    "gamma_surprise" => γ_surprise_seeds[seed_idx],
                                    "nr_upd_cycles" => nr_upd_cycles,
                                    "nparticles" => nparticles,
                                    "particlefilterseeds" => particlefilterseeds,
                                    "trial_by_trial_p_sa_max" => trial_by_trial_p_sa_max)

    signals = Step_by_step_signals(data_stats.all_subject_ids[1],
    [], #RPE
    [], #SPE
    trial_by_trial_p_sa,
    LL,
    data_stats,
    algo,
    param_v,
    param_str,
    99, #biased_action_idx, dummy value
    results_dict)
    return LL, signals
end


@inline vzero(::Val{1}, ::Type{T}) where {T<:NativeTypes} = zero(T)
@inline vzero(::StaticInt{1}, ::Type{T}) where {T<:NativeTypes} = zero(T)
@generated function _vzero(::StaticInt{W}, ::Type{T}, ::StaticInt{RS}) where {W,T<:NativeTypes,RS}
    # isone(W) && return Expr(:block, Expr(:meta,:inline), Expr(:call, :zero, T))
    if W * sizeof(T) > RS
        d, r1 = divrem(sizeof(T) * W, RS)
        Wnew, r2 = divrem(W, d)
        (iszero(r1) & iszero(r2)) || throw(ArgumentError("If broadcasting to greater than 1 vector length, should make it an integer multiple of the number of vectors."))
        t = Expr(:tuple)
        for i ∈ 1:d
            push!(t.args, :v)
        end
        # return Expr(:block, Expr(:meta,:inline), :(v = vzero(StaticInt{$Wnew}(), $T)), :(VecUnroll{$(d-1),$Wnew,$T,Vec{$Wnew,$T}}($t)))
        return Expr(:block, Expr(:meta,:inline), :(v = _vzero(StaticInt{$Wnew}(), $T, StaticInt{$RS}())), :(VecUnroll($t)))
        # return Expr(:block, Expr(:meta,:inline), :(v = _vzero(StaticInt{$Wnew}(), $T, StaticInt{$RS}())), :(VecUnroll($t)::VecUnroll{$(d-1),$Wnew,$T,Vec{$Wnew,$T}}))
    end
    typ = LLVM_TYPES[T]
    instrs = "ret <$W x $typ> zeroinitializer"
    quote
        $(Expr(:meta,:inline))
        Vec($LLVMCALL($instrs, _Vec{$W,$T}, Tuple{}))
    end
end
@generated function _vundef(::StaticInt{W}, ::Type{T}) where {W,T<:NativeTypes}
    typ = LLVM_TYPES[T]
    if W == 1
        instrs = "ret $typ undef"
        quote
            $(Expr(:meta,:inline))
            $LLVMCALL($instrs, $T, Tuple{})
        end
    else
        instrs = "ret <$W x $typ> undef"
        quote
            $(Expr(:meta,:inline))
            Vec($LLVMCALL($instrs, _Vec{$W,$T}, Tuple{}))
        end
    end
end
@generated function _vbroadcast(::StaticInt{W}, s::_T, ::StaticInt{RS}) where {W,_T<:NativeTypes,RS}
    isone(W) && return :s
    if _T <: Integer && sizeof(_T) * W > RS
        intbytes = max(4, RS ÷ W)
        T = integer_of_bytes(intbytes)
        if _T <: Unsigned
            T = unsigned(T)
        end
        # ssym = :(s % $T)
        ssym = :(convert($T, s))
    elseif sizeof(_T) * W > RS
        d, r1 = divrem(sizeof(_T) * W, RS)
        Wnew, r2 = divrem(W, d)
        (iszero(r1) & iszero(r2)) || throw(ArgumentError("If broadcasting to greater than 1 vector length, should make it an integer multiple of the number of vectors."))
        t = Expr(:tuple)
        for i ∈ 1:d
            push!(t.args, :v)
        end
        return Expr(:block, Expr(:meta,:inline), :(v = vbroadcast(StaticInt{$Wnew}(), s)), :(VecUnroll($t)))
    else
        T = _T
        ssym = :s
    end
    typ = LLVM_TYPES[T]
    vtyp = vtype(W, typ)
    instrs = """
        %ie = insertelement $vtyp undef, $typ %0, i32 0
        %v = shufflevector $vtyp %ie, $vtyp undef, <$W x i32> zeroinitializer
        ret $vtyp %v
    """
    quote
        $(Expr(:meta,:inline))
        Vec($LLVMCALL($instrs, _Vec{$W,$T}, Tuple{$T}, $ssym))
    end
end
@inline vzero(::Union{Val{W},StaticInt{W}}, ::Type{T}) where {W,T} = _vzero(StaticInt{W}(), T, register_size(T))
@inline vbroadcast(::Union{Val{W},StaticInt{W}}, s::T) where {W,T} = _vbroadcast(StaticInt{W}(), s, register_size(T))
@inline function _vbroadcast(::StaticInt{W}, vu::VecUnroll{N, 1, T, T}, ::StaticInt{RS}) where {W,N,T,RS}
    VecUnroll(fmap(_vbroadcast, StaticInt{W}(), data(vu), StaticInt{RS}()))
end

@generated function vbroadcast(::Union{Val{W},StaticInt{W}}, ptr::Ptr{T}) where {W, T}
    isone(W) && return Expr(:block, Expr(:meta, :inline), :(vload(ptr)))
    typ = LLVM_TYPES[T]
    ptyp = JULIAPOINTERTYPE
    vtyp = "<$W x $typ>"
    alignment = Base.datatype_alignment(T)
    instrs = """
        %ptr = inttoptr $ptyp %0 to $typ*
        %res = load $typ, $typ* %ptr, align $alignment
        %ie = insertelement $vtyp undef, $typ %res, i32 0
        %v = shufflevector $vtyp %ie, $vtyp undef, <$W x i32> zeroinitializer
        ret $vtyp %v
    """
    quote
        $(Expr(:meta,:inline))
        Vec($LLVMCALL( $instrs, _Vec{$W,$T}, Tuple{Ptr{$T}}, ptr ))
    end
end

@inline vbroadcast(::Union{Val{W},StaticInt{W}}, v::AbstractSIMDVector{W}) where {W} = v

@generated function vbroadcast(::Union{Val{W},StaticInt{W}}, v::V) where {W,L,T,V<:AbstractSIMDVector{L,T}}
    N, r = divrem(L, W)
    @assert iszero(r)
    V = if T === Bit
        :(Mask{$W,$(mask_type_symbol(W))})
    else
        :(Vec{$W,$T})
    end
    Expr(:block, Expr(:meta,:inline), :(vconvert(VecUnroll{$(N-1),$W,$T,$V}, v)))
end

@inline Vec{W,T}(v::Vec{W,T}) where {W,T} = v

@inline Base.zero(::Type{Vec{W,T}}) where {W,T} = _vzero(StaticInt{W}(), T, StaticInt{W}() * static_sizeof(T))
@inline Base.zero(::Vec{W,T}) where {W,T} = zero(Vec{W,T})
@inline Base.one(::Vec{W,T}) where {W,T} = vbroadcast(Val{W}(), one(T))

@inline Base.one(::Type{Vec{W,T}}) where {W,T} = vbroadcast(Val{W}(), one(T))
@inline Base.oneunit(::Type{Vec{W,T}}) where {W,T} = vbroadcast(Val{W}(), one(T))
@inline vzero(::Type{T}) where {T<:Number} = zero(T)
@inline vzero() = vzero(pick_vector_width(Float64), Float64)

@inline Vec{W,T}(s::Real) where {W,T} = vbroadcast(Val{W}(), T(s))
@inline Vec{W}(s::T) where {W,T<:NativeTypes} = vbroadcast(Val{W}(), s)
@inline Vec(s::T) where {T<:NativeTypes} = vbroadcast(pick_vector_width(T), s)

@generated function _vzero(::Type{VecUnroll{N,W,T,V}}, ::StaticInt{RS}) where {N,W,T,V,RS}
    t = Expr(:tuple); foreach(_ -> push!(t.args, :(_vzero(StaticInt{$W}(), $T, StaticInt{$RS}()))), 0:N)
    Expr(:block, Expr(:meta, :inline), :(VecUnroll($t)))
end
@inline Base.zero(::Type{VecUnroll{N,W,T,V}}) where {N,W,T,V} = _vzero(VecUnroll{N,W,T,V}, register_size())
@inline Base.zero(::VecUnroll{N,W,T,V}) where {N,W,T,V} = zero(VecUnroll{N,W,T,V})

@inline Base.one(::Type{VecUnroll{N,W,T,V}}) where {N,W,T,V} = VecUnroll{N}(one(V))

@generated function VecUnroll{N,W,T,V}(x::S) where {N,W,T,V<:AbstractSIMDVector{W,T},S<:Real}
    t = Expr(:tuple)
    for n ∈ 0:N
        push!(t.args, :(convert($V, x)))
    end
    Expr(:block, Expr(:meta,:inline), :(VecUnroll($t)))
end
@generated function VecUnroll{N,1,T,T}(x::S) where {N,T<:NativeTypes,S<:Real}
    t = Expr(:tuple)
    for n ∈ 0:N
        push!(t.args, :(convert($T, x)))
    end
    Expr(:block, Expr(:meta,:inline), :(VecUnroll($t)))
end
@inline VecUnroll{N,W,T}(x::NativeTypesV) where {N,W,T} = VecUnroll{N,W,T,Vec{W,T}}(x)
@inline VecUnroll{N}(x::V) where {N,W,T,V <: AbstractSIMDVector{W,T}} = VecUnroll{N,W,T,V}(x)
@inline VecUnroll{N}(x::T) where {N,T<:NativeTypes} = VecUnroll{N,1,T,T}(x)

@generated function zero_vecunroll(::StaticInt{N}, ::StaticInt{W}, ::Type{T}, ::StaticInt{RS}) where {N,W,T,RS}
    Expr(:block, Expr(:meta, :inline), :(_vzero(VecUnroll{$(N-1),$W,$T,Vec{$W,$T}}, StaticInt{$RS}())))
end
@inline zero_init(::Type{T}, ::StaticInt{1}, ::StaticInt{0}, ::StaticInt{RS}) where {T,RS} = zero(T)
@inline zero_init(::Type{T}, ::StaticInt{W}, ::StaticInt{0}, ::StaticInt{RS}) where {W,T,RS} = _vzero(StaticInt{W}(), T, StaticInt{RS}())
@inline zero_init(::Type{T}, ::StaticInt{W}, ::StaticInt{U}, ::StaticInt{RS}) where {W,U,T,RS} = _vzero(VecUnroll{U,W,T,Vec{W,T}}, StaticInt{RS}())
@inline zero_init(::Type{T}, ::Tuple{StaticInt{W},StaticInt{U}}, ::StaticInt{RS}) where {W,U,T,RS} = zero_init(T, StaticInt{W}(), StaticInt{U}(), StaticInt{RS})
@generated function vbroadcast_vecunroll(::StaticInt{N}, ::StaticInt{W}, s::T, ::StaticInt{RS}) where {N,W,T,RS}
    q = Expr(:block, Expr(:meta,:inline), :(v = _vbroadcast(StaticInt{$W}(), s, StaticInt{$RS}())))
    t = Expr(:tuple)
    for n ∈ 1:N
        push!(t.args, :v)
    end
    push!(q.args, :(VecUnroll($t)))
    q
end


mutable struct LatLonAccelDirection
    a_lat::Float64
    a_lon::Float64
    direction::Int
end
Base.show(io::IO, a::LatLonAccelDirection) = @printf(io, "LatLonAccelDirection(%6.3f, %6.3f, %d)", a.a_lat, a.a_lon, a.direction)
Base.length(::Type{LatLonAccelDirection}) = 3
Base.convert(::Type{LatLonAccelDirection}, v::Vector{Float64}) = LatLonAccel(v[1], v[2],convert(Int,v[3]))
function Base.copyto!(v::Vector{Float64}, a::LatLonAccelDirection)
    v[1] = a.a_lat
    v[2] = a.a_lon
    v[3] = convert(Float64,a.direction)
    v
end
function AutomotiveDrivingModels.propagate(veh::Entity{VehicleState, D, Int}, action::LatLonAccelDirection,  roadway::Roadway, Δt::Float64) where {D<:Union{VehicleDef, BicycleModel}}
    previousInd = veh.state.posF.roadind
    a_lat = action.a_lat
    a_lon = action.a_lon

     v = veh.state.v
     ϕ = veh.state.posF.ϕ
    ds = v*cos(ϕ)
     t = veh.state.posF.t
    dt = v*sin(ϕ)

    ΔT = Δt
    ΔT² = ΔT*ΔT
    Δs = ds*ΔT + 0.5*a_lon*ΔT²
    Δt = dt*ΔT + 0.5*a_lat*ΔT²

    ds₂ = ds + a_lon*ΔT
    dt₂ = dt + a_lat*ΔT
    speed₂ = sqrt(dt₂*dt₂ + ds₂*ds₂)
    v₂ = sign(ds₂)*speed₂
    ϕ₂ = atan(dt₂, ds₂) + (v₂ < 0.0)*π # handle negative speeds
    roadind = move_along_with_direction(veh.state.posF.roadind, roadway, Δs, direction = action.direction)
    if t+Δt <= roadway[roadind.tag].width/2.0
        posF = Frenet(roadind, roadway, t=t+Δt, ϕ = ϕ₂)
        return VehicleState(posF, roadway, v₂)
    else
        footpoint = roadway[roadind]
        posG = convert(VecE2, footpoint.pos) + polar(t + Δt, footpoint.pos.θ + π/2)
        posG = VecSE2(posG.x, posG.y, footpoint.pos.θ + ϕ₂)

        state = VehicleState(posG, roadway, v₂)
        projections = in_lanes(posG, roadway)
        if length(roadway[previousInd.tag].exits)>0
            for projection in projections
                dir = clamp(action.direction,1,length(roadway[previousInd.tag].exits))
                if roadway[previousInd.tag].exits[dir].target.tag.segment == projection.tag.segment && !isempty(roadway[previousInd.tag].exits)
                    posF = Frenet(projection, roadway)
                    state = VehicleState(posG, posF, v₂)
                    #println(posF)
                    return state
                end
            end
        end

        for projection in projections
            dir = clamp(action.direction,1,length(roadway[previousInd.tag].exits))
            if  previousInd.tag.segment == projection.tag.segment
                posF = Frenet(projection, roadway)
                state = VehicleState(posG, posF, v₂)
                #println(posF)
                return state
            end
        end
        return state
    end
end

function Base.get(::Type{LatLonAccelDirection}, rec::SceneRecord, roadway::Roadway, vehicle_index::Int, pastframe::Int=0)
    accel_lat = get(ACCFT, rec, roadway, vehicle_index, pastframe)
    accel_lon = get(ACCFS, rec, roadway, vehicle_index, pastframe)
    LatLonAccelDirection(accel_lat, accel_lon,1)
end
function pull_action!(::Type{LatLonAccelDirection}, a::Vector{Float64}, rec::SceneRecord, roadway::Roadway, vehicle_index::Int, pastframe::Int=0)
    a[1] = get(ACCFT, rec, roadway, vehicle_index, pastframe)
    a[2] = get(ACCFS, rec, roadway, vehicle_index, pastframe)
    a
end

mutable struct AccelSteeringDirection
    a::Float64 # accel [m/s²]
    δ::Float64 # steering angle [rad]
    direction::Int64 # turning direction
end
Base.show(io::IO, a::AccelSteeringDirection) = @printf(io, "AccelSteeringDirection(%6.3f,%6.3f,%d)", a.a, a.δ,a.direction)
Base.length(::Type{AccelSteeringDirection}) = 3
Base.convert(::Type{AccelSteeringDirection}, v::Vector{Float64}) = AccelSteeringDirection(v[1], v[2], convert(Int,v[3]))
function Base.copyto!(v::Vector{Float64}, a::AccelSteeringDirection)
    v[1] = a.a
    v[2] = a.δ
    v[3] = convert(Float64,a.direction)
    v
end
#function propagate(veh::Entity{VehicleState, BicycleModel, Int}, action::AccelSteeringAngle, roadway::Roadway, Δt::Float64)
function AutomotiveDrivingModels.propagate(veh::Entity{VehicleState, D, Int}, action::AccelSteeringDirection, roadway::Roadway, Δt::Float64) where {D<:Union{VehicleDef, BicycleModel}}
    previousInd = veh.state.posF.roadind

    L = veh.def.a + veh.def.b
    l = -veh.def.b

    a = action.a # accel [m/s²]
    δ = action.δ # steering wheel angle [rad]

    x = veh.state.posG.x
    y = veh.state.posG.y
    θ = veh.state.posG.θ
    v = veh.state.v

    s = v*Δt + a*Δt*Δt/2 # distance covered
    v′ = v + a*Δt

    posG = nothing
    if abs(δ) < 0.01 # just drive straight
        posG = veh.state.posG + polar(s, θ)
    else # drive in circle

        R = L/tan(δ) # turn radius

        β = s/R
        xc = x - R*sin(θ) + l*cos(θ)
        yc = y + R*cos(θ) + l*sin(θ)

        θ′ = mod(θ+β, 2π)
        x′ = xc + R*sin(θ+β) - l*cos(θ′)
        y′ = yc - R*cos(θ+β) - l*sin(θ′)

        posG = VecSE2(x′, y′, θ′)
    end

    #VehicleState(posG, roadway, v′)
    state = VehicleState(posG, roadway, v′)
    projections = in_lanes(posG, roadway)
    if length(roadway[previousInd.tag].exits)>0
        for projection in projections
            dir = clamp(action.direction,1,length(roadway[previousInd.tag].exits))
            if roadway[previousInd.tag].exits[dir].target.tag.segment == projection.tag.segment && !isempty(roadway[previousInd.tag].exits)
                posF = Frenet(projection, roadway)
                state = VehicleState(posG, posF, v′)
                #println(posF)
                return state
            end
        end
    end

    for projection in projections
        dir = clamp(action.direction,1,length(roadway[previousInd.tag].exits))
        if  previousInd.tag.segment == projection.tag.segment
            posF = Frenet(projection, roadway)
            state = VehicleState(posG, posF, v′)
            #println(posF)
            return state
        end
    end
    return state
end

mutable struct NextState
    x::Float64
    y::Float64
    theta::Float64
    speed::Float64
    direction::Int
end

function NextState()
    return NextState(0.0,0.0,0.0,0.0,1)
end

function AutomotiveDrivingModels.propagate(veh::Entity{VehicleState, D, Int}, action::NextState,  roadway::Roadway, Δt::Float64) where {D<:Union{VehicleDef, BicycleModel}}
    previousInd = veh.state.posF.roadind

    posG = VecSE2(action.x, action.y, action.theta)
    v′ = action.speed

    state = VehicleState(posG, roadway, v′)
    projections = in_lanes(posG, roadway)
    if length(roadway[previousInd.tag].exits)>0
        for projection in projections
            dir = clamp(action.direction,1,length(roadway[previousInd.tag].exits))
            if roadway[previousInd.tag].exits[dir].target.tag.segment == projection.tag.segment && !isempty(roadway[previousInd.tag].exits)
                posF = Frenet(projection, roadway)
                state = VehicleState(posG, posF, v′)
                #println(posF)
                return state
            end
        end
    end

    for projection in projections
        dir = clamp(action.direction,1,length(roadway[previousInd.tag].exits))
        if  previousInd.tag.segment == projection.tag.segment
            posF = Frenet(projection, roadway)
            state = VehicleState(posG, posF, v′)
            #println(posF)
            return state
        end
    end
    return state
end


export
    fovray,
    fovtrg,
    frame,
    frinfo,
    frmnam,
    ftncls,
    furnsh

"""
    fovray(inst, raydir, rframe, abcorr, observer, et)

Determine if a specified ray is within the field-of-view (FOV) of a specified instrument
at a given time.

### Arguments ###

- `inst`: Name or ID code string of the instrument
- `raydir`: Ray's direction vector
- `rframe`: Body-fixed, body-centered frame for target body
- `abcorr`: Aberration correction flag
- `observer`: Name or ID code string of the observer
- `et`: Time of the observation (seconds past J2000)

### Output ###

Returns `true` if the ray is visible.

### References ###

- [NAIF Documentation](https://naif.jpl.nasa.gov/pub/naif/toolkit_docs/C/cspice/fovray_c.html)
"""
function fovray(inst, raydir, rframe, abcorr, observer, et)
    @checkdims 3 raydir
    visible = Ref{SpiceBoolean}()
    ccall((:fovray_c, libcspice), Cvoid,
          (Cstring, Ref{SpiceDouble}, Cstring, Cstring, Cstring, Ref{SpiceDouble}, Ref{SpiceBoolean}),
          inst, raydir, rframe, abcorr, observer, et, visible)
    handleerror()
    Bool(visible[])
end

"""
    fovtrg(inst, target, tshape, tframe, abcorr, obsrvr, et)

Determine if a specified ephemeris object is within the field-of-view (FOV) of a specified
instrument at a given time.

### Arguments ###

- `inst`: Name or ID code string of the instrument.
- `target`: Name or ID code string of the target.
- `tshape`: Type of shape model used for the target.
- `tframe`: Body-fixed, body-centered frame for target body.
- `abcorr`: Aberration correction flag.
- `obsrvr`: Name or ID code string of the observer.
- `et`: Time of the observation (seconds past J2000).

### Output ###

Returns `true` if the object is visible.

### References ###

- [NAIF Documentation](https://naif.jpl.nasa.gov/pub/naif/toolkit_docs/C/cspice/fovtrg_c.html)
"""
function fovtrg(inst, target, tshape, tframe, abcorr, obsrvr, et)
    visible = Ref{SpiceBoolean}()
    ccall((:fovtrg_c, libcspice), Cvoid,
          (Cstring, Cstring, Cstring, Cstring, Cstring, Cstring, Ref{SpiceDouble}, Ref{SpiceBoolean}),
          inst, target, tshape, tframe, abcorr, obsrvr, et, visible)
    handleerror()
    Bool(visible[])
end

"""
    frame(x)

Given a vector `x`, this routine builds a right handed orthonormal frame `x`, `y`, `z`
where the output `x` is parallel to the input `x`.

### Arguments ###

- `x`: Input vector

### Output ###

- `x`: Unit vector parallel to `x` on output
- `y`: Unit vector in the plane orthogonal to `x`
- `z`: Unit vector given by `x × y`

### References ###

- [NAIF Documentation](https://naif.jpl.nasa.gov/pub/naif/toolkit_docs/C/cspice/frame_c.html)
"""
function frame(x)
    @checkdims 3 x
    x = copy(x)
    y = Array{SpiceDouble}(undef, 3)
    z = Array{SpiceDouble}(undef, 3)
    ccall((:frame_c, libcspice), Cvoid,
          (Ref{SpiceDouble}, Ref{SpiceDouble}, Ref{SpiceDouble}),
          x, y, z)
    x, y, z
end

"""
    frinfo(frcode)

Retrieve the minimal attributes associated with a frame needed for converting transformations
to and from it.

### Arguments ###

- `frcode`: The id code for a reference frame

### Output ###

- `cent`: The center of the frame
- `frclss`: The class (type) of the frame
- `clssid`: The idcode for the frame within its class

Returns `nothing` if no frame with id `frcode` could be found.

### References ###

- [NAIF Documentation](https://naif.jpl.nasa.gov/pub/naif/toolkit_docs/C/cspice/frinfo_c.html)
"""
function frinfo(frcode)
    cent = Ref{SpiceInt}()
    frclss = Ref{SpiceInt}()
    clssid = Ref{SpiceInt}()
    found = Ref{SpiceBoolean}()
    ccall((:frinfo_c, libcspice), Cvoid,
          (SpiceInt, Ref{SpiceInt}, Ref{SpiceInt}, Ref{SpiceInt}, Ref{SpiceBoolean}),
          frcode, cent, frclss, clssid, found)

    Bool(found[]) || return nothing
    Int(cent[]), Int(frclss[]), Int(clssid[])
end

"""
    frmnam(frcode)

Retrieve the name of a reference frame associated with an id code.

### Arguments ###

- `frcode`: The id code for a reference frame

### Output ###

Returns the name associated with the reference frame.

### References ###

- [NAIF Documentation](https://naif.jpl.nasa.gov/pub/naif/toolkit_docs/C/cspice/frmnam_c.html)
"""
function frmnam(frcode)
    lenout = 33
    frname = Array{SpiceChar}(undef, lenout)
    ccall((:frmnam_c, libcspice), Cvoid, (SpiceInt, SpiceInt, Ref{SpiceChar}),
          frcode, lenout, frname)
    chararray_to_string(frname)
end

@deprecate ftncls close

"""
    furnsh(kernels...)

Load one or more SPICE kernels into a program.

### Arguments ###

- `kernels`: Path(s) of SPICE kernels to load

### References ###

- [NAIF Documentation](https://naif.jpl.nasa.gov/pub/naif/toolkit_docs/C/cspice/furnsh_c.html)
"""
function furnsh(kernels...)
    for kernel in kernels
        ccall((:furnsh_c, libcspice), Cvoid, (Cstring,), kernel)
        handleerror()
    end
end