text:
import Base: +, -, *, /, \, exp, log, ≈, inv, conj
import LinearAlgebra: norm2

export
    Rotation,
    UnitQuaternion,
    MRP,
    RPY,
    RodriguesParam,
    ExponentialMap,
    VectorPart,
    MRPMap,
    CayleyMap,
    IdentityMap,
    ReNorm

export
    differential_rotation,
    retraction_map,
    scalar,
    vector,
    logm,
    expm,
    kinematics,
    rotmat,
    Lmult,
    Rmult,
    Vmat,
    Hmat,
    Tmat,
    skew,
    vee,
    ⊕,
    ⊖

function skew(v::AbstractVector)
    @assert length(v) == 3
    @SMatrix [0   -v[3]  v[2];
              v[3] 0    -v[1];
             -v[2] v[1]  0]
end

function vee(S::AbstractMatrix)
    return @SVector [S[3,2], S[1,3], S[2,1]]
end

abstract type Rotation end

abstract type DifferentialRotation end
abstract type VectorPart <: DifferentialRotation end
abstract type ExponentialMap <: DifferentialRotation end
abstract type MRPMap <: DifferentialRotation end
abstract type CayleyMap <: DifferentialRotation end
abstract type IdentityMap <: DifferentialRotation end
abstract type ReNorm <: DifferentialRotation end

# Scalings
@inline scaling(::Type{ExponentialMap}) = 0.5
@inline scaling(::Type{VectorPart}) = 1.0
@inline scaling(::Type{CayleyMap}) = 1.0
@inline scaling(::Type{MRPMap}) = 2.0

# Retraction Maps
(::Type{ExponentialMap})(ϕ) = expm(ϕ/scaling(ExponentialMap))

function (::Type{VectorPart})(v)
    μ = 1/scaling(VectorPart)
    UnitQuaternion{VectorPart}(sqrt(1-μ^2*v'v), μ*v[1], μ*v[2], μ*v[3])
end

function (::Type{CayleyMap})(g)
    g /= scaling(CayleyMap)
    M = 1/sqrt(1+g'g)
    UnitQuaternion{CayleyMap}(M, M*g[1], M*g[2], M*g[3])
end

function (::Type{MRPMap})(p)
    p /= scaling(MRPMap)
    n2 = p'p
    M = 2/(1+n2)
    UnitQuaternion{MRPMap}((1-n2)/(1+n2), M*p[1], M*p[2], M*p[3])
end

(::Type{IdentityMap})(q) = UnitQuaternion{IdentityMap}(q[1], q[2], q[3], q[4])

# Retraction Map Jacobians
function jacobian(::Type{ExponentialMap},ϕ, eps=1e-5)
    μ = 1/scaling(ExponentialMap)
    θ = norm(ϕ)
    cθ = cos(μ*θ/2)
    sincθ = sinc(μ*θ/2π)
    if θ < eps
        0.5*μ*[-0.5*μ*sincθ*ϕ'; sincθ*I + (cθ - sincθ)*ϕ*ϕ']
    else
        0.5*μ*[-0.5*μ*sincθ*ϕ'; sincθ*I + (cθ - sincθ)*ϕ*ϕ'/(ϕ'ϕ)]
    end
end

function jacobian(::Type{VectorPart}, v)
    μ = 1/scaling(VectorPart)
    μ2 = μ*μ
    M = -μ2/sqrt(1-μ2*v'v)
    @SMatrix [v[1]*M v[2]*M v[3]*M;
              μ 0 0;
              0 μ 0;
              0 0 μ]
end

function jacobian(::Type{CayleyMap}, g)
    μ = 1/scaling(CayleyMap)
    μ2 = μ*μ
    n = 1+μ2*g'g
    ni = 1/n
    μ*[-μ*g'; -μ2*g*g' + I*n]*ni*sqrt(ni)
end

function jacobian(::Type{MRPMap}, p)
    μ = 1/scaling(MRPMap)
    μ2 = μ*μ
    n = 1+μ2*p'p
    2*[-2*μ2*p'; I*μ*n - 2*μ*μ2*p*p']/n^2
end

jacobian(::Type{IdentityMap}, q) = I

const DEFAULT_QUATDIFF = VectorPart

""" $(TYPEDEF)
4-parameter attitute representation that is singularity-free. Quaternions with unit norm
represent a double-cover of SO(3). The `UnitQuaternion` does NOT strictly enforce the unit
norm constraint, but certain methods will assume you have a unit quaternion. The
`UnitQuaternion` type is parameterized by the linearization method, which maps quaternions
to the 3D plane tangent to the 4D unit sphere. Follows the Hamilton convention for quaternions.

There are currently 4 methods supported:
* `VectorPart` - uses the vector (or imaginary) part of the quaternion
* `ExponentialMap` - the most common approach, uses the exponential and logarithmic maps
* `CayleyMap` - or Rodrigues parameters (aka Gibbs vectors).
* `MRPMap` - or Modified Rodrigues Parameter, is a sterographic projection of the 4D unit sphere
onto the plane tangent to either the positive or negative real poles.

# Constructors
```julia
UnitQuaternion(s,x,y,z)  # defaults to `VectorPart`
UnitQuaternion{D}(s,x,y,z)
UnitQuaternion{D}(q::SVector{4})
UnitQuaternion{D}(r::SVector{3})  # quaternion with 0 real part
```
"""
struct UnitQuaternion{T,D<:DifferentialRotation} <: Rotation
    s::T
    x::T
    y::T
    z::T
end

UnitQuaternion(s::T,x::T,y::T,z::T) where T = UnitQuaternion{T,DEFAULT_QUATDIFF}(s,x,y,z)
UnitQuaternion(q::SVector{4}) = UnitQuaternion{DEFAULT_QUATDIFF}(q[1],q[2],q[3],q[4])
UnitQuaternion{D}(s::T,x::T,y::T,z::T) where {T,D} = UnitQuaternion{T,D}(s,x,y,z)

UnitQuaternion{D}(q::SVector{4}) where D = UnitQuaternion{D}(q[1],q[2],q[3],q[4])
UnitQuaternion{D}(r::SVector{3}) where D = UnitQuaternion{D}(0.0, r[1],r[2],r[3])
UnitQuaternion{D}(q::UnitQuaternion) where D = UnitQuaternion{D}(q.s, q.x, q.y, q.z)
UnitQuaternion{T,D}(q::R) where {T,D,R <: UnitQuaternion} =
    UnitQuaternion{T,D}(q.s, q.x, q.y, q.z)

UnitQuaternion(r::SVector{3}) = UnitQuaternion{DEFAULT_QUATDIFF}(0.0, r[1],r[2],r[3])
UnitQuaternion(q::UnitQuaternion) = q

(::Type{UnitQuaternion{T,D}})(x::SVector{4,T2}) where {T,T2,D} =
    UnitQuaternion{promote_type(T,T2),D}(x[1], x[2], x[3], x[4])

retraction_map(::UnitQuaternion{T,D}) where {T,D} = D
retraction_map(::Type{UnitQuaternion{T,D}}) where {T,D} = D

Base.rand(::Type{<:UnitQuaternion{T,D}}) where {T,D} =
    normalize(UnitQuaternion{T,D}(randn(T), randn(T), randn(T), randn(T)))

Base.rand(::Type{UnitQuaternion{T}}) where T = Base.rand(UnitQuaternion{T,VectorPart})
Base.rand(::Type{UnitQuaternion}) = Base.rand(UnitQuaternion{Float64,VectorPart})
Base.zero(::Type{Q}) where Q<:UnitQuaternion = I(Q)
Base.zero(q::Q) where Q<:UnitQuaternion = I(Q)

SVector(q::UnitQuaternion{T}) where T = SVector{4,T}(q.s, q.x, q.y, q.z)

scalar(q::UnitQuaternion) = q.s
vector(q::UnitQuaternion{T}) where T = SVector{3,T}(q.x, q.y, q.z)
vecnorm(q::UnitQuaternion) = sqrt(q.x^2 + q.y^2 + q.z^2)

conj(q::UnitQuaternion{T,D}) where {T,D} = UnitQuaternion{T,D}(q.s, -q.x, -q.y, -q.z)
inv(q::UnitQuaternion) = conj(q)

LinearAlgebra.norm2(q::UnitQuaternion) = q.s^2 + q.x^2 + q.y^2 + q.z^2
LinearAlgebra.norm(q::UnitQuaternion) = sqrt(q.s^2 + q.x^2 + q.y^2 + q.z^2)
LinearAlgebra.I(::Type{UnitQuaternion}) = UnitQuaternion(1.0, 0.0, 0.0, 0.0)
LinearAlgebra.I(::Type{Q}) where Q <: UnitQuaternion = UnitQuaternion(1.0, 0.0, 0.0, 0.0)
(::Type{Q})(I::UniformScaling) where Q<:UnitQuaternion = Q(1.0, 0.0, 0.0, 0.0)
(≈)(q::UnitQuaternion, u::UnitQuaternion) = q.s ≈ u.s && q.x ≈ u.x && q.y ≈ u.y && q.z ≈ u.z
(-)(q::UnitQuaternion{T,D}) where {T,D} = UnitQuaternion{T,D}(-q.s, -q.x, -q.y, -q.z)

function LinearAlgebra.normalize(q::UnitQuaternion{T,D}) where {T,D}
    n = norm(q)
    UnitQuaternion{T,D}(q.s/n, q.x/n, q.y/n, q.z/n)
end

function (*)(q::UnitQuaternion{T1,D1}, w::UnitQuaternion{T2,D2}) where {T1,T2,D1,D2}
    T = promote_type(T1, T2)
    D = D2
    UnitQuaternion{T,D}(q.s * w.s - q.x * w.x - q.y * w.y - q.z * w.z,
                        q.s * w.x + q.x * w.s + q.y * w.z - q.z * w.y,
                        q.s * w.y - q.x * w.z + q.y * w.s + q.z * w.x,
                        q.s * w.z + q.x * w.y - q.y * w.x + q.z * w.s)
end

function Base.:*(q::UnitQuaternion{Tq}, r::SVector{3}) where Tq
    qo = (-q.x  * r[1] - q.y * r[2] - q.z * r[3],
           q.s  * r[1] + q.y * r[3] - q.z * r[2],
           q.s  * r[2] - q.x * r[3] + q.z * r[1],
           q.s  * r[3] + q.x * r[2] - q.y * r[1])

   T = promote_type(Tq, eltype(r))

   return similar_type(r, T)(-qo[1] * q.x + qo[2] * q.s - qo[3] * q.z + qo[4] * q.y,
                             -qo[1] * q.y + qo[2] * q.z + qo[3] * q.s - qo[4] * q.x,
                             -qo[1] * q.z - qo[2] * q.y + qo[3] * q.x + qo[4] * q.s)
end

function (*)(q::Q, s::Real) where Q<:UnitQuaternion
    return Q(q.s*s, q.x*s, q.y*s, q.z*s)
end

(\)(q1::UnitQuaternion, q2::UnitQuaternion) = inv(q1)*q2
(/)(q1::UnitQuaternion, q2::UnitQuaternion) = q1*inv(q2)

(\)(q::UnitQuaternion, r::SVector{3}) = inv(q)*r

function exp(q::UnitQuaternion{T,D}) where {T,D}
    θ = vecnorm(q)
    sθ,cθ = sincos(θ)
    es = exp(q.s)
    M = es*sθ/θ
    UnitQuaternion{T,D}(es*cθ, q.x*M, q.y*M, q.z*M)
end

function expm(ϕ::SVector{3,T}) where T
    θ = norm(ϕ)
    sθ,cθ = sincos(θ/2)
    M = 0.5*sinc(θ/2π)
    UnitQuaternion{T,ExponentialMap}(cθ, ϕ[1]*M, ϕ[2]*M, ϕ[3]*M)
end

# Assumes unit quaternion
function log(q::UnitQuaternion{T,D}, eps=1e-6) where {T,D}
    θ = vecnorm(q)
    if θ > eps
        M = atan(θ, q.s)/θ
    else
        M = (1-(θ^2/(3q.s^2)))/q.s
    end
    UnitQuaternion{T,D}(0.0, q.x*M, q.y*M, q.z*M)
end

# Assumes unit quaternion
function logm(q::UnitQuaternion{T}) where T
    q = log(q)
    SVector{3,T}(2*q.x, 2*q.y, 2*q.z)
end

function rotmat(q::UnitQuaternion)
    s = q.s
    v = vector(q)
    (s^2 - v'v)*I + 2*v*v' + 2*s*skew(v)
end

function kinematics(q::UnitQuaternion{T,D}, ω::SVector{3}) where {T,D}
    SVector(q*UnitQuaternion{T,D}(0.0, 0.5*ω[1], 0.5*ω[2], 0.5*ω[3]))
end

function Base.angle(q::UnitQuaternion)
    min(2*atan(vecnorm(q), q.s), 2*atan(vecnorm(q), -q.s))
end

function (⊕)(q::UnitQuaternion{T,ExponentialMap}, δq::SVector{3}) where T
    q*expm(δq)
end

function (⊕)(q::UnitQuaternion{T,VectorPart}, δq::SVector{3}) where T
    q*UnitQuaternion{VectorPart}(sqrt(1 - δq[1]^2 - δq[2]^2 - δq[3]^2),
        δq[1], δq[2], δq[3])
end

function (⊖)(q::UnitQuaternion{T,D}, q0::UnitQuaternion) where {T,D}
    D(q0\q)
end

function (⊖)(q::UnitQuaternion{T,IdentityMap}, q0::UnitQuaternion) where {T}
    SVector(q) - SVector(q0)
    # return SVector(q0\q)
end

function rotation_between(from::SVector{3}, to::SVector{3})
    # Robustified version of implementation from https://www.gamedev.net/topic/429507-finding-the-quaternion-betwee-two-vectors/#entry3856228
    normprod = sqrt(dot(from, from) * dot(to, to))
    T = typeof(normprod)
    normprod < eps(T) && throw(ArgumentError("Input vectors must be nonzero."))
    w = normprod + dot(from, to)
    # v = abs(w) < 100 * eps(T) ? perpendicular_vector(from) : cross(from, to)
    v = from × to
    @inbounds return normalize(UnitQuaternion(w, v[1], v[2], v[3])) # relies on normalization in constructor
end

"""
Jacobian of q ⊕ ϕ, when ϕ is near zero. Useful for converting Jacobians from R⁴ to R³ and
    correctly account for unit norm constraint. Jacobians for different
    differential quaternion parameterization are the same up to a constant.
"""
function ∇differential(q::UnitQuaternion)
    1.0 * @SMatrix [
        -q.x -q.y -q.z;
         q.s -q.z  q.y;
         q.z  q.s -q.x;
        -q.y  q.x  q.s;
    ]
end

"Lmult(q2)q1 returns a vector equivalent to q2*q1 (quaternion multiplication)"
function Lmult(q::UnitQuaternion)
    @SMatrix [
        q.s -q.x -q.y -q.z;
        q.x  q.s -q.z  q.y;
        q.y  q.z  q.s -q.x;
        q.z -q.y  q.x  q.s;
    ]
end
Lmult(q::SVector{4}) = Lmult(UnitQuaternion(q))

"Rmult(q1)q2 return a vector equivalent to q2*q1 (quaternion multiplication)"
function Rmult(q::UnitQuaternion)
    @SMatrix [
        q.s -q.x -q.y -q.z;
        q.x  q.s  q.z -q.y;
        q.y -q.z  q.s  q.x;
        q.z  q.y -q.x  q.s;
    ]
end
Rmult(q::SVector{4}) = Rmult(UnitQuaternion(q))

"Tmat()q return a vector equivalent to inv(q)"
function Tmat()
    @SMatrix [
        1  0  0  0;
        0 -1  0  0;
        0  0 -1  0;
        0  0  0 -1;
    ]
end

"Vmat(q)q returns the imaginary (vector) part of the quaternion q (equivalent to vector(q))"
function Vmat()
    @SMatrix [
        0 1 0 0;
        0 0 1 0;
        0 0 0 1
    ]
end

function Hmat()
    @SMatrix [
        0 0 0;
        1 0 0;
        0 1 0;
        0 0 1.;
    ]
end

function Hmat(r)
    @assert length(r) == 3
    @SVector [0,r[1],r[2],r[3]]
end

"Jacobian of q*r with respect to the quaternion"
function ∇rotate(q::UnitQuaternion{T,D}, r::SVector{3}) where {T,D}
    rhat = UnitQuaternion{D}(r)
    R = Rmult(q)
    2Vmat()*Rmult(q)'Rmult(rhat)
end

"Jacobian of q2*q1 with respect to q1"
function ∇composition1(q2::UnitQuaternion, q1::UnitQuaternion)
    Lmult(q2)
end

"Jacobian of q2*q1 with respect to q2"
function ∇composition2(q2::UnitQuaternion, q1::UnitQuaternion)
    Rmult(q1)
end

function ∇²differential(q::UnitQuaternion, b::SVector{4})
    b1 = -SVector(q)'b
    Diagonal(@SVector fill(b1,3))
end

############################################################################################
#                             MODIFIED RODRIGUES PARAMETERS                                #
############################################################################################

""" $(TYPEDEF)
Modified Rodrigues Parameter. Is a 3D parameterization of attitude, and is a sterographic
projection of the 4D unit sphere onto the plane tangent to the negative real pole. They
have a singularity at θ = ±180°.

# Constructors
MRP(x, y, z)
MRP(r::SVector{3})
"""
struct MRP{T} <: Rotation
    x::T
    y::T
    z::T
end

MRP(r::SVector{3}) = MRP(r[1], r[2], r[3])
(::Type{<:MRP})(::Type{T},x,y,z) where T = MRP{T}(T(x),T(y),T(z))
MRP{T}(r::SVector{3}) where T = MRP(T(r[1]), T(r[2]), T(r[3]))
MRP{T}(p::MRP) where T = MRP(T(p.x), T(p.y), T(p.z))
function (::Type{<:MRP})(q::UnitQuaternion)
    M = 1/(1+q.s)
    MRP(q.x*M, q.y*M, q.z*M)
end

function UnitQuaternion(p::MRP)
    n = norm2(p)
    s = (1-n)/(1+n)
    M = 2/(1+n)
    UnitQuaternion{MRPMap}(s, p.x*M, p.y*M, p.z*M)
end

function rotmat(p::MRP)
    p = SVector(p)
    P = skew(p)
    R2 = I + 4( (1-p'p)I + 2*P )*P/(1+p'p)^2
end

SVector(p::MRP{T}) where T = SVector{3,T}(p.x, p.y, p.z)

Base.rand(::Type{MRP{T}}) where T = MRP(rand(UnitQuaternion{T}))
Base.rand(::Type{MRP}) = MRP(rand(UnitQuaternion))
Base.zero(::Type{MRP{T}}) where T = MRP(zero(T), zero(T), zero(T))
Base.zero(::Type{MRP}) = MRP(0.0, 0.0, 0.0)

LinearAlgebra.norm(p::MRP) = sqrt(p.x^2 + p.y^2 + p.z^2)
LinearAlgebra.norm2(p::MRP) = p.x^2 + p.y^2 + p.z^2

(≈)(p2::MRP, p1::MRP) = p2.x ≈ p1.x && p2.y ≈ p1.y && p2.z ≈ p1.z

function (*)(p2::MRP, p1::MRP)
    p2, p1 = SVector(p2), SVector(p1)
    MRP(((1-p2'p2)*p1 + (1-p1'p1)*p2 - cross(2p1, p2) ) / (1+p1'p1*p2'p2 - 2p1'p2))
end

(*)(p::MRP, r::SVector) = UnitQuaternion(p)*r
(\)(p::MRP, r::SVector) = UnitQuaternion(p)\r

function (\)(p1::MRP, p2::MRP)
    # fun fact: equivalent to p2 - p1 when either is 0
    p1,p2 = SVector(p1), SVector(p2)
    n1,n2 = p1'p1, p2'p2
    θ = 1/((1+n1)*(1+n2))
    s1,s2 = (1-n1), (1-n2)
    v1 = -2p1
    v2 =  2p2
    s = s1*s2 - v1'v2
    v = s1*v2 + s2*v1 + v1 × v2

    M = θ/(1+θ*s)
    return MRP(v*M)
end

function ∇err(p1::MRP, p2::MRP)
    n1,n2 = norm2(p1),   norm2(p2)
    θ = 1/((1+n1)*(1+n2))
    s1,s2 = (1-n1), (1-n2)
    p1,p2 = SVector(p1), SVector(p2)
    v1 = -2p1
    v2 =  2p2
    s = s1*s2 - v1'v2
    v = s1*v2 + s2*v1 + v1 × v2
    dsdp = -2s1*p2 - 2v1
    dvdp = 2s1*I + -2v1*p2' + 2skew(v1)
    dθdp = -θ^2*(1+n1)*2p2

    M = θ/(1+θ*s)
    dMdp = 1/(1+θ*s)*dθdp - θ/(1+θ*s)^2*(dθdp*s + θ*dsdp)
    return dvdp*M + v*dMdp'
end

function ∇²err(p1::MRP, p2::MRP, b::SVector{3})
    n1,n2 = norm2(p1),   norm2(p2)
    θ = 1/((1+n1)*(1+n2))
    s1,s2 = (1-n1), (1-n2)
    p1,p2 = SVector(p1), SVector(p2)
    v1 = -2p1
    v2 =  2p2
    s = s1*s2 - v1'v2
    v = s1*v2 + s2*v1 + v1 × v2

    dsdp = -2s1*p2 - 2v1  # 3x1
    dsdp2 = -2s1*I  # 3x3

    dvdp = 2s1*b + -2p2*v1'b - 2skew(v1)*b
    dvdp2 = -I*2v1'b

    dθdp = -θ^2*(1+n1)*2p2  # 3x1
    dθdp2 = -2θ*(1+n1)*2p2*dθdp' - θ^2*(1+n1)*2I # 3x3

    M = θ/(1+θ*s)  # scalar
    dMdp = 1/(1+θ*s)*dθdp - θ/(1+θ*s)^2*(dθdp*s + θ*dsdp) # 3x1
    dM2 = θ/(1+θ*s)^2  # scalar
    dM3 = dθdp*s + θ*dsdp  # 3x1
    dM2dp = dθdp'/(1+θ*s)^2 - 2θ/(1+θ*s)^3 * (dθdp*s + θ*dsdp)'
    dM3dp = dθdp2*s + dθdp*dsdp' + dsdp*dθdp' + θ*dsdp2

    dMdp2 = -1/(1+θ*s)^2*dθdp*(dθdp*s + dsdp*θ)' + 1/(1+θ*s)*dθdp2  # good
    dMdp2 -= dM3*dM2dp + dM2*dM3dp

    vb = s1*v2'b + s2*v1'b + b'skew(v1)*v2  # scalar
    vpdp = s1*2b' - 2p2' * (v1'b)  + b'skew(v1)*2 # good
    # vpdp = s1*2b' - 2p2 * (v1'b)

    d1 = M*dvdp2 + dvdp*dMdp'
    d2 = dMdp2*vb + dMdp*vpdp
    return d1 + d2
end

function (/)(p1::MRP, p2::MRP)
    n1,n2 = norm2(p1),   norm2(p2)
    θ = 1/((1+n1)*(1+n2))
    s1,s2 = (1-n1), (1-n2)
    p1,p2 = SVector(p1), SVector(p2)
    v1 =  2p1
    v2 = -2p2
    s = s1*s2 - v1'v2
    v = s1*v2 + s2*v1 + v1 × v2

    M = θ/(1+θ*s)
    MRP(v[1]*M, v[2]*M, v[3]*M)

end

Base.angle(p::MRP) = angle(UnitQuaternion(p))

function kinematics(p::MRP, ω)
    p = SVector(p)
    A = @SMatrix [
        1 + p[1]^2 - p[2]^2 - p[3]^2  2(p[1]*p[2] - p[3])      2(p[1]*p[3] + p[2]);
        2(p[2]*p[1] + p[3])            1-p[1]^2+p[2]^2-p[3]^2   2(p[2]*p[3] - p[1]);
        2(p[3]*p[1] - p[2])            2(p[3]*p[2] + p[1])      1-p[1]^2-p[2]^2+p[3]^2]
    0.25*A*ω
end

(⊕)(p::MRP, δp::SVector{3}) = p*MRP(δph)
(⊖)(p::MRP, p0::MRP) = SVector(p0\p)

function ∇rotate(p::MRP, r)
    p = SVector(p)
    4( (1-p'p)*skew(r)*(4p*p'/(1+p'p) - I) - (4/(1+p'p)*skew(p) + I)*2*skew(p)*r*p'
      - 2*(skew(p)*skew(r) + skew(skew(p)*r)))/(1+p'p)^2
end

function ∇composition2(p2::MRP, p1::MRP)
    p2,p1 = SVector(p2), SVector(p1)
    n1 = p1'p1
    n2 = p2'p2
    D = 1 / (1+n1*n2 - 2p1'p2)
    d1 = (-2p1*p2' + (1-n1)*I - skew(2p1) ) * D
    d2 = -((1-n2)*p1 + (1-n1)*p2 - cross(2p1, p2) ) * D^2 *
        (2p2*n1 - 2p1)'
    return d1 + d2
end

function ∇composition1(p2::MRP, p1::MRP)
    p2,p1 = SVector(p2), SVector(p1)
    n1 = p1'p1
    n2 = p2'p2
    D = 1 / (1+n1*n2 - 2p1'p2)
    d1 = ((1-n2)*I + -2p2*p1' + 2skew(p2) ) * D
    d2 = -((1-n2)*p1 + (1-n1)*p2 - cross(2p1, p2) ) * D^2 *
        (2p1*n2 - 2p2)'
    d1 + d2
end

function ∇differential(p::MRP)
    p = SVector(p)
    n = p'p
    # p = SVector(p)
    # (1-n)I + 2(skew(p) + p*p')
    # @SMatrix [n + 2p.x^2      2(p.x*p.y-p.z)  2(p.x*p.z+p.y);
    #           2(p.y*p.x+p.z)  n + 2p.y^2      2(p.y*p.z-p.x);
    #           2(p.z*p.x-p.y)  2(p.z*p.y+p.x)  n + 2p.z^2]
    #
    # p2 = SVector(p)
    # n2 = p2'p2
    return (1-n)*I + 2(skew(p) + p*p')
end

function ∇²composition1(p2::MRP, p1::MRP, b::SVector{3})
    p2,p1 = SVector(p2), SVector(p1)
    n1 = p1'p1
    n2 = p2'p2
    D = 1 / (1+n1*n2 - 2p1'p2)  # scalar
    dD = -D^2 * (n2*2p1 - 2p2)  # 3x1
    A = -((1-n2)*p1 + (1-n1)*p2 - cross(2p1, p2) )  # 3x1
    dA = -I*(1-n2) + 2p2*p1' - 2skew(p2)  # 3x3
    B = 2(p1*n2 -  p2)'b  # scalar
    dB = 2n2*b  # 3x1
    d1 = -2p2*b' * D + ((1-n2)*b + -2p2*p1'b + 2skew(p2)*b )*dD'
    d2 = dA * D^2 * B +
         A * 2D * dD' * B +
         A * D^2 * dB'
    return d1 + d2
end

function ∇²differential(p2::MRP, b::SVector{3})
    p2 = SVector(p2)
    n2 = p2'p2
    A = -p2  # 3x1
    B = -2p2  # 3x1
    D = 1
    dD = 2p2

    dA = -I*(1-n2) - 2skew(p2)  # 3x3
    dB = 2n2*I  # 3x3

    d1 = (-2p2'b*I*D) - (dA'b * dD')
    d2 = dB * A'b * D^2 +
        (-(1-n2)*b + (skew(2p2))*b)*B' * D^2 +
        B*A'b * 2D * dD'
    d1 + d2'
end

############################################################################################
#                                 Rodrigues Parameters
############################################################################################

struct RodriguesParam{T} <: Rotation
    x::T
    y::T
    z::T
end

RodriguesParam(g::SVector{3,T}) where T = RodriguesParam{T}(g[1], g[2], g[3])
(::Type{<:RodriguesParam})(::Type{T},x,y,z) where T = RodriguesParam{T}(T(x),T(y),T(z))
(::Type{<:RodriguesParam})(p::RodriguesParam) = p
SVector(g::RodriguesParam{T}) where T = SVector{3,T}(g.x, g.y, g.z)

(::Type{<:RodriguesParam})(q::UnitQuaternion{T}) where T = RodriguesParam(q.x/q.s, q.y/q.s, q.z/q.s)

function UnitQuaternion(g::RodriguesParam{T}) where T
    M = 1/sqrt(1+norm2(g))
    UnitQuaternion{T,CayleyMap}(M, M*g.x, M*g.y, M*g.z)
end

Base.rand(::Type{<:RodriguesParam}) = RodriguesParam(rand(UnitQuaternion))
Base.zero(::Type{<:RodriguesParam}) = RodriguesParam(0.0, 0.0, 0.0)

LinearAlgebra.norm(g::RodriguesParam) = sqrt(g.x^2 + g.y^2 + q.z^2)
LinearAlgebra.norm2(g::RodriguesParam) = g.x^2 + g.y^2 + g.z^2

function (≈)(g2::RodriguesParam, g1::RodriguesParam)
    g2.x ≈ g1.x && g2.y ≈ g1.y && g2.z ≈ g1.z
end

function (*)(g2::RodriguesParam, g1::RodriguesParam)
    g2 = SVector(g2)
    g1 = SVector(g1)
    RodriguesParam((g2+g1 + g2 × g1)/(1-g2'g1))
end

(*)(g::RodriguesParam, r::SVector{3}) = UnitQuaternion(g)*r
(\)(g::RodriguesParam, r::SVector{3}) = inv(UnitQuaternion(g))*r

"Same as inv(g1)*g2"
function (\)(g1::RodriguesParam, g2::RodriguesParam)
    g2 = SVector(g2)
    g1 = SVector(g1)
    RodriguesParam((g2-g1 + g2 × g1)/(1+g1'g2))
end

"Same as g2*inv(g1)"
function (/)(g1::RodriguesParam, g2::RodriguesParam)
    g2 = SVector(g2)
    g1 = SVector(g1)
    RodriguesParam((g1-g2 + g2 × g1)/(1+g1'g2))
end

function kinematics(g::RodriguesParam, ω::SVector{3})
    g = SVector(g)
    0.5*(I + skew(g) + g*g')*ω
end

(⊕)(g::RodriguesParam, δg::SVector{3}) = g*RodriguesParam(δg)
(⊖)(g::RodriguesParam, g0::RodriguesParam) = SVector(g0\g)

Base.angle(g::RodriguesParam) = angle(UnitQuaternion(g))

function rotmat(g::RodriguesParam)
    ghat = skew(SVector(g))
    I + 2*ghat*(ghat + I)/(1+norm2(g))
end

function ∇rotate(g0::RodriguesParam, r)
    g = SVector(g0)
    ghat = skew(g)
    n1 = 1/(1 + g'g)
    gxr = cross(g,r) + r
    d1 = ghat*gxr * -2*n1^2 * g'
    d2 = -(ghat*skew(r) + skew(gxr))*n1
    return 2d1 + 2d2
end

function ∇composition1(g2::RodriguesParam, g1::RodriguesParam)
    g2 = SVector(g2)
    g1 = SVector(g1)

    N = g2 + g1 + g2 × g1
    D = 1/(1 - g2'g1)
    (I + skew(g2) + D*N*g2')*D
end

function ∇²composition1(g2::RodriguesParam, g1::RodriguesParam, b::SVector{3})
    g2 = SVector(g2)
    g1 = SVector(g1)

    N = g2 + g1 + g2 × g1  # 3x1
    D = 1/(1 - g2'g1)  # scalar
    dN = I + skew(g2)
    dD = D^2*g2'
    return g2*b'*(N*(2*D*dD) + D^2*dN) + (I - skew(g2))*b*dD
end

function ∇composition2(g2::RodriguesParam, g1::RodriguesParam)
    g2 = SVector(g2)
    g1 = SVector(g1)

    N = g2 + g1 + g2 × g1
    D = 1/(1 - g2'g1)
    (I - skew(g1) + D*N*g1')*D
end

function ∇differential(g::RodriguesParam)
    g = SVector(g)
    (I + skew(g) + g*g')
end

function ∇²differential(g::RodriguesParam, b::SVector{3})
    g = SVector(g)
    return g*b'*(2g*g' + I + skew(g)) + (I - skew(g))*b*g'
end

############################################################################################
#                             Roll, Pitch, Yaw Euler Angles
############################################################################################

""" $(TYPEDEF)
Roll-pitch-yaw Euler angles.
"""
struct RPY{T} <: Rotation
    ϕ::T  # roll
    θ::T  # pitch
    ψ::T  # yaw
end

RPY(e::SVector{3,T}) where T = RPY{T}(e[1], e[2], e[3])
RPY(R::SMatrix{3,3,T}) where T =  RPY(rotmat_to_rpy(R))
RPY(q::UnitQuaternion) = RPY(rotmat(q))
RPY(p::MRP) = RPY(rotmat(p))
(::Type{<:RPY})(e::RPY) = e
(::Type{<:RPY})(::Type{T},x,y,z) where T = RPY{T}(T(x),T(y),T(z))
function RPY(ϕ::T1,θ::T2,ψ::T3) where {T1,T2,T3}
    T = promote_type(T1,T2)
    T = promote_type(T,T3)
    RPY(T(ϕ), T(θ), T(ψ))
end

SVector(e::RPY{T}) where T = SVector{3,T}(e.ϕ, e.θ, e.ψ)

Base.rand(::Type{RPY{T}}) where T = RPY(rand(UnitQuaternion{T}))
Base.rand(::Type{RPY}) = RPY(rand(UnitQuaternion))
Base.zero(::Type{RPY{T}}) where T = RPY(zero(T), zero(T), zero(T))
Base.zero(::Type{RPY}) = RPY(0.0, 0.0, 0.0)

roll(e::RPY) = e.ϕ
pitch(e::RPY) = e.θ
yaw(e::RPY) = e.ψ

Base.angle(e::RPY) = angle(UnitQuaternion(e))

(≈)(e1::RPY, e2::RPY) = rotmat(e1) ≈ rotmat(e2)

@inline rotmat(e::RPY) = rotmat(e.ϕ, e.θ, e.ψ)

function rotmat(ϕ, θ, ψ)
    # Equivalent to RotX(e[1])*RotY(e[2])*RotZ(e[3])
    if !isfinite(ϕ) || !isfinite(θ) || !isfinite(ψ)
        return @SMatrix zeros(3,3)
    end
    sϕ,cϕ = sincos(ϕ)
    sθ,cθ = sincos(θ)
    sψ,cψ = sincos(ψ)
    # A = @SMatrix [
    #     cθ*cψ          -cθ*sψ              sθ;
    #     sϕ*sθ*cψ+cϕ*sψ -sϕ*sθ*sψ + cϕ*cψ  -cθ*sϕ;
    #    -cϕ*sθ*cψ+sϕ*sψ  cϕ*sθ*sψ + sϕ*cψ   cθ*cϕ
    # ]
    A = @SMatrix [
        cψ*cθ - sϕ*sψ*sθ   -cϕ*sψ  cψ*sθ + cθ*sϕ*sψ;
        cθ*sψ + cψ*sϕ*sθ    cϕ*cψ  sψ*sθ - cψ*cθ*sϕ;
        -cϕ*sθ              sϕ          cϕ*cθ;
    ]
end

(*)(e::RPY, r::SVector{3}) = rotmat(e)*r
(\)(e::RPY, r::SVector{3}) = rotmat(e)'r

function rotmat_to_rpy(R::SMatrix{3,3,T}) where T
    # ψ = atan(-R[1,2], R[1,1])
    # ϕ = atan(-R[2,3], R[3,3])
    # θ = asin(R[1,3])
    θ = atan(-R[3,1], R[3,3])
    ψ = atan(-R[1,2], R[2,2])
    ϕ = asin(clamp(R[3,2],-1,1))
    return SVector{3,T}(ϕ, θ, ψ)
end

function from_rotmat(R::SMatrix{3,3,T}) where T
    ϕ,θ,ψ = rotmat_to_rpy(R)
    return RPY(ϕ, θ, ψ)
end

function (*)(e2::RPY, e1::RPY)
    from_rotmat(rotmat(e2)*rotmat(e1))
end

function (\)(e1::RPY, e2::RPY)
    from_rotmat(rotmat(e1)'rotmat(e2))
end

function (/)(e1::RPY, e2::RPY)
    from_rotmat(rotmat(e1)*rotmat(e2)')
end

function (⊖)(e1::RPY, e2::RPY)
    SVector(e1\e1)
end

function kinematics(e::RPY, ω::SVector{3})
    sθ,cθ = sincos(e.θ)
    sϕ,cϕ = sincos(e.ϕ)
    A = @SMatrix [
        cθ 0 -cϕ*sθ;
        0  1  sϕ;
        sθ 0  cϕ*cθ
    ]
    A\ω
end

function ∇rotate(e::RPY, r::SVector{3})
    rotate(e) = RPY(e)*r
    ForwardDiff.jacobian(rotate, SVector(e))
end

function ∇composition1(e2::RPY, e1::RPY)
    R2 = rotmat(e2)
    rotate(e) = rotmat_to_rpy(R2*rotmat(e[1],e[2],e[3]))
    ForwardDiff.jacobian(rotate,SVector(e1))
end

function ∇composition2(e2::RPY, e1::RPY)
    R1 = rotmat(e1)
    rotate(e) = rotmat_to_rpy(rotmat(e[1],e[2],e[3])*R1)
    ForwardDiff.jacobian(rotate,SVector(e2))
end

############################################################################################
#                                 CONVERSIONS
############################################################################################
""" Convert from a rotation matrix to a unit quaternion
Uses formula from Markely and Crassidis's book
    "Fundamentals of Spacecraft Attitude Determination and Control" (2014), section 2.9.3
"""
function rotmat_to_quat(A::SMatrix{3,3,T}) where T
    trA = tr(A)
    v,i = findmax(diag(A))
    if trA > v
        i = 1
    else
        i += 1
    end
    if i == 1
        q = UnitQuaternion(1+trA, A[2,3]-A[3,2], A[3,1]-A[1,3], A[1,2]-A[2,1])
    elseif i == 2
        q = UnitQuaternion(A[2,3]-A[3,2], 1 + 2A[1,1] - trA, A[1,2]+A[2,1], A[1,3]+A[3,1])
    elseif i == 3
        q = UnitQuaternion(A[3,1]-A[1,3], A[2,1]+A[1,2], 1+2A[2,2]-trA, A[2,3]+A[3,2])
    elseif i == 4
        q = UnitQuaternion(A[1,2]-A[2,1], A[3,1]+A[1,3], A[3,2]+A[2,3], 1 + 2A[3,3] - trA)
    end
    return normalize(inv(q))
end

UnitQuaternion(e::RPY) = rotmat_to_quat(rotmat(e))
(::Type{<:RPY})(q::UnitQuaternion) = from_rotmat(rotmat(q))

function (::Type{<:UnitQuaternion})(p::MRP)
    p = SVector(p)
    n2 = p'p
    M = 2/(1+n2)
    UnitQuaternion{MRPMap}((1-n2)/(1+n2), M*p[1], M*p[2], M*p[3])
end

(::Type{<:RodriguesParam})(p::MRP) = RodriguesParam(UnitQuaternion(p))

(::Type{<:RPY})(p::MRP) = RPY(UnitQuaternion(p))

############################################################################################
#                             INVERSE RETRACTION MAPS
############################################################################################
(::Type{ExponentialMap})(q::UnitQuaternion) = scaling(ExponentialMap)*logm(q)

(::Type{VectorPart})(q::UnitQuaternion) = scaling(VectorPart)*vector(q)

(::Type{CayleyMap})(q::UnitQuaternion) = scaling(CayleyMap) * vector(q)/q.s

(::Type{MRPMap})(q::UnitQuaternion) = scaling(MRPMap)*vector(q)/(1+q.s)

(::Type{IdentityMap})(q::UnitQuaternion) = SVector(q)

function jacobian(::Type{ExponentialMap}, q::UnitQuaternion, eps=1e-5)
    μ = scaling(ExponentialMap)
    s = scalar(q)
    v = vector(q)
    θ2 = v'v
    θ = sqrt(θ2)
    datan = 1/(θ2 + s^2)
    ds = -datan*v

    if θ < eps
        return 2*μ*[ds (v*v' + I)/s]
    else
        atanθ = atan(θ,s)
        dv = ((s*datan - atanθ/θ)v*v'/θ + atanθ*I )/θ
        d0 = ((s*datan - atanθ/θ)v*v'/θ^2 + atanθ/θ*I )
        d1 = (s*datan - atanθ/θ)
        d2 = v*v'/θ2
        d3 = atanθ/θ * I
        return 2*μ*[ds dv]
    end
end

function ∇jacobian(::Type{ExponentialMap}, q::UnitQuaternion, b::SVector{3}, eps=1e-5)
    μ = scaling(ExponentialMap)
    s = scalar(q)
    v = vector(q)
    θ2 = v'v
    θ = sqrt(θ2)
    datan = 1/(θ2 + s^2)
    ds = -datan*v

    if θ < eps
        # return 2*μ*[b'ds; (v*v'b + b)/s]
        return 2*μ*[b'*(datan^2*2s*v) -b'datan*I;
                    -(v*v'b +b)/s^2 (I*(v'b) + v*b')/s]
    else
        dsds = 2b's*datan^2*v
        dsdv = b'*(-datan*I + 2datan^2*v*v')

        atanθ = atan(θ,s)
        d1 = (s*datan - atanθ/θ)
        d2 = v*v'b/θ2
        d3 = atanθ/θ*b
        d1ds = (datan - 2s^2*datan^2 + datan)
        dvds = d1ds*d2 - datan*b

        d1dv =  (-2s*datan^2*v' - s*datan*v'/θ^2 + atanθ/θ^3*v')
        d2dv = (I*(v'b) + v*b')/θ2 - 2(v*v'b)/θ^4 * v'
        d3dv = b*(s*datan*v'/θ^2 - atanθ/θ^3*v')
        dvdv = d2*d1dv + d1*d2dv + d3dv

        # return 2*μ*[ds'b; dv'b]
        return 2*μ*@SMatrix [
            dsds    dsdv[1] dsdv[2] dsdv[3];
            dvds[1] dvdv[1] dvdv[4] dvdv[7];
            dvds[2] dvdv[2] dvdv[5] dvdv[8];
            dvds[3] dvdv[3] dvdv[6] dvdv[9];
        ]
    end
end

function jacobian(::Type{VectorPart}, q::UnitQuaternion)
    μ = scaling(VectorPart)
    return @SMatrix [0. μ 0 0;
                     0. 0 μ 0;
                     0. 0 0 μ]
end

function ∇jacobian(::Type{VectorPart}, q::UnitQuaternion, b::SVector{3})
    μ = scaling(VectorPart)
    @SMatrix zeros(4,4)
end

function jacobian(::Type{CayleyMap}, q::UnitQuaternion)
    μ = scaling(CayleyMap)
    si = 1/q.s
    return μ*@SMatrix [-si^2*q.x si 0 0;
                       -si^2*q.y 0 si 0;
                       -si^2*q.z 0 0 si]
end

function ∇jacobian(::Type{CayleyMap}, q::UnitQuaternion, b::SVector{3})
    μ = scaling(CayleyMap)
    si = 1/q.s
    v = vector(q)
    μ*@SMatrix [
        2*si^3*(v'b) -si^2*b[1] -si^2*b[2] -si^2*b[3];
       -si^2*b[1] 0 0 0;
       -si^2*b[2] 0 0 0;
       -si^2*b[3] 0 0 0;
    ]
end

function jacobian(::Type{MRPMap}, q::UnitQuaternion)
    μ = scaling(MRPMap)
    si = 1/(1+q.s)
    return μ*@SMatrix [-si^2*q.x si 0 0;
                       -si^2*q.y 0 si 0;
                       -si^2*q.z 0 0 si]
end

"""
Jacobian of G(q)'b, where G(q) = jacobian(MRPMap,q)
"""
function ∇jacobian(::Type{MRPMap}, q::UnitQuaternion, b::SVector{3})
    μ = scaling(MRPMap)
    si = 1/(1+q.s)
    v = vector(q)
    μ * @SMatrix [
        2*si^3*(v'b) -si^2*b[1] -si^2*b[2] -si^2*b[3];
       -si^2*b[1] 0 0 0;
       -si^2*b[2] 0 0 0;
       -si^2*b[3] 0 0 0;
    ]
end

jacobian(::Type{IdentityMap}, q::UnitQuaternion) = I

inverse_map_jacobian(q::R) where R<:Rotation = I
inverse_map_jacobian(q::UnitQuaternion{T,D}) where {T,D} = jacobian(D,q)

inverse_map_∇jacobian(q::R, b::SVector{3}) where R<:Rotation = I*0
inverse_map_∇jacobian(q::UnitQuaternion{T,D}, b::SVector{3}) where {T,D} = ∇jacobian(D, q, b)


text:
# Some utility functions for optimizing linear algebra operations that aren't specific
# to any particular rule definition

# F .* (X - X'), overwrites X
function _mulsubtrans!(X::AbstractMatrix{T}, F::AbstractMatrix{T}) where T<:Real
    k = size(X, 1)
    @inbounds for j = 1:k, i = 1:j  # Iterate the upper triangle
        if i == j
            X[i,i] = zero(T)
        else
            X[i,j], X[j,i] = F[i,j] * (X[i,j] - X[j,i]), F[j,i] * (X[j,i] - X[i,j])
        end
    end
    X
end

# I - X, overwrites X
function _eyesubx!(X::AbstractMatrix)
    n, m = size(X)
    @inbounds for j = 1:m, i = 1:n
        X[i,j] = (i == j) - X[i,j]
    end
    X
end

# X + Y, overwrites X
function _add!(X::AbstractVecOrMat{T}, Y::AbstractVecOrMat{T}) where T<:Real
    @inbounds for i = eachindex(X, Y)
        X[i] += Y[i]
    end
    X
end

ext: jl


text:
using ApproxFun
using SpectralMeasures
using Plots; Plots.gr(legend=false,linewidth=2, xlims=(-1.2,1.2),ylims=(0,1))

Δ = freejacobioperator()

# Legendre polynomials
n=1
  bLeg = (1:n-1)./sqrt(4*(1:n-1).^2-1)
  plot(spectralmeasure(zeros(n),bLeg),title="n = $(n)")

# Ultraspherical polynomials
γ = 0.6
n= 100
  bUlt = .5*sqrt(((1:n).*(2γ+(0:n-1)))./((γ+(0:n-1)).*(γ+(1:n))))
  plot(spectralmeasure(zeros(n),bUlt),title="n = $(n), \\gamma = $(γ)")

# Jacobi polynomials
α = 0.4; β = 1.9
  n=0
  a = (β.^2-α.^2)./((2.*(0:n)+α+β).*(2.*(1:n+1)+α+β))
  b = 2*sqrt(((1:n).*(α+(1:n)).*(β+(1:n)).*(α+β+(1:n)))./((2.*(1:n)+α+β-1).*((2.*(1:n)+α+β).^2).*(2.*(1:n)+α+β+1)))
  plot(spectralmeasure(a,b),title="n = $(n), \\alpha = $(α), \\beta = $(β)")


text:
# *****************************************************************************
# Written by Ritchie Lee, ritchie.lee@sv.cmu.edu
# *****************************************************************************
# Copyright ã ``2015, United States Government, as represented by the
# Administrator of the National Aeronautics and Space Administration. All
# rights reserved.  The Reinforcement Learning Encounter Simulator (RLES)
# platform is licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License. You
# may obtain a copy of the License at
# http://www.apache.org/licenses/LICENSE-2.0. Unless required by applicable
# law or agreed to in writing, software distributed under the License is
# distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
# KIND, either express or implied. See the License for the specific language
# governing permissions and limitations under the License.
# _____________________________________________________________________________
# Reinforcement Learning Encounter Simulator (RLES) includes the following
# third party software. The SISLES.jl package is licensed under the MIT Expat
# License: Copyright (c) 2014: Youngjun Kim.
# 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.
# *****************************************************************************

"""
Collection of fitness functions
"""
module FitnessFunctions

export FitnessFunction
export Gini_NumNodes

using GrammaticalEvolution
using ExprSearch, GBDMProblems, DerivationTrees
using RLESUtils, TreeUtils, MathUtils

import RLESTypes.SymbolTable
import ExprSearch.get_fitness

immutable Gini_NumNodes <: FitnessFunction
    w_gini::Float64
    w_num_nodes::Float64
end

function get_fitness(fitness_func::Gini_NumNodes, problem::GBDMProblem,
    derivtree::DerivationTree, userargs::SymbolTable=SymbolTable())
    ids = userargs[:ids]

    expr = get_expr(derivtree)
    predict_labels = apply_expr(problem, ids, expr)
    num_nodes = count_nodes(derivtree.root)

    truth = view(problem.data.labels, ids)
    metric = gini_impurity(truth[predict_labels], truth[!predict_labels])
    fitness = fitness_func.w_gini * metric + fitness_func.w_num_nodes * num_nodes
    fitness
end

end #module

max_stars_repo_name: sisl/ExprSearch

max_issues_repo_name: sisl/ExprSearch


text:
using LinearAlgebra, WriteVTK, ProgressMeter, JLD2, PyCall
import KitBase

cd(@__DIR__)
meshio = pyimport("meshio")
m = meshio.read("../assets/mesh/cylinder_circle.msh")

D = KitBase.read_dict("cylinder.txt")
set = KitBase.set_setup(D)

ps = KitBase.set_geometry(D)
for i in eachindex(ps.faceType)
    i1 = ps.facePoints[i, 1]
    i2 = ps.facePoints[i, 2]

    if i1 in [ps.cells.index[1]; ps.cells.index[2]] &&
       i2 in [ps.cells.index[1]; ps.cells.index[2]]
        ps.faceType[i] = 2
        c1 = ps.faceCells[i, 1]
        c2 = ps.faceCells[i, 2]

        if c1 != -1
            ps.cellType[c1] = 2
        elseif c2 != -1
            ps.cellType[c2] = 2
        else
            throw("index error")
        end
    end
end
for i in eachindex(ps.cellType)
    if ps.cellType[i] == 1 && ps.cellCenter[i, 1] > 0.0
        ps.cellType[i] = 3
    end
end

vs = KitBase.set_velocity(D)
gas = KitBase.set_property(D)

begin
    primL = [1.0, KitBase.sound_speed(1.0, gas.γ) * gas.Ma, 0.0, 1.0]
    wL = KitBase.prim_conserve(primL, gas.γ)
    hL = KitBase.maxwellian(vs.u, vs.v, primL)
    bL = @. hL * gas.K / 2 / primL[end]
    primR = [1.0, 0.0, 0.0, 1.0]
    wR = KitBase.prim_conserve(primR, gas.γ)
    hR = KitBase.maxwellian(vs.u, vs.v, primR)
    bR = @. hR * gas.K / 2 / primR[end]
    ib = KitBase.IB2F(wL, primL, hL, bL, primL, wL, primL, hL, bL, primR)
end

ks = KitBase.SolverSet(set, ps, vs, gas, ib, @__DIR__)

ctr, face = KitBase.init_fvm(ks, ks.pSpace)

dt = KitBase.timestep(ks, ctr, 0.0)
nt = ks.set.maxTime ÷ dt |> Int
#=
@showprogress for iter = 1:2000#nt
    @inbounds Threads.@threads for i in eachindex(face)
        vn = ks.vSpace.u .* face[i].n[1] .+ ks.vSpace.v .* face[i].n[2]
        vt = ks.vSpace.v .* face[i].n[1] .- ks.vSpace.u .* face[i].n[2]

        if !(-1 in ps.faceCells[i, :])
            KitBase.flux_kfvs!(
                face[i].fw,
                face[i].fh,
                face[i].fb,
                ctr[ps.faceCells[i, 1]].h,
                ctr[ps.faceCells[i, 1]].b,
                ctr[ps.faceCells[i, 2]].h,
                ctr[ps.faceCells[i, 2]].b,
                vn,
                vt,
                ks.vSpace.weights,
                dt,
                face[i].len,
            )
            face[i].fw .= KitBase.global_frame(face[i].fw, face[i].n[1], face[i].n[2])
        else
            idx = ifelse(ps.faceCells[i, 1] != -1, 1, 2)

            if ps.cellType[ps.faceCells[i, idx]] == 2
                bc = KitBase.local_frame(ks.ib.primR, face[i].n[1], face[i].n[2])

                KitBase.flux_boundary_maxwell!(
                    face[i].fw,
                    face[i].fh,
                    face[i].fb,
                    bc,
                    ctr[ps.faceCells[i, idx]].h,
                    ctr[ps.faceCells[i, idx]].b,
                    vn,
                    vt,
                    ks.vSpace.weights,
                    ks.gas.K,
                    dt,
                    face[i].len,
                )

                face[i].fw .= KitBase.global_frame(face[i].fw, face[i].n[1], face[i].n[2])
            end
        end
    end

    @inbounds Threads.@threads for i in eachindex(ctr)
        if ps.cellType[i] in (0, 2)
            MH = KitBase.maxwellian(vs.u, vs.v, ctr[i].prim)
            MB = MH .* ks.gas.K ./ (2.0 * ctr[i].prim[end])
            τ = KitBase.vhs_collision_time(ctr[i].prim, ks.gas.μᵣ, ks.gas.ω)

            for j in 1:3
                dirc = sign(dot(ctr[i].n[j], face[ps.cellFaces[i, j]].n))
                @. ctr[i].w -= dirc * face[ps.cellFaces[i, j]].fw / ps.cellArea[i]
                @. ctr[i].h -= dirc * face[ps.cellFaces[i, j]].fh / ps.cellArea[i]
                @. ctr[i].b -= dirc * face[ps.cellFaces[i, j]].fb / ps.cellArea[i]
            end
            ctr[i].prim .= KitBase.conserve_prim(ctr[i].w, ks.gas.γ)

            @. ctr[i].h += (MH - ctr[i].h) * dt / τ
            @. ctr[i].b += (MB - ctr[i].b) * dt / τ
            #=for q in axes(vs.v, 2), p in axes(vs.u, 1)
                ctr[i].h[p, q] = (ctr[i].h[p, q] - (face[ps.cellFaces[i, 1]].fh[p, q] + face[ps.cellFaces[i, 2]].fh[p, q] + face[ps.cellFaces[i, 3]].fh[p, q]) / ps.cellArea[i] +
                    dt / τ * MH[p, q]) / (1.0 + dt / τ)
                ctr[i].b[p, q] = (ctr[i].b[p, q] - (face[ps.cellFaces[i, 1]].fb[p, q] + face[ps.cellFaces[i, 2]].fb[p, q] + face[ps.cellFaces[i, 3]].fb[p, q]) / ps.cellArea[i] +
                    dt / τ * MB[p, q]) / (1.0 + dt / τ)
            end=#
        end
    end

    for i in eachindex(ps.cellType)
        if ps.cellType[i] == 3
            ids = ps.cellNeighbors[i, :]
            deleteat!(ids, findall(x->x==-1, ids))
            id1, id2 = ids
            ctr[i].w .= 0.5 .* (ctr[id1].w .+ ctr[id2].w)
            ctr[i].h .= 0.5 .* (ctr[id1].h .+ ctr[id2].h)
            ctr[i].b .= 0.5 .* (ctr[id1].b .+ ctr[id2].b)
            ctr[i].prim .= KitBase.conserve_prim(ctr[i].w, ks.gas.γ)
        end
    end
end
=#

@showprogress for iter = 1:nt
    @inbounds Threads.@threads for i in eachindex(face)
        vn = ks.vSpace.u .* face[i].n[1] .+ ks.vSpace.v .* face[i].n[2]
        vt = ks.vSpace.v .* face[i].n[1] .- ks.vSpace.u .* face[i].n[2]

        if !(-1 in ps.faceCells[i, :])
            KitBase.flux_kfvs!(
                face[i].fw,
                face[i].fh,
                face[i].fb,
                ctr[ps.faceCells[i, 1]].h,
                ctr[ps.faceCells[i, 1]].b,
                ctr[ps.faceCells[i, 2]].h,
                ctr[ps.faceCells[i, 2]].b,
                vn,
                vt,
                ks.vSpace.weights,
                dt,
                face[i].len,
            )
            face[i].fw .= KitBase.global_frame(face[i].fw, face[i].n[1], face[i].n[2])
        else
            idx = ifelse(ps.faceCells[i, 1] != -1, 1, 2)

            if ps.cellType[ps.faceCells[i, idx]] == 2
                bc = KitBase.local_frame(ks.ib.primR, face[i].n[1], face[i].n[2])

                KitBase.flux_boundary_maxwell!(
                    face[i].fw,
                    face[i].fh,
                    face[i].fb,
                    bc,
                    ctr[ps.faceCells[i, idx]].h,
                    ctr[ps.faceCells[i, idx]].b,
                    vn,
                    vt,
                    ks.vSpace.weights,
                    ks.gas.K,
                    dt,
                    face[i].len,
                )

                face[i].fw .= KitBase.global_frame(face[i].fw, face[i].n[1], face[i].n[2])
            end
        end
    end

    sumres = zeros(4)
    sumavg = zeros(4)
    @inbounds Threads.@threads for i in eachindex(ctr)
        if ps.cellType[i] in (0, 2)
            dirc = [sign(dot(ctr[i].n[j], face[ps.cellFaces[i, j]].n)) for j = 1:3]

            KitBase.step!(
                ctr[i].w,
                ctr[i].prim,
                ctr[i].h,
                ctr[i].b,
                face[ps.cellFaces[i, 1]].fw,
                face[ps.cellFaces[i, 1]].fh,
                face[ps.cellFaces[i, 1]].fb,
                face[ps.cellFaces[i, 2]].fw,
                face[ps.cellFaces[i, 2]].fh,
                face[ps.cellFaces[i, 2]].fb,
                face[ps.cellFaces[i, 3]].fw,
                face[ps.cellFaces[i, 3]].fh,
                face[ps.cellFaces[i, 3]].fb,
                ks.vSpace.u,
                ks.vSpace.v,
                ks.vSpace.weights,
                ks.gas.K,
                ks.gas.γ,
                ks.gas.μᵣ,
                ks.gas.ω,
                ks.gas.Pr,
                ks.pSpace.cellArea[i],
                dirc,
                dt,
                sumres,
                sumavg,
                :bgk,
            )
        end
    end

    for i in eachindex(ps.cellType)
        if ps.cellType[i] == 3
            ids = ps.cellNeighbors[i, :]
            deleteat!(ids, findall(x -> x == -1, ids))
            id1, id2 = ids
            ctr[i].w .= 0.5 .* (ctr[id1].w .+ ctr[id2].w)
            ctr[i].h .= 0.5 .* (ctr[id1].h .+ ctr[id2].h)
            ctr[i].b .= 0.5 .* (ctr[id1].b .+ ctr[id2].b)
            ctr[i].prim .= KitBase.conserve_prim(ctr[i].w, ks.gas.γ)
        end
    end
end

KitBase.write_vtk(ks, ctr)


text:
export Likelihood

@doc raw"""
    Likelihood(M<:ParameterizedMeasure, x)

"Observe" a value `x`, yielding a function from the parameters to ℝ.

Likelihoods are most commonly used in conjunction with an existing _prior_
measure to yield a new measure, the _posterior_. In Bayes's Law, we have

``P(θ|x) ∝ P(θ) P(x|θ)``

Here ``P(θ)`` is the prior. If we consider ``P(x|θ)`` as a function on ``θ``,
then it is called a likelihood.

Since measures are most commonly manipulated using `density` and `logdensity`,
it's awkward to commit a (log-)likelihood to using one or the other. To evaluate
a `Likelihood`, we therefore use `density` or `logdensity`, depending on the
circumstances. In the latter case, it is of course acting as a log-density.

For example,

    julia> ℓ = Likelihood(Normal{(:μ,)}, 2.0)
    Likelihood(Normal{(:μ,), T} where T, 2.0)

    julia> density(ℓ, (μ=2.0,))
    1.0

    julia> logdensity(ℓ, (μ=2.0,))
    -0.0

If, as above, the measure includes the parameter information, we can optionally
leave it out of the second argument in the call to `density` or `logdensity`.

    julia> density(ℓ, 2.0)
    1.0

    julia> logdensity(ℓ, 2.0)
    -0.0

With several parameters, things work as expected:

    julia> ℓ = Likelihood(Normal{(:μ,:σ)}, 2.0)
    Likelihood(Normal{(:μ, :σ), T} where T, 2.0)

    julia> logdensity(ℓ, (μ=2, σ=3))
    -1.0986122886681098

    julia> logdensity(ℓ, (2,3))
    -1.0986122886681098

    julia> logdensity(ℓ, [2, 3])
    -1.0986122886681098

---------

    Likelihood(M<:ParameterizedMeasure, constraint::NamedTuple, x)

In some cases the measure might have several parameters, and we may want the
(log-)likelihood with respect to some subset of them. In this case, we can use
the three-argument form, where the second argument is a constraint. For example,

    julia> ℓ = Likelihood(Normal{(:μ,:σ)}, (σ=3.0,), 2.0)
    Likelihood(Normal{(:μ, :σ), T} where T, (σ = 3.0,), 2.0)

Similarly to the above, we have

    julia> density(ℓ, (μ=2.0,))
    0.3333333333333333

    julia> logdensity(ℓ, (μ=2.0,))
    -1.0986122886681098

    julia> density(ℓ, 2.0)
    0.3333333333333333

    julia> logdensity(ℓ, 2.0)
    -1.0986122886681098

-----------------------

Finally, let's return to the expression for Bayes's Law,

``P(θ|x) ∝ P(θ) P(x|θ)``

The product on the right side is computed pointwise. To work with this in
MeasureTheory, we have a "pointwise product" `⊙`, which takes a measure and a
likelihood, and returns a new measure, that is, the unnormalized posterior that has density ``P(θ) P(x|θ)`` with respect to the base measure of the prior.

For example, say we have

    μ ~ Normal()
    x ~ Normal(μ,σ)
    σ = 1

and we observe `x=3`. We can compute the posterior measure on `μ` as

    julia> post = Normal() ⊙ Likelihood(Normal{(:μ, :σ)}, (σ=1,), 3)
    Normal() ⊙ Likelihood(Normal{(:μ, :σ), T} where T, (σ = 1,), 3)

    julia> logdensity(post, 2)
    -2.5
"""
struct Likelihood{F,S,X}
    k::Kernel{F,S}
    x::X
end

Likelihood(μ::AbstractMeasure, x) = Likelihood(kernel(μ),x)

Likelihood(::Type{M}, x) where {M<:AbstractMeasure} = Likelihood(kernel(M),x)

function Base.show(io::IO, ::MIME"text/plain", ℓ::Likelihood)
    io = IOContext(io, :compact => true)
    k, x = ℓ.k, ℓ.x
    print(io, "Likelihood(",k,", ", x, ")")
end

function logdensity(ℓ::Likelihood, p)
    return logdensity(ℓ.k(p), ℓ.x)
end


text:
## Generic ##

function Base.fill(
    value::Tracker.TrackedReal,
    dims::Vararg{Union{Integer, AbstractUnitRange}},
)
    return Tracker.track(fill, value, dims...)
end
Tracker.@grad function Base.fill(value::Real, dims...)
    return fill(Tracker.data(value), dims...), function(Δ)
        size(Δ) ≢  dims && error("Dimension mismatch")
        return (sum(Δ), map(_->nothing, dims)...)
    end
end

## StatsFuns ##

logsumexp(x::Tracker.TrackedArray) = Tracker.track(logsumexp, x)
Tracker.@grad function logsumexp(x::Tracker.TrackedArray)
    lse = logsumexp(Tracker.data(x))
    return lse,
          Δ->(Δ .* exp.(x .- lse),)
end

## Linear algebra ##

LinearAlgebra.UpperTriangular(A::Tracker.TrackedMatrix) = Tracker.track(UpperTriangular, A)
Tracker.@grad function LinearAlgebra.UpperTriangular(A::AbstractMatrix)
    return UpperTriangular(Tracker.data(A)), Δ->(UpperTriangular(Δ),)
end

function LinearAlgebra.cholesky(A::Tracker.TrackedMatrix; check=true)
    factors_info = turing_chol(A, check)
    factors = factors_info[1]
    info = Tracker.data(factors_info[2])
    return Cholesky{eltype(factors), typeof(factors)}(factors, 'U', info)
end
function turing_chol(A::AbstractMatrix, check)
    chol = cholesky(A, check=check)
    (chol.factors, chol.info)
end
turing_chol(A::Tracker.TrackedMatrix, check) = Tracker.track(turing_chol, A, check)
Tracker.@grad function turing_chol(A::AbstractMatrix, check)
    C, back = Zygote.pullback(unsafe_cholesky, Tracker.data(A), Tracker.data(check))
    return (C.factors, C.info), Δ->back((factors=Tracker.data(Δ[1]),))
end

unsafe_cholesky(x, check) = cholesky(x, check=check)
Zygote.@adjoint function unsafe_cholesky(Σ::Real, check)
    C = cholesky(Σ; check=check)
    return C, function(Δ::NamedTuple)
        issuccess(C) || return (zero(Σ), nothing)
        (Δ.factors[1, 1] / (2 * C.U[1, 1]), nothing)
    end
end
Zygote.@adjoint function unsafe_cholesky(Σ::Diagonal, check)
    C = cholesky(Σ; check=check)
    return C, function(Δ::NamedTuple)
        issuccess(C) || (Diagonal(zero(diag(Δ.factors))), nothing)
        (Diagonal(diag(Δ.factors) .* inv.(2 .* C.factors.diag)), nothing)
    end
end
Zygote.@adjoint function unsafe_cholesky(Σ::Union{StridedMatrix, Symmetric{<:Real, <:StridedMatrix}}, check)
    C = cholesky(Σ; check=check)
    return C, function(Δ::NamedTuple)
        issuccess(C) || return (zero(Δ.factors), nothing)
        U, Ū = C.U, Δ.factors
        Σ̄ = Ū * U'
        Σ̄ = copytri!(Σ̄, 'U')
        Σ̄ = ldiv!(U, Σ̄)
        BLAS.trsm!('R', 'U', 'T', 'N', one(eltype(Σ)), U.data, Σ̄)
        @inbounds for n in diagind(Σ̄)
            Σ̄[n] /= 2
        end
        return (UpperTriangular(Σ̄), nothing)
    end
end

# Specialised logdet for cholesky to target the triangle directly.
logdet_chol_tri(U::AbstractMatrix) = 2 * sum(log, U[diagind(U)])
logdet_chol_tri(U::Tracker.TrackedMatrix) = Tracker.track(logdet_chol_tri, U)
Tracker.@grad function logdet_chol_tri(U::AbstractMatrix)
    U_data = Tracker.data(U)
    return logdet_chol_tri(U_data), Δ->(Matrix(Diagonal(2 .* Δ ./ diag(U_data))),)
end

function LinearAlgebra.logdet(C::Cholesky{<:Tracker.TrackedReal, <:Tracker.TrackedMatrix})
    return logdet_chol_tri(C.U)
end

# Tracker's implementation of ldiv isn't good. We'll use Zygote's instead.
const TrackedVecOrMat = Union{Tracker.TrackedVector, Tracker.TrackedMatrix}
function zygote_ldiv(A::AbstractMatrix, B::AbstractVecOrMat)
    T = typeof((zero(eltype(A))*zero(eltype(B)) + zero(eltype(A))*zero(eltype(B)))/one(eltype(A)))
    BB = similar(B, T)
    copyto!(BB, B)
    ldiv!(A, BB)
end
function zygote_ldiv(A::Tracker.TrackedMatrix, B::TrackedVecOrMat)
    return Tracker.track(zygote_ldiv, A, B)
end
function zygote_ldiv(A::Tracker.TrackedMatrix, B::AbstractVecOrMat)
    return Tracker.track(zygote_ldiv, A, B)
end
zygote_ldiv(A::AbstractMatrix, B::TrackedVecOrMat) =  Tracker.track(zygote_ldiv, A, B)
Tracker.@grad function zygote_ldiv(A, B)
    Y, back = Zygote.pullback(\, Tracker.data(A), Tracker.data(B))
    return Y, Δ->back(Tracker.data(Δ))
end

function Base.:\(a::Cholesky{<:Tracker.TrackedReal, <:Tracker.TrackedArray}, b::AbstractVecOrMat)
    return (a.U \ (a.U' \ b))
end

## PDMats ##

PDMats.invquad(Σ::PDiagMat, x::Tracker.TrackedVector) = sum(abs2.(x) ./ Σ.diag)
getchol(m::PDMats.AbstractPDMat) = m.chol
getchol(m::PDMats.PDiagMat) = cholesky(Diagonal(m.diag))
getchol(m::PDMats.ScalMat) = cholesky(Diagonal(fill(m.value, m.dim)))


text:
using RDatasets
describe(dataset("datasets", "iris"))


text:
# DeclGroupRef
function DeclGroupRef(x::Decl)
    @check_ptrs x
    return DeclGroupRef(clang_DeclGroupRef_fromeDecl(x))
end

function isNull(x::DeclGroupRef)
    @check_ptrs x
    return clang_DeclGroupRef_isNull(x)
end

function isSingleDecl(x::DeclGroupRef)
    @check_ptrs x
    return clang_DeclGroupRef_isSingleDecl(x)
end

function getSingleDecl(x::DeclGroupRef)
    @check_ptrs x
    return Decl(clang_DeclGroupRef_getSingleDecl(x))
end

function isDeclGroup(x::DeclGroupRef)
    @check_ptrs x
    return clang_DeclGroupRef_isDeclGroup(x)
end


text:
function marker_find(r,side)
    move!(r,side)
    if ismarker(r)
        return false
    else
        return true
    end
end

left(side::HorizonSide) = HorizonSide(mod(Int(side::HorizonSide)+1, 4))

function movements!(r,side)
    while !isborder(r,side)
        putmarker!(r)
        move!(r,side)
    end
    putmarker!(r)
end

function west_sud(r)
    while !isborder(r,West) || !isborder(r,Sud)
        if !isborder(r,West)
            move!(r,West)
        else
            move!(r,Sud)
        end
    end
end

left(side::HorizonSide) = HorizonSide(mod(Int(side::HorizonSide)+1, 4))

function inverse(side::HorizonSide)
    new_side=HorizonSide(mod(Int(side)+2,4))
    return new_side
end

function invert(r,side)
    if isborder(r,West)
        side = right(side)
        move!(r,side)
        side=right(side)
        return side
    end
    if isborder(r,Ost)
        side = left(side)
        move!(r,side)
        side=left(side)
        return side
    end
end

function mark_all(r)
    west_sud(r)
    side=Ost
    movements!(r,side)
    while !isborder(r,Nord)
        side=invert(r,side)
        movements!(r,side)
    end
end

max_stars_repo_name: NikitaDestroyer/Basics

max_issues_repo_name: NikitaDestroyer/Basics

max_forks_repo_name: NikitaDestroyer/Basics


text:
"""
    struct NestedNameSpecifier <: Any
Hold a `clang::NestedNameSpecifier` opaque pointer.
"""
struct NestedNameSpecifier
    ptr::CXNestedNameSpecifier
end

Base.unsafe_convert(::Type{CXNestedNameSpecifier}, x::NestedNameSpecifier) = x.ptr
Base.cconvert(::Type{CXNestedNameSpecifier}, x::NestedNameSpecifier) = x


text:
using PowerDynIC
using Test

@testset "PowerDynIC.jl" begin
    # Write your own tests here.
end

max_stars_repo_name: luap-pik/PowerDynIC

max_issues_repo_name: luap-pik/PowerDynIC

max_forks_repo_name: luap-pik/PowerDynIC


text:
using Test

@testset "./src/deterministic_strategies.jl" begin

using BellScenario

@testset "DeterministicStrategy()" begin
    D = DeterministicStrategy([1.0 0.0;0.0 1.0;0 0])

    @test D isa DeterministicStrategy
    @test D.conditionals isa Matrix{Int64}
    @test D == [1 0;0 1;0 0]
    @test D.scenario == BlackBox(3,2)

    @test_throws DomainError DeterministicStrategy([0.5 0.5;0.5 0.5])
    @test_throws DomainError DeterministicStrategy([1 0;0 1], BlackBox(3,2))
end

@testset "is_deterministic()" begin
    @test is_deterministic([1 0;0 1])
    @test !is_deterministic([0.5 0.5;0.5 0.5])
    @test is_deterministic([1.0 1.0;0.0 0.0])
end

@testset "deterministic_strategies(scenario::BlackBox)" begin
    @test deterministic_strategies(BlackBox(1,5)) == [[1 1 1 1 1]]
    @test deterministic_strategies(BlackBox(2,2)) == [
        [1 1;0 0],[0 1;1 0],[1 0;0 1],[0 0;1 1]
    ]
    @test deterministic_strategies(BlackBox(2,3)) == [
        [1 1 1;0 0 0],[0 1 1;1 0 0],[1 0 1;0 1 0],[0 0 1;1 1 0],
        [1 1 0;0 0 1],[0 1 0;1 0 1],[1 0 0;0 1 1],[0 0 0;1 1 1]
    ]

    @testset "spot checks" begin
        strategies = deterministic_strategies(BlackBox(5,7))

        @test length(strategies) == 5^7
        @test length(unique(strategies)) == 5^7
        @test all(s -> is_deterministic(s), strategies)
    end
end

end


text:
function ordered_dimensions(fexpr::FieldExpr, preferred_dim_order::Array{Dimension, 1})

    used_dims = known_dimensions_expr(fexpr)
    preferred_dims = Set(preferred_dim_order)

    used_dims_not_in_pd = setdiff(used_dims, preferred_dims)

    [
        (dim for dim in preferred_dim_order if dim in used_dims)...,
        (dim for dim in used_dims_not_in_pd)...]
end

function join_fields(fs::Array{KnownField, 1}, dim::KnownDimension)

    @assert length(fs) == length(vertices(grid(dim)))

    # assuming that all fields in `fs` have the same dimensions!
    result_dims = [known_dimensions(fs[1])..., dim]
    result_dim_sizes = [length(vertices(grid(dim))) for dim in result_dims]

    backend = array_backend(fs[1].data) # FIXME
    arrs = [evaluate_to_array(backend, f, known_dimensions(fs[1])) for f in fs]
    arrs_j = [from_backend(arr) for arr in arrs]

    result_data_j = Array{eltype(arrs[1])}(result_dim_sizes...)
    colons = repeat([:], outer=length(result_dim_sizes)-1)
    for (i, arr) in enumerate(arrs_j)
        setindex!(result_data_j, arr, colons..., i)
    end
    result_data = to_backend(backend, result_data_j)

    known_field(result_data, result_dims)
end

function join_fields(
        fields::Array{KnownField, 1},
        new_dimension::KnownDimension, new_dimension_grid, generic_field)

    # FIXME: setting uniform=True for the time being to make plotting easier
    new_dim = TransverseDimension(new_dimension.name, new_dimension_grid, uniform=True)

    # Currently we're just attaching the new dimension in front,
    # but it is possible to preserve the position it has in the original field.
    new_field = KnownField([generic_field.dimensions..., new_dim])

    for (i, field) in enumerate(fields)
        # FIXME: this will require support in the array layer
        new_field.data[i] = field.data
    end

    return new_field
end

# Currently replacing with FFTs
# can be also replaced using finite differences.

function replace_differentials(fexpr::FieldExprNode)
    FieldExprNode(fexpr.head, [replace_differentials(arg) for arg in fexpr.args])
end

function replace_differentials(fexpr::Diff)
    field = fexpr.field

    result = convert(FieldExpr, field)

    dims = collect(keys(fexpr.dimensions))
    fdims = [fourier_space(dim) for dim in dims]

    forward_pairs = Dict(dim => fdim for (dim, fdim) in zip(dims, fdims))
    inverse_pairs = Dict(fdim => dim for (dim, fdim) in zip(dims, fdims))

    result = fourier(result, forward_pairs...)
    result = convert(FieldExpr, result)

    for (dim, order) in fexpr.dimensions
        result = result * (2pi * 1im * forward_pairs[dim])^order
    end
    result = fourier(result, inverse_pairs..., inverse=true)
    result = convert(FieldExpr, result)

    return result
end

function replace_differentials(fexpr)
    fexpr
end

fftfreq(n::Integer, d::Real) = [(0:(n-1)>>1)..., (-(n>>1):-1)...] ./ (d * n)

function fourier_space(x::KnownDimension)
    # possible parameters: normalization, ordered/unordered frequencies, discrete/continuous
    # (that `integral(f(x), x)` == either `integral(f(k), k)` or `sum(f(k), k)`)

    gx = grid(x)

    @assert typeof(gx) == RegularGrid

    freqs = fftfreq(gx.points, (gx.right - gx.left) / gx.points)
    TransformedDimension(ArbitraryGrid(freqs), x)
end

function has_free_random_variables(fexpr::FieldExpr)
    inspect(false, fexpr) do controls, free_rvs, x
        if typeof(x) == Sample
            skip_children!(controls)
        elseif !free_rvs && typeof(x) == RandomVariable
            free_rvs = true
        end

        free_rvs
    end
end

function _trajectories_number(fexpr::FieldExpr)
    # Perhaps can be replaced by a special field in the nodes and
    # the propagation of the stochastic info.
    inspect(Nullable{Int64}(), fexpr) do controls, trs, x
        if typeof(x) == StochasticMean
            skip_children!(controls)
        elseif typeof(x) == KnownField && !isnull(x.stochastic_info.trajectories)
            trs = x.stochastic_info.trajectories
        elseif typeof(x) == Sample
            trs = Nullable{Int64}(x.trajectories)
        end

        trs
    end
end

trajectories_number(fexpr) = get(_trajectories_number(fexpr))
is_stochastic(fexpr) = !isnull(_trajectories_number(fexpr))


text:
using SparseArrays
using BenchmarkTools
using Test
import StatsBase: sample
##

function sum_over_cols1!(dest::Vector{Float64},M::SparseMatrixCSC)
  _colptr = SparseArrays.getcolptr(M) # column indexing
  Mnz = nonzeros(M) # direct access to weights
  @inbounds @simd for i in 1:length(_colptr)-1
    accu = 0.0
    for k in _colptr[i]:_colptr[i+1]-1
      accu += Mnz[k]
    end
    dest[i] = accu
  end
  return dest
end
function sum_over_cols1B!(dest::Vector{Float64},M::SparseMatrixCSC)
  @assert length(dest) == size(M,2)
  fill!(dest,0.0)
  _colptr = SparseArrays.getcolptr(M) # column indexing
  Mnz = nonzeros(M) # direct access to weights
  @inbounds @simd for i in 1:length(_colptr)-1
    dest[i] += sum(Mnz[_colptr[i]:_colptr[i+1]-1] )
  end
  return dest
end

function sum_over_cols2!(dest::Matrix{Float64},M::SparseMatrixCSC)
  return sum!(dest,M)
end

function sum_over_cols3!(M::SparseMatrixCSC)
  return sum(M;dims=1)
end

function sum_over_cols4!(M::Matrix{Float64})
  return sum(M;dims=1)
end
##

const n = 6000
const spars = 0.3
function domat(_n,_spars)
  return sprand(_n,_n,_spars)
end

## first, test that functions return same vals
Mtest = domat(n,spars)
test1 = let ret = fill(NaN,n)
  sum_over_cols1!(ret,Mtest)
  ret
end
test1B = let ret = fill(NaN,n)
  sum_over_cols1B!(ret,Mtest)
  ret
end
test2 = let ret=fill(NaN,(1,n))
  sum_over_cols2!(ret,Mtest)
  ret[:]
end
test3 = sum_over_cols3!(Mtest)[:]
test4 = sum_over_cols4!(Matrix(Mtest))[:]

@test all(isapprox.(test1,test2))
@test all(isapprox.(test1,test1B))
@test all(isapprox.(test1,test3))
@test all(isapprox.(test1,test4))
# to visualize :
@show extrema(test1 .- test3)

## Now, benchmark them!
@info "Benchmark for function 1"
@benchmark sum_over_cols1!(dest,M) setup=(M=domat(n,spars);dest=Vector{Float64}(undef,n))
##
@info "Benchmark for function 1B"
@benchmark sum_over_cols1B!(dest,M) setup=(M=domat(n,spars);dest=Vector{Float64}(undef,n))
##
@info "Benchmark for function 2"
@benchmark sum_over_cols2!(dest,M) setup=(M=domat(n,spars);dest=Matrix{Float64}(undef,1,n))
##
@info "Benchmark for function 3"
@benchmark sum_over_cols3!(M) setup=(M=domat(n,spars))
##
@info "Benchmark for function 4"
@benchmark sum_over_cols4!(M) setup=(M=Matrix(domat(n,spars)))

##
########################
# Now repeat for row sum

const n = 6000
const spars = 0.3
function domat(_n,_spars)
  return sprand(_n,_n,_spars)
end
Mtest = domat(n,spars)

function sum_over_rows1!(dest::Vector{Float64},M::SparseMatrixCSC)
  return sum!(dest,M)
end
function sum_over_rows2!(M::SparseMatrixCSC)
  return sum(M;dims=2)
end
function sum_over_rows3!(dest::Matrix{Float64},M::SparseMatrixCSC)
  Mtr = permutedims(M)
  return sum!(dest,Mtr)
end
function sum_over_rows4!(dest::Matrix{Float64},M::SparseMatrixCSC)
  Mtr = transpose(M)
  return sum!(dest,Mtr)
end
function sum_over_rows5!(dest::Vector{Float64},M::SparseMatrixCSC)
  fill!(dest,0.0)
  _rowidx = SparseArrays.rowvals(M)
  Mnz = nonzeros(M) # direct access to weights
  @inbounds for (i,r) in enumerate(_rowidx)
    dest[r] += Mnz[i]
  end
  return dest
end
function sum_over_rows6!(M::SparseMatrixCSC)
  dest=zeros(size(M,1))
  _rowidx = SparseArrays.rowvals(M)
  Mnz = nonzeros(M) # direct access to weights
  @inbounds for (i,r) in enumerate(_rowidx)
    dest[r] += Mnz[i]
  end
  return dest
end

##

test1 = let ret = fill(NaN,n)
  sum_over_rows1!(ret,Mtest)
  ret
end
test2 = sum_over_rows2!(Mtest)
test3 = let ret = fill(NaN,(1,n))
  sum_over_rows2!(ret,Mtest)
  ret[:]
end
test4 = let ret = fill(NaN,(1,n))
  sum_over_rows3!(ret,Mtest)
  ret[:]
end
test5 = let ret = fill(NaN,n)
  sum_over_rows4!(ret,Mtest)
  ret
end
test6 = sum_over_rows5!(Mtest)

@test all(isapprox.(test1,test2))
@test all(isapprox.(test1,test3))
@test all(isapprox.(test1,test4))
@test all(isapprox.(test1,test5))
@test all(isapprox.(test1,test6))

##
@info "Benchmark for function 1"
@benchmark sum_over_rows1!(dest,M) setup=(M=domat(n,spars);dest=Vector{Float64}(undef,n))
##
@info "Benchmark for function 2"
@benchmark sum_over_rows2!(M) setup=(M=domat(n,spars))
##
@info "Benchmark for function 3"
@benchmark sum_over_rows3!(dest,M) setup=(M=domat(n,spars);dest=Matrix{Float64}(undef,(1,n)))
##
@info "Benchmark for function 4"
@benchmark sum_over_rows4!(dest,M) setup=(M=domat(n,spars);dest=Matrix{Float64}(undef,(1,n)))
##
@info "Benchmark for function 5"
@benchmark sum_over_rows5!(dest,M) setup=(M=domat(n,spars);dest=Vector{Float64}(undef,n))
##
@info "Benchmark for function 6"
@benchmark sum_over_rows6!(M) setup=(M=domat(n,spars))

##

const n = 6000
const n_sum = 400

function mysumloop(vec::Vector{Float64},idxs)
  ret=0.0
  @inbounds for idx in idxs
    ret += vec[idx]
  end
  return ret
end

##
@benchmark sum(view(vec,idxs)) setup=(vec=randn(n);idxs=sample(1:n,n_sum))
##
@benchmark mapreduce(idx->vec[idx],+,idxs) setup=(vec=randn(n);idxs=sample(1:n,n_sum))
##
@benchmark mysumloop(vec,idxs) setup=(vec=randn(n);idxs=sample(1:n,n_sum))
##
@benchmark sum(view(vec,idxs)) setup=(vec=randn(n);idxs=(1:n_sum))
##
@benchmark mapreduce(idx->vec[idx],+,idxs) setup=(vec=randn(n);idxs=(1:n_sum))
##
@benchmark mysumloop(vec,idxs) setup=(vec=randn(n);idxs=(1:n_sum))

##
# bonus, finding element in array

neus = collect(1:600)

function is_in_all1(i::I,neus::Vector{I}) where I
  return i in neus
end
function is_in_all2(i::I,neus::Vector{I}) where I
  return !isempty(searchsorted(neus,i))
end

@benchmark is_in_all1(i,$neus) setup=(i=rand(1:600))
@benchmark is_in_all2(i,$neus) setup=(i=rand(1:600))


text:
function fourdots(r::Robot)
    putmarker!(r)
    for side in (Nord,Sud,West,Ost)
        disexit(r,side)
        moveexist1(r,side)
        putmarker!(r)
        disexit(r,HorizonSide(mod(Int(side)+2, 4)))
        moveexist1(r,HorizonSide(mod(Int(side)+2, 4)))
    end
end

function disexit(r::Robot,side::HorizonSide)
    i = 0
    while (isborder(r,side))
        move!(r,HorizonSide(mod(Int(side)+1, 4)))
        i+=1
        if ((isborder(r,side)) && (isborder(r,HorizonSide(mod(Int(side)+1, 4)))))
            break
        end
    end

    if ((isborder(r,side)) && (isborder(r,HorizonSide(mod(Int(side)+1, 4)))))
        while (i > 0)
            move!(r,HorizonSide(mod(Int(side)+3, 4)))
            i -= 1
        end
        return(1)
    else
        move!(r,side)
        while ((isborder(r,HorizonSide(mod(Int(side)+3, 4)))) && (i > 0))
            move!(r,side)
        end
        while (i > 0)
            move!(r,HorizonSide(mod(Int(side)+3, 4)))
            i -= 1

        end
        return(0)
    end
end

function moveexist(r::Robot,side::HorizonSide)
    y = 0
    while (y != 1)
        y = disexit(r,side)

    end
end
function moveexist1(r::Robot,side::HorizonSide)
    y = 0
    while (y != 1)
        if (ismarker(r))
            y = 1
        else
            y = disexit(r,side)
        end
    end
end

max_stars_repo_name: bubasooo/proggggggggg

max_issues_repo_name: bubasooo/proggggggggg

max_forks_repo_name: bubasooo/proggggggggg


text:
module CachedValuesTest

using Test
using Gridap.CachedValues

a = 10

cache = CachedValue(a)

@test cache.value == 10

cache.value = 20

@test cache.value == 20

end # module


text:
@testset "setup.jl" begin

end


text:
#Creates an array of string variables, from text file, where each doc is
#separated by 2 blank  lines. Here we have 30 queries in a single text file
#MED1.

function PrepQueriesCorpus(NoQueries::Int,QueryFile::String)
    f = open(QueryFile,"r+")
    cnt = 0
    DocCount = 1
    newDoc = true
    Queries = Array(Any,NoQueries)
    for line in eachline(f)
        if length(line) != 1
            if newDoc == true
                Queries[DocCount] = line
                newDoc = false
            elseif newDoc == false
                Queries[DocCount] = join([Queries[DocCount],line]," ")
            end
        elseif length(line) == 1
            cnt=cnt+1
            if cnt == 1
                #println("firstline")
            elseif cnt == 2
                cnt = 0
                DocCount = DocCount +1
                newDoc = true
            end
        end
    end
    m=size(Queries)
    SDArr=Array{Any,1}
    for i=1:m[1]
        SDArr=vcat(SDArr,StringDocument(Queries[i]))
    end
    return Corpus(SDArr[2:m[1]+1])
end

#This function returns a standardized corpus,to the DocType parameter.
#The documents in form of variousl text files are picked from location dirname.

function PrepDocCorpus(dirname::String,DocType::Type)
    crps = DirectoryCorpus(dirname)
    #Standardize all the documents to DocType
    standardize!(crps,DocType)
    return crps
end

#This function does the pre-processing tasks from TextAnalysis.jl

function PreProcess!(crps::Corpus)#,words::Array{String})
    remove_articles!(crps)
    remove_indefinite_articles!(crps)
    remove_definite_articles!(crps)
    remove_pronouns!(crps)
    remove_prepositions!(crps)
    remove_stop_words!(crps)
    #remove_words!(crps,words)
end

#The following functions prepares the Test data

function PrepTest(path::ASCIIString)
    temp=readdlm(path,' ';has_header=false)
    T=hcat(temp[:,1],temp[:,4])
    return T
end

function PrepTest()
    temp=readdlm("./test/data/Test.txt",' ';has_header=false)
    #temp=readdlm("./Test.txt",' ';has_header=false)
    T=hcat(temp[:,1],temp[:,4])
    return T
end


text:
# sparse linear algebra functions that perform operations between sparse and (usually tall)
# dense matrices

export mm2!, mm2

"""
    mm2!(transa::SparseChar, transb::SparseChar, alpha::BlasFloat, A::CuSparseMatrix, B::CuMatrix, beta::BlasFloat, C::CuMatrix, index::SparseChar)

Multiply the sparse matrix `A` by the dense matrix `B`, filling in dense matrix `C`.
`C = alpha*op(A)*op(B) + beta*C`. `op(A)` can be nothing (`transa = N`), transpose
(`transa = T`), or conjugate transpose (`transa = C`), and similarly for `op(B)` and
`transb`.
"""
mm2!(transa::SparseChar, transb::SparseChar, alpha::BlasFloat, A::CuSparseMatrix, B::CuMatrix, beta::BlasFloat, C::CuMatrix, index::SparseChar)
for (fname,elty) in ((:cusparseSbsrmm, :Float32),
                     (:cusparseDbsrmm, :Float64),
                     (:cusparseCbsrmm, :ComplexF32),
                     (:cusparseZbsrmm, :ComplexF64))
    @eval begin
        function mm2!(transa::SparseChar,
                      transb::SparseChar,
                      alpha::$elty,
                      A::CuSparseMatrixBSR{$elty},
                      B::CuMatrix{$elty},
                      beta::$elty,
                      C::CuMatrix{$elty},
                      index::SparseChar)
            desc = CuMatrixDescriptor(CUSPARSE_MATRIX_TYPE_GENERAL, CUSPARSE_FILL_MODE_LOWER, CUSPARSE_DIAG_TYPE_NON_UNIT, index)
            m,k = A.dims
            mb = div(m,A.blockDim)
            kb = div(k,A.blockDim)
            n = size(C)[2]
            if transa == 'N' && transb == 'N'
                chkmmdims(B,C,k,n,m,n)
            elseif transa == 'N' && transb != 'N'
                chkmmdims(B,C,n,k,m,n)
            elseif transa != 'N' && transb == 'N'
                chkmmdims(B,C,m,n,k,n)
            elseif transa != 'N' && transb != 'N'
                chkmmdims(B,C,n,m,k,n)
            end
            ldb = max(1,stride(B,2))
            ldc = max(1,stride(C,2))
            $fname(handle(), A.dir,
                   transa, transb, mb, n, kb, A.nnz,
                   [alpha], desc, A.nzVal,A.rowPtr, A.colVal,
                   A.blockDim, B, ldb, [beta], C, ldc)
            C
        end
    end
end

for (fname,elty) in ((:cusparseScsrmm2, :Float32),
                     (:cusparseDcsrmm2, :Float64),
                     (:cusparseCcsrmm2, :ComplexF32),
                     (:cusparseZcsrmm2, :ComplexF64))
    @eval begin
        function mm2!(transa::SparseChar,
                      transb::SparseChar,
                      alpha::$elty,
                      A::CuSparseMatrixCSR{$elty},
                      B::CuMatrix{$elty},
                      beta::$elty,
                      C::CuMatrix{$elty},
                      index::SparseChar)
            desc = CuMatrixDescriptor(CUSPARSE_MATRIX_TYPE_GENERAL, CUSPARSE_FILL_MODE_LOWER, CUSPARSE_DIAG_TYPE_NON_UNIT, index)
            m,k = A.dims
            n = size(C)[2]
            if transa == 'N' && transb == 'N'
                chkmmdims(B,C,k,n,m,n)
            elseif transa == 'N' && transb != 'N'
                chkmmdims(B,C,n,k,m,n)
            elseif transa != 'N' && transb == 'N'
                chkmmdims(B,C,m,n,k,n)
            elseif transa != 'N' && transb != 'N'
                chkmmdims(B,C,n,m,k,n)
            end
            ldb = max(1,stride(B,2))
            ldc = max(1,stride(C,2))
            $fname(handle(),
                   transa, transb, m, n, k, A.nnz, [alpha], desc,
                   A.nzVal, A.rowPtr, A.colVal, B, ldb, [beta], C, ldc)
            C
        end
        function mm2!(transa::SparseChar,
                      transb::SparseChar,
                      alpha::$elty,
                      A::CuSparseMatrixCSC{$elty},
                      B::CuMatrix{$elty},
                      beta::$elty,
                      C::CuMatrix{$elty},
                      index::SparseChar)
            ctransa = 'N'
            if transa == 'N'
                ctransa = 'T'
            end
            desc = CuMatrixDescriptor(CUSPARSE_MATRIX_TYPE_GENERAL, CUSPARSE_FILL_MODE_LOWER, CUSPARSE_DIAG_TYPE_NON_UNIT, index)
            k,m = A.dims
            n = size(C)[2]
            if ctransa == 'N' && transb == 'N'
                chkmmdims(B,C,k,n,m,n)
            elseif ctransa == 'N' && transb != 'N'
                chkmmdims(B,C,n,k,m,n)
            elseif ctransa != 'N' && transb == 'N'
                chkmmdims(B,C,m,n,k,n)
            elseif ctransa != 'N' && transb != 'N'
                chkmmdims(B,C,n,m,k,n)
            end
            ldb = max(1,stride(B,2))
            ldc = max(1,stride(C,2))
            $fname(handle(),
                   ctransa, transb, m, n, k, A.nnz, [alpha], desc,
                   A.nzVal, A.colPtr, A.rowVal, B, ldb, [beta], C, ldc)
            C
        end
    end
end

for elty in (:Float32,:Float64,:ComplexF32,:ComplexF64)
    @eval begin
        function mm2(transa::SparseChar,
                     transb::SparseChar,
                     alpha::$elty,
                     A::Union{CuSparseMatrixCSR{$elty},CuSparseMatrixCSC{$elty},CuSparseMatrixBSR{$elty}},
                     B::CuMatrix{$elty},
                     beta::$elty,
                     C::CuMatrix{$elty},
                     index::SparseChar)
            mm2!(transa,transb,alpha,A,B,beta,copy(C),index)
        end
        function mm2(transa::SparseChar,
                     transb::SparseChar,
                     A::Union{CuSparseMatrixCSR{$elty},CuSparseMatrixCSC{$elty},CuSparseMatrixBSR{$elty}},
                     B::CuMatrix{$elty},
                     beta::$elty,
                     C::CuMatrix{$elty},
                     index::SparseChar)
            mm2(transa,transb,one($elty),A,B,beta,C,index)
        end
        function mm2(transa::SparseChar,
                     transb::SparseChar,
                     A::Union{CuSparseMatrixCSR{$elty},CuSparseMatrixCSC{$elty},CuSparseMatrixBSR{$elty}},
                     B::CuMatrix{$elty},
                     C::CuMatrix{$elty},
                     index::SparseChar)
            mm2(transa,transb,one($elty),A,B,one($elty),C,index)
        end
        function mm2(transa::SparseChar,
                     transb::SparseChar,
                     alpha::$elty,
                     A::Union{CuSparseMatrixCSR{$elty},CuSparseMatrixCSC{$elty},CuSparseMatrixBSR{$elty}},
                     B::CuMatrix{$elty},
                     index::SparseChar)
            m = transa == 'N' ? size(A)[1] : size(A)[2]
            n = transb == 'N' ? size(B)[2] : size(B)[1]
            mm2(transa,transb,alpha,A,B,zero($elty),CUDA.zeros($elty,(m,n)),index)
        end
        function mm2(transa::SparseChar,
                     transb::SparseChar,
                     A::Union{CuSparseMatrixCSR{$elty},CuSparseMatrixCSC{$elty},CuSparseMatrixBSR{$elty}},
                     B::CuMatrix{$elty},
                     index::SparseChar)
            m = transa == 'N' ? size(A)[1] : size(A)[2]
            n = transb == 'N' ? size(B)[2] : size(B)[1]
            mm2(transa,transb,one($elty),A,B,zero($elty),CUDA.zeros($elty,(m,n)),index)
        end
    end
end

# bsrsm2
for (bname,aname,sname,elty) in ((:cusparseSbsrsm2_bufferSize, :cusparseSbsrsm2_analysis, :cusparseSbsrsm2_solve, :Float32),
                                 (:cusparseDbsrsm2_bufferSize, :cusparseDbsrsm2_analysis, :cusparseDbsrsm2_solve, :Float64),
                                 (:cusparseCbsrsm2_bufferSize, :cusparseCbsrsm2_analysis, :cusparseCbsrsm2_solve, :ComplexF32),
                                 (:cusparseZbsrsm2_bufferSize, :cusparseZbsrsm2_analysis, :cusparseZbsrsm2_solve, :ComplexF64))
    @eval begin
        function bsrsm2!(transa::SparseChar,
                         transxy::SparseChar,
                         alpha::$elty,
                         A::CuSparseMatrixBSR{$elty},
                         X::CuMatrix{$elty},
                         index::SparseChar)
            desc = CuMatrixDescriptor(CUSPARSE_MATRIX_TYPE_GENERAL, CUSPARSE_FILL_MODE_UPPER, CUSPARSE_DIAG_TYPE_NON_UNIT, index)
            m,n = A.dims
            if m != n
                throw(DimensionMismatch("A must be square!"))
            end
            mb = div(m,A.blockDim)
            mX,nX = size(X)
            if transxy == 'N' && (mX != m)
                throw(DimensionMismatch(""))
            end
            if transxy != 'N' && (nX != m)
                throw(DimensionMismatch(""))
            end
            ldx = max(1,stride(X,2))
            info = bsrsm2Info_t[0]
            cusparseCreateBsrsm2Info(info)
            @workspace size=@argout(
                    $bname(handle(), A.dir, transa, transxy,
                           mb, nX, A.nnz, desc, A.nzVal, A.rowPtr,
                           A.colVal, A.blockDim, info[],
                           out(Ref{Cint}(1)))
                )[] buffer->begin
                    $aname(handle(), A.dir, transa, transxy,
                           mb, nX, A.nnz, desc, A.nzVal, A.rowPtr,
                           A.colVal, A.blockDim, info[],
                           CUSPARSE_SOLVE_POLICY_USE_LEVEL, buffer)
                    posit = Ref{Cint}(1)
                    cusparseXbsrsm2_zeroPivot(handle(), info[], posit)
                    if posit[] >= 0
                        error("Structural/numerical zero in A at ($(posit[]),$(posit[])))")
                    end
                    $sname(handle(), A.dir, transa, transxy, mb,
                           nX, A.nnz, [alpha], desc, A.nzVal, A.rowPtr,
                           A.colVal, A.blockDim, info[], X, ldx, X, ldx,
                           CUSPARSE_SOLVE_POLICY_USE_LEVEL, buffer)
                end
            cusparseDestroyBsrsm2Info(info[1])
            X
        end
        function bsrsm2(transa::SparseChar,
                        transxy::SparseChar,
                        alpha::$elty,
                        A::CuSparseMatrixBSR{$elty},
                        X::CuMatrix{$elty},
                        index::SparseChar)
            bsrsm2!(transa,transxy,alpha,A,copy(X),index)
        end
    end
end

# csrsm2
for (bname,aname,sname,elty) in ((:cusparseScsrsm2_bufferSizeExt, :cusparseScsrsm2_analysis, :cusparseScsrsm2_solve, :Float32),
                                 (:cusparseDcsrsm2_bufferSizeExt, :cusparseDcsrsm2_analysis, :cusparseDcsrsm2_solve, :Float64),
                                 (:cusparseCcsrsm2_bufferSizeExt, :cusparseCcsrsm2_analysis, :cusparseCcsrsm2_solve, :ComplexF32),
                                 (:cusparseZcsrsm2_bufferSizeExt, :cusparseZcsrsm2_analysis, :cusparseZcsrsm2_solve, :ComplexF64))
    @eval begin
        function csrsm2!(transa::SparseChar,
                         transxy::SparseChar,
                         alpha::$elty,
                         A::CuSparseMatrixCSR{$elty},
                         X::CuMatrix{$elty},
                         index::SparseChar)
            desc = CuMatrixDescriptor(CUSPARSE_MATRIX_TYPE_GENERAL, CUSPARSE_FILL_MODE_UPPER, CUSPARSE_DIAG_TYPE_NON_UNIT, index)
            m,n = A.dims
            if m != n
                throw(DimensionMismatch("A must be square!"))
            end
            mX,nX = size(X)
            if transxy == 'N' && (mX != m)
                throw(DimensionMismatch(""))
            end
            if transxy != 'N' && (nX != m)
                throw(DimensionMismatch(""))
            end
            ldx = max(1,stride(X,2))
            info = csrsm2Info_t[0]
            cusparseCreateCsrsm2Info(info)
            # use non block algo (0) for now...
            @workspace size=@argout(
                    $bname(handle(), 0, transa, transxy,
                           m, nX, A.nnz, [alpha], desc, A.nzVal, A.rowPtr,
                           A.colVal, X, ldx, info[], CUSPARSE_SOLVE_POLICY_USE_LEVEL,
                           out(Ref{UInt64}(1)))
                )[] buffer->begin
                    $aname(handle(), 0, transa, transxy,
                           m, nX, A.nnz, [alpha], desc, A.nzVal, A.rowPtr,
                           A.colVal, X, ldx, info[],
                           CUSPARSE_SOLVE_POLICY_USE_LEVEL, buffer)
                    posit = Ref{Cint}(1)
                    cusparseXcsrsm2_zeroPivot(handle(), info[1], posit)
                    if posit[] >= 0
                        error("Structural/numerical zero in A at ($(posit[]),$(posit[])))")
                    end
                    $sname(handle(), 0, transa, transxy, m,
                           nX, A.nnz, [alpha], desc, A.nzVal, A.rowPtr,
                           A.colVal, X, ldx, info[],
                           CUSPARSE_SOLVE_POLICY_USE_LEVEL, buffer)
                end
            cusparseDestroyCsrsm2Info(info[])
            X
        end
        function csrsm2(transa::SparseChar,
                        transxy::SparseChar,
                        alpha::$elty,
                        A::CuSparseMatrixCSR{$elty},
                        X::CuMatrix{$elty},
                        index::SparseChar)
            csrsm2!(transa,transxy,alpha,A,copy(X),index)
        end
    end
end


text:
create(c::Client, ::Type{Overwrite}, ch::DiscordChannel, r::Role; kwargs...) = edit_channel_permissions(c, ch.id, r.id; kwargs..., type = OT_ROLE)
create(c::Client, ::Type{Overwrite}, ch::DiscordChannel, u::User; kwargs...) = edit_channel_permissions(c, ch.id, u.id; kwargs..., type = OT_MEMBER)

update(c::Client, o::Overwrite, ch::DiscordChannel; kwargs...) = edit_channel_permissions(c, ch.id, o.id; kwargs..., type = o.type)

delete(c::Client, o::Overwrite, ch::DiscordChannel) = delete_channel_permission(c, ch.id, o.id)


text:
module MeetBot

using Discord
using Discord: Snowflake
using Dates: now, DateTime, TimePeriod, Second, Minute, Hour
using Base.Threads: SpinLock
using UUIDs: uuid4

const PREFIX = "!"

const QUEUE = Channel(100)
const MEET_GROUP = Dict()
const MEET_GROUP_LOCK = SpinLock()

const MEET_CHANNELS = Set()
const MEET_CHANNELS_LOCK = SpinLock()

mutable struct MeetRequest
    guild_id::Snowflake
    user::User
    queue_time::DateTime
    notify1::Bool
    notify2::Bool
end

struct MeetChannel
    channel_id::Snowflake
    created_time::DateTime
    name::String
end

Base.@kwdef struct Config
    meet_channel_lifetime::TimePeriod
    time_to_notify1::TimePeriod
    time_to_notify2::TimePeriod
    time_to_delete_request::TimePeriod
end

function get_config()
    if get(ENV, "MEETBOT_ENV", "DEV") == "DEV"
        return Config(
            meet_channel_lifetime = Minute(10),
            time_to_notify1 = Second(5),
            time_to_notify2 = Second(15),
            time_to_delete_request = Minute(1),
        )
    else
        return Config(
            meet_channel_lifetime = Hour(2),
            time_to_notify1 = Minute(5),
            time_to_notify2 = Minute(30),
            time_to_delete_request = Hour(2),
        )
    end
end

"Shut down bot. This can be used by specific people only."
function shutdown_command(c::Client, m::Message)
    if m.author.id in (0x079fc5a25082000b, 0x04989e135042000b)
        @info "Shutting down, please wait..." now()
        close(c)
    else
        @warn "Unauthorized shutdown command" m.author now()
    end
end

"Create a new meet request after user confirms they want to meetup."
function confirmation_command(c::Client, m::Message)
    @info "Meetup confirmation command"
    request = MeetRequest(m.guild_id, m.author, now(), false, false)
    put!(QUEUE, request)
    uid = m.author.id
    reply(c, m,
        "Hey, <@$(uid)>, you have been added to the meetup queue. " *
        "If you want to leave the queue, just type **!quit**.")
end

"""
Introductory command that shows the other commands
"""
function meet_command(c::Client, m::Message)
    @info "Meetbot's introductory command"
    uid = m.author.id
    text = "Hey <@$(uid)>, thank you for using MeetBot! " *
        "To begin meeting new people, type **!confirm** to join " *
        "the meetup queue."
    reply(c, m, text)
end

function quit_command(c::Client, m::Message)
    @info "Remove a person waiting in queue if they choose to leave"
    uid = m.author.id
    text = "Hey <@$(uid)>, sad to see you leave, but you have been " *
        "removed from the queue."
    if haskey(MEET_GROUP, m.author.id)
    cancel_request(MEET_GROUP[m.author.id])
    end
    # We will just send a reply anyways even if the user
    # was not in the queue previously. Not going to argue
    # with the user about whether it was in the queue before
    # or not :-)
    reply(c, m, text)
end

function process_meet_requests(c::Client)
    @info "Starting process_meet_requests"
    while true
        request = take!(QUEUE)

        @info "process_meet_requests: got request" request
        register_request(request)
        if length(MEET_GROUP) == 4  # should be 4
            channel = create_voice_channel(c, request.guild_id)
            notify_meet_group(channel, c)
            empty_meet_group()
        end
    end
end

"Return the current requests pending in the meet group."
current_requests() = collect(values(MEET_GROUP))

"""
Find Meetup category.
"""
function find_meetup_category(c::Client,  guild_id::Snowflake)
    channels = fetchval(Discord.get_guild_channels(c, guild_id))
    idx = findfirst(x -> x.type == CT_GUILD_CATEGORY && x.name == "Meetup", channels)
    return idx !== nothing ? channels[idx].id : nothing
end

"""
Ensure that a Meetup category channel is created.
Return the category's channel id.
"""
function ensure_meetup_category(c::Client,  guild_id::Snowflake)
    category_id = find_meetup_category(c, guild_id)
    if category_id === nothing
        guild = fetchval(get_guild(c, guild_id))
        category_channel = fetchval(create(c, DiscordChannel, guild;
            name="Meetup", type=CT_GUILD_CATEGORY))
        category_id = category_channel.id
    end
    return category_id
end

"Create a new voice channel"
function create_voice_channel(c::Client, guild_id::Snowflake)
    @info "Alright, let's create a new voice channel."
    lock(MEET_CHANNELS_LOCK) do
        category_id = ensure_meetup_category(c, guild_id)

        # get user ids from MEET_`GROUP
        uids = keys(MEET_GROUP)
        @info "User id's in MEET_GROUP" uids

        # Create an overwrite object for each user in the meet group
        overwrite_arr = []
        for id in uids
            new_overwrite = Overwrite(id, OT_MEMBER, Int(PERM_VIEW_CHANNEL), 0)
            push!(overwrite_arr, new_overwrite)
        end

        #Create overwrite object for everyone to prevent anyone else from seeing it
        everyone_id = find_id(c, guild_id, "@everyone")
        @info "@everyone id" everyone_id
        everyone_overwrite = Overwrite(everyone_id, OT_ROLE, 0, Int(PERM_VIEW_CHANNEL))
        push!(overwrite_arr, everyone_overwrite)

        # Create the voice channel/push to MEET_CHANNELS
        room_name = "Meetup Room " * string(uuid4())[1:6]
        guild = fetchval(get_guild(c, guild_id))

        vc = fetchval(create(c, DiscordChannel, guild;
            name = room_name,
            type = CT_GUILD_VOICE,
            parent_id = category_id,
            permission_overwrites = overwrite_arr))
        @info "Created voice channel" vc

        mc = MeetChannel(vc.id, now(), room_name)
        push!(MEET_CHANNELS, mc)

        return mc
    end
end

# helper function that finds the id of the given role
function find_id(c::Client, guild_id, role)
    role_lst = fetchval(get_guild_roles(c, guild_id))
    @info role_lst
    for r in role_lst
        if r.name == role
            return r.id
        end
    end
    throw(MissingException("role not found"))
end

"""
Send DM to participants of the current meet group and ask them
to join the voice channel.
"""
function notify_meet_group(mc::MeetChannel, c::Client)
    for request in current_requests()
        content = "Hey $(request.user.username), thanks for waiting!
        Please join <#" * string(mc.channel_id) * ">"
        dm = fetchval(create_dm(c; recipient_id = request.user.id))
        create_message(c, dm.id; content=content)
    end
end

"Register a request for meet-up"
function register_request(request)
    user_id = request.user.id
    lock(MEET_GROUP_LOCK) do
        if haskey(MEET_GROUP, user_id)
            @warn "User is already registered $(user_id)"
        else
            MEET_GROUP[user_id] = request
        end
    end
end

"Cancel an already registered request"
function cancel_request(request)
    @info "Cancelling request" request now()
    lock(MEET_GROUP_LOCK) do
        pop!(MEET_GROUP, request.user.id)
    end
    return nothing
end

"""
Empty the current meet group. This should be called after the current
meet group has been processed (i.e. a voice chat has been prepared).
"""
function empty_meet_group()
    @info "Emptying meet group"
    lock(MEET_GROUP_LOCK) do
        empty!(MEET_GROUP)
    end
end

"Send DM to user and tell them to be patient"
function notify_participant(c::Client, request, notify_count)
    if notify_count == 1
        message = "I know, you've been waiting for a while. Please be patient."
    elseif notify_count == 2
        message = "Unfortunately, we are still waiting for someone to meet up. " *
            "We will let you know once it's available."
    else # final notification
        message = "Sorry to keep you waiting, but there is not enough people to " *
            "meet up at this time. You have been removed from the queue for now. " *
            "Please try again later."
    end
    @info "Sending DM to $(request.user.username)" message now()
    dm = fetchval(create_dm(c; recipient_id = request.user.id))
    create_message(c, dm.id; content = message)
end

"""
Check meet group. For each participant:
1. Send a DM when the person has waited over 2 mins (be patient)
2. Send a DM when the person has waited over 15 mins (be patient)
3. Send a DM & cancel request when the person has waited over an hour
"""
function check_meet_group(c::Client)
    cfg = get_config()
    @info "Starting check_meet_group" cfg
    try
        while true
            requests = current_requests()
            if length(requests) > 0
                current_time = now()
                foreach(requests) do request
                    elapsed = current_time - request.queue_time
                    if elapsed > cfg.time_to_delete_request
                        cancel_request(request)
                        notify_participant(c, request, 3)
                    elseif elapsed > cfg.time_to_notify2
                        !request.notify2 && notify_participant(c, request, 2)
                        request.notify2 = true
                    elseif elapsed > cfg.time_to_notify1
                        !request.notify1 && notify_participant(c, request, 1)
                        request.notify1 = true
                    end
                end
            end
            sleep(1)
        end
    catch ex
        @info "check_meet_group stopped due to $(ex)" now()
    end
end

"Retrieve the Discord bot token from environment."
function get_discord_token()
    env_var = "DISCORD_MEETBOT_TOKEN"
    if haskey(ENV, env_var) && length(ENV[env_var]) > 50
        return ENV[env_var]
    else
        error("You must set up `$env_var` ennvironment variable.")
    end
end

"Garbage collect meet channels when they are too old and nobody inside"
function gc_meet_channels(c::Client)
    cfg = get_config()
    try
        while true
            current_time = now()
            @info "gc_meet_channels" current_time
            lock(MEET_CHANNELS_LOCK) do
                old_meet_channels = filter(MEET_CHANNELS) do mc
                    current_time - mc.created_time > cfg.meet_channel_lifetime
                end
                foreach(old_meet_channels) do mc
                    @info "Deleting channel" mc
                    delete_channel(c, mc.channel_id)
                    delete!(MEET_CHANNELS, mc)
                end
            end
            sleep(60)
        end
    catch ex
        @info "gc_meet_channels stopped due to $(ex)" now()
    end
end

# For development only, do not use otherwise.
function get_client()
    token = get_discord_token()
    c = Client(token;
        prefix = PREFIX,
        presence=(game=(name="MeetBot", type=AT_GAME),))
    open(c)
    @info "Try fetchval(Discord.get_current_user_guilds(c))"
    return c
end

function run()
    token = get_discord_token()
    c = Client(token; prefix = PREFIX, presence=(game=(name="MeetBot", type=AT_GAME),))

    add_command!(c, :shutdown, shutdown_command; help="shutdown bot")
    add_command!(c, :confirm, confirmation_command; help="enter person into meetup queue")
    add_command!(c, :meet, meet_command; help="give information about how meetbot works")
    add_command!(c, :quit, quit_command; help="removes a user from queue if they choose to leave")

    open(c)
    @info "Connected to client" c

    # background tasks
    @async process_meet_requests(c)
    meet_group_checker_task = @async check_meet_group(c)
    gc_meet_channels_task = @async gc_meet_channels(c)

    wait(c)
    @info "Disconnected Discord client"

    schedule(meet_group_checker_task, InterruptException(); error = true)
    schedule(gc_meet_channels_task, InterruptException(); error = true)

    return nothing
end

end