@doc raw"""
    JS_BWG_HNE{K}(α,ωₚ,γ,r,ϕₘ,β,ν,σₗ,σᵣ)
    JS_BWG_HNE{K}(x)

9 parameter model for vertical, northward and eastward displacement of a partical in waves with JONSWAP marginal spectra and bimodal wrapped Gaussian spreading.

# Type parameter
The type parameter `K` is a non-negative integer representing the ammount of aliasing to be done.
This is useful as some devices have high-pass filters which limit the ammount of aliasing that will be present in a recorded series.
In particular, a `JS_BWG_HNE{K}` model for a series recorded every `Δ` seconds would be appropriate for a series sampled every `Δ` seconds and filtered with a high-pass filter at `(2K-1)π/Δ`.

# Arguments
- `α`: The scale parameter.
- `ωₚ`: The peak angular frequency.
- `γ`: The peak enhancement factor.
- `r`: The tail decay index.
- `ϕₘ`: The mean direction.
- `β`: The limiting peak separation.
- `ν`: The peak separation shape.
- `σₗ`: The limiting angular width.
- `σᵣ`: The angular width shape.

# Vector constructor arguments
- `x`: Vector of parameters `[α,ωₚ,γ,r,ϕₘ,β,ν,σₗ,σᵣ]`.

# Background
The model is a transformation of a model for the frequency-direction spectra of a wave process.
Such a frequency-direction spectra, denoted ``f(ω,ϕ)`` is typically decomposed by writing:
```math
f(ω,ϕ) = f(ω)D(ω,ϕ)
```
where ``f(ω)`` is the marginal spectral density function and ``D(ω,ϕ)`` is the spreading function.
This model uses a JONSWAP marginal sdf and a bimodal wrapped Gaussian spreading function.
The JONSWAP spectral density function is
```math
f(ω) = αω^{-r}\exp\left \{-\frac{r}{4}\left(\frac{|ω|}{ωₚ}\right)^{-4}\right \}γ^{δ(|ω|)}
```
where
```math
δ(ω) = \exp\left\{-\tfrac{1}{2 (0.07+0.02\cdot\mathbb{1}_{ωₚ>|ω|})^2}\left (\tfrac{|ω|}{ωₚ}-1\right )^2\right\}.
```
The bimodal wrapped Gaussian spreading function is
```math
D(ω,ϕ) = \frac{1}{2σ(ω)\sqrt{2π}}\sum\limits_{k=-∞}^{∞} \sum\limits_{i=1}^{2} \exp\left\{-\frac{1}{2}\left(\frac{ϕ-ϕ_{mi}(ω)-2\pi k}{σ(ω)}\right)^2\right\}
```
where
```math
\begin{align*}
    ϕ_{m1}(ω) &= ϕ_m + β \exp\{-ν \min(ω_p/|ω|,1)\}/2 \\
    ϕ_{m2}(ω) &= ϕ_m - β \exp\{-ν \min(ω_p/|ω|,1)\}/2 \\
    σ(ω) &= σ_l - \frac{σ_r}{3}\left\{ 4\left(\frac{ω_p}{|ω|}\right)^2 - \left(\frac{ω_p}{|ω|}\right)^8 \right\}
\end{align*}
```

"""
struct JS_BWG_HNE{K} <: UnknownAcvTimeSeriesModel{3,Float64}
    α::Float64
    ωₚ::Float64
    γ::Float64
    r::Float64
    ϕₘ::Float64
    β::Float64
    ν::Float64
    σₗ::Float64
    σᵣ::Float64

    r_over4::Float64
    ωₚ²::Float64
    ωₚ³::Float64
    ωₚ⁴::Float64
    ωₚ⁶::Float64
    logγ::Float64
    σᵣ_over3::Float64
    cosϕₘ::Float64
    sinϕₘ::Float64
    cos2ϕₘ::Float64
    sin2ϕₘ::Float64
    function JS_BWG_HNE{K}(α,ωₚ,γ,r,ϕₘ,β,ν,σₗ,σᵣ) where {K}
        α > 0 || throw(ArgumentError("JS_BWG_HNE requires α > 0"))
        ωₚ > 0 || throw(ArgumentError("JS_BWG_HNE requires ωₚ > 0"))
        γ >= 1 || throw(ArgumentError("JS_BWG_HNE requires γ > 1"))
        r > 1 || throw(ArgumentError("JS_BWG_HNE requires r > 1"))
        β >= 0 || throw(ArgumentError("JS_BWG_HNE requires β > 0"))
        ν >= 0 || throw(ArgumentError("JS_BWG_HNE requires ν > 0"))
        σₗ >= 0 || throw(ArgumentError("JS_BWG_HNE requires σₗ > 0"))
        σᵣ >= 0 || throw(ArgumentError("JS_BWG_HNE requires σᵣ > 0"))
        new{K}(α,ωₚ,γ,r,ϕₘ,β,ν,σₗ,σᵣ,
        r/4,ωₚ^2,ωₚ^3,ωₚ^4,ωₚ^6,log(γ),
        σᵣ/3,cos(ϕₘ),sin(ϕₘ),cos(2ϕₘ),sin(2ϕₘ)
        )
    end
    function JS_BWG_HNE{K}(x::AbstractVector{Float64}) where {K}
        @boundscheck checkparameterlength(x,JS_BWG_HNE{K})
        @inbounds JS_BWG_HNE{K}(x[1], x[2], x[3], x[4], x[5], x[6], x[7], x[8], x[9])
    end
end

# functions to throw informative error if type parameter not provided
JS_BWG_HNE(x::AbstractVector{Float64}) = JS_BWG_HNE(ones(9)...)
JS_BWG_HNE(α,ωₚ,γ,r,ϕₘ,β,ν,σₗ,σᵣ) = error("JS_BWG_HNE process requires the ammount of aliasing specified as a type parameter. Use JS_BWG_HNE{K}() where K ∈ N₀.")

WhittleLikelihoodInference.npars(::Type{JS_BWG_HNE{K}}) where {K} = 9
WhittleLikelihoodInference.nalias(::JS_BWG_HNE{K}) where {K} = K

WhittleLikelihoodInference.lowerbounds(::Type{JS_BWG_HNE{K}}) where {K} = [0,0,1,1,-Inf,0,0,0,0]
WhittleLikelihoodInference.upperbounds(::Type{JS_BWG_HNE{K}}) where {K} = [Inf,Inf,Inf,Inf,Inf,Inf,Inf,Inf,Inf]

@propagate_inbounds @fastmath function WhittleLikelihoodInference.add_sdf!(out, model::JS_BWG_HNE{K}, ω::Real) where {K}
    @boundscheck checkbounds(out,1:6)
    @inbounds begin
        s_om = sign(ω)
        ω = abs(ω)

        if ω > 1e-10
            α,ωₚ,γ,r,β,ν,σₗ = model.α,model.ωₚ,model.γ,model.r,model.β,model.ν,model.σₗ

            σ1² = 0.0049 + 0.0032 * (ω > ωₚ)
            ω_over_ωₚ = ω / ωₚ
            ωₚ_over_ω = 1/ω_over_ωₚ
            δ = exp(-1 / (2σ1²) * (ω_over_ωₚ - 1)^2)
            ω⁻⁴ = ω^(-4)
            ωₚ⁴_over_ω⁴ = model.ωₚ⁴ * ω⁻⁴
            sdf = (α * ω^(-r) * exp(-(model.r_over4) * ωₚ⁴_over_ω⁴) * γ^δ) / 2

            PeakSep = β / exp(ν * ((ω > ωₚ)*((ωₚ_over_ω) - 1.0) + 1.0))
            σ = σₗ - model.σᵣ_over3*(4*(ωₚ_over_ω)^2 - (ωₚ⁴_over_ω⁴)^2)
            σ² = σ^2

            cosPS = cos(PeakSep)
            exp2part = (2 * exp(2.0 * σ²))
            cos2part = (model.cos2ϕₘ*cosPS) / exp2part
            imexppart = sdf * 1im * s_om / exp(0.5 * σ²) * cos(PeakSep/2)

            out[1] += sdf
            out[2] += imexppart * model.cosϕₘ
            out[3] += imexppart * model.sinϕₘ
            out[4] += sdf * (0.5 + cos2part)
            out[5] += sdf * model.sin2ϕₘ * cosPS / exp2part
            out[6] += sdf * (0.5 - cos2part)
        end # add zero otherwise
    end
    return nothing
end

@propagate_inbounds @fastmath function WhittleLikelihoodInference.grad_add_sdf!(out, model::JS_BWG_HNE{K}, ω::Real) where {K}
    @boundscheck checkbounds(out,1:6,1:9)
    @inbounds begin
        s_om = sign(ω)
        ω = abs(ω)

        if ω > 1e-10
            α,ωₚ,γ,r,β,ν,σₗ,σᵣ = model.α,model.ωₚ,model.γ,model.r,model.β,model.ν,model.σₗ,model.σᵣ

            σ1² = 0.0049 + 0.0032 * (ω > ωₚ)
            ω_over_ωₚ = ω / ωₚ
            ωₚ_over_ω = 1/ω_over_ωₚ
            δ = exp(-1 / (2σ1²) * (ω_over_ωₚ - 1)^2)
            ω⁻⁴ = ω^(-4)
            ωₚ⁴_over_ω⁴ = model.ωₚ⁴ * ω⁻⁴
            sdf = (α * ω^(-r) * exp(-(model.r_over4) * ωₚ⁴_over_ω⁴) * γ^δ) / 2

            ∂S∂α = sdf / α
            ∂S∂ωₚ = sdf * (δ*model.logγ * ω / σ1² *(ω-ωₚ) / model.ωₚ³ - r*model.ωₚ³*ω⁻⁴)
            ∂S∂γ = sdf * δ / γ
            ∂S∂r = sdf * (-log(ω)-ωₚ⁴_over_ω⁴/4)

            PeakSep = β / exp(ν * ((ω > ωₚ)*((ωₚ_over_ω) - 1.0) + 1.0))
            σ = σₗ - model.σᵣ_over3*(4*(ωₚ_over_ω)^2 - (ωₚ⁴_over_ω⁴)^2)
            σ² = σ^2

            cosϕₘ = model.cosϕₘ
            sinϕₘ = model.sinϕₘ
            cos2ϕₘ = model.cos2ϕₘ
            sin2ϕₘ = model.sin2ϕₘ
            cosPShalf = cos(PeakSep/2)
            cosPS = cos(PeakSep)
            sinPShalf = sin(PeakSep/2)
            sinPS = sin(PeakSep)

            exp2part = (exp(2.0 * σ²))
            cos2part = (cos2ϕₘ*cosPS) / exp2part
            exphalf = exp(0.5 * σ²)
            imexppart = 1im * s_om / exphalf * cosPShalf

            # hzz = 1
            hxx = (0.5 + cos2part/2)
            hyy = (0.5 - cos2part/2)
            hxz = imexppart * cosϕₘ
            hyz = imexppart * sinϕₘ
            hyx = sin2ϕₘ * cosPS / exp2part / 2

            #### h_derivs
            ## ϕₘ
            hyxpart∂ϕₘ = cosPS / exp2part
            hxx∂ϕₘ = -sin2ϕₘ * hyxpart∂ϕₘ
            hyy∂ϕₘ = -hxx∂ϕₘ
            hzpart∂ϕₘ = -1im * s_om * cosPShalf / exphalf
            hxz∂ϕₘ = sinϕₘ * hzpart∂ϕₘ
            hyz∂ϕₘ = -cosϕₘ * hzpart∂ϕₘ
            hyx∂ϕₘ = cos2ϕₘ * hyxpart∂ϕₘ

            ## β, ν
            PeakSep∂β = PeakSep / β
            PeakSep∂ν = -((ω > ωₚ)*((ωₚ_over_ω) - 1.0) + 1.0) * β / exp(ν * ((ω > ωₚ)*((ωₚ_over_ω) - 1.0) + 1.0))
            hx∂βνtemp = sinPS / 2 / exp2part
            hz∂βνtemp = -0.5im * sinPShalf / exphalf * s_om

            hx∂βt = hx∂βνtemp * PeakSep∂β
            hx∂νt = hx∂βνtemp * PeakSep∂ν
            hz∂βt = hz∂βνtemp * PeakSep∂β
            hz∂νt = hz∂βνtemp * PeakSep∂ν

            hxx∂β = -cos2ϕₘ * hx∂βt
            hyy∂β = -hxx∂β
            hxz∂β = cosϕₘ * hz∂βt
            hyz∂β = sinϕₘ * hz∂βt
            hyx∂β = -sin2ϕₘ * hx∂βt

            hxx∂ν = -cos2ϕₘ * hx∂νt
            hyy∂ν = -hxx∂ν
            hxz∂ν = cosϕₘ * hz∂νt
            hyz∂ν = sinϕₘ * hz∂νt
            hyx∂ν = -sin2ϕₘ * hx∂νt

            ## σₗ, σᵣ
            # ∂σ∂σₗ = 1
            ∂σ∂σᵣ = (σ-σₗ)/σᵣ
            hx∂σtemp = cosPS * 2 / exp2part * σ
            hz∂σtemp = -1im * cosPShalf / exphalf * σ * s_om

            hx∂σᵣt = hx∂σtemp * ∂σ∂σᵣ
            hz∂σᵣt = hz∂σtemp * ∂σ∂σᵣ

            hxx∂σₗ = -cos2ϕₘ * hx∂σtemp
            hyy∂σₗ = -hxx∂σₗ
            hxz∂σₗ = cosϕₘ * hz∂σtemp
            hyz∂σₗ = sinϕₘ * hz∂σtemp
            hyx∂σₗ = -sin2ϕₘ * hx∂σtemp

            hxx∂σᵣ = -cos2ϕₘ * hx∂σᵣt
            hyy∂σᵣ = -hxx∂σᵣ
            hxz∂σᵣ = cosϕₘ * hz∂σᵣt
            hyz∂σᵣ = sinϕₘ * hz∂σᵣt
            hyx∂σᵣ = -sin2ϕₘ * hx∂σᵣt

            ## ωₚ
            PeakSep∂ωₚ = -(ω > ωₚ) * ν/ω * β * exp(-ν*ωₚ/ω)
            ∂σ∂ωₚ = (-8σᵣ/3*(ωₚ/(ω^2)-ωₚ^7/(ω^8)))

            hx∂ωₚt = (sinPS / 2 * PeakSep∂ωₚ + 2*cosPS*σ*∂σ∂ωₚ)/ exp2part
            hz∂ωₚt = -1im * (sinPShalf / 2 * PeakSep∂ωₚ + cosPShalf*σ*∂σ∂ωₚ)/ exphalf * s_om

            hxx∂ωₚ = -cos2ϕₘ * hx∂ωₚt
            hyy∂ωₚ = -hxx∂ωₚ
            hxz∂ωₚ = cosϕₘ * hz∂ωₚt
            hyz∂ωₚ = sinϕₘ * hz∂ωₚt
            hyx∂ωₚ = -sin2ϕₘ * hx∂ωₚt

            ## α
            out[1, 1] += ∂S∂α
            out[2, 1] += ∂S∂α * hxz
            out[3, 1] += ∂S∂α * hyz
            out[5, 1] += ∂S∂α * hyx
            out[4, 1] += ∂S∂α * hxx
            out[6, 1] += ∂S∂α * hyy

            ## ωₚ
            out[1, 2] += ∂S∂ωₚ
            out[2, 2] += ∂S∂ωₚ * hxz + sdf * hxz∂ωₚ
            out[3, 2] += ∂S∂ωₚ * hyz + sdf * hyz∂ωₚ
            out[4, 2] += ∂S∂ωₚ * hxx + sdf * hxx∂ωₚ
            out[5, 2] += ∂S∂ωₚ * hyx + sdf * hyx∂ωₚ
            out[6, 2] += ∂S∂ωₚ * hyy + sdf * hyy∂ωₚ

            ## γ
            out[1, 3] += ∂S∂γ
            out[2, 3] += ∂S∂γ * hxz
            out[3, 3] += ∂S∂γ * hyz
            out[4, 3] += ∂S∂γ * hxx
            out[5, 3] += ∂S∂γ * hyx
            out[6, 3] += ∂S∂γ * hyy

            ## r
            out[1, 4] += ∂S∂r
            out[2, 4] += ∂S∂r * hxz
            out[3, 4] += ∂S∂r * hyz
            out[4, 4] += ∂S∂r * hxx
            out[5, 4] += ∂S∂r * hyx
            out[6, 4] += ∂S∂r * hyy

            ## ϕₘ
            # out[1, 5] += 0
            out[2, 5] += sdf * hxz∂ϕₘ
            out[3, 5] += sdf * hyz∂ϕₘ
            out[4, 5] += sdf * hxx∂ϕₘ
            out[5, 5] += sdf * hyx∂ϕₘ
            out[6, 5] += sdf * hyy∂ϕₘ

            ## β
            # out[1, 6] += 0
            out[2, 6] += sdf * hxz∂β
            out[3, 6] += sdf * hyz∂β
            out[4, 6] += sdf * hxx∂β
            out[5, 6] += sdf * hyx∂β
            out[6, 6] += sdf * hyy∂β

            ## ν
            # out[1, 7] += 0
            out[2, 7] += sdf * hxz∂ν
            out[3, 7] += sdf * hyz∂ν
            out[4, 7] += sdf * hxx∂ν
            out[5, 7] += sdf * hyx∂ν
            out[6, 7] += sdf * hyy∂ν

            ## σₗ
            # out[1, 8] += 0
            out[2, 8] += sdf * hxz∂σₗ
            out[3, 8] += sdf * hyz∂σₗ
            out[4, 8] += sdf * hxx∂σₗ
            out[5, 8] += sdf * hyx∂σₗ
            out[6, 8] += sdf * hyy∂σₗ

            ## σᵣ
            # out[1, 9] += 0
            out[2, 9] += sdf * hxz∂σᵣ
            out[3, 9] += sdf * hyz∂σᵣ
            out[4, 9] += sdf * hxx∂σᵣ
            out[5, 9] += sdf * hyx∂σᵣ
            out[6, 9] += sdf * hyy∂σᵣ

        end # add zero otherwise
    end

    return nothing
end

@propagate_inbounds @fastmath function WhittleLikelihoodInference.hess_add_sdf!(out, model::JS_BWG_HNE{K}, ω::Real) where {K}
    @boundscheck checkbounds(out,1:6,1:45)
    @inbounds begin
        s_om = sign(ω)
        ω = abs(ω)

        if ω > 1e-10
            α,ωₚ,γ,r,β,ν,σₗ,σᵣ = model.α,model.ωₚ,model.γ,model.r,model.β,model.ν,model.σₗ,model.σᵣ

            σ1² = 0.0049 + 0.0032 * (ω > ωₚ)
            ω_over_ωₚ = ω / ωₚ
            ωₚ_over_ω = 1/ω_over_ωₚ
            δ = exp(-1 / (2σ1²) * (ω_over_ωₚ - 1)^2)
            ω⁻⁴ = ω^(-4)
            ωₚ⁴_over_ω⁴ = model.ωₚ⁴ * ω⁻⁴
            sdf = (α * ω^(-r) * exp(-(model.r_over4) * ωₚ⁴_over_ω⁴) * γ^δ) / 2

            ∂S∂α = sdf / α
            ∂S∂ωₚ = sdf * (δ*model.logγ * ω / σ1² *(ω-ωₚ) / model.ωₚ³ - r*model.ωₚ³*ω⁻⁴)
            ∂S∂γ = sdf * δ / γ
            ∂S∂r = sdf * (-log(ω)-ωₚ⁴_over_ω⁴/4)

            PeakSep = β / exp(ν * ((ω > ωₚ)*((ωₚ_over_ω) - 1.0) + 1.0))
            σ = σₗ - model.σᵣ_over3*(4*(ωₚ_over_ω)^2 - (ωₚ⁴_over_ω⁴)^2)
            σ² = σ^2

            cosϕₘ = model.cosϕₘ
            sinϕₘ = model.sinϕₘ
            cos2ϕₘ = model.cos2ϕₘ
            sin2ϕₘ = model.sin2ϕₘ
            cosPShalf = cos(PeakSep/2)
            cosPS = cos(PeakSep)
            sinPShalf = sin(PeakSep/2)
            sinPS = sin(PeakSep)

            exp2part = (exp(2.0 * σ²))
            cos2part = (cos2ϕₘ*cosPS) / exp2part
            exphalf = exp(0.5 * σ²)
            imexppart = 1im * s_om / exphalf * cosPShalf

            # hzz = 1
            hxx = (0.5 + cos2part/2)
            hyy = (0.5 - cos2part/2)
            hxz = imexppart * cosϕₘ
            hyz = imexppart * sinϕₘ
            hyx = sin2ϕₘ * cosPS / exp2part / 2

            #### h_derivs
            ## ϕₘ
            hyxpart∂ϕₘ = cosPS / exp2part
            hxx∂ϕₘ = -sin2ϕₘ * hyxpart∂ϕₘ
            hyy∂ϕₘ = -hxx∂ϕₘ
            hzpart∂ϕₘ = -1im * s_om * cosPShalf / exphalf
            hxz∂ϕₘ = sinϕₘ * hzpart∂ϕₘ
            hyz∂ϕₘ = -cosϕₘ * hzpart∂ϕₘ
            hyx∂ϕₘ = cos2ϕₘ * hyxpart∂ϕₘ

            ## β, ν
            PeakSep∂β = PeakSep / β
            PeakSep∂ν = -((ω > ωₚ)*((ωₚ_over_ω) - 1.0) + 1.0) * β / exp(ν * ((ω > ωₚ)*((ωₚ_over_ω) - 1.0) + 1.0))
            hx∂βνtemp = sinPS / 2 / exp2part
            hz∂βνtemp = -0.5im * sinPShalf / exphalf * s_om

            hx∂βt = hx∂βνtemp * PeakSep∂β
            hx∂νt = hx∂βνtemp * PeakSep∂ν
            hz∂βt = hz∂βνtemp * PeakSep∂β
            hz∂νt = hz∂βνtemp * PeakSep∂ν

            hxx∂β = -cos2ϕₘ * hx∂βt
            hyy∂β = -hxx∂β
            hxz∂β = cosϕₘ * hz∂βt
            hyz∂β = sinϕₘ * hz∂βt
            hyx∂β = -sin2ϕₘ * hx∂βt

            hxx∂ν = -cos2ϕₘ * hx∂νt
            hyy∂ν = -hxx∂ν
            hxz∂ν = cosϕₘ * hz∂νt
            hyz∂ν = sinϕₘ * hz∂νt
            hyx∂ν = -sin2ϕₘ * hx∂νt

            ## σₗ, σᵣ
            # ∂σ∂σₗ = 1
            ∂σ∂σᵣ = (σ-σₗ)/σᵣ
            hx∂σtemp = cosPS * 2 / exp2part * σ
            hz∂σtemp = -1im * cosPShalf / exphalf * σ * s_om

            hx∂σᵣt = hx∂σtemp * ∂σ∂σᵣ
            hz∂σᵣt = hz∂σtemp * ∂σ∂σᵣ

            hxx∂σₗ = -cos2ϕₘ * hx∂σtemp
            hyy∂σₗ = -hxx∂σₗ
            hxz∂σₗ = cosϕₘ * hz∂σtemp
            hyz∂σₗ = sinϕₘ * hz∂σtemp
            hyx∂σₗ = -sin2ϕₘ * hx∂σtemp

            hxx∂σᵣ = -cos2ϕₘ * hx∂σᵣt
            hyy∂σᵣ = -hxx∂σᵣ
            hxz∂σᵣ = cosϕₘ * hz∂σᵣt
            hyz∂σᵣ = sinϕₘ * hz∂σᵣt
            hyx∂σᵣ = -sin2ϕₘ * hx∂σᵣt

            ## ωₚ
            PeakSep∂ωₚ = -(ω > ωₚ) * ν/ω * β * exp(-ν*ωₚ/ω)
            ∂σ∂ωₚ = (-8σᵣ/3*(ωₚ/(ω^2)-ωₚ^7/(ω^8)))

            hx∂ωₚt = (sinPS / 2 * PeakSep∂ωₚ + 2*cosPS*σ*∂σ∂ωₚ)/ exp2part
            hz∂ωₚt = -1im * (sinPShalf / 2 * PeakSep∂ωₚ + cosPShalf*σ*∂σ∂ωₚ)/ exphalf * s_om

            hxx∂ωₚ = -cos2ϕₘ * hx∂ωₚt
            hyy∂ωₚ = -hxx∂ωₚ
            hxz∂ωₚ = cosϕₘ * hz∂ωₚt
            hyz∂ωₚ = sinϕₘ * hz∂ωₚt
            hyx∂ωₚ = -sin2ϕₘ * hx∂ωₚt

            ## Peak seperation derivatives
            PeakSep∂β∂ν = PeakSep∂ν/β
            PeakSep∂β∂ωₚ = PeakSep∂ωₚ/β
            PeakSep∂β2 = 0
            PeakSep∂ν∂ωₚ = -(ω > ωₚ) * (ωₚ_over_ω * PeakSep∂ωₚ + PeakSep/ω)
            PeakSep∂ν2 = (((ωₚ_over_ω)^2 - 1) * (ω > ωₚ) + 1) * PeakSep
            PeakSep∂ωₚ2 = (ν/ω) ^ 2 * PeakSep * (ω > ωₚ)
            ## sigma derivatives
            ## most of these are 0, except
            ∂σ∂σᵣ∂ωₚ = ∂σ∂ωₚ / σᵣ
            ∂σ∂ωₚ2 = (-8σᵣ/3*(1/(ω^2)-7*(model.ωₚ⁶)/(ω^8)))

            ## g1
            g1 = -0.5 / exp2part * sinPS
            g1_β = g1 * PeakSep∂β
            g1_ν = g1 * PeakSep∂ν
            g1_ωₚ = g1 * PeakSep∂ωₚ

            minus_exp2part_over2 = -1/exp2part/2
            g1_β2 = minus_exp2part_over2 * PeakSep∂β^2 * cosPS
            g1_βν = minus_exp2part_over2 * (PeakSep∂β∂ν * sinPS + PeakSep∂β * PeakSep∂ν * cosPS)
            g1_βσₗ = 2/exp2part * sinPS * PeakSep∂β * σ
            g1_βσᵣ = g1_βσₗ * ∂σ∂σᵣ

            g1_ν2 = minus_exp2part_over2 * (PeakSep∂ν2 * sinPS + PeakSep∂ν^2 * cosPS)
            g1_νσₗ = 2/exp2part * sinPS * PeakSep∂ν * σ
            g1_νσᵣ = g1_νσₗ * ∂σ∂σᵣ

            g1_ωₚ2 = 1/exp2part * (-sinPS/2 * PeakSep∂ωₚ2 + PeakSep∂ωₚ * (2σ * ∂σ∂ωₚ * sinPS - 0.5 * PeakSep∂ωₚ * cosPS))

            g1_ωₚβ = minus_exp2part_over2 * (PeakSep∂β∂ωₚ * sinPS + PeakSep∂β * PeakSep∂ωₚ * cosPS)
            g1_ωₚν = minus_exp2part_over2 * (PeakSep∂ν∂ωₚ * sinPS + PeakSep∂ωₚ * PeakSep∂ν * cosPS)
            g1_ωₚσₗ = 2/exp2part * sinPS * PeakSep∂ωₚ * σ
            g1_ωₚσᵣ = g1_ωₚσₗ * ∂σ∂σᵣ

            ## g2
            g2_σₗ = -2 / exp2part * cosPS * σ
            g2_σᵣ = g2_σₗ * ∂σ∂σᵣ
            g2_ωₚ = g2_σₗ * ∂σ∂ωₚ

            one_minus_4σ² = (1 - 4σ²)
            g2_σₗ2 = -2 * cosPS / exp2part * one_minus_4σ²
            g2_σₗσᵣ = g2_σₗ2 * ∂σ∂σᵣ
            g2_σᵣ2 = g2_σₗσᵣ * ∂σ∂σᵣ
            g2_ωₚ2 = 2/exp2part * (PeakSep∂ωₚ * sinPS * ∂σ∂ωₚ * σ - cosPS * (∂σ∂ωₚ2 * σ + ∂σ∂ωₚ^2 * one_minus_4σ²))

            g2_ωₚβ = 2/exp2part * sinPS * PeakSep∂β * ∂σ∂ωₚ * σ
            g2_ωₚν = 2/exp2part * sinPS * PeakSep∂ν * ∂σ∂ωₚ * σ
            g2_ωₚσₗ = -2 * cosPS / exp2part * (∂σ∂ωₚ * one_minus_4σ²)
            g2_ωₚσᵣ = -2 * cosPS / exp2part * (σ * ∂σ∂σᵣ∂ωₚ + ∂σ∂σᵣ * ∂σ∂ωₚ * one_minus_4σ²)

            ## g3
            g3 = s_om * 0.5im / exphalf * sinPShalf
            g3_β = g3 * PeakSep∂β
            g3_ν = g3 * PeakSep∂ν
            g3_ωₚ = g3 * PeakSep∂ωₚ

            g3_β2 = s_om * 0.5im / exphalf * (PeakSep∂β2 * sinPShalf + PeakSep∂β^2 * cosPShalf / 2)
            g3_βν = s_om * 0.5im / exphalf * (PeakSep∂β∂ν * sinPShalf + PeakSep∂β * PeakSep∂ν * cosPShalf / 2)
            g3_ν2 = s_om * 0.5im / exphalf * (PeakSep∂ν2 * sinPShalf + PeakSep∂ν^2 * cosPShalf / 2)
            g3_ωₚ2 = s_om * 0.5im / exphalf * ((PeakSep∂ωₚ2 - σ * ∂σ∂ωₚ * PeakSep∂ωₚ) * sinPShalf + PeakSep∂ωₚ^2*cosPShalf/2)
            g3_βσₗ = -s_om * 0.5im / exphalf * sinPShalf * PeakSep∂β * σ
            g3_βσᵣ = g3_βσₗ * ∂σ∂σᵣ
            g3_νσₗ = -s_om * 0.5im / exphalf * sinPShalf * PeakSep∂ν * σ
            g3_νσᵣ = g3_νσₗ * ∂σ∂σᵣ

            g3_ωₚβ = s_om * 1im/exphalf/2 * (PeakSep∂β∂ωₚ * sinPShalf + PeakSep∂β * PeakSep∂ωₚ * cosPShalf/2)
            g3_ωₚν = s_om * 1im/exphalf/2 * (PeakSep∂ν∂ωₚ * sinPShalf + PeakSep∂ωₚ * PeakSep∂ν * cosPShalf/2)
            g3_ωₚσₗ = -s_om * 1im/exphalf/2 * sinPShalf * PeakSep∂ωₚ * σ
            g3_ωₚσᵣ = g3_ωₚσₗ * ∂σ∂σᵣ
            ## g4
            g4 = s_om * 1im / exphalf * cosPShalf * σ
            g4_σₗ = g4
            g4_σᵣ = g4 * ∂σ∂σᵣ
            g4_ωₚ = g4 * ∂σ∂ωₚ

            g4_σₗ2 = s_om * 1im * cosPShalf / exphalf * (1-σ²)
            g4_σₗσᵣ = g4_σₗ2 * ∂σ∂σᵣ
            g4_σᵣ2 = g4_σₗσᵣ * ∂σ∂σᵣ
            g4_ωₚ2 = -s_om * 1im / exphalf * (PeakSep∂ωₚ * ∂σ∂ωₚ * sinPShalf * σ/2 - cosPShalf * (∂σ∂ωₚ2*σ + ∂σ∂ωₚ^2 * (1 - σ²)) )

            g4_ωₚβ = -s_om * 0.5im/exphalf * sinPShalf * PeakSep∂β * ∂σ∂ωₚ * σ
            g4_ωₚν = -s_om * 0.5im/exphalf * sinPShalf * PeakSep∂ν * ∂σ∂ωₚ * σ
            g4_ωₚσₗ = s_om * 1im * cosPShalf / exphalf * (∂σ∂ωₚ * (1 - σ²))
            g4_ωₚσᵣ = s_om * 1im * cosPShalf / exphalf * (σ * ∂σ∂σᵣ∂ωₚ + ∂σ∂σᵣ * ∂σ∂ωₚ * (1 - σ²))

            ## finished required g functions

            #### hxx

            ## ϕₘ (6)
            hxx∂ϕₘbit = -2sin2ϕₘ
            hxx∂ϕₘ2 = -2cos2ϕₘ * hyxpart∂ϕₘ
            hxx∂ϕₘ∂β = hxx∂ϕₘbit * g1_β
            hxx∂ϕₘ∂ν = hxx∂ϕₘbit * g1_ν
            hxx∂ϕₘ∂σₗ = hxx∂ϕₘbit * g2_σₗ
            hxx∂ϕₘ∂σᵣ = hxx∂ϕₘbit * g2_σᵣ
            hxx∂ϕₘ∂ωₚ = hxx∂ϕₘbit * (g1_ωₚ + g2_ωₚ)
            ## ωₚ (5)
            hxx∂ωₚ2 = cos2ϕₘ * (g1_ωₚ2 + g2_ωₚ2)
            hxx∂ωₚ∂β = cos2ϕₘ * (g1_ωₚβ + g2_ωₚβ)
            hxx∂ωₚ∂ν = cos2ϕₘ * (g1_ωₚν + g2_ωₚν)
            hxx∂ωₚ∂σₗ = cos2ϕₘ * (g1_ωₚσₗ + g2_ωₚσₗ)
            hxx∂ωₚ∂σᵣ = cos2ϕₘ * (g1_ωₚσᵣ + g2_ωₚσᵣ)

            ## β (4)
            hxx∂β2 = cos2ϕₘ * g1_β2
            hxx∂β∂ν = cos2ϕₘ * g1_βν
            hxx∂β∂σₗ = cos2ϕₘ * g1_βσₗ
            hxx∂β∂σᵣ = cos2ϕₘ * g1_βσᵣ
            ## ν (3)
            hxx∂ν2 = cos2ϕₘ * g1_ν2
            hxx∂ν∂σₗ = cos2ϕₘ * g1_νσₗ
            hxx∂ν∂σᵣ = cos2ϕₘ * g1_νσᵣ
            ## σₗ (2)
            hxx∂σₗ2 = cos2ϕₘ * g2_σₗ2
            hxx∂σₗ∂σᵣ = cos2ϕₘ * g2_σₗσᵣ
            ## σᵣ (1)
            hxx∂σᵣ2 = cos2ϕₘ * g2_σᵣ2

            #### hyx

            ## ϕₘ (6)
            hyx∂ϕₘ2 = -2sin2ϕₘ * hyxpart∂ϕₘ
            hyx∂ϕₘ∂β = 2cos2ϕₘ * g1_β
            hyx∂ϕₘ∂ν = 2cos2ϕₘ * g1_ν
            hyx∂ϕₘ∂σₗ = 2cos2ϕₘ * g2_σₗ
            hyx∂ϕₘ∂σᵣ = 2cos2ϕₘ * g2_σᵣ
            hyx∂ϕₘ∂ωₚ = 2cos2ϕₘ * (g1_ωₚ + g2_ωₚ)
            ## ωₚ (5)
            hyx∂ωₚ2 = sin2ϕₘ * (g1_ωₚ2 + g2_ωₚ2)
            hyx∂ωₚ∂β = sin2ϕₘ * (g1_ωₚβ + g2_ωₚβ)
            hyx∂ωₚ∂ν = sin2ϕₘ * (g1_ωₚν + g2_ωₚν)
            hyx∂ωₚ∂σₗ = sin2ϕₘ * (g1_ωₚσₗ + g2_ωₚσₗ)
            hyx∂ωₚ∂σᵣ = sin2ϕₘ * (g1_ωₚσᵣ + g2_ωₚσᵣ)

            ## β (4)
            hyx∂β2 = sin2ϕₘ * g1_β2
            hyx∂β∂ν = sin2ϕₘ * g1_βν
            hyx∂β∂σₗ = sin2ϕₘ * g1_βσₗ
            hyx∂β∂σᵣ = sin2ϕₘ * g1_βσᵣ
            ## ν (3)
            hyx∂ν2 = sin2ϕₘ * g1_ν2
            hyx∂ν∂σₗ = sin2ϕₘ * g1_νσₗ
            hyx∂ν∂σᵣ = sin2ϕₘ * g1_νσᵣ
            ## σₗ (2)
            hyx∂σₗ2 = sin2ϕₘ * g2_σₗ2
            hyx∂σₗ∂σᵣ = sin2ϕₘ * g2_σₗσᵣ
            ## σᵣ (1)
            hyx∂σᵣ2 = sin2ϕₘ * g2_σᵣ2

            #### hxz

            ## ϕₘ (6)
            hxz∂ϕₘ2 = cosϕₘ * hzpart∂ϕₘ # minus already added here
            hxz∂ϕₘbit = sinϕₘ # double negative
            hxz∂ϕₘ∂β = hxz∂ϕₘbit * g3_β
            hxz∂ϕₘ∂ν = hxz∂ϕₘbit * g3_ν
            hxz∂ϕₘ∂σₗ = hxz∂ϕₘbit * g4_σₗ
            hxz∂ϕₘ∂σᵣ = hxz∂ϕₘbit * g4_σᵣ
            hxz∂ϕₘ∂ωₚ = hxz∂ϕₘbit * (g3_ωₚ + g4_ωₚ)
            ## ωₚ (5)
            hxz∂ωₚ2 = -cosϕₘ * (g3_ωₚ2 + g4_ωₚ2)
            hxz∂ωₚ∂β = -cosϕₘ * (g3_ωₚβ + g4_ωₚβ)
            hxz∂ωₚ∂ν = -cosϕₘ * (g3_ωₚν + g4_ωₚν)
            hxz∂ωₚ∂σₗ = -cosϕₘ * (g3_ωₚσₗ + g4_ωₚσₗ)
            hxz∂ωₚ∂σᵣ = -cosϕₘ * (g3_ωₚσᵣ + g4_ωₚσᵣ)

            ## β (4)
            hxz∂β2 = -cosϕₘ * g3_β2
            hxz∂β∂ν = -cosϕₘ * g3_βν
            hxz∂β∂σₗ = -cosϕₘ * g3_βσₗ
            hxz∂β∂σᵣ = -cosϕₘ * g3_βσᵣ
            ## ν (3)
            hxz∂ν2 = -cosϕₘ * g3_ν2
            hxz∂ν∂σₗ = -cosϕₘ * g3_νσₗ
            hxz∂ν∂σᵣ = -cosϕₘ * g3_νσᵣ
            ## σₗ (2)
            hxz∂σₗ2 = -cosϕₘ * g4_σₗ2
            hxz∂σₗ∂σᵣ = -cosϕₘ * g4_σₗσᵣ
            ## σᵣ (1)
            hxz∂σᵣ2 = -cosϕₘ * g4_σᵣ2

            #### hyz

            ## ϕₘ (6)
            hyz∂ϕₘ2 = sinϕₘ * hzpart∂ϕₘ
            hyz∂ϕₘbit = -cosϕₘ  # add in minus for conjugate
            hyz∂ϕₘ∂β = hyz∂ϕₘbit * g3_β
            hyz∂ϕₘ∂ν = hyz∂ϕₘbit * g3_ν
            hyz∂ϕₘ∂σₗ = hyz∂ϕₘbit * g4_σₗ
            hyz∂ϕₘ∂σᵣ = hyz∂ϕₘbit * g4_σᵣ
            hyz∂ϕₘ∂ωₚ = hyz∂ϕₘbit * (g3_ωₚ + g4_ωₚ)
            ## ωₚ (5)
            hyz∂ωₚ2 = -sinϕₘ * (g3_ωₚ2 + g4_ωₚ2)
            hyz∂ωₚ∂β = -sinϕₘ * (g3_ωₚβ + g4_ωₚβ)
            hyz∂ωₚ∂ν = -sinϕₘ * (g3_ωₚν + g4_ωₚν)
            hyz∂ωₚ∂σₗ = -sinϕₘ * (g3_ωₚσₗ + g4_ωₚσₗ)
            hyz∂ωₚ∂σᵣ = -sinϕₘ * (g3_ωₚσᵣ + g4_ωₚσᵣ)

            ## β (4)
            hyz∂β2 = -sinϕₘ * g3_β2
            hyz∂β∂ν = -sinϕₘ * g3_βν
            hyz∂β∂σₗ = -sinϕₘ * g3_βσₗ
            hyz∂β∂σᵣ = -sinϕₘ * g3_βσᵣ
            ## ν (3)
            hyz∂ν2 = -sinϕₘ * g3_ν2
            hyz∂ν∂σₗ = -sinϕₘ * g3_νσₗ
            hyz∂ν∂σᵣ = -sinϕₘ * g3_νσᵣ
            ## σₗ (2)
            hyz∂σₗ2 = -sinϕₘ * g4_σₗ2
            hyz∂σₗ∂σᵣ = -sinϕₘ * g4_σₗσᵣ
            ## σᵣ (1)
            hyz∂σᵣ2 = -sinϕₘ * g4_σᵣ2

            #### out

            ## α2
            # out[1, 1] += 0
            # out[2, 1] += 0
            # out[3, 1] += 0
            # out[4, 1] += 0
            # out[5, 1] += 0
            # out[6, 1] += 0

            ## α ωₚ
            ∂S∂αωₚ = ∂S∂ωₚ / α
            out[1, 2] += ∂S∂αωₚ
            out[2, 2] += ∂S∂αωₚ * hxz + ∂S∂α * hxz∂ωₚ
            out[3, 2] += ∂S∂αωₚ * hyz + ∂S∂α * hyz∂ωₚ
            out[4, 2] += ∂S∂αωₚ * hxx + ∂S∂α * hxx∂ωₚ
            out[5, 2] += ∂S∂αωₚ * hyx + ∂S∂α * hyx∂ωₚ
            out[6, 2] += ∂S∂αωₚ * hyy + ∂S∂α * hyy∂ωₚ

            ## α γ
            ∂S∂αγ = ∂S∂γ / α
            out[1, 3] += ∂S∂αγ
            out[2, 3] += ∂S∂αγ * hxz
            out[3, 3] += ∂S∂αγ * hyz
            out[4, 3] += ∂S∂αγ * hxx
            out[5, 3] += ∂S∂αγ * hyx
            out[6, 3] += ∂S∂αγ * hyy

            ## α r
            ∂S∂αr = ∂S∂r / α
            out[1, 4] += ∂S∂αr
            out[2, 4] += ∂S∂αr * hxz
            out[3, 4] += ∂S∂αr * hyz
            out[4, 4] += ∂S∂αr * hxx
            out[5, 4] += ∂S∂αr * hyx
            out[6, 4] += ∂S∂αr * hyy

            ## α ϕₘ
            # out[1, 5] += 0
            out[2, 5] += ∂S∂α * hxz∂ϕₘ
            out[3, 5] += ∂S∂α * hyz∂ϕₘ
            out[4, 5] += ∂S∂α * hxx∂ϕₘ
            out[5, 5] += ∂S∂α * hyx∂ϕₘ
            out[6, 5] += ∂S∂α * hyy∂ϕₘ

            ## α β
            # out[1, 6] += 0
            out[2, 6] += ∂S∂α * hxz∂β
            out[3, 6] += ∂S∂α * hyz∂β
            out[4, 6] += ∂S∂α * hxx∂β
            out[5, 6] += ∂S∂α * hyx∂β
            out[6, 6] += ∂S∂α * hyy∂β

            ## α ν
            # out[1, 7] += 0
            out[2, 7] += ∂S∂α * hxz∂ν
            out[3, 7] += ∂S∂α * hyz∂ν
            out[4, 7] += ∂S∂α * hxx∂ν
            out[5, 7] += ∂S∂α * hyx∂ν
            out[6, 7] += ∂S∂α * hyy∂ν

            ## α σₗ
            # out[1, 8] += 0
            out[2, 8] += ∂S∂α * hxz∂σₗ
            out[3, 8] += ∂S∂α * hyz∂σₗ
            out[4, 8] += ∂S∂α * hxx∂σₗ
            out[5, 8] += ∂S∂α * hyx∂σₗ
            out[6, 8] += ∂S∂α * hyy∂σₗ

            ## α σᵣ
            # out[1, 9] += 0
            out[2, 9] += ∂S∂α * hxz∂σᵣ
            out[3, 9] += ∂S∂α * hyz∂σᵣ
            out[4, 9] += ∂S∂α * hxx∂σᵣ
            out[5, 9] += ∂S∂α * hyx∂σᵣ
            out[6, 9] += ∂S∂α * hyy∂σᵣ

            ## ωₚ2
            ∂S∂ωₚ2part = δ * log(γ) * ω / σ1²
            ∂S∂ωₚUsepart = (∂S∂ωₚ2part * (ω-ωₚ) / (ωₚ^3)) - (r * (ωₚ^3) / (ω^4))
            ∂S∂ωₚ2 = ∂S∂ωₚ * ∂S∂ωₚUsepart + sdf * (∂S∂ωₚ2part *((-3ω + 2ωₚ)/ωₚ^4 + ω * (ω-ωₚ)^2/ωₚ^6/σ1²) - 3r*ωₚ^2/ω^4)
            out[1, 10] += ∂S∂ωₚ2
            out[2, 10] += ∂S∂ωₚ2 * hxz + 2∂S∂ωₚ * hxz∂ωₚ + sdf*hxz∂ωₚ2
            out[3, 10] += ∂S∂ωₚ2 * hyz + 2∂S∂ωₚ * hyz∂ωₚ + sdf*hyz∂ωₚ2
            out[4, 10] += ∂S∂ωₚ2 * hxx + 2∂S∂ωₚ * hxx∂ωₚ + sdf*hxx∂ωₚ2
            out[5, 10] += ∂S∂ωₚ2 * hyx + 2∂S∂ωₚ * hyx∂ωₚ + sdf*hyx∂ωₚ2
            out[6, 10] += ∂S∂ωₚ2 * hyy + 2∂S∂ωₚ * hyy∂ωₚ - sdf*hxx∂ωₚ2

            ## ωₚγ
            ∂S∂ωₚγ = ∂S∂γ * ∂S∂ωₚUsepart + sdf * δ / γ * ω / σ1²*(ω-ωₚ)/ωₚ^3
            out[1, 11] += ∂S∂ωₚγ
            out[2, 11] += ∂S∂ωₚγ * hxz + ∂S∂γ * hxz∂ωₚ
            out[3, 11] += ∂S∂ωₚγ * hyz + ∂S∂γ * hyz∂ωₚ
            out[4, 11] += ∂S∂ωₚγ * hxx + ∂S∂γ * hxx∂ωₚ
            out[5, 11] += ∂S∂ωₚγ * hyx + ∂S∂γ * hyx∂ωₚ
            out[6, 11] += ∂S∂ωₚγ * hyy + ∂S∂γ * hyy∂ωₚ

            ## ωₚr
            ∂S∂ωₚr = ∂S∂r * ∂S∂ωₚUsepart - sdf * ωₚ^3/ω^4
            out[1, 12] += ∂S∂ωₚr
            out[2, 12] += ∂S∂ωₚr * hxz + ∂S∂r * hxz∂ωₚ
            out[3, 12] += ∂S∂ωₚr * hyz + ∂S∂r * hyz∂ωₚ
            out[4, 12] += ∂S∂ωₚr * hxx + ∂S∂r * hxx∂ωₚ
            out[5, 12] += ∂S∂ωₚr * hyx + ∂S∂r * hyx∂ωₚ
            out[6, 12] += ∂S∂ωₚr * hyy + ∂S∂r * hyy∂ωₚ

            ## ωₚϕₘ
            # out[1, 13] += 0
            out[2, 13] += ∂S∂ωₚ * hxz∂ϕₘ + sdf * hxz∂ϕₘ∂ωₚ
            out[3, 13] += ∂S∂ωₚ * hyz∂ϕₘ + sdf * hyz∂ϕₘ∂ωₚ
            out[4, 13] += ∂S∂ωₚ * hxx∂ϕₘ + sdf * hxx∂ϕₘ∂ωₚ
            out[5, 13] += ∂S∂ωₚ * hyx∂ϕₘ + sdf * hyx∂ϕₘ∂ωₚ
            out[6, 13] += ∂S∂ωₚ * hyy∂ϕₘ - sdf * hxx∂ϕₘ∂ωₚ

            ## ωₚβ
            # out[1, 14] += 0
            out[2, 14] += ∂S∂ωₚ * hxz∂β + sdf * hxz∂ωₚ∂β
            out[3, 14] += ∂S∂ωₚ * hyz∂β + sdf * hyz∂ωₚ∂β
            out[4, 14] += ∂S∂ωₚ * hxx∂β + sdf * hxx∂ωₚ∂β
            out[5, 14] += ∂S∂ωₚ * hyx∂β + sdf * hyx∂ωₚ∂β
            out[6, 14] += ∂S∂ωₚ * hyy∂β - sdf * hxx∂ωₚ∂β

            ## ωₚν
            # out[1, 15] += 0
            out[2, 15] += ∂S∂ωₚ * hxz∂ν + sdf * hxz∂ωₚ∂ν
            out[3, 15] += ∂S∂ωₚ * hyz∂ν + sdf * hyz∂ωₚ∂ν
            out[4, 15] += ∂S∂ωₚ * hxx∂ν + sdf * hxx∂ωₚ∂ν
            out[5, 15] += ∂S∂ωₚ * hyx∂ν + sdf * hyx∂ωₚ∂ν
            out[6, 15] += ∂S∂ωₚ * hyy∂ν - sdf * hxx∂ωₚ∂ν

            ## ωₚσₗ
            # out[1, 16] += 0
            out[2, 16] += ∂S∂ωₚ * hxz∂σₗ + sdf * hxz∂ωₚ∂σₗ
            out[3, 16] += ∂S∂ωₚ * hyz∂σₗ + sdf * hyz∂ωₚ∂σₗ
            out[4, 16] += ∂S∂ωₚ * hxx∂σₗ + sdf * hxx∂ωₚ∂σₗ
            out[5, 16] += ∂S∂ωₚ * hyx∂σₗ + sdf * hyx∂ωₚ∂σₗ
            out[6, 16] += ∂S∂ωₚ * hyy∂σₗ - sdf * hxx∂ωₚ∂σₗ

            ## ωₚσᵣ
            # out[1, 17] += 0
            out[2, 17] += ∂S∂ωₚ * hxz∂σᵣ + sdf * hxz∂ωₚ∂σᵣ
            out[3, 17] += ∂S∂ωₚ * hyz∂σᵣ + sdf * hyz∂ωₚ∂σᵣ
            out[4, 17] += ∂S∂ωₚ * hxx∂σᵣ + sdf * hxx∂ωₚ∂σᵣ
            out[5, 17] += ∂S∂ωₚ * hyx∂σᵣ + sdf * hyx∂ωₚ∂σᵣ
            out[6, 17] += ∂S∂ωₚ * hyy∂σᵣ - sdf * hxx∂ωₚ∂σᵣ

            ## γ2
            ∂S∂γ2 = δ/γ * (∂S∂γ - sdf/γ)
            out[1, 18] += ∂S∂γ2
            out[2, 18] += ∂S∂γ2 * hxz
            out[3, 18] += ∂S∂γ2 * hyz
            out[4, 18] += ∂S∂γ2 * hxx
            out[5, 18] += ∂S∂γ2 * hyx
            out[6, 18] += ∂S∂γ2 * hyy

            ## γr
            ∂S∂γr = δ/γ * ∂S∂r
            out[1, 19] += ∂S∂γr
            out[2, 19] += ∂S∂γr * hxz
            out[3, 19] += ∂S∂γr * hyz
            out[4, 19] += ∂S∂γr * hxx
            out[5, 19] += ∂S∂γr * hyx
            out[6, 19] += ∂S∂γr * hyy

            ## γϕₘ
            # out[1, 20] += 0
            out[2, 20] += ∂S∂γ * hxz∂ϕₘ
            out[3, 20] += ∂S∂γ * hyz∂ϕₘ
            out[4, 20] += ∂S∂γ * hxx∂ϕₘ
            out[5, 20] += ∂S∂γ * hyx∂ϕₘ
            out[6, 20] += ∂S∂γ * hyy∂ϕₘ

            ## γβ
            # out[1, 21] += 0
            out[2, 21] += ∂S∂γ * hxz∂β
            out[3, 21] += ∂S∂γ * hyz∂β
            out[4, 21] += ∂S∂γ * hxx∂β
            out[5, 21] += ∂S∂γ * hyx∂β
            out[6, 21] += ∂S∂γ * hyy∂β

            ## γν
            # out[1, 22] += 0
            out[2, 22] += ∂S∂γ * hxz∂ν
            out[3, 22] += ∂S∂γ * hyz∂ν
            out[4, 22] += ∂S∂γ * hxx∂ν
            out[5, 22] += ∂S∂γ * hyx∂ν
            out[6, 22] += ∂S∂γ * hyy∂ν

            ## γσₗ
            # out[1, 23] += 0
            out[2, 23] += ∂S∂γ * hxz∂σₗ
            out[3, 23] += ∂S∂γ * hyz∂σₗ
            out[4, 23] += ∂S∂γ * hxx∂σₗ
            out[5, 23] += ∂S∂γ * hyx∂σₗ
            out[6, 23] += ∂S∂γ * hyy∂σₗ

            ## γσᵣ
            # out[1, 24] += 0
            out[2, 24] += ∂S∂γ * hxz∂σᵣ
            out[3, 24] += ∂S∂γ * hyz∂σᵣ
            out[4, 24] += ∂S∂γ * hxx∂σᵣ
            out[5, 24] += ∂S∂γ * hyx∂σᵣ
            out[6, 24] += ∂S∂γ * hyy∂σᵣ

            ##  r2
            ∂S∂r2 = (-log(ω) - 0.25*(ωₚ/ω)^4) * ∂S∂r
            out[1, 25] += ∂S∂r2
            out[2, 25] += ∂S∂r2 * hxz
            out[3, 25] += ∂S∂r2 * hyz
            out[4, 25] += ∂S∂r2 * hxx
            out[5, 25] += ∂S∂r2 * hyx
            out[6, 25] += ∂S∂r2 * hyy

            ## rϕₘ
            # out[1, 26] += 0
            out[2, 26] += ∂S∂r * hxz∂ϕₘ
            out[3, 26] += ∂S∂r * hyz∂ϕₘ
            out[4, 26] += ∂S∂r * hxx∂ϕₘ
            out[5, 26] += ∂S∂r * hyx∂ϕₘ
            out[6, 26] += ∂S∂r * hyy∂ϕₘ

            ## rβ
            # out[1, 27] += 0
            out[2, 27] += ∂S∂r * hxz∂β
            out[3, 27] += ∂S∂r * hyz∂β
            out[4, 27] += ∂S∂r * hxx∂β
            out[5, 27] += ∂S∂r * hyx∂β
            out[6, 27] += ∂S∂r * hyy∂β

            ## rν
            # out[1, 28] += 0
            out[2, 28] += ∂S∂r * hxz∂ν
            out[3, 28] += ∂S∂r * hyz∂ν
            out[4, 28] += ∂S∂r * hxx∂ν
            out[5, 28] += ∂S∂r * hyx∂ν
            out[6, 28] += ∂S∂r * hyy∂ν

            ## rσₗ
            # out[1, 29] += 0
            out[2, 29] += ∂S∂r * hxz∂σₗ
            out[3, 29] += ∂S∂r * hyz∂σₗ
            out[4, 29] += ∂S∂r * hxx∂σₗ
            out[5, 29] += ∂S∂r * hyx∂σₗ
            out[6, 29] += ∂S∂r * hyy∂σₗ

            ## rσᵣ
            # out[1, 30] += 0
            out[2, 30] += ∂S∂r * hxz∂σᵣ
            out[3, 30] += ∂S∂r * hyz∂σᵣ
            out[4, 30] += ∂S∂r * hxx∂σᵣ
            out[5, 30] += ∂S∂r * hyx∂σᵣ
            out[6, 30] += ∂S∂r * hyy∂σᵣ

            ## ϕₘ2
            # out[1, 31] += 0
            out[2, 31] += sdf * hxz∂ϕₘ2
            out[3, 31] += sdf * hyz∂ϕₘ2
            out[4, 31] += sdf * hxx∂ϕₘ2
            out[5, 31] += sdf * hyx∂ϕₘ2
            out[6, 31] += sdf * -hxx∂ϕₘ2

            ## ϕₘβ
            # out[1, 32] += 0
            out[2, 32] += sdf * hxz∂ϕₘ∂β
            out[3, 32] += sdf * hyz∂ϕₘ∂β
            out[4, 32] += sdf * hxx∂ϕₘ∂β
            out[5, 32] += sdf * hyx∂ϕₘ∂β
            out[6, 32] += sdf * -hxx∂ϕₘ∂β

            ## ϕₘν
            # out[1, 33] += 0
            out[2, 33] += sdf * hxz∂ϕₘ∂ν
            out[3, 33] += sdf * hyz∂ϕₘ∂ν
            out[4, 33] += sdf * hxx∂ϕₘ∂ν
            out[5, 33] += sdf * hyx∂ϕₘ∂ν
            out[6, 33] += sdf * -hxx∂ϕₘ∂ν

            ## ϕₘσₗ
            # out[1, 34] += 0
            out[2, 34] += sdf * hxz∂ϕₘ∂σₗ
            out[3, 34] += sdf * hyz∂ϕₘ∂σₗ
            out[4, 34] += sdf * hxx∂ϕₘ∂σₗ
            out[5, 34] += sdf * hyx∂ϕₘ∂σₗ
            out[6, 34] += sdf * -hxx∂ϕₘ∂σₗ

            ## ϕₘσᵣ
            # out[1, 35] += 0
            out[2, 35] += sdf * hxz∂ϕₘ∂σᵣ
            out[3, 35] += sdf * hyz∂ϕₘ∂σᵣ
            out[4, 35] += sdf * hxx∂ϕₘ∂σᵣ
            out[5, 35] += sdf * hyx∂ϕₘ∂σᵣ
            out[6, 35] += sdf * -hxx∂ϕₘ∂σᵣ

            ## β2
            # out[1, 36] += 0
            out[2, 36] += sdf * hxz∂β2
            out[3, 36] += sdf * hyz∂β2
            out[4, 36] += sdf * hxx∂β2
            out[5, 36] += sdf * hyx∂β2
            out[6, 36] += sdf * -hxx∂β2

            ## βν
            # out[1, 37] += 0
            out[2, 37] += sdf * hxz∂β∂ν
            out[3, 37] += sdf * hyz∂β∂ν
            out[4, 37] += sdf * hxx∂β∂ν
            out[5, 37] += sdf * hyx∂β∂ν
            out[6, 37] += sdf * -hxx∂β∂ν

            ## βσₗ
            # out[1, 38] += 0
            out[2, 38] += sdf * hxz∂β∂σₗ
            out[3, 38] += sdf * hyz∂β∂σₗ
            out[4, 38] += sdf * hxx∂β∂σₗ
            out[5, 38] += sdf * hyx∂β∂σₗ
            out[6, 38] += sdf * -hxx∂β∂σₗ

            ## βσᵣ
            # out[1, 39] += 0
            out[2, 39] += sdf * hxz∂β∂σᵣ
            out[3, 39] += sdf * hyz∂β∂σᵣ
            out[4, 39] += sdf * hxx∂β∂σᵣ
            out[5, 39] += sdf * hyx∂β∂σᵣ
            out[6, 39] += sdf * -hxx∂β∂σᵣ

            ## ν2
            # out[1, 40] += 0
            out[2, 40] += sdf * hxz∂ν2
            out[3, 40] += sdf * hyz∂ν2
            out[4, 40] += sdf * hxx∂ν2
            out[5, 40] += sdf * hyx∂ν2
            out[6, 40] += sdf * -hxx∂ν2

            ## νσₗ
            # out[1, 41] += 0
            out[2, 41] += sdf * hxz∂ν∂σₗ
            out[3, 41] += sdf * hyz∂ν∂σₗ
            out[4, 41] += sdf * hxx∂ν∂σₗ
            out[5, 41] += sdf * hyx∂ν∂σₗ
            out[6, 41] += sdf * -hxx∂ν∂σₗ

            ## νσᵣ
            # out[1, 42] += 0
            out[2, 42] += sdf * hxz∂ν∂σᵣ
            out[3, 42] += sdf * hyz∂ν∂σᵣ
            out[4, 42] += sdf * hxx∂ν∂σᵣ
            out[5, 42] += sdf * hyx∂ν∂σᵣ
            out[6, 42] += sdf * -hxx∂ν∂σᵣ

            ## σₗ2
            # out[1, 43] += 0
            out[2, 43] += sdf * hxz∂σₗ2
            out[3, 43] += sdf * hyz∂σₗ2
            out[4, 43] += sdf * hxx∂σₗ2
            out[5, 43] += sdf * hyx∂σₗ2
            out[6, 43] += sdf * -hxx∂σₗ2

            ## σₗσᵣ
            # out[1, 44] += 0
            out[2, 44] += sdf * hxz∂σₗ∂σᵣ
            out[3, 44] += sdf * hyz∂σₗ∂σᵣ
            out[4, 44] += sdf * hxx∂σₗ∂σᵣ
            out[5, 44] += sdf * hyx∂σₗ∂σᵣ
            out[6, 44] += sdf * -hxx∂σₗ∂σᵣ

            ## σᵣ2
            # out[1, 45] += 0
            out[2, 45] += sdf * hxz∂σᵣ2
            out[3, 45] += sdf * hyz∂σᵣ2
            out[4, 45] += sdf * hxx∂σᵣ2
            out[5, 45] += sdf * hyx∂σᵣ2
            out[6, 45] += sdf * -hxx∂σᵣ2

        end # otherwise add 0
    end

    return nothing
end

function WhittleLikelihoodInference.coherancy(model::JS_BWG_HNE{K}, ω::Real) where {K}
    s_om = sign(ω)
    ω = abs(ω)
    α,ωₚ,γ,r,ϕₘ,β,ν,σₗ,σᵣ = model.α,model.ωₚ,model.γ,model.r,model.ϕₘ,model.β,model.ν,model.σₗ,model.σᵣ

    ω_over_ωₚ = ω / ωₚ
    ωₚ_over_ω = 1/ω_over_ωₚ
    ω⁻⁴ = ω^(-4)
    ωₚ⁴_over_ω⁴ = model.ωₚ⁴ * ω⁻⁴

    PeakSep = β / exp(ν * ((ω > ωₚ)*((ωₚ_over_ω) - 1.0) + 1.0))
    σ = σₗ - model.σᵣ_over3*(4*(ωₚ_over_ω)^2 - (ωₚ⁴_over_ω⁴)^2)
    σ² = σ^2

    cosPS = cos(PeakSep)
    exp2part = (2 * exp(2.0 * σ²))
    cos2part = (model.cos2ϕₘ*cosPS) / exp2part
    imexppart = 1im * s_om / exp(0.5 * σ²) * cos(PeakSep/2)
    out = zeros(ComplexF64, 6)

    out[1] = 1
    out[2] = imexppart * model.cosϕₘ / sqrt(0.5 + cos2part)
    out[3] = imexppart * model.sinϕₘ / sqrt(0.5 - cos2part)
    out[4] = 1
    out[5] = model.sin2ϕₘ * cosPS / exp2part / sqrt(0.5 + cos2part) / sqrt(0.5 - cos2part)
    out[6] = 1

    return WhittleLikelihoodInference.SHermitianCompact{3, ComplexF64, 6}(out)
end


function MarkNChess(r, n)
    counter = 0
    Moves = Array{Int,1}(undef, 1000)
    Moves, counter = movetothecorner(r, West, Moves)
    markntypechess(r, n)
    movements!(r, Sud)
    movements!(r, West)
    returnfromthecorner(r, Ost, Moves, counter)
end

function markntypechess(r, n)
    side = Ost
    x_coordinate = 0
    y_coordinate = 0
    while !isborder(r, Nord)
        x_coordinate = movelineandmarkntypechess(r, x_coordinate, y_coordinate, n, side)
        move!(r, Nord)
        y_coordinate += 1
        side = inverse(side)
    end
    x_coordinate = movelineandmarkntypechess(r, x_coordinate, y_coordinate, n, side)
end

function changexcoordinate(x_coordinate, side)
    if side == Ost
        x_coordinate += 1
    end
    if side == West
        x_coordinate -= 1
    end
    return x_coordinate
end

function putrightmarker(r, x_coordinate, y_coordinate, n)
    if (floor(x_coordinate / n) + floor(y_coordinate / n)) % 2 == 0
        putmarker!(r)
    end
end

function movelineandmarkntypechess(r, x_coordinate, y_coordinate, n, side)
    putrightmarker(r, x_coordinate, y_coordinate, n)
    while !isborder(r, side)
        move!(r, side)
        x_coordinate = changexcoordinate(x_coordinate, side)
        putrightmarker(r, x_coordinate, y_coordinate, n)
    end
    return x_coordinate
end

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


"""
    Dogleg()

A trust region sub-problem solver that assumes positive definite hessians (exact
or quasi-Newton approximations such as BFGS or variants).
"""
struct Dogleg{T} <: TRSPSolver
    γ::T # unused, for double-dogleg
end
Dogleg() = Dogleg(nothing)

function (dogleg::Dogleg)(∇f, H, Δ, p, scheme; abstol=1e-10, maxiter=50)
    T = eltype(p)
    n = length(∇f)

    # find the Cauchy point; assumes ∇f is not ≈ 0
    d_cauchy = - ∇f*norm(∇f)^2/(∇f'*H*∇f)

    # If it lies outside of the trust region, accept the Cauchy point and
    # move on
    norm_d_cauchy = norm(d_cauchy)
    if norm_d_cauchy ≥ Δ
        shrink = Δ/norm_d_cauchy # inv(Δ/norm_d_cauchy) puts it on the border
        p .= d_cauchy.*shrink
        interior = false
    else
        # Else, calculate (Quasi-)Newton step. If this is interior, then take the
        # step. Otherwise find where the dog-leg path crosses the trust region

        # find the (quasi-)Newton step
        d_qn = copy(p)# -H\∇f # use find step and scheme
        find_direction!(d_qn, H, ∇f, scheme)
        norm_d_qn = norm(d_qn)
        if norm_d_qn ≤ Δ
            p .= d_qn
            if norm_d_qn < Δ
                interior = true
            else
                interior = false
            end
        else
            # we now solve a quadratic to find the step size t from d_cauchy
            # towards d_qn. We use a numerically stable way of doing this (see
            # any numerical analysis text book) See [NW, p. 75] for the expression
            # giving rise to the quadratic equation
            dot_cachy_qn = dot(d_cauchy, d_qn)
            # a is ||d_c - d_n||^2 expanded into scalar operations
            a = norm_d_cauchy^2 + norm_d_qn^2 - 2*dot_cachy_qn
            b = -dot_cachy_qn * norm_d_cauchy^2
            c = norm_d_cauchy^2 - Δ^2 # move the rhs over
            q = -(b + sign(b)*√(b^2-4*a*c))/2
            # since we know that c is necessarily negative (since d_cauchy was
            # not at the border) the discriminant is positive, and there are two
            # roots - pick the positive one. There has to be one positive and one
            # negative.
            if b > 0
                # if b is positive, q is negative. Then if c is negative, we must
                # have that c / q is the positive root.
                t = c / q
            elseif b < 0
                # else, the other root must be positive
                t = q / a
            else
                t = T(0)
            end
            p .= d_cauchy .+ t.*d_qn
            interior = false
        end
    end
    m = dot(∇f, p) + dot(p, H * p)/2

    return (p=p, mz=m, interior=interior, λ=nothing, hard_case=nothing, solved=true, Δ=Δ)
end


using AnimatedPlots

function circle(x)
	10cos(x), 10sin(x)
end

g = StaticGraph(circle)

plot(g)

waitfor(current_window())


module AdventOfCode

mutable struct Node
    name::String
    children::Array{Node}
    parent::Union{Nothing, Node}
end

depth(node::Node) = isnothing(node.parent) ? 0 : (depth(node.parent) + 1)
ancestors(node::Node) = isnothing(node.parent) ? [] : [node.parent; ancestors(node.parent)]

"""Return Dict(name => Node) from list of [parent, child] pairs"""
function build_tree(orbits)
    nodes = Dict()
    for (orbitee, orbiter) in orbits
        orbiter_node = get!(nodes, orbiter, Node(orbiter, [], nothing))
        orbitee_node = get!(nodes, orbitee, Node(orbitee, [], nothing))
        orbiter_node.parent = orbitee_node
        push!(orbitee_node.children, orbiter_node)
    end
    return nodes
end

# Part 1 - build orbit tree and compute depth of each node
orbits = open(f -> split.(readlines(f), ")"), "day06_input.txt")
nodes = build_tree(orbits)
println("Total direct/indirect orbits: $(sum(depth.(values(nodes))))")

# Part 2 - minimum orbital transfers from YOU to SAN
you_nodes = ancestors(nodes["YOU"])
santa_nodes = ancestors(nodes["SAN"])
non_common_nodes = symdiff(you_nodes, santa_nodes)
print("Transfers: $(length(non_common_nodes))")

end


using TR55
using Base.Test

# Test cases from the TR-55 manual example worksheets
# http://www.nrcs.usda.gov/Internet/FSE_DOCUMENTS/stelprdb1044171.pdf

# Figure 2-5
figure_25_regions = [{"area" => 30, "cn" => 61}, {"area" => 70, "cn" => 74}]
figure_25_cn = TR55.weighted_cn(figure_25_regions, true)
figure_25_s = TR55.max_retention(figure_25_cn)
figure_25_q = TR55.runoff(figure_25_s, 6)

@test figure_25_cn == 70
@test figure_25_s == 4.29
@test figure_25_q == 2.8

# Figure 2-6
figure_26_regions = [{"area" => 75, "cn" => 70}, {"area" => 100, "cn" => 80}, {"area" => 75, "cn" => 74}]
figure_26_cn = TR55.weighted_cn(figure_26_regions, false)
figure_26_s = TR55.max_retention(figure_26_cn)
figure_26_q = TR55.runoff(figure_26_s, 6)

@test figure_26_cn == 75
@test figure_26_s == 3.33
@test figure_26_q == 3.28

# Figure 2-7
figure_27_regions = [{"area" => 75, "cn" => 74}, {"area" => 100, "cn" => 82}, {"area" => 75, "cn" => 74}]
figure_27_cn = TR55.weighted_cn(figure_27_regions, false)
figure_27_s = TR55.max_retention(77)
figure_27_q = TR55.runoff(figure_27_s, 6)

@test figure_27_cn == 77
@test figure_27_s == 2.99
@test figure_27_q == 3.48

# Figure 2-8
figure_28_s = TR55.max_retention(74)
figure_28_q = TR55.runoff(figure_28_s, 6)
figure_28_regions = [{"area" => 75, "cn" => 70}, {"area" => 100, "cn" => 78}, {"area" => 75, "cn" => 74}]
figure_28_cn = TR55.weighted_cn(figure_28_regions, false)

@test figure_28_cn == 74
@test figure_28_s == 3.51
@test figure_28_q == 3.19

# Test that weighted CN requires areas to sum to 100 when using percents
test_regions = [{"area" => 20, "cn" => 61}, {"area" => 90, "cn" => 74}]
@test_throws ErrorException TR55.weighted_cn(test_regions, true)


function test_mean(x)
    z = mean(x)
end

function test_mean2(x)
    z = mean(x, 1)
end

test_compare(test_mean; x=rand(3))
test_compare(test_mean; x=rand(3,4))

function test_sum_1(x)
    y = sum(x, 1)
    y2 = y .+ 2
    z = sum(y2)
end

function test_sum_2(x)
    y = sum(x, 2)
    y2 = y .+ 2
    z = sum(y2)
end

test_compare(test_sum_1; x=rand(3,4))
test_compare(test_sum_2; x=rand(3,4))


# Test pair-wise correlation and covariance functions
x, y = rand(1000), rand(1000)

@test vcov(x,y, multithreaded=true) ≈ vcov(x,y, multithreaded=false) ≈ cov(x,y)
@test vcov(x,y, corrected=false, multithreaded=true) ≈ vcov(x,y, corrected=false, multithreaded=false) ≈ cov(x,y, corrected=false)
@test vcor(x,y, multithreaded=true) ≈ vcor(x,y, multithreaded=false) ≈ cor(x,y)

# Test correlation and covariance functions as applied to matrices
X = rand(100,10)

@test vcov(X, multithreaded=true) ≈ vcov(X, multithreaded=false) ≈ cov(X)
@test vcov(X, dims=1, multithreaded=true) ≈ vcov(X, dims=1, multithreaded=false) ≈ cov(X, dims=1)
@test vcov(X, dims=2, multithreaded=true) ≈ vcov(X, dims=2, multithreaded=false) ≈ cov(X, dims=2)
@test vcov(X, corrected=false, multithreaded=true) ≈ vcov(X, corrected=false, multithreaded=false) ≈ cov(X, corrected=false)

@test vcor(X, multithreaded=true) ≈ vcor(X, multithreaded=false) ≈ cor(X)
@test vcor(X, dims=1, multithreaded=true) ≈ vcor(X, dims=1, multithreaded=false) ≈ cor(X, dims=1)
@test vcor(X, dims=2, multithreaded=true) ≈ vcor(X, dims=2, multithreaded=false) ≈ cor(X, dims=2)


# ---
# title: 869. Reordered Power of 2
# id: problem869
# author: Indigo
# date: 2021-07-11
# difficulty: Medium
# categories: Math
# link: <https://leetcode.com/problems/reordered-power-of-2/description/>
# hidden: true
# ---
#
# Starting with a positive integer `N`, we reorder the digits in any order
# (including the original order) such that the leading digit is not zero.
#
# Return `true` if and only if we can do this in a way such that the resulting
# number is a power of 2.
#
#
#
# **Example 1:**
#
#
#
#     Input: 1
#     Output: true
#
#
# **Example 2:**
#
#
#
#     Input: 10
#     Output: false
#
#
# **Example 3:**
#
#
#
#     Input: 16
#     Output: true
#
#
# **Example 4:**
#
#
#
#     Input: 24
#     Output: false
#
#
# **Example 5:**
#
#
#
#     Input: 46
#     Output: true
#
#
#
#
# **Note:**
#
#   1. `1 <= N <= 10^9`
#
#
## @lc code=start
using LeetCode

function reordered_power_of2(n::Integer)
    cntr1 = OffsetArray(fill(0, 10), -1)
    cntr2 = OffsetArray(fill(0, 10), -1)
    function count_num!(cntr, n::Integer)
        fill!(cntr, 0)
        for d in digits(n)
            cntr[d] += 1
        end
        cntr
    end
    count_num!(cntr1, n)
    for i in 0:31
        (cntr1 == count_num!(cntr2, 1 << i)) && return true
    end
    return false
end
## @lc code=end


#! /usr/bin/julia

# Rosetta Code, Brownian tree

using Color, Images, FixedPointNumbers

const W = 512
const W0 = W>>1
const H = 512
const H0 = H>>1
const N = iceil(1.0*W*H)
const SIDESTICK = false

function motecolor(x::Int, y::Int)
    h = clamp(180*(atan2(y-H0, x-W0)/pi + 1.0), 0.0, 360.0)
    return HSV(h, 0.5, 0.5)
end

img = Image(zeros(RGB{Ufixed8}, H, W))
img["x", W0, "y", H0] = RGB(1, 1, 1)
isfree = trues(W, H)
isfree[W0, H0] = false
for i in 1:N
    x = rand(1:W)
    y = rand(1:H)
    isfree[x, y] || continue
    mc = motecolor(x, y)
    for j in 1:10^3
        xp = x + rand(-1:1)
        yp = y + rand(-1:1)
        iscontained = 0 < xp <= W && 0 < yp <= H
        if  iscontained && isfree[xp, yp]
            x = xp
            y = yp
            continue
        else
            if SIDESTICK || (iscontained && !isfree[xp, yp])
                isfree[x, y] = false
                img["x", x, "y", y] = mc
            end
            break
        end
    end
end

imwrite(img, "brownian_tree.png")


"""
    drawtrappedpointsR2(f [; hasescaped, maxiterations])

Return the drawing of the trapped points set of a function
    \$f:\\mathbb{R}^2\\rightarrow\\mathbb{R}^2\$,
    in a rectangular region in \$\\mathbb{R}^2\$,
    using the escape time of iterations algorithm.

The trapped points set of \$f\$ is defined as
\$\\mathcal{K}(f)=\\{(x,y)\\in\\mathbb{R}^2\\,|\\,|f^n(x,y)|\\nrightarrow\\infty\\,n\\rightarrow\\infty\\}\$

#### Arguments
- `f::Function`: A function \$f:\\mathbb{R}^2\\rightarrow\\mathbb{R}^2\$.
- `hasescaped::Function`: A boolean function to check if the iterations has escaped.
- `maxiterations::Integer`: Maximum number of iterations to check.
"""
function drawtrappedpointsR2(f::Function;
    hasescaped::Function=(x::Real,y::Real) -> x*x+y*y > 4, maxiterations::Int=100)

    # Veryfying if graphics backend supports functions
    SDDGraphics.supported(:drawpixel)

    # Verifying functions
    @assert typeof(f(1., 1.)) <: Tuple{Real,Real}
    @assert typeof(hasescaped(1., 1.)) <: Bool

    SDDGraphics.newdrawing()
    SDDGraphics.updatecolorarray(maxiterations)

    @sweeprectregion SDDGraphics.xlims() SDDGraphics.ylims() SDDGraphics.canvassize() begin
        xn,yn = x,y
        escapetime = maxiterations
        for n in 1:maxiterations
            if hasescaped(xn,yn)
                escapetime = n
                break
            end # if hasescaped
            xn,yn = f(xn,yn)
        end # for n maxiterations
        SDDGraphics.color(escapetime)
        SDDGraphics.drawpixel(i,j)
    end # Implemented algorithm

    SDDGraphics.drawing()

#= # Classical code
    width, height = SDDGraphics.canvassize()
    xmin, xmax = xlims
    ymin, ymax = ylims
    Δx, Δy = (xmax-xmin)/width, (ymax-ymin)/height

    x, y = 0., 0.

    y = ymax
    for j in 1:height

        x = xmin
        for i in 1:width

            xn, yn = x, y
            escapetime = 1.
            for n in 0:maxiterations

                if hasescaped(xn,yn)
                    escapetime = n/maxiterations
                    break
                end # if hasescaped

                xn,yn = f(xn,yn)

            end # for n maxiterations

            SDDGraphics.drawpixel(j,i,escapetime)

            x += Δx

        end # for i width

        y -= Δy

    end # for j height

    SDDGraphics.plot()
=#
end # function drawtrappedpointsR2

"""
    drawtrappedpointsC(f [; hasescaped, maxiterations])

Return the drawing of the trapped points set of a function
    \$f:\\mathbb{C}\\rightarrow\\mathbb{C}\$,
    in a rectangular region in \$\\mathbb{C}\$,
    using the escape time of iterations algorithm.

The trapped points set of \$f\$ is defined as
\$\\mathcal{K}(f)=\\{z\\in\\mathbb{C}\\,|\\,|f^n(z)|\\nrightarrow\\infty\\,n\\rightarrow\\infty\\}\$

#### Arguments
- `f::Function`: A function \$f:\\mathbb{C}\\rightarrow\\mathbb{C}\$.
- `hasescaped::Function`: A boolean function to check if the iterations has escaped.
- `maxiterations::Integer`: Maximum number of iterations to check.
"""
function drawtrappedpointsC(f::Function;
    hasescaped::Function=z::Number -> abs2(z) > 4, maxiterations::Int=100)

    # Veryfying if graphics backend supports functions
    SDDGraphics.supported(:drawpixel)

    # Verifying functions
    @assert typeof(f(1.0im)) <: Number
    @assert typeof(hasescaped(1.0im)) <: Bool

    SDDGraphics.newdrawing()
    SDDGraphics.updatecolorarray(maxiterations)

    @sweeprectregion SDDGraphics.xlims() SDDGraphics.ylims() SDDGraphics.canvassize() begin
        zn = complex(x,y)
        escapetime = maxiterations
        for n in 1:maxiterations
            if hasescaped(zn)
                escapetime = n
                break
            end # if hasescaped
            zn = f(zn)
        end # for n maxiterations
        SDDGraphics.color(escapetime)
        SDDGraphics.drawpixel(i,j)
    end # Implemented algorithm

    SDDGraphics.drawing()

end # function drawtrappedpointsC


begin
	using Plots
	using CUDA
	CUDA.versioninfo()
end

struct Spins
	N::Int32
	M::Int32
	A::Array{Float32}
end

function Init(N = 100::Int,M = 100::Int)
	S = Spins(N, M, rand(Float32, N+2 ,M+2)*2π .- π)

	S.A[1,:] .= 0.0
	S.A[N + 2, :] .= 0.0
	S.A[:,1] .= 0.0
	S.A[:,M + 2] .= 0.0

	return S
end

function EnergyGPU(S::Spins)
	E = CUDA.zeros(S.N,S.M)
	CuAr = CuArray(S.A)

	S.A[2:S.N+1,2:S.M+1]

	p = [(-1,0),(1,0),(0,-1),(0,1)]

	@sync begin
	@async begin
			Epart = *(CuAr[2:S.N+1,2:S.M+1],
					CuAr[2+p[1][1]:S.N+1+p[1][1],
						2+p[1][2]:S.M+1+p[1][2]])
			E += Epart
	end
	@async begin
			Epart = *(CuAr[2:S.N+1,2:S.M+1],
					CuAr[2+p[2][1]:S.N+1+p[2][1],
						2+p[2][2]:S.M+1+p[2][2]])
			E += Epart
	end
	@async begin
			Epart = *(CuAr[2:S.N+1,2:S.M+1],
					CuAr[2+p[3][1]:S.N+1+p[3][1],
						2+p[3][2]:S.M+1+p[3][2]])
			E += Epart
	end
	@async begin
			Epart = *(CuAr[2:S.N+1,2:S.M+1],
					CuAr[2+p[4][1]:S.N+1+p[4][1],
						2+p[4][2]:S.M+1+p[4][2]])
			E += Epart
	end
	end

	return CUDA.sum(E)
end

function EnergyGPUAlt(S::Spins)
	E = CUDA.zeros(S.N,S.M)
	CuAr = CuArray(S.A)

	S.A[2:S.N+1,2:S.M+1]

	p = [(-1,0),(1,0),(0,-1),(0,1)]
	@sync for i in p
			begin
			Epart = *(CuAr[2:S.N+1,2:S.M+1],
					CuAr[2+i[1]:S.N+1+i[1],
						2+i[2]:S.M+1+i[2]])
			E += Epart
			end
	end
	return CUDA.sum(E)
end

function MagnetisationGPU(S::Spins)
	CuAr = CuArray(S.A)
	mx = (CUDA.sum(cos.(CuAr)))/ *(S.N,S.M)
	my = (CUDA.sum(sin.(CuAr)))/ *(S.N,S.M)
	return √(mx^2 + my^2)
end

function Advance(S::Spins,max_steps=10000::Int,T=5.0::Float32)
	eps = 0.001

	E = zeros(Float32,max_steps + 1)
	m = zeros(Float32,max_steps + 1)

	Enow = EnergyGPU(S)
	E[1] = Enow
	m[1] = MagnetisationGPU(S)

	step = 1

	for _ in 1:max_steps

		step += 1
		i = rand(2:S.N+1)
		j = rand(2:S.M+1)

		InitVal = S.A[i,j]
		S.A[i,j]= (S.A[i,j] + 2π*(rand()-0.5))%π
		Enext = EnergyGPU(S)
		dE = Enext - Enow

        	if rand() < exp(-dE/T)
			Enow = Enext
	        else
			S.A[i,j] = InitVal
		end

		E[step] = Enow
		m[step] = MagnetisationGPU(S)
	end
	return E,m,S.A
end

function Run(N=100::Int,M=100::Int,max_steps=10000::Int,T=5.0::Float64)
	S = Init(N,M)
	h1 = heatmap(S.A)
	E,m,S=Advance(S,max_steps,convert(Float32,T))

	h2 = heatmap(S)

	l = @layout [a{0.5h};b{0.5h}]
	p1 = plot(m, linecolor = :blue, label="m")
	p2 = plot(E, lc = :red, label="E")

	a1 = plot(h1,h2,layout = l)
	a2 = plot(p1,p2,layout = l)

	plot(a2,a1,size=(600,300))
end


# Included in Common.jl

function eisner(edge_scores::Dict{(Int, Int), Real}, n::Int)
 	# n: number of words in sentence

	comp_rh = Dict{(Int, Int), Real}() # Complete span, right head. 	E[s][t][0][0]
	comp_lh = Dict{(Int, Int), Real}() # Complete span, left head. 		E[s][t][1][0]
	incomp_rh = Dict{(Int, Int), Real}() # Incomplete span, right head.	E[s][t][0][1]
	incomp_lh = Dict{(Int, Int), Real}() # Incomplete span, left head.	E[s][t][1][1]
	# TODO: sizehint()

	# backpointers
	bp_comp_rh = Dict{(Int, Int), Int}()
	bp_comp_lh = Dict{(Int, Int), Int}()
	bp_incomp_rh = Dict{(Int, Int), Int}()
	bp_incomp_lh = Dict{(Int, Int), Int}()

	for i = 0:n
		comp_rh[(i, i)] = 0
		comp_lh[(i, i)] = 0
		incomp_rh[(i, i)] = 0
		incomp_lh[(i, i)] = 0
	end

	for m = 1:n
		for s = 0:n
			t = s + m
			if (t > n) break end

			# E[s][t][0][1]
			tmp = -Inf
			bp = -1
			for q = s:t-1
				# get(edge_scores,(t, s), -Inf): IF edge's from a word to ROOT, then score = -Inf
				x = comp_rh[(q+1, t)] + get(edge_scores,(t, s), -Inf)
				if x > tmp
					tmp = x
					bp = q
				end
			end
			incomp_rh[(s, t)] = tmp
			bp_incomp_rh[(s, t)] = bp

			# E[s][t][1][1]
			tmp = -Inf
			bp = -1
			for q = s:t-1
				x = comp_lh[(s, q)] + comp_rh[(q+1, t)] + edge_scores[(s, t)]
				if x > tmp
					tmp = x
					bp = q
				end
			end
			incomp_lh[(s, t)] = tmp
			bp_incomp_lh[(s, t)] = bp

			# E[s][t][0][0]
			tmp = -Inf
			bp = -1
			for q = s:t-1
				x = comp_rh[(s, q)] + incomp_rh[(q, t)]
				if x > tmp
					tmp = x
					bp = q
				end
			end
			comp_rh[(s, t)] = tmp
			bp_comp_rh[(s, t)] = bp

			# E[s][t][1][0]
			tmp = -Inf
			for q = s+1:t
				x = incomp_lh[(s, q)] + comp_lh[(q, t)]
				if x > tmp
					tmp = x
					bp = q
				end
			end
			comp_lh[(s, t)] = tmp
			bp_comp_lh[(s, t)] = bp
		end
	end

	heads = cell(n)
	function backtrack(s, t, lr, ci)
		# lr: right head = 0, left head = 1
		# ci: comp = 0, incomp = 1
		if lr == 1 && ci == 0 # comp_lh
			if s != t
				q = bp_comp_lh[(s, t)]
				heads[q] = s
				heads[t] = q
				backtrack(s, q, 1, 1)
				backtrack(q, t, 1, 0)
			end
		elseif lr == 0 && ci == 0 # comp_rh
			if s != t
				q = bp_comp_rh[(s, t)]
				heads[q] = t
				heads[s] = q
				backtrack(s, q, 0, 0)
				backtrack(q, t, 0, 1)
			end
		elseif lr == 1 && ci == 1
			if s != t
				q = bp_incomp_lh[(s, t)]
				heads[t] = s
				backtrack(s, q, 1, 0)
				backtrack(q+1, t, 0, 0)
			end
		elseif lr == 0 && ci == 1
			if s != t
				q = bp_incomp_rh[(s, t)]
				heads[s] = t
				backtrack(s, q, 1, 0)
				backtrack(q+1, t, 0, 0)
			end
		end
	end
	backtrack(0, n, 1, 0)

	heads

end


using Pkg
Pkg.activate(joinpath(@__DIR__, "..", ".."))
using ZigZagBoomerang

Pkg.activate(@__DIR__)
cd(@__DIR__)
#using Revise
using DataStructures
using LinearAlgebra
using Random
using SparseArrays
using Test
using FileIO
using Statistics
using Makie, AbstractPlotting
using ForwardDiff
using StructArrays
const ρ0 = 0.0
Σ = [1 ρ0; ρ0 1]
const d = 2
const dist = 1.5

phi(x, y, rho) =  1/(2*pi*sqrt(1-rho^2))*exp(-0.5*(x^2 + y^2 - 2x*y*rho)/(1-rho^2))
logdensity(x, y) = log(phi(x - dist, y - dist, 0.0) + phi(x + dist, y + dist, 0.0))
#r = -4.5:0.01:4.5

#surf = surface(r, r, [10*exp(logdensity(x1, x2)) for x1 in r, x2 in r])
#=
function partiali()
    ith = zeros(d)
    function (x,i)
        ith[i] = 1
        sa = StructArray{ForwardDiff.Dual{}}((x, ith))
        δ = density(sa).partials[]
        ith[i] = 0
        return δ
    end
end
=#

function gradϕ(x,i)
    -ForwardDiff.partials(logdensity(ForwardDiff.Dual{}(x[1], 1.0*(i==1)), ForwardDiff.Dual{}(x[2], 1.0*(i==2))))[]
end

function gradϕ!(y, x)
    y[1] = gradϕ(x,1)
    y[2] = gradϕ(x,2)
    y
end

κ = [0.1, 0.1]
t0 = 0.0
x0 = randn(d)
θ0 = [1.0, 1.0]
c = 20.0
T = 1000.0

#sspdmp(∇ϕi, t0, x0, θ0, T, c*ones(d), ZigZag(sparse(I(d)), 0*x0), κ; adapt=false)
trace, (tT, xT, θT), (acc, num) = sspdmp(gradϕ, t0, x0, θ0, T, c*ones(d), ZigZag(sparse(I(d)), 0*x0), κ; adapt=false)
#ts, xs = ZigZagBoomerang.sep(collect(discretize(trace, 0.05)))
ts, xs = ZigZagBoomerang.sep(collect(trace))

#p1 = scatter(first.(xs), last.(xs), color=ts, markersize = 3, strokewidth = 0.1, alpha = 0.5)
p1 = lines(first.(xs), last.(xs), color=ts)

scene, layout = layoutscene(resolution = (1200, 900))
layout[1, 1] = ax1 = Axis(scene)
layout[2, 1] = ax2 = Axis(scene)

linkyaxes!(ax1, ax2)
linkxaxes!(ax1, ax2)

lines!(ax1, ts, first.(xs))
lines!(ax2, ts, last.(xs))
p1a = scene
trace, (tT, xT, θT), (acc, num) = sspdmp(gradϕ!, t0, x0, θ0, T, c, BouncyParticle(sparse(I(d)), 0*x0, 0.1), κ; adapt=false)

#ts, xs = ZigZagBoomerang.sep(collect(discretize(trace, 0.05)))
ts, xs = ZigZagBoomerang.sep(collect(trace))

p2 = lines(first.(xs), last.(xs), color=ts)

scene, layout = layoutscene(resolution = (1200, 900))
layout[1, 1] = ax1 = Axis(scene)
layout[2, 1] = ax2 = Axis(scene)

linkyaxes!(ax1, ax2)
linkxaxes!(ax1, ax2)

lines!(ax1, ts, first.(xs))
lines!(ax2, ts, last.(xs))
p2a = scene

Random.seed!(4)
BB = Boomerang(sparse(I(d)), 0*x0, 1.0)
trace, (tT, xT, θT), (acc, num) = sspdmp(gradϕ!, t0, x0, θ0, T, c, BB, κ; adapt=false)

tsxs = sdiscretize(trace.events, BB, 0.05)

ts, xs= tsxs.t, tsxs.x

p3 = scatter(first.(xs), last.(xs), color=ts, markersize = 3, strokewidth = 0.1, alpha = 0.5)

scene, layout = layoutscene(resolution = (1200, 900))
    layout[1, 1] = ax1 = Axis(scene)
    layout[2, 1] = ax2 = Axis(scene)

    linkyaxes!(ax1, ax2)
    linkxaxes!(ax1, ax2)

    lines!(ax1, ts, first.(xs))
    lines!(ax2, ts, last.(xs))
    p3a = scene

save("zigzagphase.png", p1)
save("zigzagtrace.png", p1a)

save("bouncyphase.png", p2)
save("bouncytrace.png", p2a)

save("boomyphase.png", p3)
save("boomytrace.png", p3a)


using ConstructiveGeometry
using Test
using StaticArrays
using LinearAlgebra

G = ConstructiveGeometry

@testset "2d" begin#««
nva(s::G.PolygonXor) = (length.(G.paths(s)), G.Shapes.area(s.poly))
r1 = G.square(2.,1)
r2 = G.square(1.,2)
r3 = G.square(3.,3)
r4 = [4,4] + G.square(1,1)
@test nva(mesh(union(r1,r2))) == ([6], 3)
@test nva(mesh(union(r1,r3))) == ([4], 9)
@test nva(mesh(union(r1,r4))) == ([4,4], 3)
@test nva(mesh(intersect(r1,r2))) == ([4], 1)
@test nva(mesh(intersect(r2,r1))) == ([4], 1)
@test nva(mesh(intersect(r1,r3))) == ([4], 2)
@test nva(mesh(intersect(r3,r1))) == ([4], 2)
@test nva(mesh(intersect(r1,r4))) == ([], 0)
@test nva(mesh(setdiff(r1,r2))) == ([4], 1)
@test nva(mesh(setdiff(r3,r1))) == ([6], 7)
end#»»
# @testset "Extrusion" begin #««1
# # using ConstructiveGeometry: nvertices, nfaces
# # C = vertices(circle(3.),(precision=.01,accuracy=1,symmetry=1))
# # c = [Point(20*cos(i),20*sin(i)) for i in 0:.1:π]; c=[c;[Point(0.,-1.)]]
# # @test (path_extrude(c, C)) != 0
# #
# d=setdiff(square(15.), translate([1,1])*square(8.))
# # e=linear_extrude(10.)*d
# # m=mesh(e)
# # @test nvertices(m) == 16
# # @test nfaces(m) == 32
# end
@testset "Elementary operations on objects" begin #««1
s = square(1)
@testset "Primitives" begin #««2
@test s == square([1,1])
@test s == square(1,1)
end
@testset "Operations" begin #««2
@test union(s, union()) === s
@test length(G.children(union(s, union(s)))) == 2
end
@testset "Transforms" begin #««2
@test 2s == scale(2, s)
@test scale(2)*s == scale(2, s)
@test scale(2)*[s] == scale(2, s)
@test color("red")*s == color("red", s)
end
end
# @testset "Convex hull" begin #««1
# using ConstructiveGeometry: convex_hull, convex_hull_list
# P(x...) = Point(Float64.(x)...)
# CH = convex_hull([P(0,0,0),P(0,0,10),P(10,0,0),P(0,10,0),P(1,1,1),P(1,0,0),])
# @test Set(CH[1]) == Set([P(0,0,0),P(0,0,10),P(10,0,0),P(0,10,0),])
# @test length(CH[2]) ==4
# @test convex_hull_list([ P(0., 0), P(1., 1), P(-1., 1), P(.2, 2.), P(0, .8), P(-.2, .8), ]) == [1,2,4,3]
# # @test convex_hull_list(ConstructiveGeometry.rows(SA[0. 0;1. 1;-1. 1;.2 2.;0 .8;-.2 .8])) == [1,2,4,3]
# @test convex_hull_list([
# 	P(-2.627798062316817, 1.075268817204301),
# 	P(-0.5030257403564974, -1.720430107526882),
# 	P(0.7927283156659947, 2.7956989247311825),
# 	P(0.0, 2.396978520135108),
# 	P(0.0, 0.03278003249806397),
# 	]) == [2,5,3,1]
# @test convex_hull_list([
# 	P(-2.150537634408602, 1.3494700327417308),
# 	P(-0.4301075268817205, -2.097580910437773),
# 	P(2.3655913978494625, 0.04739817471346019),
# 	P(0.0, 0.0),
# 	P(2.3038140933536018, 0.0),
# 	P(0.0, 0.7294358146330306),
# 	])== [2,5,3,6,1]
# end
# @testset "Triangulation" begin #««1
# using ConstructiveGeometry: LibTriangle, triangulate, triangulate_loop, identify_polygons, PolygonXor
# @test length(triangulate_loop(Point{2,Float64}.([(0,0),(1,0),(1,1),(0,1)]))) == 2
# square_with_hole = PolygonXor([[5.0, 5.0], [0.0, 5.0], [0.0, 0.0], [5.0, 0.0]],[[1.0, 1.0], [1.0, 3.0], [3.0, 3.0], [3.0, 1.0]])
# @test identify_polygons(square_with_hole) == [1, -1]
# @test length(triangulate(square_with_hole)) == 8
# @test CG.ladder_triangles(5,4,0,10) ==
# 	[(0,10,1),(1,10,11),(1,11,2),(2,11,12),(2,12,3),(3,12,13),(3,13,4)]
# end
# @testset "Basic geometry" begin#««1
# v = [[-1,0],[0,-1],[1,0],[0,1]]
# m = [CG.circular_sign(i,j) for i in v, j in v]
# @test sign.(m) == [0 1 1 1; -1 0 1 1; -1 -1 0 1; -1 -1 -1 0]
# end
# @testset "Triangle intersection" begin#««1
# TI=CG.TriangleIntersections
# Pos=TI.Constants
# pts(m) = ([Float64.(m[i,:]) for i in 1:size(m,1)]...,)
# function check_inter1(s, t, n)#««
# 	it = inter(s,t)
# 	return length(it)==n &&
# 		all(u[1] ==  pttype(p, s) && u[2] == pttype(p,t) for (p,u) in it)
# end#»»
# function check_inter(s1,t1,n)#««
# 	perm = (([1],),
# 		([1,2],[2,1]),
# 		([1,2,3],[2,3,1],[3,1,2],[1,3,2],[2,1,3],[3,2,1]))
# 	s = (s1[p] for p in perm[length(s1)])
# 	if length(s1) == 2
# 		t = (t1[p] for p in perm[3][1:3])
# 	else
# 		t = (t1[p] for p in perm[length(t1)])
# 	end
# 	return all(check_inter1(s2,t2,n) for s2 in s, t2 in t)
# end#»»
# function pttype(a,(u,v)::NTuple{2},ε=1e-8)#««
# 	abs(TI.det2(u,v,a)) > ε && return -1
# 	t = dot(a-u,v-u)/dot(v-u,v-u)
# 	t < 0-ε && return Pos.invalid
# 	t > 1+ε && return Pos.invalid
# 	t ≤ 0+ε && return Pos.vertex1
# 	t ≥ 1-ε && return Pos.vertex2
# 	return Pos.edge12
# end#»»
# function pttype(a,(p,q,r)::NTuple{3,<:StaticVector{2}},ε=1e-8)#««
# 	(d1,d2,d3) = (TI.det2(q,r,a),TI.det2(r,p,a),TI.det2(p,q,a))
# 	s(x) = (x > ε) ? '+' : (x<-ε) ? '-' : '0'
# 	m = s(d1)s(d2)s(d3)
# 	@assert '+' ∈ m "bad point type ($m)for $a in ($p, $q, $r)"
# 	m == "+++" && return Pos.interior
# 	m == "0++" && return Pos.edge23
# 	m == "+0+" && return Pos.edge31
# 	m == "++0" && return Pos.edge12
# 	m == "+00" && return Pos.vertex1
# 	m == "0+0" && return Pos.vertex2
# 	m == "00+" && return Pos.vertex3
# 	return Pos.invalid
# end#»»
# @inline inter(s::NTuple{2}, t::NTuple{3}) = TI.inter_segment2_triangle2(s,t)
# @inline inter(s::NTuple{3,<:StaticVector{3}}, t::NTuple{3}) = TI.inter(s,t)
# t2=pts(SA[0 0;10 0;0 10])
#
# # vertices:
# @test check_inter(pts(SA[-1 1;10 0]), t2, 2)
# @test check_inter(pts(SA[-1 1;0 10]), t2, 1)
# @test check_inter(pts(SA[-5 5;0 0]), t2, 1)
# # edges:
# @test check_inter(pts(SA[-1 1;2 0]), t2, 2)
# @test check_inter(pts(SA[-1 1;0 2]), t2, 1)
# @test check_inter(pts(SA[-1 1;7 3]), t2, 2)
# # contact with vertices:
# @test check_inter(pts(SA[-5 5;1 -1]), t2, 1)
# # inside:
# @test check_inter(pts(SA[-1 1;1 1]), t2, 2)
# # outside:
# @test check_inter(pts(SA[-1 1;2 10]), t2, 2)
# @test check_inter(pts(SA[-1 5;2 20]), t2, 1)
# @test check_inter(pts(SA[-1 5;2 30]), t2, 0)
# @test check_inter(pts(SA[-1 5;10 -3]), t2, 2)
# function pttype(a,(p,q,r)::NTuple{3,<:StaticVector{3}},ε=1e-8)#««
# 	normal2 = cross(q-p,r-p)
# 	if abs(dot(normal2, p-a)) > ε
# 		return -1
# 	end
# 	proj = TI.Projector(normal2)
# 	(a1, p1, q1, r1) = proj.((a,p,q,r))
# 	return pttype(a1, (p1,q1,r1), ε)
# end#»»
# t3 = pts(SA[0 0 0;5 0 0;0 5 0])
# function dotest(c, a, b, n)#««
# 	# triangle on plane [x+y=c],
# 	# crossing [z=0] at (3-a,a,0) and (3-b,b,0)
# 	# (edges 3 and 2)
# 	t1 = pts(SA[c-a a 1; c-a a -1; c-2b+a 2b-a  -1])
#
# 	check_inter(t3,t1,n)
# 	check_inter(t1,t3,n)
# end#»»
# @test check_inter(pts(SA[2. 0 0;2 2 0;1 1 1]),
# 		pts(SA[3. 2 0;1 2 0;2 1 1]), 2)
# 	# touch configuration:
# @test dotest(0, -2, -1, 0)
# @test dotest(0, -2, 0,  1)
# @test dotest(0, -2, 1, 1)
# @test dotest(0, 0, 1, 1)
# @test dotest(0, 1, 2, 0)
#
# 	# general position:
# @test dotest(3, -2,-1, 0)
# @test dotest(3, -1, 0, 1)
# @test dotest(3, -1, 2, 2)
# @test dotest(3, -1, 3, 2)
# @test dotest(3, -1, 4, 2)
#
# @test dotest(3, 0, 2, 2)
# @test dotest(3, 0, 3, 2)
# @test dotest(3, 0, 4, 2)
#
# @test dotest(3, 1, 2, 2)
# @test dotest(3, 1, 3, 2)
# @test dotest(3, 1, 4, 2)
#
# @test dotest(3, 3, 4, 1)
# @test dotest(3, 5, 4, 0)
#
# 	# arrow configuration:
# @test check_inter(pts(SA[-1 1 1;-1 1 -1;2 2 0]), t3, 2)
#
# 	# border configuration:
# @test check_inter(pts(SA[0 0 0;5 0 0;0 0 1]), t3, 2)
# @test check_inter(pts(SA[-1 1 0; 2 2 0;0 0 1]), t3, 2)
# @test check_inter(pts(SA[1 0 0;3 0 0;0 0 1]), t3, 2)
#
# 	# coplanar
# @test check_inter(pts(SA[1 1 0;2 1 0;1 2 0]), t3, 3)
# @test check_inter(pts(SA[-1 2 0;2 -1 0;6 6 0]), t3, 6)
# @test check_inter(pts(SA[28. 0 0;12 0 0;20 0 -8]),
# 		pts(SA[25. 0 0;20 0 -5;15 0 0]), 2)
# end
# @testset "Surfaces" begin #««1
# using ConstructiveGeometry: Surface, merge, mesh
# # using ConstructiveGeometry: nvertices, nfaces
# v=[[0,-1],[1,0],[0,1],[-1,0]]
# for j in eachindex(v), i in 1:j-1
# 	@test ConstructiveGeometry.circular_lt(v[i], v[j])
# end
# nvf(s) = (CG.nvertices(s), CG.nfaces(s))
# p=Surface([[-1.,0,0],[1,0,0],[0,1,0],[0,0,1]],
#          [[3,2,1],[4,1,2],[4,3,1],[4,2,3]])
# @test nvf(mesh(2p \ p)) == (8, 12)
# # @test connected_components([:a, :b, :c, :d, :e], [[1,2],[1,3],[4,5]]) ==
# # 	[([:a, :b, :c], [[1,2],[1,3]]),
# # 	 ([:d, :e], [[1,2]]) ]
# function pyramid(t=[0,0,0], n=0)
# 	points = ([ t, t+[2.,0,0], t+[2,2,0], t+[0,2,0], t+[1,1,1]])
#   faces = [[4,3,2],[2,1,4],[1,2,5],[2,3,5],[3,4,5],[4,1,5]]
# 	faces1 = [ f .+ n for f in faces ]
# 	return surface(points, faces1)
# end
# p1 = pyramid()
# p2 = pyramid([1,0,0])
# u12 = mesh(p1 ∪ p2)
# i12 = mesh(p1 ∩ p2)
# d12 = mesh(p1 \ p2)
# @test nvf(u12) == (14, 24)
# @test nvf(i12) == (8, 12)
# @test nvf(d12) == (8, 12)
#
# function tetrahedron(;center=[0,0,0], radius=1)
# 	return surface([center+radius*[1,0,-.5], center+radius*[-1,0,-.5],
# 		center+radius*[0,1,.5],center+radius*[0,-1,.5]],
# 		[[1,3,4],[1,2,3],[1,4,2],[2,4,3]])
# end
#
# t1 = tetrahedron()
# t2 = 2t1
# t3 = [3,0,0]+t1
# @test nvf(mesh(t1∪t2)) == (4,4)
# @test nvf(mesh(t1∪t3)) == (8,8)
# @test nvf(mesh(t1∩t2)) == (4,4)
# @test nvf(mesh(t1∩t3)) == (0,0)
# @test nvf(mesh(t1\t1)) == (0,0)
# @test nvf(mesh(t1\t3)) == (4,4)
# @test nvf(mesh(t1\t2)) == (0,0) # t1 ⊂ t2
# @test nvf(mesh(t2\t1)) == (8,8)
#
# end
# @testset "Difference of rotate_extrude()" begin# ««1
# m1=mesh(rotate_extrude(10,[2,0]+circle(1)))
# m2=mesh(rotate_extrude(11,[2,0]+circle(.5)))
# @debug "###### difference ######"
# m3=mesh(m1\m2)
# @test CG.nfaces(m3) == 2*CG.nvertices(m3)
# end
# #»»1
# vim: noet ts=2 fmr=««,»»

# @testset "Spatial sorting" begin#««
# include("../src/SpatialSorting.jl")
# # Bounding box««
# struct BoundingBox{N,T}
# 	min::SVector{N,T}
# 	max::SVector{N,T}
# end
#
# BoundingBox{N}(min::AbstractVector, max::AbstractVector) where{N} =
# 	BoundingBox{N,promote_type(eltype(min), eltype(max))}(min, max)
# BoundingBox(min::StaticVector{N}, max::StaticVector{N}) where{N} =
# 	BoundingBox{N}(min, max)
# function BoundingBox(min::AbstractVector, max::AbstractVector)
# 	length(min) == length(max) || throw(DimensionMismatch(
# 	"min and max must have same length ($(length(min)),$(length(max)))"))
# 	return BoundingBox{length(min)}(min, max)
# end
#
# @inline Base.merge(box1::BoundingBox{N}, box2::BoundingBox{N}) where{N} =
# 	BoundingBox{N}(min.(box1.min, box2.min), max.(box1.max, box2.max))
# @inline Base.:∩(box1::BoundingBox{N}, box2::BoundingBox{N}) where{N} =
# 	BoundingBox{N}(max.(box1.min, box2.min), min.(box1.max, box2.max))
# @inline Base.isempty(box::BoundingBox) = any(box.min .> box.max)
# @inline SpatialSorting.position(box::BoundingBox) = box.min + box.max
#
# function rbb(int1, int2, n)
# 	v = rand(int1, n)
# 	w = rand(int2, n)
# 	return BoundingBox(v, v+w)
# end
# boxes=[ rbb(1:500, 1:10, 3) for _ in 1:3000 ]
#
# function validate(boxes)
# 	int = extrema.(SpatialSorting.intersections(boxes))
# 	for i = 1:length(boxes), j = 1:i-1
# 		a = SpatialSorting.intersects(boxes[i], boxes[j])
# 		b = (j,i) ∈ int
# 		(a ≠ b) && return false
# 	end
# 	return true
# end
#
# @test validate(boxes)
#
# #»»
# end#»»


using JuMP
using ApproxFun
using Pseudospectral
using Plots
using FastGaussQuadrature

moi = JuMP.MathOptInterface

n = 50
const M = zeros(n + 1, n + 1) ; diff_matrix!(M, n)
const w = gausslobatto(n + 1)[2]

function cost(X, U)
    integrand = w .* ((M * U[2, :]).^2)# .+ 0.01 .* (M * U[1, :]).^2)
    sum(integrand)
end

function dynamics(X, U)
    [cos(U[1]) * U[2], sin(U[1]) * U[2]]
end

function path_constraints(X, U)
    sum((X .- 1).^2, dims=1) .- 0.6^2
end

function circle_shape(radius; origin=(0., 0.), n=50)
    xs = range(0, 2pi * (1 + 1 / n), step=2pi / n) |> x -> radius * sin.(x) |> Array{Float64}
    ys = range(0, 2pi * (1 + 1 / n), step=2pi / n) |> x -> radius * cos.(x) |> Array{Float64}
    origin[1] .+ xs, origin[2] .+ ys
end

# TODO: allow for non-uniform bounds
prob = UninitializedProblem(
    dynamics,
    path_constraints,
    cost,
    2,
    2,
    n,
    1e-6,
    Bounds([0., 0.], [2., 2.]),
    Bounds([0, 0], [2pi, 2.]),
    ([0., 0.], [2., 2.])
)

X, U, final_cost, model, τ = Pseudospectral.build_problem(prob);
p = plot(X[1, :], X[2, :], marker_z=τ, marker=:o)
plot!(p, circle_shape(0.6, origin=(1., 1.))..., label="obstacle")
savefig(p, "res.png")
scatter(τ, U[1, :], marker_z=τ, title="θ")
scatter(τ, U[2, :], marker_z=τ, title="v")

@assert (tstatus = termination_status(model); status ∈ [moi.LOCALLY_SOLVED, moi.OPTIMAL]) "Problem not solved ($status)"
@assert (pstatus = primal_status(model); pstatus == moi.FEASIBLE_POINT) "Non-feasible solution (primal - $(pstatus))"
@assert (dstatus = dual_status(model); dstatus == moi.FEASIBLE_POINT) "Non-feasible solution (dual - $(dstatus))"

using DataInterpolations

function reconstruct(X, τ; step=0.01)
    map.(LagrangeInterpolation.([X[1, :], X[2, :], τ], Ref(τ), Ref(length(τ) - 1)), Ref(-1:step:1))
end

lag_x, lag_y = LagrangeInterpolation.([X[1, :], X[2, :]], Ref(τ), Ref(n))
lag_τ = LagrangeInterpolation(τ, τ)

@assert (ext=extrema(lag_τ); ext == (-1, 1)) "Interpolated time is not within [-1, 1] ($(extrema(lag_τ)))"
@assert all(sort(lag_τ) .== lag_τ) "Interpolated time is not monotone ($(lag_y))"

p = plot(X[1, :], X[2, :], marker_z=lag_τ, marker=:o)
plot!(p, circle_shape(0.6, origin=(1., 1.))..., label="obstacle")
scatter(X[1, :], X[2, :], marker_z=τ)
τ_subsampled = -1:0.01:1

xs, ys, ts = reconstruct(X, τ)
scatter(xs, ys, marker_z=ts)


using Distributions, Random
using Test, LinearAlgebra, PDMats

v = 7.0
S = Matrix(1.0I, 2, 2)
S[1, 2] = S[2, 1] = 0.5

W = Wishart(v,S)
IW = InverseWishart(v,S)

rng = MersenneTwister(123)

@testset "Testing matrix-variates with $key" for (key, func) in
    Dict("rand(...)" => [rand, rand],
         "rand(rng, ...)" => [dist -> rand(rng, dist), (dist, n) -> rand(rng, dist, n)])

    for d in [W,IW]
        local d
        @test size(d) == size(func[1](d))
        @test length(d) == length(func[1](d))
        @test rank(d) == rank(func[1](d))
        @test typeof(d)(params(d)...) == d
        @test partype(d) == Float64
    end

    @test partype(Wishart(7, Matrix{Float32}(I, 2, 2))) == Float32
    @test partype(InverseWishart(7, Matrix{Float32}(I, 2, 2))) == Float32

    @test isapprox(mean(func[2](W,100000)) , mean(W) , atol=0.1)
    @test isapprox(mean(func[2](IW,100000)), mean(IW), atol=0.1)
    @test isapprox(cov(hcat(vec.(func[2](W, 1000000))...)'), cov(W) , atol=0.1)
    @test isapprox(cov(hcat(vec.(func[2](IW, 1000000))...)'), cov(IW), atol=0.1)

    v = 3.0

    @test isapprox(pdf(Wishart(v,S), S)         , 0.04507168, atol=1e-8)
    @test isapprox(pdf(Wishart(v,S), inv(S))    , 0.01327698, atol=1e-8)
    @test isapprox(pdf(Wishart(v,inv(S)),S)     , 0.0148086 , atol=1e-8)
    @test isapprox(pdf(Wishart(v,inv(S)),inv(S)), 0.01901462, atol=1e-8)

    @test isapprox(pdf(Wishart(v,S), [S, S])               , [0.04507168, 0.04507168], atol=1e-6)
    @test isapprox(pdf(Wishart(v,S), [inv(S), inv(S)])     , [0.01327698, 0.01327698], atol=1e-6)
    @test isapprox(pdf(Wishart(v,inv(S)), [S, S])          , [0.0148086, 0.0148086]  , atol=1e-6)
    @test isapprox(pdf(Wishart(v,inv(S)), [inv(S), inv(S)]), [0.01901462, 0.01901462], atol=1e-6)

    @test logpdf(Wishart(v,S), S)           ≈ log.(pdf(Wishart(v,S), S))
    @test logpdf(Wishart(v,S), inv(S))      ≈ log.(pdf(Wishart(v,S), inv(S)))
    @test logpdf(Wishart(v,inv(S)), S)      ≈ log.(pdf(Wishart(v,inv(S)), S))
    @test logpdf(Wishart(v,inv(S)), inv(S)) ≈ log.(pdf(Wishart(v,inv(S)), inv(S)))

    @test logpdf(Wishart(v,S), [S, S])                ≈ log.(pdf(Wishart(v,S), [S, S]))
    @test logpdf(Wishart(v,S), [inv(S), inv(S)])      ≈ log.(pdf(Wishart(v,S), [inv(S), inv(S)]))
    @test logpdf(Wishart(v,inv(S)), [S, S])           ≈ log.(pdf(Wishart(v,inv(S)), [S, S]))
    @test logpdf(Wishart(v,inv(S)), [inv(S), inv(S)]) ≈ log.(pdf(Wishart(v,inv(S)), [inv(S), inv(S)]))

    @test isapprox(pdf(InverseWishart(v,S), S)         , 0.04507168 , atol=1e-8)
    @test isapprox(pdf(InverseWishart(v,S), inv(S))    , 0.006247377, atol=1e-8)
    @test isapprox(pdf(InverseWishart(v,inv(S)),S)     , 0.03147137 , atol=1e-8)
    @test isapprox(pdf(InverseWishart(v,inv(S)),inv(S)), 0.01901462 , atol=1e-8)

    @test isapprox(pdf(InverseWishart(v,S), [S, S])               , [0.04507168,  0.04507168] , atol=1e-6)
    @test isapprox(pdf(InverseWishart(v,S), [inv(S), inv(S)])     , [0.006247377, 0.006247377], atol=1e-6)
    @test isapprox(pdf(InverseWishart(v,inv(S)), [S, S])          , [0.03147137,  0.03147137] , atol=1e-6)
    @test isapprox(pdf(InverseWishart(v,inv(S)), [inv(S), inv(S)]), [0.01901462,  0.01901462] , atol=1e-6)

    @test logpdf(InverseWishart(v,S), S)           ≈ log(pdf(InverseWishart(v,S), S))
    @test logpdf(InverseWishart(v,S), inv(S))      ≈ log(pdf(InverseWishart(v,S), inv(S)))
    @test logpdf(InverseWishart(v,inv(S)), S)      ≈ log(pdf(InverseWishart(v,inv(S)), S))
    @test logpdf(InverseWishart(v,inv(S)), inv(S)) ≈ log(pdf(InverseWishart(v,inv(S)), inv(S)))

    @test logpdf(InverseWishart(v,S), [S, S])                ≈ log.(pdf(InverseWishart(v,S), [S, S]))
    @test logpdf(InverseWishart(v,S), [inv(S), inv(S)])      ≈ log.(pdf(InverseWishart(v,S), [inv(S), inv(S)]))
    @test logpdf(InverseWishart(v,inv(S)), [S, S])           ≈ log.(pdf(InverseWishart(v,inv(S)), [S, S]))
    @test logpdf(InverseWishart(v,inv(S)), [inv(S), inv(S)]) ≈ log.(pdf(InverseWishart(v,inv(S)), [inv(S), inv(S)]))
end

@testset "Testing matrix-variates with $key" for (key, func) in
    Dict("rand!(..., false)" => [(dist, X) -> rand!(dist, X, false), rand!],
         "rand!(rng, ..., false)" => [(dist, X) -> rand!(rng, dist, X, false), (dist, X) -> rand!(rng, dist, X)])

    for d in [W,IW]
        local d
        local m = Matrix{Float64}(undef, size(d))
        local x = func[2](d, m)
        @test x ≡ m
        @test size(d) == size(x)
        @test length(d) == length(func[2](d, m))
        @test typeof(d)(params(d)...) == d
        @test partype(d) == Float64
    end

    m = [Matrix{partype(W)}(undef, size(W)) for _ in Base.OneTo(100000)]
    x = func[1](W,m)
    @test x ≡ m
    @test isapprox(mean(x) , mean(W) , atol=0.1)
    x = func[1](IW,m)
    @test x ≡ m
    @test isapprox(mean(x), mean(IW), atol=0.1)

    m3 = Array{partype(W), 3}(undef, size(W)..., 100000)
    x = func[2](W,m3)
    @test x ≡ m3
    @test isapprox(mean(x) , mean(mean(W)) , atol=0.1)
    x = func[2](IW,m3)
    @test x ≡ m3
    @test isapprox(mean(x), mean(mean(IW)), atol=0.1)
end

@testset "Testing matrix-variates with $key" for (key, func) in
    Dict("rand!(..., true)" => (dist, X) -> rand!(dist, X, true),
         "rand!(rng, true)" => (dist, X) -> rand!(rng, dist, X, true))

    m = Vector{Matrix{partype(W)}}(undef, 100000)
    x = func(W,m)
    @test x ≡ m
    @test isapprox(mean(x), mean(W) , atol=0.1)
    x = func(IW,m)
    @test x ≡ m
    @test isapprox(mean(x), mean(IW), atol=0.1)

end

repeats = 10
m = Vector{Matrix{partype(W)}}(undef, repeats)
rand!(W, m)
@test isassigned(m, 1)
m1=m[1]
rand!(W, m)
@test m1 ≡ m[1]
rand!(W, m, true)
@test m1 ≢ m[1]
m1 = m[1]
rand!(W, m, false)
@test m1 ≡ m[1]

@testset "Wishart conversion" for elty in (Float32, Float64, BigFloat)

    Del1 = convert(Wishart{elty}, W)
    Del2 = convert(Wishart{elty}, v, PDMat(S), W.c0)

    @test partype(Del1) == elty
    @test partype(Del2) == elty

end

@testset "InverseWishart conversion" for elty in (Float32, Float64, BigFloat)

    Del1 = convert(InverseWishart{elty}, IW)
    Del2 = convert(InverseWishart{elty}, v, PDMat(S), IW.c0)

    @test partype(Del1) == elty
    @test partype(Del2) == elty

end


# load requried packages
using IncrementalInference
using Test

## parameters

lm_prior_noise = 0.01
meas_noise = 0.25
odom_noise = 0.1
n_samples = 200

# initialize mean landmark locations
l0 = 0.0
l1 = 10.0
l2 = 20.0
l3 = 40.0

# "Ground-truth" robot poses
x0 = 0.0
x1 = 10.0
x2 = 20.0
x3 = 40.0

##

@testset "Basic 3 door, trinary multihypothesis tests..." begin

## Initialize empty factor graph
fg = initfg()
getSolverParams(fg).N = n_samples
getSolverParams(fg).gibbsIters = 5

# Place strong prior on locations of three "doors"
addVariable!(fg, :l0, ContinuousScalar, N=n_samples)
addFactor!(fg, [:l0], Prior(Normal(l0, lm_prior_noise)))

addVariable!(fg, :l1, ContinuousScalar, N=n_samples)
addFactor!(fg, [:l1], Prior(Normal(l1, lm_prior_noise)))

addVariable!(fg, :l2, ContinuousScalar, N=n_samples)
addFactor!(fg, [:l2], Prior(Normal(l2, lm_prior_noise)))

addVariable!(fg, :l3, ContinuousScalar, N=n_samples)
addFactor!(fg, [:l3], Prior(Normal(l3, lm_prior_noise)))

# Add first pose
addVariable!(fg, :x0, ContinuousScalar, N=n_samples)

# Make first "door" measurement
addFactor!(fg, [:x0; :l0; :l1; :l2; :l3], LinearRelative(Normal(0, meas_noise)), multihypo=[1.0; (1/4 for _=1:4)...])

# Add second pose
addVariable!(fg, :x1, ContinuousScalar, N=n_samples)

# Gaussian transition model
addFactor!(fg, [:x0; :x1], LinearRelative(Normal(x1-x0, odom_noise)))

# Make second "door" measurement
addFactor!(fg, [:x1; :l0; :l1; :l2; :l3], LinearRelative(Normal(0, meas_noise)), multihypo=[1.0; (1/4 for _=1:4)...])

##

solveGraph!(fg)

##

# check there is enough likelihood in the right places
@test 0.1 < getBelief(fg, :x0)([l0])[1]
@test 0.1 < getBelief(fg, :x0)([l1])[1]
@test getBelief(fg, :x0)([l2])[1] < 0.3

@test getBelief(fg, :x1)([l0])[1] < 0.3
@test 0.1 < getBelief(fg, :x1)([l1])[1]
@test 0.1 < getBelief(fg, :x1)([l2])[1]

##

for i in 1:3
  solveGraph!(fg);
end

##

# check there is enough likelihood in the right places
@test 0.1 < getBelief(fg, :x0)([l0])[1]
@test 0.1 < getBelief(fg, :x0)([l1])[1]
@test getBelief(fg, :x0)([l2])[1] < 0.03

@test getBelief(fg, :x1)([l0])[1] < 0.03
@test 0.1 < getBelief(fg, :x1)([l1])[1]
@test 0.1 < getBelief(fg, :x1)([l2])[1]

## Add one more pose/odometry to invoke issue #236

# Add third pose
addVariable!(fg, :x2, ContinuousScalar, N=n_samples)
addFactor!(fg, [:x1; :x2], LinearRelative(Normal(x2-x1, odom_noise)))

## Solve graph

tree = solveTree!(fg)
# drawGraph(fg)
# drawTree(tree, show=true)

##

# check there is enough likelihood in the right places
@test 0.1 < getBelief(fg, :x0)([l0])[1]
@test 0.1 < getBelief(fg, :x0)([l1])[1]
@test getBelief(fg, :x0)([l2])[1] < 0.03

@test getBelief(fg, :x1)([l0])[1] < 0.03
@test 0.1 < getBelief(fg, :x1)([l1])[1]
@test 0.1 < getBelief(fg, :x1)([l2])[1]

dx = x2-x1
@test getBelief(fg, :x2)([l0 + dx])[1] < 0.03
@test 0.1 < getBelief(fg, :x2)([l1 + dx])[1]
@test 0.1 < getBelief(fg, :x2)([l2 + dx])[1]

##

# Add third pose
addVariable!(fg, :x3, ContinuousScalar, N=n_samples)
addFactor!(fg, [:x2; :x3], LinearRelative(Normal(x3-x2, odom_noise)))

# Make third "door" measurement
addFactor!(fg, [:x3; :l0; :l1; :l2; :l3], LinearRelative(Normal(0, meas_noise)), multihypo=[1.0; (1/4 for _=1:4)...])

##

solveGraph!(fg)

##

@test isapprox(mean(getBelief(fg, :x0))[1], x0; atol = 2.0)
@test isapprox(mean(getBelief(fg, :x1))[1], x1; atol = 2.0)
@test isapprox(mean(getBelief(fg, :x2))[1], x2; atol = 2.0)
@test isapprox(mean(getBelief(fg, :x3))[1], x3; atol = 2.0)

@test isapprox(mean(getBelief(fg, :l0))[1], l0; atol = 3.0)
@test isapprox(mean(getBelief(fg, :l1))[1], l1; atol = 3.0)
@test isapprox(mean(getBelief(fg, :l2))[1], l2; atol = 3.0)
@test isapprox(mean(getBelief(fg, :l3))[1], l3; atol = 3.0)

##

# check the PPEs are the same
@test isapprox(getPPE(fg, :x0).suggested[1], x0; atol = 2.0)
@test isapprox(getPPE(fg, :x1).suggested[1], x1; atol = 2.0)
@test isapprox(getPPE(fg, :x2).suggested[1], x2; atol = 2.0)
@test isapprox(getPPE(fg, :x3).suggested[1], x3; atol = 2.0)

@test isapprox(getPPE(fg, :l0).suggested[1], l0; atol = 3.0)
@test isapprox(getPPE(fg, :l1).suggested[1], l1; atol = 3.0)
@test isapprox(getPPE(fg, :l2).suggested[1], l2; atol = 3.0)
@test isapprox(getPPE(fg, :l3).suggested[1], l3; atol = 3.0)

##

end

# using RoMEPlotting
# Gadfly.set_default_plot_size(35cm,25cm)
#
# plotBelief(fg, sortDFG(ls(fg, r"x")))

#


using FixedEffectModels, DataFrames, CSV, Test

df = CSV.read(joinpath(dirname(pathof(FixedEffectModels)), "../dataset/Cigar.csv"))
df.pState = categorical(df.State)
df.pYear = categorical(df.Year)

##############################################################################
##
## Printing Results
##
##############################################################################

model = @model Sales ~ NDI
result = reg(df, model)
show(result)
predict(result, df)
residuals(result, df)

model = @model Sales ~ CPI + (Price ~ Pimin)
result = reg(df, model)
show(result)
predict(result, df)
residuals(result, df)
@test  nobs(result) == 1380
@test vcov(result)[1] ≈ 3.5384578251636785

model = @model Sales ~ Price  fe = pState
result = reg(df, model)
show(result)
model = @model Sales ~ CPI + (Price ~ Pimin) fe = pState
result = reg(df, model)
show(result)

##############################################################################
##
## Saved Residuals
##
##############################################################################

model = @model Sales ~ Price
result = reg(df, model)
@test residuals(result, df)[1:10] ≈ [-39.2637, -37.48801, -34.38801, -36.09743, -36.97446, -43.15547, -41.22573, -40.83648, -34.52427, -28.91617] atol = 1e-4
@test r2(result) ≈ 0.0968815737054879 atol = 1e-4
@test adjr2(result) ≈ 0.0962261902321246 atol = 1e-4
@test result.nobs == 1380
@test result.F ≈ 147.8242550248069 atol= 1e-4

#weights
model = @model Sales ~ CPI  weights = Pop
result = reg(df, model)
@test residuals(result, df)[1:3] ≈ [ -35.641449,  -34.0611538,  -30.860784] atol = 1e-4

# iv
model = @model Sales ~ CPI + (Price ~ Pimin)
result = reg(df, model)
@test residuals(result, df)[1:3] ≈ [ -33.047390, -30.9518422, -28.1371048] atol = 1e-4

# iv and weights
model = @model Sales ~ CPI + (Price ~ Pimin) weights = Pop
result = reg(df, model)
@test residuals(result, df)[1:3] ≈ [ -30.2284549, -28.09507, -25.313248] atol = 1e-4

# iv, weights and subset of states
model = @model Sales ~ CPI + (Price ~ Pimin) weights = Pop subset = (State .<= 30)
result = reg(df, model)
@test residuals(result, df)[1:3] ≈ [ -34.081720, -31.906020, -29.131738] atol = 1e-4

# fixed effects
model = @model Sales ~ Price fe = pState
result = reg(df, model, save = true)
@test result.augmentdf.residuals[1:3] ≈ [-22.08499, -20.33318, -17.23318] atol = 1e-4
@test result.nobs == 1380
@test r2(result) ≈ 0.7682403747044817 atol = 1e-4
@test adjr2(result) ≈ 0.7602426682051615 atol = 1e-4
@test result.F ≈ 458.4582526109375 atol = 1e-4

# fixed effects and weights
model = @model Sales ~ Price fe = pState weights = Pop
result = reg(df, model, save = true)
@test result.augmentdf.residuals[1:3] ≈ [ -23.413793, -21.65289, -18.55289] atol = 1e-4

# fixed effects and iv
#TO CHECK WITH IVREGHDFE, NO SUPPORT RIGHT NOW
model = @model Sales ~ CPI + (Price ~ Pimin)  fe = pState
result = reg(df, model, save = true)
@test result.augmentdf.residuals[1:3] ≈ [ -16.925748, -14.835710, -12.017037] atol = 1e-4

# test different arguments for the keyword argument save
model = @model Sales ~ Price fe = pState
result = reg(df, model, save = true)
@test :residuals ∈ names(result.augmentdf)
@test :pState ∈ names(result.augmentdf)

model = @model Sales ~ Price fe = pState
result = reg(df, model, save = :residuals)
@test :residuals ∈ names(result.augmentdf)
@test :pState ∉ names(result.augmentdf)

model = @model Sales ~ Price fe = pState
result = reg(df, model, save = :fe)
@test :residuals ∉ names(result.augmentdf)
@test :pState ∈ names(result.augmentdf)

##############################################################################
##
## Saved FixedEffects
##
##############################################################################

method_s = Base.USE_GPL_LIBS ? [:cholesky, :qr, :lsmr, :lsmr_parallel, :lsmr_threads] : [:lsmr, :lsmr_parallel, :lsmr_threads]

for method in method_s
	model = @model Sales ~ Price fe = pYear
	result = reg(df, model, save = true, method = method)
	@test fes(result)[1, :pYear] ≈ 164.77833189721005

	model = @model Sales ~ Price fe = pYear + pState
	result = reg(df, model, save = true, method = method)
	@test fes(result)[1, :pYear] + fes(result)[1, :pState] ≈ 140.6852 atol = 1e-3

	model = @model Sales ~ Price fe = Year&pState
	result = reg(df, model, save = true, method = method)
	@test fes(result)[1, :YearxpState] ≈ 1.742779  atol = 1e-3

	model = @model Sales ~ Price fe = pState + Year&pState
	result = reg(df, model, save = true, method = method)
	@test fes(result)[1, :pState] ≈ -91.690635 atol = 1e-1

	model = @model Sales ~ Price fe = pState subset = (State .<= 30)
	result = reg(df, model, save = true, method = method)
	@test fes(result)[1,:pState] ≈  124.913976 atol = 1e-1
	@test ismissing(fes(result)[1380,:pState])
end

# fixed effects
df.Price2 = df.Price
model = @model Sales ~ Price + Price2 fe = pYear
result = reg(df, model, save = true)
@test fes(result)[1, :pYear] ≈ 164.77833189721005

# iv
model = @model Sales ~ (State ~ Price) fe = pYear
result = reg(df, model, save = true)
@test fes(result)[1, :pYear] ≈ -167.48093490413623

# weights
model = @model Sales ~ Price weights = Pop fe = pYear
result = reg(df, model, save = true)
@test fes(result)[2, :pYear] -  fes(result)[1, :pYear] ≈ -3.0347149502496222

# check save does not change r2
model1 = @model Sales ~ Price weights = Pop
result1 = reg(df, model1)
model2 = @model Sales ~ Price weights = Pop
result2 = reg(df, model2)
@test r2(result1) ≈ r2(result2)

# IV and weights
model = @model Sales ~ (Price ~ Pimin) weights = Pop fe = pYear
result = reg(df, model, save = true)
@test fes(result)[1, :pYear] ≈ 168.24688 atol = 1e-4

# IV, weights and both year and state fixed effects
model = @model Sales ~ (Price ~ Pimin) weights = Pop fe = pState + pYear
result = reg(df, model, save = true)
@test fes(result)[1, :pYear] + fes(result)[1, :pState]≈ 147.84145 atol = 1e-4

# subset with IV
model = @model Sales ~ (Price ~ Pimin) fe = pYear  subset = (State .<= 30)
result = reg(df, model, save = true)
@test fes(result)[1, :pYear] ≈ 164.05245824240276 atol = 1e-4
@test ismissing(fes(result)[811, :pYear])

# subset with IV, weights and year fixed effects
model = @model Sales ~ (Price ~ Pimin) weights = Pop fe = pYear subset = (State .<= 30)
result = reg(df, model, save = true)
@test fes(result)[1, :pYear] ≈ 182.71915 atol = 1e-4

# subset with IV, weights and year fixed effects
model = @model Sales ~ (Price ~ Pimin) weights = Pop fe = pState + pYear subset = (State .<= 30)
result = reg(df, model, save = true)
@test fes(result)[1, :pYear] + fes(result)[1, :pState] ≈ 158.91798 atol = 1e-4


using AutoDiff
using MNIST

type SimpleParameter <: Parameter
	data
	timestamp
end

function SimpleParameter(data)
	return SimpleParameter(data,now())
end

type SimpleGradient <: Gradient
	data
end

function update(p::SimpleParameter, g::SimpleGradient)
	# update p with parameter g
	p.data-=g.data
	p.timestamp=now()
end

function zero_gradient(p::SimpleParameter)
	data = zeros( size(p.data, 1), size(p.data, 2) );
	return SimpleGradient(data);
end

#Replace error with an arbitrary function to minimize
#datum should be a tuple (digit,label) where label is length 10 array with a single 1
function loss(params::Array,datum)
	weights1=params[1]
	biases1=params[2]

	layer1=weights1*datum[1].+biases1
	tmp=exp(layer1)
	prediction=tmp./sum(tmp)

	error=sum((prediction-datum[2]).^2)
end

function loss(params::SimpleParameter, datum)
	loss(params.data,datum)
end
srand(1)
#Dummy inputs used for initializing variables in the gradient computation
dummy_weights1=0.001*randn((10,784))
dummy_biases1=0.001*randn((10,))
dummy_input=zeros(Float64,(784,))
dummy_output=zeros(Float64,(10,))

#assume dataset is a list of indices
function compute_gradient(state::WorkerState,dataset)
	println("[WORKER] Computing gradients")
	state.update_params(state.current_params.data)
	g=0
	for i in dataset
		example=trainfeatures(i)
		label=map(x->if x==trainlabel(i); 1.0; else 0.0; end, 0:9)
		g+=state.compute_gradient((example,label))
	end
	sleep(0.1)
	return SimpleGradient(state.learning_rate*g/length(dataset))
end


#
# This file is a part of MolecularGraph.jl
# Licensed under the MIT License http://opensource.org/licenses/MIT
#

export
    minimumcyclebasis

function findmincycle(graph, S)
    G = edgesubgraph(graph, setdiff(edgeset(graph), S))
    U = disjointunion(G, G)
    for s in S
        (u, v) = getedge(graph, s)
        u1 = getunionnode(U, 1, u)
        v1 = getunionnode(U, 1, v)
        u2 = getunionnode(U, 2, u)
        v2 = getunionnode(U, 2, v)
        addedge!(U, u1, v2, DisjointUnionEdge(1, s))
        addedge!(U, u2, v1, DisjointUnionEdge(1, s))
    end
    paths = []
    for n in nodeset(graph)
        n1 = getunionnode(U, 1, n)
        n2 = getunionnode(U, 2, n)
        sp = shortestpathedges(U, n1, n2)
        push!(paths, sp)
    end
    minpath = sortstablemin(paths, by=length)
    return [getsourceedge(U, e).sourcekey for e in minpath]
end

"""
    minimumcyclebasis(graph::UndirectedGraph) -> Vector{Vector{Int}}

Returns minimum cycle basis represented as an array of edge sequence that make up a cycle.

# Reference

1. de Pina, J.C. Applications of shortest path methods. PhD thesis, University of Amsterdam,
   Nether-lands (1995)
1. Kavitha, T., Mehlhorn, K., Michail, D. & Paluch, K. E. An ``\\tilde{O}(m^{2}n)`` Algorithm for
   Minimum Cycle Basis of Graphs. Algorithmica 52, 333–349 (2008).
"""
@cachefirst function minimumcyclebasis(graph::UndirectedGraph)
    cycles = Vector{Int}[]
    for conn in connectedcomponents(graph)
        subg = nodesubgraph(graph, conn)
        S = [Set(e) for e in cotree_edges(subg)]
        N = length(S)  # N: circuit rank
        for k in 1:N
            minedges = findmincycle(subg, S[k])
            push!(cycles, minedges)
            for i in (k + 1):N
                if length(intersect(S[i], minedges)) % 2 == 1
                    S[i] = symdiff(S[i], S[k])
                end
            end
        end
    end
    return cycles
end