text:
include("preamble.jl")

# --------------------------------------------
# Overapproximation example
# --------------------------------------------

X = VPolygon([[0.8, 2.2], [2.1, 1], [1.9, -1], [0, -2],
              [-2, -2], [-3, 0.6], [-0.8, 1.8], [0.8, 2.2]])
Y = box_approximation(X)
Z1 = overapproximate(X, Zonotope, PolarDirections(3))
Z2 = overapproximate(X, Zonotope, PolarDirections(5))

fig = p(xlim=(-4.1, 2.5), ylim=(-2.5, 2.5), ratio=1, xlab="", ylab="",
           legendfontsize=25,
           tickfont=font(25, "Times"),
           guidefontsize=25,
           xguidefont=font(25, "Times"),
           yguidefont=font(25, "Times"),
           xtick = ([-4, -2, 0, 2], [L"-4", L"-2", L"0", L"2"]),
           ytick = ([-2, 0, 2], [L"-2", L"0", L"2"]),
           bottom_margin=0mm, left_margin=0mm, right_margin=0mm, top_margin=0mm, size=(500, 360))

p!(Y, alpha=0.8)
p!(Z1, alpha=0.8)
p!(Z2, alpha=0.8)
p!(X, color=:orange, alpha=0.8)

savefig("overapproximate.pdf")

# -------------------
# Polar directions
# -------------------

# ball
fig = p(xlim=(-1.1, 1.1), ylim=(-1.1, 1.1), ratio=1, xlab="", ylab="",
           legendfontsize=25,
           tickfont=font(25, "Times"),
           guidefontsize=25,
           xguidefont=font(25, "Times"),
           yguidefont=font(25, "Times"),
           xtick = ([-1, 0, 1], [L"-1", L"0", L"1"]),
           ytick = ([-1, 0, 1], [L"-1", L"0", L"1"]),
           bottom_margin=0mm, left_margin=0mm, right_margin=0mm, top_margin=0mm, size=(380, 360))

plot!(fig, Ball2(zeros(2), 1.0), alpha=.5, ratio=1., lc=:black, lw=1, c=:lightblue)

# five directions
plot!(fig, map(p -> LineSegment(zeros(2), p), PolarDirections(5)), lw=5.,
               arrow=:simple, alpha=1, markershape=:none, seriestype=:path, c=:green)

# three directions
plot!(fig, map(p -> LineSegment(zeros(2), p), PolarDirections(3)), lw=5.,
           arrow=:simple, alpha=1, markershape=:none, seriestype=:path, c=:red, ls=:dot)

savefig(fig, "polardirs.pdf")

# --------------------------------------------
# Lazy set Ω₀ = CH(X₀, Φ*X₀ ⊕ Y) example
# --------------------------------------------

A = [0 1; -(4π)^2 0]
X₀ = BallInf([1.0, 0.0], 0.1)
δ = 0.025
Φ = exp(A*δ)

r = [0.05477208, 0.07676220]
Y = Hyperrectangle(zeros(2), r)

Ω₀ = CH(X₀, Φ*X₀ ⊕ Y)

# make boxes with rounded corners? (doesn't work)
# using TreeView: TikzGraphs # required to change plot aspects
#t = TikzGraphs.plot(t, node_style="draw, rounded corners, fill=blue!10")
#TikzGraphs.plot(g, ["α", "β", "γ", "α"], node_style="draw, rounded corners, # fill=blue!10", node_styles=Dict(1=>"fill=green!10",3=>"fill=yellow!10"))


text:
include("V_based_policy.jl")
include("Q_based_policy.jl")
include("off_policy.jl")

ext: jl


text:
# =========================================================================
#    ENVIRONMENT SETTINGS
# =========================================================================

const default_optimizer = Ref{Any}(nothing)

"""
    set_default_optimizer!(O)
Sets the default optimizer. For example,
    using GLPK
    set_default_optimizer!(GLPK.Optimizer)
"""

set_default_optimizer!(O) = default_optimizer[] = O

"""
    get_default_optimizer()
Gets the default optimizer, which is set by `set_default_optimizer`.
"""

get_default_optimizer() = default_optimizer[]

"""
    reset_default_optimizer()
Resets the default optimizer.
"""

reset_default_optimizer() = default_optimizer[] = nothing


text:
"""
    covariance!(Σ, kernel_func, x1list, x2list, kernel_hyperparameters; dorder=zeros(Int64, 2 + length(kernel_hyperparameters)), symmetric=false)

Fills `Σ` with a covariance matrix by evaluating `kernel_func` with
`kernel_hyperparameters`for each pair of `x1list` and `x2list` entries.

# Keyword Arguments
- `dorder::Vector{T2}=zeros(Int64, 2 + length(kernel_hyperparameters))`: How often to differentiate the covariance function w.r.t each `kernel_hyperparameter`. (i.e. dorder=[1, 0, 1] would correspond to differenting once w.r.t. the first and third `kernel_hyperparameter`)
- `symmetric::Bool=false`: If you know that the resulting covariance matrix should be symmetric, setting this to `true` can reduce redundant calculations
"""
function covariance!(
    Σ::DenseArray{T1,2},
    kernel_func::Function,
    x1list::Vector{T1},
    x2list::Vector{T1},
    kernel_hyperparameters::Vector{T1};
    dorder::Vector{T2}=zeros(Int64, 2 + length(kernel_hyperparameters)),
    symmetric::Bool=false
    ) where {T1<:Real, T2<:Integer}

    @assert issorted(x1list)

    # are the list of x's passed identical
    same_x = (x1list == x2list)

    # are the x's passed identical and equally spaced
    if same_x
        spacing = x1list[2:end]-x1list[1:end-1]
        equal_spacing = all((spacing .- spacing[1]) .< (1e-8 * spacing[1]))
    else
        @assert issorted(x2list)
        equal_spacing = false
    end

    x1_length = length(x1list)
    x2_length = length(x2list)

    @assert size(Σ, 1) == x1_length
    @assert size(Σ, 2) == x2_length

    if equal_spacing && symmetric
        # this section is so fast, it isn't worth parallelizing
        kernline = zeros(x1_length)
        for i in 1:x1_length
            kernline[i] = kernel_func(kernel_hyperparameters, x1list[1] - x1list[i], dorder)
        end
        for i in 1:x1_length
            Σ[i, i:end] = kernline[1:(x1_length + 1 - i)]
        end
        return Symmetric(Σ)
    else
        _covariance!(Σ, kernel_func, x1list, x2list, kernel_hyperparameters, dorder, symmetric, same_x, equal_spacing)
    end
end

"""
    _covariance!(Σ::SharedArray, kernel_func, x1list, x2list, kernel_hyperparameters, dorder, symmetric, same_x, equal_spacing)

Fills `Σ` in parallel with a covariance matrix by evaluating `kernel_func` with
`kernel_hyperparameters`for each pair of `x1list` and `x2list` entries. Be
careful using a function that starts with a `_`.
"""
function _covariance!(
    Σ::SharedArray{T1,2},
    kernel_func::Function,
    x1list::Vector{T1},
    x2list::Vector{T1},
    kernel_hyperparameters::Vector{T1},
    dorder::Vector{T2},
    symmetric::Bool,
    same_x::Bool,
    equal_spacing::Bool
    ) where {T1<:Real, T2<:Integer}

    x1_length = length(x1list)
    x2_length = length(x2list)

    @assert size(Σ) == (x1_length, x2_length)

    if same_x && symmetric
        sendto(workers(), kernel_func=kernel_func, kernel_hyperparameters=kernel_hyperparameters, x1list=x1list, dorder=dorder)
        @sync @distributed for i in 1:length(x1list)
            for j in 1:length(x1list)
                if i <= j; Σ[i, j] = kernel_func(kernel_hyperparameters, x1list[i] - x1list[j], dorder) end
            end
        end
        return Symmetric(Σ)
    else
        sendto(workers(), kernel_func=kernel_func, kernel_hyperparameters=kernel_hyperparameters, x1list=x1list, x2list=x2list, dorder=dorder)
        @sync @distributed for i in 1:length(x1list)
            for j in 1:length(x2list)
                Σ[i, j] = kernel_func(kernel_hyperparameters, x1list[i] - x2list[j], dorder)
            end
        end
        return Σ
    end
end

"""
_covariance!(Σ::Matrix, kernel_func, x1list, x2list, kernel_hyperparameters, dorder, symmetric, same_x, equal_spacing)

Fills `Σ` serially with a covariance matrix by evaluating `kernel_func` with
`kernel_hyperparameters`for each pair of `x1list` and `x2list` entries. Be
careful using a function that starts with a `_`.
"""
function _covariance!(
    Σ::Matrix{T1},
    kernel_func::Function,
    x1list::Vector{T1},
    x2list::Vector{T1},
    kernel_hyperparameters::Vector{T1},
    dorder::Vector{T2},
    symmetric::Bool,
    same_x::Bool,
    equal_spacing::Bool
    ) where {T1<:Real, T2<:Integer}

    x1_length = length(x1list)
    x2_length = length(x2list)

    @assert size(Σ) == (x1_length, x2_length)

    if same_x && symmetric
        for i in 1:length(x1list)
            for j in 1:length(x1list)
                if i <= j; Σ[i, j] = kernel_func(kernel_hyperparameters, x1list[i] - x1list[j], dorder) end
            end
        end
        return Symmetric(Σ)
    else
        for i in 1:length(x1list)
            for j in 1:length(x2list)
                Σ[i, j] = kernel_func(kernel_hyperparameters, x1list[i] - x2list[j], dorder)
            end
        end
        return Σ
    end
end

"""
    covariance(kernel_func, x1list, x2list, kernel_hyperparameters; kwargs...)

Calculates a covariance matrix by evaluating `kernel_func` with
`kernel_hyperparameters`for each pair of `x1list` and `x2list` entries.

See also: [`covariance!`](@ref)
"""
function covariance(
    kernel_func::Function,
    x1list::Vector{T1},
    x2list::Vector{T1},
    kernel_hyperparameters::Vector{T1};
    kwargs...) where {T1<:Real}

    if nworkers()>1
        return covariance!(SharedArray{Float64}(length(x1list), length(x2list)), kernel_func, x1list, x2list, kernel_hyperparameters; kwargs...)
    else
        return covariance!(zeros(length(x1list), length(x2list)), kernel_func, x1list, x2list, kernel_hyperparameters; kwargs...)
    end
end

"""
    covariance(glo, x1list, x2list, total_hyperparameters; dΣdθs_total=Int64[], kwargs...)

Calculates the total GLOM covariance matrix by combining the latent covariance
matrices implied by `glo` for each pair of `x1list` and `x2list` entries.

# Notable Arguments
- `total_hyperparameters::Vector`: The current `a` values (GLOM coeffients that describe how to combine the differentiated versions of the latent GP) followed by the `kernel_hyperparameters` (i.e. lengthscales and periods)
- `dΣdθs_total=Int64[]`: Which of the `total_hyperparameters` to differentiate the covariance function w.r.t. (i.e. for a `glo` with 6 `a` values and 2 kernel hyperparameters, dΣdθs_total=[4, 8] would correspond to differenting once w.r.t. the fourth `a` value and second kernel hyperparameter)

See also: [`covariance!`](@ref)
"""
function covariance(
    glo::GLO,
    x1list::Vector{<:Real},
    x2list::Vector{<:Real},
    total_hyperparameters::Vector{<:Real};
    dΣdθs_total::Vector{<:Integer}=Int64[],
    kwargs...)

    @assert all(dΣdθs_total .>= 0)
    @assert length(total_hyperparameters) == glo.n_kern_hyper + length(glo.a)

    num_coefficients = length(total_hyperparameters) - glo.n_kern_hyper
    n_out = glo.n_out
    n_dif = glo.n_dif
    dΣdθs_kernel = dΣdθs_total .- num_coefficients
    kernel_hyperparameters = total_hyperparameters[(num_coefficients + 1):end]
    # println(length(kernel_hyperparameters))

    x1_length = length(x1list)
    x2_length = length(x2list)

    if nworkers()>1
        holder = SharedArray{Float64}(length(x1list), length(x2list))
    else
        holder = zeros(length(x1list), length(x2list))
    end

    # only calculating each sub-matrix once and using the fact that they should
    # be basically the same if the kernel has been differentiated the same amount of times
    A_list = Vector{AbstractArray{<:Real,2}}(undef, 2 * n_dif - 1)
    dorder = zeros(Int64, 2 + length(kernel_hyperparameters))
    for dΣdθ_kernel in dΣdθs_kernel
        if dΣdθ_kernel > 0; dorder[2 + dΣdθ_kernel] += 1 end
    end
    if !glo.kernel_changes_with_output
        for i in 0:(2 * n_dif - 2)
            dorder[1] = rem(i - 1, 2) + 1
            dorder[2] = 2 * div(i - 1, 2)
            # things that have been differentiated an even amount of times are symmetric about t1-t2==0
            if iseven(i)
                A_list[i + 1] = copy(covariance!(holder, glo.kernel, x1list, x2list, kernel_hyperparameters; dorder=dorder, symmetric=true))
            else
                A_list[i + 1] = copy(covariance!(holder, glo.kernel, x1list, x2list, kernel_hyperparameters; dorder=dorder))
            end
        end
    end
    # assembling the coefficient matrix
    a = reshape(total_hyperparameters[1:num_coefficients], (n_out, n_dif))

    # initializing the multi-output covariance matrix
    Σ = zeros((n_out * x1_length, n_out * x2_length))

    coeff_orders = copy(glo.coeff_orders)
    coeff_coeffs = copy(glo.coeff_coeffs)
    dif_coefficients!(n_out, n_dif, dΣdθs_total, coeff_orders, coeff_coeffs)
    for i in 1:n_out
        for j in 1:n_out
            # if i <= j  # only need to calculate one set of the off-diagonal blocks
            if glo.kernel_changes_with_output
                outputs = [i,j]
                for i in 0:(2 * n_dif - 2)
                    dorder[1] = rem(i - 1, 2) + 1
                    dorder[2] = 2 * div(i - 1, 2)
                    # things that have been differentiated an even amount of times are symmetric about t1-t2==0
                    if iseven(i)
                        A_list[i + 1] = copy(covariance!(holder, (hyper, δ, dord) -> glo.kernel(hyper, δ, dord; outputs=outputs), x1list, x2list, kernel_hyperparameters; dorder=dorder, symmetric=true))
                    else
                        A_list[i + 1] = copy(covariance!(holder, (hyper, δ, dord) -> glo.kernel(hyper, δ, dord; outputs=outputs), x1list, x2list, kernel_hyperparameters; dorder=dorder))
                    end
                end
            end
                for k in 1:n_dif
                    for l in 1:n_dif
                        # if the coefficient for the GLOM coefficients is non-zero
                        if coeff_coeffs[i, j, k, l] != 0
                            # make it negative or not based on how many times it has been differentiated in the x1 direction
                            A_mat_coeff = coeff_coeffs[i, j, k, l] * powers_of_negative_one(l - 1)
                            for m in 1:n_out
                                for n in 1:n_dif
                                    A_mat_coeff *= a[m, n] ^ coeff_orders[i, j, k, l, m, n]
                                end
                            end
                            # add the properly negative differentiated A matrix from the list
                            Σ[((i - 1) * x1_length + 1):(i * x1_length),
                                ((j - 1) * x2_length + 1):(j * x2_length)] +=
                                A_mat_coeff * A_list[k + l - 1]
                        end
                    end
                end
            # end
        end
    end

    # making the highest variances all along the main diagonals
    #TODO implement in above step (aka at initial assignment)
    Σ_rearranged = zeros((n_out * x1_length, n_out * x2_length))
    for i in 1:x1_length
        for j in 1:x2_length
            Σ_rearranged[1 + (i - 1) * n_out:i * n_out,1 + (j - 1) * n_out:j * n_out] = Σ[i:x1_length:end,:][:, j:x2_length:end]
        end
    end
    # return the symmetrized version of the covariance matrix
    # function corrects for numerical errors and notifies us if our matrix isn't
    # symmetric like it should be
    return symmetric_A(Σ_rearranged; kwargs...)
    # if x1list == x2list
    #     if chol
    #         return ridge_chol(Symmetric(Σ))
    #     else
    #         return Symmetric(Σ)
    #     end
    # else
    #     return Σ
    # end

end
"""
    covariance(glo, total_hyperparameters; kwargs...)

Calculates the total GLOM covariance matrix by combining the latent covariance
matrices implied by `glo` at each `glo.x_obs`

See also: [`covariance`](@ref)
"""
covariance(
    glo::GLO,
    total_hyperparameters::Vector{<:Real};
    kwargs...
    ) = covariance(glo, glo.x_obs, glo.x_obs, total_hyperparameters; kwargs...)

"""
    Σ_observations(Σ, measurement_noise::Vector; return_both=false, kwargs...)

Add `measurement_noise`^2 to the diagonal of `Σ` and performs a Cholesky
factorization. Optionally returns the `Σ` back as well.
"""
function Σ_observations(
    Σ::Symmetric{T, Matrix{T}},
    measurement_noise::Vector{T};
    return_both::Bool=false,
    kwargs...) where {T<:Real}

    Σ_obs = symmetric_A(Σ + Diagonal(measurement_noise .* measurement_noise); chol=true, kwargs...)
    return return_both ? (Σ_obs, Σ) : Σ_obs
end

"""
    Σ_observations(Σ, measurement_covariance::Array{T, 3}; return_both=false, kwargs...)

Add `measurement_covariance` to the block diagonal of `Σ` and performs a
Cholesky factorization. Optionally returns the `Σ` back as well.
"""
function Σ_observations(
    Σ::Symmetric{T, Matrix{T}},
    measurement_covariance::Array{T, 3};
    return_both::Bool=false,
    kwargs...) where {T<:Real}

    n_meas = size(measurement_covariance, 1)
    n_out = size(measurement_covariance, 2)
    @assert n_out == size(measurement_covariance, 3) == size(Σ, 2) / n_meas
    Σ_obs = Matrix(Σ)
    # for i in 1:n_out
    #     for j in 1:n_out
    #         if i <= j; Σ_obs[diagind(Σ_obs, i - 1)[((j - i) * n_meas + 1):((j - i + 1) * n_meas)]] += measurement_covariance[:, i, j] end
    #     end
    # end
    for i in 1:n_meas
        Σ_obs[1 + (i - 1) * n_out : i * n_out, 1 + (i - 1) * n_out : i * n_out] += measurement_covariance[i, :, :]
    end
    Σ_obs = symmetric_A(Σ_obs; chol=true, kwargs...)
    return return_both ? (Σ_obs, Σ) : Σ_obs
end

"""
    Σ_observations(kernel_func, x_obs, measurement_noise, kernel_hyperparameters; ignore_asymmetry=false, return_both)

Calculates the covariance matrix of `kernel_func` at `x_obs` and adds
`measurement_noise`^2 to the diagonal and performs a Cholesky factorization.
Optionally returns the `Σ` back as well.
"""
Σ_observations(
    kernel_func::Function,
    x_obs::Vector{T},
    measurement_noise::Vector{T},
    kernel_hyperparameters::Vector{T};
    ignore_asymmetry::Bool=false,
    kwargs...
    ) where {T<:Real} =
    Σ_observations(
        symmetric_A(
            covariance(kernel_func, x_obs, x_obs, kernel_hyperparameters);
            ignore_asymmetry=ignore_asymmetry),
        measurement_noise;
        kwargs...)

"""
    Σ_observations(glo, total_hyperparameters; ignore_asymmetry=false, return_both=false)

Calculates a Cholesky decomposition of the GLOM covariance matrix implied by
`glo` and `total_hyperparameters` including `glo.noise` or `glo.covariance` on
the (block) diagonal
"""
function Σ_observations(
    glo::GLO,
    total_hyperparameters::Vector{T};
    ignore_asymmetry::Bool=false,
    kwargs...
    ) where {T<:Real}

    return glo.has_covariance ?
        Σ_observations(symmetric_A(covariance(glo, total_hyperparameters); ignore_asymmetry=ignore_asymmetry), glo.covariance; kwargs...) :
        Σ_observations(symmetric_A(covariance(glo, total_hyperparameters); ignore_asymmetry=ignore_asymmetry), glo.noise; kwargs...)
end

"""
    get_σ(L_obs, Σ_obs_samp, diag_Σ_samp)

Calculate the GP posterior standard deviation at each sampled point. Algorithm
2.1 from Rasmussen and Williams
"""
function get_σ(
    L_obs::LowerTriangular{T,Matrix{T}},
    Σ_obs_samp::Union{Transpose{T,Matrix{T}},Symmetric,Matrix{T}},
    diag_Σ_samp::Vector{T}
    ) where {T<:Real}

    v = L_obs \ Σ_obs_samp
    # thing = diag_Σ_samp - [dot(v[:, i], v[:, i]) for i in 1:length(diag_Σ_samp)]
    # thing[abs.(thing) .< 1e-10] .= 0
    # return sqrt.(thing)  # σ
    return sqrt.(diag_Σ_samp - [dot(v[:, i], v[:, i]) for i in 1:length(diag_Σ_samp)])  # σ
    # return sqrt.(diag_Σ_samp - diag(Σ_samp_obs * (Σ_obs \ Σ_obs_samp)))  # much slower
end

"""
    get_σ(glo, x_samp, total_hyperparameters)

Calculate the `glo` GP (using `total_hyperparameters`) posterior standard
deviation at each `x_samp` point.
"""
function get_σ(
    glo::GLO,
    x_samp::Vector{T},
    total_hyperparameters::Vector{T}
    ) where {T<:Real}

    (Σ_samp, Σ_obs, _, Σ_obs_samp) = covariance_permutations(glo, x_samp, total_hyperparameters)
    return get_σ(ridge_chol(Σ_obs).L, Σ_obs_samp, diag(Σ_samp))
end

"""
    covariance_permutations(kernel_func, x_obs, x_samp, measurement_noise, kernel_hyperparameters; return_both=false)

Calculate all of the different versions of the covariance matrices using
`kernel_func` with `kernel_hyperparameters` between each of the pairs of `x_obs`
and `x_samp` and themselves.
"""
function covariance_permutations(
    kernel_func::Function,
    x_obs::Vector{T},
    x_samp::Vector{T},
    measurement_noise::Vector{T},
    kernel_hyperparameters::Vector{T};
    kwargs...
    ) where {T<:Real}

    Σ_samp = covariance(kernel_func, x_samp, x_samp, kernel_hyperparameters)
    Σ_samp_obs = covariance(kernel_func, x_samp, x_obs, kernel_hyperparameters; ignore_asymmetry=true)
    Σ_obs_samp = transpose(Σ_samp_obs)
    if return_both
        Σ_obs, Σ_obs_raw = Σ_observations(kernel_func, x_obs, measurement_noise, kernel_hyperparameters; kwargs...)
        return Σ_samp, Σ_obs, Σ_samp_obs, Σ_obs_samp, Σ_obs_raw
    else
        Σ_obs = Σ_observations(kernel_func, x_obs, measurement_noise, kernel_hyperparameters; kwargs...)
        return Σ_samp, Σ_obs, Σ_samp_obs, Σ_obs_samp
    end
end

"""
    covariance_permutations(glo, x_samp, total_hyperparameters; return_both=false)

Calculate all of the different versions of the covariance matrices using
the `glo` GP with `total_hyperparameters` between each of the pairs of
`glo.x_obs` and `x_samp` and themselves.
"""
function covariance_permutations(
    glo::GLO,
    x_samp::Vector{T},
    total_hyperparameters::Vector{T};
    kwargs...
    ) where {T<:Real}

    Σ_samp = covariance(glo, x_samp, x_samp, total_hyperparameters)
    Σ_samp_obs = covariance(glo, x_samp, glo.x_obs, total_hyperparameters; ignore_asymmetry=true)
    Σ_obs_samp = transpose(Σ_samp_obs)
    if return_both
        Σ_obs, Σ_obs_raw = Σ_observations(glo, total_hyperparameters; kwargs...)
        return Σ_samp, Σ_obs, Σ_samp_obs, Σ_obs_samp, Σ_obs_raw
    else
        Σ_obs = Σ_observations(glo, total_hyperparameters; kwargs...)
        return Σ_samp, Σ_obs, Σ_samp_obs, Σ_obs_samp
    end
end

"""
    GP_posteriors_from_covariances(y_obs, Σ_samp, Σ_obs, Σ_samp_obs, Σ_obs_samp, Σ_obs_raw; return_Σ=true, kwargs...)

Calculate the sampled posterior mean and std, observed posterior mean, and
(optionally) the posterior covariance matrix for the GP used to calculate
`Σ_samp`, `Σ_obs`, `Σ_samp_obs`, `Σ_obs_samp`, and `Σ_obs_raw`
"""
function GP_posteriors_from_covariances(
    y_obs::Vector{T},
    Σ_samp::Union{Cholesky,Symmetric,Matrix{T}},
    Σ_obs::Cholesky,
    Σ_samp_obs::Union{Symmetric,Matrix{T}},
    Σ_obs_samp::Union{Transpose{T,Matrix{T}},Symmetric,Matrix{T}},
    Σ_obs_raw::Symmetric;
    return_Σ::Bool=true,
    kwargs...
    ) where {T<:Real}

    # posterior mean calcuation from RW alg. 2.1

    # these are all equivalent but have different computational costs
    # α = inv(Σ_obs) * y_obs
    # α = transpose(L) \ (L \ y_obs)
    α = Σ_obs \ y_obs

    mean_post = Σ_samp_obs * α
    mean_post_obs = Σ_obs_raw * α

    # posterior standard deviation calcuation from RW alg. 2.1
    σ = get_σ(Σ_obs.L, Σ_obs_samp, diag(Σ_samp))

    # posterior covariance calculation is from eq. 2.24 of RW
    if return_Σ
        Σ_post = symmetric_A(Σ_samp - (Σ_samp_obs * (Σ_obs \ Σ_obs_samp)); kwargs...)
        return mean_post, σ, mean_post_obs, Σ_post
    else
        return mean_post, σ, mean_post_obs
    end

end

"""
    GP_posteriors_from_covariances(y_obs, Σ_samp, Σ_obs, Σ_samp_obs, Σ_obs_samp; return_Σ=true, kwargs...)

Calculate the sampled posterior mean and std and (optionally) the posterior
covariance matrix for the GP used to calculate `Σ_samp`, `Σ_obs`, `Σ_samp_obs`,
and `Σ_obs_samp`
"""
function GP_posteriors_from_covariances(
    y_obs::Vector{T},
    Σ_samp::Union{Cholesky,Symmetric,Matrix{T}},
    Σ_obs::Cholesky,
    Σ_samp_obs::Union{Symmetric,Matrix{T}},
    Σ_obs_samp::Union{Transpose{T,Matrix{T}},Symmetric,Matrix{T}};
    return_Σ::Bool=true,
    kwargs...
    ) where {T<:Real}

    # posterior mean calcuation from RW alg. 2.1

    # these are all equivalent but have different computational costs
    # α = inv(Σ_obs) * y_obs
    # α = transpose(L) \ (L \ y_obs)
    α = Σ_obs \ y_obs

    mean_post = Σ_samp_obs * α

    # posterior standard deviation calcuation from RW alg. 2.1
    σ = get_σ(Σ_obs.L, Σ_obs_samp, diag(Σ_samp))

    # posterior covariance calculation is from eq. 2.24 of RW
    if return_Σ
        Σ_post = symmetric_A(Σ_samp - (Σ_samp_obs * (Σ_obs \ Σ_obs_samp)); kwargs...)
        return mean_post, σ, Σ_post
    else
        return mean_post, σ
    end

end

"""
    GP_posteriors_from_covariances(y_obs, Σ_obs, Σ_obs_raw)

Calculate the observed posterior mean for the GP used to calculate `Σ_obs` and
`Σ_obs_raw`
"""
GP_posteriors_from_covariances(
    y_obs::Vector{T},
    Σ_obs::Cholesky,
    Σ_obs_raw::Symmetric) where {T<:Real} =
    Σ_obs_raw * (Σ_obs \ y_obs)

"""
    GP_posteriors(kernel_func, x_obs, y_obs, x_samp, measurement_noise, kernel_hyperparameters; return_mean_obs=false, kwargs...)

Calculate the posterior mean and std at `x_samp`, (optionally) posterior mean
at `x_obs` and (optionally) the posterior covariance matrix for the GP described
by `kernel_func` and `kernel_hyperparameters`
"""
function GP_posteriors(
    kernel_func::Function,
    x_obs::Vector{T},
    y_obs::Vector{T},
    x_samp::Vector{T},
    measurement_noise::Vector{T},
    kernel_hyperparameters::Vector{T};
    return_mean_obs::Bool=false,
    kwargs...) where {T<:Real}

    if return_mean_obs
        (Σ_samp, Σ_obs, Σ_samp_obs, Σ_obs_samp, Σ_obs_raw) = covariance_permutations(kernel_func, x_obs, x_samp, measurement_noise, kernel_hyperparameters; return_both=return_mean_obs)
        return GP_posteriors_from_covariances(y_obs, Σ_samp, Σ_obs, Σ_samp_obs, Σ_obs_samp, Σ_obs_raw; kwargs...)
    else
        (Σ_samp, Σ_obs, Σ_samp_obs, Σ_obs_samp) = covariance_permutations(kernel_func, x_obs, x_samp, measurement_noise, kernel_hyperparameters; return_both=return_mean_obs)
        return GP_posteriors_from_covariances(y_obs, Σ_samp, Σ_obs, Σ_samp_obs, Σ_obs_samp; kwargs...)
    end
end

"""
    GP_posteriors(glo, x_samp, total_hyperparameters; return_mean_obs=false, y_obs=glo.y_obs, kwargs...)

Calculate the posterior mean and std at `x_samp`, (optionally) posterior mean
at `glo.x_obs` and (optionally) the posterior covariance matrix for the GP described
by `glo` and `total_hyperparameters`
"""
function GP_posteriors(
    glo::GLO,
    x_samp::Vector{T},
    total_hyperparameters::Vector{T};
    return_mean_obs::Bool=false,
    y_obs::Vector{T}=glo.y_obs,
    kwargs...) where {T<:Real}

    if return_mean_obs
        (Σ_samp, Σ_obs, Σ_samp_obs, Σ_obs_samp, Σ_obs_raw) = covariance_permutations(glo, x_samp, total_hyperparameters; return_both=return_mean_obs)
        return GP_posteriors_from_covariances(y_obs, Σ_samp, Σ_obs, Σ_samp_obs, Σ_obs_samp, Σ_obs_raw; kwargs...)
    else
        (Σ_samp, Σ_obs, Σ_samp_obs, Σ_obs_samp) = covariance_permutations(glo, x_samp, total_hyperparameters; return_both=return_mean_obs)
        return GP_posteriors_from_covariances(y_obs, Σ_samp, Σ_obs, Σ_samp_obs, Σ_obs_samp; kwargs...)
    end

end

"""
    GP_posteriors(glo, total_hyperparameters; y_obs=glo.y_obs, kwargs...)

Calculate the posterior mean at `glo.x_obs` for the GP described by `glo` and
`total_hyperparameters`
"""
function GP_posteriors(
    glo::GLO,
    total_hyperparameters::Vector{T};
    y_obs::Vector{T}=glo.y_obs,
    kwargs...
    ) where {T<:Real}
    Σ_obs, Σ_obs_raw = Σ_observations(glo, total_hyperparameters; return_both=true, kwargs...)
    return GP_posteriors_from_covariances(y_obs, Σ_obs, Σ_obs_raw)
end

"""
    coefficient_orders(n_out, n_dif; a=ones(n_out, n_dif))

Find the powers that each GLOM coefficient is taken to for each part of the
matrix construction before differentiating by any hyperparameters.

# Outputs
- `coeff_orders::Array{Int, 6}`: filled with integers for what power each coefficient is taken to in the construction of a given block of the total covariance matrix. For example, coeff_orders[1,1,2,3,:,:] would tell you the powers of each coefficient (:,:) that are multiplied by the covariance matrix constructed by evaluating the partial derivative of the kernel (once by t1 and twice by t2) at every pair of time points (2,3) in the construction of the first block of the total covariance matrix (1,1)
- `coeff_coeffs::Array{Int, 4}`: Filled with ones anywhere that coeff_orders indicates that coefficients exists to multiply a given covariance matrix for a given block
"""
function coefficient_orders(
    n_out::Integer,
    n_dif::Integer;
    a::Matrix{T}=ones(n_out, n_dif)
    ) where {T<:Real}

    @assert size(a) == (n_out, n_dif)

    # ((output pair), (which A matrix to use), (which a coefficent to use))
    coeff_orders = zeros(Int64, n_out, n_out, n_dif, n_dif, n_out, n_dif)
    coeff_coeffs = zeros(Int64, n_out, n_out, n_dif, n_dif)
    for i in 1:n_out
        for j in 1:n_out
            for k in 1:n_dif
                for l in 1:n_dif
                    for m in 1:n_out
                        for n in 1:n_dif
                            if a[m, n] != 0
                                if [m, n] == [i, k]; coeff_orders[i, j, k, l, m, n] += 1 end
                                if [m, n] == [j, l]; coeff_orders[i, j, k, l, m, n] += 1 end
                            end
                        end
                    end
                    # There should be two coefficients being applied to every
                    # matrix. If there are less than two, that means one of the
                    # coefficients was zero, so we should just set them both to
                    # zero
                    if sum(coeff_orders[i, j, k, l, :, :]) != 2
                        coeff_orders[i, j, k, l, :, :] .= 0
                    else
                        coeff_coeffs[i, j, k, l] = 1
                    end
                end
            end
        end
    end

    return coeff_orders, coeff_coeffs

end

"""
    dif_coefficients!(n_out, n_dif, dΣdθ_total::Int, coeff_orders, coeff_coeffs)

Modify `coeff_orders` and `coeff_coeffs` with the coefficients for constructing
differentiated version of the kernel (for the differentiation implied by
`dΣdθ_total`) using the powers that each coefficient is taken to for each part
of the matrix construction
"""
function dif_coefficients!(
    n_out::Integer,
    n_dif::Integer,
    dΣdθ_total::Integer,
    coeff_orders::AbstractArray{T,6},
    coeff_coeffs::AbstractArray{T,4}
    ) where {T<:Integer}

    @assert size(coeff_orders) == (n_out, n_out, n_dif, n_dif, n_out, n_dif)
    @assert size(coeff_orders) == (n_out, n_out, n_dif, n_dif, n_out, n_dif)

    # only do something if a derivative is being taken
    if dΣdθ_total > 0 && dΣdθ_total <= (n_out*n_dif)
        proper_indices = [((dΣdθ_total - 1) % n_out) + 1, div(dΣdθ_total - 1, n_out) + 1]
        for i in 1:n_out
            for j in 1:n_out
                for k in 1:n_dif
                    for l in 1:n_dif
                        if coeff_orders[i, j, k, l, proper_indices[1], proper_indices[2]] != 0
                            coeff_coeffs[i, j, k, l] *= coeff_orders[i, j, k, l, proper_indices[1], proper_indices[2]]
                            coeff_orders[i, j, k, l, proper_indices[1], proper_indices[2]] -= 1
                        else
                            coeff_coeffs[i, j, k, l] = 0
                            coeff_orders[i, j, k, l, :, :] .= 0
                        end
                    end
                end
            end
        end
    end
end

"""
    dif_coefficients!(n_out, n_dif, dΣdθs_total::Vector, coeff_orders, coeff_coeffs)

Modify `coeff_orders` and `coeff_coeffs` with the coefficients for constructing
differentiated version of the kernel (for the differentiations implied by
`dΣdθ_totals`) using the powers that each coefficient is taken to for each part
of the matrix construction
"""
function dif_coefficients!(
    n_out::Integer,
    n_dif::Integer,
    dΣdθs_total::Vector{T},
    coeff_orders::AbstractArray{T,6},
    coeff_coeffs::AbstractArray{T,4}
    ) where {T<:Integer}

    for dΣdθ_total in dΣdθs_total
        dif_coefficients!(n_out, n_dif, dΣdθ_total, coeff_orders, coeff_coeffs)
    end
end

# TODO: rename nlogL_normalization
"""
    nlogL(Σ, y; α= Σ \\ y, nlogL_normalization=logdet(Σ)+length(y)*log(2*π))

Negative log likelihood for data `y` assuming it was drawn from a multivariate
normal distribution with 0 mean and covariance `Σ` (usually `Σ + noise`)
"""
function nlogL(
    Σ::Union{Cholesky,Diagonal},
    y::Vector{T};
    α::Vector{T} = Σ \ y,
    nlogL_normalization::T=logdet(Σ)+length(y)*log(2*π)
    ) where {T<:Real}

    # n = length(y)

    # 2 times negative goodness of fit term
    data_fit = transpose(y) * α
    # 2 times negative complexity penalization term
    # complexity_penalty = log(det(Σ_obs))
    # complexity_penalty = logdet(Σ_obs)  # half memory but twice the time
    # 2 times negative normalization term (doesn't affect fitting)
    # normalization = n * log(2 * π)

    return (data_fit + nlogL_normalization) / 2

end

"""
    dnlogLdθ(y, α, β)

First partial derivative of `nlogL(Σ, y)` w.r.t. hyperparameters that affect `Σ`

# Arguments
- `y::Vector`: The observations at each time point
- `α::Vector`: `inv(Σ) * y`
- `β::Matrix`: `inv(Σ) * dΣ_dθ` where `dΣ_dθ` is the partial derivative of the `Σ` w.r.t. a hyperparameter
"""
function dnlogLdθ(
    y::Vector{T},
    α::Vector{T},
    β::Matrix{T}
    ) where {T<:Real}

    # 2 times negative derivative of goodness of fit term
    data_fit = -(transpose(y) * β * α)
    # 2 times negative derivative of complexity penalization term
    complexity_penalty = tr(β)

    # return -1 / 2 * tr((α * transpose(α) - inv(Σ_obs)) * dΣ_dθj)
    return (data_fit + complexity_penalty) / 2

end

"""
    dnlogLdθ(y1, α)

First partial derivative of `nlogL(Σ, y)` w.r.t. hyperparameters that affect `y`

# Arguments
- `y1::Vector`: The derivative of observations at each time point
- `α::Vector`: `inv(Σ) * y`
"""
function dnlogLdθ(
    y1::Vector{T},
    α::Vector{T}
    ) where {T<:Real}

    return transpose(y1) * α
    # return transpose(y) * α1
    # return (transpose(y1) * Σ^-1 * y + transpose(y) * Σ^-1 * y1) / 2
end

"""
    ∇nlogL(y, α, βs)

Gradient of `nlogL(Σ, y)` w.r.t. hyperparameters that affect `Σ`

# Arguments
- `y::Vector`: The observations at each time point
- `α::Vector`: inv(Σ) * y
- `βs::Vector{Matrix}`: List of `inv(Σ) * dΣ_dθ` where `dΣ_dθ` is the partial derivative of `Σ` w.r.t. each hyperparameter
"""
∇nlogL(y::Vector{T}, α::Vector{T}, βs::Vector{Matrix{T}}) where {T<:Real} = [dnlogLdθ(y, α, β) for β in βs]

"""
    d2nlogLdθ(y, α, β1, β2, β12)

Second partial derivative of `nlogL(Σ, y)` w.r.t. two hyperparameters that
affect `Σ`

# Arguments
- `y::Vector`: The observations at each time point
- `α::Vector`: `inv(Σ) * y`
- `β1::Matrix`: `inv(Σ) * dΣ_dθ` where `dΣ_dθ` is the partial derivative of the `Σ` w.r.t. the first hyperparameter
- `β2::Matrix`: Same as `β1` but for the second hyperparameter
- `β12::Matrix`: `inv(Σ_obs) * d2Σ_dθ1dθ2` where `d2Σ_dθ1dθ2` is the partial derivative of the covariance matrix Σ_obs w.r.t. both of the hyperparameters being considered
"""
function d2nlogLdθ(
    y::Vector{T},
    α::Vector{T},
    β1::Matrix{T},
    β2::Matrix{T},
    β12::Matrix{T}
    ) where {T<:Real}

    β12mβ2β1 = β12 - β2 * β1

    # 2 times negative second derivative of goodness of fit term
    data_fit = -(transpose(y) * (β12mβ2β1 - β1 * β2) * α)
    # 2 times negative derivative of complexity penalization term
    complexity_penalty = tr(β12mβ2β1)

    return (data_fit + complexity_penalty) / 2

end

"""
    d2nlogLdθ(y2, y12, α, α1)

Second partial derivative of `nlogL(Σ, y)` w.r.t. two parameters that affect `y`

# Arguments
- `y2::Vector`: The derivative of observations w.r.t the second parameter at each time point
- `y12::Vector`: The derivative of observations w.r.t the both parameters at each time point
- `α::Vector`: `inv(Σ) * y`
- `α1::Vector`: `inv(Σ) * y1` where `y1` is the derivative of observations w.r.t the first parameter at each time point
"""
function d2nlogLdθ(
    y2::Vector{T},
    y12::Vector{T},
    α::Vector{T},
    α1::Vector{T}
    ) where {T<:Real}

    return transpose(y12) * α + transpose(y2) * α1
    # return (transpose(y12) * α + transpose(y1) * α2 + transpose(y2) * α1 + transpose(y) * α12) / 2

end

"""
    d2nlogLdθ(y, y1, α, α1, β2)

Second partial derivative of `nlogL(Σ, y)` w.r.t. a parameter that affects `y` and a hyperparameter that affects `Σ`

# Arguments
- `y::Vector`: The observations at each time point
- `y1::Vector`: The derivative of observations w.r.t the `y`-affecting parameter at each time point
- `α::Vector`: `inv(Σ) * y`
- `α1::Vector`: `inv(Σ) * y1` where `y1` is the derivative of observations w.r.t the first parameter at each time point
- `β2::Matrix`: `inv(Σ) * dΣ_dθ` where `dΣ_dθ` is the partial derivative of the `Σ` w.r.t. the `Σ`-affecting hyperparameter
"""
function d2nlogLdθ(
    y::Vector{T},
    y1::Vector{T},
    α::Vector{T},
    α1::Vector{T},
    β2::Matrix{T}
    ) where {T<:Real}

    return -(transpose(y1) * β2 * α + transpose(y) * β2 * α1) / 2

end

"""
	nlogL_matrix_workspace{T<:Real}

A structure that holds all of the relevant information for calculating nlogL()
derivatives. Used to prevent recalculations during optimization.

# Arguments
- `nlogL_hyperparameters::Vector`: The current hyperparameters
- `Σ_obs::Cholesky`: The covariance matrix based on `nlogL_hyperparameters`
- `∇nlogL_hyperparameters::Vector`: The `nlogl` gradient at `nlogL_hyperparameters`
- `βs::Vector{Matrix}`: List of `inv(Σ) * dΣ_dθ` where `dΣ_dθ` is the partial derivative of `Σ` w.r.t. each `nlogL_hyperparameters`
"""
struct nlogL_matrix_workspace{T<:Real}
    nlogL_hyperparameters::Vector{T}
    Σ_obs::Cholesky
    ∇nlogL_hyperparameters::Vector{T}
    βs::Vector{Matrix{T}}

    function nlogL_matrix_workspace(glo::GLO,
        total_hyperparameters::Vector{<:Real})

        total_hyper = reconstruct_total_hyperparameters(glo::GLO, total_hyperparameters)
        Σ_obs = Σ_observations(glo, total_hyper; ignore_asymmetry=true)
        return nlogL_matrix_workspace(
            total_hyper,
            Σ_obs,
            copy(total_hyper),
            [Σ_obs \ covariance(glo, total_hyper; dΣdθs_total=[i]) for i in glo.non_zero_hyper_inds])
    end
    nlogL_matrix_workspace(
        nlogL_hyperparameters::Vector{T},
        Σ_obs::Cholesky,
        ∇nlogL_hyperparameters::Vector{T},
        βs::Vector{Matrix{T}}
        ) where T<:Real = new{typeof(nlogL_hyperparameters[1])}(nlogL_hyperparameters, Σ_obs, ∇nlogL_hyperparameters, βs)
end

"""
    calculate_shared_nlogL_matrices(glo, non_zero_hyperparameters; Σ_obs=Σ_observations(glo, reconstruct_total_hyperparameters(glo, non_zero_hyperparameters); ignore_asymmetry=true))

Calculates the quantities shared by the nlogL and ∇nlogL calculations
"""
function calculate_shared_nlogL_matrices(
    glo::GLO,
    non_zero_hyperparameters::Vector{<:Real};
    Σ_obs::Cholesky where T<:Real=Σ_observations(glo, reconstruct_total_hyperparameters(glo, non_zero_hyperparameters); ignore_asymmetry=true))

    # this allows us to prevent the optimizer from seeing the constant zero coefficients
    total_hyperparameters = reconstruct_total_hyperparameters(glo, non_zero_hyperparameters)

    return total_hyperparameters, Σ_obs

end

"""
    calculate_shared_nlogL_matrices!(workspace, glo, non_zero_hyperparameters)

Calculates the quantities shared by the nlogL and ∇nlogL calculations and stores
them in the existing `workspace`
"""
function calculate_shared_nlogL_matrices!(
    workspace::nlogL_matrix_workspace,
    glo::GLO,
    non_zero_hyperparameters::Vector{<:Real})

    # this allows us to prevent the optimizer from seeing the constant zero coefficients
    total_hyperparameters = reconstruct_total_hyperparameters(glo, non_zero_hyperparameters)

    if workspace.nlogL_hyperparameters != total_hyperparameters
        workspace.nlogL_hyperparameters[:] = total_hyperparameters
        workspace.Σ_obs.factors[:,:] = Σ_observations(glo, total_hyperparameters).factors
    end

end

"""
    calculate_shared_∇nlogL_matrices(glo, non_zero_hyperparameters; kwargs...)

Calculates the quantities shared by the ∇nlogL and ∇∇nlogL calculations
"""
function calculate_shared_∇nlogL_matrices(
    glo::GLO,
    non_zero_hyperparameters::Vector{<:Real};
    kwargs...)

    total_hyperparameters, Σ_obs = calculate_shared_nlogL_matrices(glo, non_zero_hyperparameters; kwargs...)

    βs = [Σ_obs \ covariance(glo, total_hyperparameters; dΣdθs_total=[i]) for i in glo.non_zero_hyper_inds]

    return total_hyperparameters, Σ_obs, βs

end

"""
    calculate_shared_∇nlogL_matrices!(workspace, glo, non_zero_hyperparameters)

Calculates the quantities shared by the ∇nlogL and ∇∇nlogL calculations and
stores them in the existing `workspace`
"""
function calculate_shared_∇nlogL_matrices!(
    workspace::nlogL_matrix_workspace,
    glo::GLO,
    non_zero_hyperparameters::Vector{<:Real})

    calculate_shared_nlogL_matrices!(workspace, glo, non_zero_hyperparameters)

    if workspace.∇nlogL_hyperparameters != workspace.nlogL_hyperparameters
        workspace.∇nlogL_hyperparameters[:] = workspace.nlogL_hyperparameters
        workspace.βs[:] = [workspace.Σ_obs \ covariance(glo, workspace.∇nlogL_hyperparameters; dΣdθs_total=[i]) for i in findall(!iszero, workspace.∇nlogL_hyperparameters)]
    end

end

"""
    include_kernel(kernel_name)

Tries to include the specified kernel, assuming it was included with GLOM.
Returns the kernel function and number of hyperparameters it uses
"""
function include_kernel(kernel_name::AbstractString)
    if !occursin("_kernel", kernel_name)
        kernel_name *= "_kernel"
    end
    return include(joinpath(pathof(GPLinearODEMaker),"..","kernels", kernel_name * ".jl"))
end

"""
    prep_parallel_covariance(kernel_name, kernel_path; kwargs...)

Make it easy to run the covariance calculations on many processors. Makes sure
every worker has access to kernel function, importing it from the
`kernel_path`.
"""
function prep_parallel_covariance(
    kernel_name::AbstractString,
    kernel_path::AbstractString;
    kwargs...)

    prep_parallel(; kwargs...)
    sendto(workers(), kernel_name=kernel_name)
    @everywhere include(kernel_path)
end

"""
    prep_parallel_covariance(kernel_name; kwargs...)

Make it easy to run the covariance calculations on many processors. Makes sure
every worker has access to kernel function.
"""
function prep_parallel_covariance(
    kernel_name::AbstractString;
    kwargs...)

    prep_parallel(; kwargs...)
    sendto(workers(), kernel_name=kernel_name)
    @everywhere include_kernel(kernel_name)
end

"""
    nlogL_GLOM(glo, total_hyperparameters; y_obs=copy(glo.y_obs), kwargs...)

Negative log likelihood for `glo` using the non-zero `total_hyperparameters` to
construct the covariance matrix.
"""
function nlogL_GLOM(
    glo::GLO,
    total_hyperparameters::Vector{T};
    y_obs::Vector{T}=copy(glo.y_obs),
    kwargs...) where {T<:Real}

    total_hyperparameters, Σ_obs = calculate_shared_nlogL_matrices(
        glo, remove_zeros(total_hyperparameters); kwargs...)

    return nlogL(Σ_obs, y_obs)
end

"""
    nlogL_GLOM!(workspace, glo, total_hyperparameters; y_obs=copy(glo.y_obs))

Negative log likelihood for `glo` using `total_hyperparameters` to construct the
covariance matrix and storing the intermediate results in `workspace`.
"""
function nlogL_GLOM!(
    workspace::nlogL_matrix_workspace,
    glo::GLO,
    total_hyperparameters::Vector{T};
    y_obs::Vector{T}=copy(glo.y_obs)
    ) where {T<:Real}

    calculate_shared_nlogL_matrices!(workspace, glo, total_hyperparameters)
    return nlogL(workspace.Σ_obs, y_obs)
end

"""
    ∇nlogL_GLOM(glo, total_hyperparameters; y_obs=copy(glo.y_obs), kwargs...)

`nlogL` gradient for `glo` using the non-zero `total_hyperparameters` to
construct the covariance matrices.
"""
function ∇nlogL_GLOM(
    glo::GLO,
    total_hyperparameters::Vector{T};
    y_obs::Vector{T}=copy(glo.y_obs),
    kwargs...) where {T<:Real}

    total_hyperparameters, Σ_obs, βs = calculate_shared_∇nlogL_matrices(
        glo, remove_zeros(total_hyperparameters); kwargs...)

    return ∇nlogL(y_obs, Σ_obs \ y_obs, βs)

end

"""
    ∇nlogL_GLOM!(workspace, glo, total_hyperparameters; y_obs=copy(glo.y_obs))

`nlogL` gradient for `glo` using the non-zero `total_hyperparameters` to
construct the covariance matrices and storing the intermediate results in `workspace`.
"""
function ∇nlogL_GLOM!(
    workspace::nlogL_matrix_workspace,
    glo::GLO,
    total_hyperparameters::Vector{T};
    y_obs::Vector{T}=copy(glo.y_obs)
    ) where {T<:Real}

    calculate_shared_∇nlogL_matrices!(workspace, glo, total_hyperparameters)

    return ∇nlogL(y_obs, workspace.Σ_obs \ y_obs, workspace.βs)

end

"""
    ∇∇nlogL_GLOM(glo, total_hyperparameters, Σ_obs, y_obs, α, βs)

`nlogL` Hessian for `glo` using the non-zero `total_hyperparameters` to
construct the covariance matrices.
"""
function ∇∇nlogL_GLOM(
    glo::GLO,
    total_hyperparameters::Vector{T},
    Σ_obs::Cholesky,
    y_obs::Vector{T},
    α::Vector{T},
    βs::Array{Matrix{T},1}
    ) where {T<:Real}

    non_zero_inds = copy(glo.non_zero_hyper_inds)
    H = zeros(length(non_zero_inds), length(non_zero_inds))
    for (i, nzind1) in enumerate(non_zero_inds)
        for (j, nzind2) in enumerate(non_zero_inds)
            if i <= j
                H[i, j] = d2nlogLdθ(y_obs, α, βs[i], βs[j], Σ_obs \ covariance(glo, total_hyperparameters; dΣdθs_total=[nzind1, nzind2]))
            end
        end
    end
    H = Symmetric(H)
    return H

end

"""
    ∇∇nlogL_GLOM(glo, total_hyperparameters; y_obs=copy(glo.y_obs))

`nlogL` Hessian for `glo` using the non-zero `total_hyperparameters` to
construct the covariance matrices.
"""
function ∇∇nlogL_GLOM(
    glo::GLO,
    total_hyperparameters::Vector{T};
    y_obs::Vector{T}=copy(glo.y_obs),
    kwargs...) where {T<:Real}

    total_hyperparameters, Σ_obs, βs = calculate_shared_∇nlogL_matrices(
        glo, remove_zeros(total_hyperparameters); kwargs...)

    return ∇∇nlogL_GLOM(glo, total_hyperparameters, Σ_obs, y_obs, Σ_obs \ y_obs, βs)

end

"""
    ∇∇nlogL_GLOM!(workspace, glo, total_hyperparameters; y_obs=copy(glo.y_obs))

`nlogL` Hessian for `glo` using the non-zero `total_hyperparameters` to
construct the covariance matrices and storing the intermediate results in
`workspace`.
"""
function ∇∇nlogL_GLOM!(
    workspace::nlogL_matrix_workspace,
    glo::GLO,
    total_hyperparameters::Vector{T};
    y_obs::Vector{T}=copy(glo.y_obs)
    ) where {T<:Real}

    calculate_shared_∇nlogL_matrices!(workspace, glo, total_hyperparameters)

    return ∇∇nlogL_GLOM(glo, workspace.nlogL_hyperparameters, workspace.Σ_obs, y_obs, workspace.Σ_obs \ y_obs, workspace.βs)

end

"""
    reconstruct_total_hyperparameters(glo, hyperparameters)

Reinsert the zero coefficients into the non-zero hyperparameter list if needed
"""
function reconstruct_total_hyperparameters(
    glo::GLO,
    hyperparameters::Vector{T}
    ) where {T<:Real}

    if length(hyperparameters)!=(glo.n_kern_hyper + length(glo.a))
        new_coeff_array = reconstruct_array(hyperparameters[1:end - glo.n_kern_hyper], glo.a)
        coefficient_hyperparameters = collect(Iterators.flatten(new_coeff_array))
        total_hyperparameters = append!(coefficient_hyperparameters, hyperparameters[end - glo.n_kern_hyper + 1:end])
    else
        total_hyperparameters = copy(hyperparameters)
    end

    @assert length(total_hyperparameters)==(glo.n_kern_hyper + length(glo.a))

    return total_hyperparameters

end


text:
module ShipMMG

using DifferentialEquations
using ParameterizedFunctions
using Dierckx
using Plots
using Parameters
using Distributions
using Turing

include("data.jl")
export calc_position,
    ShipData,
    get_KVLCC2_L7_basic_params,
    get_KVLCC2_L7_maneuvering_params,
    get_KVLCC2_L7_params,
    nuts_sampling_single_thread,
    nuts_sampling_multi_threads

include("kt.jl")
export kt_simulate,
    kt_model!,
    kt_zigzag_test,
    estimate_kt_lsm,
    estimate_kt_lsm_time_window_sampling,
    create_model_for_mcmc_sample_kt

include("mmg.jl")
export Mmg3DofBasicParams,
    Mmg3DofManeuveringParams,
    mmg_3dof_simulate,
    mmg_3dof_model!,
    mmg_3dof_zigzag_test,
    estimate_mmg_approx_lsm,
    estimate_mmg_approx_lsm_time_window_sampling,
    create_model_for_mcmc_sample_mmg

include("draw.jl")
export draw_gif_result, calc_position

end

max_stars_repo_name: mitsuyukiLab/JuliaShipMMG

max_issues_repo_name: mitsuyukiLab/JuliaShipMMG


text:
using Documenter, CUSPARSE

makedocs(
         modules=[CUSPARSE]
        )


text:
# Copyright 2020 Digital Domain 3.0
#
# Licensed under the Apache License, Version 2.0 (the "Apache License")
# with the following modification; you may not use this file except in
# compliance with the Apache License and the following modification to it:
# Section 6. Trademarks. is deleted and replaced with:
#
# 6. Trademarks. This License does not grant permission to use the trade
#    names, trademarks, service marks, or product names of the Licensor
#    and its affiliates, except as required to comply with Section 4(c) of
#    the License and to reproduce the content of the NOTICE file.
#
# You may obtain a copy of the Apache License at
#
#     http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the Apache License with the above modification is
# distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
# KIND, either express or implied. See the Apache License for the specific
# language governing permissions and limitations under the Apache License.

module HalfEdges

using Base.Iterators, LinearAlgebra, SparseArrays

export
  Topology,
  HalfEdge,
  HalfEdgeHandle,
  VertexHandle,
  FaceHandle,
  EdgeHandle,
  OneRing,
  OneRingVerts,
  Rim,
  Polygon,
  BoundaryLoop,
  UniqueHalfEdges,
  IncidentEdges,
  IncidentHalfEdges,
  nvertices,
  nverts,
  nedges,
  nfaces,
  nhalfedges,
  facelist,
  polygons,
  eff,
  incidence,
  loadmesh,
  head,
  tail,
  isboundary,
  opposite,
  boundary_verts,
  boundary_vertices,
  boundary_interior,
  vertexnormals,
  normals,
  face,
  halfedge,
  vertices,
  solide_angle,
  winding_number,
  winding_numbers,
  winding_number_cache,
  floodfill,
  find_islands,
  dihedral_angle,
  edge,
  add_ring,
  remove_ring,
  gluing_map,
  edge_length_map,
  opposite_corner_angle,
  next_side,
  other_side,
  diagonal_length,
  is_delaunay,
  flip_edge!,
  intrinsic_Δ,
  intrinsic_M,
  edge_in_face_basis

include("Handles.jl")

partial = (f::Function, y...)->(z...)->f(y..., z...)
∂(f::Function,y...) = partial(f, y...)
second(c) = c[2]

@HandleType HalfEdgeHandle
@HandleType VertexHandle
@HandleType FaceHandle
@HandleType EdgeHandle

using StaticArrays

const Vector3{T}=SVector{3,T}
const Vector3f=Vector3{Float32}
const Vector3d=Vector3{Float64}

""" a halfedge datastructure """
struct HalfEdge
  head     :: VertexHandle
  next     :: HalfEdgeHandle
  opposite :: HalfEdgeHandle
  face     :: Union{FaceHandle,Nothing}
  HalfEdge( head_vert, next_edge, opp ) = new( head_vert, next_edge, opp, nothing )
  HalfEdge( head_vert, next_edge, opp, face ) = new( head_vert, next_edge, opp, face )
end

""" a manifold set of polygons """
struct Topology
  v2he    :: Vector{HalfEdgeHandle}
  face2he :: Vector{HalfEdgeHandle}
  he      :: Vector{HalfEdge}
  he2edge :: Vector{EdgeHandle}
end

const Mesh = Tuple{Topology, Vector{Vector3d}}
const Edge = Tuple{Int,Int}

HalfEdge(topo::Topology, heh::HalfEdgeHandle) = topo.he[heh]
""" dereference the HalfEdgeHandle to get the struct """
halfedge(topo::Topology, heh::HalfEdgeHandle)  = HalfEdge(topo, heh)
get(topo::Topology, heh::HalfEdgeHandle)  = HalfEdge(topo, heh)

""" get a halfedge for this face """
halfedge(topo::Topology, fh::FaceHandle) = topo.face2he[fh]

""" get a halfedge for this vertex """
halfedge(topo::Topology, vh::VertexHandle) = topo.v2he[vh]

"""
    halfedge( topo, vpair )

The oriented halfedge for the edge connecting two vertex indices/handles.
The halfedge points from first to second vertex in tuple vpair.
"""
halfedge(topo::Topology, e::T) where {T <: Tuple{VertexHandle, VertexHandle}} =
  Iterators.filter(h->head(topo, h)==e[2], OneRing(topo, e[1])) |> first

halfedge(topo::Topology, e::T) where {V<:Integer, T<:Tuple{V,V}} = halfedge(topo, VertexHandle.(e))

HalfEdgeHandle(topo::Topology, vh::VertexHandle) = topo.v2he[vh]

""" return the VertexHandle for the vert pointed to by the given halfedge """
head(he::HalfEdge) = he.head
head(topo::Topology, heh::HalfEdgeHandle) = head(topo.he[heh])

""" return the VertexHandle for the vert pointed to by the given halfedge """
vertex(x::HalfEdge) = head(x)
vertex(topo::Topology, x) = head(topo, x)

vertices(topo::Topology) = topo.v2he
vertices(topo::Topology, c) = map(h->vertex(topo,h), c)
vertices(topo::Topology, heh::HalfEdgeHandle) = vertices(topo, Polygon(topo,heh))
vertices(topo::Topology, fh::FaceHandle) = vertices(topo, faces(topo)[fh])
verts(x...) = vertices(x...)

""" return the VertexHandle for the vert at the tail of the given halfedge """
tail(topo::Topology, he::HalfEdge) = head(topo,opposite(he))
tail(topo::Topology, heh::HalfEdgeHandle) = tail(topo,topo.he[heh])

""" return the next halfedge ccw on the same face """
function next(he::HalfEdge)
  he.next
end

function next(topo::Topology, heh::HalfEdgeHandle)
  next(topo.he[heh])
end

""" return halfedge pointing to this halfedge """
function prev(topo::Topology, heh::HalfEdgeHandle)
  if isboundary(topo, heh)
    nothing
  else
    [ih for ih in Polygon(topo,heh)][end]
  end
end

function opposite(he::HalfEdge)
  he.opposite
end

"""
    opposite(topo, halfedgehandle)

the Handle of the oppositely oriented halfedge sharing the same topological edge.

        b
       /|\\
      /↑|↓\\
     / ↑|↓ \\
    ∘---a---∘

 ↑ and ↓ are opposite halfedges on the edge ab
"""
function opposite(topo::Topology, heh::HalfEdgeHandle)
  opposite(topo.he[heh])
end

"iterate around a polygon"
struct Polygon
  topo::Topology
  starth::HalfEdgeHandle
  Polygon(topo::Topology, heh::HalfEdgeHandle) = new(topo, heh)
end

Base.IteratorSize(::Type{Polygon}) = Base.SizeUnknown()
Base.eltype(::Type{Polygon}) = HalfEdgeHandle

""" iterator for a polygon """
Polygon(topo::Topology, fh::FaceHandle) = Polygon(topo, topo.face2he[fh])

Base.iterate( poly::Polygon ) = (poly.starth, next(poly.topo, poly.starth))

Base.iterate( poly::Polygon, heh::HalfEdgeHandle ) =
  (heh == poly.starth) ?  nothing : (heh, next(poly.topo, heh))

function BoundaryLoop(topo::Topology, heh::HalfEdgeHandle)
  if( isboundary(topo,heh) )
    Polygon(topo,heh)
  elseif( isboundary(topo,opposite(topo,heh)) )
    Polygon(topo,opposite(topo,heh))
  else
    nothing
  end
end

halfedges(topo) = HalfEdgeHandle.(1:length(topo.he))
halfedges(poly::Polygon) = [heh for heh in poly]

edge(topo::Topology, heh::HalfEdgeHandle) =  (tail(topo,heh), head(topo,heh))
"get an edge tuple from an EdgeHandle.  very slow"
edge(topo::Topology, eh::EdgeHandle) = edges(topo)[eh]
edgehandle(topo::Topology, heh::HalfEdgeHandle) = topo.he2edge[heh]

edges(topo::Topology) = Iterators.filter(ab->isless(ab...), Iterators.map(heh->edge(topo,heh),halfedges(topo))) |> collect
edges(topo::Topology, c) = map(h->edge(topo, h), c)
edges(topo::Topology, heh::HalfEdgeHandle) = edges(topo, Polygon(topo,heh))
edges(topo::Topology, fh::FaceHandle) = edges(topo, faces(topo)[fh])

mapedge(f, topo::Topology) = f.(Iterators.filter(ab->isless(ab...), Iterators.map(heh->edge(topo,heh),halfedges(topo))))

UniqueHalfEdges(topo::Topology) = Iterators.filter(heh->isless(edge(topo,heh)...), halfedges(topo))

"""
    UniqueHalfEdges(topo, flip_on_boundary)

Unique halfedges are halfedges whose ordering is in one-to-one correspondence with list of oriented edges.
we choose the edge (VertexHandle₁, VertexHandle₂) where VertexHandle₁ < VertexHandle₂.

i.e. map(heh->edge(topo,heh), UniqueHalfEdges(topo)) == edges(topo)

If flip_on_boundary is true then the one-to-one correspondence is not true when mapped to VertexHandle tuple.
In this case, the order of the UniqueHalfEdges still follows EdgeHandle order, but the orientation will be flipped at boundary.  This way you are guaranteed a valid face at each UniqueHalfEdge without having to check isboundary.
"""
function UniqueHalfEdges(topo::Topology, flip_on_boundary)
  uhe = UniqueHalfEdges(topo)
  if flip_on_boundary
    Iterators.map(heh->isboundary(topo, heh) ? opposite(topo, heh) : heh, uhe)
  else
    uhe
  end
end

"""
unique halfedges not on boundary.
"""
unique_inside_halfedges(topo::Topology) = Iterators.filter(h->isboundary(topo, opposite(topo, h)) || (!isboundary(topo, h) && h  < opposite(topo, h)), halfedges(topo))

"unique indices for vertices"
vertex_indices(topo) = 1:length(topo.v2he)
"unique indices for faces"
face_indices(topo) = 1:length(topo.face2he)
"unique indices for edges"
edge_indices(topo) = 1:length(edges(topo)) # or length(topo.he)/2, but that is implemenation dependent

""" a boundary halfedge is connected to a single face """
isboundary(he::HalfEdge) =  he.face == nothing
isboundary(topo::Topology, heh::HalfEdgeHandle) =  isboundary(topo.he[heh])
isboundary(topo::Topology, heh::Nothing) =  true

""" a vertex is a boundary if it's starting halfedge is boundary """
# this assumes we always wind the halfedge to the boundary
# safer would be to iterate over the spokes and check if any halfedge is a boundary
isboundary(topo::T, vh::VertexHandle) where T = isboundary(topo, HalfEdgeHandle(topo, vh))

faces(topo::Topology) = topo.face2he

face(he::HalfEdge) =  he.face
face(topo::Topology, heh::HalfEdgeHandle) =  face(topo.he[heh])
face(topo::Topology, heh::Nothing) =  nothing

#!me can I reverse the order here so topology comes first?  Now that Handles are disticnt type I think I can
""" iterator for circulating clockwise around the one-ring neighbourhood of a vertex """
struct OneRing
  starth::HalfEdgeHandle
  topo::Topology
end

""" iterator to circulate around the one-ring neighbourhood of a vertex in cw direction """
OneRing(topo::Topology, vh::VertexHandle) = OneRing( HalfEdgeHandle(topo, vh), topo )

""" iterate around the one-ring of the vert in cw direction """
function Base.iterate(i::OneRing)
  heh = i.starth
  iszero(heh) ? nothing : (heh,next(i.topo,opposite(i.topo,heh)))
end

Base.iterate(topo::Topology, h::T) where T =  iterate(OneRing(topo,h))

# iterate around the one-ring of the vert in cw direction
function Base.iterate(i::OneRing, heh::HalfEdgeHandle)
  if heh == i.starth
    nothing
  else
    (heh,next(i.topo,opposite(i.topo,heh)))
  end
end

OneRingVerts(topo::Topology, h) = map(heh->head(topo, heh), OneRing(topo, h))

Base.isempty(oring::OneRing) = iszero(oring.starth)

"""
  until( topo, he, v )

all halfedges from spoke a to b.  going ccw
"""
function until( topo::Topology, he::HalfEdgeHandle, v::VertexHandle )
  r = Vector{HalfEdgeHandle}()
  push!( r, he )
  while( head(topo,he) != v )
    he = next( topo, he )
    push!( r, he )
  end
  r
end

"""
  Rim(topo, i)

i is a VertexHandle

Iterates around rim of vertex ( outside edges of one-ring neighbourhood ).
"""
function Rim(topo::Topology, i::VertexHandle)
  spokeb = OneRing(topo,i)
  if isboundary(topo, first(spokeb))
    spokea = drop(spokeb, 1)
  else
    spokea = drop(cycle(spokeb), 1)
  end
  ( until(topo, next(topo, a), head(topo, b)) for (a,b) in zip(spokea, spokeb) ) |> flatten
end

#Base.length(i::OneRing) =  reduce((acc,_ )-> acc+1, i; init=0)
#!me compare performance of counting size first
Base.IteratorSize(::Type{OneRing}) = Base.SizeUnknown()

Base.eltype(i::OneRing) =  HalfEdgeHandle

nhalfedges(topo) = length(topo.he)
nedges(topo) = nhalfedges(topo) >> 1
nvertices(topo) = length(vertices(topo))
nverts(topo) = nvertices(topo)
nfaces(topo) = length(faces(topo))

Topology( tris::Vector{Tuple{T,T,T}}, nVert::TT ) where {T<:Integer, TT<:Integer} =
  Topology( map(t->(map(VertexHandle,t)...,),tris), nVert )

function Topology( poly::Vector{VT}, nVert; handle_bad_geo = true ) where {T<:Integer,
                                                                           VT<:AbstractVector{T}}
  nFace = length(poly)
  nHEdge = sum( map(length,poly) )
  topo_v2he, topo_he = (fill(HalfEdgeHandle(0),nVert), Vector{HalfEdge}(undef,nHEdge))
  topo_face2he = Vector{HalfEdgeHandle}(undef,nFace)

  edgemap = Dict{Tuple{VertexHandle, VertexHandle}, HalfEdgeHandle}()
  heᵢ::HalfEdgeHandle = 1
  he2tail_vh = Vector{VertexHandle}(undef,nHEdge)

  for (i,fᵢ) in enumerate(poly)

    nv = length(fᵢ)
    topo_face2he[i] = heᵢ
    # each half edge needs to link to next half edge.
    # loop over edges and next halfedge handle, increment halfedge handle as we go
    heloop = zip(zip(drop(cycle(fᵢ), nv-1), fᵢ), take(drop(cycle(heᵢ:heᵢ+nv-1), 1), nv))
    for ((k,j),he_next) in heloop
      topo_he[heᵢ] = HalfEdge(j,he_next,he_next,FaceHandle(i))
      topo_v2he[j] = he_next
      edgemap[(k,j)] = heᵢ
      he2tail_vh[heᵢ] = k
      heᵢ += 1
    end

  end

  boundary_he = Vector{HalfEdge}(undef,0)
  topo_he2edge = Vector{EdgeHandle}(undef,0)
  prototopo = Topology(topo_v2he, topo_face2he, topo_he, topo_he2edge)
  topo_he =
    map( zip(topo_he, 1:length(topo_he)) ) do (he,hehi)
      headvert = head(he)
      tailvert = he2tail_vh[hehi]
      if haskey(edgemap, (headvert, tailvert))
        opp_heh = edgemap[(headvert, tailvert)]
        HalfEdge(he.head, he.next, opp_heh, he.face )
      else
        # create boundary halfedges. one on inside and one on outside.
        opposite_he = push!(boundary_he,
                            HalfEdge(tailvert, HalfEdgeHandle(0), HalfEdgeHandle(hehi), nothing))
        opp_heh = HalfEdgeHandle(nHEdge + length(boundary_he))
        HalfEdge(he.head, he.next, opp_heh, he.face)
      end
    end

  prototopo = Topology(topo_v2he, topo_face2he, vcat(topo_he, boundary_he), topo_he2edge)

  # iterate over boundaries.  set vertex map so boundary vertex tail is boundary he tail
  for i in 1:length(boundary_he)
    b_heh = HalfEdgeHandle(i+nHEdge)
    tailvert = head(prototopo, opposite(prototopo, b_heh))
    topo_v2he[tailvert] = b_heh
  end

  # now we can make a proper boundary halfedge
  boundary_he =
    map( boundary_he ) do b_he
      # half edge from tail of the head vertex of this boundary he is next heh
      # this *__>*
      #       ^  \  next
      #        \  v
      #          *
      HalfEdge( head(b_he), topo_v2he[head(b_he)] ,opposite(b_he), nothing )
    end

  prototopo = Topology(topo_v2he, topo_face2he, vcat(topo_he, boundary_he), topo_he2edge)

  # make a map to EdgeHandles as this is quite useful for interfacing with linear algebra
  resize!(topo_he2edge, nhalfedges(prototopo))
  for (eh, hh) in enumerate(UniqueHalfEdges(prototopo))
    topo_he2edge[hh] = EdgeHandle(eh)
    topo_he2edge[opposite(prototopo, hh)] = EdgeHandle(eh)
  end

  Topology(topo_v2he, topo_face2he, vcat(topo_he, boundary_he), topo_he2edge)
end

function Topology( tris::Vector{T}, nVert::TT ) where {T<:Integer, TT<:Integer}
  trit = vec.(partition(tris, 3))
  if( length(trit[end]) < 3 )
    trit = trit[1:end-1]
  end
  Topology( trit, nVert )
end

"""
  Topology( P )

Represent a list of connected polygons P with halfedge datastructures.  P is either a flat list of triangles or a nested list of polygons with arbitrary vertex counts.  Polygons will be a list of vertex indices.

For best results the polygons P should be manifold.

"""
Topology(polys) = Topology(polys,reduce(max,flatten(polys)))

# some useful geometry stuff
area(P,eab,ebc,eca) = norm((P[eab[2]] - P[eab[1]]) × (P[ebc[2]]-P[eab[1]]))*0.5

function area(P::T,eaz...) where {T<:AbstractArray}
  p₀ = P[eaz[1][2]]
  second = x->x[2]
  a = reduce(partition(second.(eaz[2:end]),2); init = 0.0) do acc,(i,j)
    acc+norm((P[i]-p₀) × (P[j]-p₀))
  end
  a*0.5
end

area(P::T,el) where {T<:AbstractArray} = area(P,el...)
area(topo::Topology, P, fh::FaceHandle) = area(P, edges(topo, fh)...)
area(topo::Topology, P, heh::HalfEdgeHandle) = area(P, edges(topo, heh)...)
area(topo::Topology, P, poly::Polygon) = area(topo, P, poly.starth)

""" area from 3 edge lengths. Heron's formula """
function area( a::T, b::T, c::T ) where T<:Number
  s = (a+b+c)/2
  sqrt(s*(s-a)*(s-b)*(s-c))
end

function circumcenter_triangle(a::T,b::T,c::T) where T
  ac = c-a
  ab = b-a
  w = ab×ac

  u = (w×ab)*(ac⋅ac)
  v = (ac×w)*(ab⋅ab)
  x = (u+v)/(2.0 * w⋅w)

  x+a
end

function circumcenter_triangle(topo, P, h::HalfEdgeHandle)
  a = P[head(topo, h)]
  b = P[head(topo, next(topo, h))]

  if isboundary(topo, h)
    return (a+b)*0.5
  end

  c = P[head(topo, next(topo, next(topo, h)))]
  circumcenter_triangle(a,b,c)
end

function dihedral_angle(topo::Topology, P, h)
  if isboundary(topo, h) || isboundary(topo, opposite(topo, h))
    return 0.0
  end

  n1 = trinormal(topo, P, h)
  n2 = trinormal(topo, P, opposite(topo, h))
  w = (normalize∘-)(P[[edge(topo, h)...]]...)

  cosθ = n1⋅n2
  sinθ = (n1×n2)⋅w

  atan(sinθ, cosθ)
end

"""
    inside_verts(topo, P, tris)

return the vertices on the inside of a set of overlapping tris
"""
function inside_verts(topo, P, overlaptris::Tuple{HalfEdgeHandle, HalfEdgeHandle})
  #cache = winding_number_cache(topo, P)

end

"""
    enclose(topo, P, boundary_faces)

return a list of all vertices enclosed by a contour of intersecting faces.
If the mesh is not closed this may return all the vertices in the mesh.
"""
function enclose(topo, P, boundary::Vector{Tuple{HalfEdgeHandle, HalfEdgeHandle}})
  #nPi = map(boundary_faces) do heh

  #end
end

#enclose(topo, P, bf::Vector{FaceHandle}) = enclose(topo, P, ∂(halfedge, topo).(bf))

function triangle_edges_inflated(inflate, a, b, c, d, e, f)
  inflateit(a,b,c) = begin
    com = (a+b+c)*0.333333333333333333
    a = a+(a-com)*(1.0+inflate)
    b = b+(b-com)*(1.0+inflate)
    c = c+(c-com)*(1.0+inflate)
    (a,b,c)
  end

  Collision.triangle_edges(inflateit(a,b,c)..., inflateit(d,e,f)...)
end

"""
    floodfill(topo, P; verbose=true)

flood fill values at vertices where intersecting triangles create barrier.
"""
function floodfill(topo::Topology, P; verbose=false, inflate_triangles=0.001)
  # find intersections
  hits = collide_self(topo, P);
  edgehits = sort.(collide_self_edges(topo, P, partial(triangle_edges_inflated, inflate_triangles)))

  blockers = Dict(map(ei->(ei,1), edgehits));

  Fhit = (unique∘sort)(vcat( map(x->x[1],hits), map(x->x[2],hits)));
  Vhit = (unique∘sort∘collect∘Iterators.flatten)(map(iF->vertices(topo, FaceHandle(iF)), Fhit));

  V = Vhit;
  isls = Archipelago(length(P));

  for vᵢ ∈ (VertexHandle(i) for i in 1:length(P))
    vring = OneRingVerts(topo, VertexHandle(vᵢ))
    for vrᵢ in Iterators.filter(vrᵢ->haskey(blockers, sort((vᵢ, vrᵢ)))==false, vring)
      IslandFind.union!(isls, (Int(vᵢ), Int(vrᵢ)))
    end
  end
  find_islands!(isls, !verbose)
end

"""
    polygons(topo)

extract polygons as arrays of vertex indices
"""
polygons(topo) = map(f->vertices(topo,Polygon(topo,f)), (faces(topo)))

"""
  eff(topo)

the nfacesx3 matrix F
"""
eff(topo) = reduce(vcat, polygons(topo)')

minussigned( ::Val{false} ) = 1.0
minussigned( ::Val{true} ) = -1.0

"""
    incidence(topo, AHandleType, BHandleType, oriented = false)

build incidence matrix mapping between two elements
|b|x|a| matrix. i.e. incidence of {vertex} and {face} has size(vertex) columns and size(face) rows

If oriented then matrix element will be negative if elements are oriented in opposite sense.
For a vertex-edge this means positive if the vertex is at the head of the edge.
For edge-face this means positive if direction of circulation of the edge direction is ccw relative to face
"""
function incidence(topo, a::Type{VertexHandle}, b::Type{EdgeHandle}, oriented = false)
  R = Int[]
  C = Int[]
  for (eᵢⱼ,(vᵢ,vⱼ)) in enumerate(edges(topo))
    push!(C, vᵢ)
    push!(C, vⱼ)
    push!(R, eᵢⱼ)
    push!(R, eᵢⱼ)
  end

  if oriented
    V = map(i->(i%2 ==1) ? -1.0 : 1.0, 1:length(C))
  else
    V = ones(Float64, length(C))
  end

  sparse(R,C,V)
end

function incidence(topo, a::Type{EdgeHandle}, b::Type{FaceHandle}, oriented = false)
  R = Int[]
  C = Int[]
  V = Float64[]
  for (eᵢⱼ,heᵢⱼ) in enumerate(UniqueHalfEdges(topo))
    f = face(topo, heᵢⱼ)
    if f != nothing
      push!(R, f)
      push!(C, eᵢⱼ)
      push!(V, 1.0)
    end
    f = face(topo, opposite(topo,heᵢⱼ))
    if f != nothing
      push!(R, f)
      push!(C, eᵢⱼ)
      push!(V, minussigned(Val(oriented)))
    end
  end

  sparse(R,C,V)
end

hashedge(topo::Topology, a, b) = Int((b << (leading_zeros(0)-leading_zeros(nedges(topo)) + 1)) + a)

hashedge(topo::Topology, (a,b)) = hashedge(topo, a, b)

function unhashedge(topo::Topology, ab::Int)
  bshift = (leading_zeros(0)-leading_zeros(nedges(topo)) + 1)
  b = ab >> bshift
  a = ab - (b << bshift)
  (a,b)
end

Base.sort((a,b)::T) where {V,T<:Tuple{V,V}} = a>b ? (b,a) : (a,b)

"""
  IncidentEdges( topo, HandleType )

Get list of incident edges as EdgeHandle[]
Edges will be ordered correctly with winding order of face.
"""
function IncidentEdges(topo::Topology, ::Type{FaceHandle})
  #!me can rewrite this to using he2edge map

  # first place hash of edges in correctly ordered positions
  incidente = Vector{Vector{EdgeHandle}}()
  sizehint!(incidente, nfaces(topo))
  for shf in faces(topo)
    ev = Vector{EdgeHandle}()
    sizehint!(ev,3)
    nhf = shf
    while true
      ab = edge(topo, nhf)
      push!(ev, EdgeHandle(hashedge(topo, sort(ab))))
      nhf = next(topo, nhf)
      nhf == shf && break
    end
    push!(incidente, ev)
  end

  # find and replace hashed edges with unique edge handles
  for (ie, heh) in enumerate(UniqueHalfEdges(topo))
    ehsh = hashedge(topo, sort(edge(topo, heh)))
    if !isboundary(topo, heh)
      replace!(incidente[face(topo, heh)], ehsh=>ie; count=1)
    end
    oheh = opposite(topo, heh)
    if !isboundary(topo, oheh)
      replace!(incidente[face(topo, oheh)], ehsh=>ie; count=1)
    end
  end

  incidente
end

"""
  IncidentHalfEdges( topo, HandleType )

Get list of incident halfedges as HalfEdgeHandle[]
Halfedges will be ordered correctly with winding order of face.
"""
function IncidentHalfEdges(topo::Topology, ::Type{FaceHandle})
  # first place hash of edges in correctly ordered positions
  incidente = Vector{Vector{HalfEdgeHandle}}()
  sizehint!(incidente, nfaces(topo))
  for shf in faces(topo)
    ev = Vector{HalfEdgeHandle}()
    sizehint!(ev,3)
    nhf = shf
    while true
      ab = edge(topo, nhf)
      push!(ev, HalfEdgeHandle(hashedge(topo, sort(ab))))
      nhf = next(topo, nhf)
      nhf == shf && break
    end
    push!(incidente, ev)
  end

  # find and replace hashed edges with unique halfedge handles
  for heh in UniqueHalfEdges(topo)
    ehsh = hashedge(topo, sort(edge(topo, heh)))
    if !isboundary(topo, heh)
      replace!(incidente[face(topo, heh)], ehsh=>heh; count=1)
    end
    oheh = opposite(topo, heh)
    if !isboundary(topo, oheh)
      replace!(incidente[face(topo, oheh)], ehsh=>heh; count=1)
    end
  end

  incidente
end

"""
GeometryTypes mesh to halfedge and extracted tris, verts
"""
function geometry2hemesh( cat )
  tris = map(t->[map(i->Int(i)+1,t)...],cat.faces)
  P = Vector3d.(cat.vertices)
  hemesh = Topology(tris)
  (hemesh,tris,P)
end

"""
  facelist(topo)

return faces as arrays of integers
"""
facelist(topo::Topology) = (x->Int.(vertices(topo,Polygon(topo,x)))).(faces(topo))

"""
  trinormal(topo, P, h)

normal for triangle associated with HalfEdgeHandle h
"""
function trinormal(topo::Topology, P, h::HalfEdgeHandle)
  h = isboundary(topo, h) ? opposite(topo, h) : h
  pa = P[head(topo, h)]; h = next(topo, h)
  pb = P[head(topo, h)]; h = next(topo, h)
  pc = P[head(topo, h)]
  n = (pb-pa)×(pc-pa)
  nn = n⋅n
  if isapprox(nn, 0, atol=eps())
    normalize(ones(pa))
  else
    n/sqrt(nn)
  end
end

"""
    normals(topo, P)

triangle normals for all faces
"""
normals(topo::Topology, P) = map( fheh->trinormal(topo, P, fheh), faces(topo))

"""
    weightednormal(topo, P, heh)

area weighted normal of a triangle
"""
function weightednormal(mesh, P, heh::HalfEdgeHandle)
  p₀ = P[head(mesh, heh)]
  heh₁ = next(mesh, heh)
  p₁ = P[head(mesh, heh₁)]
  p₂ = P[head(mesh, next(mesh, heh₁))]

  0.5*(p₁-p₀)×(p₂-p₀)
end

"""
    vertexnormal(topo, p, ring)

normal of a vertex given it's OneRing
"""
function vertexnormal( mesh, P, ring::OneRing, normal = weightednormal )
  iring = Iterators.filter(heh->!isboundary(mesh,heh),ring)
  if isempty(iring)
    zero(eltype(P))
  else
    normalize(mapreduce(heh->normal(mesh,P,heh),+,iring))
  end
end

"""
    vertexnormals(topo, P, weighted=true)

vertex normals for all vertices in mesh, optionally area weighted.
"""
function vertexnormals( mesh, P, weighted = true )
  rings = (heh->OneRing(heh,mesh)).(vertices(mesh))
  if weighted
    n = ring->vertexnormal( mesh, P, ring, weightednormal )
  else
    n = ring->vertexnormal( mesh, P, ring, normalize∘weightednormal )
  end
  n.(rings)
end

"""
  angles(topo, P, poly)

Interior angles for polygon as ccw rotation around tail of each halfedge
"""
function angles(topo::Topology, P, poly::Polygon)
  n = length(poly|>collect)
  hv = (head(topo,h) for h in cycle(poly))
  abc = zip(drop(hv,n-1), hv, drop(hv,n-2))
  map(take(abc,n)) do (a,b,c)
    acos(normalize(P[b]-P[a])⋅normalize(P[c]-P[a]))
  end
end

"""
  angle(topo, P, heh)

Interior angle across from the halfedge
"""
function angle(topo, P, h::HalfEdgeHandle)
  poly = Polygon(topo, h)
  n = length(poly|>collect)
  nop = n >> 1
  hv = (head(topo,h) for h in cycle(poly))
  a,b,c = P[[first(drop(hv,nop)), first(drop(hv,nop-1)), first(drop(hv,nop+1))]]
  acos(normalize(b-a)⋅normalize(c-a))
end

function is_delaunay(topo::Topology, P, heh::HalfEdgeHandle)
  angle(topo, P, heh) + angle(topo, P, opposite(topo, heh)) <= π + 1e-5
end

"""
    orientation( (a,b,c), (u,v) )

returns 1.0 if the edge u,v has same winding as a,b,c otherwise -1.0
"""
function orientation( a::H ,b::H, c::H, u::H, v::H ) where {H<:Union{VertexHandle, HalfEdgeHandle}}
  (u==a && v==b) || ((u==b) && (v==c)) || ((u==c) && (v==a)) ? 1.0 : -1.0
end

orientation( abc::FT, uv::ET ) where {FT<:Union{Vector, Tuple}, ET<:Union{Vector, Tuple}} =
  orientation(abc..., uv...)

"""
  improve_mesh(topo, P; smoothness)

Improve the quality of the mesh.
Particularly so discrete differential geometry operators yield well conditioned matrices
Note: input topology will not longer match.  i.e. FaceHandle(i) will not necessarily return same vert tuple

Currently element counts remain the same.  The only operation performed is to flip edges between two triangles so that all angles are closer to π/2

Returns the number of edges flipped
"""
#==
#!me  not really working
If smoothness is set to something other than 0, edge flips will not occur if the volume of the tetrahedron formed from the 4 vertices involved as a ratio against triangle areas is less than the smoothness.
In other words, smoothness helps preserve the silhouette of the mesh.

==#
function improve_mesh(topo::Topology, P) #; smoothness=0.0)
  # reverse map into Topology::v2he and Topology::face2he data
  he2v = Vector{Union{Missing,Int}}(missing, length(topo.he))
  he2face = Vector{Union{Missing,Int}}(missing,length(topo.he))

  for (i,heᵢ) in enumerate(topo.v2he)
    he2v[heᵢ] = i
  end

  for (i,heᵢ) in enumerate(topo.face2he)
    he2face[heᵢ] = i
  end

  for heᵢ in halfedges(topo)
    if ismissing(he2face[heᵢ])
      for hpᵢ in Polygon(topo, HalfEdgeHandle(heᵢ))
        if !ismissing(he2face[hpᵢ])
          he2face[heᵢ] = he2face[hpᵢ]
          break;
        end
      end
    end
  end

  hasflipped = falses(length(topo.face2he))

  for heᵢ in unique_inside_halfedges(topo)
    bscale = 0.0
    α = 0.0
    β = 0.0
    skipα = false
    skipβ = false
    oheᵢ = opposite(topo, heᵢ)

    if hasflipped[he2face[heᵢ]] ||
      (!isboundary(topo, oheᵢ) && hasflipped[he2face[oheᵢ]])
      continue
    end

    if isboundary(topo, oheᵢ) || length(Polygon(topo, FaceHandle(he2face[oheᵢ]))|>collect) != 3
      skipβ = true
    else
      β = angle(topo, P, oheᵢ)
      polyβ = he2face[oheᵢ]
      heflip = oheᵢ
      bscale = 0.5
    end
    if isboundary(topo, heᵢ) || length(Polygon(topo, FaceHandle(he2face[heᵢ]))|>collect) != 3
      skipα = true
    else
      α = angle(topo, P, heᵢ)
      polyα = he2face[heᵢ]
      heflip = heᵢ
      bscale += 0.5
    end

    if α + β > π*bscale

      if !skipα && !skipβ
        #==
            N
           /α\
          / ↳ \
         W=⃭=⃭=⃭=⃭=⃭E
          \ ↰ /
           \β/
            S

            to

            N
           /|\
          / | \
         W ↰| ↰E
          \α|β/
           \|/
            S
        ==#
        hmidα = heᵢ; midα = topo.he[heᵢ]
        hmidβ = oheᵢ; midβ = topo.he[oheᵢ]
        hNE = midα.next; NE = topo.he[hNE]
        hSW = midβ.next; SW = topo.he[hSW]
        hNW = NE.next; NW = topo.he[hNW]
        hSE = SW.next; SE = topo.he[hSE]

        N = NE.head; W = NW.head; S = SW.head; E = SE.head

        # skip if the silhouette will be made significantly less smooth
        #==
        if smoothness > 0.0
          # check ratio of volume of tetrahedron vs avg area
          n = P[N]; s = P[S]; e = P[E]; w = P[W]
          se = e-s; sw = w-s;
          base = se×sw
          h = normalize(base)⋅(n-s)
          avgl = (norm(n-s) + norm(w-e))*0.5

          if h/avgl > 1.0/smoothness
            continue
          end
        end
        ==#

        # new midα. overwrite midα
        topo.he[heᵢ] = HalfEdge(N, hNW, hmidβ, midα.face)
        # new midβ. overwrite midβ
        topo.he[oheᵢ] = HalfEdge(S, hSE, hmidα, midβ.face)

        # new NE
        topo.he[hNE] = HalfEdge(N, hmidβ, NE.opposite, midβ.face)
        # new NW
        topo.he[hNW] = HalfEdge(W, hSW, NW.opposite, midα.face)
        # new SW
        topo.he[hSW] = HalfEdge(S, hmidα, SW.opposite, midα.face)
        # new SE
        topo.he[hSE] = HalfEdge(E, hNE, SE.opposite, midβ.face)

        # fix facelist
        topo.face2he[he2face[hmidα]] = hmidα
        topo.face2he[he2face[hmidβ]] = hmidβ

        # fix vertexlist.  keep in mind iteration from boundary, so we can't pick arbitrary he
        if topo.v2he[E] == hmidβ
          topo.v2he[E] = hNE
        end
        if topo.v2he[W] == hmidα
          topo.v2he[W] = hSW
        end

        hasflipped[polyα] = true
        hasflipped[polyβ] = true

      else
        # ignore boundary for now as it requires adding a new point and face
      end

    end
  end
  sum(hasflipped)
end

#==
struct Manifold
  soup2manifold::Vector{VertexHandle}
  manifold2soup::Vector{VertexHandle}
  manifoldpoly::Vector{Vector{VertexHandle}}

  orphan::Vector{VertexHandle}
end

function make_manifold( P, poly )
  nVert = length(P)

  # weld overlapping verts
  # AABBTree()

  # fix any fins or other non-manifold topology
  #

  # isolate islands and keep the biggest one?

  orphanv = VertexHandle[]
  floaters = setdiff(1:nVert,reduce(vcat,poly))
  for i in floaters
    push!(orphanv, i)
  end

end
==#

"""
  boundary_verts(topo)

find loops of boundary verts
"""
function boundary_verts(topo)
  bh = filter(∂(isboundary, topo), halfedges(topo))

  bvl = Vector{VertexHandle}[]

  while !isempty(bh)
    bl = BoundaryLoop(topo, bh |> first)
    bh = setdiff(bh, bl)
    push!(bvl, map(∂(tail, topo), bl))
  end

  bvl
end

boundary_vertices(x) = boundary_verts(x)

"""
  boundary_interior(topo, boundary)

the set of vertices connected to the boundary verts but not part of the boundary
"""
function boundary_interior(mesh, boundary)
  allverts = mapreduce(x->OneRing(mesh, x) |> ∂(∂(mapreduce,(x->[x...])∘∂(edge,mesh)),vcat), vcat , boundary)
  setdiff(allverts,boundary)
end

boundary_interior(topo) = map(b->boundary_interior(topo, b), boundary_verts(topo))

# single airty versions

for fn in (:next, :head, :tail, :prev, :opposite, :isboundary, :edge, :halfedge)
  @eval $fn(h::T) where T<:Handle = topo->$fn(topo, h)
  @eval $fn(topo::Topology) = h->$fn(topo, h)
  @eval $fn(f::F) where {F<:Function} = (topo,h)->$fn(topo,f(topo,h))
end

"""
parse an .obj file format mesh into points and faces.
"""
function parseobj(instream)
  # of form "f v1 v2 v3 ....""
  process_face(words::Vector{S}) where {S <: AbstractString} = (words,) # make same format as the others
  # of form "f v1//vn1 v2//vn2 v3//vn3 ..."
  process_face_normal(words::Vector{S}) where {S <: AbstractString} = mapreduce(first,vcat,split.(words, "//"))
  # of form "f v1/vt1 v2/vt2 v3/vt3 ..." or of form "f v1/vt1/vn1 v2/vt2/vn2 v3/vt3/vn3 ...."
  process_face_uv_or_normal(words::Vector{S}) where {S <: AbstractString} = mapreduce(first,vcat,split.(words, Ref('/')))
  process_face_uv(words::Vector{S}) where {S <: AbstractString} = mapreduce(second,vcat,split.(words, Ref('/')))

  # this is cribbed from https://github.com/JuliaIO/MeshIO.jl/blob/master/src/io/obj.jl
  #  normals, uvs etc can be parsed using that package
  v = Vector3d[]
  uv = Vector{Float64}[]
  f = Vector{Int}[]
  f_uv = Vector{Int}[]
  for line in filter(ln->!(startswith(ln,"#") ||
                           isempty(ln) ||
                           all(iscntrl,ln)),
                     (strip∘chomp).(eachline(instream)))
    # assuming one element per line
    words = split(line)
    command = popfirst!(words)

    if command == "v"
      push!(v, Vector3d(parse.(Float64, words)))
    elseif command == "vt"
      push!(uv, parse.(Float64, words))
    elseif command == "f"
      if any(x->occursin("//", x), words)
        fs = process_face_normal(words)
        push!(f,parse.(Int,fs))
      elseif any(x->occursin("/", x), words)
        fs = process_face_uv_or_normal(words)
        push!(f,parse.(Int,fs))
        fs_uv = process_face_uv(words)
        push!(f_uv,parse.(Int,fs_uv))
      else
        fs = words
        push!(f,parse.(Int,fs))
      end
    end

  end

  # obj uses base 1 indexing
  mesh = Dict("P"=>v, "Poly"=>f)
  if length(f_uv) > 0
    mesh["PolyUV"] = f_uv
    mesh["UV"] = uv
  end

  mesh
end

"""
    loadmesh("pathto.obj")

Load a Mesh from an obj file.  Topology and points.
"""
function loadmesh(filename::AbstractString)
  if split( filename, "." )[end] == "obj"
    local topo, P
    open(filename, "r") do f
      rp = parseobj(f)
      topo = Topology(rp["Poly"]);
      P = rp["P"];
    end
    (topo, P)
  else
    :FileTypeNotSupported
  end
end

#==========  Mesh Editing ==========#
#
""" ordered list of all vertex indices referenced by faces """
vertex_indices(F) = (unique∘sort)(reduce(vcat, F))
vertex_indices(topo::Topology) = vertex_indices(polygons(topo))

"""
make an array to remap from vertex indices used by F to simple ordinal list of smallest size

julia> remapV([[2,3,5],[5,7,2]], 8)
8-element Array{Int64,1}:
0
1
2
0
3
0
4
0

"""
function remapV(F, nP)
  VF = vertex_indices(F)
  toFsub = zeros(Int64,nP)
  toFsub[VF] = collect(1:length(VF))
  toFsub
end

remapV(topo::Topology) = remapV(polygons(topo), nverts(topo))

"""
  add_ring(topo, C)

the collection C and all the vertices directly connected to any vertex in the collection C
"""
function add_ring(topo::Topology, H::Vector{HalfEdgeHandle})
  reduce(vcat,
         (let
            tl = tail(topo, heh)
            vcat(tl, OneRingVerts(topo, tl))
          end
          for heh ∈ H)) |> sort |> unique |> V->map(vᵢ->opposite(topo, halfedge(topo, vᵢ)), V)
end

add_ring(topo::Topology, V::Vector{VertexHandle}) = vertex.([topo], add_ring(topo, halfedge.([topo], V)))

function remove_ring(topo::Topology, V::Vector{VertexHandle})
  ismember = fill(false, nverts(topo))
  ismember[V] .= true

  filter(V) do vᵢ
    oring = OneRing(topo, vᵢ)
    reduce((a,b)->a && ismember[head(topo,b)], oring; init=true)
  end
end

#==========  Collision Detection ============#

include("BoundingVolumeTrees.jl")
include("Collision.jl")
include("IslandFind.jl")

using .BoundingVolumeTrees
using .Collision
using .IslandFind

export
BVH,
Collider,
collide_self,
collide_self_edges,
query_aabb,
update_collider

const BVH = BoundingVolumeTrees

struct Collider{T,F}
  bvh::BVH.DAABBNode{T,F}
  mesh::BVH.Triangles{T,F}
end

"""
    Collider(topo, P)

precompute collision detection accelerators
"""
function Collider(topo::Topology, P::V) where {T, PT<:AbstractVector{T}, V<:Vector{PT}}
  mesh = BVH.Triangles((collect∘flatten)(facelist(topo)),P)
  bvh = BVH.createBVH(mesh)
  Collider{Int64,T}(bvh, mesh)
end

"""
    collide_self(topo, P, collidef)

return a list of faces which are intersecting in the mesh.
provide the face vs face collision method in collidef

collidef should be a function that accepts 6 points as arguments, which are the points of the two triangle.
It should return a tuple where the first element is a boolean value indicating if there was a collision or not
"""
collide_self(topo::Topology, P, collidef::F = Collision.triangle_triangle) where {F<:Function} = collide_self(Collider(topo, P), collidef)

"""
    collide_self(collider, collidef)

return a list of faces which are intersecting in the mesh.
provide the face vs face collision method in collidef
"""
function collide_self(collider::Collider, collidef::F = Collision.triangle_triangle) where{F<:Function}
  BVH.selfintersect(collider.bvh, collider.mesh, collidef)
end

function edgeshit2((a,b,(_,((s,(v1,v2)), (t,(w1,w2))))), triP)
    ab = (a,b)
    ((triP[ab[s]][v1], triP[ab[s]][v2]),
        (triP[ab[t]][w1], triP[ab[t]][w2]))
end

function edgeshit(hit::T, triP) where T<:Tuple{Int64,Int64,Tuple{Bool,Vector{Tuple{Int64,Tuple{Int64,Int64}}}}}
  (a,b,(_,E)) = hit
  ab = (a,b)
  map(E) do (s,(v1,v2))
    (triP[ab[s]][v1], triP[ab[s]][v2])
  end
end

"""
    collide_self_edges(collider)

collect all (VertexHandle, VertexHandle) tuples representing edges of the mesh in collision with triangles of th e mesh.
"""
function collide_self_edges(topo, collider::Collider, colfn = Collision.triangle_edges)
  hits = collide_self(collider, colfn)
  Fhit = (unique∘sort)(vcat( map(x->x[1],hits), map(x->x[2],hits)))
  if isempty(Fhit)
    return []
  end
  triP = Vector{Vector{VertexHandle}}(undef, reduce(max, Fhit))
  triP[Fhit] = map(iF->vertices(topo, FaceHandle(iF)), Fhit)

  map(hit->edgeshit(hit, triP), hits) |> Iterators.flatten |> collect
end

collide_self_edges(topo::Topology, P::Vector{T}, colfn = Collision.triangle_edges) where T<:AbstractVector = collide_self_edges(topo, Collider(topo, P), colfn)
"""
    query_aabb(collider, aabb, hits)

query the collision detection structure for a list of faces overlapping the given axis aligned bounding box
new hits are pushed onto the passed in hits vector
"""
query_aabb(c::Collider{T,F}, aabb::BVH.AABB{F}, hits::Vector{T}) where {T,F} = BVH.query(c.bvh, aabb, hits)

function query_aabb(c::Collider{T,F}, aabb::BVH.AABB{F}) where {T,F}
  hits = Vector{T}()
  BVH.query(c.bvh, aabb, hits)
  hits
end

"""
    update_collider(collider)

update the collision detection structures in collider to reflect any change in mesh positions
"""
update_collider(collider::C) where C<:Collider = BVH.updateBVH(collider.bvh, collider.mesh)

""" face to edge lengths. for intrinsic triangulations """
function edge_length_map(topo::Topology, P)
  reduce(vcat,
         map(faces(topo)) do heha
           hehb = next(topo, heha)
           hehc = next(topo, hehb)
           map( norm, [P[head(topo, heha)]-P[head(topo, hehc)],
                       P[head(topo, hehb)]-P[head(topo, heha)],
                       P[head(topo, hehc)]-P[head(topo, hehb)]]' )
         end)
end

"""
  the gluing map for intrinsic triangulations.
  currently happening largely outside of HalfEdges data structures. It's mostly a quick and dirty
  port of https://github.com/nmwsharp/intrinsic-triangulations-tutorial/blob/master/tutorial_completed.py
"""
function gluing_map(topo::Topology)
  FaceEdgeOrNo = Union{Tuple{FaceHandle, Int64}, Nothing}
  G = Array{FaceEdgeOrNo, 2}(nothing, nfaces(topo), 3)
  map(enumerate(faces(topo))) do (i,heha)
    hehb = next(topo, heha)
    hehc = next(topo, hehb)
    polyo = heh-> let
      faceo = face(topo, opposite(topo, heh))
      if isnothing(faceo)
        nothing
      else
        (faceo,
         indexin(opposite(topo, heh), collect(Polygon(topo, faceo)))[1])
      end
    end

    G[i,:] = [polyo(heha), polyo(hehb), polyo(hehc)]
  end
  G
end

next_side(fs::Nothing) = fs
next_side(fs::Tuple{T, Int64}) where T<:Union{FaceHandle, Int64} = (fs[1], (fs[2]%3)+1)
other_side(G, fs::Nothing) = fs
other_side(G, fs) = G[fs...]

""" angle given 3 edge lengths a,b,c opposite to a """
function opposite_corner_angle(a::T, b::T, c::T) where T<:Number
  acos((b^2 + c^2 - a^2)/(2*b*c))
end

opposite_corner_angle(l::T, fs::Nothing) where T<:Array = nothing

function opposite_corner_angle(l::T, fs) where T<:Array
  a = l[fs...]
  b = l[next_side(fs)...]
  c = l[next_side(next_side(fs))...]
  opposite_corner_angle(a, b, c)
end

"""
    Computes the length of the opposite diagonal of the diamond formed by the
    triangle containing fs, and the neighboring triangle adjacent to fs.
    This is the new edge length needed when flipping the edge fs.
    :param G: |F|x3x2 gluing map
    :param l: |F|x3 array of face-side edge lengths
    :param fs: A face-side (f,s)
    :returns: The diagonal length
"""
function diagonal_length(G, l, fs)
  # /  |  \
  # \ fs  / op
  #  u | u
  #   \|/
  fs_opp = other_side(G, fs)
  u = l[next_side(next_side(fs))...]
  v = l[next_side(fs_opp)...]
  theta_A = opposite_corner_angle(l, next_side(fs))
  theta_B = opposite_corner_angle(l, next_side(next_side((fs_opp))))

  # Law of cosines
  sqrt(u^2 + v^2 - 2 * u * v * cos(theta_A + theta_B))
end

function is_delaunay(G, l, fs::Tuple{T, Int64}) where T<:Union{FaceHandle, Int64}
  fs_opp = other_side(G, fs)

  A = opposite_corner_angle(l, fs)
  B = opposite_corner_angle(l, fs_opp)
  A+B <= π + 1e-5
end

halfedge(topo::Topology, fs::Tuple{T, Int64}) where T<:Union{FaceHandle, Int64} =
  collect(Polygon(topo, FaceHandle(fs[1])))[fs[2]]

"""
  Glues together the two specified face sides.
  The gluing map G is updated in-place.
  :param G: |F|x3x2 gluing map
  :param fs1: a face-side (f1,s1)
  :param fs2: another face-side (f2,s2)
"""
function glue_together!(G, fs1, fs2)
    G[fs1...] = fs2
    G[fs2...] = fs1
end

"""
  Performs an intrinsic edge flip on the edge given by face-side s0. The
  arrays F, G, and l are updated in-place.
  This routine _does not_ check if the edge is flippable. Conveniently, in
  the particular case of flipping to Delaunay, non-Delaunay edges can always
  be flipped.
  :param F: |F|x3 face list. ==>  polygons(topo).
  :param G: |F|x3x2 gluing map G
  :param l: |F|x3 edge-lengths array, giving the length of each face-side
  :param s0: A face-side of the edge that we want to flip
  :returns: The new identity of the side fs_a
"""
function flip_edge!(F, G, l, s0)
  # Get the neighboring face-side
    s1 = other_side(G, s0)

    # Get the 3 sides of each face
    s2, s3 = next_side(s0), next_side(next_side(s0))
    s4, s5 = next_side(s1), next_side(next_side(s1))

    # Get the sides glued to each edge of the diamond
    s6, s7, s8, s9 = other_side(G,s2), other_side(G,s3), other_side(G,s4), other_side(G,s5)

    # Get vertex indices for the vertices of the diamond
    #!me slight difference, seems I constructed different convention to ref vertices
    #!me in my convention the vertex v1 is the vertex at the head of the edge point at v1
    #!me the entry in the length table and elsewhere should reflect this
    v0, v1, v2, v3 = F[s0...], F[s2...], F[s3...], F[s4...]

    # Get the two faces from our face-sides
    f0, f1 = s0[1], s1[1]

    # Get the original lengths of the outside edges of the diamond
    l2, l3, l4, l5 = l[s2...], l[s3...], l[s4...], l[s5...]

    # Compute the length of the new edge
    new_length = diagonal_length(G, l, s0)

    # Update the adjacency list F
    F[f0,:] = [v1, v2, v3]
    F[f1,:] = [v3, v0, v1]

    # Re-label elements.
    # Usually this does nothing, but in a Delta-complex one of the neighbors of
    # these faces might be one of the faces themselves! In that case, this
    # re-labels the neighbors according to the updated labels after the edge flip.
    relabel = s -> let
        # NOTE: these variables (s2, f0, etc) are automatically accessed from the outer scope
        # in Python; in other languages we could add them as additional arguments.
        if s == s2 return (f1, 3) end
        if s == s3 return (f0, 2) end
        if s == s4 return (f0, 3) end
        if s == s5 return (f1, 2) end
        s
      end
    s6, s7, s8, s9 = relabel(s6), relabel(s7), relabel(s8), relabel(s9)

    # Update the gluing map G
    glue_together!(G, (f0, 1), (f1, 1))
    glue_together!(G, (f0, 2), s7)
    glue_together!(G, (f0, 3), s8)
    glue_together!(G, (f1, 2), s9)
    glue_together!(G, (f1, 3), s6)

    # Update the edge lengths
    # Note that even the edges we didn't flip have been re-labeled, so we need to
    # update those too.
    l[f0,:] = [new_length, l3, l4]
    l[f1,:] = [new_length, l5, l2]

    (f0, 1)
end

include("WindingNumbers.jl")
include("DifferentialGeometry.jl")

using .DifferentialGeometry
DifferentialGeometry.Δ( topo::Topology, P ) = DifferentialGeometry.Δ(polygons(topo) |> Iterators.flatten |> collect, P)

"""
    intrinsic_Δ(F, l)

Laplace-Beltrami operator built from list of triangle indices as matrix and |F|x3 edge length matrix.
Positions are not used, so this is compatible with intrinsic triangulations.

"""
function intrinsic_Δ(F::Array{T,2}, l::Array{FT,2}) where {T<:Integer, FT<:Real}
  n = size(F)[1]*9
  indexI = Vector{Int}(undef,n)
  indexJ = Vector{Int}(undef,n)
  element = Vector{FT}(undef,n)
  s = 1
  @inbounds for (f, ijk) in enumerate(eachrow(F))
    i,j,k = ijk

    b = cot(opposite_corner_angle(l, (f, 1)))*0.5
    c = cot(opposite_corner_angle(l, (f, 2)))*0.5
    a = cot(opposite_corner_angle(l, (f, 3)))*0.5

    #L[i,i] += b+c
    indexI[s] = i; indexJ[s] = i; element[s] = b+c; s+=1
    #L[i,j] -= c
    indexI[s] = i; indexJ[s] = j; element[s] = -c; s+=1
    #L[i,k] -= b
    indexI[s] = i; indexJ[s] = k; element[s] = -b; s+=1
    #L[j,i] -= c
    indexI[s] = j; indexJ[s] = i; element[s] = -c; s+=1
    #L[j,j] += c+a
    indexI[s] = j; indexJ[s] = j; element[s] = c+a; s+=1
    #L[j,k] -= a
    indexI[s] = j; indexJ[s] = k; element[s] = -a; s+=1
    #L[k,i] -= b
    indexI[s] = k; indexJ[s] = i; element[s] = -b; s+=1
    #L[k,j] -= a
    indexI[s] = k; indexJ[s] = j; element[s] = -a; s+=1
    #L[k,k] += a+b
    indexI[s] = k; indexJ[s] = k; element[s] = a+b; s+=1
  end
  sparse(indexI, indexJ, element)
end

"""
    intrinsic_M(F, l)

Lumped mass matrix for intrinsic triangulations
"""

function intrinsic_M(F::Array{T,2}, l::Array{FT,2}) where {T<:Integer, FT<:Real}
  M = zeros(reduce(max, F; init=0))
  map(enumerate(eachrow(F))) do (f,ijk)
    a = area(l[f,:]...)/3.0
    M[ijk] += [a,a,a]
  end
  Diagonal(M)
end

"""
    edge_in_face_basis(l, fs)

compute an edge vector in a 2D basis for an intrinsic triangle
l is a |F|x3 array of edge lengths per triangle
fs is a (face, side) tuple.
origin at tail of side 1, which is vertex 3
"""
function edge_in_face_basis(l, (f,s))
  θ = opposite_corner_angle(l, (f, 2))
  local_vert_positions = [[0.0,0], [l[f,1], 0], [cos(θ)*l[f,3], sin(θ)*l[f,3]]]

  sa = s==1 ? 3 : s-1
  local_vert_positions[s]-local_vert_positions[sa]
end

#==
"""

"""
function edgecentric_basis(l, f)
  l₃₁, l₁₂, l₂₃ = l[f,:]
  sᵢ = sqrt( l₃₁^2+l₁₂^2-l₂₃^2)*√2
  sⱼ = sqrt(-l₃₁^2+l₁₂^2+l₂₃^2)*√2
  sₖ = sqrt( l₃₁^2-l₁₂^2+l₂₃^2)*√2
  Diagonal([sᵢ,sⱼ,sₖ])
end

function edgecentric_to_barycentric(l, f, v, s::Diagonal{Float64})
  v .* inv(s)
end
==#

end # module