struct WanMerweWeightingParameters <: AbstractWeightingParameters
    α::Float64
    β::Float64
    κ::Float64
end

function WanMerweWeightingParameters()
    WanMerweWeightingParameters(1e-3, 2, 0)
end

struct MeanSetWeightingParameters <: AbstractWeightingParameters
    ω₀::Float64
end

function MeanSetWeightingParameters()
    MeanSetWeightingParameters(1/3)
end

struct GaussSetWeightingParameters <: AbstractWeightingParameters
    κ::Float64
end

function GaussSetWeightingParameters()
    GaussSetWeightingParameters(3)
end

struct ScaledSetWeightingParameters <: AbstractWeightingParameters
    α::Float64
    β::Float64
    κ::Float64
end

function ScaledSetWeightingParameters()
    ScaledSetWeightingParameters(1e-3, 2.0, 1.0)
end

lambda(weight_params::WanMerweWeightingParameters, L) = weight_params.α^2 * (L + weight_params.κ) - L

function calc_mean_weights(weight_params::WanMerweWeightingParameters, num_states)
    λ = lambda(weight_params, num_states)
    weight_0 = λ / (num_states + λ)
    weight_i = 1 / (2 * (num_states + λ))
    weight_0, weight_i
end

function calc_cov_weights(weight_params::WanMerweWeightingParameters, num_states)
    weight_0, weight_i = calc_mean_weights(weight_params, num_states)
    weight_0 + 1 - weight_params.α^2 + weight_params.β, weight_i
end

function calc_cholesky_weight(weight_params::WanMerweWeightingParameters, num_states::Real)
    num_states + lambda(weight_params, num_states)
end

function calc_mean_weights(weight_params::ScaledSetWeightingParameters, num_states)
    weight_0 = (weight_params.α^2 * weight_params.κ - num_states) / (weight_params.α^2 * weight_params.κ)
    weight_i = 1 / (2 * weight_params.α^2 * weight_params.κ)
    weight_0, weight_i
end

function calc_cov_weights(weight_params::ScaledSetWeightingParameters, num_states)
    weight_0, weight_i = calc_mean_weights(weight_params, num_states)
    weight_0 + 1 - weight_params.α^2 + weight_params.β, weight_i
end

function calc_cholesky_weight(weight_params::ScaledSetWeightingParameters, num_states::Real)
    weight_params.α^2 * weight_params.κ
end

function calc_mean_weights(weight_params::MeanSetWeightingParameters, num_states)
    weight_params.ω₀, (1 - weight_params.ω₀) / (2 * num_states)
end

calc_cov_weights(weight_params::AbstractWeightingParameters, num_states) =
    calc_mean_weights(weight_params, num_states)

function calc_cholesky_weight(weight_params::MeanSetWeightingParameters, num_states::Real)
    num_states / (1 - weight_params.ω₀)
end

function calc_mean_weights(weight_params::GaussSetWeightingParameters, num_states)
    1 - num_states / weight_params.κ, 1 / (2 * weight_params.κ)
end

function calc_cholesky_weight(weight_params::GaussSetWeightingParameters, num_states::Real)
    weight_params.κ
end

struct SPTimeUpdate{X,P,O} <: AbstractTimeUpdate{X,P}
    state::X
    covariance::P
    χ::O
end

struct SPMeasurementUpdate{X,P,O,I,IC,K} <: AbstractMeasurementUpdate{X,P}
    state::X
    covariance::P
    𝓨::O
    innovation::I
    innovation_covariance::IC
    kalman_gain::K
end

abstract type AbstractSigmaPoints{T} <: AbstractMatrix{T} end

struct SigmaPoints{T, V <: AbstractVector{T}, L <: LowerTriangular{T}, W <: AbstractWeightingParameters} <: AbstractSigmaPoints{T}
    x0::V
    P_chol::L
    weight_params::W
    SigmaPoints{T, V, L, W}(x0, P_chol, weight_params) where {T<:Real, V<:AbstractVector{T}, L<:LowerTriangular{T}, W<:AbstractWeightingParameters} =
        size(x0, 1) == size(P_chol, 1) == size(P_chol, 2) ?
        new{T, V, L, W}(x0, P_chol, weight_params) :
        error("The length of the first dimension must be equal to the size of P_chol")
end

SigmaPoints(x0::V, P_chol::Cholesky{T}, weight_params::W) where {T<:Real, V<:AbstractVector{T}, W<:AbstractWeightingParameters} =
    SigmaPoints{T, V, typeof(P_chol.L), W}(x0, P_chol.L, weight_params)
SigmaPoints(x0::V, P_chol::L, weight_params::W) where {T<:Real, V<:AbstractVector{T}, L<:LowerTriangular{T}, W<:AbstractWeightingParameters} =
    SigmaPoints{T, V, L, W}(x0, P_chol, weight_params)

function calc_sigma_points(
    x::V,
    P::AbstractMatrix{T},
    weight_params::W
) where {T, V<:AbstractVector{T}, W<:AbstractWeightingParameters}
    weight = calc_cholesky_weight(weight_params, P)
    P_chol = cholesky(Hermitian(P .* weight, :L))
    SigmaPoints{T, V, typeof(P_chol.L), W}(x, P_chol.L, weight_params)
end

function calc_sigma_points(
    x::V,
    P::Cholesky{T},
    weight_params::W
) where {T, V<:AbstractVector{T}, W<:AbstractWeightingParameters}
    weight = calc_cholesky_weight(weight_params, P)
    weighted_P_chol = P.L * sqrt(weight)
    SigmaPoints{T, V, typeof(weighted_P_chol), W}(x, weighted_P_chol, weight_params)
end

function calc_sigma_points!(
    P_chol_temp::M,
    x::V,
    P::M,
    weight_params::W
) where {T, V<:AbstractVector{T}, M<:AbstractMatrix{T}, W<:AbstractWeightingParameters}
    weight = calc_cholesky_weight(weight_params, P)
    P_chol_temp .= P .* weight
    P_chol = cholesky!(Hermitian(P_chol_temp, :L))
    SigmaPoints{T, V, typeof(P_chol.L), W}(x, P_chol.L, weight_params)
end

function calc_sigma_points!(
    P_chol_temp::M,
    x::V,
    P::Cholesky{T},
    weight_params::W
) where {T, V<:AbstractVector{T}, M<:AbstractMatrix{T}, W<:AbstractWeightingParameters}
    weight = calc_cholesky_weight(weight_params, P)
    P_chol_temp .= (P.uplo === 'U' ? transpose(P.U) : P.L) .* sqrt(weight)
    SigmaPoints(x, LowerTriangular(P_chol_temp), weight_params)
end

struct TransformedSigmaPoints{T, V <: AbstractVector{T}, M <: AbstractMatrix{T}, W <: AbstractWeightingParameters} <: AbstractSigmaPoints{T}
    x0::V
    xi::M
    weight_params::W
    TransformedSigmaPoints{T, V, M, W}(x0, xi, weight_params) where {T<:Real, V<:AbstractVector{T}, M<:AbstractMatrix{T}, W<:AbstractWeightingParameters} =
        size(x0, 1) == size(xi, 1) ?
        new{T, V, M, W}(x0, xi, weight_params) :
        error("The length of the first dimension must be the same for all inputs")
end

TransformedSigmaPoints(x0::V, xi::M, weight_params::W) where {T<:Real, V<:AbstractVector{T}, M<:AbstractMatrix{T}, W<:AbstractWeightingParameters} =
    TransformedSigmaPoints{T, V, M, W}(x0, xi, weight_params)

function transform(F, χ::SigmaPoints{T}) where T
    𝓨_x0 = F(χ.x0)
    num_x = length(χ.x0)
    𝓨_xi = Matrix{T}(undef, length(𝓨_x0), 2 * length(χ.x0))
    xi_temp = copy(χ.x0)
    @inbounds for i = length(χ.x0):-1:1
        xi_temp[i:num_x] .= @view(χ.x0[i:num_x]) .+ @view(χ.P_chol.data[i:num_x, i])
        𝓨_xi[:, i] = F(xi_temp)
        xi_temp[i:num_x] .= @view(χ.x0[i:num_x]) .- @view(χ.P_chol.data[i:num_x, i])
        𝓨_xi[:, i + length(χ.x0)] = F(xi_temp)
    end
    TransformedSigmaPoints(𝓨_x0, 𝓨_xi, χ.weight_params)
end

function transform!(𝓨::TransformedSigmaPoints{T}, xi_temp, F!, χ::SigmaPoints{T}) where T
    F!(𝓨.x0, χ.x0)
    num_x = length(χ.x0)
    xi_temp .= χ.x0
    @inbounds for i = length(χ.x0):-1:1
        xi_temp[i:num_x] .= @view(χ.x0[i:num_x]) .+ @view(χ.P_chol.data[i:num_x, i])
        F!(@view(𝓨.xi[:, i]), xi_temp)
        xi_temp[i:num_x] .= @view(χ.x0[i:num_x]) .- @view(χ.P_chol.data[i:num_x, i])
        F!(@view(𝓨.xi[:, i + length(χ.x0)]), xi_temp)
    end
    TransformedSigmaPoints(𝓨.x0, 𝓨.xi, χ.weight_params)
end

function mean(𝓨::TransformedSigmaPoints)
    weight_0, weight_i = calc_mean_weights(𝓨)
    𝓨.x0 .* weight_0 .+ vec(sum(𝓨.xi, dims = 2)) .* weight_i
end

function mean!(y::Vector, 𝓨::TransformedSigmaPoints)
    weight_0, weight_i = calc_mean_weights(𝓨)
    sum!(y, 𝓨.xi)
    y .= y .* weight_i .+ 𝓨.x0 .* weight_0
end

function substract_mean(𝓨::TransformedSigmaPoints, y)
    𝓨 .- y
end

function substract_mean!(unbiased_𝓨::TransformedSigmaPoints, 𝓨::TransformedSigmaPoints, y)
    unbiased_𝓨 .= 𝓨 .- y
    TransformedSigmaPoints(unbiased_𝓨.x0, unbiased_𝓨.xi, 𝓨.weight_params)
end

function cov(unbiased_𝓨::TransformedSigmaPoints, Q::AbstractMatrix)
    weight_0, weight_i = calc_cov_weights(unbiased_𝓨)
    cov(unbiased_𝓨::TransformedSigmaPoints, Augment(Q)) .+ Q
end

function cov!(P, unbiased_𝓨::TransformedSigmaPoints, Q::AbstractMatrix)
    weight_0, weight_i = calc_cov_weights(unbiased_𝓨)
    P .= @~ unbiased_𝓨.x0 * unbiased_𝓨.x0' .* weight_0 .+ unbiased_𝓨.xi * unbiased_𝓨.xi' .* weight_i .+ Q
end

function cov(χ::SigmaPoints, unbiased_𝓨::TransformedSigmaPoints)
    weight_0, weight_i = calc_cov_weights(χ)
    num_states = length(χ.x0)
    (χ.P_chol * (@view(unbiased_𝓨.xi[:, 1:num_states]))' .-
        χ.P_chol * (@view(unbiased_𝓨.xi[:, num_states + 1:2 * num_states]))') .* weight_i
end

function cov!(P, χ::SigmaPoints, unbiased_𝓨::TransformedSigmaPoints)
    weight_0, weight_i = calc_cov_weights(χ)
    num_states = length(χ.x0)
    P .= @~ χ.P_chol * (@view(unbiased_𝓨.xi[:, 1:num_states]))'
    P .-= @~ χ.P_chol * (@view(unbiased_𝓨.xi[:, num_states + 1:2 * num_states]))'
    P .*= weight_i
end

function mean_and_cov(𝓨::TransformedSigmaPoints, Q)
    y = mean(𝓨)
    Ryy = cov(𝓨 .- y, Q)
    y, Ryy
end

function calc_mean_weights(χ::AbstractSigmaPoints)
    calc_mean_weights(χ.weight_params, (size(χ, 2) - 1) >> 1)
end

function calc_cov_weights(χ::AbstractSigmaPoints)
    calc_cov_weights(χ.weight_params, (size(χ, 2) - 1) >> 1)
end

function calc_cholesky_weight(weight_params::AbstractWeightingParameters, P)
    calc_cholesky_weight(weight_params, size(P, 2))
end

Base.size(S::SigmaPoints) = (length(S.x0), 2 * length(S.x0) + 1)

Base.getindex(S::SigmaPoints, inds::Vararg{Int,2}) =
    @inbounds if inds[2] == 1
        S.x0[inds[1]]
    elseif 1 < inds[2] <= length(S.x0) + 1
        S.x0[inds[1]] + S.P_chol[inds[1], inds[2] - 1]
    else
        S.x0[inds[1]] - S.P_chol[inds[1], inds[2] - length(S.x0) - 1]
    end

# make TransformedSigmaPoints broadcastable
Base.size(S::TransformedSigmaPoints) = (length(S.x0), size(S.xi, 2) + 1)

Base.getindex(S::TransformedSigmaPoints{T}, inds::Vararg{Int,2}) where {T} =
    @inbounds if inds[2] == 1
        S.x0[inds[1]]
    else
        S.xi[inds[1], inds[2] - 1]
    end

Base.setindex!(S::TransformedSigmaPoints{T}, val, inds::Vararg{Int,2}) where {T} =
    @inbounds if inds[2] == 1
        S.x0[inds[1]] = val
    else
        S.xi[inds[1], inds[2] - 1] = val
    end

Base.BroadcastStyle(::Type{<:TransformedSigmaPoints}) = Broadcast.ArrayStyle{TransformedSigmaPoints}()

Base.similar(bc::Broadcast.Broadcasted{Broadcast.ArrayStyle{TransformedSigmaPoints}}, ::Type{T}) where T =
    TransformedSigmaPoints(similar(bc.args[1].x0), similar(bc.args[1].xi), bc.args[1].weight_params)

function Base.copyto!(dest::TransformedSigmaPoints, bc::Broadcast.Broadcasted{Broadcast.ArrayStyle{TransformedSigmaPoints}})
    for idx in eachindex(bc)
        @inbounds dest[idx] = bc[idx]
    end
    dest
end


# Generate some simple random data just to make sure the
# models work; note that we don't check the validity of
# the models, we assume they work fine, we just check
# they get called properly and have appropriate metadata

Random.seed!(1551)
n, p = 100, 5
X  = randn(n, p)
X1 = hcat(X, ones(100))
θ  = randn(p+1)
y  = X1 * θ .+ 0.1 .* randn(n)

θ_ls = X1 \ y

@testset "ARD" begin
    m = ARDRegressor()
    f, _, _ = fit(m, 1, X, y)
    @test isapprox(f[1].score(X, y), 0.998859, rtol=1e-5)
    @test isapprox(norm(predict(m, f, X) .- y)/norm(y),  0.032693, rtol=1e-5)
    # testing that the fitted params is proper
    fp = fitted_params(m, f)
    @test keys(fp) == (:coef, :intercept, :alpha, :lambda, :sigma, :scores)
end

@testset "BayesianRidge" begin
    m = BayesianRidgeRegressor()
    f, _, _ = fit(m, 1, X, y)
    @test isapprox(f[1].score(X, y), 0.998859, rtol=1e-5)
    @test isapprox(norm(predict(m, f, X) .- y)/norm(y), 0.0326918, rtol=1e-5)
    # testing that the fitted params is proper
    fp = fitted_params(m, f)
    @test keys(fp) == (:coef, :intercept, :alpha, :lambda, :sigma, :scores)
end

@testset "ElasticNet" begin
    m = ElasticNetRegressor()
    f, _, _ = fit(m, 1, X, y)
    @test isapprox(f[1].score(X, y), 0.769795, rtol=1e-5)
    @test isapprox(norm(predict(m, f, X) .- y)/norm(y), 0.464447, rtol=1e-5)
    # testing that the fitted params is proper
    fp = fitted_params(m, f)
    @test keys(fp) == (:coef, :intercept)
end

@testset "ElasticNetCV" begin
    m = ElasticNetCVRegressor()
    f, _, _ = fit(m, 1, X, y)
    @test isapprox(f[1].score(X, y), 0.99884, rtol=1e-5)
    @test isapprox(norm(predict(m, f, X) .- y)/norm(y), 0.0328523, rtol=1e-5)
    # testing that the fitted params is proper
    fp = fitted_params(m, f)
    @test keys(fp) == (:coef, :intercept, :l1_ratio, :mse_path, :alphas)
end

@testset "Huber" begin
    m = HuberRegressor()
    f, _, _ = fit(m, 1, X, y)
    @test isapprox(f[1].score(X, y), 0.99884, rtol=1e-5)
    @test isapprox(norm(predict(m, f, X) .- y)/norm(y), 0.032935, rtol=1e-5)
    # testing that the fitted params is proper
    fp = fitted_params(m, f)
    @test keys(fp) == (:coef, :intercept, :scale, :outliers)
end

@testset "Lars" begin
    m = LarsRegressor()
    f, _, _ = fit(m, 1, X, y)
    @test isapprox(f[1].score(X, y), 0.998859, rtol=1e-5)
    @test isapprox(norm(predict(m, f, X) .- y)/norm(y), 0.0326918, rtol=1e-5)
    # testing that the fitted params is proper
    fp = fitted_params(m, f)
    @test keys(fp) == (:coef, :intercept, :alphas, :active, :coef_path)
end

@testset "LarsCV" begin
    m = LarsCVRegressor()
    f, _, _ = fit(m, 1, X, y)
    @test isapprox(f[1].score(X, y), 0.998859, rtol=1e-5)
    @test isapprox(norm(predict(m, f, X) .- y)/norm(y), 0.0326918, rtol=1e-5)
    # testing that the fitted params is proper
    fp = fitted_params(m, f)
    @test keys(fp) == (:coef, :intercept, :alpha, :alphas, :cv_alphas, :mse_path, :coef_path)
end

@testset "Lasso" begin
    m = LassoRegressor(alpha = 0.1)
    f, _, _ = fit(m, 1, X, y)
    @test isapprox(f[1].score(X, y), 0.9946343, rtol=1e-5)
    @test isapprox(norm(predict(m, f, X) .- y)/norm(y), 0.0709070, rtol=1e-5)
    # testing that the fitted params is proper
    fp = fitted_params(m, f)
    @test keys(fp) == (:coef, :intercept)
end

@testset "LassoCV" begin
    m = LassoCVRegressor(random_state=0, cv=6)
    f, _, _ = fit(m, 1, X, y)
    @test isapprox(f[1].score(X, y), 0.998857, rtol=1e-5)
    @test isapprox(norm(predict(m, f, X) .- y)/norm(y), 0.03272594, rtol=1e-5)
    # testing that the fitted params is proper
    fp = fitted_params(m, f)
    @test keys(fp) == (:coef, :intercept, :alpha, :alphas, :mse_path, :dual_gap)
end

@testset "LassoLars" begin
    m = LassoLarsRegressor(alpha = 0.01)
    f, _, _ = fit(m, 1, X, y)
    @test isapprox(f[1].score(X, y), 0.994317, rtol=1e-5)
    @test isapprox(norm(predict(m, f, X) .- y)/norm(y), 0.072973, rtol=1e-5)
    # testing that the fitted params is proper
    fp = fitted_params(m, f)
    @test keys(fp) == (:coef, :intercept, :alphas, :active, :coef_path)
end

@testset "LassoLarsCV" begin
    m = LassoLarsCVRegressor(cv=4)
    f, _, _ = fit(m, 1, X, y)
    @test isapprox(f[1].score(X, y), 0.998859, rtol=1e-5)
    @test isapprox(norm(predict(m, f, X) .- y)/norm(y), 0.0326918, rtol=1e-5)
    # testing that the fitted params is proper
    fp = fitted_params(m, f)
    @test keys(fp) == (:coef, :intercept, :coef_path, :alpha, :alphas, :cv_alphas, :mse_path)
end

@testset "LassoLarsIC" begin
    m = LassoLarsICRegressor()
    f, _, _ = fit(m, 1, X, y)
    @test isapprox(f[1].score(X, y), 0.9920703, rtol=1e-5)
    @test isapprox(norm(predict(m, f, X) .- y)/norm(y), 0.08619978, rtol=1e-5)
    # testing that the fitted params is proper
    fp = fitted_params(m, f)
    @test keys(fp) == (:coef, :intercept, :alpha)
end

@testset "LinReg" begin
    m = LinearRegressor()
    f, _, _ = fit(m, 1, X, y)
    # test if things work
    @test isapprox(f[1].score(X, y), 0.998859, rtol=1e-4)
    @test isapprox(norm(predict(m, f, X) .- y) / norm(y), 0.032691, rtol=1e-4)
    fp = fitted_params(m, f)
    @test fp.coef      ≈ θ_ls[1:p]
    @test fp.intercept ≈ θ_ls[end]
    # testing that the fitted params is proper
    fp = fitted_params(m, f)
    @test keys(fp) == (:coef, :intercept)
end

@testset "OMP" begin
    m = OrthogonalMatchingPursuitRegressor()
    f, _, _ = fit(m, 1, X, y)
    @test isapprox(f[1].score(X, y), 0.4593868, rtol=1e-5)
    @test isapprox(norm(predict(m, f, X) .- y)/norm(y), 0.711741, rtol=1e-5)
    # testing that the fitted params is proper
    fp = fitted_params(m, f)
    @test keys(fp) == (:coef, :intercept)
end

@testset "OMPCV" begin
    m = OrthogonalMatchingPursuitCVRegressor(cv = 5)
    f, _, _ = fit(m, 1, X, y)
    @test isapprox(f[1].score(X, y), 0.998859, rtol=1e-5)
    @test isapprox(norm(predict(m, f, X) .- y)/norm(y), 0.0326918, rtol=1e-5)
    # testing that the fitted params is proper
    fp = fitted_params(m, f)
    @test keys(fp) == (:coef, :intercept, :n_nonzero_coefs)
end

@testset "OMPCV" begin
    m = OrthogonalMatchingPursuitCVRegressor(cv = 5)
    f, _, _ = fit(m, 1, X, y)
    @test isapprox(f[1].score(X, y), 0.998859, rtol=1e-5)
    @test isapprox(norm(predict(m, f, X) .- y)/norm(y), 0.0326918, rtol=1e-5)
    # testing that the fitted params is proper
    fp = fitted_params(m, f)
    @test keys(fp) == (:coef, :intercept, :n_nonzero_coefs)
end

@testset "PassAggr" begin
    m = PassiveAggressiveRegressor()
    f, _, _ = fit(m, 1, X, y)
    @test 0.980 ≤ f[1].score(X, y) ≤ 0.999
    @test norm(predict(m, f, X) .- y)/norm(y) ≤ 0.1
    # testing that the fitted params is proper
    fp = fitted_params(m, f)
    @test keys(fp) == (:coef, :intercept)
end

@testset "RANSAC" begin
    m = RANSACRegressor()
    f, _, _ = fit(m, 1, X, y)
    @test 0.99 ≤ f[1].score(X, y) ≤ 0.999
    @test 0.02 ≤ norm(predict(m, f, X) .- y)/norm(y) ≤ 0.05
    # testing that the fitted params is proper
    fp = fitted_params(m, f)
    @test keys(fp) == (:estimator, :n_trials, :inlier_mask, :n_skips_no_inliers, :n_skips_invalid_data, :n_skips_invalid_model)
end

@testset "Ridge" begin
    m = RidgeRegressor(alpha = 1.0)
    f, _, _ = fit(m, 1, X, y)
    @test isapprox(f[1].score(X, y), 0.998793, rtol=1e-5)
    @test isapprox(norm(predict(m, f, X) .- y)/norm(y), 0.0336254, rtol=1e-5)
    # testing that the fitted params is proper
    fp = fitted_params(m, f)
    @test keys(fp) == (:coef, :intercept)
end

@testset "RidgeCV" begin
    m = RidgeCVRegressor(cv=nothing, store_cv_values=true)
    f, _, _ = fit(m, 1, X, y)
    @test isapprox(f[1].score(X, y), 0.998858, rtol=1e-5)
    @test isapprox(norm(predict(m, f, X) .- y)/norm(y), 0.0327014, rtol=1e-5)
    # testing that the fitted params is proper
    fp = fitted_params(m, f)
    @test keys(fp) == (:coef, :intercept, :alpha, :cv_values)
    @test fp[:cv_values] isa Matrix
end

@testset "SGDReg" begin
    m = SGDRegressor()
    f, _, _ = fit(m, 1, X, y)
    @test 0.980 ≤ f[1].score(X, y) ≤ 0.999
    @test norm(predict(m, f, X) .- y)/norm(y) ≤ 0.1
    # testing that the fitted params is proper
    fp = fitted_params(m, f)
    @test keys(fp) == (:coef, :intercept, :average_coef, :average_intercept)
end

@testset "TheilSen" begin
    m = TheilSenRegressor()
    f, _, _ = fit(m, 1, X, y)
    @test 0.980 ≤ f[1].score(X, y) ≤ 0.999
    @test norm(predict(m, f, X) .- y)/norm(y) ≤ 0.1
    # testing that the fitted params is proper
    fp = fitted_params(m, f)
    @test keys(fp) == (:coef, :intercept, :breakdown, :n_subpopulation)
end

##############
# MULTI TASK #
##############

y2 = (t1=y, t2=y)

@testset "MTLassoCV" begin
    m = MultiTaskLassoRegressor()
    f, _, _ = fit(m, 1, X, y2)
    @test f[1].coef_ isa Matrix
    # testing that the fitted params is proper
    fp = fitted_params(m, f)
    @test keys(fp) == (:coef, :intercept)
    pred = predict(m, f, X)
    @test pred isa Tables.MatrixTable
    @test Tables.schema(pred).names == (:t1, :t2)
end

@testset "MTLassoCV" begin
    m = MultiTaskLassoCVRegressor(cv = 5, random_state = 0)
    f, _, _ = fit(m, 1, X, y2)
    @test f[1].coef_ isa Matrix
    # testing that the fitted params is proper
    fp = fitted_params(m, f)
    @test keys(fp) == (:coef, :intercept, :alpha, :mse_path, :alphas)
    @test predict(m, f, X) isa Tables.MatrixTable
end

X = MLJBase.table([0 0; 1 1; 2 2])
y = MLJBase.table([0 0; 1 1; 2 2])

@testset "MTElNet" begin
    m = MultiTaskElasticNetRegressor(alpha = 0.1, l1_ratio = 0.5)
    f, _, _ = fit(m, 1, X, y)
    @test f[1].coef_ isa Matrix
    # testing that the fitted params is proper
    fp = fitted_params(m, f)
    @test keys(fp) == (:coef, :intercept)
    @test predict(m, f, X) isa Tables.MatrixTable
end

@testset "MTElNetCV" begin
    m = MultiTaskElasticNetCVRegressor(cv = 3, n_alphas = 100, l1_ratio = 0.5)
    f, _, _ = fit(m, 1, X, y)
    @test f[1].coef_ isa Matrix
    # testing that the fitted params is proper
    fp = fitted_params(m, f)
    @test keys(fp) == (:coef, :intercept, :alpha, :mse_path, :l1_ratio)
    @test predict(m, f, X) isa Tables.MatrixTable
end


using DReal
using Base.Test
reset_ctx!()

A = Var(Bool,"aa")
B = Var(Bool,"bb")
C = Var(Bool,"cc")

formula = add!((A & B) | C)
@show is_satisfiable()
@show a, b, c = model(A, B, C)
# @test (a & b) | c


"""
    HullSearch(tolerance::Float64)

HullSearch, SGSV Combine HullReach, performs over-approximated reachability analysis to compute the over-approximated output reachable set for a network.

# Problem requirement
1. Network: any depth, any activation that is monotone
2. Input: `HPolytope`
3. Output: `HPolytope`

# Return
`BasicResult`

# Method
Search and Reachability

# Property
Sound but not complete.
"""
@with_kw struct HullTree
    tolerance::Float64 = 1.0
end

# This is the main function
function solve(solver::HullTree, problem::Problem)
    input = problem.input
    stack = Vector{Hyperrectangle}(undef, 0)
    push!(stack, input)
    #count = 1
    while !isempty(stack)
        interval = popfirst!(stack)
        reach = forward_network(solver, problem.network, interval)
        if issubset(reach, problem.output)
            continue
        else
            if get_largest_width(interval) > solver.tolerance
                sections = bisect(interval)
                for i in 1:2
                    if isborder(sections[i], problem.input)
                        push!(stack, sections[i])
                        #count += 1
                    end
                end
            else
                return BasicResult(:violated)
            end
        end
    end
    #print("\n$(count)\n")
    return BasicResult(:holds)
end

#to determine whether x has intersection with any border of y
function isborder(x::Hyperrectangle, y::Hyperrectangle)
    x_lower, x_upper = low(x), high(x)
    y_lower, y_upper = low(y), high(y)
    for i in 1:lastindex(x_lower)
        if x_lower[i] == y_lower[i] || x_upper[i] == y_upper[i]
            return true
        end
    end
    return false
end

function forward_layer(solver::HullTree, L::Layer, input::Hyperrectangle)
    (W, b, act) = (L.weights, L.bias, L.activation)
    center = zeros(size(W, 1))
    gamma  = zeros(size(W, 1))
    for j in 1:size(W, 1)
        node = Node(W[j,:], b[j], act)
        center[j], gamma[j] = forward_node(solver, node, input)
    end
    return Hyperrectangle(center, gamma)
end

function forward_node(solver::HullTree, node::Node, input::Hyperrectangle)
    output    = node.w' * input.center + node.b
    deviation = sum(abs.(node.w) .* input.radius)
    βmax = node.act(output + deviation)
    βmin = node.act(output - deviation)
    return ((βmax + βmin)/2, (βmax - βmin)/2)
end


module NNSparseCoding
using Random,Distributions
using LinearAlgebra
using Images, FileIO
using MultivariateStats

struct NNSC_optimizer{T}
  X::Matrix{T}
  AS::Matrix{T}
  A::Matrix{T}
  S::Matrix{T}
  lambda::T
end

function NNSC_optimizer(X::Matrix{<:Real},d::Integer,
    lambda::Real)
  nfull,nsampl=size(X)
  A,S = initialize_mats(X,d)
  project!(A)
  NNSC_optimizer(X,A*S,A,S,lambda)
end
function nnsc_objective(o::NNSC_optimizer)
  return 0.5 * mapreduce( (x,x̃) -> (x-x̃)^2,
    +,o.AS,o.X) + o.lambda * sum(o.S)
end

function nnsc_objective(X::M,A::M,S::M,lambda::Real) where M <: Matrix{<:Real}
  X̃ = A*S
  @. X̃ = (X̃ - X)^2
  return 0.5*sum(X̃) + lambda * sum(S)
end

function project!(A::Matrix{<:Real})
  # no negative values
  for i in eachindex(A)
    @inbounds begin
      if A[i] < 0.0
        A[i] = 0.0
  end end end
  # normalize columns
  for c in 1:size(A,2)
    @inbounds A[:,c] ./= norm(A[:,c])
  end
  return nothing
end
function projectA!(o::NNSC_optimizer)
  project!(o.A)
  return nothing
end

"""
    initialize_mats(n,b,k)
`n` : number of dimensions
`b` : number of basis vectors
`k` : number of samples
"""
function initialize_mats(n::I,b::I,k::I) where I<:Integer
  A = rand(distr_initA,(n,b))
  project!(A)
  B = rand(distr_initB,(b,k))
  return A,B
end
const distr_initA = Exponential(1.0)
const distr_initB = Exponential(1.0)

function initialize_mats(X,d)
  (nfull,ndat) = size(X)
  Xv = X[:]
  A = sqrt.(sample(Xv,(nfull,d))./ d)
  B = sqrt.(sample(Xv,(d,ndat)) ./ d)
  return A,B
end

function updateS!(S::M,A::M,X::M,lambda::Real) where M<:Matrix{<:Real}
  return S .*= (A' * X) ./ (A' * A * S .+ lambda)
end
function updateS!(o::NNSC_optimizer)
  o.S .*= o.A'*o.X
  o.S ./= (o.A'*o.AS .+ o.lambda)
  updateAS!(o)
  return nothing
end
@inline function updateAS!(o::NNSC_optimizer)
  mul!(o.AS,o.A,o.S)
  return nothing
end

function updateA!(A::M,S::M,X::M,lambda::Real,
    stepsize::Real,
    obj_pre::Union{Nothing,<:Real}=nothing) where M<:Matrix{<:Real}
  obj_pre = something(obj_pre, nnsc_objective(X,A,S,lambda))
  Apost = A .- stepsize .* ((A*S-X)*S')
  project!(Apost)
  obj_post = nnsc_objective(X,Apost,S,lambda)
  if (obj_post < obj_pre || stepsize < 1E-9)
    copy!(A,Apost)
    return stepsize
  else
    return updateA!(A,S,X,lambda,0.5*stepsize,obj_pre)
  end
end

function updateA!(o::NNSC_optimizer,stepsize::Real,obj_pre::Real)
  Apre = copy(o.A)
  ASpre = copy(o.AS)
  o.A .-= stepsize .* ((o.AS-o.X)*o.S')
  projectA!(o)
  updateAS!(o)
  obj_post = nnsc_objective(o)
  delta_obj = obj_pre - obj_post
  if (delta_obj > 0.0 || stepsize < 1E-9)
    return (stepsize,obj_post,delta_obj)
  else
    copy!(o.A,Apre)
    copy!(o.AS,ASpre)
    return updateA!(o,0.5*stepsize,obj_pre)
  end
end

function nnsc_optimize(X::Matrix{<:Real},d::Integer,
  lambda::Real ; step_zero::Real=0.05, nstepmax::Integer=10_000,
    opt_threshold::Real=1E-4,verbose::Bool=false)
  o = NNSC_optimizer(X,d,lambda)
  stepsize=step_zero
  obj_temp = nnsc_objective(o)
  delta_obj = Inf
  nsteps=0
  while ( (nsteps < nstepmax) && (delta_obj > opt_threshold) )
    nsteps +=1
    (stepsize,obj_temp,delta_obj) = updateA!(o, stepsize,obj_temp)
    updateS!(o)
    if verbose && (nsteps == nstepmax)
      @info """NNSC completed:
      threshold NOT reached: $opt_threshold
      after : $nsteps (max) iterations
      objective : $obj_temp
      """
    elseif verbose && (delta_obj <= opt_threshold)
      @info """NNSC completed:
      threshold reached: $opt_threshold
      after : $nsteps iterations
      objective : $obj_temp
      """
    end
  end
  o.S[o.S .< 1E-7] .= 0.0
  return o
end

#################
# What follows is manipulaton of image patches and files

# regularization types
abstract type ReguType end
# thou shall not broadcast!
Base.Broadcast.broadcastable(reg::ReguType)=Ref(reg)

struct NoRegu <: ReguType end
struct StandardRegu <: ReguType end

function regularize_image(img,rt::Type{NoRegu})::Matrix{Float64}
    return convert(Array{Float64},Gray.(img))
end
function regularize_image(img,rt::Type{StandardRegu})::Matrix{Float64}
    im=regularize_image(img,NoRegu)
    _mu,_std = mean(im),std(im)
    @. im = (im -_mu)/_std
    return im
end

"""

"""
function read_natural_images(
    dir,
    sz,
    (regu::Type{<:ReguType}) = StandardRegu;
    rotate::Bool = true,
    verbose::Bool = false,
    format::String = ".jpg")::Vector{Matrix{Float64}}
  @assert isdir(dir) "directory $dir not found!"
  filenames = filter(f-> occursin(Regex("$format\\b"),f),  readdir(dir))
  filenames = joinpath.(dir,filenames)
  good_size(img, sz) = min(size(img)...) >= sz
  @assert !isempty(filenames) "no files with extension $format found in $dir"
  ret = Matrix{Any}[]
  _rot_fun = rotate ? rotation_functions : [identity]
  nfiles = length(filenames)
  for (k, file) in enumerate(filenames)
    verbose && println("reading file $k/$(nfiles)")
    img = load(file)
    if good_size(img, sz)
      for rot in _rot_fun
        push!(ret, rot(img))
      end
    else
      @warn "image $file has size smaller than $sz , ignored"
    end
  end
  return cut_patch.(regularize_image.(ret,regu),Ref(sz))
end

# rotate images exactly
function rot_90(img)
    r,c=size(img)
    out=similar(img,c,r)
    for cc in 1:c
        out[cc,:] = img[end:-1:1,cc]
    end
    out
end
function rot_m90(img)
    r,c=size(img)
    out=similar(img,c,r)
    for (cc,mcc) in zip(1:c,c:-1:1)
        out[cc,:] = img[:,mcc]
    end
    out
end
function rot_180(img)
    r,c=size(img)
    out=similar(img)
    for (cc,mcc) in zip(1:c,c:-1:1)
        out[:,cc] = img[r:-1:1,mcc]
    end
    out
end
const rotation_functions = [identity , rot_90, rot_m90, rot_180 ]

"""
        function cut_patch(img, sz)

cuts a random square section of size `sz` from the matrix `img`
"""
function cut_patch(img::AbstractArray{T}, sz) where T
  _rows, _cols = size(img)
  _crows,_ccols =_rows-sz+1, _cols-sz+1
  i,j= rand(1:_crows), rand(1:_ccols)
  return img[i:i+sz-1,j:j+sz-1]
end

"""
    sampling_tiles(n_samples::Integer, images::Vector{Matrix{Float64}},
        patch_size::Integer)
Generates `n_samples` patches of size `patch_size`,
"""
function get_random_patches(n_samples::Integer, images::Vector{Matrix{Float64}},
    patch_size::Integer)
  n_img=length(images)
  @assert !isempty(images) "images are missing!"
  #check how many patches per image
  n_full,n_rem = divrem(n_samples,n_img)
  n_imgsampl = fill(n_full,n_img)
  # add the rem, picking them randomly!
  if n_rem>0
    idx_rem=sample(1:n_img , n_rem ;replace=false)
    n_imgsampl[idx_rem] .+= 1
  end
  @assert  sum(n_imgsampl) == n_samples "wrong selection?"
  ret = Vector{Matrix{Float64}}(undef,n_samples)
  k=0
  for (im,ns) in zip(images,n_imgsampl) , s in 1:ns
    k+=1
    ret[k]=cut_patch(im,patch_size)
  end
  return ret
end

function regularize01!(mat)
    l,u = extrema(mat)
    @. mat = (mat-l)/(u-l)
end

function merge_mats(mats::Vector{Matrix{R}};
   zeromean::Bool=true) where R<:Real
  sz = size(mats[1],1)
  nmats = length(mats)
  ret = Matrix{R}(undef,sz*sz,nmats)
  for (k,mat) in enumerate(mats)
    ret[:,k] .= mat[:]
  end
  if zeromean
    broadcast!(-,ret,ret,mean(ret;dims=2))
  end
  return ret
end

# pretty much wrappers to MultivariateStats

function plus_minus_parts(M::Array{T}) where T<:Real
  plus,minus = zero(M),zero(M)
  for i in eachindex(M)
    @inbounds begin
    if M[i] > 0
      plus[i] = M[i]
    else
      minus[i] = -M[i]
  end end end
  return plus,minus
end

abstract type Preprocess end

struct PreprocessWhiten <: Preprocess end
function preprocess_plus_minus(X,::Type{PreprocessWhiten})
  M = fit(Whitening,X)
  @assert all(x-> isapprox(x,0.0 ; atol=1E-6),mean(M)) "The input should have zero mean!"
  return (M, plus_minus_parts(transform(M,X))...)
end
function revert_plus_minus(Mp,Mm,W::Whitening)
  return  W.W' \ (Mp - Mm)
end

struct PreprocessPCA <: Preprocess
  d::Integer
end

function preprocess_plus_minus(X,p::PreprocessPCA)
  M = fit(PCA,X;maxoutdim=p.d)
  @assert all(x-> isapprox(x,0.0 ; atol=1E-6),mean(M)) "The input should have zero mean!"
  return (M, plus_minus_parts(transform(M,X))...)
end
function revert_plus_minus(Mp,Mm,p::PCA)
  return reconstruct(p, Mp - Mm)
end

struct PreprocessNot <: Preprocess end
function preprocess_plus_minus(X,::Type{PreprocessNot})
  @assert all(x-> isapprox(x,0.0 ; atol=1E-6),mean(X;dims=2)) "The input should have zero mean!"
  return (PreprocessNot, plus_minus_parts(X)...)
end
function revert_plus_minus(Mp,Mm,::Type{PreprocessNot})
  return (Mp - Mm)
end

end #module


if nworkers()<=1
	addprocs(2);
end

using  jInv.Mesh
using  jInv.Utils
using  jInv.InverseSolve
using  EikonalInv
using  MAT

#############################################################################################################
modelDir = "../examples";

dataDir = pwd();
include("../drivers/prepareTravelTimeDataFiles.jl");
include("../drivers/readModelAndGenerateMeshMref.jl");
include("../drivers/setupTravelTimeTomography.jl");

########################################################################################################
######################################## for SEG 256X512 ###############################################
#######################################################################################################
dim     = 2;
pad     = 1;
jump    = 32;
offset  = 256;
(m,Minv,mref,boundsHigh,boundsLow) = readModelAndGenerateMeshMref(modelDir,"SEGmodel2Dsalt.dat",dim,pad,[0.0,13.5,0.0,4.2],[64,32]);
dataFilenamePrefix = string(dataDir,"/DATA_SEG",tuple((Minv.n+1)...));

prepareTravelTimeDataFiles(m,Minv,mref,boundsHigh,boundsLow,dataFilenamePrefix,pad,jump,offset);
resultsFilenamePrefix = "";
(Q,P,pMisRFs,SourcesSubInd,contDiv,Iact,sback,mref,boundsHigh,boundsLow,resultsFilename) = setupTravelTimeTomography(m,dataFilenamePrefix, resultsFilenamePrefix);

########################################################################################################
##### Set up Inversion #################################################################################
########################################################################################################
maxStep=0.1*maximum(boundsHigh);
a = minimum(boundsLow);
b = maximum(boundsHigh);
function modfun(m)
	bm,dbm = getBoundModel(m,a,b);
	bs,dbs = velocityToSlowSquared(bm);
	dsdm = dbm*dbs;
	return bs,dsdm;
end
mref = getBoundModelInv(mref,a,b);
boundsHigh = boundsHigh*10000000.0;
boundsLow = -boundsLow*100000000.0

cgit  = 2;
maxit = 1;
alpha = 1e+0;
pcgTol = 1e-1;

HesPrec=getSSORCGRegularizationPreconditioner(1.0,1e-5,2)

################################################# GIT VERSION OF JINV #################################################

regparams = [1.0,1.0,1.0,1e-2];
regfun(m, mref, M) = wdiffusionRegNodal(m, mref, M, Iact=Iact, C = regparams);

pInv = getInverseParam(Minv,modfun,regfun,alpha,mref[:],boundsLow,boundsHigh,
                     maxStep=maxStep,pcgMaxIter=cgit,pcgTol=pcgTol,
					 minUpdate=1e-3, maxIter = maxit,HesPrec=HesPrec);

mc,Dc,flag = projGNCG(mref[:],pInv,pMisRFs,indCredit = []);

clear!(pMisRFs);
#############################################################################################
rm("DATA_SEG(66, 33)_travelTime.dat");
rm("DATA_SEG(66, 33)_rcvMap.dat");
rm("DATA_SEG(66, 33)_srcMap.dat");
rm("DATA_SEG(66, 33)_PARAM.mat");
rm("jInv.out");


module FlowPattern

using Blink
using Plots
using Interact

const default_exec_file = joinpath(@__DIR__, "..\\external\\Flowpat.exe")

function calculate_flow_pattern(;
    USᴳ = 0.070, # [m/s]
    USᴸ = 51.70, # [m/s]
    μᴳ = 0.000018, # [Pa.s]
    μᴸ = 0.005, # [Pa.s]
    ρᴳ = 2.8833234073128, # [kg/m³]
    ρᴸ = 997.950268197708, # [kg/m³]
    D = 0.0508, # [m]
    ε = 0, # [m]
    σ = 7.40500030517578E-02, # [N/m]
    θ = 0, # [°]
    exec_file = default_exec_file,
)
    flowpat = `$exec_file`
    input = open("Input.txt", "w")
    flowpat_mode = 2
    println(input, "$flowpat_mode\tMode 1-Transition Lines, 2-FlowPattern Prediction")
    println(input, "$USᴸ\t$USᴳ\t$ρᴸ\t$μᴸ\t$ρᴳ\t$μᴳ\t$σ\t$D\t$θ\t$ε")
    close(input)
    run(flowpat)
    flow_pattern_id, error_code = parse.(Int, split(read("Output.txt", String))[end-1:end])

    if error_code > 0
        return nothing
    end

    flowpattern = ("SS", "SW", "A", "DB", "B", "SL")
    return flowpattern[flow_pattern_id]
end

function calculate_flow_pattern_transitions(
    μᴳ,
    μᴸ,
    ρᴳ,
    ρᴸ,
    D,
    ε,
    σ,
    θ,
    interface;
    exec_file = default_exec_file,
)
    n_transitions = 5
    flowpat = `$exec_file`
    input = open("Input.txt", "w")
    flowpat_mode = 1
    println(input, "$flowpat_mode\tMode 1-Transition Lines, 2-FlowPattern Prediction")
    println(input, "$ρᴸ\tLiquid Density [Kg/m^3]")
    println(input, "$μᴸ\tLiquid Viscosity [Pa s]")
    println(input, "$σ\tSurface Tension [N/m]")
    println(input, "$ρᴳ\tGas Density [Kg/m^3]")
    println(input, "$μᴳ\tGas Viscosity [Pa s]")
    println(input, "$D\tPipe Diameter [m]")
    println(input, "$θ\tPipe inclination  Angle [deg]")
    println(input, "$ε\tPipe Roughness [m]")
    println(input, "$interface\tInterface 1-Smooth, 2-Wavy")
    close(input)
    run(flowpat)

    function init_xy(size)
        x = Vector{Vector{Float64}}()
        y = Vector{Vector{Float64}}()
        for i = 1:size
            push!(x, Vector{Float64}())
            push!(y, Vector{Float64}())
        end
        return x, y
    end

    x, y = init_xy(n_transitions)
    function push_if_available_sync!(arr_x, arr_y, val_x, val_y)
        if val_x > -999 && val_y > -999
            push!(arr_x, val_x)
            push!(arr_y, val_y)
        end
    end
    open("Output.txt") do file
        for (i, ln) in enumerate(eachline(file))
            result = parse.(Float64, split(ln))'
            for j = 1:n_transitions
                push_if_available_sync!(x[j], y[j], result[2j-1], result[2j])
            end
        end
    end

    return x, y
end

function plot_flow_pattern_transitions(θ, D, ρᴳ, ρᴸ, μᴳ, μᴸ, σ, ε, interface)
    x, y = calculate_flow_pattern_transitions(μᴳ, μᴸ, ρᴳ, ρᴸ, D, ε, σ, θ, interface)
    p = plot(
        x,
        y,
        xlabel = "USᴳ [m/s]",
        ylabel = "USᴸ [m/s]",
        xlims = (1e-3, 100),
        ylims = (1e-3, 100),
        xaxis = :log,
        yaxis = :log,
        label = ["Stratified" "Annular" "Wavy" "Dispersed-Bubble" "Bubble"],
        legend = :outertopright,
        grid = :all,
        minorgrid = true,
        formatter = string, # A method which converts a number to a string for tick labeling.
    )
end

function flowpattern_main()
    ##
    # Create sliders and plot
    θ_slider = widget(0:0.5:90, label = "θ [°]"; value = 0.0)
    D_slider = widget(0.01:0.01:1, label = "D [m]"; value = 0.1)
    ρᴳ_slider = widget(0.5:0.1:1100, label = "ρᴳ [kg/m³]"; value = 1.0)
    ρᴸ_slider = widget(0.5:0.1:1100, label = "ρᴸ [kg/m³]"; value = 1000.0)
    μᴳ_slider = widget(1e-5:1e-5:1e-1, label = "μᴳ [Pa.s]"; value = 1e-5)
    μᴸ_slider = widget(1e-5:1e-5:1e-1, label = "μᴸ [Pa.s]"; value = 1e-3)
    σ_slider = widget(1e-3:1e-3:1, label = "σ [N/m]"; value = 1e-2)
    ε_slider = widget(0:1e-5:1e-2, label = "ε [m]"; value = 0)
    interface_selector = widget(Dict("Stratified" => 1, "Wavy" => 2), label = "Interface")

    interactive_plot = map(
        plot_flow_pattern_transitions,
        θ_slider,
        D_slider,
        ρᴳ_slider,
        ρᴸ_slider,
        μᴳ_slider,
        μᴸ_slider,
        σ_slider,
        ε_slider,
        interface_selector,
    )

    ## Create UI layout
    el1 = hbox(
        vbox(θ_slider, ρᴳ_slider, μᴳ_slider, ε_slider),
        vbox(D_slider, ρᴸ_slider, μᴸ_slider, σ_slider),
    )
    el2 = interface_selector
    el3 = Interact.hline()
    el4 = interactive_plot

    ui = vbox(el1, el2, el3, el4)

    ## Create final window
    w = Window()
    size(w, 700, 700)
    title(w, "Flow Pattern Map Calculator")
    body!(w, ui)

    if "--quiet" in ARGS
        close(w)
    else
        # Workaround for keeping Blink window alive when calling `julia <my_script>.jl`
        while active(w)
            sleep(1)
        end
    end
end

function julia_main()
    try
        flowpattern_main()
    catch
        Base.invokelatest(Base.display_error, Base.catch_stack())
        return 1
    end
    return 0
end

end # module FlowPattern


@testset "Closeness" begin
    g5 = DiGraph(4)
    add_edge!(g5,1,2); add_edge!(g5,2,3); add_edge!(g5,1,3); add_edge!(g5,3,4)
    for g in testdigraphs(g5)
      y = @inferred(closeness_centrality(g; normalize=false))
      z = @inferred(closeness_centrality(g))
      @test y == [0.75, 0.6666666666666666, 1.0, 0.0]
      @test z == [0.75, 0.4444444444444444, 0.3333333333333333, 0.0]
    end

    g5 = Graph(5)
    add_edge!(g5,1,2)
    for g in testgraphs(g5)
      z = @inferred(closeness_centrality(g))
      @test z[1] == z[2] == 0.25
      @test z[3] == z[4] == z[5] == 0.0
    end
end


using PanelMatrices
using Test
using StaticArrays
using StaticNumbers
using LinearAlgebra

@testset "set/get" begin
    y = reshape(collect(1:100), 10, 10)
    x = PanelMatrix(y)
    @test x[1,1] == y[1,1]
    @test all(x .== y)

    v = PanelMatrices.panel_view(x, 1, 1)
    @test all(v .== [1 11 21 31; 2 12 22 32; 3 13 23 33; 4 14 24 34])
    @test all(PanelMatrices.panel_view(x, 3, 1) .== [9 19 29 39; 10 20 30 40])
    @test all(PanelMatrices.get_panel(x, 3, 1) .== [9 19 29 39; 10 20 30 40])
    @test all(PanelMatrices.panel_view(x, 1, 3) .== [81 91; 82 92; 83 93; 84 94])
    @test all(PanelMatrices.get_panel(x, 1, 3) .== [81 91; 82 92; 83 93; 84 94])
    @test all(PanelMatrices.panel_view(x, 3, 3) .== [89 99; 90 100])
    @test all(PanelMatrices.get_panel(x, 3, 3) .== [89 99; 90 100])
    @test size(PanelMatrices.full_panel_view(x, 3, 3)) == (4, 4)

    p = PanelMatrices.get_full_panel(x, 1, 1)
    @test p === @SMatrix [1 11 21 31; 2 12 22 32; 3 13 23 33; 4 14 24 34]

    z = reshape(101:116, 4, 4)
    PanelMatrices.set_full_panel!(x, z, 2, 2)
    @test x[5,5] == z[1,1]
    @test all([x[i,j] for i ∈ 5:8, j ∈ 5:8] .== z)

    # TODO: Write tests that ensure that there is no allocation in critical routines.
end

@testset "matmul" begin
    for M = (5,8), N = (6,8), K = (7,8)
        A0 = reshape(collect(1.0:M*K), M, K)
        B0 = reshape(collect(1.0:K*N), K, N)
        C0 = A0*B0

        A = PanelMatrix(A0, static.((4,4)))
        B = PanelMatrix(B0, static.((4,4)))
        C = PanelMatrix{Float64}(undef, (M,N), static.((4,4)))

        mul!(C, A, B)
        @test C ≈ C0
    end
end


using LightGraphs, SimpleWeightedGraphs, DataStructures

function limited_dijkstra_shortest_paths(g::AbstractGraph,
    srcs::Vector{U},
    distmx::AbstractMatrix{T}=weights(g);
    allpaths=false,
    trackvertices=false,
    limit::T=zero(T)
    ) where T <: Real where U <: Integer

    nvg = nv(g)
    dists = fill(typemax(T), nvg)
    parents = zeros(U, nvg)
    visited = zeros(Bool, nvg)

    pathcounts = zeros(UInt64, nvg)
    preds = fill(Vector{U}(), nvg)
    H = PriorityQueue{U,T}()
    # fill creates only one array.

    dolimit = (limit > zero(T))

    for src in srcs
        dists[src] = zero(T)
        visited[src] = true
        pathcounts[src] = 1
        H[src] = zero(T)
    end

    closest_vertices = Vector{U}()  # Maintains vertices in order of distances from source
    sizehint!(closest_vertices, nvg)

    while !isempty(H)
        u = dequeue!(H)

        if trackvertices
            push!(closest_vertices, u)
        end

        d = dists[u] # Cannot be typemax if `u` is in the queue

        if dolimit
            if d > limit
                break
            end
        end

        for v in outneighbors(g, u)
            alt = d + distmx[u, v]

            if !visited[v]
                visited[v] = true
                dists[v] = alt
                parents[v] = u

                pathcounts[v] += pathcounts[u]
                if allpaths
                    preds[v] = [u;]
                end
                H[v] = alt
            elseif alt < dists[v]
                dists[v] = alt
                parents[v] = u
                #615
                pathcounts[v] = pathcounts[u]
                if allpaths
                    resize!(preds[v], 1)
                    preds[v][1] = u
                end
                H[v] = alt
            elseif alt == dists[v]
                pathcounts[v] += pathcounts[u]
                if allpaths
                    push!(preds[v], u)
                end
            end
        end
    end

    if trackvertices
        for s in vertices(g)
            if !visited[s]
                push!(closest_vertices, s)
            end
        end
    end

    for src in srcs
        pathcounts[src] = 1
        parents[src] = 0
        empty!(preds[src])
    end

    dijkstrastate = LightGraphs.DijkstraState{T,U}(parents, dists, preds, pathcounts, closest_vertices)
    trim_dijkstra_state!(dijkstrastate, limit, skipinf=true)
    return dijkstrastate
end

# single vertice
limited_dijkstra_shortest_paths(g::AbstractGraph, src::Integer, distmx::AbstractMatrix=weights(g); allpaths=false, trackvertices=false, limit=0.) =
limited_dijkstra_shortest_paths(g, [src;], distmx; allpaths=allpaths, trackvertices=trackvertices, limit=limit)

function trim_dijkstra_state(dijkstra_state::LightGraphs.DijkstraState{T,U}, limit::T=zero(T); skipinf=true) where T <: Real where U <: Integer
    s = LightGraphs.DijkstraState(
        deepcopy(dijkstra_state.parents),
        deepcopy(dijkstra_state.dists),
        deepcopy(dijkstra_state.predecessors), # no need for deep copy because we are not modifying content of predecessors itself
        deepcopy(dijkstra_state.pathcounts),
        deepcopy(dijkstra_state.closest_vertices)
    )
    return trim_dijkstra_state!(s, limit; skipinf=true)
end

function trim_dijkstra_state!(dijkstra_state::LightGraphs.DijkstraState{T,U}, limit::T=zero(T); skipinf=true) where T <: Real where U <: Integer
    s = dijkstra_state
    dolimit = (limit > zero(T))

    if (dolimit)
        for i in 1:length(s.dists)
            d = s.dists[i]
            if d > limit && (d < typemax(T) || !skipinf)
                s.parents[i] = zero(U)
                s.dists[i] = typemax(T)
                s.predecessors[i] = Vector{U}()
                s.pathcounts[i] = zero(U)
            end
        end
    end
    return s
end


"""
   randommask(n, Bernoulli(p)[ symmetric=true])

Generate a random Bernoulli mask of n × n.
"""
function randommask(n::Int, distribution::Bernoulli{T}; symmetric=true) where T
    n ≥ 2 || throw(DomainError(n, "n < 2"))

    if symmetric
        # Generates a random lower triangular matrix and then symmetrizes it
        return Symmetric([i[1] > i[2] ? convert(Int, rand(distribution)) : 1 for i in CartesianIndices((1:n,1:n))], :L)
    else
        # Generates random entries for all entries
        return [i[1] != i[2] ? convert(Int, rand(distribution)) : 1 for i in CartesianIndices((1:n,1:n))]
    end
end

"""
   randommask(D, Bernoulli(p)[ symmetric=true])

Generate a random Bernoulli mask of size(D, 1).
"""
function randommask(D::EDM, distribution::Bernoulli{T}; symmetric=true) where T
    return randommask(size(D, 1), distribution, symmetric=symmetric)
end

"""
    randommask(n::Int, number_of_deletions::Int)

Randomly and uniformly create a mask with 2 × "number_of_deletions" number of deletions. The
2 × multiplier is due to the deletions done only on the entries below the diagonal,
and then symmetrized.
"""
function randommask(n::Int, number_of_deletions::Int)
    n ≥ 2 || throw(DomainError(n, "n < 2"))
    number_of_deletions > 0 || throw(DomainError(number_of_deletions, "deletions < 1"))

    # We create a lower diagonal matrix from which we can sample CartesianIndices
    # uniformly at random without replacement using sample from StatsBase
    mask = UnitLowerTriangular(ones(Int, n, n)) - Matrix(I, n, n)
    indices = sample(findall(x -> x == 1, mask), number_of_deletions, replace=false)

    # We set the values of indices to zero, and symmetrize
    mask[indices] .= 0
    mask = mask + Matrix(I, n, n)

    return Symmetric(mask, :L)
end

"""
    randommask(D::EDM, number_of_deletions::Int)

Randomly and uniformly create a mask with 2 × "number_of_deletions" number of deletions for D.
The 2 × multiplier is due to the deletions done only on the entries below the diagonal,
and then symmetrized.
"""
function randommask(D::EDM, number_of_deletions::Int)
    return randommask(size(D, 1), number_of_deletions)
end

"""
    randommask(n::Int, fraction_of_deletions::Real)

Randomly and uniformly create a mask with n * (n - 1) * fraction_of_deletions number of deleted
elements.
"""
function randommask(n::Int, fraction_of_deletions::Float64)
    n ≥ 2 || throw(DomainError(n, "n < 2"))
    1 ≥ fraction_of_deletions > 0 || throw(DomainError(fraction_of_deletions, "We need 0 < fraction ≤ 1 of entries."))

    deletions = round(Int, n * (n-1) / 2 * fraction_of_deletions)
    return randommask(n, deletions)
end

"""
    randommask(D::EDM, fraction_of_deletions::Real)

Randomly and uniformly create a mask with n * (n - 1) * fraction_of_deletions number of deleted
elements.
"""
function randommask(D::EDM, fraction_of_deletions::Real)
    return randommask(size(D, 1), fraction_of_deletions)
end

"""
    idxmask(D::EDM, indices::CartesianIndices)

Generate a mask from CartesianIndices.

The indices along the diagonal are ignored.
"""
function idxmask(D::EDM, indices::CartesianIndices; symmetric::Bool=true)
    mask = ones(Int, size(D))

    # We set values to zero for all non-diagonal indices
    mask[[idx for idx in indices if idx[1] != idx[2]]] .= 0

    if symmetric && issymmetric(mask)
        return mask
    elseif symmetric && !issymmetric(mask)
        # If there are more zeros in the lower triangle
        if count(x -> x == 0, LowerTriangular(mask)) > count(x -> x == 0, UpperTriangular(mask))
            return Symmetric(mask, :L)
        else # It's not symmetric, so the zeros must be above the diagonal
            return Symmetric(mask, :U)
        end
    else
        return mask
    end

end


struct FreeTimeConstraints <: Constraints
	n::Int
	ineq
end

function free_time_constraints(T)
	n = T - 2
	ineq = (1:0)

	return FreeTimeConstraints(n, ineq)
end

function constraints!(c, Z, con::FreeTimeConstraints, model, idx, h, T)
	for t = 1:T-2
		h = view(Z,idx.u[t])[end]
		h⁺ = view(Z,idx.u[t + 1])[end]
		c[t] = h⁺ - h
	end

    return nothing
end

function constraints_jacobian!(∇c, Z, con::FreeTimeConstraints, model, idx, h, T)
	shift = 0
	for t = 1:T-2
		r_idx = (t:t)

		c_idx = (idx.u[t][end]:idx.u[t][end])
		len = 1
		∇c[shift + len] = -1.0
		shift += 1

		c_idx = (idx.u[t + 1][end]:idx.u[t + 1][end])
		len = 1
		∇c[shift + len] = 1.0
		shift += 1
	end

	return nothing
end

function constraints_sparsity(con::FreeTimeConstraints, model, idx, T;
	shift_row = 0, shift_col = 0)
    row = []
    col = []

	for t = 1:T-2
		r_idx = shift_row .+ (t:t)

		c_idx = shift_col .+ (idx.u[t][end]:idx.u[t][end])
		row_col!(row, col, r_idx, c_idx)

		c_idx = shift_col .+ (idx.u[t + 1][end]:idx.u[t + 1][end])
		row_col!(row, col, r_idx, c_idx)
	end
    return collect(zip(row, col))
end


using DrWatson
@quickactivate "NonlinearDynamicsTextbook"
include(srcdir("style.jl"))
using DynamicalSystems, PyPlot, Random

a = 1.4; b = 0.3
he = Systems.henon(; a, b)
fig = figure()

x0, y0 = columns(trajectory(he, 5000, Ttr = 100))
x1 = x0
y1 = @. 1 - a*x0^2 + y0

x2 = b .* x1
y2 = y1
y3 = x2
x3 = y2

ax1 = subplot(141)
ax1.scatter(x0, y0, color = COLORS[1], s = 1, label = "initial")
ax1.text(0.1, 0.05, "initial", transform=ax1.transAxes)

αprev = 0.75
ax2 = subplot(142)
ax2.scatter(x0, y0, color = COLORS[1], s = 0.25, alpha = αprev)
ax2.scatter(x1, y1, color = COLORS[2], s = 1, label = "stretch/bend")
ax2.text(0.1, 0.05, "bend/stretch", transform=ax2.transAxes)
ax2.arrow(0,-0.19, 0, 0.8, head_width=0.1, head_length=0.1, linewidth=4, color=COLORS[5], length_includes_head=true)
ax2.arrow(-1.5, 0.0,  0, -0.5, head_width=0.1, head_length=0.1, linewidth=4, color=COLORS[5], length_includes_head=true)
ax2.arrow(1.5, 0.0,  0, -0.5, head_width=0.1, head_length=0.1, linewidth=4, color=COLORS[5], length_includes_head=true)

ax3 = subplot(143)
ax3.scatter(x1, y1, color = COLORS[2], s = 0.25, alpha = αprev)
ax3.scatter(x2, y2, color = COLORS[3], s = 1)
ax3.text(0.1, 0.05, "squeeze", transform=ax3.transAxes)
ax3.arrow(-1.5, 0.0,  1, 0, head_width=0.1, head_length=0.1, linewidth=4, color=COLORS[5], length_includes_head=true)
ax3.arrow(1.5, 0.0,  -1, 0, head_width=0.1, head_length=0.1, linewidth=4, color=COLORS[5], length_includes_head=true)

ax4 = subplot(144)
ax4.scatter(x2, y2, color = COLORS[3], s = 0.25, alpha = αprev)
ax4.scatter(x3, y3, color = COLORS[4], s = 1, label = "reflect")
ax4.text(0.1, 0.05, "reflect", transform=ax4.transAxes)

ax4.plot([-2,2], [-2,2], ls = "dashed", lw = 2, color = COLORS[5])

# ticks and stuff
ax1.set_ylabel("\$y\$")
for ax in (ax1, ax2, ax3, ax4)
    ax.set_xlim(-2, 2)
    ax.set_ylim(-1.5, 1.5)
    ax.set_yticks(-1.5:0.5:1.5)
    ax.set_xticks(-1.5:1:1.5)
    ax.grid("on")
    ax.set_yticklabels([])
    ax.set_xticklabels([])
    ax.set_xlabel("\$x\$", labelpad = -10)
    # ax.legend(markerscale = 4, scatterpoints=1)
end
tight_layout(pad = 0.33)
add_identifiers!(fig; xloc = 0)
wsave(plotsdir("3", "stretchinghenon"), fig)


n = 10 # rand(1:10)
@test matrixdepot("chow", n) == matrixdepot("chow", Float64, n)
@test matrixdepot("chow",Int, 5, 5, 0)[5,1] == 3125
@test diag(matrixdepot("chow", n, 2, 4)) == ones(Float64, n) * 6
println("'chow' passed test...")


module ModalC0BasesTests

using Test
using Gridap.TensorValues
using Gridap.Fields
using Gridap.Polynomials
# using BenchmarkTools

import Gridap.Fields: Broadcasting

# Real-valued Q space with isotropic order

x1 = Point(0.0)
x2 = Point(0.5)
x3 = Point(1.0)

V = Float64
G = gradient_type(V,x1)
H = gradient_type(G,x1)
order = 12

order = 3
a = fill(Point(-0.5),order+1)
b = fill(Point(2.5),order+1)
b1 = ModalC0Basis{1}(V,order,a,b)
∇b1 = Broadcasting(∇)(b1)
∇∇b1 = Broadcasting(∇)(∇b1)

@test evaluate(b1,[x1,x2,x3,]) ≈ [1.0 0.0 -0.0 0.0;
                                  0.5 0.5 -0.4330127018922193 0.18633899812498247;
                                  0.0 1.0 -0.0 -0.0]
@test evaluate(∇b1,[x1,x2,x3,]) ≈ G[(-1.0,) (1.0,) (-1.7320508075688772,) (1.4907119849998598,);
                                    (-1.0,) (1.0,) (-0.0,) (-0.37267799624996495,);
                                    (-1.0,) (1.0,) (1.7320508075688772,) (-0.0,)]
@test evaluate(∇∇b1,[x1,x2,x3,]) ≈ H[(0.0,) (0.0,) (3.4641016151377544,) (-5.962847939999439,);
                                     (0.0,) (0.0,) (3.4641016151377544,) (-1.4907119849998598,);
                                     (0.0,) (0.0,) (3.4641016151377544,) (2.9814239699997196,)]

x1 = Point(0.0,0.0)
x2 = Point(0.5,0.5)
x3 = Point(1.0,1.0)
a = [ Point(0.0,0.0),Point(0.0,0.0),Point(0.0,0.0),Point(0.0,0.0),
      Point(-0.5,2.5),Point(-0.5,2.5),Point(0.0,1.5),Point(0.0,1.5),
      Point(-1.0,-1.0),Point(-1.0,-1.0),Point(-1.0,-1.0),Point(-1.0,-1.0),
      Point(0.0,0.0),Point(0.0,0.0),Point(0.0,0.0),Point(0.0,0.0) ]
b = [ Point(1.0,1.0),Point(1.0,1.0),Point(1.0,1.0),Point(1.0,1.0),
      Point(2.5,2.5),Point(2.5,2.5),Point(1.5,1.5),Point(1.5,1.5),
      Point(-1.0,1.0),Point(-1.0,1.0),Point(-1.0,1.25),Point(-1.0,1.25),
      Point(1.0,1.0),Point(1.0,1.0),Point(1.0,1.0),Point(1.0,1.0) ]
b2 = ModalC0Basis{2}(V,order,a,b)
∇b2 = Broadcasting(∇)(b2)
∇∇b2 = Broadcasting(∇)(∇b2)

G = gradient_type(V,x1)
H = gradient_type(G,x1)

@test evaluate(b2,[x1,x2,x3,]) ≈ [ 1.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0;
                                   0.25 0.25 0.25 0.25 -0.21650635094610965 0.09316949906249124 -0.21650635094610965 0.09316949906249124 -0.21650635094610965 -0.13975424859373686 -0.21650635094610965 -0.09316949906249122 0.18749999999999997 0.0 0.0 0.0;
                                   0.0 0.0 0.0 1.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 ]
@test evaluate(∇b2,[x1,x2,x3,])[:,10] ≈ G[ (0.0, 0.0,); (0.2795084971874737, -0.2795084971874737,); (0.0, 0.0,) ]
@test evaluate(∇∇b2,[x1,x2,x3,])[:,10] ≈ H[ (0.0, 0.0, 0.0, -4.47213595499958);
                                            (0.0, 0.5590169943749475, 0.5590169943749475, 1.118033988749895);
                                            (0.0, -2.23606797749979, -2.23606797749979, 0.0) ]

end # module


#= --------------------------------------------------------- =#
mutable struct TBModel{T<:Real}
    pos::Matrix{T}  		# position of elements in unit cell
    periodicity::BitVector  # periodicity along latx,laty,latz (boolean = yes/no)
    neighbors::Vector{Vector{Int}} # neighboring indices
	hopfun::Function        # hopping function (input is two positions; output is basisdim×basisdim array)
    basisdim::Int           # dimension of basis on each site (must be same everywhere, for now)
end
TBModel(hopfun) = TBModel( Matrix{Float64}(undef,0,2),  # pos
                     falses(2),                   # periodicity
					 Vector{Vector{Int}}(),       # neighbors
					 hopfun,                      # hopfun
                     1)                           # basisdim

#= --------------------------------------------------------- =#
function interDistance(ri::AbstractVector{<:Real}, rj::AbstractVector{<:Real},
					   periodicity::AbstractVector{Bool}=falses(length(first(ri))))

	# TODO: get rid of all the array allocations here...!
	# not periodic in any dimension; regular flat metric
	if all(!, periodicity)
		return sqrt( sum( abs2.(ri - rj) ) )

	# periodic in some directions; use distance on a flat n-torus,
	# with n = sum(periodicity)
	else
		# TODO: don't hardcode this to a [0,1]² box
		s = zero(eltype(ri))
		rij = abs.(ri - rj)
		for xyz in eachindex(ri)
			if periodicity[xyz] # dimension is periodic
				s += min(rij[xyz], 1-rij[xyz])^2
			else			    # dimension is not periodic
				s += rij[xyz]^2
			end
		end
		return sqrt(s)
	end

end

#= --------------------------------------------------------- =#
function interAngle(ri::AbstractVector{<:Real}, rj::AbstractVector{<:Real},
                    periodicity::AbstractVector{Bool}=falses(length(first(ri))))
    #= calculates the angle between two 2D points ri and rj, on a
       possibly periodic (flat) manifold =#
    rij = ri - rj
    for xy = 1:2 # fold to shortest distance over periodic directions
		if periodicity[xy]
			# TODO: Don't hardcode this to a [0,1]² box...
            if abs(rij[xy]) > abs(rij[xy]-1)
                rij[xy] -= 1
            elseif abs(rij[xy]) > abs(rij[xy]+1)
                rij[xy] += 1
            end
        end
    end
    return atan(rij[2],rij[1])
end

#= --------------------------------------------------------- =#
function setHopping!(tb::TBModel)

	n = size(tb.pos,1)
    basisiter = Base.OneTo(tb.basisdim)
    H = spzeros(ComplexF64, n*tb.basisdim, n*tb.basisdim)

	#= fill upper triangle with hopping amplitude by evaluating
    the function hopfun as a function of (i,j)-positions  =#
	for i = Base.OneTo(n)
		for j = (i+1):n
			t = tb.hopfun(tb.pos[i,:], tb.pos[j,:], tb.periodicity)
            for k = basisiter
                for l = basisiter
                    if !iszero(t[k,l])
                        H[i+(k-1)*n,j+(l-1)*n] = t[k,l]  # upper triangular part
                    end
                end
			end
		end
	end

    #= fill out the lower triangle, using Hermicity of H =#
    H = H + H' # when done this way, it has be done _before_ diagonal part is entered

	# ---- diagonal of H should be computed here ----
    for i = 1:n
        # stupid implementation; but easier to extend to index-dependence
        t = tb.hopfun(tb.pos[i,:], tb.pos[i,:], tb.periodicity)
        for k = basisiter
            for l = basisiter
                H[i+(k-1)*n,i+(l-1)*n] = t[k,l]
            end
        end
    end

	# set neighbors (useful for visualization and analysis)
	tb.neighbors = Vector{Vector{Int}}(undef, n)
	for i = 1:n
		# TODO: This is a horrible way to do this with a sparse array, lol... Should use a
		#		combination of column iteration and nzrange instead...
		tb.neighbors[i] = findall(!iszero, H[i,1:n])
	end

	return H
end

#= --------------------------------------------------------- =#
function buildHoneycombRestriction(restrictfun::Function, maxscale::Real)
	maxscale≤0 && throw(DomainError(maxscale, "maxscale must be a positive value"))

	intralat = [ [sqrt(3)*0.5, 0.5], [0.0, 1.0] ]
	shiftmag = 0.5/sqrt(3)
	steplist = -round(1.25*maxscale):round(1.25*maxscale)

	posa = Matrix{Float64}(undef, 0,2) # initialize arrays
	posb = similar(posa)

	# grow A and B sublattice arrays if points inside restriction
	for n = steplist
		for m = steplist
            newposa = sum(intralat.*[n,m]) + [shiftmag,0]
            newposb = sum(intralat.*[n,m]) - [shiftmag,0]

			if restrictfun(newposa)
				posa = vcat(posa, newposa')
			end
			if restrictfun(newposb)
				posb = vcat(posb, newposb')
			end

		end
	end

	# remove dangling bonds
	dangleremoved = 1
	while dangleremoved != 0
		dangleremoved = 0

		# fix A lattice
		delrows = Int[]
		for i in 1:size(posa,1)
			nneighbors = sum(map((x,y) -> interDistance(posa[i,:], [x,y]), posb[:,1], posb[:,2]) .< 0.58)
			if nneighbors < 2 # dangling bond
			    push!(delrows,i)
			end
		end
		posa = posa[setdiff(1:end,delrows),:];
		dangleremoved += length(delrows)

		# fix B lattice
		delrows = Int[]
		for i in 1:size(posb,1)
			nneighbors = sum(map((x,y) -> interDistance([x,y], posb[i,:]), posa[:,1], posa[:,2]) .< 0.58)
			if nneighbors < 2 # dangling bond
			    push!(delrows,i)
			end
		end
		posb = posb[setdiff(1:end,delrows),:];
		dangleremoved += length(delrows)
	end

	return vcat(posa, posb)
end

#= --------------------------------------------------------- =#
function solve(tb::TBModel; eigvecflag::Bool=false)
	H = setHopping!(tb)
    if !eigvecflag
    	ε = eigvals(Hermitian(Matrix(H)))
        return ε
    else
        F = eigen(Hermitian(Matrix(H)))
        return F.values, F.vectors
    end

end

#= --------------------------------------------------------- =#
function hopHoneycomb(ri::AbstractVector{<:Real}, rj::AbstractVector{<:Real},
                      periodicity::AbstractVector{Bool}=falses(length(first(ri))))
    #= Nearest neighbor hopping on a honeycomb (graphene-like) lattice
       with unity hopping amplitude =#
	a = interDistance(ri, rj, periodicity)
	t = a<.58 ? -1.0 : 0.0

end

#= --------------------------------------------------------- =#
function hopAgarwala(ri::AbstractVector{<:Real}, rj::AbstractVector{<:Real},
                     periodicity::AbstractVector{Bool}=trues(length(first(ri))),
                     R=1.0/6, M=-0.5, t₂=0.25, λ=0.5, a=1.0/24)
    #= Implementation of the hopping type described in Eq. (5) of Agarwala &
       Shenoy, PRL 118, 236402 (2017) =#

	r = interDistance(ri, rj, periodicity)

    if r > R
		t = zeros(ComplexF64, 2, 2)
    elseif r > zero(eltype(r))
        ϕ = interAngle(ri, rj, periodicity) # works properly also in periodic cases

        # the minus sign in front of exp _must_ be there in front of _both_
        # offdiag terms in order to get the right result; i.e. t is not Hermitian
		# on its own (but H is still Hermitian, by construction)
		imcisϕ = -cis(-ϕ)*im
		sin²ϕ  = sin(ϕ)^2
        offdiagᴬᴮ = ( imcisϕ + λ*(sin²ϕ*(1+im)-1) ) / 2
        offdiagᴮᴬ = ( imcisϕ + λ*(sin²ϕ*(1-im)-1) ) / 2
        diagᴬᴬ = 0.5 * (-1 + t₂)
        diagᴮᴮ = 0.5 * (+1 + t₂)

        t = exp(1 - r/a) .* [ diagᴬᴬ offdiagᴬᴮ; offdiagᴮᴬ diagᴮᴮ ]
    else # on-site/self term (r=0)
        t = [(2+M) (1-im)*λ; (1+im)*λ -(2+M)] # this term must be Hermitian though
    end

	return t
end

#= --------------------------------------------------------- =#
circularRestriction(r,a) = sqrt(sum(r.^2)) < a
circularRestriction(r) = circularRestriction(r,15)


# ------------------------------------------------------------------------------------------
# # 多重派发
# 在这个 notebook 中我们将探索 Julia 的重要特性之一：**多重派发**。
#
# 多重派发让软件更**通用（generic）、更快**！
#
# ## 目录
#
# - [从熟悉的开始讲](#从熟悉的开始讲)
# - [指定传入变量的类型](#指定传入变量的类型)
# ------------------------------------------------------------------------------------------

# ------------------------------------------------------------------------------------------
# ## 从熟悉的开始讲
#
# 从我们已经接触过的知识来理解多重派发。
#
# 我们可以声明一个 Julia 函数而不用给出传入变量的任何类型信息：
# ------------------------------------------------------------------------------------------

f(x) = x^2

# ------------------------------------------------------------------------------------------
# Julia 自会判断哪些传入变量是合理的，哪些又是不合理的：
# ------------------------------------------------------------------------------------------

f(10)

f([1, 2, 3])

# ------------------------------------------------------------------------------------------
# ## 指定传入变量的类型
#
# 但是，我们*也可以*显式地指定传入变量的类型。
#
# 比如，我们写一个只接受字符串类型传入变量的函数 `foo`。
# ------------------------------------------------------------------------------------------

foo(x::String, y::String) = println("My inputs x and y are both strings!")

# ------------------------------------------------------------------------------------------
# 可以看到，要限制 `x` 和 `y` 的类型为 `String`，只要在形参名后面加上两个冒号和关键字 `String` 就行了。
#
# 现在 `foo` 只有在传入变量为 `String` 类型才正常工作。
# ------------------------------------------------------------------------------------------

foo("hello", "hi!")

foo(3, 4)

# ------------------------------------------------------------------------------------------
# 要让 `foo` 函数接受整型（`Int`）的传入变量，只要在声明 `foo` 函数时在形参后面加上 `::Int`。
# ------------------------------------------------------------------------------------------

foo(x::Int, y::Int) = println("My inputs x and y are both integers!")

foo(3, 4)

# ------------------------------------------------------------------------------------------
# 现在函数 `foo` 能接受整型变量了！但是注意，函数 `foo` 仍然能接受字符串变量！
# ------------------------------------------------------------------------------------------

foo("hello", "hi!")

# ------------------------------------------------------------------------------------------
# 这就涉及到多重派发的核心了。当我们声明
#
# ```julia
# foo(x::Int, y::Int) = println("My inputs x and y are both integers!")
# ```
# 我们并没有重写或覆盖
# ```julia
# foo(y::String, y::String)
# ```
# 而是给叫做 `foo` 的**通用函数（generic function）**添加了一个**新方法（method）**。
#
# 一个**通用函数** 是一个表示特定操作的抽象概念。
# 举个例子，通用函数 `+` 代表了加法这个概念
#
# 一个**方法** 是一个**通用函数** 接受*特定参数类型* 的一个具体实现。
# 举个例子，`+` 有不同的方法以接受浮点数、整型、矩阵等等。
#
# 我们可以通过函数 `methods` 来查看函数 `foo` 有多少种方法。
# ------------------------------------------------------------------------------------------

methods(foo)

methods(+)

# ------------------------------------------------------------------------------------------
# 所以，我们现在可以在调用 `foo` 的时候传入整型或字符串了。
# 当你调用 `foo` 时传入特定组合的参数时，Julia 将根据传入参数的类型派发对应的方法。*这* 就是多重派发。
#
# 多重派发让我们的代码更加通用更加快。
# 代码更加通用更灵活是因为我们是在写代码描述抽象操作例如加法和乘法，而不是描述具体的实现。
# 同时，因为 Julia 可以为相关类型调用高效的方法，代码可以运行得很快。
#
# 想查看我们调用一个通用函数时派发的是哪个方法，我们可以用宏 `@which`：
# ------------------------------------------------------------------------------------------

@which foo(3, 4)

@which 3.0 + 3.0

# ------------------------------------------------------------------------------------------
# 考虑到一个特别写给浮点数的方法派发给了 `3.0 + 3.0`，生成的 LLVM 代码极其简练：
# ------------------------------------------------------------------------------------------

@code_llvm 3.0 + 3.0

# ------------------------------------------------------------------------------------------
# 要注意的是 Julia 在我们写一般的函数定义时也很快，因为具体的、定制的方法最终是在后台调用的。
#
# 举个例子，我们声明加法函数 `myadd` 时没有类型注释——
# ------------------------------------------------------------------------------------------

myadd(x, y) = x + y

# ------------------------------------------------------------------------------------------
# 虽然我们没有限制 `x` 和 `y` 的类型，但是 `myadd(3.0, 3.0)` 生成的 LLVM 代码和 `3.0 + 3.0` 差不多
# ------------------------------------------------------------------------------------------

@code_llvm myadd(3.0, 3.0)


using Test
using FlightMechanicsSimulator
using FlightMechanicsUtils

#  Stevens, B. L., Lewis, F. L., & Johnson, E. N. (2015). Aircraft control
#  and simulation: dynamics, controls design, and autonomous systems. John Wiley
#  & Sons. (page 193 table 3.6-2)
trim_test_data = [
#   TAS   thtl    AOA     DE       thtl_tol    AOA_tol    DE_tol
#   ft/s  unit    deg     deg      unit        deg        deg
    130   0.816   45.6    20.1     0.0005      0.05       0.15
    140   0.736   40.3   -1.36     0.001       0.05       0.05
    150   0.619   34.6    0.173    0.0005      0.05       0.05
    170   0.464   27.2    0.621    0.001       0.05       0.05
    640   0.23    0.742  -0.871    0.0005      0.015      0.0005
    800   0.378   -0.045 -0.943    0.0005      0.001      0.001
    200   0.287   19.7    0.723    0.0005      0.05       0.05
    260   0.148   11.6   -0.09     0.0005      0.05       0.05
    300   0.122   8.49   -0.591    0.0005      0.01       0.005
    350   0.107   5.87   -0.539    0.001       0.005      0.005
    400   0.108   4.16   -0.591    0.0005      0.005      0.005
    440   0.113   3.19   -0.671    0.0005      0.005      0.005
    500   0.137   2.14   -0.756    0.001       0.01       0.005
    540   0.16    1.63   -0.798    0.0005      0.005      0.005
    600   0.2     1.04   -0.846    0.0005      0.01       0.005
    700   0.282   0.382  -0.9      0.0005      0.001      0.0005
]

xcg = 0.35

for case in eachrow(trim_test_data)
    local x, controls, x_trim, controls_trim, x_dot_trim, outputs_trim, cost
    x = [
        case[1]*FT2M,  #-> vt (m/s)
        deg2rad(10.),  # -> alpha (rad)
        0.0,  # -> beta (rad)
        0.0,  # -> phi (rad)
        deg2rad(10.),  # -> theta (rad)
        0.0,  #  -> psi (rad)
        0.0,  # -> P (rad/s)
        0.0,  # -> Q (rad/s)
        0.0,  # -> R (rad/s)
        0.0,  # -> North (m)
        0.0,  # -> East (m)
        0.0,  # -> Altitude (m)
        50.0,  # -> Pow
    ]

    controls = [
        0.5,  # thtl
        0.0,  # elev
        0.0,  # ail
        0.0,  # rudder
    ]

    # TRIM
    dssd, controls_trim, outputs_trim, cost = trim(
        SixDOFAeroEuler(x),
        controls,
        F16(F16Stevens.MASS, F16Stevens.INERTIA, xcg),
        F16StevensAtmosphere(x[12]),
        LHDownGravity(FlightMechanicsSimulator.F16Stevens.GD*FT2M),
        0.0,
        0.0,
    )

    x_trim = get_x(dssd)
    x_dot_trim = get_xdot(dssd)

    @test isapprox(cost, zeros(6), atol=1e-12)
    @test isapprox(controls_trim[1], case[2], atol=case[5])  # THTL
    @test isapprox(rad2deg(x_trim[2]), case[3], atol=case[6])  # AOA
    @test isapprox(controls_trim[2], case[4], atol=case[7])  # DE
end

#  Stevens, B. L., Lewis, F. L., & Johnson, E. N. (2015). Aircraft control
#  and simulation: dynamics, controls design, and autonomous systems. John Wiley
#  & Sons. (page 195 table 3.6-3)

# NOMINAL (first column)
xcg = 0.35

x = [
    502*FT2M,  #-> vt (m/s)
    deg2rad(10.),  # -> alpha (rad)
    0.0,  # -> beta (rad)
    0.0,  # -> phi (rad)
    deg2rad(10.),  # -> theta (rad)
    0.0,  #  -> psi (rad)
    0.0,  # -> P (rad/s)
    0.0,  # -> Q (rad/s)
    0.0,  # -> R (rad/s)
    0.0,  # -> North (m)
    0.0,  # -> East (m)
    0.0,  # -> Altitude (m)
    50.0,  # -> Pow
]

controls = [
    0.5,  # thtl
    0.0,  # elev
    0.0,  # ail
    0.0,  # rudder
]

dssd, controls_trim, outputs_trim, cost = trim(
        SixDOFAeroEuler(x),
        controls,
        F16(F16Stevens.MASS, F16Stevens.INERTIA, xcg),
        F16StevensAtmosphere(x[12]),
        LHDownGravity(FlightMechanicsSimulator.F16Stevens.GD*FT2M),
        0.0,
        0.0,
    )

x_trim = get_x(dssd)
x_dot_trim = get_xdot(dssd)

@test isapprox(cost, zeros(6), atol=1e-12)
@test isapprox(x_trim[2], 0.03691, atol=0.00005)  # AOA
@test isapprox(x_trim[3], -4e-9, atol=1e-8)  # AOS
@test isapprox(x_trim[4], 0)  # PHI
@test isapprox(x_trim[5], 0.03691, atol=0.00005)  # THETA
@test isapprox(x_trim[7], 0)  # P
@test isapprox(x_trim[8], 0)  # Q
@test isapprox(x_trim[9], 0)  # R
@test isapprox(controls_trim[1], 0.1385, atol=0.0001)  # THTL
@test isapprox(controls_trim[2], -0.7588, atol=0.0002)  # DE
@test isapprox(controls_trim[3], -1.2e-7, atol=1e-6)  # DA
@test isapprox(controls_trim[4], 6.2e-7, atol=1e-6)  # DR

# XCG = 0.3 (second column)
xcg = 0.3

x = [
    502*FT2M,  #-> vt (m/s)
    deg2rad(10.),  # -> alpha (rad)
    0.0,  # -> beta (rad)
    0.0,  # -> phi (rad)
    deg2rad(10.),  # -> theta (rad)
    0.0,  #  -> psi (rad)
    0.0,  # -> P (rad/s)
    0.0,  # -> Q (rad/s)
    0.0,  # -> R (rad/s)
    0.0,  # -> North (m)
    0.0,  # -> East (m)
    0.0,  # -> Altitude (m)
    50.0,  # -> Pow
]

controls = [
    0.5,  # thtl
    0.0,  # elev
    0.0,  # ail
    0.0,  # rudder
]

dssd, controls_trim, outputs_trim, cost = trim(
        SixDOFAeroEuler(x),
        controls,
        F16(F16Stevens.MASS, F16Stevens.INERTIA, xcg),
        F16StevensAtmosphere(x[12]),
        LHDownGravity(FlightMechanicsSimulator.F16Stevens.GD*FT2M),
        0.0,
        0.0,
    )

x_trim = get_x(dssd)
x_dot_trim = get_xdot(dssd)

@test isapprox(cost, zeros(6), atol=1e-12)
@test isapprox(x_trim[2], 0.03936, atol=0.00005)  # AOA
@test isapprox(x_trim[3], 4.1e-9, atol=1e-8)  # AOS
@test isapprox(x_trim[4], 0)  # PHI
@test isapprox(x_trim[5], 0.03936, atol=0.00005)  # THETA
@test isapprox(x_trim[7], 0)  # P
@test isapprox(x_trim[8], 0)  # Q
@test isapprox(x_trim[9], 0)  # R
@test isapprox(controls_trim[1], 0.1485, atol=0.00005)  # THTL
@test isapprox(controls_trim[2], -1.931, atol=0.0001)  # DE
@test isapprox(controls_trim[3], -7e-8, atol=1e-6)  # DA
@test isapprox(controls_trim[4], 8.3e-7, atol=1e-6)  # DR

# XCG = 0.38 (third column)
xcg = 0.38

x = [
    502*FT2M,  #-> vt (m/s)
    deg2rad(10.),  # -> alpha (rad)
    0.0,  # -> beta (rad)
    0.0,  # -> phi (rad)
    deg2rad(10.),  # -> theta (rad)
    0.0,  #  -> psi (rad)
    0.0,  # -> P (rad/s)
    0.0,  # -> Q (rad/s)
    0.0,  # -> R (rad/s)
    0.0,  # -> North (m)
    0.0,  # -> East (m)
    0.0,  # -> Altitude (m)
    50.0,  # -> Pow
]

controls = [
    0.5,  # thtl
    0.0,  # elev
    0.0,  # ail
    0.0,  # rudder
]

dssd, controls_trim, outputs_trim, cost = trim(
        SixDOFAeroEuler(x),
        controls,
        F16(F16Stevens.MASS, F16Stevens.INERTIA, xcg),
        F16StevensAtmosphere(x[12]),
        LHDownGravity(FlightMechanicsSimulator.F16Stevens.GD*FT2M),
        0.0,
        0.0,
    )

x_trim = get_x(dssd)
x_dot_trim = get_xdot(dssd)

@test isapprox(cost, zeros(6), atol=1e-12)
@test isapprox(x_trim[2], 0.03544, atol=0.00005)  # AOA
@test isapprox(x_trim[3], 3.1e-8, atol=1e-7)  # AOS
@test isapprox(x_trim[4], 0)  # PHI
@test isapprox(x_trim[5], 0.03544, atol=0.00005)  # THETA
@test isapprox(x_trim[7], 0)  # P
@test isapprox(x_trim[8], 0)  # Q
@test isapprox(x_trim[9], 0)  # R
@test isapprox(controls_trim[1], 0.1325, atol=0.0001)  # THTL
@test isapprox(controls_trim[2], -0.05590, atol=0.0005)  # DE
@test isapprox(controls_trim[3], -5.1e-8, atol=1e-6)  # DA
@test isapprox(controls_trim[4], 4.3e-6, atol=1e-5)  # DR

# Coordinated turn (fourth column)
xcg = 0.3

x = [
    502*FT2M,  #-> vt (m/s)
    deg2rad(10.),  # -> alpha (rad)
    0.0,  # -> beta (rad)
    0.0,  # -> phi (rad)
    deg2rad(10.),  # -> theta (rad)
    0.0,  #  -> psi (rad)
    0.0,  # -> P (rad/s)
    0.0,  # -> Q (rad/s)
    0.0,  # -> R (rad/s)
    0.0,  # -> North (m)
    0.0,  # -> East (m)
    0.0,  # -> Altitude (m)
    50.0,  # -> Pow
]

controls = [
    0.5,  # thtl
    0.0,  # elev
    0.0,  # ail
    0.0,  # rudder
]

dssd, controls_trim, outputs_trim, cost = trim(
        SixDOFAeroEuler(x),
        controls,
        F16(F16Stevens.MASS, F16Stevens.INERTIA, xcg),
        F16StevensAtmosphere(x[12]),
        LHDownGravity(FlightMechanicsSimulator.F16Stevens.GD*FT2M),
        0.0,
        0.3,  # rad/s
    )

x_trim = get_x(dssd)
x_dot_trim = get_xdot(dssd)

@test isapprox(cost, zeros(6), atol=1e-12)
@test isapprox(x_trim[2], 0.2485, atol=0.0005)  # AOA
@test isapprox(x_trim[3], 4.8e-4, atol=0.00005)  # AOS
@test isapprox(x_trim[4], 1.367, atol=0.0005)  # PHI
@test isapprox(x_trim[5], 0.05185, atol=0.00005)  # THETA
@test isapprox(x_trim[7], -0.01555, atol=0.00001)  # P
@test isapprox(x_trim[8], 0.2934, atol=0.00005)  # Q
@test isapprox(x_trim[9], 0.06071, atol=0.000005)  # R
@test isapprox(controls_trim[1], 0.8499, atol=0.0005)  # THTL
@test isapprox(controls_trim[2], -6.256, atol=0.001)  # DE
@test isapprox(controls_trim[3], 0.09891, atol=0.00005)  # DA
@test isapprox(controls_trim[4], -0.4218, atol=0.0005)  # DR


# 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.

using Test
using Multivectors
using LinearAlgebra

include("Blades_runtests.jl")
include("KVectors_runtests.jl")

module PG2
  using Multivectors, LinearAlgebra
  @generate_basis("++0")

  const eₒ_index = 3
  const eₒ = e₃
  const e₂ₒ = e₂₃
  const e₁ₒ = e₁₃

  meet(a, b) = a∧b
  point(x::Real, y::Real) = x*e₂ₒ + y*e₁ₒ + one(x)*e₁₂
  direction(x::Real, y::Real) = x*e₂ₒ + y*e₁ₒ
  point( v::KVector{T} ) where T = point(coords(v)[1:2]...)
  line(a,b) = a∨b

  """ project point to euclidean (1eₒ) plane """
  projr(a) = a/(dual(1.0e₃)⋅a)

  function circumcentre( a, b, c )
    ab = line(a, b)
    bc = line(b, c)
    ab_mid = 0.5*(a+b)
    bc_mid = 0.5*(b+c)
    abperp = ab⋅ab_mid
    bcperp = bc⋅bc_mid

    projr(abperp∧bcperp)
  end

end

using .PG2
@testset "PG2" begin
  @test PG2.circumcentre( PG2.point(0.0, 0.0), PG2.point(1.0,0.0), PG2.point(0.0,1.0) ) == PG2.point(0.5, 0.5)
end

module HomogeneousG2
  using Multivectors, LinearAlgebra
  @generate_basis("+++")

  const eₒ_index = 3
  const eₒ = e₃

  ishomog( v::KVector{T} ) where T = filter(x->x==eₒ_index, subspace.(v)) |> isempty
  point( a, b ) = a*e₁+b*e₂ + one(a)*e₃
  point( v::KVector{T} ) where T = ishomog(v) ? v : v + one(T)eₒ
  line( p, q ) = p∧q
  plane( p, q, r ) = p∧q∧r
  unitize( p::KVector{T} ) where T = (α = one(T)eₒ⋅p; iszero(α) ? p : p*inv(alpha))

  function meet(a, b)
    J = inv(1.0e₁₂₃)
    ((a*J)∧(b*J))*J
  end

  """ project to euclidean (1eₒ) plane """
  projr(a) = a/norm(1.0eₒ⋅a)

  function circumcentre( a, b, c )
    mid_ab = 0.5*(a+b)
    mid_bc = 0.5*(b+c)
    ab = (a∧b)
    bc = (b∧c)
    rab = 1.0eₒ⋅ab
    rbc = 1.0eₒ⋅bc
    tri = rab∧rbc
    abperp = rab*inv(tri)
    bcperp = rbc*inv(tri)
    centre_ab = mid_ab∧abperp
    centre_bc = mid_bc∧bcperp
    meet(centre_ab, centre_bc)
  end
end

using .HomogeneousG2
@testset "HomogeneousG2" begin
  pointh = HomogeneousG2.point
  @test HomogeneousG2.circumcentre( pointh(0.0, 0.0), pointh(1.0,0.0), pointh(0.0,1.0) ) == pointh(0.5, 0.5)
end

module CGA
  using Multivectors, LinearAlgebra

  @generate_basis("++++-",false,true)
  const eo = 0.5*(1.0e₄ + 1.0e₅)
  const e∞ = 1.0e₅-1.0e₄

  point( p::K ) where {T, K<:Union{KVector{T,1}, Blade{T,1}}} = one(T)*eo + p + (one(T)/2)*p*p*e∞
  locate( a, M ) = (a⋅M)/M
  circumcentre( a, b, c) = locate(a, (a∧b + b∧c + c∧a)/(a∧b∧c∧e∞))
end

using .CGA

@testset "CGA3D" begin
  e₁, e₂, e₃ = alle( CGA, 5 )[1:3]
  using .CGA: eo, e∞, point, circumcentre
  @test e∞⋅e∞ == 0
  @test eo⋅e∞ == -1.0

  # test circumcentre of triangle

  a = point(0.0e₁+0.0e₂)
  b = point(1.0e₁)
  c = point(1.0e₂)

  C = (a∧b + b∧c + c∧a)/(a∧b∧c∧e∞)
  ccentre = (a⋅C)/C
  @test coords(grade(ccentre,1))[1:2] == [0.5,0.5]
  a = point(0.0e₁+0.0e₂)
  b = point(1.0e₁)
  c = point((sqrt(3.0)/2.0)e₂)
  ccentre = grade(circumcentre(a,b,c), 1)

  @test length(a.B) >= length(prune(a).B)
  @test iszero(prune(a-a))
  @test norm(ccentre - grade(a,1)) ≈ norm(ccentre - grade(b,1)) ≈ norm(ccentre - grade(c,1))

  i = 1.0e₁+2.0e₂; j = 3.0(e₁∧e₃) + 4.0(e₂∧e₃); k = KVector(5.0CGA.e₂₃₄)
  @test i+j == Multivector(i) + Multivector(j) == j+i == Multivector(i) + j == i + Multivector(j)
  @test i+j+k == i+(j+k) == (i+k)+j

  v = i*1.1+j+k+1.2
  @test Multivectors.Δⱼ(v, 1) == involute(v)
  @test Multivectors.Δⱼ(v, 2) == ~v
  @test prune(shirokov_inv(i+j+k)*(i+j+k)) ≈ 1.0
end

module PG3
  using Multivectors
  @generate_basis("+++0")
end
using .PG3

@testset "PGA3D" begin
  e₁, e₂, e₃, e₄ = alle( PG3, 4)[1:4]
  e₁₂ = PG3.e₁₂; e₃₄ = PG3.e₃₄; e₁₂₃₄ = PG3.e₁₂₃₄;
  e₁₂₃ = PG3.e₁₂₃; e₂₃₄ = PG3.e₂₃₄; e₁₃₄ = PG3.e₁₃₄; e₁₂₄ = PG3.e₁₂₄
  𝐼 = e₁₂₃₄
  @test KVector(dual(1𝐼)) == dual(KVector(1𝐼))
  @test Multivector(KVector(dual(1𝐼))) == Multivector(dual(KVector(1𝐼))) == dual(Multivector(1𝐼))

  point(x, y, z) = -Float64(x)*e₂₃₄ + Float64(y)*e₁₃₄ - Float64(z)*e₁₂₄ + 1.0*e₁₂₃
  dir(x, y, z) = -Float64(x)*e₂₃₄ + Float64(y)*e₁₃₄ - Float64(z)*e₁₂₄
  line(p, q) = p∨q
  Line{T} = Union{KVector{T, 2}, Blade{T, 2}}

  # dual quaternion for translation
  # constructed as ratio between point (0,0,0) and (x,y,z)
  translator(x,y,z) = -0.5*dir(x,y,z)*1.0e₁₂₃+1.0
  # rotation constructed with ratio between line a+b and line a.  so that (a+b)/a * a *~(a+b)/a = b
  # a rotation from a to b. lines should intersect at origin or we'll get a translation
  rotator(a::Line, b::Line) = normalize(a+b)/a

  q = rotator(line(point(0,0,0), point(1,0,0)),
              line(point(0,0,0), point(1/sqrt(2), 1/sqrt(2), 0)))

  # rotate (1,0,0.1) by 45 degrees around z axis twice
  rxz = normalize(q*q*point(1,0,0.1)*reverse(q)*reverse(q))
  rxz_xyz = coords(dual(rxz)[1])
  rxz_xyz = rxz_xyz .* (1.0/rxz_xyz[4])
  @test isapprox(rxz_xyz[1], 0.0; atol = 1e-10)
  @test isapprox(rxz_xyz[2], 1.0; atol = 1e-10)
  @test isapprox(rxz_xyz[3], 0.1; atol = 1e-10)

  tx = translator(1,0,0)
  @test tx*point(1,0,10)*reverse(tx) == point(2,0,10)
  @test (tx*tx)*point(1,0,10)*reverse(tx*tx) == point(3,0,10)

  # dual quaternion is composition of translator and rotator
  x = point(1,0,0)
  dq = tx*q  # rotate then translate
  qd = q*tx  # translate then rotate
  dqx = dq*x*reverse(dq)
  # check x coord after transformation
  @test (dqx∧1.0e₁)[4][1] |> scalar ≈ 1/sqrt(2) + 1.0
  qdx = qd*x*reverse(qd)
  @test (qdx∧1.0e₁)[4][1] |> scalar ≈ 2/sqrt(2)

  # z coord should not have changed
  qdxz = qd*point(1.0,0,0.1)*reverse(qd)
  dqxz = dq*point(1.0,0,0.1)*reverse(dq)
  @test (qdxz∧1.0e₃)[4][1] |> scalar ≈ 0.1
  @test (dqxz∧1.0e₃)[4][1] |> scalar ≈ 0.1

  a = e₁(1.0); b = e₂(2.0); c = e₃(3.0); d = e₄(4.0)
  @test typeof(a+b*c) == typeof(Multivector{Float64,2}())
  B1 = KVector(a)
  B2 = a*b + b*c
  B3 = a*b*c + b*c*d
  B4 = KVector(a*b*c*d)
  M13 = B1+B3
  @test grade(M13) == 3
  @test grades(M13) == [1,3]
  @test grade(M13, 2) == 0.0
  @test grade(M13, 1) == B1
  @test grade(M13, 3) == B3
  @test grade(M13, 0) == 0.0
  M013 = M13+42.0
  @test grade(M013, 0) == 42.0
  B = a+b
  @test B*B == a*a+a*b+b*a+b*b
  @test grade((2e₂+3e₃)*(2e₂+3e₃), 0) == 2*2+3*3

  # degenerate wedge
  D = 1.0 + 2.0e₄
  E = 2.0 + 3.0(e₁∧e₄)
  @test grade(D∧E, 3) == grade(D*E, 3)
  F = 1.0 + 2.0e₃
  G = 2.0 + 3.0(e₁∧e₃)
  @test grade(F∧G, 3) == grade(F*G, 3)
  @test grade(D∧G, 3) == grade(D*G, 3)

  A = 2e₃ +3e₂
  B = 5e₄+6e₃
  # if we had an e₁ (degen metric) component this would not be true.
  @test A⋅!B == !(A∧B)

  A₀ = 2e₁ + 3e₂
  B₀ = 5e₁ + 6e₃

  # interesting though, this is ok.
  𝐼 = 1e₁₂₃₄
  @test A₀⋅(B₀⋅𝐼) == (A₀∧B₀)⋅𝐼 == (A₀∧B₀)*𝐼 == A₀⋅(B₀*𝐼)# == A₀⋅(B₀*dual(1))

  C = 1e₁₂+4e₃₄
  @test (A∧B)⋅C == A⋅(B⋅C)

  # inner product tests
  # conjugate symmetry? a property an innerproduct is supposed to have.  probably not
  #  @test A⋅C == conj(C⋅A)
  # linearity in first argument
  @test (A+B)⋅C == (A⋅C)+(B⋅C)

  @test 2*A×B == (A*B-B*A)
  A = 2-A
  B = 2*B + 3(e₂∧e₃)
  @test 2A×B == (A*B-B*A)

  v = 1.0+2.0e₁₂ + 1.1(e₁∧e₃)
  @test isapprox(norm(v*inv(v)), 1.0; atol=1e-4)
end

module G4
  using Multivectors
  @generate_basis("++++")
end
using .G4

@testset "conversion" begin
  e₁, e₂, e₃, e₄  = alle( G4, 4)[1:4]
  @test  1.0e₁ + 2e₁ == 1e₁ + 2.0e₁ == 1.0e₁ + 2.0e₁
  @test  1.0e₁ + 2e₂ == 1e₁ + 2.0e₂ == 1.0e₁ + 2.0e₂
end

@testset "contraction" begin

  e₁, e₂, e₃, e₄  = alle( G4, 4)[1:4]
  e₁₃ = G4.e₁₃

  rand1() = rand(1)[1]
  a = rand1()∧e₁ + rand()∧e₃
  # test lcontraction result is orthogonal to original
  B = rand()*(e₁∧e₂)
  @test a⋅(a⋅B) == 0.0
  B = B + rand()*(e₃∧e₄)
  @test a⋅(a⋅B) == 0.0

  α = Multivector(3.0)
  β = 3.0

  A = 1.0 + 3.0G4.e₁₃
  @test lcontraction(α, β) == lcontraction(β, α) == lcontraction(α, α) == 9.0
  @test rcontraction(α, β) == rcontraction(β, α) == rcontraction(α, α) == 9.0
  @test lcontraction(α, A) == α*A
  @test lcontraction(A, α) == α*A[0]
  @test rcontraction(α, A) == α*A[0]
  @test rcontraction(A, α) == A*α
  @test ∙(α, A) == ∙(A, α) == 0.0
  @test ∙(a, a) == grade(a*a, 0)
  @test A*a == A[0]*a + A[2]*a
  @test lcontraction(a,A) == a⋅A == grade(a, 1)*A[2]

  C = A + a
  D = a + 2.0*pseudoscalar(e₁)
  @test scalarprod(A∧C, D) == scalarprod(A, lcontraction(C,D))
  @test scalarprod(D, C∧A) == scalarprod(rcontraction(D,C), A)
  @test_broken rcontraction(C,D) == C[1]*D[1] + C[2]*D[1]  #!me not sure about the new right contraction, comes from mix of Lengyal and Browne
  @test_broken rcontraction(D,C) == D*C[0] + D[1]*C[1] + D[4]*C[1] + D[4]*C[2]
  @test lcontraction(C,D) == C[0]*D + C[1]*D[1] + C[1]*D[4] + C[2]*D[4]
  @test_broken reverse(lcontraction(A,C)) == rcontraction(reverse(C), reverse(A))
  @test_broken reverse(lcontraction(C,D)) == rcontraction(reverse(D), reverse(C))
  @test_broken reverse(lcontraction(A,D)) == rcontraction(reverse(D), reverse(A))
  @test_broken reverse(lcontraction(D,A)) == rcontraction(reverse(A), reverse(D))
  @test grades(A) == [0,2]
  show(A)
  @test first(A) == 1.0
  @test [i for i in A] == map(i->i, A) |>collect == (i for i in A) |> collect
  @test length(A) == 2
  @test mapreduce(identity, +, A) == A
  @test mapreduce(conj, +, A) == conj(A)
  @test 1.0e₁ + A[2] + A[0] == A + 1.0e₁
  @test iszero(grade(A,10))
  @test grades(1.0) == [0]
  @test grade(1.0, 0) == 1.0
  @test grade(1.0, 1) == 0.0
  @test grades(A[2]) == [2]
  @test prune(dual(A)*pseudoscalar(A[2])) == A
  @test A/2.0 == A*0.5
  @test A[2] == A-A[0]
  @test mapreduce(LinearAlgebra.norm_sqr, +, A) == scalarprod(A,reverse(A)) == 10.0
  @test normalize(A) == A/sqrt(scalarprod(A,reverse(A)))

  for i in A
    show(i)
  end

  a = 1.0 + 1.5(e₁) + 2.0(e₁∧e₂)
  b = 1.0 - 10.0e₁ - 10.0e₄ + 2.5(e₁∧e₂∧e₄) + 3.0(e₁∧e₂∧e₃∧e₄)
  @test a⋅b != grade(a*b, grade(b)-grade(a)) # only works for homogeneous k-vectors
  @test a∧b != grade(a*b, grade(b)+grade(a)) # only works for homogeneous k-vectors

  a = 1.0e₁+2.0e₂+3.0e₃
  B = b
  C = 1.0e₁ + 2.0G4.e₁₂ + 3.0G4.e₁₂₃ + 1.1e₂ + 2.2G4.e₂₄
  @test a⋅(B∧C) == (a⋅B)∧C + involute(B)∧(a⋅C)
  @test_broken rcontraction(B, a) == -a⋅involute(B)
  @test lcontraction(a, B) == 0.5*(a*B - involute(B)*a)
  @test_broken rcontraction(B, a) == 0.5*(B*a - a*involute(B))

  # less common multivector products
  A,B = Multivector(2.0e₁+1.0e₁₃), Multivector(2.0e₁+3.0e₂)
  @test length(A×B) > 0
  @test length(B⋅A) == 2
  @test length(A⋅B) == 1
  @test length(B∧A) == 2
  @test length(A∧B) == 2
  @test 2.0⋅A == 2.0*A
  @test 2.0∧A == 2.0*A
  @test A⋅2.0 == 0.0

  @test (1e₁ ∈ 1G4.e₁₂) == true
  @test (1e₁ ∈ 1G4.e₂₃+1G4.e₁₂) == false
  @test (1+1e₁ ∈ 1G4.e₂₃+1G4.e₁₂) == false

  @test isapprox(newton_inv(10.0+2.0G4.e₂₄+3.0G4.e₂₃+0.1e₂)*(10.0+2.0G4.e₂₄+3.0G4.e₂₃+0.1e₂), Multivector(1.0); atol = 0.001)
end

module G3
  using Multivectors
  @generate_basis("+++",false,true,true)
end
using .G3

@testset "Outermorphism" begin
  e₁, e₂, e₃, e₁₂, e₁₃, e₂₃, e₁₂₃ = alle( G3, 3)
  b = 3.0e₁₃
  k = 1.0e₁ + 2.0e₂
  M = 42.0 + b + k
  scale = [2.0 0 0; 0 0.75 0; 0 0 10.0]
  @test outermorphism(scale, b) == 10.0*2.0*b
  @test prune(outermorphism(scale, M)[1]) == 2.0e₁ + 0.75*2.0e₂
end

@testset "Quaternion" begin

  e₁, e₂, e₃, e₁₂, e₁₃, e₂₃, e₁₂₃ = alle( G3, 3)
#==
  Quaternion multiplication
×	i	j	k
i	−1	k	−j
j	−k	−1	i
k	j	−i	−1
==#

  # quaternion basis
  𝑖 = e₂₃; 𝑗 = e₁₃; 𝑘 = e₁₂
  @test 1𝑖*1𝑗*1𝑘 == -1

  @test (1𝑖*1𝑖, 1𝑖*1𝑗, 1𝑖*1𝑘) == (-1, 1𝑘, -1𝑗)
  @test (1𝑗*1𝑖, 1𝑗*1𝑗, 1𝑗*1𝑘) == (-1𝑘, -1, 1𝑖)
  @test (1𝑘*1𝑖, 1𝑘*1𝑗, 1𝑘*1𝑘) == (1𝑗, -1𝑖, -1)

  𝑖 = e₁₂; 𝑗 = e₂₃; 𝑘 = e₁₃
  @test 1𝑖*1𝑗*1𝑘 == -1
  @test (1𝑖*1𝑖, 1𝑖*1𝑗, 1𝑖*1𝑘) == (-1, 1𝑘, -1𝑗)
  @test (1𝑗*1𝑖, 1𝑗*1𝑗, 1𝑗*1𝑘) == (-1𝑘, -1, 1𝑖)
  @test (1𝑘*1𝑖, 1𝑘*1𝑗, 1𝑘*1𝑘) == (1𝑗, -1𝑖, -1)

  half45 = normalize(1.0e₁ + normalize(1.0e₁+1.0e₂))
  q = half45/1.0e₁

  # Transform a 1-vector with the sandwich product.
  v = reverse(q)*(1.0e₁+1.0e₂+1.0e₃)*q

  v′ = grade(v, 1) |> prune∘sort_basis
  @test v′⋅1.0e₃ == 1.0
  @test v′⋅1.0e₁ ≈ sqrt(2.0)

  # Rotors can be constructed using half-angle of trig functions, like quaternions.

  @test cos(π/8) - sin(π/8)*1.0e₁₂ == q

  @test isapprox(grade(log(exp((π/10)*𝑖)), 2), π/10*𝑖; atol = 0.0001)
  @test isapprox(grade(log(exp((π/10)*𝑖)), 0), 0.0; atol = 0.0001)

end

@testset "Barycentric" begin

  e₁, e₂, e₃  = alle( G3, 3)[1:3]

  a = 0.0e₁+0.0e₂; b = 1.0e₁ + 0.0e₂; c = 0.0e₁ + 1.0e₂;  # a simple right angle triangle

  A = (b-a)∧(c-a)  # twice the area of the triangle. we don't worry about the factor of 2

  # Make a function to calculate barycentric coords as the ratio of the area of a triangle made with a point `p` and an edge over original triangle.  i.e. the barycentric coord for vertex `a` is the ratio Δpbc/Δabc

  barycoords(p) = ((c-b)∧(p-b)/A, (a-c)∧(p-c)/A, (b-a)∧(p-a)/A)  # a tuple of coords

  # Notice how the code very directly represents the geometric relationship.  The body of the function is also coordinate free ( we never index into the points or vertices ).

  @test barycoords(0.0e₁)[1] == 1.0
  @test barycoords(1.0e₁)[2] == 1.0
  @test barycoords(0.5e₁+0.5e₂) == (0.0, 0.5, 0.5)

  baryscalars(p) = map(k->grade(k, 0), barycoords(p))

  @test baryscalars(0.1e₁+0.25e₂+10.0e₃) == (0.65, 0.1, 0.25)
  @test baryscalars(1.0e₁) == (0.0, 1.0, 0.0)

  d = 1.0e₃
  V = A∧d

  # tetrahedron
  barycoords4(p) = ((c-b)∧(p-b)∧(d-c)/V,
                    (a-c)∧(p-c)∧(d-a)/V,
                    (b-a)∧(p-a)∧(d-a)/V,
                    (b-a)∧(p-a)∧(a-c)/V)

  @test barycoords4(0.25e₁+0.25e₂+0.25e₃) == (0.25, 0.25, 0.25, 0.25)
end

@testset "Cayley" begin
  e₁, e₂, e₃  = alle( G3, 3)[1:3]

  γ₁ = matrix_representation(e₁)
  γ₂ = matrix_representation(e₂)
  γ₁₂ = matrix_representation(G3.e₁₂)

  @test γ₂ ≈ -γ₁₂*γ₁

  @test cayley_table(e₁)[2,4] == 1G3.e₁₃
end


function mm_spec_kj5_dimspec()::MPCCDimSpec
    dimspec = MPCCDimSpec(3, 1, 2, 0, 2, 1, 0)
    return dimspec
end

function mm_spec_kj5_defn(x, pr, ps)

    f = x[1] + x[2] - (1-pr[1]) * x[3]

    ce = Vector{Num}()

    ci = Vector{Num}(undef, 2)
    ci[1] = 4x[1] - x[3]
    ci[2] = 4x[2] - x[3]

    F = Matrix{Num}(undef, 2, 1)
    F[1,1] = x[1]
    F[2,1] = x[2]

    return MPCCDefinition(f, ce, ci, F)
end

function mm_spec_kj5_nzmask()::MPCCModelNZMask
    nz_mask = MPCCModelNZMask(
        # ce
        [],
        # ci
        [   Set{Int64}([1, 2]),
            Set{Int64}([1]) ],
        # F
        [
            # F1
            [   Set{Int64}([1]) ],
            # F2
            [   Set{Int64}([2]) ]
        ] )
    return nz_mask
end

function mm_spec_kj5_testvectors()
    return Vector{MPCCModelTestVector}(undef, 0)
end

function mm_spec_kj5_knownsols()

    function local_kj5_knownsol(pr::Vector{R}, ps::Vector{Int64}) where {R <: Real}
        @assert ( 1 == length(pr) && 0 == length(ps) )

        t = pr[1]
        if ( t < 0 || t > 1)
            error("t out of range, must be in [0,1]")
        end

        x_sol = [0, 0, 0]
        f_sol = 0

        return (x_sol, f_sol)
    end

    return [ local_kj5_knownsol ]
end


"""
    `getmatfail(sigma1::Real, sigma2::Real, tau12::Real, xt::Real,
        xc::Real, yt::Real, yc::Real, s::Real, method::String)`

    `getmatfail(sigma1::Real, sigma2::Real, tau12::Real, mat::Material,
        method::String)`

    `getmatfail(stress::AbstractArray{<:Real,1}, mat::Material, method::String)`

    `getmatfail(stress::AbstractArray{<:Real,1},xt::Real,xc::Real,yt::Real,
        yc::Real,s::Real, method::String)`

    `getmatfail(stress::AbstractArray{<:AbstractArray{<:Real,1},1},
        mat::AbstractArray{Material}, lam::Laminate, method::String)`

Determines ply failure according to specified theory. Values greater than one
signify ply failure. Choose between "maxstress", "tsaiwu", and "hashinrotem".
Safety factors are stored in the second return argument.

# Theory Specific Output
Maximum Ply Stress:
1. Longitudinal tension
2. Longitudinal compression
3. Transverse tension
4. Transverse compression
5. Positive shear
6. Negative shear

Tsai-Wu
1. Failure Criterion

Hashin-Rotem
1. Fiber failure in tension
2. Fiber failure in compression
3. Matrix failure in tension
4. Matrix failure in compression
"""
function getmatfail(sigma1::Real, sigma2::Real, tau12::Real, xt::Real,
    xc::Real, yt::Real, yc::Real, s::Real, method::AbstractString)

    if method == "maxstress"
        return maxstress(sigma1, sigma2, tau12, xt, xc, yt, yc, s)
    elseif method == "tsaiwu"
        return tsaiwu(sigma1, sigma2, tau12, xt, xc, yt, yc, s)
    elseif method == "hashinrotem"
        return hashinrotem(sigma1, sigma2, tau12, xt, xc, yt, yc, s)
    else
        error("Chosen method ($method) not implemented. Choose between `maxstress`,
            `tsaiwu`, and `hashinrotem`")
    end
end

getmatfail(stress::AbstractArray{<:Real,1}, mat::Material, method::AbstractString) =
    getmatfail(stress[1], stress[2], stress[3], mat.xt, mat.xc, mat.yt, mat.yc,
    mat.s, method)
getmatfail(stress::AbstractArray{<:Real,1}, xt::Real, xc::Real, yt::Real,
    yc::Real, s::Real, method::AbstractString) = getmatfail(stress[1], stress[2],
    stress[3], xt, xc, yt, yc, s, method)
getmatfail(sigma1::Real, sigma2::Real, tau12::Real, mat::Material, method::AbstractString) =
    getmatfail(sigma1, sigma2, tau12, mat.xt, mat.xc, mat.yt, mat.yc, mat.s, method)

function getmatfail(stress::AbstractArray{<:AbstractArray{<:Real,1},1},
    mat::AbstractArray{<:Material,1}, lam::Laminate, method::AbstractString)

    result = [getmatfail(stress[i][1], stress[i][2], stress[i][3],
        mat[lam.matid[i]].xt, mat[lam.matid[i]].xc,
        mat[lam.matid[i]].yt, mat[lam.matid[i]].yc,
        mat[lam.matid[i]].s, method) for i in 1:length(lam.matid)]

    matfail = [result[i][1] for i in 1:length(lam.matid)]
    sf = [result[i][2] for i in 1:length(lam.matid)]

    return matfail, sf
end

function maxstress(sigma1::Real, sigma2::Real, tau12::Real, xt::Real,
    xc::Real, yt::Real, yc::Real, s::Real)

    fail = [sigma1/xt, -sigma1/xc, sigma2/yt, -sigma2/yc, tau12/s, -tau12/s]

    safetyfactor = 1.0./fail

    return fail, safetyfactor
end
maxstress(stress::AbstractArray{<:Real,1}, mat::Material) =
    maxstress(stress[1], stress[2], stress[3], mat.xt, mat.xc, mat.yt, mat.yc, mat.s)
maxstress(stress::AbstractArray{<:Real,1}, xt::Real, xc::Real, yt::Real,
    yc::Real, s::Real) = maxstress(stress[1], stress[2], stress[3], xt, xc, yt, yc, s)
maxstress(sigma1::Real, sigma2::Real, tau12::Real, mat::Material) =
    maxstress(sigma1, sigma2, tau12, mat.xt, mat.xc, mat.yt, mat.yc, mat.s)

function maxstress(stress::AbstractArray{<:AbstractArray{<:Real,1},1},
    mat::AbstractArray{<:Material,1}, lam::Laminate)

    result = [maxstress(stress[i][1], stress[i][2], stress[i][3],
        mat[lam.matid[i]].xt, mat[lam.matid[i]].xc,
        mat[lam.matid[i]].yt, mat[lam.matid[i]].yc,
        mat[lam.matid[i]].s, method) for i in 1:length(lam.matid)]

    matfail = [result[i][1] for i in 1:length(lam.matid)]
    sf = [result[i][2] for i in 1:length(lam.matid)]

    return matfail, sf
end

function tsaiwu(sigma1::Real, sigma2::Real, tau12::Real, xt::Real,
    xc::Real, yt::Real, yc::Real, s::Real)

    a = (sigma1^2.0/(xt*xc))+(sigma2^2.0/(yt*yc))-
         sqrt(1.0/(xt*xc)*1.0/(yt*yc))*sigma1*sigma2+tau12^2.0/s^2.0
    b = (1.0/xt-1.0/xc)*sigma1+(1.0/yt-1.0/yc)*sigma2
    c = -1
    fail = a+b
    safetyfactor = abs(fail) > 1e-20 ? (-b+sqrt(b^2-4*a*c))/(2a) : Inf

    return fail, safetyfactor
end
tsaiwu(stress::AbstractArray{<:Real,1}, mat::Material) =
    tsaiwu(stress[1], stress[2], stress[3], mat.xt, mat.xc, mat.yt, mat.yc, mat.s)
tsaiwu(stress::AbstractArray{<:Real,1}, xt::Real, xc::Real, yt::Real,
    yc::Real, s::Real) = tsaiwu(stress[1], stress[2], stress[3], xt, xc, yt, yc, s)
tsaiwu(sigma1::Real, sigma2::Real, tau12::Real, mat::Material) =
    tsaiwu(sigma1, sigma2, tau12, mat.xt, mat.xc, mat.yt, mat.yc, mat.s)

function tsaiwu(stress::AbstractArray{<:AbstractArray{<:Real,1},1},
    mat::AbstractArray{<:Material,1}, lam::Laminate)

    result = [tsaiwu(stress[i][1], stress[i][2], stress[i][3],
        mat[lam.matid[i]].xt, mat[lam.matid[i]].xc,
        mat[lam.matid[i]].yt, mat[lam.matid[i]].yc,
        mat[lam.matid[i]].s, method) for i in 1:length(lam.matid)]

    matfail = [result[i][1] for i in 1:length(lam.matid)]
    sf = [result[i][2] for i in 1:length(lam.matid)]

    return matfail, sf
end

function hashinrotem(sigma1::Real, sigma2::Real, tau12::Real, xt::Real,
    xc::Real, yt::Real, yc::Real, s::Real)

    tensionfail = (sigma2^2.0/yt^2.0+tau12^2.0/s^2.0)
    compressionfail = (sigma2^2.0/yc^2.0+tau12^2.0/s^2.0)
    fail = [sigma1/xt, -sigma1/xc, sign(sigma2)*tensionfail, sign(sigma2)*-compressionfail]
    safetyfactor = [xt/sigma1, -xc/sigma1, 1/sqrt(tensionfail), sign(sigma2)*-1/sqrt(compressionfail)]

    return fail, safetyfactor
end
hashinrotem(stress::AbstractArray{<:Real,1}, mat::Material) =
    hashinrotem(stress[1], stress[2], stress[3], mat.xt, mat.xc, mat.yt, mat.yc, mat.s)
hashinrotem(stress::AbstractArray{<:Real,1}, xt::Real, xc::Real, yt::Real,
    yc::Real, s::Real) = hashinrotem(stress[1], stress[2], stress[3], xt, xc, yt, yc, s)
hashinrotem(sigma1::Real, sigma2::Real, tau12::Real, mat::Material) =
    hashinrotem(sigma1, sigma2, tau12, mat.xt, mat.xc, mat.yt, mat.yc, mat.s)

function hashinrotem(stress::AbstractArray{<:AbstractArray{<:Real,1},1},
    mat::AbstractArray{<:Material,1}, lam::Laminate)

    result = [hashinrotem(stress[i][1], stress[i][2], stress[i][3],
        mat[lam.matid[i]].xt, mat[lam.matid[i]].xc,
        mat[lam.matid[i]].yt, mat[lam.matid[i]].yc,
        mat[lam.matid[i]].s, method) for i in 1:length(lam.matid)]

    matfail = [result[i][1] for i in 1:length(lam.matid)]
    sf = [result[i][2] for i in 1:length(lam.matid)]

    return matfail, sf
end