# We have rotations along one, two or three axes (e.g. RotX, RotXY and RotXYZ).
# They compose together nicely, so the user can make Euler angles by:
#
#     RotX(θx) * RotY(θy) * RotZ(θz) -> RotXYZ(θx, θy, θz)
#
# and never get confused by the order of application, etc.

#########################
# Single axis rotations #
#########################
"""
    immutable RotX{T} <: Rotation{T}
    RotX(theta)

A 3×3 rotation matrix which represents a rotation by `theta` about the X axis.
"""
immutable RotX{T} <: Rotation{T}
    theta::T
end

@inline convert{R<:RotX}(::Type{R}, r::RotX) = R(r.theta)

# These 2 functions are enough to satisfy the entire StaticArrays interface:
@inline (::Type{R}){R<:RotX}(t::NTuple{9}) = error("Cannot construct a cardinal axis rotation from a matrix")
@inline function Base.getindex{T}(r::RotX{T}, i::Integer)
    if i == 1
        one(T)
    elseif i < 5
        zero(T)
    elseif i == 5
        cos(r.theta)
    elseif i == 6
        sin(r.theta)
    elseif i == 7
        zero(T)
    elseif i == 8
        -sin(r.theta)
    elseif i == 9
        cos(r.theta)
    else
        throw(BoundsError(r,i))
    end
end

@inline function Base.convert{T}(::Type{Tuple}, r::RotX{T})
    (one(T),  zero(T),      zero(T),   # transposed representation
     zero(T), cos(r.theta), sin(r.theta),
     zero(T), -sin(r.theta), cos(r.theta))
end

@inline function Base.:*(r::RotX, v::StaticVector)
    if length(v) != 3
        throw("Dimension mismatch: cannot rotate a vector of length $(length(v))")
    end

    ct, st = cos(r.theta), sin(r.theta)
    T = promote_type(eltype(r), eltype(v))
    return similar_type(v,T)(v[1],
                             v[2] * ct - v[3] * st,
                             v[3] * ct + v[2] * st)
end

@inline Base.:*(r1::RotX, r2::RotX) = RotX(r1.theta + r2.theta)

@inline inv(r::RotX) = RotX(-r.theta)

# define null rotations for convenience
@inline eye(::Type{RotX}) = RotX(0.0)
@inline eye{T}(::Type{RotX{T}}) = RotX{T}(zero(T))

"""
    immutable RotY{T} <: Rotation{T}
    RotY(theta)

A 3×3 rotation matrix which represents a rotation by `theta` about the Y axis.
"""
immutable RotY{T} <: Rotation{T}
    theta::T
end

@inline convert{R<:RotY}(::Type{R}, r::RotY) = R(r.theta)

@inline (::Type{R}){R<:RotY}(t::NTuple{9}) = error("Cannot construct a cardinal axis rotation from a matrix")
@inline function Base.getindex{T}(r::RotY{T}, i::Integer)
    if i == 1
        cos(r.theta)
    elseif i == 2
        zero(T)
    elseif i == 3
        -sin(r.theta)
    elseif i == 4
        zero(T)
    elseif i == 5
        one(T)
    elseif i == 6
        zero(T)
    elseif i == 7
        sin(r.theta)
    elseif i == 8
        zero(T)
    elseif i == 9
        cos(r.theta)
    else
        throw(BoundsError(r,i))
    end
end

@inline function Base.convert{T}(::Type{Tuple}, r::RotY{T})
    (cos(r.theta), zero(T), -sin(r.theta),   # transposed representation
     zero(T),      one(T),   zero(T),
     sin(r.theta), zero(T),  cos(r.theta))
end

@inline function Base.:*(r::RotY, v::StaticVector)
    if length(v) != 3
        throw("Dimension mismatch: cannot rotate a vector of length $(length(v))")
    end

    ct, st = cos(r.theta), sin(r.theta)
    T = promote_type(eltype(r), eltype(v))
    return similar_type(v,T)(v[1] * ct + v[3] * st,
                             v[2],
                             v[3] * ct - v[1] * st)
end

@inline Base.:*(r1::RotY, r2::RotY) = RotY(r1.theta + r2.theta)

@inline inv(r::RotY) = RotY(-r.theta)

# define null rotations for convenience
@inline eye(::Type{RotY}) = RotY(0.0)
@inline eye{T}(::Type{RotY{T}}) = RotY{T}(zero(T))

"""
    immutable RotZ{T} <: Rotation{T}
    RotZ(theta)

A 3×3 rotation matrix which represents a rotation by `theta` about the Z axis.
"""
immutable RotZ{T} <: Rotation{T}
    theta::T
end

@inline convert{R<:RotZ}(::Type{R}, r::RotZ) = R(r.theta)

@inline (::Type{R}){R<:RotZ}(t::NTuple{9}) = error("Cannot construct a cardinal axis rotation from a matrix")
@inline function Base.getindex{T}(r::RotZ{T}, i::Integer)
    if i == 1
        cos(r.theta)
    elseif i == 2
        sin(r.theta)
    elseif i == 3
        zero(T)
    elseif i == 4
        -sin(r.theta)
    elseif i == 5
        cos(r.theta)
    elseif i < 9
        zero(T)
    elseif i == 9
        one(T)
    else
        throw(BoundsError(r,i))
    end
end

@inline function Base.convert{T}(::Type{Tuple}, r::RotZ{T})
    ( cos(r.theta), sin(r.theta), zero(T),   # transposed representation
     -sin(r.theta), cos(r.theta), zero(T),
      zero(T),      zero(T),      one(T))
end

@inline function Base.:*(r::RotZ, v::StaticVector)
    if length(v) != 3
        throw("Dimension mismatch: cannot rotate a vector of length $(length(v))")
    end

    ct, st = cos(r.theta), sin(r.theta)
    T = promote_type(eltype(r), eltype(v))
    return similar_type(v,T)(v[1] * ct - v[2] * st,
                             v[2] * ct + v[1] * st,
                             v[3])
end

@inline Base.:*(r1::RotZ, r2::RotZ) = RotZ(r1.theta + r2.theta)

# define null rotations for convenience
@inline eye(::Type{RotZ}) = RotZ(0.0)
@inline eye{T}(::Type{RotZ{T}}) = RotZ{T}(zero(T))

@inline inv(r::RotZ) = RotZ(-r.theta)

function Base.rand{R <: Union{RotX,RotY,RotZ}}(::Type{R})
    T = eltype(R)
    if T == Any
        T = Float64
    end

    return R(2*pi*rand(T))
end

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

######################
# Two axis rotations #
######################

"""
    immutable RotXY{T} <: Rotation{T}
    RotXY(theta_x, theta_y)

A 3×3 rotation matrix which represents a rotation by `theta_y` about the Y axis,
followed by a rotation by `theta_x` about the X axis.
"""
immutable RotXY{T} <: Rotation{T}
    theta1::T
    theta2::T
end

@inline (::Type{R}){R<:RotXY}(r::RotXY) = R(r.theta1, r.theta2)
@inline convert{R<:RotXY}(::Type{R}, r::RotXY) = R(r.theta1, r.theta2)

# StaticArrays will take over *all* the constructors and put everything in a tuple...
# but this isn't quite what we mean when we have 2 inputs (not 9).
@inline (::Type{RotXY}){X,Y}(x::X, y::Y) = RotXY{promote_type(X, Y)}(x, y)

# These 2 functions are enough to satisfy the entire StaticArrays interface:
@inline (::Type{R}){R<:RotXY}(t::NTuple{9}) = error("Cannot construct a two-axis rotation from a matrix")
@inline function Base.getindex{T}(r::RotXY{T}, i::Integer)
    Tuple(r)[i] # Slow...
end

@inline function Base.convert{T}(::Type{Tuple}, r::RotXY{T})
    sinθ₁ = sin(r.theta1)
    cosθ₁ = cos(r.theta1)
    sinθ₂ = sin(r.theta2)
    cosθ₂ = cos(r.theta2)

    # transposed representation
    (cosθ₂,  sinθ₁*sinθ₂,  cosθ₁*-sinθ₂,
     zero(T),      cosθ₁,          sinθ₁,
     sinθ₂,  -sinθ₁*cosθ₂,   cosθ₁*cosθ₂)
end

@inline function Base.:*(r::RotXY, v::StaticVector)
    if length(v) != 3
        throw("Dimension mismatch: cannot rotate a vector of length $(length(v))")
    end

    T = promote_type(eltype(r), eltype(v))

    sinθ₁ = sin(r.theta1)
    cosθ₁ = cos(r.theta1)
    sinθ₂ = sin(r.theta2)
    cosθ₂ = cos(r.theta2)

    return similar_type(v,T)(cosθ₂*v[1] + sinθ₂*v[3],
                             sinθ₁*sinθ₂*v[1] + cosθ₁*v[2] + -sinθ₁*cosθ₂*v[3],
                             cosθ₁*-sinθ₂*v[1] + sinθ₁*v[2] + cosθ₁*cosθ₂*v[3])
end

@inline Base.:*(r1::RotX, r2::RotY) = RotXY(r1.theta, r2.theta)
@inline Base.:*(r1::RotXY, r2::RotY) = RotXY(r1.theta1, r1.theta2 + r2.theta)
@inline Base.:*(r1::RotX, r2::RotXY) = RotXY(r1.theta + r2.theta1, r2.theta2)

@inline inv(r::RotXY) = RotYX(-r.theta2, -r.theta1)

# define null rotations for convenience
@inline eye(::Type{RotXY}) = RotXY(0.0, 0.0)
@inline eye{T}(::Type{RotXY{T}}) = RotXY{T}(zero(T), zero(T))

"""
    immutable RotYX{T} <: Rotation{T}
    RotYX(theta_y, theta_x)

A 3×3 rotation matrix which represents a rotation by `theta_x` about the X axis,
followed by a rotation by `theta_y` about the Y axis.
"""
immutable RotYX{T} <: Rotation{T}
    theta1::T
    theta2::T
end

@inline (::Type{R}){R<:RotYX}(r::RotYX) = R(r.theta1, r.theta2)
@inline convert{R<:RotYX}(::Type{R}, r::RotYX) = R(r.theta1, r.theta2)

# StaticArrays will take over *all* the constructors and put everything in a tuple...
# but this isn't quite what we mean when we have 2 inputs (not 9).
@inline (::Type{RotYX}){Y,X}(y::Y, x::X) = RotYX{promote_type(Y, X)}(y, x)

# These 2 functions are enough to satisfy the entire StaticArrays interface:
@inline (::Type{R}){R<:RotYX}(t::NTuple{9}) = error("Cannot construct a two-axis rotation from a matrix")
@inline function Base.getindex{T}(r::RotYX{T}, i::Integer)
    Tuple(r)[i] # Slow...
end

@inline function Base.convert{T}(::Type{Tuple}, r::RotYX{T})
    sinθ₁ = sin(r.theta1)
    cosθ₁ = cos(r.theta1)
    sinθ₂ = sin(r.theta2)
    cosθ₂ = cos(r.theta2)

    # transposed representation
    (cosθ₁,        zero(T),       -sinθ₁,
     sinθ₁*sinθ₂,  cosθ₂,   cosθ₁*sinθ₂,
     sinθ₁*cosθ₂,  -sinθ₂,  cosθ₁*cosθ₂)
end

@inline function Base.:*(r::RotYX, v::StaticVector)
    if length(v) != 3
        throw("Dimension mismatch: cannot rotate a vector of length $(length(v))")
    end

    T = promote_type(eltype(r), eltype(v))

    sinθ₁ = sin(r.theta1)
    cosθ₁ = cos(r.theta1)
    sinθ₂ = sin(r.theta2)
    cosθ₂ = cos(r.theta2)

    return similar_type(v,T)(cosθ₁*v[1] + sinθ₁*sinθ₂*v[2] + sinθ₁*cosθ₂*v[3],
                             cosθ₂*v[2] + -sinθ₂*v[3],
                             -sinθ₁*v[1] + cosθ₁*sinθ₂*v[2] + cosθ₁*cosθ₂*v[3])
end

@inline Base.:*(r1::RotY, r2::RotX) = RotYX(r1.theta, r2.theta)
@inline Base.:*(r1::RotYX, r2::RotX) = RotYX(r1.theta1, r1.theta2 + r2.theta)
@inline Base.:*(r1::RotY, r2::RotYX) = RotYX(r1.theta + r2.theta1, r2.theta2)

@inline inv(r::RotYX) = RotXY(-r.theta2, -r.theta1)

# define null rotations for convenience
@inline eye(::Type{RotYX}) = RotYX(0.0, 0.0)
@inline eye{T}(::Type{RotYX{T}}) = RotYX{T}(zero(T), zero(T))

"""
    immutable RotXZ{T} <: Rotation{T}
    RotXZ(theta_x, theta_z)

A 3×3 rotation matrix which represents a rotation by `theta_z` about the Z axis,
followed by a rotation by `theta_x` about the X axis.
"""
immutable RotXZ{T} <: Rotation{T}
    theta1::T
    theta2::T
end

@inline (::Type{R}){R<:RotXZ}(r::RotXZ) = R(r.theta1, r.theta2)
@inline convert{R<:RotXZ}(::Type{R}, r::RotXZ) = R(r.theta1, r.theta2)

# StaticArrays will take over *all* the constructors and put everything in a tuple...
# but this isn't quite what we mean when we have 2 inputs (not 9).
@inline (::Type{RotXZ}){X,Z}(x::X, z::Z) = RotXZ{promote_type(X, Z)}(x, z)

# These 2 functions are enough to satisfy the entire StaticArrays interface:
@inline (::Type{R}){R<:RotXZ}(t::NTuple{9}) = error("Cannot construct a two-axis rotation from a matrix")
@inline function Base.getindex{T}(r::RotXZ{T}, i::Integer)
    Tuple(r)[i] # Slow...
end

@inline function Base.convert{T}(::Type{Tuple}, r::RotXZ{T})
    sinθ₁ = sin(r.theta1)
    cosθ₁ = cos(r.theta1)
    sinθ₂ = sin(r.theta2)
    cosθ₂ = cos(r.theta2)

    # transposed representation
    (cosθ₂,   cosθ₁*sinθ₂,  sinθ₁*sinθ₂,
     -sinθ₂,  cosθ₁*cosθ₂,  sinθ₁*cosθ₂,
     zero(T),       -sinθ₁,       cosθ₁)
end

@inline function Base.:*(r::RotXZ, v::StaticVector)
    if length(v) != 3
        throw("Dimension mismatch: cannot rotate a vector of length $(length(v))")
    end

    T = promote_type(eltype(r), eltype(v))

    sinθ₁ = sin(r.theta1)
    cosθ₁ = cos(r.theta1)
    sinθ₂ = sin(r.theta2)
    cosθ₂ = cos(r.theta2)

    return similar_type(v,T)(cosθ₂*v[1] + -sinθ₂*v[2],
                             cosθ₁*sinθ₂*v[1] + cosθ₁*cosθ₂*v[2] + -sinθ₁*v[3],
                             sinθ₁*sinθ₂*v[1] + sinθ₁*cosθ₂*v[2] + cosθ₁*v[3])
end

@inline Base.:*(r1::RotX, r2::RotZ) = RotXZ(r1.theta, r2.theta)
@inline Base.:*(r1::RotXZ, r2::RotZ) = RotXZ(r1.theta1, r1.theta2 + r2.theta)
@inline Base.:*(r1::RotX, r2::RotXZ) = RotXZ(r1.theta + r2.theta1, r2.theta2)

@inline inv(r::RotXZ) = RotZX(-r.theta2, -r.theta1)

# define null rotations for convenience
@inline eye(::Type{RotXZ}) = RotXZ(0.0, 0.0)
@inline eye{T}(::Type{RotXZ{T}}) = RotXZ{T}(zero(T), zero(T))

"""
    immutable RotZX{T} <: Rotation{T}
    RotZX(theta_z, theta_x)

A 3×3 rotation matrix which represents a rotation by `theta_x` about the X axis,
followed by a rotation by `theta_z` about the Z axis.
"""
immutable RotZX{T} <: Rotation{T}
    theta1::T
    theta2::T
end

@inline (::Type{R}){R<:RotZX}(r::RotZX) = R(r.theta1, r.theta2)
@inline convert{R<:RotZX}(::Type{R}, r::RotZX) = R(r.theta1, r.theta2)

# StaticArrays will take over *all* the constructors and put everything in a tuple...
# but this isn't quite what we mean when we have 2 inputs (not 9).
@inline (::Type{RotZX}){Z,X}(z::Z, x::X) = RotZX{promote_type(Z, X)}(z, x)

# These 2 functions are enough to satisfy the entire StaticArrays interface:
@inline (::Type{R}){R<:RotZX}(t::NTuple{9}) = error("Cannot construct a two-axis rotation from a matrix")
@inline function Base.getindex{T}(r::RotZX{T}, i::Integer)
    Tuple(r)[i] # Slow...
end

@inline function Base.convert{T}(::Type{Tuple}, r::RotZX{T})
    sinθ₁ = sin(r.theta1)
    cosθ₁ = cos(r.theta1)
    sinθ₂ = sin(r.theta2)
    cosθ₂ = cos(r.theta2)

    # transposed representation
    ( cosθ₁,         sinθ₁,         zero(T),
     -sinθ₁*cosθ₂,   cosθ₁*cosθ₂,   sinθ₂,
      sinθ₁*sinθ₂,   cosθ₁*-sinθ₂,  cosθ₂)
end

@inline function Base.:*(r::RotZX, v::StaticVector)
    if length(v) != 3
        throw("Dimension mismatch: cannot rotate a vector of length $(length(v))")
    end

    T = promote_type(eltype(r), eltype(v))

    sinθ₁ = sin(r.theta1)
    cosθ₁ = cos(r.theta1)
    sinθ₂ = sin(r.theta2)
    cosθ₂ = cos(r.theta2)

    return similar_type(v,T)(cosθ₁*v[1] + -sinθ₁*cosθ₂*v[2] + sinθ₁*sinθ₂*v[3],
                             sinθ₁*v[1] + cosθ₁*cosθ₂*v[2] + cosθ₁*-sinθ₂*v[3],
                             sinθ₂*v[2] + cosθ₂*v[3])
end

@inline Base.:*(r1::RotZ, r2::RotX) = RotZX(r1.theta, r2.theta)
@inline Base.:*(r1::RotZX, r2::RotX) = RotZX(r1.theta1, r1.theta2 + r2.theta)
@inline Base.:*(r1::RotZ, r2::RotZX) = RotZX(r1.theta + r2.theta1, r2.theta2)

@inline inv(r::RotZX) = RotXZ(-r.theta2, -r.theta1)

# define null rotations for convenience
@inline eye(::Type{RotZX}) = RotZX(0.0, 0.0)
@inline eye{T}(::Type{RotZX{T}}) = RotZX{T}(zero(T), zero(T))

"""
    immutable RotZY{T} <: Rotation{T}
    RotZY(theta_z, theta_y)

A 3×3 rotation matrix which represents a rotation by `theta_y` about the Y axis,
followed by a rotation by `theta_z` about the Z axis.
"""
immutable RotZY{T} <: Rotation{T}
    theta1::T
    theta2::T
end

@inline (::Type{R}){R<:RotZY}(r::RotZY) = R(r.theta1, r.theta2)
@inline convert{R<:RotZY}(::Type{R}, r::RotZY) = R(r.theta1, r.theta2)

# StaticArrays will take over *all* the constructors and put everything in a tuple...
# but this isn't quite what we mean when we have 2 inputs (not 9).
@inline (::Type{RotZY}){Z,Y}(z::Z, y::Y) = RotZY{promote_type(Z, Y)}(z, y)

# These 2 functions are enough to satisfy the entire StaticArrays interface:
@inline (::Type{R}){R<:RotZY}(t::NTuple{9}) = error("Cannot construct a two-axis rotation from a matrix")
@inline function Base.getindex{T}(r::RotZY{T}, i::Integer)
    Tuple(r)[i] # Slow...
end

@inline function Base.convert{T}(::Type{Tuple}, r::RotZY{T})
    sinθ₁ = sin(r.theta1)
    cosθ₁ = cos(r.theta1)
    sinθ₂ = sin(r.theta2)
    cosθ₂ = cos(r.theta2)

    # transposed representation
    ( cosθ₁*cosθ₂,  sinθ₁*cosθ₂, -sinθ₂,
     -sinθ₁,        cosθ₁,        zero(T),
      cosθ₁*sinθ₂,  sinθ₁*sinθ₂,  cosθ₂)
end

@inline function Base.:*(r::RotZY, v::StaticVector)
    if length(v) != 3
        throw("Dimension mismatch: cannot rotate a vector of length $(length(v))")
    end

    T = promote_type(eltype(r), eltype(v))

    sinθ₁ = sin(r.theta1)
    cosθ₁ = cos(r.theta1)
    sinθ₂ = sin(r.theta2)
    cosθ₂ = cos(r.theta2)

    return similar_type(v,T)(cosθ₁*cosθ₂*v[1] + -sinθ₁*v[2] + cosθ₁*sinθ₂*v[3],
                             sinθ₁*cosθ₂*v[1] + cosθ₁*v[2] + sinθ₁*sinθ₂*v[3],
                             -sinθ₂*v[1] + cosθ₂*v[3])
end

@inline Base.:*(r1::RotZ, r2::RotY) = RotZY(r1.theta, r2.theta)
@inline Base.:*(r1::RotZY, r2::RotY) = RotZY(r1.theta1, r1.theta2 + r2.theta)
@inline Base.:*(r1::RotZ, r2::RotZY) = RotZY(r1.theta + r2.theta1, r2.theta2)

@inline inv(r::RotZY) = RotYZ(-r.theta2, -r.theta1)

# define null rotations for convenience
@inline eye(::Type{RotZY}) = RotZY(0.0, 0.0)
@inline eye{T}(::Type{RotZY{T}}) = RotZY{T}(zero(T), zero(T))

"""
    immutable RotYZ{T} <: Rotation{T}
    RotYZ(theta_y, theta_z)

A 3×3 rotation matrix which represents a rotation by `theta_z` about the Z axis,
followed by a rotation by `theta_y` about the Y axis.
"""
immutable RotYZ{T} <: Rotation{T}
    theta1::T
    theta2::T
end

@inline (::Type{R}){R<:RotYZ}(r::RotYZ) = R(r.theta1, r.theta2)
@inline convert{R<:RotYZ}(::Type{R}, r::RotYZ) = R(r.theta1, r.theta2)

# StaticArrays will take over *all* the constructors and put everything in a tuple...
# but this isn't quite what we mean when we have 2 inputs (not 9).
@inline (::Type{RotYZ}){Y,Z}(y::Y, z::Z) = RotYZ{promote_type(Y, Z)}(y, z)

# These 2 functions are enough to satisfy the entire StaticArrays interface:
@inline (::Type{R}){R<:RotYZ}(t::NTuple{9}) = error("Cannot construct a two-axis rotation from a matrix")
@inline function Base.getindex{T}(r::RotYZ{T}, i::Integer)
    Tuple(r)[i] # Slow...
end

@inline function Base.convert{T}(::Type{Tuple}, r::RotYZ{T})
    sinθ₁ = sin(r.theta1)
    cosθ₁ = cos(r.theta1)
    sinθ₂ = sin(r.theta2)
    cosθ₂ = cos(r.theta2)

    # transposed representation
    (cosθ₁*cosθ₂,   sinθ₂,    -sinθ₁*cosθ₂,
     cosθ₁*-sinθ₂,  cosθ₂,     sinθ₁*sinθ₂,
     sinθ₁,         zero(T),      cosθ₁)
end

@inline function Base.:*(r::RotYZ, v::StaticVector)
    if length(v) != 3
        throw("Dimension mismatch: cannot rotate a vector of length $(length(v))")
    end

    T = promote_type(eltype(r), eltype(v))

    sinθ₁ = sin(r.theta1)
    cosθ₁ = cos(r.theta1)
    sinθ₂ = sin(r.theta2)
    cosθ₂ = cos(r.theta2)

    return similar_type(v,T)(cosθ₁*cosθ₂*v[1] + cosθ₁*-sinθ₂*v[2] + sinθ₁*v[3],
                             sinθ₂*v[1] + cosθ₂*v[2],
                             -sinθ₁*cosθ₂*v[1] + sinθ₁*sinθ₂*v[2] + cosθ₁*v[3])
end

@inline Base.:*(r1::RotY, r2::RotZ) = RotYZ(r1.theta, r2.theta)
@inline Base.:*(r1::RotYZ, r2::RotZ) = RotYZ(r1.theta1, r1.theta2 + r2.theta)
@inline Base.:*(r1::RotY, r2::RotYZ) = RotYZ(r1.theta + r2.theta1, r2.theta2)

# define null rotations for convenience
@inline eye(::Type{RotYZ}) = RotYZ(0.0, 0.0)
@inline eye{T}(::Type{RotYZ{T}}) = RotYZ{T}(zero(T), zero(T))

@inline inv(r::RotYZ) = RotZY(-r.theta2, -r.theta1)

function Base.rand{R <: Union{RotXY,RotYZ,RotZX, RotXZ, RotYX, RotZY}}(::Type{R})
    T = eltype(R)
    if T == Any
        T = Float64
    end

    # Not really sure what this distribution is, but it's also not clear what
    # it should be! rand(RotXY) *is* invariant to pre-rotations by a RotX and
    # post-rotations by a RotY...
    return R(2*pi*rand(T), 2*pi*rand(T))
end

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

##########################
# Proper Euler Rotations #
##########################

"""
    immutable RotXYX{T} <: Rotation{T}
    RotXYX(theta1, theta2, theta3)

A 3×3 rotation matrix parameterized by the "proper" XYX Euler angle convention,
consisting of first a rotation about the X axis by `theta3`, followed by a
rotation about the Y axis by `theta2`, and finally a rotation about the X axis
by `theta1`.
"""
immutable RotXYX{T} <: Rotation{T}
    theta1::T
    theta2::T
    theta3::T
end

# StaticArrays will take over *all* the constructors and put everything in a tuple...
# but this isn't quite what we mean when we have 3 inputs (not 9).
@inline (::Type{RotXYX}){X,Y,Z}(x::X, y::Y, z::Z) = RotXYX{promote_type(promote_type(X, Y), Z)}(x, y, z)

# These 2 functions are enough to satisfy the entire StaticArrays interface:
@inline function (::Type{Rot}){Rot <: RotXYX}(t::NTuple{9})
    R = SMatrix{3,3}(t)

    t1 = atan2(R[2, 1], (-R[3, 1] + eps(t[1])) - eps(t[1]))  # TODO: handle denormal numbers better, as atan2(0,0) != atan2(0,-0)
    ct1, st1 = cos(t1), sin(t1)

    Rot(t1,
        atan2((R[1, 2] * R[1, 2] + R[1, 3] * R[1, 3])^(1/2), R[1, 1]),
        atan2(- R[2, 3]*ct1 - R[3, 3]*st1, R[2, 2]*ct1 + R[3, 2]*st1))
end
@inline function Base.getindex{T}(r::RotXYX{T}, i::Integer)
    Tuple(r)[i] # Slow...
end

@inline function Base.convert{T}(::Type{Tuple}, r::RotXYX{T})
    sinθ₁ = sin(r.theta1)
    cosθ₁ = cos(r.theta1)
    sinθ₂ = sin(r.theta2)
    cosθ₂ = cos(r.theta2)
    sinθ₃ = sin(r.theta3)
    cosθ₃ = cos(r.theta3)

    # transposed representation
    (cosθ₂,        sinθ₁*sinθ₂,                        cosθ₁*-sinθ₂,
     sinθ₂*sinθ₃,  cosθ₁*cosθ₃ + -sinθ₁*cosθ₂*sinθ₃,   sinθ₁*cosθ₃ + cosθ₁*cosθ₂*sinθ₃,
     sinθ₂*cosθ₃,  cosθ₁*-sinθ₃ + -sinθ₁*cosθ₂*cosθ₃,  sinθ₁*-sinθ₃ + cosθ₁*cosθ₂*cosθ₃)
end

@inline function Base.:*(r::RotXYX, v::StaticVector)
    if length(v) != 3
        throw("Dimension mismatch: cannot rotate a vector of length $(length(v))")
    end

    T = promote_type(eltype(r), eltype(v))

    sinθ₁ = sin(r.theta1)
    cosθ₁ = cos(r.theta1)
    sinθ₂ = sin(r.theta2)
    cosθ₂ = cos(r.theta2)
    sinθ₃ = sin(r.theta3)
    cosθ₃ = cos(r.theta3)

    return similar_type(v,T)(
        cosθ₂*v[1] + sinθ₂*sinθ₃*v[2] + sinθ₂*cosθ₃*v[3],
        sinθ₁*sinθ₂*v[1] + (cosθ₁*cosθ₃ + -sinθ₁*cosθ₂*sinθ₃)*v[2] + (cosθ₁*-sinθ₃ + -sinθ₁*cosθ₂*cosθ₃)*v[3],
        cosθ₁*-sinθ₂*v[1] + (sinθ₁*cosθ₃ + cosθ₁*cosθ₂*sinθ₃)*v[2] + (sinθ₁*-sinθ₃ + cosθ₁*cosθ₂*cosθ₃)*v[3])
end

@inline Base.:*(r1::RotX, r2::RotYX) = RotXYX(r1.theta, r2.theta1, r2.theta2)
@inline Base.:*(r1::RotXY, r2::RotX) = RotXYX(r1.theta1, r1.theta2, r2.theta)
@inline Base.:*(r1::RotXYX, r2::RotX) = RotXYX(r1.theta1, r1.theta2, r1.theta3 + r2.theta)
@inline Base.:*(r1::RotX, r2::RotXYX) = RotXYX(r1.theta + r2.theta1, r2.theta2, r2.theta3)

@inline inv(r::RotXYX) = RotXYX(-r.theta3, -r.theta2, -r.theta1)

# define null rotations for convenience
@inline eye(::Type{RotXYX}) = RotXYX(0.0, 0.0, 0.0)
@inline eye{T}(::Type{RotXYX{T}}) = RotXYX{T}(zero(T), zero(T), zero(T))

"""
    immutable RotXZX{T} <: Rotation{T}
    RotXZX(theta1, theta2, theta3)

A 3×3 rotation matrix parameterized by the "proper" XZX Euler angle convention,
consisting of first a rotation about the X axis by `theta3`, followed by a
rotation about the Z axis by `theta2`, and finally a rotation about the X axis
by `theta1`.
"""
immutable RotXZX{T} <: Rotation{T}
    theta1::T
    theta2::T
    theta3::T
end

# StaticArrays will take over *all* the constructors and put everything in a tuple...
# but this isn't quite what we mean when we have 3 inputs (not 9).
@inline (::Type{RotXZX}){X,Y,Z}(x::X, y::Y, z::Z) = RotXZX{promote_type(promote_type(X, Y), Z)}(x, y, z)

# These 2 functions are enough to satisfy the entire StaticArrays interface:
@inline function (::Type{Rot}){Rot <: RotXZX}(t::NTuple{9})
    R = SMatrix{3,3}(t)

    t1 = atan2(R[3, 1], R[2, 1])
    ct1, st1 = cos(t1), sin(t1)

    Rot(t1,
        atan2((R[1, 2] * R[1, 2] + R[1, 3] * R[1, 3])^(1/2), R[1, 1]),
        atan2(R[3, 2]*ct1 - R[2, 2]*st1, R[3, 3]*ct1 - R[2, 3]*st1))
end
@inline function Base.getindex{T}(r::RotXZX{T}, i::Integer)
    Tuple(r)[i] # Slow...
end

@inline function Base.convert{T}(::Type{Tuple}, r::RotXZX{T})
    sinθ₁ = sin(r.theta1)
    cosθ₁ = cos(r.theta1)
    sinθ₂ = sin(r.theta2)
    cosθ₂ = cos(r.theta2)
    sinθ₃ = sin(r.theta3)
    cosθ₃ = cos(r.theta3)

    # transposed representation
    (cosθ₂,         cosθ₁*sinθ₂,                        sinθ₁*sinθ₂,
     -sinθ₂*cosθ₃,  cosθ₁*cosθ₂*cosθ₃ + -sinθ₁*sinθ₃,   sinθ₁*cosθ₂*cosθ₃ + cosθ₁*sinθ₃,
     sinθ₂*sinθ₃,   cosθ₁*cosθ₂*-sinθ₃ + -sinθ₁*cosθ₃,  sinθ₁*cosθ₂*-sinθ₃ + cosθ₁*cosθ₃)
end

@inline function Base.:*(r::RotXZX, v::StaticVector)
    if length(v) != 3
        throw("Dimension mismatch: cannot rotate a vector of length $(length(v))")
    end

    T = promote_type(eltype(r), eltype(v))

    sinθ₁ = sin(r.theta1)
    cosθ₁ = cos(r.theta1)
    sinθ₂ = sin(r.theta2)
    cosθ₂ = cos(r.theta2)
    sinθ₃ = sin(r.theta3)
    cosθ₃ = cos(r.theta3)

    return similar_type(v,T)(
        cosθ₂*v[1] + -sinθ₂*cosθ₃*v[2] + sinθ₂*sinθ₃*v[3],
        cosθ₁*sinθ₂*v[1] + (cosθ₁*cosθ₂*cosθ₃ + -sinθ₁*sinθ₃)*v[2] + (cosθ₁*cosθ₂*-sinθ₃ + -sinθ₁*cosθ₃)*v[3],
        sinθ₁*sinθ₂*v[1] + (sinθ₁*cosθ₂*cosθ₃ + cosθ₁*sinθ₃)*v[2] + (sinθ₁*cosθ₂*-sinθ₃ + cosθ₁*cosθ₃)*v[3])
end

@inline Base.:*(r1::RotX, r2::RotZX) = RotXZX(r1.theta, r2.theta1, r2.theta2)
@inline Base.:*(r1::RotXZ, r2::RotX) = RotXZX(r1.theta1, r1.theta2, r2.theta)
@inline Base.:*(r1::RotXZX, r2::RotX) = RotXZX(r1.theta1, r1.theta2, r1.theta3 + r2.theta)
@inline Base.:*(r1::RotX, r2::RotXZX) = RotXZX(r1.theta + r2.theta1, r2.theta2, r2.theta3)

@inline inv(r::RotXZX) = RotXZX(-r.theta3, -r.theta2, -r.theta1)

# define null rotations for convenience
@inline eye(::Type{RotXZX}) = RotXZX(0.0, 0.0, 0.0)
@inline eye{T}(::Type{RotXZX{T}}) = RotXZX{T}(zero(T), zero(T), zero(T))

"""
    immutable RotYXY{T} <: Rotation{T}
    RotYXY(theta1, theta2, theta3)

A 3×3 rotation matrix parameterized by the "proper" YXY Euler angle convention,
consisting of first a rotation about the Y axis by `theta3`, followed by a
rotation about the X axis by `theta2`, and finally a rotation about the Y axis
by `theta1`.
"""
immutable RotYXY{T} <: Rotation{T}
    theta1::T
    theta2::T
    theta3::T
end

# StaticArrays will take over *all* the constructors and put everything in a tuple...
# but this isn't quite what we mean when we have 3 inputs (not 9).
@inline (::Type{RotYXY}){X,Y,Z}(x::X, y::Y, z::Z) = RotYXY{promote_type(promote_type(X, Y), Z)}(x, y, z)

# These 2 functions are enough to satisfy the entire StaticArrays interface:
@inline function (::Type{Rot}){Rot <: RotYXY}(t::NTuple{9})
    R = SMatrix{3,3}(t)

    t1 = atan2(R[1, 2], R[3, 2])
    ct1, st1 = cos(t1), sin(t1)

    Rot(t1,
        atan2((R[2, 1] * R[2, 1] + R[2, 3] * R[2, 3])^(1/2), R[2, 2]),
        atan2(R[1, 3]*ct1 - R[3, 3]*st1, R[1, 1]*ct1 - R[3, 1]*st1))
end
@inline function Base.getindex{T}(r::RotYXY{T}, i::Integer)
    Tuple(r)[i] # Slow...
end

@inline function Base.convert{T}(::Type{Tuple}, r::RotYXY{T})
    sinθ₁ = sin(r.theta1)
    cosθ₁ = cos(r.theta1)
    sinθ₂ = sin(r.theta2)
    cosθ₂ = cos(r.theta2)
    sinθ₃ = sin(r.theta3)
    cosθ₃ = cos(r.theta3)

    # transposed representation
    (cosθ₁*cosθ₃ + sinθ₁*cosθ₂*-sinθ₃,  sinθ₂*sinθ₃,   -sinθ₁*cosθ₃ + cosθ₁*cosθ₂*-sinθ₃,
     sinθ₁*sinθ₂,                       cosθ₂,         cosθ₁*sinθ₂,
     cosθ₁*sinθ₃ + sinθ₁*cosθ₂*cosθ₃,   -sinθ₂*cosθ₃,  -sinθ₁*sinθ₃ + cosθ₁*cosθ₂*cosθ₃)
end

@inline function Base.:*(r::RotYXY, v::StaticVector)
    if length(v) != 3
        throw("Dimension mismatch: cannot rotate a vector of length $(length(v))")
    end

    T = promote_type(eltype(r), eltype(v))

    sinθ₁ = sin(r.theta1)
    cosθ₁ = cos(r.theta1)
    sinθ₂ = sin(r.theta2)
    cosθ₂ = cos(r.theta2)
    sinθ₃ = sin(r.theta3)
    cosθ₃ = cos(r.theta3)

    return similar_type(v,T)(
        (cosθ₁*cosθ₃ + sinθ₁*cosθ₂*-sinθ₃)*v[1] + sinθ₁*sinθ₂*v[2] + (cosθ₁*sinθ₃ + sinθ₁*cosθ₂*cosθ₃)*v[3],
        sinθ₂*sinθ₃*v[1] + cosθ₂*v[2] + -sinθ₂*cosθ₃*v[3],
        (-sinθ₁*cosθ₃ + cosθ₁*cosθ₂*-sinθ₃)*v[1] + cosθ₁*sinθ₂*v[2] + (-sinθ₁*sinθ₃ + cosθ₁*cosθ₂*cosθ₃)*v[3])
end

@inline Base.:*(r1::RotY, r2::RotXY) = RotYXY(r1.theta, r2.theta1, r2.theta2)
@inline Base.:*(r1::RotYX, r2::RotY) = RotYXY(r1.theta1, r1.theta2, r2.theta)
@inline Base.:*(r1::RotYXY, r2::RotY) = RotYXY(r1.theta1, r1.theta2, r1.theta3 + r2.theta)
@inline Base.:*(r1::RotY, r2::RotYXY) = RotYXY(r1.theta + r2.theta1, r2.theta2, r2.theta3)

@inline inv(r::RotYXY) = RotYXY(-r.theta3, -r.theta2, -r.theta1)

# define null rotations for convenience
@inline eye(::Type{RotYXY}) = RotYXY(0.0, 0.0, 0.0)
@inline eye{T}(::Type{RotYXY{T}}) = RotYXY{T}(zero(T), zero(T), zero(T))

"""
    immutable RotYZY{T} <: Rotation{T}
    RotYZY(theta1, theta2, theta3)

A 3×3 rotation matrix parameterized by the "proper" YXY Euler angle convention,
consisting of first a rotation about the Y axis by `theta3`, followed by a
rotation about the Z axis by `theta2`, and finally a rotation about the Y axis
by `theta1`.
"""
immutable RotYZY{T} <: Rotation{T}
    theta1::T
    theta2::T
    theta3::T
end

# StaticArrays will take over *all* the constructors and put everything in a tuple...
# but this isn't quite what we mean when we have 3 inputs (not 9).
@inline (::Type{RotYZY}){X,Y,Z}(x::X, y::Y, z::Z) = RotYZY{promote_type(promote_type(X, Y), Z)}(x, y, z)

# These 2 functions are enough to satisfy the entire StaticArrays interface:
@inline function (::Type{Rot}){Rot <: RotYZY}(t::NTuple{9})
    R = SMatrix{3,3}(t)

    t1 = atan2(R[3, 2], -R[1, 2])  # TODO: handle denormal numbers better, as atan2(0,0) != atan2(0,-0)
    ct1, st1 = cos(t1), sin(t1)

    Rot(t1,
        atan2((R[2, 1] * R[2, 1] + R[2, 3] * R[2, 3])^(1/2), R[2, 2]),
        atan2(- R[3, 1]*ct1 - R[1, 1]*st1, R[3, 3]*ct1 + R[1, 3]*st1))
end
@inline function Base.getindex{T}(r::RotYZY{T}, i::Integer)
    Tuple(r)[i] # Slow...
end

@inline function Base.convert{T}(::Type{Tuple}, r::RotYZY{T})
    sinθ₁ = sin(r.theta1)
    cosθ₁ = cos(r.theta1)
    sinθ₂ = sin(r.theta2)
    cosθ₂ = cos(r.theta2)
    sinθ₃ = sin(r.theta3)
    cosθ₃ = cos(r.theta3)

    # transposed representation
    (cosθ₁*cosθ₂*cosθ₃ + sinθ₁*-sinθ₃,  sinθ₂*cosθ₃,  -sinθ₁*cosθ₂*cosθ₃ + cosθ₁*-sinθ₃,
     cosθ₁*-sinθ₂,                      cosθ₂,        sinθ₁*sinθ₂,
     cosθ₁*cosθ₂*sinθ₃ + sinθ₁*cosθ₃,   sinθ₂*sinθ₃,  -sinθ₁*cosθ₂*sinθ₃ + cosθ₁*cosθ₃)
end

@inline function Base.:*(r::RotYZY, v::StaticVector)
    if length(v) != 3
        throw("Dimension mismatch: cannot rotate a vector of length $(length(v))")
    end

    T = promote_type(eltype(r), eltype(v))

    sinθ₁ = sin(r.theta1)
    cosθ₁ = cos(r.theta1)
    sinθ₂ = sin(r.theta2)
    cosθ₂ = cos(r.theta2)
    sinθ₃ = sin(r.theta3)
    cosθ₃ = cos(r.theta3)

    return similar_type(v,T)(
        (cosθ₁*cosθ₂*cosθ₃ + sinθ₁*-sinθ₃)*v[1] + cosθ₁*-sinθ₂*v[2] + (cosθ₁*cosθ₂*sinθ₃ + sinθ₁*cosθ₃)*v[3],
        sinθ₂*cosθ₃*v[1] + cosθ₂*v[2] + sinθ₂*sinθ₃*v[3],
        (-sinθ₁*cosθ₂*cosθ₃ + cosθ₁*-sinθ₃)*v[1] + sinθ₁*sinθ₂*v[2] + (-sinθ₁*cosθ₂*sinθ₃ + cosθ₁*cosθ₃)*v[3])
end

@inline Base.:*(r1::RotY, r2::RotZY) = RotYZY(r1.theta, r2.theta1, r2.theta2)
@inline Base.:*(r1::RotYZ, r2::RotY) = RotYZY(r1.theta1, r1.theta2, r2.theta)
@inline Base.:*(r1::RotYZY, r2::RotY) = RotYZY(r1.theta1, r1.theta2, r1.theta3 + r2.theta)
@inline Base.:*(r1::RotY, r2::RotYZY) = RotYZY(r1.theta + r2.theta1, r2.theta2, r2.theta3)

@inline inv(r::RotYZY) = RotYZY(-r.theta3, -r.theta2, -r.theta1)

# define null rotations for convenience
@inline eye(::Type{RotYZY}) = RotYZY(0.0, 0.0, 0.0)
@inline eye{T}(::Type{RotYZY{T}}) = RotYZY{T}(zero(T), zero(T), zero(T))

"""
    immutable RotZXZ{T} <: Rotation{T}
    RotZXZ(theta1, theta2, theta3)

A 3×3 rotation matrix parameterized by the "proper" ZXZ Euler angle convention,
consisting of first a rotation about the Z axis by `theta3`, followed by a
rotation about the X axis by `theta2`, and finally a rotation about the Z axis
by `theta1`.
"""
immutable RotZXZ{T} <: Rotation{T}
    theta1::T
    theta2::T
    theta3::T
end

# StaticArrays will take over *all* the constructors and put everything in a tuple...
# but this isn't quite what we mean when we have 3 inputs (not 9).
@inline (::Type{RotZXZ}){X,Y,Z}(x::X, y::Y, z::Z) = RotZXZ{promote_type(promote_type(X, Y), Z)}(x, y, z)

# These 2 functions are enough to satisfy the entire StaticArrays interface:
@inline function (::Type{Rot}){Rot <: RotZXZ}(t::NTuple{9})
    R = SMatrix{3,3}(t)

    t1 = atan2(R[1, 3], (-R[2, 3] + eps()) - eps())  # TODO: handle denormal numbers better, as atan2(0,0) != atan2(0,-0)
    ct1, st1 = cos(t1), sin(t1)

    Rot(t1,
        atan2((R[3, 1] * R[3, 1] + R[3, 2] * R[3, 2])^(1/2), R[3, 3]),
        atan2(- R[1, 2]*ct1 - R[2, 2]*st1, R[1, 1]*ct1 + R[2, 1]*st1))
end

@inline function Base.getindex{T}(r::RotZXZ{T}, i::Integer)
    Tuple(r)[i] # Slow...
end

@inline function Base.convert{T}(::Type{Tuple}, r::RotZXZ{T})
    sinθ₁ = sin(r.theta1)
    cosθ₁ = cos(r.theta1)
    sinθ₂ = sin(r.theta2)
    cosθ₂ = cos(r.theta2)
    sinθ₃ = sin(r.theta3)
    cosθ₃ = cos(r.theta3)

    # transposed representation
    (cosθ₁*cosθ₃ + -sinθ₁*cosθ₂*sinθ₃,   sinθ₁*cosθ₃ + cosθ₁*cosθ₂*sinθ₃,   sinθ₂*sinθ₃,
     cosθ₁*-sinθ₃ + -sinθ₁*cosθ₂*cosθ₃,  sinθ₁*-sinθ₃ + cosθ₁*cosθ₂*cosθ₃,  sinθ₂*cosθ₃,
     sinθ₁*sinθ₂,                        cosθ₁*-sinθ₂,                      cosθ₂)
end

@inline function Base.:*(r::RotZXZ, v::StaticVector)
    if length(v) != 3
        throw("Dimension mismatch: cannot rotate a vector of length $(length(v))")
    end

    T = promote_type(eltype(r), eltype(v))

    sinθ₁ = sin(r.theta1)
    cosθ₁ = cos(r.theta1)
    sinθ₂ = sin(r.theta2)
    cosθ₂ = cos(r.theta2)
    sinθ₃ = sin(r.theta3)
    cosθ₃ = cos(r.theta3)

    return similar_type(v,T)(
         (cosθ₁*cosθ₃ + -sinθ₁*cosθ₂*sinθ₃)*v[1] + (cosθ₁*-sinθ₃ + -sinθ₁*cosθ₂*cosθ₃)*v[2] + sinθ₁*sinθ₂*v[3],
         (sinθ₁*cosθ₃ + cosθ₁*cosθ₂*sinθ₃)*v[1] + (sinθ₁*-sinθ₃ + cosθ₁*cosθ₂*cosθ₃)*v[2] + cosθ₁*-sinθ₂*v[3],
         sinθ₂*sinθ₃*v[1] + sinθ₂*cosθ₃*v[2] + cosθ₂*v[3])
end

@inline Base.:*(r1::RotZ, r2::RotXZ) = RotZXZ(r1.theta, r2.theta1, r2.theta2)
@inline Base.:*(r1::RotZX, r2::RotZ) = RotZXZ(r1.theta1, r1.theta2, r2.theta)
@inline Base.:*(r1::RotZXZ, r2::RotZ) = RotZXZ(r1.theta1, r1.theta2, r1.theta3 + r2.theta)
@inline Base.:*(r1::RotZ, r2::RotZXZ) = RotZXZ(r1.theta + r2.theta1, r2.theta2, r2.theta3)

@inline inv(r::RotZXZ) = RotZXZ(-r.theta3, -r.theta2, -r.theta1)

# define null rotations for convenience
@inline eye(::Type{RotZXZ}) = RotZXZ(0.0, 0.0, 0.0)
@inline eye{T}(::Type{RotZXZ{T}}) = RotZXZ{T}(zero(T), zero(T), zero(T))

"""
    immutable RotZYZ{T} <: Rotation{T}
    RotZYZ(theta1, theta2, theta3)

A 3×3 rotation matrix parameterized by the "proper" ZXZ Euler angle convention,
consisting of first a rotation about the Z axis by `theta3`, followed by a
rotation about the Y axis by `theta2`, and finally a rotation about the Z axis
by `theta1`.
"""
immutable RotZYZ{T} <: Rotation{T}
    theta1::T
    theta2::T
    theta3::T
end

# StaticArrays will take over *all* the constructors and put everything in a tuple...
# but this isn't quite what we mean when we have 3 inputs (not 9).
@inline (::Type{RotZYZ}){X,Y,Z}(x::X, y::Y, z::Z) = RotZYZ{promote_type(promote_type(X, Y), Z)}(x, y, z)

# These 2 functions are enough to satisfy the entire StaticArrays interface:
@inline function (::Type{Rot}){Rot <: RotZYZ}(t::NTuple{9})
    R = SMatrix{3,3}(t)

    t1 = atan2(R[2, 3], R[1, 3])
    ct1, st1 = cos(t1), sin(t1)

    Rot(t1,
        atan2((R[3, 1] * R[3, 1] + R[3, 2] * R[3, 2])^(1/2), R[3, 3]),
        atan2(R[2, 1]*ct1 - R[1, 1]*st1, R[2, 2]*ct1 - R[1, 2]*st1))
end

@inline function Base.getindex{T}(r::RotZYZ{T}, i::Integer)
    Tuple(r)[i] # Slow...
end

@inline function Base.convert{T}(::Type{Tuple}, r::RotZYZ{T})
    sinθ₁ = sin(r.theta1)
    cosθ₁ = cos(r.theta1)
    sinθ₂ = sin(r.theta2)
    cosθ₂ = cos(r.theta2)
    sinθ₃ = sin(r.theta3)
    cosθ₃ = cos(r.theta3)

    # transposed representation
    (cosθ₁*cosθ₂*cosθ₃ + -sinθ₁*sinθ₃,   sinθ₁*cosθ₂*cosθ₃ + cosθ₁*sinθ₃,   -sinθ₂*cosθ₃,
     cosθ₁*cosθ₂*-sinθ₃ + -sinθ₁*cosθ₃,  sinθ₁*cosθ₂*-sinθ₃ + cosθ₁*cosθ₃,  sinθ₂*sinθ₃,
     cosθ₁*sinθ₂,                        sinθ₁*sinθ₂,                       cosθ₂)
end

@inline function Base.:*(r::RotZYZ, v::StaticVector)
    if length(v) != 3
        throw("Dimension mismatch: cannot rotate a vector of length $(length(v))")
    end

    T = promote_type(eltype(r), eltype(v))

    sinθ₁ = sin(r.theta1)
    cosθ₁ = cos(r.theta1)
    sinθ₂ = sin(r.theta2)
    cosθ₂ = cos(r.theta2)
    sinθ₃ = sin(r.theta3)
    cosθ₃ = cos(r.theta3)

    return similar_type(v,T)(
        (cosθ₁*cosθ₂*cosθ₃ + -sinθ₁*sinθ₃)*v[1] + (cosθ₁*cosθ₂*-sinθ₃ + -sinθ₁*cosθ₃)*v[2] + cosθ₁*sinθ₂*v[3],
        (sinθ₁*cosθ₂*cosθ₃ + cosθ₁*sinθ₃)*v[1] + (sinθ₁*cosθ₂*-sinθ₃ + cosθ₁*cosθ₃)*v[2] + sinθ₁*sinθ₂*v[3],
        -sinθ₂*cosθ₃*v[1] + sinθ₂*sinθ₃*v[2] + cosθ₂*v[3])
end

@inline Base.:*(r1::RotZ, r2::RotYZ) = RotZYZ(r1.theta, r2.theta1, r2.theta2)
@inline Base.:*(r1::RotZY, r2::RotZ) = RotZYZ(r1.theta1, r1.theta2, r2.theta)
@inline Base.:*(r1::RotZYZ, r2::RotZ) = RotZYZ(r1.theta1, r1.theta2, r1.theta3 + r2.theta)
@inline Base.:*(r1::RotZ, r2::RotZYZ) = RotZYZ(r1.theta + r2.theta1, r2.theta2, r2.theta3)

@inline inv(r::RotZYZ) = RotZYZ(-r.theta3, -r.theta2, -r.theta1)

# define null rotations for convenience
@inline eye(::Type{RotZYZ}) = RotZYZ(0.0, 0.0, 0.0)
@inline eye{T}(::Type{RotZYZ{T}}) = RotZYZ{T}(zero(T), zero(T), zero(T))

###############################
# Tait-Bryant Euler Rotations #
###############################

"""
    immutable RotXYZ{T} <: Rotation{T}
    RotXYZ(theta1, theta2, theta3)
    RotXYZ(roll=r, pitch=p, yaw=y)

A 3×3 rotation matrix parameterized by the "Tait-Bryant" XYZ Euler angle
convention, consisting of first a rotation about the Z axis by `theta3`,
followed by a rotation about the Y axis by `theta2`, and finally a rotation
about the X axis by `theta1`.

The keyword argument form applies roll, pitch and yaw to the X, Y and Z axes
respectively, in XYZ order. (Because it is a right-handed coordinate system,
note that positive pitch is heading in the negative Z axis).
"""
immutable RotXYZ{T} <: Rotation{T}
    theta1::T
    theta2::T
    theta3::T
end

# StaticArrays will take over *all* the constructors and put everything in a tuple...
# but this isn't quite what we mean when we have 3 inputs (not 9).
@inline (::Type{RotXYZ}){X,Y,Z}(x::X, y::Y, z::Z) = RotXYZ{promote_type(promote_type(X, Y), Z)}(x, y, z)
@inline (::Type{Rot}){Rot<:RotXYZ}(; roll=0, pitch=0, yaw=0) = Rot(roll, pitch, yaw)

# These 2 functions are enough to satisfy the entire StaticArrays interface:
@inline function (::Type{Rot}){Rot <: RotXYZ}(t::NTuple{9})
    R = SMatrix{3,3}(t)

    t1 = atan2(-R[2, 3], R[3, 3])
    ct1, st1 = cos(t1), sin(t1)

    Rot(t1,
        atan2(R[1, 3], (R[1, 1] * R[1, 1] + R[1, 2] * R[1, 2])^(1/2)),
        atan2(R[2, 1]*ct1 + R[3, 1]*st1, R[2, 2]*ct1 + R[3, 2]*st1))
end

@inline function Base.getindex{T}(r::RotXYZ{T}, i::Integer)
    Tuple(r)[i] # Slow...
end

@inline function Base.convert{T}(::Type{Tuple}, r::RotXYZ{T})
    sinθ₁ = sin(r.theta1)
    cosθ₁ = cos(r.theta1)
    sinθ₂ = sin(r.theta2)
    cosθ₂ = cos(r.theta2)
    sinθ₃ = sin(r.theta3)
    cosθ₃ = cos(r.theta3)

    # transposed representation
    (cosθ₂*cosθ₃,   sinθ₁*sinθ₂*cosθ₃ + cosθ₁*sinθ₃,   cosθ₁*-sinθ₂*cosθ₃ + sinθ₁*sinθ₃,
     cosθ₂*-sinθ₃,  sinθ₁*sinθ₂*-sinθ₃ + cosθ₁*cosθ₃,  cosθ₁*-sinθ₂*-sinθ₃ + sinθ₁*cosθ₃,
     sinθ₂,         -sinθ₁*cosθ₂,                      cosθ₁*cosθ₂)
end

@inline function Base.:*(r::RotXYZ, v::StaticVector)
    if length(v) != 3
        throw("Dimension mismatch: cannot rotate a vector of length $(length(v))")
    end

    T = promote_type(eltype(r), eltype(v))

    sinθ₁ = sin(r.theta1)
    cosθ₁ = cos(r.theta1)
    sinθ₂ = sin(r.theta2)
    cosθ₂ = cos(r.theta2)
    sinθ₃ = sin(r.theta3)
    cosθ₃ = cos(r.theta3)

    return similar_type(v,T)(
        cosθ₂*cosθ₃*v[1] + cosθ₂*-sinθ₃*v[2] + sinθ₂*v[3],
        (sinθ₁*sinθ₂*cosθ₃ + cosθ₁*sinθ₃)*v[1] + (sinθ₁*sinθ₂*-sinθ₃ + cosθ₁*cosθ₃)*v[2] + -sinθ₁*cosθ₂*v[3],
        (cosθ₁*-sinθ₂*cosθ₃ + sinθ₁*sinθ₃)*v[1] + (cosθ₁*sinθ₂*sinθ₃ + sinθ₁*cosθ₃)*v[2] + cosθ₁*cosθ₂*v[3])
end

@inline Base.:*(r1::RotX, r2::RotYZ) = RotXYZ(r1.theta, r2.theta1, r2.theta2)
@inline Base.:*(r1::RotXY, r2::RotZ) = RotXYZ(r1.theta1, r1.theta2, r2.theta)
@inline Base.:*(r1::RotXYZ, r2::RotZ) = RotXYZ(r1.theta1, r1.theta2, r1.theta3 + r2.theta)
@inline Base.:*(r1::RotX, r2::RotXYZ) = RotXYZ(r1.theta + r2.theta1, r2.theta2, r2.theta3)

@inline inv(r::RotXYZ) = RotZYX(-r.theta3, -r.theta2, -r.theta1)

# define null rotations for convenience
@inline eye(::Type{RotXYZ}) = RotXYZ(0.0, 0.0, 0.0)
@inline eye{T}(::Type{RotXYZ{T}}) = RotXYZ{T}(zero(T), zero(T), zero(T))

"""
    immutable RotZYX{T} <: Rotation{T}
    RotZYX(theta1, theta2, theta3)
    RotZYX(roll=r, pitch=p, yaw=y)

A 3×3 rotation matrix parameterized by the "Tait-Bryant" ZYX Euler angle
convention, consisting of first a rotation about the X axis by `theta3`,
followed by a rotation about the Y axis by `theta2`, and finally a rotation
about the Z axis by `theta1`.

The keyword argument form applies roll, pitch and yaw to the X, Y and Z axes
respectively, in ZYX order. (Because it is a right-handed coordinate system,
note that positive pitch is heading in the negative Z axis).
"""
immutable RotZYX{T} <: Rotation{T}
    theta1::T
    theta2::T
    theta3::T
end

# StaticArrays will take over *all* the constructors and put everything in a tuple...
# but this isn't quite what we mean when we have 3 inputs (not 9).
@inline (::Type{RotZYX}){X,Y,Z}(x::X, y::Y, z::Z) = RotZYX{promote_type(promote_type(X, Y), Z)}(x, y, z)
@inline (::Type{Rot}){Rot<:RotZYX}(; roll=0, pitch=0, yaw=0) = Rot(yaw, pitch, roll)

# These 2 functions are enough to satisfy the entire StaticArrays interface:
@inline function (::Type{Rot}){Rot <: RotZYX}(t::NTuple{9})
    R = SMatrix{3,3}(t)

    t1 = atan2(R[2, 1], R[1, 1])
    ct1, st1 = cos(t1), sin(t1)

    Rot(t1,
        atan2(-R[3, 1], (R[3, 2] * R[3, 2] + R[3, 3] * R[3, 3])^(1/2)),
        atan2(R[1, 3]*st1 - R[2, 3]*ct1, R[2, 2]*ct1 - R[1, 2]*st1))
end

@inline function Base.getindex{T}(r::RotZYX{T}, i::Integer)
    Tuple(r)[i] # Slow...
end

@inline function Base.convert{T}(::Type{Tuple}, r::RotZYX{T})
    sinθ₁ = sin(r.theta1)
    cosθ₁ = cos(r.theta1)
    sinθ₂ = sin(r.theta2)
    cosθ₂ = cos(r.theta2)
    sinθ₃ = sin(r.theta3)
    cosθ₃ = cos(r.theta3)

    # transposed representation
    ( cosθ₁*cosθ₂,                       sinθ₁*cosθ₂,                      -sinθ₂,
     -sinθ₁*cosθ₃ + cosθ₁*sinθ₂*sinθ₃,   cosθ₁*cosθ₃ + sinθ₁*sinθ₂*sinθ₃,   cosθ₂*sinθ₃,
      sinθ₁*sinθ₃ + cosθ₁*sinθ₂*cosθ₃,   cosθ₁*-sinθ₃ + sinθ₁*sinθ₂*cosθ₃,  cosθ₂*cosθ₃)
end

@inline function Base.:*(r::RotZYX, v::StaticVector)
    if length(v) != 3
        throw("Dimension mismatch: cannot rotate a vector of length $(length(v))")
    end

    T = promote_type(eltype(r), eltype(v))

    sinθ₁ = sin(r.theta1)
    cosθ₁ = cos(r.theta1)
    sinθ₂ = sin(r.theta2)
    cosθ₂ = cos(r.theta2)
    sinθ₃ = sin(r.theta3)
    cosθ₃ = cos(r.theta3)

    return similar_type(v,T)(
        cosθ₁*cosθ₂*v[1] + (-sinθ₁*cosθ₃ + cosθ₁*sinθ₂*sinθ₃)*v[2] + (sinθ₁*sinθ₃ + cosθ₁*sinθ₂*cosθ₃)*v[3],
        sinθ₁*cosθ₂*v[1] + (cosθ₁*cosθ₃ + sinθ₁*sinθ₂*sinθ₃)*v[2] + (cosθ₁*-sinθ₃ + sinθ₁*sinθ₂*cosθ₃)*v[3],
        -sinθ₂*v[1] + cosθ₂*sinθ₃*v[2] + cosθ₂*cosθ₃*v[3])
end

@inline Base.:*(r1::RotZ, r2::RotYX) = RotZYX(r1.theta, r2.theta1, r2.theta2)
@inline Base.:*(r1::RotZY, r2::RotX) = RotZYX(r1.theta1, r1.theta2, r2.theta)
@inline Base.:*(r1::RotZYX, r2::RotX) = RotZYX(r1.theta1, r1.theta2, r1.theta3 + r2.theta)
@inline Base.:*(r1::RotZ, r2::RotZYX) = RotZYX(r1.theta + r2.theta1, r2.theta2, r2.theta3)

@inline inv(r::RotZYX) = RotXYZ(-r.theta3, -r.theta2, -r.theta1)

# define null rotations for convenience
@inline eye(::Type{RotZYX}) = RotZYX(0.0, 0.0, 0.0)
@inline eye{T}(::Type{RotZYX{T}}) = RotZYX{T}(zero(T), zero(T), zero(T))

"""
    immutable RotXZY{T} <: Rotation{T}
    RotXZY(theta1, theta2, theta3)
    RotXZY(roll=r, pitch=p, yaw=y)

A 3×3 rotation matrix parameterized by the "Tait-Bryant" XZY Euler angle
convention, consisting of first a rotation about the Y axis by `theta3`,
followed by a rotation about the Z axis by `theta2`, and finally a rotation
about the X axis by `theta1`.

The keyword argument form applies roll, pitch and yaw to the X, Y and Z axes
respectively, in XZY order. (Because it is a right-handed coordinate system,
note that positive pitch is heading in the negative Z axis).
"""
immutable RotXZY{T} <: Rotation{T}
    theta1::T
    theta2::T
    theta3::T
end

# StaticArrays will take over *all* the constructors and put everything in a tuple...
# but this isn't quite what we mean when we have 3 inputs (not 9).
@inline (::Type{RotXZY}){X,Y,Z}(x::X, y::Y, z::Z) = RotXZY{promote_type(promote_type(X, Y), Z)}(x, y, z)
@inline (::Type{Rot}){Rot<:RotXZY}(; roll=0, pitch=0, yaw=0) = Rot(roll, yaw, pitch)

# These 2 functions are enough to satisfy the entire StaticArrays interface:
@inline function (::Type{Rot}){Rot <: RotXZY}(t::NTuple{9})
    R = SMatrix{3,3}(t)

    t1 = atan2(R[3, 2], R[2, 2])
    ct1, st1 = cos(t1), sin(t1)

    Rot(t1,
        atan2(-R[1, 2], (R[1, 1] * R[1, 1] + R[1, 3] * R[1, 3])^(1/2)),
        atan2(R[2, 1]*st1 - R[3, 1]*ct1, R[3, 3]*ct1 - R[2, 3]*st1))
end

@inline function Base.getindex{T}(r::RotXZY{T}, i::Integer)
    Tuple(r)[i] # Slow...
end

@inline function Base.convert{T}(::Type{Tuple}, r::RotXZY{T})
    sinθ₁ = sin(r.theta1)
    cosθ₁ = cos(r.theta1)
    sinθ₂ = sin(r.theta2)
    cosθ₂ = cos(r.theta2)
    sinθ₃ = sin(r.theta3)
    cosθ₃ = cos(r.theta3)

    # transposed representation
    ( cosθ₂*cosθ₃,  cosθ₁*sinθ₂*cosθ₃ + sinθ₁*sinθ₃,   sinθ₁*sinθ₂*cosθ₃ + cosθ₁*-sinθ₃,
     -sinθ₂,        cosθ₁*cosθ₂,                       sinθ₁*cosθ₂,
      cosθ₂*sinθ₃,  cosθ₁*sinθ₂*sinθ₃ + -sinθ₁*cosθ₃,  sinθ₁*sinθ₂*sinθ₃ + cosθ₁*cosθ₃)
end

@inline function Base.:*(r::RotXZY, v::StaticVector)
    if length(v) != 3
        throw("Dimension mismatch: cannot rotate a vector of length $(length(v))")
    end

    T = promote_type(eltype(r), eltype(v))

    sinθ₁ = sin(r.theta1)
    cosθ₁ = cos(r.theta1)
    sinθ₂ = sin(r.theta2)
    cosθ₂ = cos(r.theta2)
    sinθ₃ = sin(r.theta3)
    cosθ₃ = cos(r.theta3)

    return similar_type(v,T)(
        cosθ₂*cosθ₃*v[1] + -sinθ₂*v[2] + cosθ₂*sinθ₃*v[3],
        (cosθ₁*sinθ₂*cosθ₃ + sinθ₁*sinθ₃)*v[1] + cosθ₁*cosθ₂*v[2] + (cosθ₁*sinθ₂*sinθ₃ + -sinθ₁*cosθ₃)*v[3],
        (sinθ₁*sinθ₂*cosθ₃ + cosθ₁*-sinθ₃)*v[1] + sinθ₁*cosθ₂*v[2] + (sinθ₁*sinθ₂*sinθ₃ + cosθ₁*cosθ₃)*v[3])
end

@inline Base.:*(r1::RotX, r2::RotZY) = RotXZY(r1.theta, r2.theta1, r2.theta2)
@inline Base.:*(r1::RotXZ, r2::RotY) = RotXZY(r1.theta1, r1.theta2, r2.theta)
@inline Base.:*(r1::RotXZY, r2::RotY) = RotXZY(r1.theta1, r1.theta2, r1.theta3 + r2.theta)
@inline Base.:*(r1::RotX, r2::RotXZY) = RotXZY(r1.theta + r2.theta1, r2.theta2, r2.theta3)

@inline inv(r::RotXZY) = RotYZX(-r.theta3, -r.theta2, -r.theta1)

# define null rotations for convenience
@inline eye(::Type{RotXZY}) = RotXZY(0.0, 0.0, 0.0)
@inline eye{T}(::Type{RotXZY{T}}) = RotXZY{T}(zero(T), zero(T), zero(T))

"""
    immutable RotYZX{T} <: Rotation{T}
    RotYZX(theta1, theta2, theta3)
    RotYZX(roll=r, pitch=p, yaw=y)

A 3×3 rotation matrix parameterized by the "Tait-Bryant" YZX Euler angle
convention, consisting of first a rotation about the X axis by `theta3`,
followed by a rotation about the Z axis by `theta2`, and finally a rotation
about the Y axis by `theta1`.

The keyword argument form applies roll, pitch and yaw to the X, Y and Z axes
respectively, in YZX order. (Because it is a right-handed coordinate system,
note that positive pitch is heading in the negative Z axis).
"""
immutable RotYZX{T} <: Rotation{T}
    theta1::T
    theta2::T
    theta3::T
end

# StaticArrays will take over *all* the constructors and put everything in a tuple...
# but this isn't quite what we mean when we have 3 inputs (not 9).
@inline (::Type{RotYZX}){X,Y,Z}(x::X, y::Y, z::Z) = RotYZX{promote_type(promote_type(X, Y), Z)}(x, y, z)
@inline (::Type{Rot}){Rot<:RotYZX}(; roll=0, pitch=0, yaw=0) = Rot(pitch, yaw, roll)

# These 2 functions are enough to satisfy the entire StaticArrays interface:
@inline function (::Type{Rot}){Rot <: RotYZX}(t::NTuple{9})
    R = SMatrix{3,3}(t)

    t1 = atan2(-R[3, 1], R[1, 1])
    ct1, st1 = cos(t1), sin(t1)

    Rot(t1,
        atan2(R[2, 1], (R[2, 2] * R[2, 2] + R[2, 3] * R[2, 3])^(1/2)),
        atan2(R[3, 2]*ct1 + R[1, 2]*st1, R[3, 3]*ct1 + R[1, 3]*st1))
end

@inline function Base.getindex{T}(r::RotYZX{T}, i::Integer)
    Tuple(r)[i] # Slow...
end

@inline function Base.convert{T}(::Type{Tuple}, r::RotYZX{T})
    sinθ₁ = sin(r.theta1)
    cosθ₁ = cos(r.theta1)
    sinθ₂ = sin(r.theta2)
    cosθ₂ = cos(r.theta2)
    sinθ₃ = sin(r.theta3)
    cosθ₃ = cos(r.theta3)

    # transposed representation
    (cosθ₁*cosθ₂,                        sinθ₂,         -sinθ₁*cosθ₂,
     cosθ₁*-sinθ₂*cosθ₃ + sinθ₁*sinθ₃,   cosθ₂*cosθ₃,   sinθ₁*sinθ₂*cosθ₃ + cosθ₁*sinθ₃,
     cosθ₁*sinθ₂*sinθ₃ + sinθ₁*cosθ₃,    cosθ₂*-sinθ₃,  sinθ₁*sinθ₂*-sinθ₃ + cosθ₁*cosθ₃)
end

@inline function Base.:*(r::RotYZX, v::StaticVector)
    if length(v) != 3
        throw("Dimension mismatch: cannot rotate a vector of length $(length(v))")
    end

    T = promote_type(eltype(r), eltype(v))

    sinθ₁ = sin(r.theta1)
    cosθ₁ = cos(r.theta1)
    sinθ₂ = sin(r.theta2)
    cosθ₂ = cos(r.theta2)
    sinθ₃ = sin(r.theta3)
    cosθ₃ = cos(r.theta3)

    return similar_type(v,T)(
        cosθ₁*cosθ₂*v[1] + (cosθ₁*-sinθ₂*cosθ₃ + sinθ₁*sinθ₃)*v[2] + (cosθ₁*-sinθ₂*-sinθ₃ + sinθ₁*cosθ₃)*v[3],
        sinθ₂*v[1] + cosθ₂*cosθ₃*v[2] + cosθ₂*-sinθ₃*v[3],
        -sinθ₁*cosθ₂*v[1] + (sinθ₁*sinθ₂*cosθ₃ + cosθ₁*sinθ₃)*v[2] + (sinθ₁*sinθ₂*-sinθ₃ + cosθ₁*cosθ₃)*v[3])
end

@inline Base.:*(r1::RotY, r2::RotZX) = RotYZX(r1.theta, r2.theta1, r2.theta2)
@inline Base.:*(r1::RotYZ, r2::RotX) = RotYZX(r1.theta1, r1.theta2, r2.theta)
@inline Base.:*(r1::RotYZX, r2::RotX) = RotYZX(r1.theta1, r1.theta2, r1.theta3 + r2.theta)
@inline Base.:*(r1::RotY, r2::RotYZX) = RotYZX(r1.theta + r2.theta1, r2.theta2, r2.theta3)

@inline inv(r::RotYZX) = RotXZY(-r.theta3, -r.theta2, -r.theta1)

# define null rotations for convenience
@inline eye(::Type{RotYZX}) = RotYZX(0.0, 0.0, 0.0)
@inline eye{T}(::Type{RotYZX{T}}) = RotYZX{T}(zero(T), zero(T), zero(T))

"""
    immutable RotYXZ{T} <: Rotation{T}
    RotYXZ(theta1, theta2, theta3)
    RotYXZ(roll=r, pitch=p, yaw=y)

A 3×3 rotation matrix parameterized by the "Tait-Bryant" YXZ Euler angle
convention, consisting of first a rotation about the Z axis by `theta3`,
followed by a rotation about the X axis by `theta2`, and finally a rotation
about the Y axis by `theta1`.

The keyword argument form applies roll, pitch and yaw to the X, Y and Z axes
respectively, in YXZ order. (Because it is a right-handed coordinate system,
note that positive pitch is heading in the negative Z axis).
"""
immutable RotYXZ{T} <: Rotation{T}
    theta1::T
    theta2::T
    theta3::T
end

# StaticArrays will take over *all* the constructors and put everything in a tuple...
# but this isn't quite what we mean when we have 3 inputs (not 9).
@inline (::Type{RotYXZ}){X,Y,Z}(x::X, y::Y, z::Z) = RotYXZ{promote_type(promote_type(X, Y), Z)}(x, y, z)
@inline (::Type{Rot}){Rot<:RotYXZ}(; roll=0, pitch=0, yaw=0) = Rot(pitch, roll, yaw)

# These 2 functions are enough to satisfy the entire StaticArrays interface:
@inline function (::Type{Rot}){Rot <: RotYXZ}(t::NTuple{9})
    R = SMatrix{3,3}(t)

    t1 = atan2(R[1, 3], R[3, 3])
    ct1, st1 = cos(t1), sin(t1)

    Rot(t1,
        atan2(-R[2, 3], (R[2, 1] * R[2, 1] + R[2, 2] * R[2, 2])^(1/2)),
        atan2(R[3, 2]*st1 - R[1, 2]*ct1, R[1, 1]*ct1 - R[3, 1]*st1))
end

@inline function Base.getindex{T}(r::RotYXZ{T}, i::Integer)
    Tuple(r)[i] # Slow...
end

@inline function Base.convert{T}(::Type{Tuple}, r::RotYXZ{T})
    sinθ₁ = sin(r.theta1)
    cosθ₁ = cos(r.theta1)
    sinθ₂ = sin(r.theta2)
    cosθ₂ = cos(r.theta2)
    sinθ₃ = sin(r.theta3)
    cosθ₃ = cos(r.theta3)

    # transposed representation
    (cosθ₁*cosθ₃ + sinθ₁*sinθ₂*sinθ₃,   cosθ₂*sinθ₃,  -sinθ₁*cosθ₃ + cosθ₁*sinθ₂*sinθ₃,
     cosθ₁*-sinθ₃ + sinθ₁*sinθ₂*cosθ₃,  cosθ₂*cosθ₃,  sinθ₁*sinθ₃ + cosθ₁*sinθ₂*cosθ₃,
     sinθ₁*cosθ₂,                       -sinθ₂,       cosθ₁*cosθ₂)
end

@inline function Base.:*(r::RotYXZ, v::StaticVector)
    if length(v) != 3
        throw("Dimension mismatch: cannot rotate a vector of length $(length(v))")
    end

    T = promote_type(eltype(r), eltype(v))

    sinθ₁ = sin(r.theta1)
    cosθ₁ = cos(r.theta1)
    sinθ₂ = sin(r.theta2)
    cosθ₂ = cos(r.theta2)
    sinθ₃ = sin(r.theta3)
    cosθ₃ = cos(r.theta3)

    return similar_type(v,T)(
        (cosθ₁*cosθ₃ + sinθ₁*sinθ₂*sinθ₃)*v[1] + (cosθ₁*-sinθ₃ + sinθ₁*sinθ₂*cosθ₃)*v[2] + sinθ₁*cosθ₂*v[3],
        cosθ₂*sinθ₃*v[1] + cosθ₂*cosθ₃*v[2] + -sinθ₂*v[3],
        (-sinθ₁*cosθ₃ + cosθ₁*sinθ₂*sinθ₃)*v[1] + (sinθ₁*sinθ₃ + cosθ₁*sinθ₂*cosθ₃)*v[2] + cosθ₁*cosθ₂*v[3])
end

@inline Base.:*(r1::RotY, r2::RotXZ) = RotYXZ(r1.theta, r2.theta1, r2.theta2)
@inline Base.:*(r1::RotYX, r2::RotZ) = RotYXZ(r1.theta1, r1.theta2, r2.theta)
@inline Base.:*(r1::RotYXZ, r2::RotZ) = RotYXZ(r1.theta1, r1.theta2, r1.theta3 + r2.theta)
@inline Base.:*(r1::RotY, r2::RotYXZ) = RotYXZ(r1.theta + r2.theta1, r2.theta2, r2.theta3)

@inline inv(r::RotYXZ) = RotZXY(-r.theta3, -r.theta2, -r.theta1)

# define null rotations for convenience
@inline eye(::Type{RotYXZ}) = RotYXZ(0.0, 0.0, 0.0)
@inline eye{T}(::Type{RotYXZ{T}}) = RotYXZ{T}(zero(T), zero(T), zero(T))

"""
    immutable RotZXY{T} <: Rotation{T}
    RotZXY(theta1, theta2, theta3)
    RotZXY(roll=r, pitch=p, yaw=y)

A 3×3 rotation matrix parameterized by the "Tait-Bryant" ZXY Euler angle
convention, consisting of first a rotation about the Y axis by `theta3`,
followed by a rotation about the X axis by `theta2`, and finally a rotation
about the Z axis by `theta1`.

The keyword argument form applies roll, pitch and yaw to the X, Y and Z axes
respectively, in ZXY order. (Because it is a right-handed coordinate system,
note that positive pitch is heading in the negative Z axis).
"""
immutable RotZXY{T} <: Rotation{T}
    theta1::T
    theta2::T
    theta3::T
end

# StaticArrays will take over *all* the constructors and put everything in a tuple...
# but this isn't quite what we mean when we have 3 inputs (not 9).
@inline (::Type{RotZXY}){X,Y,Z}(x::X, y::Y, z::Z) = RotZXY{promote_type(promote_type(X, Y), Z)}(x, y, z)
@inline (::Type{Rot}){Rot<:RotZXY}(; roll=0, pitch=0, yaw=0) = Rot(yaw, roll, pitch)

# These 2 functions are enough to satisfy the entire StaticArrays interface:
@inline function (::Type{Rot}){Rot <: RotZXY}(t::NTuple{9})
    R = SMatrix{3,3}(t)

    t1 = atan2(-R[1, 2], R[2, 2])
    ct1, st1 = cos(t1), sin(t1)

    Rot(t1,
        atan2(R[3, 2], (R[3, 1] * R[3, 1] + R[3, 3] * R[3, 3])^(1/2)),
        atan2(R[1, 3]*ct1 + R[2, 3]*st1, R[1, 1]*ct1 + R[2, 1]*st1))
end

@inline function Base.getindex{T}(r::RotZXY{T}, i::Integer)
    Tuple(r)[i] # Slow...
end

@inline function Base.convert{T}(::Type{Tuple}, r::RotZXY{T})
    sinθ₁ = sin(r.theta1)
    cosθ₁ = cos(r.theta1)
    sinθ₂ = sin(r.theta2)
    cosθ₂ = cos(r.theta2)
    sinθ₃ = sin(r.theta3)
    cosθ₃ = cos(r.theta3)

    # transposed representation
    ( cosθ₁*cosθ₃ + sinθ₁*sinθ₂*-sinθ₃,  sinθ₁*cosθ₃ + cosθ₁*-sinθ₂*-sinθ₃,  cosθ₂*-sinθ₃,
     -sinθ₁*cosθ₂,                       cosθ₁*cosθ₂,                        sinθ₂,
      cosθ₁*sinθ₃ + sinθ₁*sinθ₂*cosθ₃,   sinθ₁*sinθ₃ + cosθ₁*-sinθ₂*cosθ₃,   cosθ₂*cosθ₃)
end

@inline function Base.:*(r::RotZXY, v::StaticVector)
    if length(v) != 3
        throw("Dimension mismatch: cannot rotate a vector of length $(length(v))")
    end

    T = promote_type(eltype(r), eltype(v))

    sinθ₁ = sin(r.theta1)
    cosθ₁ = cos(r.theta1)
    sinθ₂ = sin(r.theta2)
    cosθ₂ = cos(r.theta2)
    sinθ₃ = sin(r.theta3)
    cosθ₃ = cos(r.theta3)

    return similar_type(v,T)(
        (cosθ₁*cosθ₃ + sinθ₁*sinθ₂*-sinθ₃)*v[1] + -sinθ₁*cosθ₂*v[2] + (cosθ₁*sinθ₃ + sinθ₁*sinθ₂*cosθ₃)*v[3],
        (sinθ₁*cosθ₃ + cosθ₁*-sinθ₂*-sinθ₃)*v[1] + cosθ₁*cosθ₂*v[2] + (sinθ₁*sinθ₃ + cosθ₁*-sinθ₂*cosθ₃)*v[3],
         cosθ₂*-sinθ₃*v[1] + sinθ₂*v[2] + cosθ₂*cosθ₃*v[3])
end

@inline Base.:*(r1::RotZ, r2::RotXY) = RotZXY(r1.theta, r2.theta1, r2.theta2)
@inline Base.:*(r1::RotZX, r2::RotY) = RotZXY(r1.theta1, r1.theta2, r2.theta)
@inline Base.:*(r1::RotZXY, r2::RotY) = RotZXY(r1.theta1, r1.theta2, r1.theta3 + r2.theta)
@inline Base.:*(r1::RotZ, r2::RotZXY) = RotZXY(r1.theta + r2.theta1, r2.theta2, r2.theta3)

@inline inv(r::RotZXY) = RotYXZ(-r.theta3, -r.theta2, -r.theta1)

# define null rotations for convenience
@inline eye(::Type{RotZXY}) = RotZXY(0.0, 0.0, 0.0)
@inline eye{T}(::Type{RotZXY{T}}) = RotZXY{T}(zero(T), zero(T), zero(T))


using Plots; gr()
using Measures
using FFTW
push!(LOAD_PATH, pwd())
using DHC_2DUtils

function plot_filter_bank_QA(filt, info; fname="filter_bank_QA.png")

    # -------- define plot1 to append plot to a list
    function plot1(ps, image; clim=nothing, bin=1.0, fsz=12, label=nothing)
        # ps    - list of plots to append to
        # image - image to heatmap
        # clim  - tuple of color limits (min, max)
        # bin   - bin down by this factor, centered on nx/2+1
        # fsz   - font size
        marg   = -1mm
        nx, ny = size(image)
        nxb    = nx/round(Integer, 2*bin)

        # -------- center on nx/2+1
        i0 = max(1,round(Integer, (nx/2+2)-nxb-1))
        i1 = min(nx,round(Integer, (nx/2)+nxb+1))
        lims = [i0,i1]
        subim = image[i0:i1,i0:i1]
        push!(ps, heatmap(image,aspect_ratio=:equal,clim=clim,
            xlims=lims, ylims=lims, size=(400,400),
            legend=false,xtickfontsize=fsz,ytickfontsize=fsz,#tick_direction=:out,
            rightmargin=marg,leftmargin=marg,topmargin=marg,bottommargin=marg))
        if label != nothing
            annotate!(lims'*[.96,.04],lims'*[.09,.91],text(label,:left,:white,16))
        end
        return
    end

    # -------- initialize array of plots
    ps   = []
    ind  = info["filter_index"]
    jval = info["j_value"]
    J    = length(jval)
    L    = length(info["theta_value"])
    # plot the first 3 and last 3 j values
    jplt = [1:3;J-2:J]

    # -------- loop over j
    for j_ind in jplt
        # first 3 filters in Fourier space
        j = jval[j_ind]
        bfac = max(1,2.0^(j-1))
        for l = 1:3
            label=string("j=",j_ind," ℓ=",l-1)
            #label=latexstring("\\tilde{\\psi}_{$j_ind,$(l-1)}")
            plot1(ps, fftshift(filt[:,:,ind[j_ind,l]]), clim=(0,1),
                  bin=bfac, label=label)
        end
        fac = max(1,2.0^(5-j))
        # real part of config space of 3rd filter
        plot1(ps, fftshift(real(ifft(filt[:,:,ind[j_ind,3]]))),
              bin=fac, label=string("j=",j_ind," ℓ=",3-1))

        # sum all filters to form (partial) ring
        ring = dropdims(sum(filt[:,:,ind[j_ind,:]].^2, dims=3), dims=3)
        plot1(ps, fftshift(ring[:,:,1]), clim=(0,1),
              bin=bfac, label=string("j=",j_ind," ℓ=",0,":",L-1))
    end

    wavepow = fftshift(dropdims(sum(filt[:,:,ind].^2, dims=(3,4)), dims=(3,4)))
    plot1(ps, wavepow, clim=(-0.1,1), label=string("j=1:",J," ℓ=0:",L-1))

    if info["pc"]==1
        wavepow += circshift(wavepow[end:-1:1,end:-1:1],(1,1))
    end
    plot1(ps, wavepow, clim=(-0.1,1), bin=32, label="missing power")
    phi = filt[:,:,info["phi_index"]]
    phi_shift = fftshift(phi)
    plot1(ps, phi_shift.^2, clim=(-0.1,1), bin=32, label="ϕ")
    disc = wavepow+(phi_shift.^2)
    plot1(ps, fftshift(real(ifft(phi))), label="ϕ")
    plot1(ps, disc, clim=(-0.1,1), bin=32, label="all")

    myplot = plot(ps..., layout=(7,5), size=(1400,2000))
    savefig(myplot, fname)
end

# -------- define a filter bank

filt, info  = fink_filter_bank(1, 8, pc=1, wd=1)
hash = fink_filter_hash(1, 8, pc=1, wd=1)
plot_filter_bank_QA(filt, info, fname="filt-8-pc1-wd1.png")

filt, info  = fink_filter_bank(1, 8, pc=1, wd=2)
plot_filter_bank_QA(filt, info, fname="filt-8-pc1-wd2.png")

filt, info  = fink_filter_bank(1, 8, pc=1, wd=3)
plot_filter_bank_QA(filt, info, fname="filt-8-pc1-wd3.png")

filt, info  = fink_filter_bank(1, 8, pc=2, wd=1)
plot_filter_bank_QA(filt, info, fname="filt-8-pc2-wd1.png")

filt, info  = fink_filter_bank(1, 16, pc=2, wd=1)
plot_filter_bank_QA(filt, info, fname="filt-16-pc2-wd1.png")

filt, info  = fink_filter_bank(1, 16, pc=2, wd=2)
plot_filter_bank_QA(filt, info, fname="filt-16-pc2-wd2.png")

filt, info  = fink_filter_bank(1, 16, pc=1, wd=1)
plot_filter_bank_QA(filt, info, fname="filt-16-pc1-wd1.png")

filt, info  = fink_filter_bank(2, 8, pc=1, wd=1)
plot_filter_bank_QA(filt, info, fname="filt2-8-pc1-wd1.png")

filt, info  = fink_filter_bank(2, 8, pc=2, wd=1)
plot_filter_bank_QA(filt, info, fname="filt2-8-pc2-wd1.png")

filt, info  = fink_filter_bank(2, 16, pc=2, wd=1)
plot_filter_bank_QA(filt, info, fname="filt2-16-pc2-wd1.png")

filt, info  = fink_filter_bank(2, 16, pc=2, wd=2)
plot_filter_bank_QA(filt, info, fname="filt2-16-pc2-wd2.png")

filt, info  = fink_filter_bank(1, 8, pc=1, wd=1, shift=true)
plot_filter_bank_QA(filt, info, fname="filt-8-pc1-wd1-shift.png")

filt, info  = fink_filter_bank(1, 16, pc=2, wd=1, shift=true)
plot_filter_bank_QA(filt, info, fname="filt-16-pc2-wd1-shift.png")

filt, info  = fink_filter_bank(1, 16, pc=2, wd=2, shift=true)
plot_filter_bank_QA(filt, info, fname="filt-16-pc2-wd2-shift.png")
DH


using Sundials, Compat

## f routine. Compute function f(t,y).

function f(t, y_nv, ydot_nv, user_data)
    y = convert(Vector, y_nv)
    ydot = convert(Vector, ydot_nv)
    ydot[1] = -0.04*y[1] + 1.0e4*y[2]*y[3]
    ydot[3] = 3.0e7*y[2]*y[2]
    ydot[2] = -ydot[1] - ydot[3]
    return Sundials.CV_SUCCESS
end

## g routine. Compute functions g_i(t,y) for i = 0,1.

function g(t, y_nv, gout_ptr, user_data)
    y = convert(Vector, y_nv)
    gout = Sundials.asarray(gout_ptr, (2,))
    gout[1] = y[1] - 0.0001
    gout[2] = y[3] - 0.01
    return Sundials.CV_SUCCESS
end

## Jacobian routine. Compute J(t,y) = df/dy.
# broken -- needs a wrapper from Sundials._DlsMat to Matrix and Jac user function wrapper
function Jac(N, t, ny, fy, Jptr, user_data, tmp1, tmp2, tmp3)
    y = convert(Vector, ny)
    dlsmat = unpack(IOString(@compat unsafe_wrap(convert(Ptr{UInt8}, Jptr),
                                                 (sum(map(sizeof, Sundials._DlsMat))+10,), false)),
                    Sundials._DlsMat)
    J = @compat unsafe_wrap(unsafe_ref(dlsmat.cols), (Int(neq), Int(neq)), false)
    J[1,1] = -0.04
    J[1,2] = 1.0e4*y[3]
    J[1,3] = 1.0e4*y[2]
    J[2,1] = 0.04
    J[2,2] = -1.0e4*y[3] - 6.0e7*y[2]
    J[2,3] = -1.0e4*y[2]
    J[3,2] = 6.0e7*y[2]
    return Sundials.CV_SUCCESS
end

const neq = 3

const t0 = 0.0
const t1 = 0.4
const tmult = 10.0
const nout = 12
const y0 = [1.0, 0.0, 0.0]
const reltol = 1e-4
const abstol = [1e-8, 1e-14, 1e-6]

cvode_mem = Sundials.CVodeCreate(Sundials.CV_BDF, Sundials.CV_NEWTON)
Sundials.@checkflag Sundials.CVodeInit(cvode_mem, f, t0, y0)
Sundials.@checkflag Sundials.CVodeSVtolerances(cvode_mem, reltol, abstol)
Sundials.@checkflag Sundials.CVodeRootInit(cvode_mem, 2, g)
Sundials.@checkflag Sundials.CVDense(cvode_mem, neq)
#Sundials.@checkflag Sundials.CVDlsSetDenseJacFn(cvode_mem, Jac)

iout = 0
tout = t1
const t = [t0]

while iout < nout
    y = similar(y0)
    flag = Sundials.CVode(cvode_mem, tout, y, t, Sundials.CV_NORMAL)
    println("T=", tout, ", Y=", y)
    if flag == Sundials.CV_ROOT_RETURN
        rootsfound = zeros(Cint, 2)
        Sundials.@checkflag Sundials.CVodeGetRootInfo(cvode_mem, rootsfound)
        println("roots=", rootsfound)
    elseif flag == Sundials.CV_SUCCESS
      iout += 1
      tout *= tmult
    end
end


#=
Copyright (c) 2015, Intel Corporation

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:

    * Redistributions of source code must retain the above copyright notice,
      this list of conditions and the following disclaimer.
    * Redistributions in binary form must reproduce the above copyright
      notice, this list of conditions and the following disclaimer in the
      documentation and/or other materials provided with the distribution.
    * Neither the name of Intel Corporation nor the names of its contributors
      may be used to endorse or promote products derived from this software
      without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
=#

using FactCheck
using Latte

net = Net(8)
data,  data_value = MemoryDataLayer(net, :data, (20,))
fc1               = InnerProductLayer(:fc1, net, data, 10)
tanh1             = TanhLayer(:tanh1, net, fc1)

data_value[:]     = rand(Float32, size(data_value)...)

init(net)

ϵ = 1e-5
facts("Testing Tanh Layer") do
    fc1_expected = get_buffer(net, :fc1weights)' * data_value
    context("Forward") do
        forward(net; phase=Latte.Test)

        expected = tanh(fc1_expected)
        @fact expected --> roughly(get_buffer(net, :tanh1value))
    end
    context("Backward") do
        top_diff = get_buffer(net, :tanh1∇)
        rand!(top_diff)
        ∇tanh(x) = x .* (1.0f0 .- x)
        expected = ∇tanh(tanh(fc1_expected)) .* top_diff
        backward(net)
        @fact expected --> roughly(get_buffer(net, :fc1∇))
    end
end

FactCheck.exitstatus()


# ---
# title: 739. Daily Temperatures
# id: problem739
# author: Indigo
# date: 2021-01-28
# difficulty: Medium
# categories: Hash Table, Stack
# link: <https://leetcode.com/problems/daily-temperatures/description/>
# hidden: true
# ---
#
# Given a list of daily temperatures `T`, return a list such that, for each day
# in the input, tells you how many days you would have to wait until a warmer
# temperature. If there is no future day for which this is possible, put `0`
# instead.
#
# For example, given the list of temperatures `T = [73, 74, 75, 71, 69, 72, 76,
# 73]`, your output should be `[1, 1, 4, 2, 1, 1, 0, 0]`.
#
# **Note:** The length of `temperatures` will be in the range `[1, 30000]`. Each
# temperature will be an integer in the range `[30, 100]`.
#
#
## @lc code=start
using LeetCode

function daily_temperatures(T::Vector{Int})
    res = fill(0, length(T))
    stk = Int[]
    for i in 1:length(T)
        while !isempty(stk) && T[stk[end]] < T[i]
            res[stk[end]] = i - stk[end]
            pop!(stk)
        end
        push!(stk, i)
    end
    res
end
## @lc code=end


let i, n, A, B, C, θ
n = 5 # rand(1:5)

A = matrixdepot("rando", Int, n, 1)
B = matrixdepot("rando", Int, n, 2)
C = matrixdepot("rando", Int, n, 3)

for i in length(A)
    @test A[i] == 0 || A[i] == 1
end

@test all([bi == -1 || bi == 1 for bi in B])
@test all([bi == -1 || bi == 0 || bi == 1 for bi in C])

@test_throws ArgumentError matrixdepot("rando", 3, 3, 5)

θ = matrixdepot("rando", n)
end
println("'rando' passed test...")


function spline2d(xr, yr, mx, my)

    out = zeros(Float64, mx*my)

    for i=1:mx
        for j=1:my
            k = i + (j-1)*mx
            a = xr^(i-1) / factorial(i-1)
            b = yr^(j-1) / factorial(j-1)
            out[k] = a * b
        end
    end

    return out
end

function stencil2d(mx, my)

    # (i,j) indices for center point
    ic = Int(ceil(mx/2.0))
    jc = Int(ceil(my/2.0))

    M = zeros(Float64, (mx*my, mx*my))

    for i=1:mx
        for j=1:my
            k = i + (j-1)*mx
            M[k,:] = spline2d(i-ic, j-jc, mx, my)
        end
    end

    return Matrix(transpose(inv(M)))
end

function interpolation_row(x, y, mx, my, MinvT, grd::Grid)

    x0, y0 = grd.r0
    dx = grd.dr
    dy = grd.dr
    Nx = grd._Nx
    Ny = grd._Ny

    # (i,j) indices for center point of stencil
    ics = Int(ceil(mx/2.0))
    jcs = Int(ceil(my/2.0))

    # indices for center point in grid
    # NOTE: the addition of 1e-5 before calling floor is to compensate for annoying floating point imprecision.
    icg = Int(floor((x - x0)/dx + 1e-5)) + 1 + grd._Nbuffer
    jcg = Int(floor((y - y0)/dy + 1e-5)) + 1 + grd._Nbuffer

    # values for row
    xr = (x - x0)/dx - (icg - 1 - grd._Nbuffer)
    yr = (y - y0)/dy - (jcg - 1 - grd._Nbuffer)
    spline = spline2d(xr, yr, mx, my)
    row_dat = MinvT * spline

    # collective index for first nonzero element of output
    k0 = (icg - ics + 1) + (jcg - jcs)*Nx

    # indices for nonzero elements of row
    row_js = zeros(Int, mx*my)
    for i=1:mx
        for j=1:my
            row_js[i+(j-1)*mx] = k0 + (i-1) + (j-1)*Nx
        end
    end

    return row_dat, row_js
end

# function bilinear_coefficients(x, y, grd::Grid)
#
#     i = Int64(floor((x - grd.r0[1]) / grd.dr + 1e-5)) + 1 + grd._Nbuffer
#     j = Int64(floor((y - grd.r0[2]) / grd.dr + 1e-5)) + 1 + grd._Nbuffer
#     u = (rem(x - grd.r0[1], grd.dr) + 1e-5) / grd.dr
#     v = (rem(y - grd.r0[2], grd.dr) + 1e-5) / grd.dr
#
#     k0 = i + (j - 1)*grd._Nx
#     ks = Int64[k0, k0+grd._Nx, k0+1, k0+grd._Nx+1]
#     C = [(1 - u)*(1 - v), (1 - u)*v, u*(1 - v), u*v]
#
#     return ks, C
# end

function parallel_laplacian(q::Float64, Nz::Int64, grd::Grid)

    is = Int64[]
    js = Int64[]
    dats = Float64[]

    W = exp(im*2*pi/(q*Nz))
    m = 2
    MinvT = stencil2d(m, m)

    for k=1:grd.Nk

        z = Complex(grd.points[:,k]...)
        z_fwd = W*z
        z_bck = z / W
        ds = abs(z)*2*pi/(q*Nz)

        row_dat, row_js = interpolation_row(real(z), imag(z), m, m, MinvT, grd)
        is = [is; k*ones(Int64, m^2)]
        js = [js; row_js]
        dats = [dats; -2*row_dat / ds^2]

        row_dat, row_js = interpolation_row(real(z_fwd), imag(z_fwd), m, m, MinvT, grd)
        is = [is; k*ones(Int64, m^2)]
        js = [js; row_js]
        dats = [dats; row_dat / ds^2]

        row_dat, row_js = interpolation_row(real(z_bck), imag(z_bck), m, m, MinvT, grd)
        is = [is; k*ones(Int64, m^2)]
        js = [js; row_js]
        dats = [dats; row_dat / ds^2]

        # ks, C = bilinear_coefficients(real(z_fwd), imag(z_fwd), grd)
        # is = [is; k*ones(Int64, 4)]
        # js = [js; ks]
        # dats = [dats; C / ds^2]
        #
        # ks, C = bilinear_coefficients(real(z_bck), imag(z_bck), grd)
        # is = [is; k*ones(Int64, 4)]
        # js = [js; ks]
        # dats = [dats; C / ds^2]

    end

    return sparse(is, js, dats, grd.Nk, grd._Nx*grd._Ny) * transpose(grd.Proj)
end

function derivative_1d(N, n, m)

    # make stencil
    ic = Int(ceil(m/2.0))
    M = zeros((m, m))
    for i=1:m
        for j=1:m
            M[i, j] = (i - ic)^(j-1) / factorial(j-1)
        end
    end
    stencil = inv(M)[n+1,:]

    D = sparse(zeros(Float64, (N, N)))
    for i=1:m
        k = i - ic
        vec = stencil[i] * ones(N - abs(k))
        D += spdiagm(k => vec)
    end

    return D
end

function mixed_derivative(nx, ny, m, grd)

    operator_to_grid(grd) do
        Dx = derivative_1d(grd._Nx, nx, m)
        Dy = derivative_1d(grd._Ny, ny, m)
        kron(Dy, Dx) / grd.dr^(nx + ny)
    end
end

function hyperdiffusion(n, mu, grd)

    operator_to_grid(grd) do
        Dx = derivative_1d(grd._Nx, 2*n, 2*n+1)
        Dy = derivative_1d(grd._Ny, 2*n, 2*n+1)
        Ix = sparse(I, grd._Nx, grd._Nx)
        Iy = sparse(I, grd._Ny, grd._Ny)

        I + (-1)^n*mu*(kron(Iy, Dx) + kron(Dy, Ix))
    end

end

function direct_solve(A, b, bval, extr)

    Anew, bnew = extr(A, b, bval)
    bnew = transpose(A)*b
    Anew = transpose(A)*A
    return extr(Anew \ bnew, bval)
end

function alfven_wave_update!(u, v, Dss, va, Dh, extr, p1, p2)

    bval = zeros(size(u))
    v_t = va^2 * Dss * u
    v[:] = (v + p1.*v_t + p2.*extr(v, bval)) ./ (1 .+ p2)
    v[:] = direct_solve(Dh, v, bval, extr)
    u[:] = (u + p1.*v + p2.*extr(u, bval)) ./ (1 .+ p2)
    u[:] = direct_solve(Dh, u, bval, extr)
end


using AdFem

function plot_cond(iter, k_chip_nodes_pred)

    k_chip_nodes = eval_f_on_fem_pts(k_exact,mesh)[chip_fem_idx_nodes]
    k_chip_nodes_out = k_chip_nodes_pred

    k_all  = k_mold * ones(nnode)
    k_all[chip_fem_idx_nodes] .= k_chip_nodes

    k_all_out = k_mold * ones(nnode)
    k_all_out[chip_fem_idx_nodes] .= k_chip_nodes_out

    figure(figsize=(15,4))
    subplot(131)
    visualize_scalar_on_fem_points(k_all, mesh, vmin=minimum(k_chip_nodes), vmax=maximum(k_chip_nodes));
    xlim(chip_left, chip_right); ylim(chip_top, chip_bottom)
    title("exact solid conductivity")

    subplot(132)
    visualize_scalar_on_fem_points(k_all_out, mesh, vmin=minimum(k_chip_nodes), vmax=maximum(k_chip_nodes));
    xlim(chip_left, chip_right); ylim(chip_top, chip_bottom)
    title("predicted solid conductivity")

    subplot(133)
    visualize_scalar_on_fem_points(k_all_out .- k_all, mesh)
    xlim(chip_left, chip_right); ylim(chip_top, chip_bottom)
    title("difference in solid conductivity")

    tight_layout()
    savefig("fn$trialnum/nn_cond$iter.png")

end

function plot_velo_pres_temp_cond(iter)
    S = run(sess, S_computed)
    u_out, v_out, p_out, T_out = S[1:nnode],
                                 S[ndof+1:ndof+nnode],
                                 S[2*ndof+1:2*ndof+nelem],
                                 S[2*ndof+nelem+1:2*ndof+nelem+nnode]

    u_obs, v_obs, p_obs, T_obs = S_data[1:nnode],
                                 S_data[ndof+1:ndof+nnode],
                                 S_data[2*ndof+1:2*ndof+nelem],
                                 S_data[2*ndof+nelem+1:2*ndof+nelem+nnode]

    figure(figsize=(15,4))
    subplot(131)
    title("exact x velocity")
    visualize_scalar_on_fem_points(u_obs .* u_std, mesh)
    subplot(132)
    title("predicted x velocity")
    visualize_scalar_on_fem_points(u_out .* u_std, mesh)
    subplot(133)
    title("difference in x velocity")
    visualize_scalar_on_fem_points(u_out .* u_std .- u_obs .* u_std, mesh)
    tight_layout()
    savefig("fn$trialnum/nn_velox$iter.png")

    figure(figsize=(15,4))
    subplot(131)
    title("exact y velocity")
    visualize_scalar_on_fem_points(v_obs .* u_std, mesh)
    subplot(132)
    title("predicted y velocity")
    visualize_scalar_on_fem_points(v_out .* u_std, mesh)
    subplot(133)
    title("difference in y velocity")
    visualize_scalar_on_fem_points(v_out .* u_std .- v_obs .* u_std, mesh)
    tight_layout()
    savefig("fn$trialnum/nn_veloy$iter.png")

    figure(figsize=(15,4))
    subplot(131)
    title("exact pressure")
    visualize_scalar_on_fvm_points(p_obs .* p_std, mesh)
    subplot(132)
    title("predicted pressure")
    visualize_scalar_on_fvm_points(p_out .* p_std, mesh)
    subplot(133)
    title("difference in pressure")
    visualize_scalar_on_fvm_points(p_out .* p_std .- p_obs .* p_std,  mesh)
    tight_layout()
    savefig("fn$trialnum/nn_pres$iter.png")

    figure(figsize=(15,4))
    subplot(131)
    title("exact temperature")
    visualize_scalar_on_fem_points(T_obs .* T_infty .+ T_infty, mesh)
    subplot(132)
    title("predicted temperature")
    visualize_scalar_on_fem_points(T_out .* T_infty .+ T_infty, mesh)
    subplot(133)
    title("difference in temperature")
    visualize_scalar_on_fem_points(T_out  .* T_infty .- T_obs .* T_infty, mesh)
    tight_layout()
    savefig("fn$trialnum/nn_temp$iter.png")

    #---------------------------------------------------------------------------
    k_chip_nodes = eval_f_on_fem_pts(k_exact,mesh)[chip_fem_idx_nodes]
    # k_chip_nodes_out = eval_f_on_fem_pts(k_nn,mesh)[chip_fem_idx_nodes]

    # xy = mesh.nodes
    # xy2 = zeros(mesh.nedge, 2)
    # for i = 1:mesh.nedge
    #     xy2[i,:] = (mesh.nodes[mesh.edges[i,1], :] + mesh.nodes[mesh.edges[i,2], :])/2
    # end
    # xy = [xy;xy2]

    # x, y = xy[chip_fem_idx_nodes, 1], xy[chip_fem_idx_nodes, 2]
    k_chip_nodes_out = run(sess, k_chip)[1:length(chip_fem_idx_nodes)]

    k_all  = k_mold * ones(nnode)
    k_all[chip_fem_idx_nodes] .= k_chip_nodes

    k_all_out = k_mold * ones(nnode)
    k_all_out[chip_fem_idx_nodes] .= k_chip_nodes_out

    figure(figsize=(15,4))
    subplot(131)
    visualize_scalar_on_fem_points(k_all, mesh, vmin=minimum(k_chip_nodes), vmax=maximum(k_chip_nodes));
    xlim(chip_left, chip_right); ylim(chip_top, chip_bottom)
    title("exact solid conductivity")

    subplot(132)
    visualize_scalar_on_fem_points(k_all_out, mesh, vmin=minimum(k_chip_nodes), vmax=maximum(k_chip_nodes));
    xlim(chip_left, chip_right); ylim(chip_top, chip_bottom)
    title("predicted solid conductivity")

    subplot(133)
    visualize_scalar_on_fem_points(k_all_out .- k_all, mesh)
    xlim(chip_left, chip_right); ylim(chip_top, chip_bottom)
    title("difference in solid conductivity")

    tight_layout()
    savefig("fn$trialnum/nn_cond$iter.png")

end

function pix_plot_velo_pres_temp_cond(iter)

    S = run(sess, S_computed)
    u_out, v_out, p_out, T_out = S[1:nnode],
                                 S[ndof+1:ndof+nnode],
                                 S[2*ndof+1:2*ndof+nelem],
                                 S[2*ndof+nelem+1:2*ndof+nelem+nnode]

    u_obs, v_obs, p_obs, T_obs = S_data[1:nnode],
                                 S_data[ndof+1:ndof+nnode],
                                 S_data[2*ndof+1:2*ndof+nelem],
                                 S_data[2*ndof+nelem+1:2*ndof+nelem+nnode]

    figure(figsize=(15,4))
    subplot(131)
    title("exact x velocity")
    visualize_scalar_on_fem_points(u_obs .* u_std, mesh)
    subplot(132)
    title("predicted x velocity")
    visualize_scalar_on_fem_points(u_out .* u_std, mesh)
    subplot(133)
    title("difference in x velocity")
    visualize_scalar_on_fem_points(u_out .* u_std .- u_obs .* u_std, mesh)
    tight_layout()
    savefig("fn$trialnum/pix_velox$iter.png")

    figure(figsize=(15,4))
    subplot(131)
    title("exact y velocity")
    visualize_scalar_on_fem_points(v_obs .* u_std, mesh)
    subplot(132)
    title("predicted y velocity")
    visualize_scalar_on_fem_points(v_out .* u_std, mesh)
    subplot(133)
    title("difference in y velocity")
    visualize_scalar_on_fem_points(v_out .* u_std .- v_obs .* u_std, mesh)
    tight_layout()
    savefig("fn$trialnum/pix_veloy$iter.png")

    figure(figsize=(15,4))
    subplot(131)
    title("exact pressure")
    visualize_scalar_on_fvm_points(p_obs .* p_std, mesh)
    subplot(132)
    title("predicted pressure")
    visualize_scalar_on_fvm_points(p_out .* p_std, mesh)
    subplot(133)
    title("difference in pressure")
    visualize_scalar_on_fvm_points(p_out .* p_std .- p_obs .* p_std,  mesh)
    tight_layout()
    savefig("fn$trialnum/pix_pres$iter.png")

    figure(figsize=(15,4))
    subplot(131)
    title("exact temperature")
    visualize_scalar_on_fem_points(T_obs .* T_infty .+ T_infty, mesh)
    subplot(132)
    title("predicted temperature")
    visualize_scalar_on_fem_points(T_out .* T_infty .+ T_infty, mesh)
    subplot(133)
    title("difference in temperature")
    visualize_scalar_on_fem_points(T_out  .* T_infty .- T_obs .* T_infty, mesh)
    tight_layout()
    savefig("fn$trialnum/pix_temp$iter.png")

    #---------------------------------------------------------------------------
    k_chip_nodes = eval_f_on_fem_pts(k_exact,mesh)[chip_fem_idx_nodes]

    xy = mesh.nodes
    xy2 = zeros(mesh.nedge, 2)
    for i = 1:mesh.nedge
        xy2[i,:] = (mesh.nodes[mesh.edges[i,1], :] + mesh.nodes[mesh.edges[i,2], :])/2
    end
    xy = [xy;xy2]

    x, y = xy[chip_fem_idx_nodes, 1], xy[chip_fem_idx_nodes, 2]
    k_chip_nodes_out = run(sess, k_chip[1:length(chip_fem_idx_nodes)])

    k_all  = k_mold * ones(nnode)
    k_all[chip_fem_idx_nodes] .= k_chip_nodes

    k_all_out = k_mold * ones(nnode)
    k_all_out[chip_fem_idx_nodes] .= k_chip_nodes_out

    figure(figsize=(15,4))
    subplot(131)
    visualize_scalar_on_fem_points(k_all, mesh, vmin=minimum(k_chip_nodes), vmax=maximum(k_chip_nodes));
    xlim(chip_left, chip_right); ylim(chip_top, chip_bottom)
    title("exact solid conductivity")

    subplot(132)
    visualize_scalar_on_fem_points(k_all_out, mesh, vmin=minimum(k_chip_nodes), vmax=maximum(k_chip_nodes));
    xlim(chip_left, chip_right); ylim(chip_top, chip_bottom)
    title("predicted solid conductivity")

    subplot(133)
    visualize_scalar_on_fem_points(k_all_out .- k_all, mesh)
    xlim(chip_left, chip_right); ylim(chip_top, chip_bottom)
    title("difference in solid conductivity")

    tight_layout()
    savefig("fn$trialnum/pix_cond$iter.png")

end


my_f(x,y) = 2*x+3*y

derivative_of_my_f(x,y) = ForwardDiff.derivative(x->my_f(x,y),x)


import Base.show
import Base.==
import Base.isequal
import SymPy.solve
import SymPy.degree
import SymPy.subs

coeff(x::Sym, y::Sym) = x.coeff(y)
is_polynomial(x::Sym, y::Sym) = x.is_polynomial(y)
Poly(x::Sym, y::Sym) = sympy.Poly(x, y)

abstract type Recurrence end

struct CFiniteRecurrence <: Recurrence
    coeffs::Array{Sym}
    f::SymFunction
    n::Sym
    inhom::Sym
end

CFiniteRecurrence(coeffs::Array{Sym}, f::SymFunction, n::Sym) = CFiniteRecurrence(coeffs, f, n, Sym(0))
CFiniteRecurrence(coeffs::Array{Sym}, f::SymFunction, n::Symbol) = CFiniteRecurrence(coeffs, f, Sym(n))

function order(r::Recurrence)
    return length(r.coeffs) - 1
end

function relation(r::Recurrence)
    return hompart(r) + r.inhom
end

function Base.show(io::IO, r::Recurrence)
    print(io, "$(lhs(r)) = $(rhs(r))")
end

function hompart(r::Recurrence)
    return sum([c * r.f(r.n + (i - 1)) for (i, c) in enumerate(r.coeffs)])
end

function is_homogeneous(r::Recurrence)
    return simplify(r.inhom) == 0
end

function rhs(r::Recurrence)
    if length(r.coeffs) == 1
        return -r.inhom
    end
    simplify(-sum([c * r.f(r.n + (i - 1)) for (i, c) in enumerate(r.coeffs[1:end-1])]) - r.inhom)
end

function lhs(r::Recurrence)
    t = r.f(r.n + order(r))
    if length(r.coeffs) > 1
        t *= r.coeffs[end]
    end
    t
end

function homogeneous(r::Recurrence)
    n = r.n
    if is_homogeneous(r)
        return r
    elseif is_polynomial(r.inhom, r.n)
        d = convert(Int64, degree(Poly(r.inhom, n)))
        res = relation(r)
        for i in 1:d + 1
            res = (res |> subs(n, n + 1)) - res
        end
        res = simplify(res)

        idx = n + order(r) + d + 1
        c = coeff(res, r.f(idx))
        return eq2rec(simplify(res / c), r.f, r.n)
    else
        res = hompart(r) / r.inhom
        res = expand((res |> subs(n, n + 1)) - res)
        idx = n + order(r) + 1
        c = coeff(res, r.f(idx))
        return eq2rec(simplify(res / c), r.f, r.n)
    end
end

function rewrite(d::Dict{Sym,Sym})
    # TODO: successively apply rewrite rules do values of dict
    dd = Dict{Sym,Sym}()
    while dd != d
        dd = d
        for (k, v) in d
            for (g, h) in dd
                dd[g] = h |> subs(k, v)
            end
        end
    end
    dd
end

function closedform(orig::CFiniteRecurrence)
    # println("Original: ", orig)
    r = homogeneous(orig)
    # println("Homogeneous: ", r)

    shift = order(r) - order(orig)
    rh = rhs(orig)
    ord = order(orig)
    init = Dict{Sym,Sym}([(orig.f(i), SymPy.replace(rh, orig.n, i-ord)) for i in ord:shift+ord-1])
    # init = rewrite(init)

    rel = relation(r)
    # println("Homogeneous: ", homogeneous(r))
    w0  = Wild("w0")
    @syms lbd
    cpoly   = SymPy.replace(rel, r.f(r.n), 1)
    cpoly   = SymPy.replace(cpoly, r.f(r.n + w0), lbd^w0)
    # println("CPoly: ", cpoly |> simplify)
    # factors = factor_list(cpoly)
    proots   = roots(cpoly)
    # d = hcat([[uniquevar() * r.n^i, z^r.n] for (z, m) in roots for i in 0:m - 1])
    # println(d[:,2])
    ansatz = sum([sum([uniquevar() * r.n^i * z^r.n for i in 0:m - 1]) for (z, m) in proots])
    # println(ansatz)
    # println(free_symbols(ansatz(n)))
    unknowns = filter(e -> e != r.n, free_symbols(ansatz))
    system = [Eq(r.f(i), ansatz |> subs(r.n, i)) for i in 0:order(r) - 1]
    sol = solve(system, unknowns)
    sol = ansatz |> subs(sol)
    if !isempty(init)
        tmp = nothing
        while true
            tmp = (sol |> subs(init)) |> simplify
            if tmp == sol
                break
            end
            sol = tmp

        end
        sol = simplify(tmp)
    end

    exp = [z for (z, _) in proots]
    exp = filter(x -> x!=Sym(1), exp)
    push!(exp, Sym(1))
    coeff = exp_coeffs(sol, [z^r.n for z in exp])
    return CFiniteClosedForm(r.f, r.n, exp, coeff)
end

function exp_coeffs(expr::Sym, exp::Array{Sym,1})
    # assume if 1 in exp then it is at the end
    coeffs = Sym[]
    for ex in exp
        if ex == Sym(1)
            push!(coeffs, expr)
        else
            c = coeff(expr, simplify(ex))
            push!(coeffs, c)
            expr = simplify(expr - c*ex)
        end
    end
    coeffs
end

function rec_dependency(recs::Array{<: Recurrence,1})
    rec_dependency([], [], recs...)
end

function rec_dependency(indep, dep, rec::Recurrence, recs::Recurrence...)
    expr = rhs(rec)
    fns = symfunctions(expr)
    depvars = filter(x -> (x.func!=Sym(rec.f.x) && !in(Sym(0), x.args)), fns)

    if isempty(depvars)
        push!(indep, rec)
    else
        push!(dep, (rec, depvars))
    end
    rec_dependency(indep, dep, recs...)
end

function rec_dependency(indep, dep)
    indep, dep
end

function subs(cf::ClosedForm, r::CFiniteRecurrence)
    rel = relation(r)
    rhs = poly(cf)
    fns = symfunctions(rel)
    for fn in fns
        if fn.func == Sym(cf.f.x)
            w0 = Wild("w0")
            idx = match(cf.f(cf.n + w0), fn)[w0]
            sub = rhs |> SymPy.subs(cf.n, cf.n + idx)
            rel = rel |> SymPy.subs(cf.f(cf.n + idx), sub) |> simplify
        end
    end
    return eq2rec(rel, r.f, r.n)
end

function rec_solve(recsorig::Array{<: Recurrence,1})
    # println("Recurrences: ", recs)
    if length(recsorig) == 0
        return ClosedForm[]
    end

    varlist = [(r.f(r.n), r.f) for r in recsorig]
    recs, varlist = rec_simplify(Recurrence[], varlist, recsorig...)
    # println("Simplifed recurrences: ", recs)
    # println("Varlist: ", varlist)
    # recs = loop.body
    solved = ClosedForm[]
    solvable = true
    while solvable
        indep, dep = rec_dependency(recs)
        indepcnt = length(indep)
        depcnt = length(dep)

        if indepcnt == 0 && depcnt != 0
            error("Illegal coupling detected in loop body")
        elseif indepcnt == 0 && depcnt == 0
            break
        end

        recs = setdiff(recs, indep)
        for rec in indep
            cf = closedform(rec)

            # TODO: deal with the case when a recurrence is not solvable

            push!(solved, cf)
            recs = filter(x -> x!=rec, recs)

            recs = [subs(cf, r) for r in recs]
        end
        if isempty(recs)
            break
        end
    end

    inivars = []
    for cf in solved
        inivars = [inivars; initvars(cf)]
    end
    inivars = [(SymFunction(string(Sym(x.func))), x.args[1]) for x in inivars]
    inivars = filter(x -> x[2]>0, inivars)
    iniexpr = init_expr(recsorig, inivars)
    # println("Initial expression: ", iniexpr)

    for var in varlist
        # TODO: assume var is something like (t(n-1), x)
        fn = SymFunction(string(var[1].func))
        if fn != var[2]
            for cf in solved
                if cf.f == fn
                    sh = args(var[1])[1] - cf.n
                    tmp = shift(cf, sh)
                    tmp.f = var[2]
                    push!(solved, tmp)
                    break
                end
            end
        end
    end
    [subs!(cf, iniexpr...) for cf in solved]
end

function ==(f::SymFunction, g::SymFunction)
    isequal(Sym(f.x), Sym(g.x))
end

function init_expr(recs::Array{<:Recurrence,1}, vars::Array{Tuple{SymFunction,Sym},1})
    initrules = Pair[]
    for (var, idx) in vars
        rec = [r for r in recs if string(r.f) == string(var)][1]
        rhs = initial(rec, idx)
        while true
            newvars = [(SymFunction(string(Sym(t.func))), t.args[1]) for t in symfunctions(rhs) if t.args[1] > 0]
            # newvars = filter(x -> x[2]>0, newvars)
            if isempty(newvars)
                break
            end
            rules = init_expr(recs, newvars)
            rhs = (rhs |> subs(rules...)) |> simplify
        end
        push!(initrules, Pair(var(idx), rhs))
    end
    initrules
end

function initial(r::CFiniteRecurrence, n::Sym)
    if n < order(r)
        error("Cannot give expression for $(r.f(n))")
    end
    i = n - order(r)
    rhs(r) |> subs(r.n, i)
end

function initvars(r::ClosedForm)
    symfunctions(rhs(r))
end

#-------------------------------------------------------------------------------

function contains_func(expr::Sym, f::SymFunction)
    fns = symfunctions(expr)
    w0 = Wild("w0")
    args = [get(match(f(w0), fn), w0, nothing) for fn in fns]
    args = filter(x -> x!=nothing, args)
    !isempty(args)
end

function rec_subs(r::Recurrence, rec::Recurrence)
    # println("subs $(rec) in $(r)")
    expr = rhs(r)
    fns = symfunctions(expr)
    w0 = Wild("w0")
    args = [get(match(rec.f(w0), fn), w0, nothing) for fn in fns]
    args = filter(x -> x!=nothing, args)
    if isempty(args)
        return r
    end
    for arg in args
        diff = rec.n + 1 - arg
        sub = rhs(rec) |> subs(rec.n, rec.n - diff)
        expr = expr |> subs(rec.f(arg), sub)
    end
    # assume lhs of rec is always f(n+1) (just temporarily)
    # println("result: ", expr)
    eq2rec(r.f(r.n+1) - simplify(expr), r.f, r.n)
end

function rec_subs(expr::Sym, rec::Recurrence)
    # println("subs $(rec) in $(r)")
    # expr = rhs(r)
    fns = symfunctions(expr)
    w0 = Wild("w0")
    args = [get(match(rec.f(w0), fn), w0, nothing) for fn in fns]
    args = filter(x -> x!=nothing, args)
    for arg in args
        diff = rec.n + 1 - arg
        sub = rhs(rec) |> subs(rec.n, rec.n - diff)
        expr = expr |> subs(rec.f(arg), sub)
    end
    simplify(expr)
end

# function rec_simplify(recs::Array{<:Recurrence,1})
#     # processed = Recurrence[]
#     # for rec in recs
#     #     expr = rhs(rec)
#     #     # fns = symfunctions(expr)
#     #     # w0 = Wild("w0")
#     #     # args = [match(rec.f(w0), expr)[w0] for fn in fns]
#     #     if !contains_func(expr, rec.f) # not a real recurrence
#     #         processed = [rec_subs(r, rec) for r in processed]
#     #         recs = [rec_subs(r, rec) for r in recs]
#     #     else
#     #         push!(processed, rec)
#     #     end
#     # end
#     # processed
#     rec_simplify(Recurrence[], recs...)
# end

function rec_simplify(processed::Array{<:Recurrence}, varlist::Array{Tuple{Sym,SymFunction},1}, rec::Recurrence, recs::Recurrence...)
    expr = rhs(rec)
    # fns = symfunctions(expr)
    # w0 = Wild("w0")
    # args = [match(rec.f(w0), expr)[w0] for fn in fns]
    if !contains_func(expr, rec.f) # not a real recurrence
        varlist = [var_simplify(var, rec) for var in varlist]
        processed = [rec_subs(r, rec) for r in processed]
        recs = [rec_subs(r, rec) for r in recs]
    else
        push!(processed, rec)
    end
    rec_simplify(processed, varlist, recs...)
end

function rec_simplify(processed::Array{<:Recurrence}, varlist::Array{Tuple{Sym,SymFunction},1})
    processed, varlist
end

function var_simplify(var::Tuple{Sym,SymFunction}, rec::Recurrence)
    rec_subs(var[1], rec), var[2]
end


using Test
using PolynomialBasis
using ImplicitDomainQuadrature
# using Revise
using CutCellDG
include("../useful_routines.jl")

polyorder = 1
numqp = 2

basis = LagrangeTensorProductBasis(2, polyorder)
x0 = [0.0, 0.0]
widths = [3.0, 1.0]
nelements = [3, 1]
dim, nodesperelement = size(interpolation_points(basis))
mesh = CutCellDG.CGMesh([0.0, 0.0], [3.0, 1.0], [3, 1], nodesperelement)
normal = [1.0, 0.0]
x0 = [0.5, 0.0]
levelset =
    CutCellDG.LevelSet(x -> plane_distance_function(x, normal, x0), mesh, basis)
nodalcoordinates = CutCellDG.nodal_coordinates(mesh)

tol = 1e-3
perturbation = 0.0

cellsign = CutCellDG.cell_sign!(levelset, tol, perturbation)
@test allequal(cellsign, [0, 1, 1])

normal = [1.0, 0.0]
x0 = [1.0, 0.0]
tol = 1e-3
perturbation = 1e-2
levelset =
    CutCellDG.LevelSet(x -> plane_distance_function(x, normal, x0), mesh, basis)
cellsign = CutCellDG.cell_sign!(levelset, tol, perturbation)

@test allapprox(
    CutCellDG.coefficients(levelset, 1),
    [-1.0 + perturbation, -1.0 + perturbation, perturbation, perturbation],
)
@test allapprox(
    CutCellDG.coefficients(levelset, 2),
    [perturbation, perturbation, 1.0, 1.0],
)
@test allapprox(CutCellDG.coefficients(levelset, 3), [1.0, 1.0, 2.0, 2.0])

basis = HermiteTensorProductBasis(2)
normal = [1.0, 0.0]
x0 = [1.0, 0.0]
tol = 1e-3
perturbation = 1e-2
levelset = CutCellDG.LevelSet(
    x -> plane_distance_function(x, normal, x0),
    mesh,
    basis,
)
cellsign = CutCellDG.cell_sign!(levelset,tol,perturbation)
testcellsign = [0,1,1]
@test allequal(testcellsign,cellsign)
coeffs1 = reshape(CutCellDG.coefficients(levelset,1),4,:)
jac = CutCellDG.jacobian(mesh)
testcoeffs1 = [-1+perturbation   -1+perturbation   perturbation   perturbation
                0.0               0.0              0.0            0.0
                jac[1]            jac[1]           jac[1]         jac[1]
                0.0               0.0              0.0            0.0]
@test allapprox(coeffs1,testcoeffs1,1e4eps())
coeffs2 = reshape(CutCellDG.coefficients(levelset,2),4,:)
testcoeffs2 = [perturbation    perturbation   1.0            1.0
               0.0             0.0            0.0            0.0
               jac[1]          jac[1]         jac[1]         jac[1]
               0.0             0.0            0.0            0.0]
@test allapprox(coeffs2,testcoeffs2,1e4eps())
coeffs3 = reshape(CutCellDG.coefficients(levelset,3),4,:)
testcoeffs3 = [1.0     1.0    2.0    2.0
               0.0     0.0    0.0    0.0
               jac[1]  jac[1] jac[1] jac[1]
               0.0     0.0    0.0    0.0]
@test allapprox(coeffs3,testcoeffs3,1e4eps())

basis = LagrangeTensorProductBasis(2,1)
refpoints = interpolation_points(basis)
dgmesh = CutCellDG.DGMesh(x0,widths,nelements,refpoints)
posactivenodeids = CutCellDG.active_node_ids(dgmesh,+1,cellsign)
@test allequal(posactivenodeids, 1:12)
negactivenodeids = CutCellDG.active_node_ids(dgmesh,-1, cellsign)
@test allequal(negactivenodeids, 1:4)

totalnumnodes = CutCellDG.number_of_nodes(dgmesh)
cutmeshnodeids =
    CutCellDG.cut_mesh_nodeids(posactivenodeids, negactivenodeids, totalnumnodes)
testcutmeshnodeids = [
    1 2 3 4 5 6 7 8 9 10 11 12
    13 14 15 16 0 0 0 0 0 0 0 0
]
@test allequal(cutmeshnodeids, testcutmeshnodeids)

cutmesh = CutCellDG.CutMesh(dgmesh, levelset)
@test allequal(CutCellDG.nodal_connectivity(cutmesh, +1, 1), [1, 2, 3, 4])
@test allequal(CutCellDG.nodal_connectivity(cutmesh, -1, 1), [13, 14, 15, 16])