text:
module Math

export sin, cos, tan, sinh, cosh, tanh, asin, acos, atan,
       asinh, acosh, atanh, sec, csc, cot, asec, acsc, acot,
       sech, csch, coth, asech, acsch, acoth,
       sinpi, cospi, sinc, cosc,
       cosd, cotd, cscd, secd, sind, tand,
       acosd, acotd, acscd, asecd, asind, atand, atan2,
       rad2deg, deg2rad,
       log, log2, log10, log1p, exponent, exp, exp2, exp10, expm1,
       cbrt, sqrt, erf, erfc, erfcx, erfi, dawson,
       ceil, floor, trunc, round, significand,
       lgamma, hypot, gamma, lfact, max, min, minmax, ldexp, frexp,
       clamp, modf, ^, mod2pi,
       airy, airyai, airyprime, airyaiprime, airybi, airybiprime,
       besselj0, besselj1, besselj, bessely0, bessely1, bessely,
       hankelh1, hankelh2, besseli, besselk, besselh,
       beta, lbeta, eta, zeta, polygamma, invdigamma, digamma, trigamma,
       erfinv, erfcinv

import Base: log, exp, sin, cos, tan, sinh, cosh, tanh, asin,
             acos, atan, asinh, acosh, atanh, sqrt, log2, log10,
             max, min, minmax, ceil, floor, trunc, round, ^, exp2, exp10

import Core.Intrinsics: nan_dom_err, sqrt_llvm, box, unbox, powi_llvm

# non-type specific math functions

clamp{X,L,H}(x::X, lo::L, hi::H) =
    ifelse(x > hi, convert(promote_type(X,L,H), hi),
           ifelse(x < lo,
                  convert(promote_type(X,L,H), lo),
                  convert(promote_type(X,L,H), x)))

clamp{T}(x::AbstractArray{T,1}, lo, hi) = [clamp(xx, lo, hi) for xx in x]
clamp{T}(x::AbstractArray{T,2}, lo, hi) =
    [clamp(x[i,j], lo, hi) for i in 1:size(x,1), j in 1:size(x,2)]
clamp{T}(x::AbstractArray{T}, lo, hi) =
    reshape([clamp(xx, lo, hi) for xx in x], size(x))

# evaluate p[1] + x * (p[2] + x * (....)), i.e. a polynomial via Horner's rule
macro horner(x, p...)
    ex = esc(p[end])
    for i = length(p)-1:-1:1
        ex = :($(esc(p[i])) + $(esc(x)) * $ex)
    end
    ex
end

function sinpi(x::Real)
    if isinf(x)
        return throw(DomainError())
    elseif isnan(x)
        return nan(x)
    end

    rx = copysign(float(rem(x,2)),x)
    arx = abs(rx)

    if arx < oftype(rx,0.25)
        return sin(pi*rx)
    elseif arx <= oftype(rx,0.75)
        arx = oftype(rx,0.5) - arx
        return copysign(cos(pi*arx),rx)
    elseif arx < oftype(rx,1.25)
        rx = (one(rx) - arx)*sign(rx)
        return sin(pi*rx)
    elseif arx <= oftype(rx,1.75)
        arx = oftype(rx,1.5) - arx
        return -copysign(cos(pi*arx),rx)
    else
        rx = rx - copysign(oftype(rx,2.0),rx)
        return sin(pi*rx)
    end
end

function cospi(x::Real)
    if isinf(x)
        return throw(DomainError())
    elseif isnan(x)
        return nan(x)
    end

    rx = abs(float(rem(x,2)))

    if rx <= oftype(rx,0.25)
        return cos(pi*rx)
    elseif rx < oftype(rx,0.75)
        rx = oftype(rx,0.5) - rx
        return sin(pi*rx)
    elseif rx <= oftype(rx,1.25)
        rx = one(rx) - rx
        return -cos(pi*rx)
    elseif rx < oftype(rx,1.75)
        rx = rx - oftype(rx,1.5)
        return sin(pi*rx)
    else
        rx = oftype(rx,2.0) - rx
        return cos(pi*rx)
    end
end

sinpi(x::Integer) = zero(x)
cospi(x::Integer) = isodd(x) ? -one(x) : one(x)

function sinpi(z::Complex)
    zr, zi = reim(z)
    if !isfinite(zi) && zr == 0 return complex(zr, zi) end
    if isnan(zr) && !isfinite(zi) return complex(zr, zi) end
    if !isfinite(zr) && zi == 0 return complex(oftype(zr, NaN), zi) end
    if !isfinite(zr) && isfinite(zi) return complex(oftype(zr, NaN), oftype(zi, NaN)) end
    if !isfinite(zr) && !isfinite(zi) return complex(zr, oftype(zi, NaN)) end
    pizi = pi*zi
    complex(sinpi(zr)*cosh(pizi), cospi(zr)*sinh(pizi))
end

function cospi(z::Complex)
    zr, zi = reim(z)
    if !isfinite(zi) && zr == 0
        return complex(isnan(zi) ? zi : oftype(zi, Inf),
                       isnan(zi) ? zr : zr*-sign(zi))
    end
    if !isfinite(zr) && isinf(zi)
        return complex(oftype(zr, Inf), oftype(zi, NaN))
    end
    if isinf(zr)
        return complex(oftype(zr, NaN), zi==0 ? -copysign(zi, zr) : oftype(zi, NaN))
    end
    if isnan(zr) && zi==0 return complex(zr, abs(zi)) end
    pizi = pi*zi
    complex(cospi(zr)*cosh(pizi), -sinpi(zr)*sinh(pizi))
end
@vectorize_1arg Number sinpi
@vectorize_1arg Number cospi

sinc(x::Number) = x==0 ? one(x)  : oftype(x,sinpi(x)/(pi*x))
sinc(x::Integer) = x==0 ? one(x) : zero(x)
sinc{T<:Integer}(x::Complex{T}) = sinc(float(x))
@vectorize_1arg Number sinc
cosc(x::Number) = x==0 ? zero(x) : oftype(x,(cospi(x)-sinpi(x)/(pi*x))/x)
cosc(x::Integer) = cosc(float(x))
cosc{T<:Integer}(x::Complex{T}) = cosc(float(x))
@vectorize_1arg Number cosc

rad2deg(z::Real) = oftype(z, 57.29577951308232*z)
deg2rad(z::Real) = oftype(z, 0.017453292519943295*z)
rad2deg(z::Integer) = rad2deg(float(z))
deg2rad(z::Integer) = deg2rad(float(z))
@vectorize_1arg Real rad2deg
@vectorize_1arg Real deg2rad

for (finv, f) in ((:sec, :cos), (:csc, :sin), (:cot, :tan),
                  (:sech, :cosh), (:csch, :sinh), (:coth, :tanh),
                  (:secd, :cosd), (:cscd, :sind), (:cotd, :tand))
    @eval begin
        ($finv){T<:Number}(z::T) = one(T) / (($f)(z))
        ($finv){T<:Number}(z::AbstractArray{T}) = one(T) ./ (($f)(z))
    end
end

for (fa, fainv) in ((:asec, :acos), (:acsc, :asin), (:acot, :atan),
                    (:asech, :acosh), (:acsch, :asinh), (:acoth, :atanh))
    @eval begin
        ($fa){T<:Number}(y::T) = ($fainv)(one(T) / y)
        ($fa){T<:Number}(y::AbstractArray{T}) = ($fainv)(one(T) ./ y)
    end
end

function sind(x::Real)
    if isinf(x)
        return throw(DomainError())
    elseif isnan(x)
        return nan(x)
    end

    rx = copysign(float(rem(x,360)),x)
    arx = abs(rx)

    if arx < oftype(rx,45.0)
        return sin(deg2rad(rx))
    elseif arx <= oftype(rx,135.0)
        arx = oftype(rx,90.0) - arx
        return copysign(cos(deg2rad(arx)),rx)
    elseif arx < oftype(rx,225.0)
        rx = (oftype(rx,180.0) - arx)*sign(rx)
        return sin(deg2rad(rx))
    elseif arx <= 315.0
        arx = oftype(rx,270.0) - arx
        return -copysign(cos(deg2rad(arx)),rx)
    else
        rx = rx - copysign(oftype(rx,360.0),rx)
        return sin(deg2rad(rx))
    end
end
@vectorize_1arg Real sind

function cosd(x::Real)
    if isinf(x)
        return throw(DomainError())
    elseif isnan(x)
        return nan(x)
    end

    rx = abs(float(rem(x,360)))

    if rx <= oftype(rx,45.0)
        return cos(deg2rad(rx))
    elseif rx < oftype(rx,135.0)
        rx = oftype(rx,90.0) - rx
        return sin(deg2rad(rx))
    elseif rx <= oftype(rx,225.0)
        rx = oftype(rx,180.0) - rx
        return -cos(deg2rad(rx))
    elseif rx < oftype(rx,315.0)
        rx = rx - oftype(rx,270.0)
        return sin(deg2rad(rx))
    else
        rx = oftype(rx,360.0) - rx
        return cos(deg2rad(rx))
    end
end
@vectorize_1arg Real cosd

tand(x::Real) = sind(x) / cosd(x)
@vectorize_1arg Real tand

for (fd, f) in ((:sind, :sin), (:cosd, :cos), (:tand, :tan))
    @eval begin
        ($fd)(z) = ($f)(deg2rad(z))
    end
end

for (fd, f) in ((:asind, :asin), (:acosd, :acos), (:atand, :atan),
                (:asecd, :asec), (:acscd, :acsc), (:acotd, :acot))
    @eval begin
        ($fd)(y) = rad2deg(($f)(y))
        @vectorize_1arg Real $fd
    end
end

log(b,x) = log(x)./log(b)

# type specific math functions

const libm = Base.libm_name
const openspecfun = "libopenspecfun"

# functions with no domain error
for f in (:cbrt, :sinh, :cosh, :tanh, :atan, :asinh, :exp, :erf, :erfc, :exp2, :expm1)
    @eval begin
        ($f)(x::Float64) = ccall(($(string(f)),libm), Float64, (Float64,), x)
        ($f)(x::Float32) = ccall(($(string(f,"f")),libm), Float32, (Float32,), x)
        ($f)(x::Real) = ($f)(float(x))
        @vectorize_1arg Number $f
    end
end

# fallback definitions to prevent infinite loop from $f(x::Real) def above
cbrt(x::FloatingPoint) = x^(1//3)
exp2(x::FloatingPoint) = 2^x
for f in (:sinh, :cosh, :tanh, :atan, :asinh, :exp, :erf, :erfc, :expm1)
    @eval ($f)(x::FloatingPoint) = error("not implemented for ", typeof(x))
end

# TODO: GNU libc has exp10 as an extension; should openlibm?
exp10(x::Float64) = 10.0^x
exp10(x::Float32) = 10.0f0^x
exp10(x::Integer) = exp10(float(x))
@vectorize_1arg Number exp10

# functions that return NaN on non-NaN argument for domain error
for f in (:sin, :cos, :tan, :asin, :acos, :acosh, :atanh, :log, :log2, :log10,
          :lgamma, :log1p)
    @eval begin
        ($f)(x::Float64) = nan_dom_err(ccall(($(string(f)),libm), Float64, (Float64,), x), x)
        ($f)(x::Float32) = nan_dom_err(ccall(($(string(f,"f")),libm), Float32, (Float32,), x), x)
        ($f)(x::Real) = ($f)(float(x))
        @vectorize_1arg Number $f
    end
end

sqrt(x::Float64) = box(Float64,sqrt_llvm(unbox(Float64,x)))
sqrt(x::Float32) = box(Float32,sqrt_llvm(unbox(Float32,x)))
sqrt(x::Real) = sqrt(float(x))
@vectorize_1arg Number sqrt

for f in (:ceil, :trunc, :significand) # :rint, :nearbyint
    @eval begin
        ($f)(x::Float64) = ccall(($(string(f)),libm), Float64, (Float64,), x)
        ($f)(x::Float32) = ccall(($(string(f,"f")),libm), Float32, (Float32,), x)
        @vectorize_1arg Real $f
    end
end

round(x::Float32) = ccall((:roundf, libm), Float32, (Float32,), x)
@vectorize_1arg Real round

floor(x::Float32) = ccall((:floorf, libm), Float32, (Float32,), x)
@vectorize_1arg Real floor

hypot(x::Real, y::Real) = hypot(promote(float(x), float(y))...)
function hypot{T<:FloatingPoint}(x::T, y::T)
    x = abs(x)
    y = abs(y)
    if x < y
        x, y = y, x
    end
    if x == 0
        r = y/one(x)
    else
        r = y/x
        if isnan(r)
            isinf(x) && return x
            isinf(y) && return y
            return r
        end
    end
    x * sqrt(one(r)+r*r)
end

atan2(x::Real, y::Real) = atan2(promote(float(x),float(y))...)
atan2{T<:FloatingPoint}(x::T, y::T) = Base.no_op_err("atan2", T)

for f in (:atan2, :hypot)
    @eval begin
        ($f)(x::Float64, y::Float64) = ccall(($(string(f)),libm), Float64, (Float64, Float64,), x, y)
        ($f)(x::Float32, y::Float32) = ccall(($(string(f,"f")),libm), Float32, (Float32, Float32), x, y)
        @vectorize_2arg Number $f
    end
end

gamma(x::Float64) = nan_dom_err(ccall((:tgamma,libm),  Float64, (Float64,), x), x)
gamma(x::Float32) = nan_dom_err(ccall((:tgammaf,libm),  Float32, (Float32,), x), x)
gamma(x::Real) = gamma(float(x))
@vectorize_1arg Number gamma

function lgamma_r(x::Float64)
    signp = Array(Int32, 1)
    y = ccall((:lgamma_r,libm),  Float64, (Float64, Ptr{Int32}), x, signp)
    return y, signp[1]
end
function lgamma_r(x::Float32)
    signp = Array(Int32, 1)
    y = ccall((:lgammaf_r,libm),  Float32, (Float32, Ptr{Int32}), x, signp)
    return y, signp[1]
end
lgamma_r(x::Real) = lgamma_r(float(x))

lfact(x::Real) = (x<=1 ? zero(float(x)) : lgamma(x+one(x)))
@vectorize_1arg Number lfact

max{T<:FloatingPoint}(x::T, y::T) = ifelse((y > x) | (x != x), y, x)
@vectorize_2arg Real max

min{T<:FloatingPoint}(x::T, y::T) = ifelse((y < x) | (x != x), y, x)
@vectorize_2arg Real min

minmax{T<:FloatingPoint}(x::T, y::T) =  x <= y ? (x, y) :
                                        x >  y ? (y, x) :
                                        x == x ? (x, x) : (y, y)

function exponent(x::Float64)
    if x==0 || !isfinite(x)
        throw(DomainError())
    end
    int(ccall((:ilogb,libm), Int32, (Float64,), x))
end
function exponent(x::Float32)
    if x==0 || !isfinite(x)
        throw(DomainError())
    end
    int(ccall((:ilogbf,libm), Int32, (Float32,), x))
end
@vectorize_1arg Real exponent

ldexp(x::Float64,e::Int) = ccall((:scalbn,libm),  Float64, (Float64,Int32), x, int32(e))
ldexp(x::Float32,e::Int) = ccall((:scalbnf,libm), Float32, (Float32,Int32), x, int32(e))
# TODO: vectorize ldexp

begin
    local exp::Array{Int32,1} = zeros(Int32,1)
    global frexp
    function frexp(x::Float64)
        s = ccall((:frexp,libm), Float64, (Float64, Ptr{Int32}), x, exp)
        (s, int(exp[1]))
    end
    function frexp(x::Float32)
        s = ccall((:frexpf,libm), Float32, (Float32, Ptr{Int32}), x, exp)
        (s, int(exp[1]))
    end
    function frexp(A::Array{Float64})
        f = similar(A)
        e = Array(Int, size(A))
        for i = 1:length(A)
            f[i] = ccall((:frexp,libm), Float64, (Float64, Ptr{Int32}), A[i], exp)
            e[i] = exp[1]
        end
        return (f, e)
    end
    function frexp(A::Array{Float32})
        f = similar(A)
        e = Array(Int, size(A))
        for i = 1:length(A)
            f[i] = ccall((:frexpf,libm), Float32, (Float32, Ptr{Int32}), A[i], exp)
            e[i] = exp[1]
        end
        return (f, e)
    end
end

modf(x) = rem(x,one(x)), trunc(x)

^(x::Float64, y::Float64) = nan_dom_err(ccall((:pow,libm),  Float64, (Float64,Float64), x, y), x+y)
^(x::Float32, y::Float32) = nan_dom_err(ccall((:powf,libm), Float32, (Float32,Float32), x, y), x+y)

^(x::Float64, y::Integer) =
    box(Float64, powi_llvm(unbox(Float64,x), unbox(Int32,int32(y))))
^(x::Float32, y::Integer) =
    box(Float32, powi_llvm(unbox(Float32,x), unbox(Int32,int32(y))))

# special functions

for jy in ("j","y"), nu in (0,1)
    jynu = Expr(:quote, symbol(string(jy,nu)))
    jynuf = Expr(:quote, symbol(string(jy,nu,"f")))
    bjynu = symbol(string("bessel",jy,nu))
    if jy == "y"
        @eval begin
            $bjynu(x::Float64) = nan_dom_err(ccall(($jynu,libm),  Float64, (Float64,), x), x)
            $bjynu(x::Float32) = nan_dom_err(ccall(($jynuf,libm), Float32, (Float32,), x), x)
        end
    else
        @eval begin
            $bjynu(x::Float64) = ccall(($jynu,libm),  Float64, (Float64,), x)
            $bjynu(x::Float32) = ccall(($jynuf,libm), Float32, (Float32,), x)
        end
    end
    @eval begin
        $bjynu(x::Real) = $bjynu(float(x))
        $bjynu(x::Complex) = $(symbol(string("bessel",jy)))($nu,x)
        @vectorize_1arg Number $bjynu
    end
end

type AmosException <: Exception
    info::Int32
end

let
    const ai::Array{Float64,1} = Array(Float64,2)
    const ae::Array{Int32,1} = Array(Int32,2)
global airy
function airy(k::Int, z::Complex128)
    id = int32(k==1 || k==3)
    if k == 0 || k == 1
        ccall((:zairy_,openspecfun), Void,
              (Ptr{Float64}, Ptr{Float64}, Ptr{Int32}, Ptr{Int32},
               Ptr{Float64}, Ptr{Float64}, Ptr{Int32}, Ptr{Int32}),
              &real(z), &imag(z),
              &id, &1,
              pointer(ai,1), pointer(ai,2),
              pointer(ae,1), pointer(ae,2))
        if ae[2] == 0 || ae[2] == 3
            return complex(ai[1],ai[2])
        else
            throw(AmosException(ae[2]))
        end
    elseif k == 2 || k == 3
        ccall((:zbiry_,openspecfun), Void,
              (Ptr{Float64}, Ptr{Float64}, Ptr{Int32}, Ptr{Int32},
               Ptr{Float64}, Ptr{Float64}, Ptr{Int32}),
              &real(z), &imag(z),
              &id, &1,
              pointer(ai,1), pointer(ai,2),
              pointer(ae,1))
        if ae[1] == 0 || ae[1] == 3  # ignore underflow
            return complex(ai[1],ai[2])
        else
            throw(AmosException(ae[2]))
        end
    else
        error("invalid argument")
    end
end
end

airy(z) = airy(0,z)
@vectorize_1arg Number airy
airyprime(z) = airy(1,z)
@vectorize_1arg Number airyprime
airyai(z) = airy(0,z)
@vectorize_1arg Number airyai
airyaiprime(z) = airy(1,z)
@vectorize_1arg Number airyaiprime
airybi(z) = airy(2,z)
@vectorize_1arg Number airybi
airybiprime(z) = airy(3,z)
@vectorize_1arg Number airybiprime

airy(k::Number, x::FloatingPoint) = oftype(x, real(airy(k, complex(x))))
airy(k::Number, x::Real) = airy(k, float(x))
airy(k::Number, z::Complex64) = complex64(airy(k, complex128(z)))
airy(k::Number, z::Complex) = airy(convert(Int,k), complex128(z))
@vectorize_2arg Number airy

const cy = Array(Float64,2)
const ae = Array(Int32,2)
const wrk = Array(Float64,2)

function _besselh(nu::Float64, k::Integer, z::Complex128)
    ccall((:zbesh_,openspecfun), Void,
          (Ptr{Float64}, Ptr{Float64}, Ptr{Float64}, Ptr{Int32}, Ptr{Int32},
           Ptr{Int32}, Ptr{Float64}, Ptr{Float64}, Ptr{Int32}, Ptr{Int32}),
          &real(z), &imag(z), &nu, &1, &k, &1,
          pointer(cy,1), pointer(cy,2),
          pointer(ae,1), pointer(ae,2))
    if ae[2] == 0 || ae[2] == 3
        return complex(cy[1],cy[2])
    else
        throw(AmosException(ae[2]))
    end
end

function _besseli(nu::Float64, z::Complex128)
    ccall((:zbesi_,openspecfun), Void,
          (Ptr{Float64}, Ptr{Float64}, Ptr{Float64}, Ptr{Int32}, Ptr{Int32},
           Ptr{Float64}, Ptr{Float64}, Ptr{Int32}, Ptr{Int32}),
          &real(z), &imag(z), &nu, &1, &1,
          pointer(cy,1), pointer(cy,2),
          pointer(ae,1), pointer(ae,2))
    if ae[2] == 0 || ae[2] == 3
        return complex(cy[1],cy[2])
    else
        throw(AmosException(ae[2]))
    end
end

function _besselj(nu::Float64, z::Complex128)
    ccall((:zbesj_,openspecfun), Void,
          (Ptr{Float64}, Ptr{Float64}, Ptr{Float64}, Ptr{Int32}, Ptr{Int32},
           Ptr{Float64}, Ptr{Float64}, Ptr{Int32}, Ptr{Int32}),
          &real(z), &imag(z), &nu, &1, &1,
          pointer(cy,1), pointer(cy,2),
          pointer(ae,1), pointer(ae,2))
    if ae[2] == 0 || ae[2] == 3
        return complex(cy[1],cy[2])
    else
        throw(AmosException(ae[2]))
    end
end

function _besselk(nu::Float64, z::Complex128)
    ccall((:zbesk_,openspecfun), Void,
          (Ptr{Float64}, Ptr{Float64}, Ptr{Float64}, Ptr{Int32}, Ptr{Int32},
           Ptr{Float64}, Ptr{Float64}, Ptr{Int32}, Ptr{Int32}),
          &real(z), &imag(z), &nu, &1, &1,
          pointer(cy,1), pointer(cy,2),
          pointer(ae,1), pointer(ae,2))
    if ae[2] == 0 || ae[2] == 3
        return complex(cy[1],cy[2])
    else
        throw(AmosException(ae[2]))
    end
end

function _bessely(nu::Float64, z::Complex128)
    ccall((:zbesy_,openspecfun), Void,
          (Ptr{Float64}, Ptr{Float64}, Ptr{Float64}, Ptr{Int32},
           Ptr{Int32}, Ptr{Float64}, Ptr{Float64}, Ptr{Int32},
           Ptr{Float64}, Ptr{Float64}, Ptr{Int32}),
          &real(z), &imag(z), &nu, &1, &1,
          pointer(cy,1), pointer(cy,2),
          pointer(ae,1), pointer(wrk,1),
          pointer(wrk,2), pointer(ae,2))
    if ae[2] == 0 || ae[2] == 3
        return complex(cy[1],cy[2])
    else
        throw(AmosException(ae[2]))
    end
end

function besselh(nu::Float64, k::Integer, z::Complex128)
    if nu < 0
        s = (k == 1) ? 1 : -1
        return _besselh(-nu, k, z) * complex(cospi(nu),-s*sinpi(nu))
    end
    return _besselh(nu, k, z)
end

function besseli(nu::Float64, z::Complex128)
    if nu < 0
        return _besseli(-nu,z) - 2_besselk(-nu,z)*sinpi(nu)/pi
    else
        return _besseli(nu, z)
    end
end

function besselj(nu::Float64, z::Complex128)
    if nu < 0
        return _besselj(-nu,z)cos(pi*nu) + _bessely(-nu,z)*sinpi(nu)
    else
        return _besselj(nu, z)
    end
end

besselj(nu::Integer, x::FloatingPoint) = typemin(Int32) <= nu <= typemax(Int32) ?
    oftype(x, ccall((:jn, libm), Float64, (Cint, Float64), nu, x)) :
    besselj(float64(nu), x)

besselj(nu::Integer, x::Float32) = typemin(Int32) <= nu <= typemax(Int32) ?
    ccall((:jnf, libm), Float32, (Cint, Float32), nu, x) :
    besselj(float64(nu), x)

besselk(nu::Float64, z::Complex128) = _besselk(abs(nu), z)

function bessely(nu::Float64, z::Complex128)
    if nu < 0
        return _bessely(-nu,z)*cospi(nu) - _besselj(-nu,z)*sinpi(nu)
    else
        return _bessely(nu, z)
    end
end

besselh(nu, z) = besselh(nu, 1, z)
besselh(nu::Real, k::Integer, z::Complex64) = complex64(besselh(float64(nu), k, complex128(z)))
besselh(nu::Real, k::Integer, z::Complex) = besselh(float64(nu), k, complex128(z))
besselh(nu::Real, k::Integer, x::Real) = besselh(float64(nu), k, complex128(x))
@vectorize_2arg Number besselh

besseli(nu::Real, z::Complex64) = complex64(besseli(float64(nu), complex128(z)))
besseli(nu::Real, z::Complex) = besseli(float64(nu), complex128(z))
besseli(nu::Real, x::Integer) = besseli(nu, float64(x))
function besseli(nu::Real, x::FloatingPoint)
    if x < 0 && !isinteger(nu)
        throw(DomainError())
    end
    oftype(x, real(besseli(float64(nu), complex128(x))))
end
@vectorize_2arg Number besseli

function besselj(nu::FloatingPoint, x::FloatingPoint)
    if isinteger(nu)
        if typemin(Int32) <= nu <= typemax(Int32)
            return besselj(int(nu), x)
        end
    elseif x < 0
        throw(DomainError())
    end
    oftype(x, real(besselj(float64(nu), complex128(x))))
end

besselj(nu::Real, z::Complex64) = complex64(besselj(float64(nu), complex128(z)))
besselj(nu::Real, z::Complex) = besselj(float64(nu), complex128(z))
besselj(nu::Real, x::Integer) = besselj(nu, float(x))
@vectorize_2arg Number besselj

besselk(nu::Real, z::Complex64) = complex64(besselk(float64(nu), complex128(z)))
besselk(nu::Real, z::Complex) = besselk(float64(nu), complex128(z))
besselk(nu::Real, x::Integer) = besselk(nu, float64(x))
function besselk(nu::Real, x::FloatingPoint)
    if x < 0
        throw(DomainError())
    end
    oftype(x, real(besselk(float64(nu), complex128(x))))
end
@vectorize_2arg Number besselk

bessely(nu::Real, z::Complex64) = complex64(bessely(float64(nu), complex128(z)))
bessely(nu::Real, z::Complex) = bessely(float64(nu), complex128(z))
bessely(nu::Real, x::Integer) = bessely(nu, float64(x))
function bessely(nu::Real, x::FloatingPoint)
    if x < 0
        throw(DomainError())
    end
    if isinteger(nu) && typemin(Int32) <= nu <= typemax(Int32)
        return bessely(int(nu), x)
    end
    oftype(x, real(bessely(float64(nu), complex128(x))))
end
function bessely(nu::Integer, x::FloatingPoint)
    if x < 0
        throw(DomainError())
    end
    return oftype(x, ccall((:yn, libm), Float64, (Cint, Float64), nu, x))
end
function bessely(nu::Integer, x::Float32)
    if x < 0
        throw(DomainError())
    end
    return ccall((:ynf, libm), Float32, (Cint, Float32), nu, x)
end
@vectorize_2arg Number bessely

hankelh1(nu, z) = besselh(nu, 1, z)
@vectorize_2arg Number hankelh1

hankelh2(nu, z) = besselh(nu, 2, z)
@vectorize_2arg Number hankelh2

function angle_restrict_symm(theta)
    const P1 = 4 * 7.8539812564849853515625e-01
    const P2 = 4 * 3.7748947079307981766760e-08
    const P3 = 4 * 2.6951514290790594840552e-15

    y = 2*floor(theta/(2*pi))
    r = ((theta - y*P1) - y*P2) - y*P3
    if (r > pi)
        r -= (2*pi)
    end
    return r
end

const clg_coeff = [76.18009172947146,
                   -86.50532032941677,
                   24.01409824083091,
                   -1.231739572450155,
                   0.1208650973866179e-2,
                   -0.5395239384953e-5]

function clgamma_lanczos(z)
    const sqrt2pi = 2.5066282746310005

    y = x = z
    temp = x + 5.5
    zz = log(temp)
    zz = zz * (x+0.5)
    temp -= zz
    ser = complex(1.000000000190015, 0)
    for j=1:6
        y += 1.0
        zz = clg_coeff[j]/y
        ser += zz
    end
    zz = sqrt2pi*ser / x
    return log(zz) - temp
end

function lgamma(z::Complex)
    if real(z) <= 0.5
        a = clgamma_lanczos(1-z)
        b = log(sinpi(z))
        const logpi = 1.14472988584940017
        z = logpi - b - a
    else
        z = clgamma_lanczos(z)
    end
    complex(real(z), angle_restrict_symm(imag(z)))
end

gamma(z::Complex) = exp(lgamma(z))

# Derivatives of the digamma function
function psifn(x::Float64, n::Int, kode::Int, m::Int)
# Translated from http://www.netlib.org/slatec/src/dpsifn.f
# Note: Underflow handling at 380 in original is skipped
    const nmax = 100
    ans = Array(Float64, m)
#-----------------------------------------------------------------------
#             bernoulli numbers
#-----------------------------------------------------------------------
    const b =                         [1.00000000000000000e+00,
              -5.00000000000000000e-01,1.66666666666666667e-01,
              -3.33333333333333333e-02,2.38095238095238095e-02,
              -3.33333333333333333e-02,7.57575757575757576e-02,
              -2.53113553113553114e-01,1.16666666666666667e+00,
              -7.09215686274509804e+00,5.49711779448621554e+01,
              -5.29124242424242424e+02,6.19212318840579710e+03,
              -8.65802531135531136e+04,1.42551716666666667e+06,
              -2.72982310678160920e+07,6.01580873900642368e+08,
              -1.51163157670921569e+10,4.29614643061166667e+11,
              -1.37116552050883328e+13,4.88332318973593167e+14,
              -1.92965793419400681e+16]
    trm = Array(Float64, 22)
    trmr = Array(Float64, 100)
#***first executable statement  dpsifn
    if x <= 0.0 throw(DomainError()) end
    if n < 0 error("n must be non-negative") end
    if kode < 1 | kode > 2 error("kode must be one or two") end
    if m < 1 error("m must be larger than one") end
    mm = m
    const nx = min(-exponent(realmin(Float64)) + 1, exponent(realmax(Float64)))
    const r1m5 = log10(2)
    const r1m4 = Base.eps(Float64) * 0.5
    const wdtol = max(r1m4, 0.5e-18)
#-----------------------------------------------------------------------
#     elim = approximate exponential over and underflow limit
#-----------------------------------------------------------------------
    const elim = 2.302*(nx*r1m5 - 3.0)
    xln = log(x)
    nn = n + mm - 1
    fn = nn
    t = (fn + 1)*xln
#-----------------------------------------------------------------------
#     overflow and underflow test for small and large x
#-----------------------------------------------------------------------
    if abs(t) > elim
        if t <= 0.0 error("n too large") end
        error("overflow; x too small or n+m-1 too large or both")
    end
    if x < wdtol
        ans[1] = x^(-n - 1)
        if mm != 1
            k = 1
            for i = 2:mm
                ans[k + 1] = ans[k]/x
                k += 1
            end
        end
        if n != 0 return ans end
        if kode == 2 ans[1] = ans[1] + xln end
        return ans
    end
#-----------------------------------------------------------------------
#     compute xmin and the number of terms of the series, fln+1
#-----------------------------------------------------------------------
    rln = r1m5 * precision(x)
    rln = min(rln, 18.06)
    fln = max(rln, 3.0) - 3.0
    yint = 3.50 + 0.40*fln
    slope = 0.21 + fln*(0.0006038*fln + 0.008677)
    xm = yint + slope*fn
    mx = itrunc(xm) + 1
    xmin = mx
    if n != 0
        xm = -2.302*rln - min(0.0,xln)
        arg = xm/n
        arg = min(0.0,arg)
        eps = exp(arg)
        xm = 1.0 - eps
        if abs(arg) < 1.0e-3 xm = -arg end
        fln = x*xm/eps
        xm = xmin - x
        if (xm > 7.0) & (fln < 15.0)
            nn = itrunc(fln) + 1
            np = n + 1
            t1 = (n + 1)*xln
            t = exp(-t1)
            s = t
            den = x
            for i = 1:nn
                den += 1.0
                trm[i] = den^(-np)
                s += trm[i]
            end
            ans[1] = s
            if n == 0
                if kode == 2 ans[1] = s + xln end
            end
            if mm == 1 return ans end
#-----------------------------------------------------------------------
#     generate higher derivatives, j.gt.n
#-----------------------------------------------------------------------
            tol = wdtol/5.0
            for j = 2:mm
                t = t/x
                s = t
                tols = t*tol
                den = x
                for i = 1:nn
                    den += 1.0
                    trm[i] = trm[i]/den
                    s += trm[i]
                    if trm[i] < tols break end
                end
                ans[j] = s
            end
            return ans
        end
    end

    xdmy = x
    xdmln = xln
    xinc = 0.0
    if x < xmin
        nx = itrunc(x)
        xinc = xmin - nx
        xdmy = x + xinc
        xdmln = log(xdmy)
    end
#-----------------------------------------------------------------------
#     generate w(n+mm-1,x) by the asymptotic expansion
#-----------------------------------------------------------------------
    t = fn*xdmln
    t1 = xdmln + xdmln
    t2 = t + xdmln
    tk = max(abs(t), abs(t1), abs(t2))
    if tk > elim error("underflow") end
    tss = exp(-t)
    tt = 0.5/xdmy
    t1 = tt
    tst = wdtol*tt
    if nn != 0 t1 = tt + 1.0/fn end
    rxsq = 1.0/(xdmy*xdmy)
    ta = 0.5*rxsq
    t = (fn + 1)*ta
    s = t*b[3]
    if abs(s) >= tst
        tk = 2.0
        for k = 4:22
            t = t*((tk + fn + 1)/(tk + 1.0))*((tk + fn)/(tk + 2.0))*rxsq
            trm[k] = t*b[k]
            if abs(trm[k]) < tst break end
            s += trm[k]
            tk += 2.0
        end
    end
    s = (s + t1)*tss
    while true
        if xinc != 0.0
#-----------------------------------------------------------------------
#     backward recur from xdmy to x
#-----------------------------------------------------------------------
            nx = itrunc(xinc)
            np = nn + 1
            if nx > nmax error("n too large") end
            if nn == 0 break end
            xm = xinc - 1.0
            fx = x + xm
#-----------------------------------------------------------------------
#     this loop should not be changed. fx is accurate when x is small
#-----------------------------------------------------------------------
            for i = 1:nx
                trmr[i] = fx^(-np)
                s += trmr[i]
                xm -= 1.0
                fx = x + xm
            end
        end
        ans[mm] = s
        if fn == 0
            if kode != 2
                ans[1] = s - xdmln
                return ans
            end
            if xdmy == x return ans end
            xq = xdmy/x
            ans[1] = s - log(xq)
            return ans
        end
#-----------------------------------------------------------------------
#     generate lower derivatives, j.lt.n+mm-1
#-----------------------------------------------------------------------
        if mm == 1 return ans end
        for j = 2:mm
            fn -= 1
            tss *= xdmy
            t1 = tt
            if fn != 0 t1 = tt + 1.0/fn end
            t = (fn + 1)*ta
            s = t*b[3]
            if abs(s) >= tst
                tk = 4 + fn
                for k = 4:22 #110
                    trm[k] = trm[k]*(fn + 1)/tk
                    if abs(trm[k]) < tst break end
                    s += trm[k]
                    tk += 2.0
                end
            end
            s = (s + t1)*tss
            if xinc != 0.0
                if fn == 0 break end
                xm = xinc - 1.0
                fx = x + xm
                for i = 1:nx
                    trmr[i] = trmr[i]*fx
                    s += trmr[i]
                    xm -= 1.0
                    fx = x + xm
                end
            end
            mx = mm - j + 1
            ans[mx] = s
            if fn == 0
                if kode != 2
                    ans[1] = s - xdmln
                    return ans
                end
                if xdmy == x return ans end
                xq = xdmy/x
                ans[1] = s - log(xq)
                return ans
            end
        end
        if fn == 0 break end
        return ans
    end
#-----------------------------------------------------------------------
#     recursion for n = 0
#-----------------------------------------------------------------------
    for i = 1:nx
        s += 1.0/(x + nx - i)
    end
    if kode != 2
        ans[1] = s - xdmln
        return ans
    end
    if xdmy == x return ans end
    xq = xdmy/x
    ans[1] = s - log(xq)
    return ans
end
polygamma(k::Int, x::Float64) = (2rem(k,2) - 1)*psifn(x, k, 1, 1)[1]/gamma(k + 1)
polygamma(k::Int, x::Float32) = float32(polygamma(k, float64(x)))
polygamma(k::Int, x::Real) = polygamma(k, float64(x))

# Translation of psi.c from cephes
function digamma(x::Float64)
    negative = false
    nz = 0.0

    if x <= 0.0
        negative = true
        q = x
        p = floor(q)
        if p == q
            return NaN
        end

        nz = q - p
        if nz != 0.5
            if nz > 0.5
                p += 1.0
                nz = q - p
            end
            nz = pi / tan(pi * nz)
        else
            nz = 0.0
        end
        x = 1.0 - x
    end

    if x <= 10.0 && x == floor(x)
        y = 0.0
        for i = 1:x-1
            y += 1.0 / i
        end
        y -= γ  # γ == -digamma(1) == 0.5772156649015328606065121;

        if negative
            y -= nz
        end
        return y
    end

    w = 0.0
    while x < 10.0
        w += 1.0 / x
        x += 1.0
    end

    if x < 1.0e17
        z = 1.0 / (x*x)
        y = @horner(z, 8.33333333333333333333e-2, -8.33333333333333333333e-3, 3.96825396825396825397e-3,
                       -4.16666666666666666667e-3, 7.57575757575757575758e-3,-2.10927960927960927961e-2,
                       8.33333333333333333333e-2)
        y *= z
    else
        y = 0.0
    end

    y = log(x) - 0.5/x - y - w

    if negative
        y -= nz
    end

    return y
end
digamma(x::Float32) = float32(digamma(float64(x)))
digamma(x::Real) = digamma(float64(x))
@vectorize_1arg Real digamma

trigamma(x::Real) = polygamma(1, x)
@vectorize_1arg Real trigamma

# Inverse digamma function
#
# Implementation of fixed point algorithm described in
#  "Estimating a Dirichlet distribution" by Thomas P. Minka, 2000
function invdigamma(y::Float64)
    # Closed form initial estimates
    if y >= -2.22
        x_old = exp(y) + 0.5
        x_new = x_old
    else
        x_old = -1.0 / (y - digamma(1.0))
        x_new = x_old
    end

    # Fixed point algorithm
    delta = Inf
    iteration = 0
    while delta > 1e-12 && iteration < 25
        iteration += 1
        x_new = x_old - (digamma(x_old) - y) / trigamma(x_old)
        delta = abs(x_new - x_old)
        x_old = x_new
    end

    return x_new
end
invdigamma(x::Float32) = float32(invdigamma(float64(x)))
invdigamma(x::Real) = invdigamma(float64(x))
@vectorize_1arg Real invdigamma

function beta(x::Number, w::Number)
    yx, sx = lgamma_r(x)
    yw, sw = lgamma_r(w)
    yxw, sxw = lgamma_r(x+w)
    return copysign(exp(yx + yw - yxw), sx*sw*sxw)
end
lbeta(x::Number, w::Number) = lgamma(x)+lgamma(w)-lgamma(x+w)
@vectorize_2arg Number beta
@vectorize_2arg Number lbeta

const eta_coeffs =
    [.99999999999999999997,
     -.99999999999999999821,
     .99999999999999994183,
     -.99999999999999875788,
     .99999999999998040668,
     -.99999999999975652196,
     .99999999999751767484,
     -.99999999997864739190,
     .99999999984183784058,
     -.99999999897537734890,
     .99999999412319859549,
     -.99999996986230482845,
     .99999986068828287678,
     -.99999941559419338151,
     .99999776238757525623,
     -.99999214148507363026,
     .99997457616475604912,
     -.99992394671207596228,
     .99978893483826239739,
     -.99945495809777621055,
     .99868681159465798081,
     -.99704078337369034566,
     .99374872693175507536,
     -.98759401271422391785,
     .97682326283354439220,
     -.95915923302922997013,
     .93198380256105393618,
     -.89273040299591077603,
     .83945793215750220154,
     -.77148960729470505477,
     .68992761745934847866,
     -.59784149990330073143,
     .50000000000000000000,
     -.40215850009669926857,
     .31007238254065152134,
     -.22851039270529494523,
     .16054206784249779846,
     -.10726959700408922397,
     .68016197438946063823e-1,
     -.40840766970770029873e-1,
     .23176737166455607805e-1,
     -.12405987285776082154e-1,
     .62512730682449246388e-2,
     -.29592166263096543401e-2,
     .13131884053420191908e-2,
     -.54504190222378945440e-3,
     .21106516173760261250e-3,
     -.76053287924037718971e-4,
     .25423835243950883896e-4,
     -.78585149263697370338e-5,
     .22376124247437700378e-5,
     -.58440580661848562719e-6,
     .13931171712321674741e-6,
     -.30137695171547022183e-7,
     .58768014045093054654e-8,
     -.10246226511017621219e-8,
     .15816215942184366772e-9,
     -.21352608103961806529e-10,
     .24823251635643084345e-11,
     -.24347803504257137241e-12,
     .19593322190397666205e-13,
     -.12421162189080181548e-14,
     .58167446553847312884e-16,
     -.17889335846010823161e-17,
     .27105054312137610850e-19]

function eta(z::Union(Float64,Complex128))
    if z == 0
        return oftype(z, 0.5)
    end
    re, im = reim(z)
    if im==0 && re < 0 && re==round(re/2)*2
        return zero(z)
    end
    reflect = false
    if re < 0.5
        z = 1-z
        reflect = true
    end
    s = zero(z)
    for n = length(eta_coeffs):-1:1
        c = eta_coeffs[n]
        p = n^-z
        s += c * p
    end
    if reflect
        z2 = 2.0^z
        b = 2.0 - (2.0*z2)
        f = z2 - 2
        piz = pi^z

        b = b/f/piz

        return s * gamma(z) * b * cospi(z/2)
    end
    return s
end

eta(x::Integer) = eta(float64(x))
eta(x::Real)    = oftype(x,eta(float64(x)))
eta(z::Complex) = oftype(z,eta(complex128(z)))
@vectorize_1arg Number eta

function zeta(z::Number)
    zz = 2^z
    eta(z) * zz/(zz-2)
end
@vectorize_1arg Number zeta

@unix_only if WORD_SIZE == 64
# TODO: complex return only on 64-bit for now
for f in (:erf, :erfc, :erfcx, :erfi, :Dawson)
    fname = (f === :Dawson) ? :dawson : f
    @eval begin
        ($fname)(z::Complex128) = complex128(ccall(($(string("Faddeeva_",f)),openspecfun), Complex{Float64}, (Complex{Float64}, Float64), z, zero(Float64)))
        ($fname)(z::Complex64) = complex64(ccall(($(string("Faddeeva_",f)),openspecfun), Complex{Float64}, (Complex{Float64}, Float64), complex128(z), float64(eps(Float32))))
        ($fname)(z::Complex) = ($fname)(complex128(z))
    end
end
end
for f in (:erfcx, :erfi, :Dawson)
    fname = (f === :Dawson) ? :dawson : f
    @eval begin
        ($fname)(x::Float64) = ccall(($(string("Faddeeva_",f,"_re")),openspecfun), Float64, (Float64,), x)
        ($fname)(x::Float32) = float32(ccall(($(string("Faddeeva_",f,"_re")),openspecfun), Float64, (Float64,), float64(x)))
        ($fname)(x::Integer) = ($fname)(float(x))
        @vectorize_1arg Number $fname
    end
end

# Compute the inverse of the error function: erf(erfinv(x)) == x,
# using the rational approximants tabulated in:
#     J. M. Blair, C. A. Edwards, and J. H. Johnson, "Rational Chebyshev
#     approximations for the inverse of the error function," Math. Comp. 30,
#     pp. 827--830 (1976).
#         http://dx.doi.org/10.1090/S0025-5718-1976-0421040-7
#         http://www.jstor.org/stable/2005402
function erfinv(x::Float64)
    a = abs(x)
    if a >= 1.0
        if x == 1.0
            return inf(Float64)
        elseif x == -1.0
            return -inf(Float64)
        end
        throw(DomainError())
    elseif a <= 0.75 # Table 17 in Blair et al.
        t = x*x - 0.5625
        return x * @horner(t, 0.16030_49558_44066_229311e2,
                             -0.90784_95926_29603_26650e2,
                              0.18644_91486_16209_87391e3,
                             -0.16900_14273_46423_82420e3,
                              0.65454_66284_79448_7048e2,
                             -0.86421_30115_87247_794e1,
                              0.17605_87821_39059_0) /
                   @horner(t, 0.14780_64707_15138_316110e2,
                             -0.91374_16702_42603_13936e2,
                              0.21015_79048_62053_17714e3,
                             -0.22210_25412_18551_32366e3,
                              0.10760_45391_60551_23830e3,
                             -0.20601_07303_28265_443e2,
                              0.1e1)
    elseif a <= 0.9375 # Table 37 in Blair et al.
        t = x*x - 0.87890625
        return x * @horner(t, -0.15238_92634_40726_128e-1,
                               0.34445_56924_13612_5216,
                              -0.29344_39867_25424_78687e1,
                               0.11763_50570_52178_27302e2,
                              -0.22655_29282_31011_04193e2,
                               0.19121_33439_65803_30163e2,
                              -0.54789_27619_59831_8769e1,
                               0.23751_66890_24448) /
                   @horner(t, -0.10846_51696_02059_954e-1,
                               0.26106_28885_84307_8511,
                              -0.24068_31810_43937_57995e1,
                               0.10695_12997_33870_14469e2,
                              -0.23716_71552_15965_81025e2,
                               0.24640_15894_39172_84883e2,
                              -0.10014_37634_97830_70835e2,
                               0.1e1)
    else # Table 57 in Blair et al.
        t = 1.0 / sqrt(-log(1.0 - a))
        return @horner(t, 0.10501_31152_37334_38116e-3,
                          0.10532_61131_42333_38164_25e-1,
                          0.26987_80273_62432_83544_516,
                          0.23268_69578_89196_90806_414e1,
                          0.71678_54794_91079_96810_001e1,
                          0.85475_61182_21678_27825_185e1,
                          0.68738_08807_35438_39802_913e1,
                          0.36270_02483_09587_08930_02e1,
                          0.88606_27392_96515_46814_9) /
              (copysign(t, x) *
               @horner(t, 0.10501_26668_70303_37690e-3,
                          0.10532_86230_09333_27531_11e-1,
                          0.27019_86237_37515_54845_553,
                          0.23501_43639_79702_53259_123e1,
                          0.76078_02878_58012_77064_351e1,
                          0.11181_58610_40569_07827_3451e2,
                          0.11948_78791_84353_96667_8438e2,
                          0.81922_40974_72699_07893_913e1,
                          0.40993_87907_63680_15361_45e1,
                          0.1e1))
    end
end

function erfinv(x::Float32)
    a = abs(x)
    if a >= 1.0f0
        if x == 1.0f0
            return inf(Float32)
        elseif x == -1.0f0
            return -inf(Float32)
        end
        throw(DomainError())
    elseif a <= 0.75f0 # Table 10 in Blair et al.
        t = x*x - 0.5625f0
        return x * @horner(t, -0.13095_99674_22f2,
                               0.26785_22576_0f2,
                              -0.92890_57365f1) /
                   @horner(t, -0.12074_94262_97f2,
                               0.30960_61452_9f2,
                              -0.17149_97799_1f2,
                               0.1f1)
    elseif a <= 0.9375f0 # Table 29 in Blair et al.
        t = x*x - 0.87890625f0
        return x * @horner(t, -0.12402_56522_1f0,
                               0.10688_05957_4f1,
                              -0.19594_55607_8f1,
                               0.42305_81357f0) /
                   @horner(t, -0.88276_97997f-1,
                               0.89007_43359f0,
                              -0.21757_03119_6f1,
                               0.1f1)
    else # Table 50 in Blair et al.
        t = 1.0f0 / sqrt(-log(1.0f0 - a))
        return @horner(t, 0.15504_70003_116f0,
                          0.13827_19649_631f1,
                          0.69096_93488_87f0,
                         -0.11280_81391_617f1,
                          0.68054_42468_25f0,
                         -0.16444_15679_1f0) /
              (copysign(t, x) *
               @horner(t, 0.15502_48498_22f0,
                          0.13852_28141_995f1,
                          0.1f1))
    end
end

erfinv(x::Integer) = erfinv(float(x))
@vectorize_1arg Real erfinv

# Inverse complementary error function: use Blair tables for y = 1-x,
# exploiting the greater accuracy of y (vs. x) when y is small.
function erfcinv(y::Float64)
    if y > 0.0625
        return erfinv(1.0 - y)
    elseif y <= 0.0
        if y == 0.0
            return inf(Float64)
        end
        throw(DomainError())
    elseif y >= 1e-100 # Table 57 in Blair et al.
        t = 1.0 / sqrt(-log(y))
        return @horner(t, 0.10501_31152_37334_38116e-3,
                          0.10532_61131_42333_38164_25e-1,
                          0.26987_80273_62432_83544_516,
                          0.23268_69578_89196_90806_414e1,
                          0.71678_54794_91079_96810_001e1,
                          0.85475_61182_21678_27825_185e1,
                          0.68738_08807_35438_39802_913e1,
                          0.36270_02483_09587_08930_02e1,
                          0.88606_27392_96515_46814_9) /
              (t *
               @horner(t, 0.10501_26668_70303_37690e-3,
                          0.10532_86230_09333_27531_11e-1,
                          0.27019_86237_37515_54845_553,
                          0.23501_43639_79702_53259_123e1,
                          0.76078_02878_58012_77064_351e1,
                          0.11181_58610_40569_07827_3451e2,
                          0.11948_78791_84353_96667_8438e2,
                          0.81922_40974_72699_07893_913e1,
                          0.40993_87907_63680_15361_45e1,
                          0.1e1))
    else # Table 80 in Blair et al.
        t = 1.0 / sqrt(-log(y))
        return @horner(t, 0.34654_29858_80863_50177e-9,
                          0.25084_67920_24075_70520_55e-6,
                          0.47378_13196_37286_02986_534e-4,
                          0.31312_60375_97786_96408_3388e-2,
                          0.77948_76454_41435_36994_854e-1,
                          0.70045_68123_35816_43868_271e0,
                          0.18710_42034_21679_31668_683e1,
                          0.71452_54774_31351_45428_3e0) /
          (t * @horner(t, 0.34654_29567_31595_11156e-9,
                          0.25084_69079_75880_27114_87e-6,
                          0.47379_53129_59749_13536_339e-4,
                          0.31320_63536_46177_68848_0813e-2,
                          0.78073_48906_27648_97214_733e-1,
                          0.70715_04479_95337_58619_993e0,
                          0.19998_51543_49112_15105_214e1,
                          0.15072_90269_27316_80008_56e1,
                          0.1e1))
    end
end

function erfcinv(y::Float32)
    if y > 0.0625f0
        return erfinv(1.0f0 - y)
    elseif y <= 0.0f0
        if y == 0.0f0
            return inf(Float32)
        end
        throw(DomainError())
    else # Table 50 in Blair et al.
        t = 1.0f0 / sqrt(-log(y))
        return @horner(t, 0.15504_70003_116f0,
                          0.13827_19649_631f1,
                          0.69096_93488_87f0,
                         -0.11280_81391_617f1,
                          0.68054_42468_25f0,
                         -0.16444_15679_1f0) /
        (t *
         @horner(t, 0.15502_48498_22f0,
                    0.13852_28141_995f1,
                    0.1f1))
    end
end

erfcinv(x::Integer) = erfcinv(float(x))
@vectorize_1arg Real erfcinv

## mod2pi-related calculations ##

function add22condh(xh::Float64, xl::Float64, yh::Float64, yl::Float64)
    # as above, but only compute and return high double
    r = xh+yh
    s = (abs(xh) > abs(yh)) ? (xh-r+yh+yl+xl) : (yh-r+xh+xl+yl)
    zh = r+s
    return zh
end

function ieee754_rem_pio2(x::Float64)
    # rem_pio2 essentially computes x mod pi/2 (ie within a quarter circle)
    # and returns the result as
    # y between + and - pi/4 (for maximal accuracy (as the sign bit is exploited)), and
    # n, where n specifies the integer part of the division, or, at any rate,
    # in which quadrant we are.
    # The invariant fulfilled by the returned values seems to be
    #  x = y + n*pi/2 (where y = y1+y2 is a double-double and y2 is the "tail" of y).
    # Note: for very large x (thus n), the invariant might hold only modulo 2pi
    # (in other words, n might be off by a multiple of 4, or a multiple of 100)

    # this is just wrapping up
    # https://github.com/JuliaLang/openlibm/blob/master/src/e_rem_pio2.c

    y = [0.0,0.0]
    n = ccall((:__ieee754_rem_pio2, libm), Cint, (Float64,Ptr{Float64}), x, y)
    return (n,y)
end

# multiples of pi/2, as double-double (ie with "tail")
const pi1o2_h  = 1.5707963267948966     # convert(Float64, pi * BigFloat(1/2))
const pi1o2_l  = 6.123233995736766e-17  # convert(Float64, pi * BigFloat(1/2) - pi1o2_h)

const pi2o2_h  = 3.141592653589793      # convert(Float64, pi * BigFloat(1))
const pi2o2_l  = 1.2246467991473532e-16 # convert(Float64, pi * BigFloat(1) - pi2o2_h)

const pi3o2_h  = 4.71238898038469       # convert(Float64, pi * BigFloat(3/2))
const pi3o2_l  = 1.8369701987210297e-16 # convert(Float64, pi * BigFloat(3/2) - pi3o2_h)

const pi4o2_h  = 6.283185307179586      # convert(Float64, pi * BigFloat(2))
const pi4o2_l  = 2.4492935982947064e-16 # convert(Float64, pi * BigFloat(2) - pi4o2_h)

function mod2pi(x::Float64) # or modtau(x)
# with r = mod2pi(x)
# a) 0 <= r < 2π  (note: boundary open or closed - a bit fuzzy, due to rem_pio2 implementation)
# b) r-x = k*2π with k integer

# note: mod(n,4) is 0,1,2,3; while mod(n-1,4)+1 is 1,2,3,4.
# We use the latter to push negative y in quadrant 0 into the positive (one revolution, + 4*pi/2)

    if x < pi4o2_h
        if 0.0 <= x return x end
        if x > -pi4o2_h
            return add22condh(x,0.0,pi4o2_h,pi4o2_l)
        end
    end

    (n,y) = ieee754_rem_pio2(x)

    if iseven(n)
        if n & 2 == 2 # add pi
            return add22condh(y[1],y[2],pi2o2_h,pi2o2_l)
        else # add 0 or 2pi
            if y[1] > 0.0
                return y[1]
            else # else add 2pi
                return add22condh(y[1],y[2],pi4o2_h,pi4o2_l)
            end
        end
    else # add pi/2 or 3pi/2
        if n & 2 == 2 # add 3pi/2
            return add22condh(y[1],y[2],pi3o2_h,pi3o2_l)
        else # add pi/2
            return add22condh(y[1],y[2],pi1o2_h,pi1o2_l)
        end
    end
end

mod2pi(x::Float32) = float32(mod2pi(float64(x)))
mod2pi(x::Int32) = mod2pi(float64(x))
function mod2pi(x::Int64)
  fx = float64(x)
  fx == x || error("Integer argument to mod2pi is too large: $x")
  mod2pi(fx)
end

end # module

max_stars_repo_name: tobig/julia

max_issues_repo_name: tobig/julia

max_forks_repo_name: tobig/julia


text:
using DataFrames

function save_trace(path, x, init_approx_d, approx_d, true_d,
                    iter_vec, α_vec,
                    eval_vec, KL_vec)
  pdf_init_approx = pdf(init_approx_d, x)
  pdf_approx = pdf(approx_d, x)
  pdf_true = pdf(true_d, x)
  df_pdf = DataFrame(x = x,
                     pdf_init = pdf(init_approx_d, x),
                     pdf_approx = pdf(approx_d, x),
                     pdf_true = pdf(true_d, x))
  mkpath(path)
  writetable(joinpath(path, "density.csv"), df_pdf)
  df_alpha = DataFrame(iter = iter_vec,
                       alpha = α_vec)
  writetable(joinpath(path, "alpha.csv"), df_alpha)
  df_KL = DataFrame(KL_iter = eval_vec, KL = KL_vec)
  writetable(joinpath(path, "KL.csv"), df_KL)
  println("Resuls saved to ", path)
end

approx_d = init_approx_d

α_vec = zeros(max_iter)
μ_vec = zeros(max_iter)

iter_vec = collect(1:max_iter)
eval_interval = 2
eval_on = map(i->(i%eval_interval==1), 1:max_iter)
eval_vec = iter_vec[eval_on]
elbo_vec = Vector{Float64}()

for iter in 1:max_iter
    if iter%30==0
        println(@sprintf "iter %d" iter)
    end
    # get new component by matching to functional gradient
    try
        h = BVB.get_new_component_laplacian(ll, approx_d,
                                        lower=lower, upper=upper, verbose=true)
    catch x
        println(x)
        continue
    end
    println(h)
    α = BVB.search_α_exclusive_sgd(approx_d, h, ll, n=100, verbose=true)
    approx_d = BVB.add_to_mixture(approx_d, h, α)

    # tracing
    α_vec[iter] = α
    μ_vec[iter] = mean(h)
    if eval_on[iter]
        push!(elbo_vec, BVB.ELBO(approx_d, ll))
    end
end

figure()
subplot(3,2,1)
plot(x, exp(ll(x)), label="true")
plot(x, pdf(init_approx_d, x), label="init approx")
plot(x, pdf(approx_d, x), label="approximation")

subplot(3,2,2)
xlabel("iter"); ylabel("alpha")
vlines(iter_vec, 0, α_vec)

subplot(3,2,3)
plot(1:length(probs(approx_d)), sort(probs(approx_d), rev=true), ".-")
xlabel("component"); ylabel("weight")

subplot(3,2,4)
plot(iter_vec, μ_vec, ".")
xlabel("iter"); ylabel("mean of h")

subplot(3,2,5)
plot(x, ll(x) - logpdf(approx_d,x), "-")
axhline(y=0., ls="--", color="red")
xlabel("x"); ylabel("log L - log p")

subplot(3, 2, 6)
plot(eval_vec, -elbo_vec, ".-")
xlabel("iter"); ylabel("KL")

figure()
BVB.plot_mixture_sequential(approx_d, x)

ext: jl

max_stars_repo_name: richardkwo/boosting-VB

max_issues_repo_name: richardkwo/boosting-VB

max_forks_repo_name: richardkwo/boosting-VB


text:
#  Copyright 2017, Iain Dunning, Joey Huchette, Miles Lubin, and contributors
#  This Source Code Form is subject to the terms of the Mozilla Public
#  License, v. 2.0. If a copy of the MPL was not distributed with this
#  file, You can obtain one at https://mozilla.org/MPL/2.0/.
#############################################################################
# JuMP
# An algebraic modeling language for Julia
# See https://github.com/jump-dev/JuMP.jl
#############################################################################

"""
    AbstractShape

Abstract vectorizable shape. Given a flat vector form of an object of shape
`shape`, the original object can be obtained by [`reshape_vector`](@ref).
"""
abstract type AbstractShape end

"""
    dual_shape(shape::AbstractShape)::AbstractShape

Returns the shape of the dual space of the space of objects of shape `shape`. By
default, the `dual_shape` of a shape is itself. See the examples section below
for an example for which this is not the case.

## Examples

Consider polynomial constraints for which the dual is moment constraints and
moment constraints for which the dual is polynomial constraints. Shapes for
polynomials can be defined as follows:
```julia
struct Polynomial
    coefficients::Vector{Float64}
    monomials::Vector{Monomial}
end
struct PolynomialShape <: AbstractShape
    monomials::Vector{Monomial}
end
JuMP.reshape_vector(x::Vector, shape::PolynomialShape) = Polynomial(x, shape.monomials)
```
and a shape for moments can be defined as follows:
```julia
struct Moments
    coefficients::Vector{Float64}
    monomials::Vector{Monomial}
end
struct MomentsShape <: AbstractShape
    monomials::Vector{Monomial}
end
JuMP.reshape_vector(x::Vector, shape::MomentsShape) = Moments(x, shape.monomials)
```
Then `dual_shape` allows the definition of the shape of the dual of polynomial
and moment constraints:
```julia
dual_shape(shape::PolynomialShape) = MomentsShape(shape.monomials)
dual_shape(shape::MomentsShape) = PolynomialShape(shape.monomials)
```
"""
dual_shape(shape::AbstractShape) = shape

"""
    reshape_set(vectorized_set::MOI.AbstractSet, shape::AbstractShape)

Return a set in its original shape `shape` given its vectorized form
`vectorized_form`.

## Examples

Given a [`SymmetricMatrixShape`](@ref) of vectorized form
`[1, 2, 3] in MOI.PositiveSemidefinieConeTriangle(2)`, the
following code returns the set of the original constraint
`Symmetric(Matrix[1 2; 2 3]) in PSDCone()`:
```jldoctest; setup = :(using JuMP)
julia> reshape_set(MOI.PositiveSemidefiniteConeTriangle(2), SymmetricMatrixShape(2))
PSDCone()
```
"""
function reshape_set end

"""
    reshape_vector(vectorized_form::Vector, shape::AbstractShape)

Return an object in its original shape `shape` given its vectorized form
`vectorized_form`.

## Examples

Given a [`SymmetricMatrixShape`](@ref) of vectorized form `[1, 2, 3]`, the
following code returns the matrix `Symmetric(Matrix[1 2; 2 3])`:
```jldoctest; setup = :(using JuMP)
julia> reshape_vector([1, 2, 3], SymmetricMatrixShape(2))
2×2 LinearAlgebra.Symmetric{Int64,Array{Int64,2}}:
 1  2
 2  3
```
"""
function reshape_vector end

"""
    shape(c::AbstractConstraint)::AbstractShape

Return the shape of the constraint `c`.
"""
function shape end

"""
    ScalarShape

Shape of scalar constraints.
"""
struct ScalarShape <: AbstractShape end
reshape_vector(α, ::ScalarShape) = α

"""
    VectorShape

Vector for which the vectorized form corresponds exactly to the vector given.
"""
struct VectorShape <: AbstractShape end
reshape_vector(vectorized_form, ::VectorShape) = vectorized_form
vectorize(x, ::VectorShape) = x

max_stars_repo_name: ethansaxenian/RosettaDecode

max_issues_repo_name: ethansaxenian/RosettaDecode

max_forks_repo_name: ethansaxenian/RosettaDecode


text:
#===========================================================================================
    \begin{tags}
    Tools for declaring tags
===========================================================================================#
macro projecttag(name::Symbol)
    esc(:(struct $name <: ProjectTag end))
end

macro tag(name::Symbol)
    esc(:(struct $name <: Tag end))
end

macro tabletag(name::Symbol)
    esc(:(struct $name <: TableTag end))
end
#===========================================================================================
    \end{tags}
===========================================================================================#

#============================================================================================
    \begin{basic interface}
============================================================================================#
metadata(ds::AbstractDataSet) = ds.metadata

Base.keys(ds::AbstractDataSet) = keys(ds.data)
Base.getindex(ds::AbstractDataSet, ::Type{T}) where {T<:Tag} = ds.data[T]
Base.setindex!(ds::AbstractDataSet, v, ::Type{T}) where {T<:Tag} = setindex!(ds, v, T)

Base.empty(ds::AbstractDataSet) = empty(ds.data)
Base.empty!(ds::AbstractDataSet) = empty!(ds.data)
Base.length(ds::AbstractDataSet) = length(ds.data)

"""
    alltags(ds::AbstractDataSet)

Returns a complete list of relevant tags for dataset `ds`.

This should be defined when implementing TheDataMustFlow interface.
"""
alltags(ds::AbstractDataSet) = Type[]

"""
    iscomplete(ds::AbstractDataSet, tags=alltags(ds))

Checks whether the dataset `ds` has all tags listed by `tags`.  By default, this will be
whatever is defined as the complete set of tags by the `alltags` function.
"""
function iscomplete(ds::AbstractDataSet, tags::AbstractVector=alltags(ds))
    all(t ∈ tags for t ∈ keys(ds))
end

"""
    initialize!(ds::AbstractDataSet)
    initialize!(dr::DataRemote, ds::AbstractDataSet)

Perform initialization actions on dataset `ds`.
"""
initialize!(dr::DataRemote, ds::AbstractDataSet) = ds
initialize!(ds::AbstractDataSet) = ds
#============================================================================================
    \end{basic interface}
============================================================================================#

#============================================================================================
    <loading>
============================================================================================#
"""
    load(dr::DataRemote, ds::AbstractDataSet, ::Type{T}) where {T<:Tag}

Load the object tagged by `T` from the remote source `dr` using metadata from dataset
`ds`.
"""
load(dr::DataRemote, m::AbstractMetaData, ::Type{<:Tag}) = missing
load(dr::DataRemote, ds::AbstractDataSet, ::Type{T}) where {T<:Tag} = load(dr, ds.metadata, T)

function load(dr::DataRemote, ds::AbstractDataSet, ::Type{T}) where {T<:TableTag}
    v = loadinit(dr, ds, T)
    ismissing(v) && (return missing)
    v = prep(dr, ds, T, v)
    v = aggregate(dr, ds, T, v)
    postprep(dr, ds, T, v)
end

prep(dr::DataRemote, ds::AbstractDataSet, ::Type{<:Tag}, v) = v
aggregate(dr::DataRemote, ds::AbstractDataSet, ::Type{<:Tag}, v) = v
postprep(dr::DataRemote, ds::AbstractDataSet, ::Type{<:Tag}, v) = v

"""
    load!(dr::DataRemote, ds::AbstractDataSet, ::Type{T}) where {T<:Tag}

Load the object tagged by `T` from the remote source `dr` using metadata from dataset `ds`
and store it in the dataset.  Will be skipped silently if object is `missing`.

# Example
```julia
load!(dr, ds, T)
ds[T]  # this now retrieves the object tagged by `T`
```
"""
function load!(dr::DataRemote, ds::AbstractDataSet, ::Type{T}) where {T<:Tag}
    v = load(dr, ds, T)
    ismissing(v) || (ds[T] = v)
    v
end

"""
    load!(dr::DataRemote, ds::AbstractDataSet, tags=alltags(ds))

Load data tagged by the tags `tags` from the source `dr` into the dataset `ds`.
By default this will be done for the set of tags returned by `alltags`.
"""
function load!(dr::DataRemote, ds::AbstractDataSet, tags::AbstractVector=alltags(ds))
    for t ∈ tags
        load!(dr, ds, t)
        @info("Loaded $t")
    end
    ds
end
#============================================================================================
    </loading>
============================================================================================#

#============================================================================================
    <saving>
============================================================================================#
"""
    save(dr::DataRemote, ds::AbstractDataSet, ::Type{T}) where {T<:Tag}

Save data tagged by `T` in the dataset `ds` to `dr`.  By default this function will call
back to a call to `save(dr, T, ds[T])`.  That method should be defined by packages
implementing TheDataMustFlow interface.
"""
save(dr::DataRemote, ds::AbstractDataSet, ::Type{T}) where {T<:Tag} = save(dr, T, ds[T])

function save(dr::DataRemote, ds::AbstractDataSet, tags::AbstractVector=alltags(ds))
    for t ∈ tags
        save(dr, ds, t)
        @info("Saved $t")
    end
    ds
end
#============================================================================================
    </saving>
============================================================================================#


text:
using HorizonSideRobots

function poisk(r)
    j = 0
    i = 1
    while !ismarker(r)
        for n in 1:i
            move!(r,HorizonSide((j + 1) % 4))
            if ismarker(r)
                break
            end
        end
        if ismarker(r)
            break
        end
        for n in 1:i
            move!(r,HorizonSide((j + 2) % 4))
            if ismarker(r)
                break
            end
        end
        i += 1
        j += 2
    end
end

poisk(r)

max_stars_repo_name: Cheburkova/mirea-progs

max_issues_repo_name: Cheburkova/mirea-progs

max_forks_repo_name: Cheburkova/mirea-progs


text:
using Pkg, InteractiveUtils
println("julia> Pkg.status()")
Pkg.status()
println("\njulia> versioninfo()")
versioninfo()

max_stars_repo_name: ericphanson/Attempt-diff-eigmin

max_issues_repo_name: ericphanson/Attempt-diff-eigmin

max_forks_repo_name: ericphanson/Attempt-diff-eigmin


text:
function _plot!(simple_plot::SimplePlot, cur_mode::AbstractString, varargs...; kwargs...)
  vararg_count = length(varargs)
  @assert vararg_count < 3

  haskey(kwargs, :title) && title!(simple_plot, kwargs[:title])

  haskey(kwargs, :xlabel) && xlabel!(simple_plot, kwargs[:xlabel])
  haskey(kwargs, :ylabel) && ylabel!(simple_plot, kwargs[:ylabel])

  haskey(kwargs, :xscale) && xscale!(simple_plot, kwargs[:xscale])
  haskey(kwargs, :yscale) && yscale!(simple_plot, kwargs[:yscale])

  tmp_lims = nothing
  haskey(kwargs, :xlim) && ( tmp_lims = kwargs[:xlim] )
  haskey(kwargs, :xlims) && ( tmp_lims = kwargs[:xlims] )
  isnothing(tmp_lims) || xlims!(simple_plot, tmp_lims)

  tmp_lims = nothing
  haskey(kwargs, :ylim) && ( tmp_lims = kwargs[:ylim] )
  haskey(kwargs, :ylims) && ( tmp_lims = kwargs[:ylims] )
  isnothing(tmp_lims) || ylims!(simple_plot, tmp_lims)

  haskey(kwargs, :xticks) && xticks!(simple_plot, kwargs[:xticks])
  haskey(kwargs, :yticks) && yticks!(simple_plot, kwargs[:yticks])

  if haskey(kwargs, :legend)
    cur_legend = kwargs[:legend]
  elseif haskey(kwargs, :leg)
    cur_legend = kwargs[:leg]
  else
    cur_legend = nothing
  end

  if !isnothing(cur_legend)
    if isa(cur_legend, Bool)
      simple_plot.layout["showlegend"] = cur_legend

      delete!(simple_plot.layout["legend"], "xanchor")
      delete!(simple_plot.layout["legend"], "yanchor")

      delete!(simple_plot.layout["legend"], "x")
      delete!(simple_plot.layout["legend"], "y")

      delete!(simple_plot.layout["legend"], "bordercolor")
      delete!(simple_plot.layout["legend"], "borderwidth")
    else
      @assert !is_repl "No legend placement in REPL"

      simple_plot.layout["showlegend"] = true

      legend_position = cur_legend

      if isa(legend_position, Symbol)
        legend_position = string(legend_position)
      else
        @assert isa(legend_position, AbstractString)
      end

      found_anchor = nothing
      for (y_index, y_anchor) in enumerate(["bottom", "middle", "top"])
        cur_string = ""
        ( y_anchor == "middle" ) || ( cur_string *= y_anchor )
        for (x_index, x_anchor) in enumerate(["left", "center", "right"])
          tmp_string = cur_string
          ( x_anchor == "center" ) || ( tmp_string *= x_anchor )

          ( legend_position == tmp_string ) || continue
          found_anchor = true

          simple_plot.layout["legend"]["xanchor"] = x_anchor
          simple_plot.layout["legend"]["yanchor"] = y_anchor

          simple_plot.layout["legend"]["x"] = string( (x_index-1) / 2 )
          simple_plot.layout["legend"]["y"] = string( (y_index-1) / 2 )

          simple_plot.layout["legend"]["bordercolor"] = "#DDD"
          simple_plot.layout["legend"]["borderwidth"] = "1"

          break
        end
      end

      @assert !isnothing(found_anchor)
    end
  end

  iszero(vararg_count) && return _plot

  if vararg_count == 1
    cur_y = first(varargs)
    cur_x = 1:length(cur_y)
  else
    cur_x = first(varargs)
    cur_y = last(varargs)
  end

  cur_dict = Dict(
    "x" => collect(cur_x),
    "y" => collect(cur_y),
    "mode" => cur_mode
  )

  cur_alpha = nothing
  haskey(kwargs, :alpha) && ( cur_alpha = kwargs[:alpha] )
  haskey(kwargs, :opacity) && ( cur_alpha = kwargs[:opacity] )
  isnothing(cur_alpha) || ( cur_dict["opacity"] = cur_alpha )

  if haskey(kwargs, :color)
    cur_index = kwargs[:color]
    @assert isa(cur_index, Int)
  else
    simple_plot.index += 1
    cur_index = simple_plot.index
  end

  cur_index = 1 + ( (cur_index-1) % length(_palette) )
  cur_color = _palette[cur_index]

  if haskey(kwargs, :xerr)
    cur_dict["error_x"] = Dict(
      "type" => "data",
      "array" => collect(kwargs[:xerr]),
      "opacity" => haskey(kwargs, :fillalpha) ? kwargs[:fillalpha] : (cur_alpha/2),
      "color" => cur_color,
      "width" => 0,
      "thickness" => 2
    )
  end

  if haskey(kwargs, :yerr)
    cur_dict["error_y"] = Dict(
      "type" => "data",
      "array" => collect(kwargs[:yerr]),
      "opacity" => haskey(kwargs, :fillalpha) ? kwargs[:fillalpha] : (cur_alpha/2),
      "color" => cur_color,
      "width" => 0,
      "thickness" => 2
    )
  end

  cur_label = nothing
  haskey(kwargs, :name) && ( cur_label = kwargs[:name] )
  haskey(kwargs, :label) && ( cur_label = kwargs[:label] )

  if !isnothing(cur_label)
    if strip(cur_label) == ""
      cur_dict["showlegend"] = false
    else
      cur_dict["name"] = cur_label
    end
  end

  sub_dict = Dict()
  sub_dict["color"] = cur_color

  if cur_mode == "lines"
    cur_width = nothing
    haskey(kwargs, :width) && ( cur_width = kwargs[:width] )
    haskey(kwargs, :linewidth) && ( cur_width = kwargs[:linewidth] )
    isnothing(cur_width) || ( sub_dict["width"] = cur_width )

    if haskey(kwargs, :linestyle) && string(kwargs[:linestyle]) != "auto"
      sub_dict["dash"] = kwargs[:linestyle]
    end

    cur_dict["line"] = sub_dict
  else
    @assert cur_mode == "markers"

    cur_size = nothing
    haskey(kwargs, :size) && ( cur_size = kwargs[:size] )
    haskey(kwargs, :markersize) && ( cur_size = kwargs[:markersize] )
    isnothing(cur_size) || ( sub_dict["size"] = cur_size )

    cur_dict["marker"] = sub_dict
  end

  push!(simple_plot.data, cur_dict)

  validate!(simple_plot)
end


text:
# This file is a part of ForwardDiffPullbacks.jl, licensed under the MIT License (MIT).

# ToDo: Use ProjectTo in rrules (requires ChainRulesCore >= v0.10.11).

struct FwdDiffPullbackThunk{F<:Base.Callable,T<:Tuple,i,U<:Any} <: ChainRulesCore.AbstractThunk
    f::F
    xs::T
    ΔΩ::U
end

function FwdDiffPullbackThunk(f::F, xs::T, ::Val{i}, ΔΩ::U) where {F<:Base.Callable,T<:Tuple,i,U<:Any}
    FwdDiffPullbackThunk{F,T,i,U}(f, xs, ΔΩ)
end

@inline function ChainRulesCore.unthunk(tnk::FwdDiffPullbackThunk{F,T,i,U}) where {F,T,i,U}
    forwarddiff_fwd_back(tnk.f, tnk.xs, Val(i), unthunk(tnk.ΔΩ))
end

# ToDo: Remove (obsolete with ChainRulesCore >= v0.10.):
(tnk::FwdDiffPullbackThunk)() = ChainRulesCore.unthunk(tnk)

Base.@generated function _forwarddiff_pullback_thunks(f::Base.Callable, xs::NTuple{N,Any}, ΔΩ::Any) where N
    Expr(:tuple, NoTangent(), (:(ForwardDiffPullbacks.FwdDiffPullbackThunk(f, xs, Val($i), ΔΩ)) for i in 1:N)...)
end

g_ΔΩ = nothing

@inline function ChainRulesCore.rrule(wrapped_f::WithForwardDiff, xs::Vararg{Any,N}) where N
    # @info "RUN ChainRulesCore.rrule(wrapped_f::WithForwardDiff, xs) with N = $N"
    f = wrapped_f.f
    y = f(xs...)
    with_fwddiff_pullback(ΔΩ) = begin
        #@info "XXXXXXXXXX" ΔΩ
        #global g_ΔΩ = ΔΩ
        _forwarddiff_pullback_thunks(f, xs, ΔΩ)
    end
    return y, with_fwddiff_pullback
end

struct FwdDiffBCPullbackThunk{F<:Base.Callable,T<:Tuple,i,U} <: ChainRulesCore.AbstractThunk
    f::F
    Xs::T
    ΔΩA::U
end

function FwdDiffBCPullbackThunk(f::F, Xs::T, ::Val{i}, ΔΩA::U) where {F<:Base.Callable,T<:Tuple,i,U}
    FwdDiffBCPullbackThunk{F,T,i,U}(f, Xs, ΔΩA)
end

@inline function ChainRulesCore.unthunk(tnk::FwdDiffBCPullbackThunk{F,T,i,U}) where {F,T,i,U}
    forwarddiff_bc_fwd_back(tnk.f, tnk.Xs, Val(i), unthunk(tnk.ΔΩA))
end

# ToDo: Remove (obsolete with ChainRulesCore >= v0.10.):
(tnk::FwdDiffBCPullbackThunk)() = ChainRulesCore.unthunk(tnk)

Base.@generated function _forwarddiff_bc_pullback_thunks(f::Base.Callable, Xs::NTuple{N,Any}, ΔΩA::Any) where N
    Expr(:tuple, NoTangent(), NoTangent(), (:(ForwardDiffPullbacks.FwdDiffBCPullbackThunk(f, Xs, Val($i), ΔΩA)) for i in 1:N)...)
end

function ChainRulesCore.rrule(::typeof(Base.broadcasted), wrapped_f::WithForwardDiff, Xs::Vararg{Any,N}) where N
    # @info "RUN ChainRulesCore.rrule(Base.broadcasted, wrapped_f::WithForwardDiff, Xs) with N = $N"
    f = wrapped_f.f
    y = broadcast(f, Xs...)
    bc_with_fwddiff_pullback(ΔΩA) = _forwarddiff_bc_pullback_thunks(f, Xs, ΔΩA)
    return y, bc_with_fwddiff_pullback
end


text:
# MIT License
#
# Copyright (c) 2021 Juha Tapio Heiskala
#
# Permission is hereby granted, free of charge, to any person obtaining a copy
# of this software and associated documentation files (the "Software"), to deal
# in the Software without restriction, including without limitation the rights
# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
# copies of the Software, and to permit persons to whom the Software is
# furnished to do so, subject to the following conditions:
#
# The above copyright notice and this permission notice shall be included in all
# copies or substantial portions of the Software.
#
# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
# SOFTWARE.

module ITerSOLvers

using SparseArrays
import ITSOL_2_jll
import ZITSOL_1_jll

const libitsol2 = ITSOL_2_jll.libITSOL_2
const libzitsol1 = ZITSOL_1_jll.libZITSOL_1

# Sparse matrix data structures used by ITSOL libraries (ITSOL_2, ZITSOL_1)
mutable struct SpaFmt{T}
    n::Int32
    nzcount::Ptr{Cint}
    ja::Ptr{Ptr{Cint}}
    ma::Ptr{Ptr{T}}

    ja_ptrs::Array{Ptr{Cint},1}
    ma_ptrs::Array{Ptr{T},1}

    nzc_data::Array{Cint,1}
    ja_data::Array{Cint,1}
end

mutable struct ILUfac{T}
    n::Int32
    L::Ptr{Cvoid}
    D::Ptr{T}
    U::Ptr{Cvoid}
    work::Ptr{Cint}
end

mutable struct IluSpar{T}
    n::Cint
    C::Ptr{Cvoid}
    L::Ptr{Cvoid}
    U::Ptr{Cvoid}

    rperm::Ptr{Cint}
    perm::Ptr{Cint}
    perm2::Ptr{Cint}

    D1::Ptr{T}
    D2::Ptr{T}
    wk::Ptr{T}
end

# ILU0 solver

function ilu0(A::SparseMatrixCSC{T}, milu) where T <: Number
    itsolA = make_SpaFmt(A)
    iLU = ILUfac{T}(A.n, C_NULL, C_NULL, C_NULL, C_NULL)

    if T <: Real
        f = ccall((:itsol_pc_ilukC, libitsol2), Int32, (Int32, Ref{SpaFmt}, Ref{ILUfac}, Cint, Ptr{Cvoid}),
                  Cint(0), itsolA, iLU, Cint(milu), C_NULL)
    else
        f = ccall((:zilukC, libzitsol1), Int32, (Int32, Ref{SpaFmt}, Ref{ILUfac}, Cint, Ptr{Cvoid}),
                  Cint(0), itsolA, iLU, Cint(milu), C_NULL)
    end

    L = _make_jl_sparse(T, iLU.L)
    U = _make_jl_sparse(T, iLU.U)

    D = Array{T, 1}(undef, iLU.n)
    unsafe_copyto!(pointer(D), iLU.D, iLU.n)

    return (L, D, U)
end

# ILUC solver

function iluc(A::SparseMatrixCSC{T}, droptol, milu) where T <: Number
    itsolA = make_SpaFmt(A)

    inputILU = ILUfac{T}(A.n, C_NULL, C_NULL, C_NULL, C_NULL)

    if T<:Real
        f = ccall((:itsol_CSClumC, libitsol2), Int32, (Ref{SpaFmt}, Ref{ILUfac}, Cint), itsolA, inputILU, Cint(0))

        iLU = ILUfac{T}(A.n, C_NULL, C_NULL, C_NULL, C_NULL)

        f = ccall((:itsol_pc_ilutc, libitsol2), Cint, (Ref{ILUfac}, Ref{ILUfac}, Cint, Cdouble, Cint, Cint, Ptr{Cvoid}),
                  inputILU, iLU, Cint(A.n), Cdouble(droptol), Cint(5), Cint(milu), C_NULL)
    else
        f = ccall((:zCSClumC, libzitsol1), Int32, (Ref{SpaFmt}, Ref{ILUfac}, Cint), itsolA, inputILU, Cint(0))

        iLU = ILUfac{T}(A.n, C_NULL, C_NULL, C_NULL, C_NULL)

        f = ccall((:zilutc, libzitsol1), Cint, (Ref{ILUfac}, Ref{ILUfac}, Cint, Cdouble, Cint, Cint, Ptr{Cvoid}),
                  inputILU, iLU, Cint(A.n), Cdouble(droptol), Cint(5), Cint(milu), C_NULL)
    end

    L = _make_jl_sparse(T, iLU.L)
    U = _make_jl_sparse(T, iLU.U)

    D = Array{T, 1}(undef, iLU.n)
    unsafe_copyto!(pointer(D), iLU.D, iLU.n)

    return (L, D, U)
end

# ILUtp solver

function ilutp(A::SparseMatrixCSC{T}, droptol, thresh, milu, udiag) where T<:Number
    itsolA = make_SpaFmt(A)
    iLU = IluSpar{T}(A.n, C_NULL, C_NULL, C_NULL, C_NULL, C_NULL,
                  C_NULL, C_NULL, C_NULL, C_NULL)

    droptolA = zeros(Cdouble, 7)
    droptolA[6] = droptol
    droptolA[7] = droptol

    lfilA = zeros(Cint, 7)
    lfilA[6] = A.n
    lfilA[7] = A.n

    if T<:Real
        f = ccall((:itsol_pc_ilutpC, libitsol2), Int32,
                  (Ref{SpaFmt}, Ptr{Cdouble}, Ptr{Cint}, Cdouble, Cint, Ref{IluSpar}, Cint),
                  itsolA, pointer(droptolA), pointer(lfilA), thresh, Cint(A.n), iLU, Cint(udiag))
    else
        f = ccall((:zilutpC, libzitsol1), Int32,
                  (Ref{SpaFmt}, Ptr{Cdouble}, Ptr{Cint}, Cdouble, Cint, Ref{IluSpar}, Cint),
                  itsolA, pointer(droptolA), pointer(lfilA), thresh, Cint(A.n), iLU, Cint(udiag))
    end

    L = _make_jl_sparse(T, iLU.L)
    U = _make_jl_sparse(T, iLU.U)

    P = Array{Cint, 1}(undef, iLU.n)
    unsafe_copyto!(pointer(P), iLU.perm2, iLU.n)

    return (L,U,P)
end

# convert from Julia SparseArray to ITSOL sparse array
function make_SpaFmt(A::SparseMatrixCSC{T}) where T <: Number

    nz_data = Array{Cint,1}(undef, A.n)
    ja_data = Array{Cint,1}(undef, nnz(A))

    ja_ptrs = Array{Ptr{Cint},1}(undef, A.n)
    ma_ptrs = Array{Ptr{T},1}(undef, A.n)

    cum_nnz = 0
    for c_ix = 1:A.n
        col_nnz = Cint(length(nzrange(A,c_ix)))
        nz_data[c_ix] = col_nnz

        for r_ix = A.colptr[c_ix]:(A.colptr[c_ix]+col_nnz-1)
            ja_data[r_ix]  = Cint(A.rowval[r_ix]-1)
        end
        ja_ptrs[c_ix] = pointer(ja_data) + cum_nnz*sizeof(Cint)
        ma_ptrs[c_ix] = pointer(A.nzval) + cum_nnz*sizeof(T)
        cum_nnz += col_nnz
    end

    itsolA = SpaFmt{T}(Cint(A.n), pointer(nz_data), pointer(ja_ptrs), pointer(ma_ptrs),
                    ja_ptrs, ma_ptrs, nz_data, ja_data)

    return itsolA
end

# convert from ITSOL SparseArray to Julia sparse array
function  _make_jl_sparse(T, A::Ptr{Cvoid})
    n = unsafe_load(Ptr{Cint}(A))

    row_nz_cnt = Array{Int32,1}(undef, n+1)
    row_nz_cnt[1] = Int32(1)
    unsafe_copyto!(pointer(row_nz_cnt)+sizeof(Int32), unsafe_load(Ptr{Ptr{Cint}}(A+8)), n)
    colptr = cumsum(row_nz_cnt)

    n_nz = colptr[end]-1

    rowval = Vector{Int32}(undef, n_nz)
    nzval = Vector{T}(undef, n_nz)

    r_ix_array_ptr = unsafe_load(Ptr{Ptr{Cvoid}}(A+16))
    val_array_ptr = unsafe_load(Ptr{Ptr{Cvoid}}(A+24))
    for c = 1:n
        unsafe_copyto!(pointer(rowval)+(colptr[c]-1)*sizeof(Cint),
                       Ptr{Cint}(unsafe_load(Ptr{Ptr{Cvoid}}(r_ix_array_ptr), c)), row_nz_cnt[c+1])
        unsafe_copyto!(pointer(nzval)+(colptr[c]-1)*sizeof(T),
                       Ptr{T}(unsafe_load(Ptr{Ptr{Cvoid}}(val_array_ptr), c)), row_nz_cnt[c+1])

    end
    jlA = SparseMatrixCSC{T,Int64}(n, n, Int64.(colptr), (Int64.(rowval)).+Int64(1), nzval)
    return jlA
end

end # module


text:
function shapeobject(box::Box)
    x,y,z = Tuple(box.xyz)
    return GeometryTypes.HyperRectangle(Vec(-x/2,-y/2,-z/2),Vec(x,y,z))
end

function shapeobject(cylinder::Cylinder)
    r,h = Tuple(cylinder.rh)
    return GeometryTypes.Cylinder(Point(0.0,0.0,-h/2),Point(0.0,0.0,h/2), r)
end

function shapeobject(sphere::Sphere)
    r = sphere.r
    return GeometryTypes.Sphere(Point(0.0,0.0,0.0), r)
end

function shapeobject(mesh::Mesh)
    return shape = load(mesh.path, GLUVMesh)
end

function visualize!(mechanism::Mechanism)
    vis = Visualizer()
    open(vis, Blink.Window())

    storage = deepcopy(mechanism.storage)
    oid = mechanism.origin.id
    oshapeind = 0

    for (ind,shape) in enumerate(mechanism.shapes)
        for id in shape.bodyids
            if id >= 0
                for i in mechanism.steps
                    storage.x[id][i] += vrotate(shape.xoff, storage.q[id][i])
                    storage.q[id][i] *= shape.qoff
                end
                visshape = shapeobject(shape)
                setobject!(vis["bundle/visshape"*string(id)], visshape, MeshPhongMaterial(color=shape.color))
            else
                @assert id == oid
                oshapeind = ind
                visshape = shapeobject(shape)
                setobject!(vis["bundle/visshape"*string(id)], visshape, MeshPhongMaterial(color=shape.color))
            end
        end
    end

    framerate = Int64(round(1/mechanism.Δt))
    anim = MeshCat.Animation(Dict{MeshCat.SceneTrees.Path,MeshCat.AnimationClip}(), framerate)

    for k=mechanism.steps
        MeshCat.atframe(anim, k) do
            for (id,body) in pairs(mechanism.bodies)
                settransform!(vis["bundle/visshape"*string(id)], compose(Translation((storage.x[id][k])...),LinearMap(Quat((storage.q[id][k])...))))
            end
            if oshapeind > 0
                shape = mechanism.shapes[oshapeind]
                settransform!(vis["bundle/visshape"*string(oid)], compose(Translation((shape.xoff)...),LinearMap(Quat((shape.qoff)...))))
            end
        end
    end

    MeshCat.setanimation!(vis, anim)
    return
end

function convert_meshcat_to_video(;filename="video",input_path="util\\",output_path="util\\")
    # Saving MeshCat sequence as a video.
    meshcat_sequence_dir = joinpath(@__DIR__, "..", input_path)
    if filename==nothing
        filenames = readdir(meshcat_sequence_dir)
    else
        filenames = [filename * ".tar"]
    end
    for filename in filenames
        println("Converting " * filename * " to video." )
        video_dir = joinpath(@__DIR__, "..", output_path, filename[1:end-4] * ".mp4",)
        MeshCat.convert_frames_to_video(
            meshcat_sequence_dir * filename,
            video_dir,
            overwrite=true)
    end
    return
end


text:
export BrickletRotaryEncoderCountCallbackThreshold
struct BrickletRotaryEncoderCountCallbackThreshold
    option::Char
    min::Integer
    max::Integer
end

export BrickletRotaryEncoderIdentity
struct BrickletRotaryEncoderIdentity
    uid::String
    connected_uid::String
    position::Char
    hardware_version::Vector{Integer}
    firmware_version::Vector{Integer}
    device_identifier::Integer
end

export BrickletRotaryEncoder
"""
360° rotary encoder with push-button
"""
mutable struct BrickletRotaryEncoder <: TinkerforgeDevice
    ipcon::IPConnection
    deviceInternal::PyObject

    """
    Creates an object with the unique device ID *uid* and adds it to
    the IP Connection *ipcon*.
    """
    function BrickletRotaryEncoder(uid::String, ipcon::IPConnection)
        package = pyimport("tinkerforge.bricklet_rotary_encoder")
        deviceInternal = package.BrickletRotaryEncoder(uid, ipcon.ipconInternal)

        return new(ipcon, deviceInternal)
    end
end

export get_count
"""
    $(SIGNATURES)

Returns the current count of the encoder. If you set reset
to true, the count is set back to 0 directly after the
current count is read.

The encoder has 24 steps per rotation

Turning the encoder to the left decrements the counter,
so a negative count is possible.
"""
function get_count(device::BrickletRotaryEncoder, reset)
    return device.deviceInternal.get_count(reset)
end

export set_count_callback_period
"""
    $(SIGNATURES)

Sets the period with which the :cb:`Count` callback is triggered
periodically. A value of 0 turns the callback off.

The :cb:`Count` callback is only triggered if the count has changed since the
last triggering.
"""
function set_count_callback_period(device::BrickletRotaryEncoder, period)
    device.deviceInternal.set_count_callback_period(period)
end

export get_count_callback_period
"""
    $(SIGNATURES)

Returns the period as set by :func:`Set Count Callback Period`.
"""
function get_count_callback_period(device::BrickletRotaryEncoder)
    return device.deviceInternal.get_count_callback_period()
end

export set_count_callback_threshold
"""
    $(SIGNATURES)

Sets the thresholds for the :cb:`Count Reached` callback.

The following options are possible:

.. csv-table::
 :header: "Option", "Description"
 :widths: 10, 100

 "'x'",    "Callback is turned off"
 "'o'",    "Callback is triggered when the count is *outside* the min and max values"
 "'i'",    "Callback is triggered when the count is *inside* the min and max values"
 "'<'",    "Callback is triggered when the count is smaller than the min value (max is ignored)"
 "'>'",    "Callback is triggered when the count is greater than the min value (max is ignored)"
"""
function set_count_callback_threshold(device::BrickletRotaryEncoder, option, min, max)
    device.deviceInternal.set_count_callback_threshold(option, min, max)
end

export get_count_callback_threshold
"""
    $(SIGNATURES)

Returns the threshold as set by :func:`Set Count Callback Threshold`.
"""
function get_count_callback_threshold(device::BrickletRotaryEncoder)
    return device.deviceInternal.get_count_callback_threshold()
end

export set_debounce_period
"""
    $(SIGNATURES)

Sets the period with which the threshold callback

* :cb:`Count Reached`

is triggered, if the thresholds

* :func:`Set Count Callback Threshold`

keeps being reached.
"""
function set_debounce_period(device::BrickletRotaryEncoder, debounce)
    device.deviceInternal.set_debounce_period(debounce)
end

export get_debounce_period
"""
    $(SIGNATURES)

Returns the debounce period as set by :func:`Set Debounce Period`.
"""
function get_debounce_period(device::BrickletRotaryEncoder)
    return device.deviceInternal.get_debounce_period()
end

export is_pressed
"""
    $(SIGNATURES)

Returns *true* if the button is pressed and *false* otherwise.

It is recommended to use the :cb:`Pressed` and :cb:`Released` callbacks
to handle the button.
"""
function is_pressed(device::BrickletRotaryEncoder)
    return device.deviceInternal.is_pressed()
end

export get_identity
"""
    $(SIGNATURES)

Returns the UID, the UID where the Bricklet is connected to,
the position, the hardware and firmware version as well as the
device identifier.

The position can be 'a', 'b', 'c', 'd', 'e', 'f', 'g' or 'h' (Bricklet Port).
A Bricklet connected to an :ref:`Isolator Bricklet <isolator_bricklet>` is always at
position 'z'.

The device identifier numbers can be found :ref:`here <device_identifier>`.
|device_identifier_constant|
"""
function get_identity(device::BrickletRotaryEncoder)
    return device.deviceInternal.get_identity()
end

export register_callback
"""
Registers the given *function* with the given *callback_id*.
"""
function register_callback(device::BrickletRotaryEncoder, callback_id, function_)
    if isnothing(function_)
        device.registered_callbacks.pop(callback_id, None)
    else
        device.registered_callbacks[callback_id] = function_
    end
end


text:
# new Sandshark example
# add more julia processes
# nprocs() < 7 ? addprocs(7-nprocs()) : nothing

using Caesar
using Interpolations

using RoMEPlotting
using Gadfly, DataFrames
using ProgressMeter

const TU = TransformUtils

using Pkg
using DelimitedFiles

Gadfly.set_default_plot_size(35cm,25cm)

include(joinpath(Pkg.dir("Caesar"), "examples", "marine", "auv", "Sandshark","Plotting.jl"))

## Load the data
# Data structures are:
#   - epochs
#   - NAV
#   - odoDict
#   - ppbrDict
include(joinpath(Pkg.dir("Caesar"), "examples", "marine", "auv", "Sandshark","DataUtils.jl"))

# Initializing Graff session
using GraffSDK
using Dates
config = loadGraffConfig();
config.userId = "Dehann"
config.robotId = "Sandshark"
config.sessionId = "Sandshark_"*Dates.format(now(), "yyyymmdd_HHMMSS")
# config.sessionId = "Sandshark_20190126_2101334"
# config.sessionId = "Sandshark_20190126_2201325"
println(" - Creating or retrieving robot '$(config.robotId)'...")
if !isRobotExisting()
    # Create a new one programatically - can also do this via the UI.
    println(" -- Robot '$(config.robotId)' doesn't exist, creating it...")
    newRobot = RobotRequest(config.robotId, "Sandshark", "Sandshark dataset", "Active");
    robot = addRobot(newRobot);
end
if !isSessionExisting()
    println(" -- Session '$(config.sessionId)' doesn't exist for robot '$(config.robotId)', creating it...")
    newSessionRequest = SessionDetailsRequest(config.sessionId, "Sandshark dataset!", "Pose2", false)
    session = addSession(newSessionRequest)
end

## Step: Building the factor graph
fg = initfg()
l1Uncertainty = 0.1
nonParamStep = 1 # Number of poses between nonparametric acoustic factors
# Add a central beacon with a prior
addVariable!(fg, :l1, Point2)
addVariable(:l1, Point2, String[])
prior = IIF.Prior( MvNormal([0.6; -16], l1Uncertainty^2*[1.0 .0;.0 1.0]) )
addFactor!(fg, [:l1], prior)
addFactor([:l1], prior)
global index = 0
for ep in epochs
    global index
    curvar = Symbol("x$index")
    addVariable!(fg, curvar, Pose2)
    addVariable(curvar, Pose2, String[])

    # xi -> l1 - nonparametric factor
    if (index+1) % nonParamStep == 0
        @info " - Adding $curvar->l1 factor..."
        # addFactor!(fg, [curvar; (index < length(epochs)/2 ? :l1 : :l2)], ppbrDict[ep])
        addFactor!(fg, [curvar; :l1], ppbrDict[ep])
        addFactor([curvar; :l1], ppbrDict[ep])
    end

    if ep != epochs[1]
      # Odo factor x(i-1) -> xi
      @info " - Adding x$(index-1)->$curvar odo factor"
      addFactor!(fg, [Symbol("x$(index-1)"); curvar], odoDict[ep])
      addFactor([Symbol("x$(index-1)"); curvar], odoDict[ep])
    else
      # Prior to the first pose location (a "GPS" prior)
      initLoc = [interp_x(ep);interp_y(ep);interp_yaw(ep)]
      @info " - Adding a initial location prior at $curvar, $initLoc"
      initPrior = IIF.Prior( MvNormal(initLoc, diagm(0 => [0.1;0.1;0.05].^2)) )
      addFactor!(fg, [curvar], initPrior)
      addFactor([curvar], initPrior)
    end
    # Heading partial prior
    @info " - Adding heading prior on $curvar"
    headingPrior = RoME.PartialPriorYawPose2(Normal(interp_yaw(ep), deg2rad(3)))
    addFactor!(fg, [curvar], headingPrior)
    addFactor([curvar], headingPrior)
    index+=1

    if index % 10 == 0
        putReady(true)
    end
end

# Let's wait for all nodes to be processed
@time while getSessionBacklog() > 0
    @info "...Session backlog currently has $(getSessionBacklog()) entries, waiting until complete..."
    sleep(2)
end
@info "Okay processing the last node! ...Yeah so we're going to focus on making that faster in the next release (please go write an issue in GraffSDK to spur us along!)"

# Lets see that everything processed successfully
@info "Session dead queue length = $(getSessionDeadQueueLength())"
if getSessionDeadQueueLength() > 0
    @error "This shouldn't happen, please examine the failed messages below to see what went wrong:"
    deadMsgs = getSessionDeadQueueMessages()
    map(d -> println(d["error"]), deadMsgs)
else
    @info "No messages in the dead queue (errors) on the server, looks good!"
end

writeGraphPdf(fg, engine="dot")

# ensureAllInitialized!(fg)
batchSolve!(fg)

# Testing packing
# odoTest = odoDict[epochs[2]]
# ppbrTest = ppbrDict[epochs[2]]
# import Base: convert
# odoPacked = convert(PackedPose2Pose2, odoTest)
# ppbrPacked = convert(PackedPose2Point2BearingRange, ppbrTest)
# odoUnpacked = convert(Pose2Pose2, odoTest)
# ppbrUnpacked = convert(Pose2Point2BearingRange, ppbrPacked)

# IIF.wipeBuildNewTree!(fg, drawpdf=true)
# run(`evince bt.pdf`)
# run(`evince /tmp/caesar/bt.pdf`)

# And I'll redefine anywhere
# Anywhere I RoME
# Where I lay my head is home
# Carved upon my stone
# My body lies, but still I RoME :D
endDogLeg = [interp_x[epochs[end]]; interp_y[epochs[end]]]
estDogLeg = [get2DPoseMeans(fg)[1][end]; get2DPoseMeans(fg)[2][end]]
endDogLeg - estDogLeg

drawPosesLandms(fg, meanmax=:max)

ls(fg, :x25)

#To Boldly Believe... The Good, the Bad, and the Unbelief-able
X25 = getVertKDE(fg, :x25)

# i
pts, = predictbelief(fg, :x21, [:x20x21f1; :x21l1f1])
plotKDE([kde!(pts);X25], dims=[1;2], levels=1, c=["red";"green"])

pts, = predictbelief(fg, :x25, :)
plotKDE([kde!(pts);X25], dims=[1;2], levels=1, c=["red";"green"])
plotKDE([kde!(pts);X25], dims=[3], levels=1, c=["red";"green"])

# Solvery! Roll dice for solvery check
# writeGraphPdf(fg)
# ensureAllInitialized!(fg)
t = string(now())
savejld(fg, file="presolve_$t.jld")
IIF.batchSolve!(fg) #, N=100
savejld(fg, file="postsolve_$t.jld")

# pl = drawPoses(fg, spscale=2.75) # Just for odo plot
# Roll again for inspiration check
## PLOT BEAM PATTERNS
Gadfly.push_theme(:default)
pla = drawPosesLandmarksAndOdo(fg, ppbrDict, navkeys, X, Y, lblX, lblY)
Gadfly.draw(PDF("/tmp/sandshark-beacon_$t.pdf", 12cm, 15cm), pla)
Gadfly.draw(PNG("sandshark-beacon_$t.png", 12cm, 15cm), pla)

####  DEBUG PPBRDict ====================

LL = [0.6; -16; 0]
function expectedLocalBearing(curPose, LL)
    v = [LL[1] - curPose[1]; LL[2] - curPose[2]]
    world = atan2(v[2], v[1])
    loc = rad2deg(TU.wrapRad(world - curPose[3]))
    while loc < 0
        loc += 360
    end
    return loc
end
# println("$(interp_x[ep]), $(interp_y[ep]), world yaw = $(interp_yaw[ep] * 180 / pi)")
# XX = [interp_x(ep); interp_y[ep]; interp_yaw[ep]]
# expectedLocalBearing(XX, LL)
# println("Expected local yaw = $(expectedLocalBearing(XX, LL)). Sonar local yaw = $(rad2deg(getKDEMax(ppbr.bearing)[1]))")
# xyt = se2vee(SE2(XX[1:3]) \ SE2(LL))
# bear = rad2deg(TU.wrapRad(atan2(xyt[2],xyt[1]) -XX[3]))

bearExp = []
bearAcoustics = []
for ep in epochs
    XX = [interp_x(ep); interp_y[ep]; interp_yaw[ep]]
    ppbr = ppbrDict[ep]
    push!(bearExp, expectedLocalBearing(XX, LL))
    push!(bearAcoustics, rad2deg(getKDEMax(ppbr.bearing)[1]))
    println("local beacon heading: Expected = $(expectedLocalYaw(XX, LL)). Sonar = $(rad2deg(getKDEMax(ppbr.bearing)[1]))")
end

layers = []
push!(layers, Gadfly.layer(x=1:length(bearExp), y=bearExp, Geom.path())[1], Theme(default_color=color("red")))
push!(layers, Gadfly.layer(x=1:length(bearAcoustics), y=bearAcoustics, Geom.path(), Theme(default_color=color("blue"))))
Gadfly.plot(layers...)
# end

####################################

######################################################

# pl = drawPoses(fg, spscale=2.75)
# drawPosesLandms(fg, spscale=0.75) #Means so we don't run into MM == Union() || Dict{} in
# You rolled 20!

# Roll again for debug check
## Debugging strange headings
# posePts = get2DPoseMeans(fg)
# landPts = get2DLandmMeans(fg)
## Debugging landmark bearing range

# ppbrDict[epoch_slice[1]]
# getSample(ppbrDict[epoch_slice[1]],100)
#
#
# addVariable!(fg, :l1, Point2)
# addFactor!(fg, [:x0; :l1], ppbrDict[epoch_slice[1]])
#
# ls(fg, :l1)
# pts = IIF.approxConv(fg, :x0l1f1, :l1)
#
# fct = getData(getVert(fg, :x0l1f1, nt=:fnc))
#
# fct.fnc.zDim

# dev
# ep = epochs[1]
# # azidata[ep][:,1]
# pll = layerBeamPatternRose(ppbrDict[ep].bearing)
# Gadfly.plot(pll...)
#
# pll = layerBeamPatternRose(ppbrDict[ep].bearing, wRr=TU.R(pi/2))
# Gadfly.plot(pll...)

#
# diff(epochs[[end;1]])

## GADFLY EXAMPLE
# plotLocalProduct(fg, :x49, dims=[1;2])
# stuff = IncrementalInference.localProduct(fg, :x49)
# plotKDE(stuff[2], dims=[1;2], levels=10)
#
# # STUFF
# fsym = :x49l1f1
# const TU = TransformUtils
# XX, LL = (KDE.getKDEMax.(IIF.getVertKDE.(fg, IIF.lsf(fg, fsym)))...)
# @show xyt = se2vee(SE2(XX[1:3]) \ SE2([LL[1:2];0.0]))
# bear= rad2deg(TU.wrapRad(atan2(-xyt[2],xyt[1]) -XX[3]))
# b = IncrementalInference.getData(fg, fsym, nt=:fnc).fnc.usrfnc!
# b.bearing
# p = plotKDE(b.range)
# Gadfly.draw(PDF("multimodal_bearing_range.pdf", 12cm, 15cm), p)
#
# # SAVE THIS PLOT
# # epochs = timestamps[11:2:200]
# g = getVertKDE.(fg, [:l1, :x49])
# m1 = KDE.marginal(g[1], [1;2])
# m2 = KDE.marginal(g[2], [1;2])
# norm(diff(KDE.getKDEMax.([m1; m2])))
#
# kde!(rand(Normal(4.0, 0.1),100))
#
# ###############
#
# PL = []
#
# push!(PL, Gadfly.layer(x=collect(1:10), y=randn(10), Geom.path, Theme(default_color=colorant"red"))[1])
# push!(PL, Gadfly.layer(x=collect(1:10), y=randn(10), Geom.path, Theme(default_color=colorant"green"))[1])
# push!(PL, Coord.Cartesian(xmin=-1.0,xmax=11, ymin=-5.0, ymax=5.0))
# push!(PL, Guide.ylabel("test"))
# push!(PL, Guide.ylabel("test"))
# pl = drawPoses(fg)
# push!(pl.layers, )
#
# pl = drawPoses(fg)
# pl = Gadfly.plot(pl.layers...)
#
#
#
# pl = Gadfly.plot(PL...)
#
# @show fieldnames(pl)
# push!(pl.layers, Gadfly.layer(x=collect(1:10), y=randn(10), Geom.path, Theme(default_color=colorant"magenta"))[1] )
#
# pl
# # vstasck, hstack
#
#
# Gadfly.push_theme(:default)
# # SVG, PDF, PNG
# Gadfly.draw(PDF("/tmp/testfig.pdf", 12cm, 8cm), pl)
# run(`evince /tmp/testfig.pdf`)
#
#
# ### PATH + Poses
#
# pl = drawPosesLandms(fg, spscale=2.5)
# # Add odo
# navdf = DataFrame(
#   ts = navkeys,
#   x = X,
#   y = Y
# )
# # pl = Gadfly.layer(navdf, x=:x, y=:y, Geom.path())
# push!(pl.layers, Gadfly.layer(navdf, x=:x, y=:y, Geom.path())[1])
# Gadfly.plot(pl.layers)

fsym = :x49l1f1
const TU = TransformUtils
XX, LL = (KDE.getKDEMax.(IIF.getVertKDE.(fg, IIF.lsf(fg, fsym)))...)
@show xyt = se2vee(SE2(XX[1:3]) \ SE2([LL[1:2];0.0]))
bear= rad2deg(TU.wrapRad(atan2(-xyt[2],xyt[1]) -XX[3]))

#


text:
using Symbolics

@variables a
function huber(a)
    δ = 1.0
    if abs(a) <= δ
        return 0.5*a^2
    else
        return δ*(abs(a) - 0.5*δ)
    end
end
da = huber(a)

max_issues_repo_name: thowell/DirectMotionPlanning


text:
module EquationEditor
using WebSockets
import WebSockets.ServerWS
using Latexify
import WebSockets: with_logger,
                   Dates,
                   LogLevel,
                   Logging.Debug,
                   Logging.Info,
                   Logging.Warn,
                   Wslog

export latexify, serveEE
include(joinpath(dirname(pathof(WebSockets)), "..", "benchmark", "functions_open_browsers.jl"))
include("serve.jl")
end  #module

max_stars_repo_name: hustf/EquationEditor

max_issues_repo_name: hustf/EquationEditor

max_forks_repo_name: hustf/EquationEditor


text:
module TestSplitBy
include("preamble.jl")
using Transducers: ReduceSplitBy, Bulk, End

function just_bulk(x)
    if x isa Bulk
        Some(x[])
    else
        x::End
        nothing
    end
end

@testset "ReduceSplitBy" begin
    @test 1:5 |> ReduceSplitBy(iseven, Map(getindex)'(string), "_") |> collect ==
          ["_12", "_34", "_5"]
    @test 1:5 |> ReduceSplitBy(isodd, Map(getindex)'(string), "_") |> collect ==
          ["_1", "_23", "_45"]
    @test 1:5 |> ReduceSplitBy(iseven, KeepSomething(just_bulk)'(string), "_") |> collect ==
          ["_1", "_3", "_5"]
    @test 1:5 |> ReduceSplitBy(isodd, KeepSomething(just_bulk)'(string), "_") |> collect ==
          ["_", "_2", "_4"]
end

@testset for ex in [
    SequentialEx(),
    PreferParallel(basesize = 1),
    PreferParallel(basesize = 2),
    PreferParallel(basesize = 3),
    PreferParallel(),
]
    @test 1:5 |> ReduceSplitBy(iseven, Map(getindex)'(string), "") |> fcollect(ex) ==
          ["12", "34", "5"]
    @test 1:5 |> ReduceSplitBy(isodd, Map(getindex)'(string), "") |> fcollect(ex) ==
          ["1", "23", "45"]
    @test 1:5 |> ReduceSplitBy(iseven, KeepSomething(just_bulk)'(string), "") |> fcollect(ex) ==
          ["1", "3", "5"]
    @test 1:5 |> ReduceSplitBy(isodd, KeepSomething(just_bulk)'(string), "") |> fcollect(ex) ==
          ["", "2", "4"]

    @test 1:5 |> SplitBy(==(4)) |> fcollect(ex) == [1:3, 5:5]
    @test 1:5 |> SplitBy(==(5)) |> fcollect(ex) == [1:4]
    @test 1:5 |> SplitBy(==(4); keepend = true) |> fcollect(ex) == [1:4, 5:5]
    @test 1:5 |> SplitBy(==(5); keepend = true) |> fcollect(ex) == [1:5]
    @test 1:5 |> SplitBy(==(4); keepempty = true) |> fcollect(ex) == [1:3, 5:5]
    @test 1:5 |> SplitBy(==(5); keepempty = true) |> fcollect(ex) == [1:4]

    @test 1:5 |> SplitBy(>=(4)) |> fcollect(ex) == [1:3]
    @test 1:5 |> SplitBy(>=(4); keepend = true) |> fcollect(ex) == [1:4, 5:5]
    @test 1:5 |> SplitBy(>=(4); keepempty = true) |> fcollect(ex) == [1:3, 5:4]

    @test 1:5 |> SplitBy(isodd) |> fcollect(ex) == [2:2, 4:4]
    @test 1:5 |> SplitBy(isodd; keepend = true) |> fcollect(ex) == [1:1, 2:3, 4:5]
    @test 1:5 |> SplitBy(isodd; keepempty = true) |> fcollect(ex) == [1:0, 2:2, 4:4]

    @test 1:4 |> SplitBy(iseven) |> fcollect(ex) == [1:1, 3:3]
    @test 1:4 |> SplitBy(iseven; keepend = true) |> fcollect(ex) == [1:2, 3:4]
    @test 1:4 |> SplitBy(iseven; keepempty = true) |> fcollect(ex) == [1:1, 3:3]

    @test "1 2  3" |> SplitBy(==(' ')) |> fcollect(ex) == ["1", "2", "3"]
    @test "1 α 3" |> SplitBy(==(' ')) |> fcollect(ex) == ["1", "α", "3"]
end

end  # module


text:
# example from
# Barraud, A.Y., "A numerical algorithm to solve A'XA - X = Q",
# IEEE Trans. Auto. Contr., AC-22, pp. 883-885, 1977.

A = [1.5 2.5 3.5 4.5 5.5 6.5 7.5;
		1 -1.5 2.5 3.5 4.5 5.5 6.5;
		0 0 1.5 -2.5 3.5 4.5 5.5;
		0 0 0 1.5 2.5 -3.5 4.5;
		0 0 0 1 1.5 2.5 -3.5;
		0 0 0 0 0 1.5 -2.5;
		0 0 0 0 0 1 1.5]

q = [5.25 1.5 19.25 22.5 55.75 60 49.25;
		0 0 3.25 -0.25 -6 -11 11.25;
		0 0 62.75 83 183.25 222.75 129.75;
		0 0 0 56.5 208.25 232.25 250.75;
		0 0 0 0 532.25 739.5 476.75;
		0 0 0 0 0 631 745.75;
		0 0 0 0 0 0 133.75]

Q = (q + q') - diagm(0 => diag(q))

x = [1 1 1 1 1 1 1;
		0 1 2 3 4 5 6;
		0 0 1 3 6 10 15;
		0 0 0 1 4 10 20;
		0 0 0 0 1 5 15;
		0 0 0 0 0 1 6;
		0 0 0 0 0 0 1];

X = (x + x') - diagm(0 => diag(x)) # theoretical solution

A = Array(A') # A' gets Adjoint{Float64,Array{Float64,2}}, but sylvester needs Array
Q = -Q

X1 = dlyap(A,Q) # computed solution
##################################################

print("Testing dlyap: ")
if sum((X-X1).^2) < 1e-6
	println("OK")
end