@testset "Polyhedral" begin
    @testset "Start solutions iterator" begin
        f = equations(cyclic(5))
        iter = HC.PolyhedralStartSolutionsIterator(f)
        @test isnothing(iter.mixed_cells)
        @test isnothing(iter.lifting)
        @test length(iter) == 70
        @test !isnothing(iter.mixed_cells)
        @test !isnothing(iter.lifting)
        starts = collect(iter)
        @test length(starts) == 70
        @test all(starts) do (cell, x)
            all(1:length(cell.indices)) do i
                a, b = cell.indices[i]
                v_i = iter.start_coefficients[i][a] * prod(x .^ iter.support[i][:, a]) +
                      iter.start_coefficients[i][b] * prod(x .^ iter.support[i][:, b])
                abs(v_i) < 1e-12
            end
        end
        @test length(collect(HC.PolyhedralStartSolutionsIterator(f))) == 70

        Random.seed!(130793) # this seed should result in the use of 128 bit hnf
        f = equations(PolynomialTestSystems.tritangents())
        iter = HC.PolyhedralStartSolutionsIterator(f)
        @test length(iter) == 12636
        @test length(collect(iter)) == 12636

        # Catch overflow in mixed_volume
        f = equations(PolynomialTestSystems.cyclooctane())
        F = [f; randn(2, 18) * [HC.variables(f); 1]]
        iter = HC.PolyhedralStartSolutionsIterator(F)
        @test_throws OverflowError length(iter)
    end

    @testset "Tracking" begin
        tracker, starts = pathtracker_startsolutions(
            equations(cyclic(5));
            start_system = :polyhedral,
            seed = 123122,
        )
        S = collect(starts)
        HC.prepare!(tracker, starts)
        @test count(s -> is_success(track!(tracker, s)), S) == 70

        @polyvar x y
        tracker, starts = pathtracker_startsolutions(
            [(x - 3), (y - 2)];
            start_system = :polyhedral,
            system_scaling = nothing,
            seed = 23121,
        )
        S = collect(starts)
        HC.prepare!(tracker, starts)
        @test solution(track(tracker, S[1])) ≈ [3, 2] atol = 1e-8

        tracker, starts = pathtracker_startsolutions(
            [(x - 3), (y - 2)];
            start_system = :polyhedral,
            system_scaling = :equations_and_variables,
            seed = 23121,
        )
        S = collect(starts)
        HC.prepare!(tracker, starts)
        @test solution(track(tracker, S[1])) ≈ [3, 2] atol = 1e-8
    end

    @testset "All affine solutions" begin
        @polyvar x y
        f = [2y + 3 * y^2 - x * y^3, x + 4 * x^2 - 2 * x^3 * y]
        tracker, starts = pathtracker_startsolutions(
            f,
            start_system = :polyhedral,
            only_torus = true,
        )
        S = collect(starts)
        HC.prepare!(tracker, starts)
        results = map(s -> track(tracker, s), S)
        @test count(is_success, results) == 3
        @test length(results) == 3

        tracker, starts = pathtracker_startsolutions(
            f,
            start_system = :polyhedral,
            only_torus = false,
        )
        S = collect(starts)
        HC.prepare!(tracker, starts)
        results = map(s -> track(tracker, s), S)
        @test count(is_success, results) == 6
        @test length(results) == 8
    end

end


using Images

canny_edges = canny(img, sigma = 1.4, upperThreshold = 0.80, lowerThreshold = 0.20)


@testset "disparity" begin
    ft = @load_toyfairtensor
    M = [true_positive_rate, false_positive_rate, ppv]
    @test_throws ArgumentError disparity(M, ft)
    d = disparity(M, ft; refGrp="Education")
    @test names(d) == ["labels", "true_positive_rate_disparity", "false_positive_rate_disparity",
                        "positive_predictive_value_disparity"]
    @test size(d)==(3, 4)
end

@testset "Disparity" begin
    X, y = @load_toydata()
    model = ConstantClassifier()
    result = evaluate(model, X, y, measure=Disparity(tpr, grp=:Sex, refGrp="M", func=-))
    @test length(keys(result.per_fold[1][1])) == 3
    result = evaluate(model, X, y, measures=Disparities([tpr, fpr], grp=:Sex, refGrp="M", func=/))
    @test length(keys(result.per_fold[1][1])) == 3
    @test length(result.per_fold) == 2
end

@testset "Parity" begin
    ft = @load_toyfairtensor
    M = [true_positive_rate]
    df = disparity(M, ft; refGrp="Board")
    df = parity(df, 0.2)
    @test names(df)[3] == "true_positive_rate_parity"
    arr = df[:, :true_positive_rate_parity]
    @test arr[1] && !arr[2] && !arr[3]
end


# Filter
b = [1., 2., 3., 4.]
x = [1., 1., 0., 1., 1., 0., 0., 0.]
@test filt(b, 1., x)  == [1., 3., 5., 8., 7., 5., 7., 4.]
@test filt(b, [1., -0.5], x)  == [1., 3.5, 6.75, 11.375, 12.6875, 11.34375, 12.671875, 10.3359375]

# Convolution
a = [1., 2., 1., 2.]
b = [1., 2., 3.]
@test_approx_eq conv(a, b) [1., 4., 8., 10., 7., 6.]
@test_approx_eq conv(complex(a, ones(4)), complex(b)) complex([1., 4., 8., 10., 7., 6.], [1., 3., 6., 6., 5., 3.])

# Discrete cosine transform (DCT) tests

a = rand(8,11) + im*rand(8,11)
@test norm(idct(dct(a)) - a) < 1e-8

X = reshape([1,2,7,2,1,5,9,-1,3,4,6,9],3,4)
Y = rand(17,14)
Y[3:5,9:12] = X
sX = slice(Y,3:5,9:12)

true_Xdct = [  13.856406460551018  -3.863239728836245   2.886751345948129  -0.274551994240164; -2.828427124746190  -2.184015211898548  -4.949747468305834   3.966116180118245; 4.898979485566356  -0.194137576915510  -2.857738033247041   2.731723009609389 ]

true_Xdct_1 = [    5.773502691896258   4.618802153517007   6.350852961085884  10.969655114602890; -4.242640687119286  -2.121320343559643   4.242640687119286  -3.535533905932738; 1.632993161855452   2.041241452319315   5.715476066494083   0.408248290463863 ]

true_Xdct_2 = [    8.  -3.854030797826254  -3.0  3.761176226848022;
   4.0  -2.071929829606556   4.0  -2.388955165168770; 12.  -0.765366864730179   4.0  -1.847759065022573 ]

Xdct = dct(X)
Xdct! = float(X); dct!(Xdct!)
Xdct_1 = dct(X,1)
Xdct!_1 = float(X); dct!(Xdct!_1,1)
Xdct_2 = dct(X,2)
Xdct!_2 = float(X); dct!(Xdct!_2,2)

Xidct = idct(true_Xdct)
Xidct! = copy(true_Xdct); idct!(Xidct!)
Xidct_1 = idct(true_Xdct_1,1)
Xidct!_1 = copy(true_Xdct_1); idct!(Xidct!_1,1)
Xidct_2 = idct(true_Xdct_2,2)
Xidct!_2 = copy(true_Xdct_2); idct!(Xidct!_2,2)

pXdct = plan_dct(X)(X)
pXdct! = float(X); plan_dct!(pXdct!)(pXdct!)
pXdct_1 = plan_dct(X,1)(X)
pXdct!_1 = float(X); plan_dct!(pXdct!_1,1)(pXdct!_1)
pXdct_2 = plan_dct(X,2)(X)
pXdct!_2 = float(X); plan_dct!(pXdct!_2,2)(pXdct!_2)

pXidct = plan_idct(true_Xdct)(true_Xdct)
pXidct! = copy(true_Xdct); plan_idct!(pXidct!)(pXidct!)
pXidct_1 = plan_idct(true_Xdct_1,1)(true_Xdct_1)
pXidct!_1 = copy(true_Xdct_1); plan_idct!(pXidct!_1,1)(pXidct!_1)
pXidct_2 = plan_idct(true_Xdct_2,2)(true_Xdct_2)
pXidct!_2 = copy(true_Xdct_2); plan_idct!(pXidct!_2,2)(pXidct!_2)

sXdct = dct(sX)
psXdct = plan_dct(sX)(sX)
sYdct! = copy(Y); sXdct! = slice(sYdct!,3:5,9:12); dct!(sXdct!)
psYdct! = copy(Y); psXdct! = slice(psYdct!,3:5,9:12); plan_dct!(psXdct!)(psXdct!)

for i = 1:length(X)
    @test_approx_eq Xdct[i] true_Xdct[i]
    @test_approx_eq Xdct![i] true_Xdct[i]
    @test_approx_eq Xdct_1[i] true_Xdct_1[i]
    @test_approx_eq Xdct!_1[i] true_Xdct_1[i]
    @test_approx_eq Xdct_2[i] true_Xdct_2[i]
    @test_approx_eq Xdct!_2[i] true_Xdct_2[i]

    @test_approx_eq pXdct[i] true_Xdct[i]
    @test_approx_eq pXdct![i] true_Xdct[i]
    @test_approx_eq pXdct_1[i] true_Xdct_1[i]
    @test_approx_eq pXdct!_1[i] true_Xdct_1[i]
    @test_approx_eq pXdct_2[i] true_Xdct_2[i]
    @test_approx_eq pXdct!_2[i] true_Xdct_2[i]

    @test_approx_eq Xidct[i] X[i]
    @test_approx_eq Xidct![i] X[i]
    @test_approx_eq Xidct_1[i] X[i]
    @test_approx_eq Xidct!_1[i] X[i]
    @test_approx_eq Xidct_2[i] X[i]
    @test_approx_eq Xidct!_2[i] X[i]

    @test_approx_eq pXidct[i] X[i]
    @test_approx_eq pXidct![i] X[i]
    @test_approx_eq pXidct_1[i] X[i]
    @test_approx_eq pXidct!_1[i] X[i]
    @test_approx_eq pXidct_2[i] X[i]
    @test_approx_eq pXidct!_2[i] X[i]

    @test_approx_eq sXdct[i] true_Xdct[i]
    @test_approx_eq psXdct[i] true_Xdct[i]
    @test_approx_eq sXdct![i] true_Xdct[i]
    @test_approx_eq psXdct![i] true_Xdct[i]
end


function create_ray(x1::T, y1::T, z1::T, x2::T, y2::T, z2::T) where {T<:AbstractFloat}
    dx = x2 - x1
    dy = y2 - y1
    dz = z2 - z1
    l = sqrt((dx^dx) + (dy*dy) + (dz*dz))
    dx = dx / l
    dy = dy / l
    dz = dz / l
    ray = Ray(x1, y1, z1, dx, dy, dz)
    return ray
end

function create_rays(minx::AbstractFloat, miny::AbstractFloat, maxx::AbstractFloat, maxy::AbstractFloat, height::AbstractFloat, dx::AbstractFloat, dy::AbstractFloat, dz::AbstractFloat, n::Int)
    x = rand(Uniform(minx, maxx), n)
    y = rand(Uniform(miny, maxy), n)
    rays = Ray[]
    for i=1:n
        push!(rays, Ray(x[i], y[i], height, dx, dy, dz))
    end
    return rays
end

function create_ray(walls::Walls, dx::AbstractFloat, dy::AbstractFloat, dz::AbstractFloat)
    minx, maxx = walls.west + SMALL_NUM, walls.east - SMALL_NUM
    miny, maxy = walls.south + SMALL_NUM, walls.north - SMALL_NUM # check this though!
    height = walls.top - SMALL_NUM
    x = rand(Uniform(minx, maxx))
    y = rand(Uniform(miny, maxy))
    ray = Ray(x, y, height, dx, dy, dz)
    return ray
end

function create_rays(walls::Walls, dx::AbstractFloat, dy::AbstractFloat, dz::AbstractFloat, n::Int)
    minx, maxx = walls.west + SMALL_NUM, walls.east - SMALL_NUM
    miny, maxy = walls.south + SMALL_NUM, walls.north - SMALL_NUM # check this though!
    height = walls.top - SMALL_NUM
    rays = create_rays(minx, miny, maxx, maxy, height, dx, dy, dz, n)
    return rays
end

function create_coords(camera::Camera{T,S,A}) where {T<:AbstractFloat, S<:Integer, A<:AbstractArray{T,1}}
    # construct an array of coordinate, just for the shape...
    coords = Array{Coord{S}}(undef, camera.xResolution, camera.yResolution)
    for x::S=1:camera.xResolution
        for y::S=1:camera.yResolution
            coords[x,y] = Coord(x, y)
        end
    end
    return coords
end


using Distances
using NearestNeighbors
using StaticArrays

struct NeighborList
    i::Vector{Vector{Int}}
    j::Vector{Vector{Int}}
    R::Vector{Vector{Float64}}
    r::Vector{Vector{SVector{3,Float64}}}
end
length(nn::NeighborList) = length(nn.i)

function get_distance(L::SVector{3,<:AbstractFloat}, x::SVector{3,<:AbstractFloat}, y::SVector{3,<:AbstractFloat})
    broadcast(L, x, y) do L, x, y
        d = y - x
        2mod(abs(d), L) < L ? d : L - d
    end
end

to_array(A::AbstractSystem) = [SVector{3}(ustrip.(p)) for p ∈ position(A)]

function neighborlist(A::AbstractSystem{3}, rcutoff::Float64)
    # Convert Positions to Matrix for Ball tree
    X = to_array(A)

    # Create Metric for Periodic Boundary Conditions
    periodic = periodicity(A)
    L = @SVector [periodic[i] ? ustrip(bounding_box(A)[i][i]) : Inf for i = 1:3]
    d = Distances.PeriodicEuclidean(L)

    # Build Ball tree
    tree = BallTree(X, d)

    # Intialize empty vectors
    i = Vector{Int64}[] # i
    j = Vector{Int64}[] # j
    R = Vector{Float64}[] # Distances
    r = Vector{SVector{3,Float64}}[] # Positions

    # Fill vectors
    for n = 1:length(X)
        neighbors = inrange(tree, X[n], rcutoff, true)
        itemp = Int64[]
        jtemp = Int64[]
        Rtemp = Float64[]
        rtemp = SVector{3,Float64}[]
        for neighbor in neighbors
            if neighbor != n
                push!(itemp, n)
                push!(jtemp, neighbor)
                rr = get_distance(L, X[n], X[neighbor])
                push!(rtemp, rr)
                push!(Rtemp, norm(rr))
            end
        end

        push!(i, itemp)
        push!(j, jtemp)
        push!(R, Rtemp)
        push!(r, rtemp)
    end
    NeighborList(i, j, R, r)
end


using DiscreteChoice
using Optim
using ForwardDiff
using Random, StatsBase
using ProfileView

#################
# Simulate data
#################

Random.seed!(42)

N = 20000
K1 = 1
K2 = 5
L = 4
J = 10

Y = rand(1:J,N)
X1 = randn(N,J,K1)
X2 = randn(J,K2)
D = randn(N,L)

α = randn(K1)
Π = randn(K2, L)
ξ_all = vcat(0, rand(J-1))
ξ = ξ_all[2:end]

Py = DiscreteChoice.y_prob(X1,X2,D, α, Π, ξ_all)
Y = zeros(Int64, N)
for i in eachindex(Y)
    Y[i] = sample(1:J, Weights(Py[i,:]))
end
[mean(Y.==i) for i in 1:J]

@time llk_sample_fullset(Y, X1, X2, D, α, Π, ξ)

# Try with random choice subset
C = DiscreteChoice.get_random_set(J, 5, Y)
@time llk_sample_subset(Y, X1, X2, D, C, α, Π, ξ)

#######################
# Estimate the model
#######################

model_homo = DCModel(Y,X1,X2,D; hetero_preference=false)
estDCModel!(model_homo; init_guess = initial_guess(model_homo))
updateCoef!(model_homo)

model_homo.α .- α
model_homo.Π .- Π
model_homo.ξ .- ξ

# Notice that if we start from the true value, it gives correct results
# but if we start from random value, the results are very off, even for merely 10 parameters

para_true = vcat(α, Π[:], ξ)
model_homo_2 = DCModel(Y,X1,X2,D; hetero_preference=false)
# estDCModel!(model_homo_2; init_guess = initial_guess(model_homo_2), subset = C)
estDCModel!(model_homo_2; init_guess = para_true, subset = C)
updateCoef!(model_homo_2)

model_homo_2.α .- α
model_homo_2.Π .- Π
model_homo_2.ξ .- ξ


function _arb_get_fmpq(x::arb)
  mid = ccall((:arb_mid_ptr, :libarb), Ptr{arf_struct}, (Ptr{arb}, ), &x)
  e = fmpz()
  m = fmpz()
  b = ccall((:arf_get_fmpz_2exp, :libarb), Cint, (Ptr{fmpz}, Ptr{fmpz}, Ptr{arf_struct}), &m, &e, mid)
  @assert abs(e) < 2^63 - 1
  ee = Int(e)
  return fmpq(m, fmpz(1))*fmpq(2)^(ee)
end


function compute_ν(n, dim)
    if dim == 2
        ν = n^2
    else
        ν = n * (n + 1)
    end

    ν
end


##### Diffusion approximation for semi-infinite media #####

#=
@article{contini1997photon,
  title={Photon migration through a turbid slab described by a model based on diffusion approximation. I. Theory},
  author={Contini, Daniele and Martelli, Fabrizio and Zaccanti, Giovanni},
  journal={Applied optics},
  volume={36},
  number={19},
  pages={4587--4599},
  year={1997},
  publisher={Optical Society of America}
}

@article{kienle1997improved,
  title={Improved solutions of the steady-state and the time-resolved diffusion equations for reflectance from a semi-infinite turbid medium},
  author={Kienle, Alwin and Patterson, Michael S},
  journal={JOSA A},
  volume={14},
  number={1},
  pages={246--254},
  year={1997},
  publisher={Optical Society of America}
}
=#

### TD Reflectance ###
"""
    refl_DA_semiinf_TD(t, β::Array{Float64,1}, ρ::Float64, ndet::Float64, nmed::Float64)

Compute the time-domain reflectance from a semi-infinite medium from Eqn. 36 Contini 97 (m = 0).

# Arguments
- `t`: the time vector (ns).
- `β::Array{Float64,1}`: the optical properties [μa, μs'] (cm⁻¹)
- `ρ::Float64`: the source detector separation (cm⁻¹)
- `ndet::Float64`: the boundary's index of refraction (air or detector)
- `nmed::Float64`: the sample medium's index of refraction

# Examples
```jldoctest
julia> refl_DA_semiinf_TD(0:1:5, [0.1,10.0], 1.0, 1.0, 1.0)
6-element Array{Float64,1}:
 0.0
 0.0001440103022493725
 1.446739954231315e-6
 2.7354735244571076e-8
 6.794070985483474e-10
 1.9657536202689858e-11
```
"""
function refl_DA_semiinf_TD(t, β::Array{Float64,1}, ρ::Float64, ndet::Float64, nmed::Float64)
    n::Float64 = nmed/ndet
    μa::Float64 = β[1]
    μsp::Float64 = β[2]
    D::Float64 = 1/3μsp
  	ν::Float64 = 29.9792345/nmed

    Rt = Array{Float64}(undef, length(t))

	if n > 1.0
		  A = 504.332889 - 2641.00214n + 5923.699064n^2 - 7376.355814n^3 +
		  5507.53041n^4 - 2463.357945n^5 + 610.956547n^6 - 64.8047n^7
	elseif n < 1.0
		  A = 3.084635 - 6.531194n + 8.357854n^2 - 5.082751n^3
	else
		  A = 1.0
	end

	zs::Float64 = 1/μsp
	ze::Float64 = 2A*D

	z3m::Float64 = - zs
    z4m::Float64 = 2ze +zs

    Threads.@threads for n in eachindex(t)

        Rt[n] = -(ρ^2/(4D*ν*t[n]))
        Rt[n] = Rt[n] - μa*ν*t[n]
        Rt[n] = -exp(Rt[n])/(2*(4π*D*ν)^(3/2)*sqrt(t[n]*t[n]*t[n]*t[n]*t[n]))
        Rt[n] = Rt[n]*(z3m*exp(-(z3m^2/(4D*ν*t[n]))) - z4m*exp(-(z4m^2/(4D*ν*t[n]))))

        if isnan(Rt[n])
            Rt[n] = 0
        end

    end

     return Rt
end

### TD Fluence ###
"""
    fluence_DA_semiinf_TD(t, β::Array{Float64,1}, ρ::Float64, ndet::Float64, nmed::Float64, z::Float64)

Compute the time-domain fluence in a semi-infinite medium (Eqn. 33 Contini).

# Arguments
- `t`: the time vector (ns).
- `β::Array{Float64,1}`: the optical properties [μa, μs'] (cm⁻¹)
- `ρ::Float64`: the source detector separation (cm⁻¹)
- `ndet::Float64`: the boundary's index of refraction (air or detector)
- `nmed::Float64`: the sample medium's index of refraction
- `z::Float64`: the z-depth in medium

# Examples
julia> fluence_DA_semiinf_TD(0:1:5, [0.1,10.0], 1.0, 1.0, 1.0, 1.0)
"""
function fluence_DA_semiinf_TD(t, β::Array{Float64,1}, ρ::Float64, ndet::Float64, nmed::Float64, z::Float64)
    n::Float64 = nmed/ndet
    μa::Float64 = β[1]
    μsp::Float64 = β[2]
    D::Float64 = 1/3μsp
  	ν::Float64 = 29.9792345/nmed

    Rt = Array{Float64}(undef, length(t))

	if n > 1.0
		  A = 504.332889 - 2641.00214n + 5923.699064n^2 - 7376.355814n^3 +
		  5507.53041n^4 - 2463.357945n^5 + 610.956547n^6 - 64.8047n^7
	elseif n < 1.0
		  A = 3.084635 - 6.531194n + 8.357854n^2 - 5.082751n^3
	else
		  A = 1.0
	end

	zs::Float64 = 1/μsp
	ze::Float64 = 2A*D

    Threads.@threads for n in eachindex(t)

        Rt[n] = -(ρ^2/(4D*ν*t[n]))
        Rt[n] = Rt[n] - μa*ν*t[n]
        Rt[n] = exp(Rt[n])/(2*(4π*D*ν*t[n])^(3/2))
        Rt[n] = Rt[n]*(exp(-((z - zs)^2/(4D*ν*t[n]))) - exp(-((z + 2*ze + zs)^2/(4D*ν*t[n]))))

        if isnan(Rt[n])
            Rt[n] = 0
        end

    end

     return Rt
end

### CW Fluence ###
"""
    fluence_DA_semiinf_CW(ρ::Float64, β::Array{Float64,1}, ndet::Float64, nmed::Float64, z::Float64)

Compute the steady-state fluence in a semi-infinite geometry according to Eqn. 3 of Kienle 1997.

# Arguments
- `ρ::Float64`: the source detector separation (cm⁻¹)
- `β::Array{Float64,1}`: the optical properties [μa, μs'] (cm⁻¹)
- `ndet::Float64`: the boundary's index of refraction (air or detector)
- `nmed::Float64`: the sample medium's index of refraction
- `z::Float64`: the z-depth within slab (cm)

# Examples
julia> fluence_DA_semiinf_CW(1.0, [0.1,10.0], 1.0,1.0, 0.0)
"""
function fluence_DA_semiinf_CW(ρ::Float64, β::Array{Float64,1}, ndet::Float64, nmed::Float64, z::Float64)
      n = nmed/ndet
      μa = β[1]
      μsp = β[2]
      D = 1/3μsp
      μeff = sqrt(3*μa*μsp)

	  ϕ = 0.0

		if n == 1.0
			A= 1.0
		elseif n > 1.0
			A = 504.332889 - 2641.00214n + 5923.699064n^2 - 7376.355814n^3 +
			5507.53041n^4 - 2463.357945n^5 + 610.956547n^6 - 64.8047n^7
		else
			A = 3.084635 - 6.531194n + 8.357854n^2 - 5.082751n^3
		end

		zs = 1/μsp
		ze = 2A*D

        ϕ += exp(-μeff*sqrt(ρ^2 + (z - zs)^2))/(sqrt(ρ^2 + (z - zs)^2))
        ϕ -= exp(-μeff*sqrt(ρ^2 + (z + 2*ze + zs)^2))/(sqrt(ρ^2 + (z + 2*ze + zs)^2))

        if isnan(ϕ)
            ϕ = 0
        end

	return ϕ/(4*π*D)
end

function fluence_DA_semiinf_FD(ρ::Float64, β::Array{Float64,1}, ndet::Float64, nmed::Float64, z::Float64)
    n = nmed/ndet
    ν = 29.9792345/nmed
    μa = β[1] + ω*im/ν
    μsp = β[2]
    D = 1/3μsp
    μeff = sqrt(3*μa*μsp)

    ϕ = 0.0

      if n == 1.0
          A= 1.0
      elseif n > 1.0
          A = 504.332889 - 2641.00214n + 5923.699064n^2 - 7376.355814n^3 +
          5507.53041n^4 - 2463.357945n^5 + 610.956547n^6 - 64.8047n^7
      else
          A = 3.084635 - 6.531194n + 8.357854n^2 - 5.082751n^3
      end

      zs = 1/μsp
      ze = 2A*D

      ϕ += exp(-μeff*sqrt(ρ^2 + (z - zs)^2))/(sqrt(ρ^2 + (z - zs)^2))
      ϕ -= exp(-μeff*sqrt(ρ^2 + (z + 2*ze + zs)^2))/(sqrt(ρ^2 + (z + 2*ze + zs)^2))

      if isnan(ϕ)
          ϕ = 0
      end

  return ϕ/(4*π*D)
end


using MathProgBase, GLPKMathProgInterface

import Base: rand,
             ∈

export VPolytope,
       vertices_list,
       convex_hull,
       cartesian_product,
       linear_map,
       remove_redundant_vertices

"""
    VPolytope{N<:Real} <: AbstractPolytope{N}

Type that represents a convex polytope in V-representation.

### Fields

- `vertices` -- the list of vertices
"""
struct VPolytope{N<:Real} <: AbstractPolytope{N}
    vertices::Vector{Vector{N}}
end

# constructor for a VPolytope with empty vertices list
VPolytope{N}() where {N<:Real} = VPolytope{N}(Vector{Vector{N}}())

# constructor for a VPolytope with no vertices of type Float64
VPolytope() = VPolytope{Float64}()

# constructor from a polygon in V-representation
function VPolytope(P::VPolygon, share::Bool=false)
    v = vertices_list(P)
    if !share
        v = copy(v)
    end
    return VPolytope(v)
end

# --- LazySet interface functions ---

"""
    dim(P::VPolytope)::Int

Return the dimension of a polytope in V-representation.

### Input

- `P`  -- polytope in V-representation

### Output

The ambient dimension of the polytope in V-representation.
If it is empty, the result is ``-1``.

### Examples

```jldoctest
julia> v = VPolytope();

julia> dim(v) > 0
false

julia> v = VPolytope([ones(3)])
VPolytope{Float64}(Array{Float64,1}[[1.0, 1.0, 1.0]])

julia> dim(v) == 3
true

```
"""
function dim(P::VPolytope)::Int
    return length(P.vertices) == 0 ? -1 : length(P.vertices[1])
end

"""
    σ(d::AbstractVector{N}, P::VPolytope{N}) where {N<:Real}

Return the support vector of a polytope in V-representation in a given
direction.

### Input

- `d` -- direction
- `P` -- polytope in V-representation

### Output

The support vector in the given direction.

### Algorithm

A support vector maximizes the support function.
For a polytope, the support function is always maximized in some vertex.
Hence it is sufficient to check all vertices.
"""
function σ(d::AbstractVector{N}, P::VPolytope{N}) where {N<:Real}
    # base cases
    m = length(P.vertices)
    if m == 0
        error("the support function for an empty polytope is undefined")
    elseif m == 1
        return P.vertices[1]
    end

    # evaluate support function in every vertex
    max_ρ = N(-Inf)
    max_idx = 0
    for (i, vi) in enumerate(P.vertices)
        ρ_i = dot(d, vi)
        if ρ_i > max_ρ
            max_ρ = ρ_i
            max_idx = i
        end
    end
    return P.vertices[max_idx]
end

"""
    ∈(x::AbstractVector{N}, P::VPolytope{N};
      solver=GLPKSolverLP(method=:Simplex))::Bool where {N<:Real}

Check whether a given point is contained in a polytope in vertex representation.

### Input

- `x` -- point/vector
- `P` -- polytope in vertex representation

### Output

`true` iff ``x ∈ P``.

### Algorithm

We check, using linear programming, the definition of a convex polytope that a
point is in the set if and only if it is a convex combination of the vertices.

Let ``\\{v_j\\}`` be the ``m`` vertices of `P`.
Then we solve the following ``m``-dimensional linear program.

```math
\\max 0 \\text{s.t.}
\bigwedge_{i=1}^n \\sum_{j=1}^m λ_j v_j[i] = x[i]
\\sum_{j=1}^m λ_j = 1
\bigwedge_{j=1}^m λ_j ≥ 0
```
"""
function ∈(x::AbstractVector{N}, P::VPolytope{N};
           solver=GLPKSolverLP(method=:Simplex))::Bool where {N<:Real}
    vertices = P.vertices
    m = length(vertices)

    # special cases: 0 or 1 vertex
    if m == 0
        return false
    elseif m == 1
        return x == vertices[1]
    end

    n = length(x)
    @assert n == dim(P) "a vector of length $(length(x)) cannot be " *
        "contained in a polytope of dimension $(dim(P))"

    A = Matrix{N}(undef, n+1, m)
    for j in 1:m
        v_j = vertices[j]
        # ⋀_i Σ_j λ_j v_j[i] = x[i]
        for i in 1:n
            A[i, j] = v_j[i]
        end
        # Σ_j λ_j = 1
        A[n+1, j] = one(N)
    end
    b = [x; one(N)]

    lbounds = zeros(N, m)
    ubounds = Inf
    sense = [fill('=', n); '<']
    obj = zeros(N, m)

    lp = linprog(obj, A, sense, b, lbounds, ubounds, solver)
    if lp.status == :Optimal
        return true
    elseif lp.status == :Infeasible
        return false
    end
    @assert false "LP returned status $(lp.status) unexpectedly"
end

"""
    rand(::Type{VPolytope}; [N]::Type{<:Real}=Float64, [dim]::Int=2,
         [rng]::AbstractRNG=GLOBAL_RNG, [seed]::Union{Int, Nothing}=nothing,
         [num_vertices]::Int=-1)::VPolytope{N}

Create a random polytope in vertex representation.

### Input

- `VPolytope`    -- type for dispatch
- `N`            -- (optional, default: `Float64`) numeric type
- `dim`          -- (optional, default: 2) dimension
- `rng`          -- (optional, default: `GLOBAL_RNG`) random number generator
- `seed`         -- (optional, default: `nothing`) seed for reseeding
- `num_vertices` -- (optional, default: `-1`) upper bound on the number of
                    vertices of the polytope (see comment below)

### Output

A random polytope in vertex representation.

### Algorithm

All numbers are normally distributed with mean 0 and standard deviation 1.

The number of vertices can be controlled with the argument `num_vertices`.
For a negative value we choose a random number in the range `dim:5*dim` (except
if `dim == 1`, in which case we choose in the range `1:2`).
Note that we do not guarantee that the vertices are not redundant.
"""
function rand(::Type{VPolytope};
              N::Type{<:Real}=Float64,
              dim::Int=2,
              rng::AbstractRNG=GLOBAL_RNG,
              seed::Union{Int, Nothing}=nothing,
              num_vertices::Int=-1
             )::VPolytope{N}
    rng = reseed(rng, seed)
    if num_vertices < 0
        num_vertices = (dim == 1) ? rand(1:2) : rand(dim:5*dim)
    end
    vertices = [randn(rng, N, dim) for i in 1:num_vertices]
    return VPolytope(vertices)
end

"""
    linear_map(M::AbstractMatrix{N}, P::VPolytope{N}) where {N<:Real}

Concrete linear map of a polytope in vertex representation.

### Input

- `M` -- matrix
- `P` -- polytope in vertex representation

### Output

A polytope in vertex representation.

### Algorithm

The linear map ``M`` is applied to each vertex of the given set ``P``, obtaining
a polytope in V-representation. The output type is again a `VPolytope`.
"""
function linear_map(M::AbstractMatrix{N}, P::VPolytope{N}) where {N<:Real}
    @assert dim(P) == size(M, 2) "a linear map of size $(size(M)) cannot be applied to a set of dimension $(dim(P))"
    return _linear_map_vrep(M, P)
end

@inline function _linear_map_vrep(M::AbstractMatrix{N}, P::VPolytope{N}) where {N<:Real}
    return broadcast(v -> M * v, vertices_list(P)) |> VPolytope{N}
end

"""
    translate(P::VPolytope{N}, v::AbstractVector{N}) where {N<:Real}

Translate (i.e., shift) a polytope in vertex representation by a given vector.

### Input

- `P` -- polytope in vertex representation
- `v` -- translation vector

### Output

A translated polytope in vertex representation.

### Algorithm

We add the vector to each vertex of the polytope.
"""
function translate(P::VPolytope{N}, v::AbstractVector{N}) where {N<:Real}
    @assert length(v) == dim(P) "cannot translate a $(dim(P))-dimensional " *
                                "set by a $(length(v))-dimensional vector"
    return VPolytope([x + v for x in vertices_list(P)])
end

# --- AbstractPolytope interface functions ---

"""
    vertices_list(P::VPolytope{N})::Vector{Vector{N}} where {N<:Real}

Return the list of vertices of a polytope in V-representation.

### Input

- `P` -- polytope in vertex representation

### Output

List of vertices.
"""
function vertices_list(P::VPolytope{N})::Vector{Vector{N}} where {N<:Real}
    return P.vertices
end

"""
    constraints_list(P::VPolytope{N}) where {N<:Real}

Return the list of constraints defining a polytope in V-representation.

### Input

- `P` -- polytope in V-representation

### Output

The list of constraints of the polytope.

### Algorithm

First the H-representation of ``P`` is computed, then its list of constraints
is returned.
"""
function constraints_list(P::VPolytope{N}) where {N<:Real}
    return constraints_list(tohrep(P))
end

"""
    convex_hull(P1::VPolytope{N}, P2::VPolytope{N};
                [backend]=default_polyhedra_backend(P1, N)) where {N<:Real}

Compute the convex hull of the set union of two polytopes in V-representation.

### Input

- `P1`         -- polytope
- `P2`         -- another polytope
- `backend`    -- (optional, default: `nothing`) the polyhedral
                  computations backend

### Output

The `VPolytope` obtained by the concrete convex hull of `P1` and `P2`.

### Notes

This function takes the union of the vertices of each polytope and then relies
on a concrete convex hull algorithm. For low dimensions, a specialized implementation
for polygons is used. For higher dimensions, `convex_hull` relies on the polyhedral
computations backend that can be specified using the `backend` keyword argument.

For performance reasons, it is suggested to use the `CDDLib.Library()` backend
for the `convex_hull`.
"""
function convex_hull(P1::VPolytope{N}, P2::VPolytope{N};
                     backend=nothing) where {N<:Real}
    vunion = [P1.vertices; P2.vertices]
    convex_hull!(vunion; backend=backend)
    return VPolytope(vunion)
end

"""
    cartesian_product(P1::VPolytope{N}, P2::VPolytope{N};
                      [backend]=default_polyhedra_backend(P1, N)) where {N}

Compute the Cartesian product of two polytopes in V-representation.

### Input

- `P1`         -- polytope
- `P2`         -- another polytope
- `backend`    -- (optional, default: `default_polyhedra_backend(P1, N)`) the polyhedral
                  computations backend, see
                  [Polyhedra's documentation](https://juliapolyhedra.github.io/Polyhedra.jl/latest/installation.html#Getting-Libraries-1)
                  for further information

### Output

The `VPolytope` obtained by the concrete Cartesian product of `P1` and `P2`.
"""
function cartesian_product(P1::VPolytope{N}, P2::VPolytope{N};
                           backend=default_polyhedra_backend(P1, N)) where {N}
    require(:Polyhedra; fun_name="cartesian_product")
    Pcp = vcartesianproduct(polyhedron(P1; backend=backend),
                            polyhedron(P2; backend=backend))
    return VPolytope(Pcp)
end

"""
    remove_redundant_vertices(P::VPolytope{N};
                              [backend]=nothing,
                              [solver]=nothing)::VPolytope{N} where {N<:Real}

Return the polytope obtained by removing the redundant vertices of the given polytope.

### Input

- `P`       -- polytope in vertex representation
- `backend` -- (optional, default: `nothing`) the polyhedral
               computations backend, see `default_polyhedra_backend(P1, N)` or
               [Polyhedra's documentation](https://juliapolyhedra.github.io/Polyhedra.jl/latest/installation.html#Getting-Libraries-1)
               for further information on the available backends
- `solver`  -- (optional, default: `nothing`) the linear programming
               solver used in the backend, if needed; see `default_lp_solver(N)`

### Output

A new polytope such that its vertices are the convex hull of the given polytope.

### Notes

The optimization problem associated to removing redundant vertices is handled
by `Polyhedra`. If the polyhedral computations backend requires an LP solver but
it has not been set, we use `default_lp_solver(N)` to define such solver.
Otherwise, the redundancy removal function of the polyhedral backend is used.
"""
function remove_redundant_vertices(P::VPolytope{N};
                                   backend=nothing,
                                   solver=nothing)::VPolytope{N} where {N<:Real}
    require(:Polyhedra; fun_name="remove_redundant_vertices")
    if backend == nothing
        backend = default_polyhedra_backend(P, N)
    end
    Q = polyhedron(P; backend=backend)
    if Polyhedra.supportssolver(typeof(Q))
        if solver == nothing
            solver = default_lp_solver(N)
        end
        vQ = Polyhedra.vrep(Q)
        Polyhedra.setvrep!(Q, Polyhedra.removevredundancy(vQ, solver))
    else
        Polyhedra.removevredundancy!(Q)
    end
    return VPolytope(Q)
end

"""
    tohrep(P::VPolytope{N};
           [backend]=default_polyhedra_backend(P, N)) where {N<:Real}

Transform a polytope in V-representation to a polytope in H-representation.

### Input

- `P`       -- polytope in vertex representation
- `backend` -- (optional, default: `default_polyhedra_backend(P, N)`) the
               backend for polyhedral computations

### Output

The `HPolytope` which is the constraint representation of the given polytope
in vertex representation.

### Notes

The conversion may not preserve the numeric type (e.g., with `N == Float32`)
depending on the backend.
For further information on the supported backends see
[Polyhedra's documentation](https://juliapolyhedra.github.io/Polyhedra.jl/latest/installation.html#Getting-Libraries-1).
"""
function tohrep(P::VPolytope{N};
                backend=default_polyhedra_backend(P, N)) where {N<:Real}
    vl = P.vertices
    if isempty(vl)
        return EmptySet{N}()
    end
    require(:Polyhedra; fun_name="tohrep")
    return HPolytope(polyhedron(P; backend=backend))
end

"""
    tovrep(P::VPolytope)

Return a vertex representation of the given polytope in vertex
representation (no-op).

### Input

- `P` -- polytope in vertex representation

### Output

The same polytope instance.
"""
function tovrep(P::VPolytope)
    return P
end

# ==========================================
# Lower level methods that use Polyhedra.jl
# ==========================================

function load_polyhedra_vpolytope() # function to be loaded by Requires
return quote
# see the interface file AbstractPolytope.jl for the imports

export vertices_list,
       tohrep,
       tovrep

# VPolytope from a VRep
function VPolytope(P::VRep{N}) where {N}
    vertices = Vector{Vector{N}}()
    for vi in Polyhedra.points(P)
        push!(vertices, vi)
    end
    return VPolytope(vertices)
end

"""
    polyhedron(P::VPolytope{N};
               [backend]=default_polyhedra_backend(P, N)) where {N<:Real}

Return an `VRep` polyhedron from `Polyhedra.jl` given a polytope in V-representation.

### Input

- `P`       -- polytope
- `backend` -- (optional, default: `default_polyhedra_backend(P, N)`) the polyhedral
               computations backend, see [Polyhedra's documentation](https://juliapolyhedra.github.io/Polyhedra.jl/latest/installation.html#Getting-Libraries-1)
               for further information
- `relative_dimension` -- (default, optional: `nothing`) an integer representing
                          the (relative) dimension of the polytope; this argument
                          is mandatory if the polytope is empty

### Output

A `VRep` polyhedron.

### Notes

The *relative dimension* (or just *dimension*) refers to the dimension of the set
relative to itself, independently of the ambient dimension. For example, a point
has (relative) dimension zero, and a line segment has (relative) dimension one.

In this library, `LazySets.dim` always returns the ambient dimension of the set,
such that a line segment in two dimensions has dimension two. However,
`Polyhedra.dim` will assign a dimension equal to one to a line segment
because it uses a different convention.
"""
function polyhedron(P::VPolytope{N};
                    backend=default_polyhedra_backend(P, N),
                    relative_dimension=nothing) where {N<:Real}
    if isempty(P)
        if relative_dimension == nothing
            error("the conversion to a `Polyhedra.polyhedron` requires the (relative) dimension " *
                  "of the `VPolytope` to be known, but it cannot be inferred from an empty set; " *
                  "try passing it in the keyword argument `relative_dimension`")
        end
        return polyhedron(Polyhedra.vrep(P.vertices, d=relative_dimension), backend)
    end
    return polyhedron(Polyhedra.vrep(P.vertices), backend)
end

end # quote
end # function load_polyhedra_vpolytope()


using Test
using ClimateMachine.MoistThermodynamics
using NCDatasets
using Random
using RootSolvers
MT = MoistThermodynamics
using LinearAlgebra

using CLIMAParameters
using CLIMAParameters.Planet

struct EarthParameterSet <: AbstractEarthParameterSet end
const param_set = EarthParameterSet()

# Tolerances for tested quantities:
const atol_temperature = 1e-1
const atol_pressure = MSLP(param_set) * 2e-2
const atol_energy = 1e-1
const rtol_temperature = 1e-1
const rtol_pressure = 1e-1
const rtol_energy = 1e-1

float_types = [Float32, Float64]

include("profiles.jl")
include("data_tests.jl")

@testset "moist thermodynamics - isentropic processes" begin
    for FT in [Float64]
        _R_d = FT(R_d(param_set))
        _molmass_ratio = FT(molmass_ratio(param_set))
        _cp_d = FT(cp_d(param_set))
        _cp_v = FT(cp_v(param_set))
        _cp_l = FT(cp_l(param_set))
        _cv_d = FT(cv_d(param_set))
        _cv_v = FT(cv_v(param_set))
        _cv_l = FT(cv_l(param_set))
        _cv_i = FT(cv_i(param_set))
        _T_0 = FT(T_0(param_set))
        _e_int_v0 = FT(e_int_v0(param_set))
        _e_int_i0 = FT(e_int_i0(param_set))
        _LH_v0 = FT(LH_v0(param_set))
        _LH_s0 = FT(LH_s0(param_set))
        _cp_i = FT(cp_i(param_set))
        _LH_f0 = FT(LH_f0(param_set))
        _press_triple = FT(press_triple(param_set))
        _R_v = FT(R_v(param_set))
        _T_triple = FT(T_triple(param_set))
        _T_freeze = FT(T_freeze(param_set))
        _T_min = FT(T_min(param_set))
        _MSLP = FT(MSLP(param_set))
        _T_max = FT(T_max(param_set))
        _kappa_d = FT(kappa_d(param_set))

        # for FT in float_types
        z, e_int, ρ, q_tot, q_pt, T, p, θ_liq_ice =
            tested_profiles(param_set, 50, FT)
        Φ = FT(1)
        Random.seed!(15)
        perturbation = FT(0.1) * rand(length(T))

        # TODO: Use reasonable values for ambient temperature/pressure
        T∞, p∞ = T .* perturbation, p .* perturbation
        @test air_temperature.(
            Ref(param_set),
            p,
            θ_liq_ice,
            Ref(DryAdiabaticProcess()),
        ) ≈ (p ./ _MSLP) .^ (_R_d / _cp_d) .* θ_liq_ice
        @test air_pressure_given_θ.(
            Ref(param_set),
            θ_liq_ice,
            Φ,
            Ref(DryAdiabaticProcess()),
        ) ≈ _MSLP .* (1 .- Φ ./ (θ_liq_ice .* _cp_d)) .^ (_cp_d / _R_d)
        @test air_pressure.(
            Ref(param_set),
            T,
            T∞,
            p∞,
            Ref(DryAdiabaticProcess()),
        ) ≈ p∞ .* (T ./ T∞) .^ (FT(1) / _kappa_d)
    end
end

@testset "moist thermodynamics - correctness" begin
    FT = Float64
    _R_d = FT(R_d(param_set))
    _molmass_ratio = FT(molmass_ratio(param_set))
    _cp_d = FT(cp_d(param_set))
    _cp_v = FT(cp_v(param_set))
    _cp_l = FT(cp_l(param_set))
    _cv_d = FT(cv_d(param_set))
    _cv_v = FT(cv_v(param_set))
    _cv_l = FT(cv_l(param_set))
    _cv_i = FT(cv_i(param_set))
    _T_0 = FT(T_0(param_set))
    _e_int_v0 = FT(e_int_v0(param_set))
    _e_int_i0 = FT(e_int_i0(param_set))
    _LH_v0 = FT(LH_v0(param_set))
    _LH_s0 = FT(LH_s0(param_set))
    _cp_i = FT(cp_i(param_set))
    _LH_f0 = FT(LH_f0(param_set))
    _press_triple = FT(press_triple(param_set))
    _R_v = FT(R_v(param_set))
    _T_triple = FT(T_triple(param_set))
    _T_freeze = FT(T_freeze(param_set))
    _T_min = FT(T_min(param_set))
    _MSLP = FT(MSLP(param_set))
    _T_max = FT(T_max(param_set))
    _kappa_d = FT(kappa_d(param_set))
    _T_icenuc = FT(T_icenuc(param_set))

    # ideal gas law
    @test air_pressure(param_set, FT(1), FT(1), PhasePartition(FT(1))) === _R_v
    @test air_pressure(
        param_set,
        FT(1),
        FT(2),
        PhasePartition(FT(1), FT(0.5), FT(0)),
    ) === _R_v
    @test air_pressure(param_set, FT(1), FT(1)) === _R_d
    @test air_pressure(param_set, FT(1), FT(2)) === 2 * _R_d
    @test air_density(param_set, FT(1), FT(1)) === 1 / _R_d
    @test air_density(param_set, FT(1), FT(2)) === 2 / _R_d

    # gas constants and heat capacities
    @test gas_constant_air(param_set, PhasePartition(FT(0))) === _R_d
    @test gas_constant_air(param_set, PhasePartition(FT(1))) === _R_v
    @test gas_constant_air(param_set, PhasePartition(FT(0.5), FT(0.5))) ≈
          _R_d / 2
    @test gas_constant_air(param_set, FT) == _R_d

    @test cp_m(param_set, PhasePartition(FT(0))) === _cp_d
    @test cp_m(param_set, PhasePartition(FT(1))) === _cp_v
    @test cp_m(param_set, PhasePartition(FT(1), FT(1))) === _cp_l
    @test cp_m(param_set, PhasePartition(FT(1), FT(0), FT(1))) === _cp_i
    @test cp_m(param_set, FT) == _cp_d

    @test cv_m(param_set, PhasePartition(FT(0))) === _cp_d - _R_d
    @test cv_m(param_set, PhasePartition(FT(1))) === _cp_v - _R_v
    @test cv_m(param_set, PhasePartition(FT(1), FT(1))) === _cv_l
    @test cv_m(param_set, PhasePartition(FT(1), FT(0), FT(1))) === _cv_i
    @test cv_m(param_set, FT) == _cv_d

    # speed of sound
    @test soundspeed_air(param_set, _T_0 + 20, PhasePartition(FT(0))) ==
          sqrt(_cp_d / _cv_d * _R_d * (_T_0 + 20))
    @test soundspeed_air(param_set, _T_0 + 100, PhasePartition(FT(1))) ==
          sqrt(_cp_v / _cv_v * _R_v * (_T_0 + 100))

    # specific latent heats
    @test latent_heat_vapor(param_set, _T_0) ≈ _LH_v0
    @test latent_heat_fusion(param_set, _T_0) ≈ _LH_f0
    @test latent_heat_sublim(param_set, _T_0) ≈ _LH_s0

    # saturation vapor pressure and specific humidity
    p = FT(1.e5)
    q_tot = FT(0.23)
    ρ = FT(1.0)
    ρ_v_triple = _press_triple / _R_v / _T_triple
    @test saturation_vapor_pressure(param_set, _T_triple, Liquid()) ≈
          _press_triple
    @test saturation_vapor_pressure(param_set, _T_triple, Ice()) ≈ _press_triple

    @test q_vap_saturation(param_set, _T_triple, ρ, PhasePartition(FT(0))) ==
          ρ_v_triple / ρ
    @test q_vap_saturation(
        param_set,
        _T_triple,
        ρ,
        PhasePartition(q_tot, q_tot),
    ) == ρ_v_triple / ρ

    @test q_vap_saturation_generic(param_set, _T_triple, ρ; phase = Liquid()) ==
          ρ_v_triple / ρ
    @test q_vap_saturation_generic(param_set, _T_triple, ρ; phase = Ice()) ==
          ρ_v_triple / ρ
    @test q_vap_saturation_generic(
        param_set,
        _T_triple - 20,
        ρ;
        phase = Liquid(),
    ) >= q_vap_saturation_generic(param_set, _T_triple - 20, ρ; phase = Ice())

    @test saturation_excess(param_set, _T_triple, ρ, PhasePartition(q_tot)) ==
          q_tot - ρ_v_triple / ρ
    @test saturation_excess(
        param_set,
        _T_triple,
        ρ,
        PhasePartition(q_tot / 1000),
    ) == 0.0

    # energy functions and inverse (temperature)
    T = FT(300)
    e_kin = FT(11)
    e_pot = FT(13)
    @test air_temperature(param_set, _cv_d * (T - _T_0)) === FT(T)
    @test air_temperature(
        param_set,
        _cv_d * (T - _T_0),
        PhasePartition(FT(0)),
    ) === FT(T)

    @test air_temperature(
        param_set,
        cv_m(param_set, PhasePartition(FT(0))) * (T - _T_0),
        PhasePartition(FT(0)),
    ) === FT(T)
    @test air_temperature(
        param_set,
        cv_m(param_set, PhasePartition(FT(q_tot))) * (T - _T_0) +
        q_tot * _e_int_v0,
        PhasePartition(q_tot),
    ) ≈ FT(T)

    @test total_energy(param_set, FT(e_kin), FT(e_pot), _T_0) ===
          FT(e_kin) + FT(e_pot)
    @test total_energy(param_set, FT(e_kin), FT(e_pot), FT(T)) ≈
          FT(e_kin) + FT(e_pot) + _cv_d * (T - _T_0)
    @test total_energy(param_set, FT(0), FT(0), _T_0, PhasePartition(q_tot)) ≈
          q_tot * _e_int_v0

    # phase partitioning in equilibrium
    q_liq = FT(0.1)
    T = FT(_T_icenuc - 10)
    ρ = FT(1.0)
    q_tot = FT(0.21)
    @test liquid_fraction(param_set, T) === FT(0)
    @test liquid_fraction(param_set, T, PhasePartition(q_tot, q_liq, q_liq)) ===
          FT(0.5)
    q = PhasePartition_equil(param_set, T, ρ, q_tot)
    @test q.liq ≈ FT(0)
    @test 0 < q.ice <= q_tot

    T = FT(_T_freeze + 10)
    ρ = FT(0.1)
    q_tot = FT(0.60)
    @test liquid_fraction(param_set, T) === FT(1)
    @test liquid_fraction(
        param_set,
        T,
        PhasePartition(q_tot, q_liq, q_liq / 2),
    ) === FT(2 / 3)
    q = PhasePartition_equil(param_set, T, ρ, q_tot)
    @test 0 < q.liq <= q_tot
    @test q.ice ≈ 0

    # saturation adjustment in equilibrium (i.e., given the thermodynamic
    # variables E_int, p, q_tot, compute the temperature and partitioning of the phases
    tol_T = 1e-1
    q_tot = FT(0)
    ρ = FT(1)
    @test MT.saturation_adjustment_SecantMethod(
        param_set,
        internal_energy_sat(param_set, 300.0, ρ, q_tot),
        ρ,
        q_tot,
        10,
        ResidualTolerance(1e-2),
    ) ≈ 300.0
    @test abs(
        MT.saturation_adjustment(
            param_set,
            internal_energy_sat(param_set, 300.0, ρ, q_tot),
            ρ,
            q_tot,
            10,
            ResidualTolerance(1e-2),
        ) - 300.0,
    ) < tol_T

    q_tot = FT(0.21)
    ρ = FT(0.1)
    @test MT.saturation_adjustment_SecantMethod(
        param_set,
        internal_energy_sat(param_set, 200.0, ρ, q_tot),
        ρ,
        q_tot,
        10,
        ResidualTolerance(1e-2),
    ) ≈ 200.0
    @test abs(
        MT.saturation_adjustment(
            param_set,
            internal_energy_sat(param_set, 200.0, ρ, q_tot),
            ρ,
            q_tot,
            10,
            ResidualTolerance(1e-2),
        ) - 200.0,
    ) < tol_T
    q = PhasePartition_equil(param_set, T, ρ, q_tot)
    @test q.tot - q.liq - q.ice ≈
          vapor_specific_humidity(q) ≈
          q_vap_saturation(param_set, T, ρ)

    ρ = FT(1)
    ρu = FT[1, 2, 3]
    ρe = FT(1100)
    e_pot = FT(93)
    @test internal_energy(ρ, ρe, ρu, e_pot) ≈ 1000.0

    # potential temperatures
    T = FT(300)
    @test liquid_ice_pottemp_given_pressure(param_set, T, _MSLP) === T
    @test liquid_ice_pottemp_given_pressure(param_set, T, _MSLP / 10) ≈
          T * 10^(_R_d / _cp_d)
    @test liquid_ice_pottemp_given_pressure(
        param_set,
        T,
        _MSLP / 10,
        PhasePartition(FT(1)),
    ) ≈ T * 10^(_R_v / _cp_v)

    # dry potential temperatures. FIXME: add correctness tests
    T = FT(300)
    p = FT(1.e5)
    q_tot = FT(0.23)
    @test dry_pottemp_given_pressure(param_set, T, p, PhasePartition(q_tot)) isa
          typeof(p)
    @test air_temperature_from_liquid_ice_pottemp_given_pressure(
        param_set,
        dry_pottemp_given_pressure(param_set, T, p, PhasePartition(q_tot)),
        p,
        PhasePartition(q_tot),
    ) ≈ T

    # Exner function. FIXME: add correctness tests
    p = FT(1.e5)
    q_tot = FT(0.23)
    @test exner_given_pressure(param_set, p, PhasePartition(q_tot)) isa
          typeof(p)
end

@testset "moist thermodynamics - default behavior accuracy" begin
    # Input arguments should be accurate within machine precision
    # Temperature is approximated via saturation adjustment, and should be within a physical tolerance

    for FT in float_types
        rtol = FT(1e-2)
        z, e_int, ρ, q_tot, q_pt, T, p, θ_liq_ice =
            tested_profiles(param_set, 50, FT)

        # PhaseEquil
        ts_exact = PhaseEquil.(Ref(param_set), e_int, ρ, q_tot, 100, FT(1e-3))
        ts = PhaseEquil.(Ref(param_set), e_int, ρ, q_tot)
        # Should be machine accurate (because ts contains `e_int`,`ρ`,`q_tot`):
        @test all(
            getproperty.(PhasePartition.(ts), :tot) .≈
            getproperty.(PhasePartition.(ts_exact), :tot),
        )
        @test all(internal_energy.(ts) .≈ internal_energy.(ts_exact))
        @test all(air_density.(ts) .≈ air_density.(ts_exact))
        # Approximate (temperature must be computed via saturation adjustment):
        @test all(isapprox.(
            air_pressure.(ts),
            air_pressure.(ts_exact),
            rtol = rtol,
        ))
        @test all(isapprox.(
            air_temperature.(ts),
            air_temperature.(ts_exact),
            rtol = rtol,
        ))

        # PhaseEquil
        ts_exact =
            PhaseEquil.(
                Ref(param_set),
                e_int,
                ρ,
                q_tot,
                100,
                FT(1e-3),
                MT.saturation_adjustment_SecantMethod,
            )
        ts =
            PhaseEquil.(
                Ref(param_set),
                e_int,
                ρ,
                q_tot,
                35,
                FT(1e-1),
                MT.saturation_adjustment_SecantMethod,
            ) # Needs to be in sync with default
        # Should be machine accurate (because ts contains `e_int`,`ρ`,`q_tot`):
        @test all(
            getproperty.(PhasePartition.(ts), :tot) .≈
            getproperty.(PhasePartition.(ts_exact), :tot),
        )
        @test all(internal_energy.(ts) .≈ internal_energy.(ts_exact))
        @test all(air_density.(ts) .≈ air_density.(ts_exact))
        # Approximate (temperature must be computed via saturation adjustment):
        @test all(isapprox.(
            air_pressure.(ts),
            air_pressure.(ts_exact),
            rtol = rtol,
        ))
        @test all(isapprox.(
            air_temperature.(ts),
            air_temperature.(ts_exact),
            rtol = rtol,
        ))

        # LiquidIcePotTempSHumEquil
        ts_exact =
            LiquidIcePotTempSHumEquil.(
                Ref(param_set),
                θ_liq_ice,
                ρ,
                q_tot,
                40,
                FT(1e-3),
            )
        ts = LiquidIcePotTempSHumEquil.(Ref(param_set), θ_liq_ice, ρ, q_tot)
        # Should be machine accurate:
        @test all(air_density.(ts) .≈ air_density.(ts_exact))
        @test all(
            getproperty.(PhasePartition.(ts), :tot) .≈
            getproperty.(PhasePartition.(ts_exact), :tot),
        )
        # Approximate (temperature must be computed via saturation adjustment):
        @test all(isapprox.(
            internal_energy.(ts),
            internal_energy.(ts_exact),
            rtol = 10 * rtol,
        ))
        @test all(isapprox.(
            liquid_ice_pottemp.(ts),
            liquid_ice_pottemp.(ts_exact),
            rtol = rtol,
        ))
        @test all(isapprox.(
            air_temperature.(ts),
            air_temperature.(ts_exact),
            rtol = rtol,
        ))

        # LiquidIcePotTempSHumEquil_given_pressure
        ts_exact =
            LiquidIcePotTempSHumEquil_given_pressure.(
                Ref(param_set),
                θ_liq_ice,
                p,
                q_tot,
                40,
                FT(1e-3),
            )
        ts =
            LiquidIcePotTempSHumEquil_given_pressure.(
                Ref(param_set),
                θ_liq_ice,
                p,
                q_tot,
            )
        # Should be machine accurate:
        @test all(
            getproperty.(PhasePartition.(ts), :tot) .≈
            getproperty.(PhasePartition.(ts_exact), :tot),
        )
        # Approximate (temperature must be computed via saturation adjustment):
        @test all(isapprox.(
            air_density.(ts),
            air_density.(ts_exact),
            rtol = rtol,
        ))
        @test all(isapprox.(
            internal_energy.(ts),
            internal_energy.(ts_exact),
            rtol = rtol,
        ))
        @test all(isapprox.(
            liquid_ice_pottemp.(ts),
            liquid_ice_pottemp.(ts_exact),
            rtol = rtol,
        ))
        @test all(isapprox.(
            air_temperature.(ts),
            air_temperature.(ts_exact),
            rtol = rtol,
        ))

        # LiquidIcePotTempSHumNonEquil
        ts_exact =
            LiquidIcePotTempSHumNonEquil.(
                Ref(param_set),
                θ_liq_ice,
                ρ,
                q_pt,
                40,
                FT(1e-3),
            )
        ts = LiquidIcePotTempSHumNonEquil.(Ref(param_set), θ_liq_ice, ρ, q_pt)
        # Should be machine accurate:
        @test all(
            getproperty.(PhasePartition.(ts), :tot) .≈
            getproperty.(PhasePartition.(ts_exact), :tot),
        )
        @test all(
            getproperty.(PhasePartition.(ts), :liq) .≈
            getproperty.(PhasePartition.(ts_exact), :liq),
        )
        @test all(
            getproperty.(PhasePartition.(ts), :ice) .≈
            getproperty.(PhasePartition.(ts_exact), :ice),
        )
        @test all(air_density.(ts) .≈ air_density.(ts_exact))
        # Approximate (temperature must be computed via non-linear solve):
        @test all(isapprox.(
            internal_energy.(ts),
            internal_energy.(ts_exact),
            rtol = rtol,
        ))
        @test all(isapprox.(
            liquid_ice_pottemp.(ts),
            liquid_ice_pottemp.(ts_exact),
            rtol = rtol,
        ))
        @test all(isapprox.(
            air_temperature.(ts),
            air_temperature.(ts_exact),
            rtol = rtol,
        ))

    end

end

@testset "moist thermodynamics - constructor consistency" begin

    # Make sure `ThermodynamicState` arguments are returned unchanged

    for FT in float_types
        rtol = FT(1e-2)

        _MSLP = FT(MSLP(param_set))

        z, e_int, ρ, q_tot, q_pt, T, p, θ_liq_ice =
            tested_profiles(param_set, 50, FT)

        # PhaseDry
        ts = PhaseDry.(Ref(param_set), e_int, ρ)
        @test all(internal_energy.(ts) .≈ e_int)
        @test all(air_density.(ts) .≈ ρ)

        ts = PhaseDry_given_pT.(Ref(param_set), p, T)
        @test all(internal_energy.(ts) .≈ internal_energy.(Ref(param_set), T))
        @test all(air_density.(ts) .≈ ρ)

        # PhaseEquil
        ts =
            PhaseEquil.(
                Ref(param_set),
                e_int,
                ρ,
                q_tot,
                40,
                FT(1e-1),
                Ref(MT.saturation_adjustment_SecantMethod),
            )
        @test all(internal_energy.(ts) .≈ e_int)
        @test all(getproperty.(PhasePartition.(ts), :tot) .≈ q_tot)
        @test all(air_density.(ts) .≈ ρ)

        ts = PhaseEquil.(Ref(param_set), e_int, ρ, q_tot)
        @test all(internal_energy.(ts) .≈ e_int)
        @test all(getproperty.(PhasePartition.(ts), :tot) .≈ q_tot)
        @test all(air_density.(ts) .≈ ρ)

        # PhaseNonEquil
        ts = PhaseNonEquil.(Ref(param_set), e_int, ρ, q_pt)
        @test all(internal_energy.(ts) .≈ e_int)
        @test all(
            getproperty.(PhasePartition.(ts), :tot) .≈ getproperty.(q_pt, :tot),
        )
        @test all(
            getproperty.(PhasePartition.(ts), :liq) .≈ getproperty.(q_pt, :liq),
        )
        @test all(
            getproperty.(PhasePartition.(ts), :ice) .≈ getproperty.(q_pt, :ice),
        )
        @test all(air_density.(ts) .≈ ρ)

        # air_temperature_from_liquid_ice_pottemp_given_pressure-liquid_ice_pottemp inverse
        θ_liq_ice_ =
            liquid_ice_pottemp_given_pressure.(Ref(param_set), T, p, q_pt)
        @test all(
            air_temperature_from_liquid_ice_pottemp_given_pressure.(
                Ref(param_set),
                θ_liq_ice_,
                p,
                q_pt,
            ) .≈ T,
        )

        # liquid_ice_pottemp-air_temperature_from_liquid_ice_pottemp_given_pressure inverse
        T =
            air_temperature_from_liquid_ice_pottemp_given_pressure.(
                Ref(param_set),
                θ_liq_ice,
                p,
                q_pt,
            )
        @test all(
            liquid_ice_pottemp_given_pressure.(Ref(param_set), T, p, q_pt) .≈
            θ_liq_ice,
        )

        # Accurate but expensive `LiquidIcePotTempSHumNonEquil` constructor (Non-linear temperature from θ_liq_ice)
        T_non_linear =
            air_temperature_from_liquid_ice_pottemp_non_linear.(
                Ref(param_set),
                θ_liq_ice,
                ρ,
                20,
                ResidualTolerance(FT(5e-5)),
                q_pt,
            )
        T_expansion =
            air_temperature_from_liquid_ice_pottemp.(
                Ref(param_set),
                θ_liq_ice,
                ρ,
                q_pt,
            )
        @test all(isapprox.(T_non_linear, T_expansion, rtol = rtol))
        e_int_ = internal_energy.(Ref(param_set), T_non_linear, q_pt)
        ts = PhaseNonEquil.(Ref(param_set), e_int_, ρ, q_pt)
        @test all(T_non_linear .≈ air_temperature.(ts))
        @test all(isapprox(θ_liq_ice, liquid_ice_pottemp.(ts), rtol = rtol))

        # LiquidIcePotTempSHumEquil
        ts =
            LiquidIcePotTempSHumEquil.(
                Ref(param_set),
                θ_liq_ice,
                ρ,
                q_tot,
                40,
                FT(1e-3),
            )
        @test all(isapprox.(liquid_ice_pottemp.(ts), θ_liq_ice, atol = 1e-1))
        @test all(isapprox.(air_density.(ts), ρ, rtol = rtol))
        @test all(getproperty.(PhasePartition.(ts), :tot) .≈ q_tot)

        # The LiquidIcePotTempSHumEquil_given_pressure constructor
        # passes the consistency test within sufficient physical precision,
        # however, it fails to satisfy the consistency test within machine
        # precision for the input pressure.

        # LiquidIcePotTempSHumEquil_given_pressure
        ts =
            LiquidIcePotTempSHumEquil_given_pressure.(
                Ref(param_set),
                θ_liq_ice,
                p,
                q_tot,
                35,
                FT(1e-3),
            )
        @test all(isapprox.(liquid_ice_pottemp.(ts), θ_liq_ice, atol = 1e-1))
        @test all(
            getproperty.(PhasePartition.(ts), :tot) .≈ getproperty.(q_pt, :tot),
        )
        @test all(isapprox.(air_pressure.(ts), p, atol = _MSLP * 2e-2))

        # LiquidIcePotTempSHumNonEquil_given_pressure
        ts =
            LiquidIcePotTempSHumNonEquil_given_pressure.(
                Ref(param_set),
                θ_liq_ice,
                p,
                q_pt,
            )
        @test all(liquid_ice_pottemp.(ts) .≈ θ_liq_ice)
        @test all(air_pressure.(ts) .≈ p)
        @test all(
            getproperty.(PhasePartition.(ts), :tot) .≈ getproperty.(q_pt, :tot),
        )
        @test all(
            getproperty.(PhasePartition.(ts), :liq) .≈ getproperty.(q_pt, :liq),
        )
        @test all(
            getproperty.(PhasePartition.(ts), :ice) .≈ getproperty.(q_pt, :ice),
        )

        # LiquidIcePotTempSHumNonEquil
        ts =
            LiquidIcePotTempSHumNonEquil.(
                Ref(param_set),
                θ_liq_ice,
                ρ,
                q_pt,
                5,
                FT(1e-3),
            )
        @test all(isapprox.(θ_liq_ice, liquid_ice_pottemp.(ts), rtol = rtol))
        @test all(air_density.(ts) .≈ ρ)
        @test all(
            getproperty.(PhasePartition.(ts), :tot) .≈ getproperty.(q_pt, :tot),
        )
        @test all(
            getproperty.(PhasePartition.(ts), :liq) .≈ getproperty.(q_pt, :liq),
        )
        @test all(
            getproperty.(PhasePartition.(ts), :ice) .≈ getproperty.(q_pt, :ice),
        )
    end

end

@testset "moist thermodynamics - type-stability" begin

    # NOTE: `Float32` saturation adjustment tends to have more difficulty
    # with converging to the same tolerances as `Float64`, so they're relaxed here.
    FT = Float32
    z, e_int, ρ, q_tot, q_pt, T, p, θ_liq_ice =
        tested_profiles(param_set, 50, FT)

    ρu = FT[1.0, 2.0, 3.0]
    e_pot = FT(100.0)
    @test typeof.(internal_energy.(ρ, ρ .* e_int, Ref(ρu), Ref(e_pot))) ==
          typeof.(e_int)

    ts_dry = PhaseDry.(Ref(param_set), e_int, ρ)
    ts_dry_pT = PhaseDry_given_pT.(Ref(param_set), p, T)
    ts_eq = PhaseEquil.(Ref(param_set), e_int, ρ, q_tot, 15, FT(1e-1))
    ts_T =
        TemperatureSHumEquil.(
            Ref(param_set),
            air_temperature.(ts_dry),
            air_pressure.(ts_dry),
            q_tot,
        )
    ts_neq = PhaseNonEquil.(Ref(param_set), e_int, ρ, q_pt)
    ts_θ_liq_ice_eq =
        LiquidIcePotTempSHumEquil.(
            Ref(param_set),
            θ_liq_ice,
            ρ,
            q_tot,
            40,
            FT(1e-3),
        )
    ts_θ_liq_ice_eq_p =
        LiquidIcePotTempSHumEquil_given_pressure.(
            Ref(param_set),
            θ_liq_ice,
            p,
            q_tot,
            40,
            FT(1e-3),
        )
    ts_θ_liq_ice_neq =
        LiquidIcePotTempSHumNonEquil.(Ref(param_set), θ_liq_ice, ρ, q_pt)
    ts_θ_liq_ice_neq_p =
        LiquidIcePotTempSHumNonEquil_given_pressure.(
            Ref(param_set),
            θ_liq_ice,
            p,
            q_pt,
        )

    for ts in (
        ts_dry,
        ts_dry_pT,
        ts_eq,
        ts_T,
        ts_neq,
        ts_θ_liq_ice_eq,
        ts_θ_liq_ice_eq_p,
        ts_θ_liq_ice_neq,
        ts_θ_liq_ice_neq_p,
    )
        @test typeof.(soundspeed_air.(ts)) == typeof.(e_int)
        @test typeof.(gas_constant_air.(ts)) == typeof.(e_int)
        @test typeof.(vapor_specific_humidity.(ts)) == typeof.(e_int)
        @test typeof.(relative_humidity.(ts)) == typeof.(e_int)
        @test typeof.(air_pressure.(ts)) == typeof.(e_int)
        @test typeof.(air_density.(ts)) == typeof.(e_int)
        @test typeof.(total_specific_humidity.(ts)) == typeof.(e_int)
        @test typeof.(cp_m.(ts)) == typeof.(e_int)
        @test typeof.(cv_m.(ts)) == typeof.(e_int)
        @test typeof.(air_temperature.(ts)) == typeof.(e_int)
        @test typeof.(internal_energy_sat.(ts)) == typeof.(e_int)
        @test typeof.(internal_energy.(ts)) == typeof.(e_int)
        @test typeof.(latent_heat_vapor.(ts)) == typeof.(e_int)
        @test typeof.(latent_heat_sublim.(ts)) == typeof.(e_int)
        @test typeof.(latent_heat_fusion.(ts)) == typeof.(e_int)
        @test typeof.(q_vap_saturation.(ts)) == typeof.(e_int)
        @test typeof.(saturation_excess.(ts)) == typeof.(e_int)
        @test typeof.(liquid_fraction.(ts)) == typeof.(e_int)
        @test typeof.(liquid_ice_pottemp.(ts)) == typeof.(e_int)
        @test typeof.(dry_pottemp.(ts)) == typeof.(e_int)
        @test typeof.(exner.(ts)) == typeof.(e_int)
        @test typeof.(liquid_ice_pottemp_sat.(ts)) == typeof.(e_int)
        @test typeof.(specific_volume.(ts)) == typeof.(e_int)
        @test typeof.(virtual_pottemp.(ts)) == typeof.(e_int)
        @test eltype.(gas_constants.(ts)) == typeof.(e_int)
        @test typeof.(getproperty.(PhasePartition.(ts), :tot)) == typeof.(e_int)
    end

end

@testset "moist thermodynamics - dry limit" begin

    FT = Float64
    z, e_int, ρ, q_tot, q_pt, T, p, θ_liq_ice =
        tested_profiles(param_set, 50, FT)

    # PhasePartition test is noisy, so do this only once:
    ts_dry = PhaseDry(param_set, first(e_int), first(ρ))
    ts_eq = PhaseEquil(param_set, first(e_int), first(ρ), typeof(first(ρ))(0))
    @test PhasePartition(ts_eq).tot ≈ PhasePartition(ts_dry).tot
    @test PhasePartition(ts_eq).liq ≈ PhasePartition(ts_dry).liq
    @test PhasePartition(ts_eq).ice ≈ PhasePartition(ts_dry).ice

    ts_dry = PhaseDry.(Ref(param_set), e_int, ρ)
    ts_eq = PhaseEquil.(Ref(param_set), e_int, ρ, q_tot .* 0)

    @test all(gas_constant_air.(ts_eq) .≈ gas_constant_air.(ts_dry))
    @test all(relative_humidity.(ts_eq) .≈ relative_humidity.(ts_dry))
    @test all(air_pressure.(ts_eq) .≈ air_pressure.(ts_dry))
    @test all(air_density.(ts_eq) .≈ air_density.(ts_dry))
    @test all(specific_volume.(ts_eq) .≈ specific_volume.(ts_dry))
    @test all(
        total_specific_humidity.(ts_eq) .≈ total_specific_humidity.(ts_dry),
    )
    @test all(cp_m.(ts_eq) .≈ cp_m.(ts_dry))
    @test all(cv_m.(ts_eq) .≈ cv_m.(ts_dry))
    @test all(air_temperature.(ts_eq) .≈ air_temperature.(ts_dry))
    @test all(internal_energy.(ts_eq) .≈ internal_energy.(ts_dry))
    @test all(internal_energy_sat.(ts_eq) .≈ internal_energy_sat.(ts_dry))
    @test all(soundspeed_air.(ts_eq) .≈ soundspeed_air.(ts_dry))
    @test all(latent_heat_vapor.(ts_eq) .≈ latent_heat_vapor.(ts_dry))
    @test all(latent_heat_sublim.(ts_eq) .≈ latent_heat_sublim.(ts_dry))
    @test all(latent_heat_fusion.(ts_eq) .≈ latent_heat_fusion.(ts_dry))
    @test all(q_vap_saturation.(ts_eq) .≈ q_vap_saturation.(ts_dry))
    @test all(saturation_excess.(ts_eq) .≈ saturation_excess.(ts_dry))
    @test all(liquid_fraction.(ts_eq) .≈ liquid_fraction.(ts_dry))
    @test all(liquid_ice_pottemp.(ts_eq) .≈ liquid_ice_pottemp.(ts_dry))
    @test all(dry_pottemp.(ts_eq) .≈ dry_pottemp.(ts_dry))
    @test all(virtual_pottemp.(ts_eq) .≈ virtual_pottemp.(ts_dry))
    @test all(liquid_ice_pottemp_sat.(ts_eq) .≈ liquid_ice_pottemp_sat.(ts_dry))
    @test all(exner.(ts_eq) .≈ exner.(ts_dry))

    @test all(
        saturation_vapor_pressure.(ts_eq, Ref(Ice())) .≈
        saturation_vapor_pressure.(ts_dry, Ref(Ice())),
    )
    @test all(
        saturation_vapor_pressure.(ts_eq, Ref(Liquid())) .≈
        saturation_vapor_pressure.(ts_dry, Ref(Liquid())),
    )
    @test all(first.(gas_constants.(ts_eq)) ≈ first.(gas_constants.(ts_dry)))
    @test all(last.(gas_constants.(ts_eq)) ≈ last.(gas_constants.(ts_dry)))

end


using Statistics
using Plots
using BenchmarkTools
using Profile
using FFTW
using Statistics
using Optim
using Images, FileIO, ImageIO
using Printf
using Revise
using Profile
using LinearAlgebra
using Random
using Distributions
using FITSIO
using SparseArrays

push!(LOAD_PATH, pwd()*"/main")
using DHC_2DUtils
#include("Deriv_Utils_New.jl")
#import Deriv_Utils_New
push!(LOAD_PATH, pwd()*"/scratch_NM")
using Deriv_Utils_New
using Data_Utils
using Visualization
using ReconFuncs

#Rewritten
logbool= false                                                #Calculate coefficients of the log of the image?
apdbool = true                                                #Apodize?
isobool = false                                               #Iso?

#Image you want to start with
Nx=64
sfdall = readsfd_fromsrc("scratch_NM/data/dust10000.fits", Nx, logbool=false)
input = sfdall[:, :, 5]                     #Less of a memory crunch if you just read in the one image you want to start with

filter_hash = fink_filter_hash(1, 8, nx=Nx, pc=1, wd=1, Omega=true)
dhc_args = Dict(:doS2=>false, :doS20=>true, :apodize=>apdbool, :iso=>isobool)
lambda = 0.0
optim_settings = Dict([("iterations", 1000), ("norm", false), ("minmethod", ConjugateGradient())])

#Coefficient to optimize wrt
Jvalue = 1
Lvalue = 0
#Code currently requires you to access S1 through the S2 matrix diagonal
mask_nf = (filter_hash["J_L"][:, 1] .== Jvalue) .& (filter_hash["J_L"][:, 2] .== Lvalue)
mask_nf2 = Diagonal(mask_nf)
(Nf,) = size(filter_hash["filt_index"])
coeff_mask = falses(2+Nf+Nf^2)
coeff_mask[3+Nf:end] .= mask_nf2[:]

#Setting a target coefficient: currently I'm taking the 10*true target coefficient
#=

if logbool
    procdbn = log.(sfdall)
else
    procdbn = sfdall
end

dbncoeffs = get_dbn_coeffs(procdbn, filter_hash, dhc_args, coeff_mask = coeff_mask)
sigval = std(dbncoeffs, dims=1)
=#

if logbool
    s_targ_mean = 0.1*DHC_compute_wrapper(log.(input), filter_hash, norm=false; dhc_args...)[coeff_mask]             #target coefficient vector = twice the current wst value
    s_targ_invcov = Diagonal([0.1.^(-2)])
    res, recon_img = Deriv_Utils_New.image_recon_derivsum_regularized(log.(input), filter_hash, s_targ_mean, s_targ_invcov, falses(Nx, Nx), dhc_args, coeff_mask=coeff_mask, optim_settings=optim_settings, lambda=lambda)
    recon_img = exp.(recon_img)
else
    s_targ_mean = 0.1*DHC_compute_wrapper(input, filter_hash, norm=false; dhc_args...)[coeff_mask]             #target coefficient vector = twice the current wst value
    s_targ_invcov = Diagonal([0.0015.^(-2)])          #Might need to tune. inverse covariance matrix with sigma= true_coeff/10 for J=1 L=2
    res, recon_img = Deriv_Utils_New.image_recon_derivsum_regularized(input, filter_hash, s_targ_mean, s_targ_invcov, falses(Nx, Nx), dhc_args, optim_settings=optim_settings, coeff_mask=coeff_mask, lambda=lambda)
end

heatmap(input)
heatmap(recon_img)

#Iso
#Rewritten
logbool= false                                               #Calculate coefficients of the log of the image?
apdbool = true                                               #Apodize?
isobool = true                                               #Iso?

#Image you want to start with
Nx=64
sfdall = readsfd_fromsrc("scratch_NM/data/dust10000.fits", Nx, logbool=false)
input = sfdall[:, :, 5]                     #Less of a memory crunch if you just read in the one image you want to start with

filter_hash = fink_filter_hash(1, 8, nx=Nx, pc=1, wd=1, Omega=true)
dhc_args = Dict(:doS2=>false, :doS20=>true, :apodize=>apdbool, :iso=>isobool)
lambda = 0.0
optim_settings = Dict([("iterations", 1000), ("norm", false), ("minmethod", ConjugateGradient())])

#Coefficient to optimize wrt
Jvalue = 1
Lvalue = 0
filter_hash["S2_iso_mat"]
#Code currently requires you to access S1 through the S2 matrix diagonal
mask_nf = (filter_hash["J_L"][:, 1] .== Jvalue) .& (filter_hash["J_L"][:, 2] .== Lvalue)
mask_nf2 = Diagonal(mask_nf)
(Nf,) = size(filter_hash["filt_index"])
coeff_mask = falses(2+Nf+Nf^2)
coeff_mask[3+Nf:end] .= mask_nf2[:]

if logbool
    s_targ_mean = 0.1*DHC_compute_wrapper(log.(input), filter_hash, norm=false; dhc_args...)[coeff_mask]             #target coefficient vector = twice the current wst value
    s_targ_invcov = Diagonal([0.1.^(-2)])
    res, recon_img = Deriv_Utils_New.image_recon_derivsum_regularized(log.(input), filter_hash, s_targ_mean, s_targ_invcov, falses(Nx, Nx), dhc_args, coeff_mask=coeff_mask, optim_settings=optim_settings, lambda=lambda)
    recon_img = exp.(recon_img)
else
    s_targ_mean = 0.1*DHC_compute_wrapper(input, filter_hash, norm=false; dhc_args...)[coeff_mask]             #target coefficient vector = twice the current wst value
    s_targ_invcov = Diagonal([0.0015.^(-2)])          #Might need to tune. inverse covariance matrix with sigma= true_coeff/10 for J=1 L=2
    res, recon_img = Deriv_Utils_New.image_recon_derivsum_regularized(input, filter_hash, s_targ_mean, s_targ_invcov, falses(Nx, Nx), dhc_args, optim_settings=optim_settings, coeff_mask=coeff_mask, lambda=lambda)
end

heatmap(input)
heatmap(recon_img)


###################################################################################################
#
# ResNet50 model for Flux
#   Reference: [Deep Residual Learning for Image Recognition](https://arxiv.org/pdf/1512.03385.pdf)
#
####################################################################################################
using Flux

include("config.jl")

"""
    The identity block is the block that has no conv layer at shortcut.

    # Arguments
        kernel_size: defualt 3, the kernel size of middle conv layer at main path
        filters: list of integers, the filterss of 3 conv layer at main path
Returns the block model.
"""
function identity_block(kernel_size, filters)
    filter0, filter1, filter2, filter3 = filters
    Chain(
        Conv((1, 1), filter0=>filter1),
        BatchNorm(filter1),
        x -> relu.(x),
        Conv(kernel_size, filter1=>filter2, pad=(1, 1)),
        BatchNorm(filter2),
        x -> relu.(x),
        Conv((1, 1), filter2=>filter3),
        BatchNorm(filter3),
    ) |> gpu
end

struct IDBlock
    kernel_size::Tuple
    filters::AbstractArray
    model::Chain
    IDBlock(kernel_size, filters) = new(kernel_size, filters, identity_block(kernel_size, filters))
end

function (ib::IDBlock)(x)
    relu.(ib.model(x) + x)
end

"""
    conv_block is the block that has a conv layer at shortcut

    # Arguments
        input_tensor: input for the model
        kernel_size: the kernel size of middle conv layer at main path
        filters: list of integers, the filterss of 3 conv layer at main path
    Note: the shortcut should have strides=(2, 2)
Returns block model.
"""
function plain_model(kernel_size, filters; strides=(2, 2))
    filter0, filter1, filter2, filter3 = filters
    Chain(
        Conv((1, 1), filter0=>filter1, stride=strides),
        BatchNorm(filter1),
        x -> relu.(x),
        Conv(kernel_size, filter1=>filter2, pad=(1, 1)),
        BatchNorm(filter2),
        x -> relu.(x),
        Conv((1, 1), filter2=>filter3),
        BatchNorm(filter3),
    ) |> gpu
end

struct ConvBlock
    kernel_size::Tuple
    filters::AbstractArray
    strides::Tuple
    plain::Chain
    shortcut::Chain
    ConvBlock(kernel_size, filters; strides=(2, 2)) = new(kernel_size, filters, strides,
        plain_model(kernel_size, filters, strides=strides),
        Chain(Conv((1, 1), filters[2]=>filters[4], stride=strides), BatchNorm(filters[4])) |> gpu)
end

function (cb::ConvBlock)(x)
    relu.(cb.plain(x) + cb.shortcut(x))
end

"""
    Zero-padding layer for input

    # Arguments
        inputs: input tensor
        padding: symmetric padding values for height and width [default=(1, 1)]
Returns add rows and columns of zeros at the top, bottom, left and right side of an input tensor.
"""
function zero_padding(inputs; padding=(1, 1))
    row, col, c, s = size(inputs)
    h, w = padding
    result = vcat(inputs, zeros(h, row, c, s))
    result = hcat(result, zeros(col + h, w, c, s))
    result = hcat(zeros(row + h, w, c, s), result)
    vcat(zeros(h, col + w * 2, c, s), result)
end

ResNetModel = Chain(
    x -> zero_padding(x, padding=(3, 3)),
    Conv((7, 7), 1=>64, stride=(2, 2)),
    BatchNorm(64),
    x -> relu.(x),
    x -> maxpool(x, (3, 3); stride=(2, 2)),
    ConvBlock((3, 3), [64, 64, 64, 256], strides=(1, 1)),
    IDBlock((3, 3), [256, 64, 64, 256]),
    IDBlock((3, 3), [256, 64, 64, 256]),
    ConvBlock((3, 3), [256, 128, 128, 512]),
    IDBlock((3, 3), [512, 128, 128, 512]),
    IDBlock((3, 3), [512, 128, 128, 512]),
    IDBlock((3, 3), [512, 128, 128, 512]),
    ConvBlock((3, 3), [512, 256, 256, 1024]),
    IDBlock((3, 3), [1024, 256, 256, 1024]),
    IDBlock((3, 3), [1024, 256, 256, 1024]),
    IDBlock((3, 3), [1024, 256, 256, 1024]),
    IDBlock((3, 3), [1024, 256, 256, 1024]),
    IDBlock((3, 3), [1024, 256, 256, 1024]),
    ConvBlock((3, 3), [1024, 512, 512, 2048]),
    IDBlock((3, 3), [2048, 512, 512, 2048]),
    IDBlock((3, 3), [2048, 512, 512, 2048]),
    x -> meanpool(x, (7, 7)),
    x -> reshape(x, :, size(x, 4)),
    Dense(2048, NUM_CLASSES),
    softmax
) |> gpu


@testset "sf -> node" begin
    function normal_model(t, x)
        loc = sample(t, "loc", Normal())
        log_scale = sample(t, "log_scale", Normal())
        observe(t, "data", Normal(loc, exp(log_scale)), x)
    end

    sf = TypedSF(normal_model, Maybe{Float64}, Float64)
    input = 3.0
    csf = compile(sf, input, ["log_scale", "loc"])

    t = trace()
    sample(t, "test", csf)
    @info t
    @test abs(t["test"].value["loc"] - input) < 0.1
end


function Kprec( pw_grid::PWGrid, psi::Array{ComplexF64,2} )
    Ncol = size(psi)[2]
    G2 = pw_grid.G2
    Npoints = pw_grid.Npoints
    Kpsi = zeros( ComplexF64, size(psi) )
    for ic = 1:Ncol
        for ip = 1:Npoints
          Kpsi[ip,ic] = psi[ip,ic] / ( 1.0 + G2[ip] )
        end
    end
    return Kpsi
end

function Kprec(psi)
  return psi
end


facts("fast exponentiation methods") do

  context("odd") do

    @fact odd(0) => false
    @fact odd(1) => true
    @fact odd(2) => false
    @fact odd(3) => true

    @fact odd(2^100) => false

    @fact odd(BigInt(2)^2011) => false

  end

  @fact fast_exponentiation(10, 0, 2) => 1

  @fact fast_exponentiation(1, 2, 3) => 1

  @fact fast_exponentiation(2, 2011, 15) => int(mod(BigFloat(2)^2011, 15))

end


using DataStructures

function main()
    H, W = parse.(Int, split(readline()))
    mp = [readline() for _ = 1:H]

    dx = [1, 0, -1, 0]
    dy = [0, 1, 0, -1]

    xₛ, yₛ, xₑ, yₑ = -1, -1, -1, -1
    for i = 1:H, j = 1:W
        if mp[i][j] == 's'
            xₛ, yₛ = i, j
        elseif mp[i][j] == 'g'
            xₑ, yₑ = i, j
        end
    end

    que = Deque{Tuple{Int, Int}}()
    pushfirst!(que, (xₛ, yₛ))
    dists = 10^9 * ones(Int, H, W)
    dists[xₛ, yₛ] = 0

    while !isempty(que)
        x, y = popfirst!(que)

        for d = 1:4
            x′ = x + dx[d]
            y′ = y + dy[d]

            if 1 ≤ x′ ≤ H && 1 ≤ y′ ≤ W
                if mp[x′][y′] == '#'
                    if dists[x′, y′] > dists[x, y] + 1
                        dists[x′, y′] = dists[x, y] + 1
                        push!(que, (x′, y′))
                    end
                else
                    if dists[x′, y′] > dists[x, y]
                        dists[x′, y′] = dists[x, y]
                        pushfirst!(que, (x′, y′))
                    end
                end
            end
        end
    end

    if dists[xₑ, yₑ] ≤ 2
        println("YES")
    else
        println("NO")
    end

end

main()


type SAGAcbsd <: StepData
	A
	y
	m::Int64
	numClasses::Int64
	c::Vector{Int64}
	batchSize::Int64
	getStep::Function
	stepString::AbstractString
	function SAGAcbsd(numVars::Int64,y, stepString::AbstractString,numDp::Int64, batchSize::Int64,c)
		A = sum(y)
		new(A, y, numDp, size(y)[1],c,batchSize, SAGAcbComputation, stepString)
	end
end

function SAGAcb(numVars::Int64, y,batchSize::Int64, c, numDP::Int64)
	SAGAcbsd(numVars,y, "SAGAcb.$batchSize", numDP,batchSize,c)
end

function SAGAcbComputation(x, k, gnum, sd::SAGAcbsd, problem::Problem; outputLevel =0 )

	outputLevel >0 && println("SAGAcbComputation started")

	B=zeros(size(x))

	sumy=zeros(size(x))

	indices = sample(1:sd.m,sd.batchSize;replace=false)
	nummm=0
	for i in indices

		B +=sd.y[sd.c[i]]
		(f,sampleG) = (problem.getSampleFunctionAt(i))(x)
		sd.y[sd.c[i]]=sampleG
		sumy += sampleG
		nummm +=1
		gnum += 1

		#step 3
	end
	outputLevel >0 && println("Acutal step determination")

	step =((1/sd.numClasses)*sd.A) - (1/nummm)*(B - sumy)

	outputLevel >0 && println("Updating current sum")
	sd.A=sd.A-B+sumy
	outputLevel >0 && println("SAGAcbComputation finished")
	(step,gnum)

end


using Poirot, IRTools, Test
using Poirot.Abstract: @trace

thunk = () -> begin
  x = rand(Uniform(0, 10))
  x^2 > 50
end

ir = @trace thunk()

lpdf = IRTools.func(Poirot.logprob(ir)[1])

@test lpdf(nothing, (7.0,)) == (false, logpdf(Uniform(0, 10), 7.0))
@test lpdf(nothing, (7.5,)) == (true, logpdf(Uniform(0, 10), 7.5))


using Random,
    DataFrames, XLSX, LinearAlgebra, Statistics, StatsBase, Distributions, Plots, CSV, RCall, JLD;
include("fcns/mainsup.jl")

function phidraw(y,x,m,p,nsmpl,t)
    maxλ = 1
    ϕd = nothing
    while maxλ >= 1
        pos = rand(1:nsmpl-t)
        yn = y[pos:pos+t-1,:]
        xn = x[pos:pos+t-1,:]
        βd, ϕd = OLSbetas(yn, xn, m, p, Xblock = true, Nx = 3)
        maxλ = maximum(abs.(eigvals(ϕd)))
    end
    return ϕd
end

function boots(B,Y,E,p,nsmpl)
    t, m = size(Y)
    nx  = m*p+1
    ebo = E[rand(1:t,nsmpl),:]
    rst = rand(p+1:t)
    y = Array{Float32,2}(undef,m,nsmpl)
    x = Array{Float32,2}(undef,nx,nsmpl)
    x[:,1] = [vec(Y[rst:-1:rst-1,:]') ; 1]
    βm = B'
    for i in 1:nsmpl-1
        y[:,i] = βm * x[:,i] + ebo[i,:]
        x[:,i+1] = [y[:,i] ; x[1:m,i] ; 1]
    end
    y[:,nsmpl] = βm * x[:,nsmpl] + ebo[nsmpl,:]
    y = y'
    x = x'
    collΦ = Array{Any,1}(undef,nrep)
    for i in 1:nrep
        collΦ[i] = phidraw(y,x,m,p,nsmpl,t)
    end
    return collΦ
end

p = 2
h = 40
nrep = 1000
nsmpl = 100000
dd = load("GSdata.jld")
dataset = dd["dataset"]
nc = nfields(dataset);
Φdraw = Array{Any,1}(undef,nc)
for k in 1:nc
    Y, X = VARData(dataset[k],p)
    B = (X'*X) \ (X'*Y)
    E = Y - X*B
    Φdraw[k] = boots(B,Y,E,p,nsmpl)
end


export rstd, rstc, dab, c2d_roots2poly, c2d_poly2poly, zpconv#, lsima, indirect_str

"""`[sysd, x0map] = c2d(sys, Ts, method=:zoh)`

Convert the continuous system `sys` into a discrete system with sample time
`Ts`, using the provided method. Currently only `:zoh` and `:foh` are provided.

Returns the discrete system `sysd`, and a matrix `x0map` that transforms the
initial conditions to the discrete domain by
`x0_discrete = x0map*[x0; u0]`"""
function c2d(sys::StateSpace, Ts::Real, method::Symbol=:zoh)
    if !iscontinuous(sys)
        error("sys must be a continuous time system")
    end
    A, B, C, D = ssdata(sys)
    ny, nu = size(sys)
    nx = nstates(sys)
    if method == :zoh
        M = exp([A*Ts  B*Ts;
            zeros(nu, nx + nu)])
        Ad = M[1:nx, 1:nx]
        Bd = M[1:nx, nx+1:nx+nu]
        Cd = C
        Dd = D
        x0map = [Matrix{Float64}(I, nx, nx) zeros(nx, nu)] # Cant use I if nx==0
    elseif method == :foh
        M = exp([A*Ts B*Ts zeros(nx, nu);
            zeros(nu, nx + nu) Matrix{Float64}(I, nu, nu);
            zeros(nu, nx + 2*nu)])
        M1 = M[1:nx, nx+1:nx+nu]
        M2 = M[1:nx, nx+nu+1:nx+2*nu]
        Ad = M[1:nx, 1:nx]
        Bd = Ad*M2 + M1 - M2
        Cd = C
        Dd = D + C*M2
        x0map = [Matrix{Float64}(I, nx, nx)  (-M2)]
    elseif method == :tustin || method == :matched
        error("NotImplemented: Only `:zoh` and `:foh` implemented so far")
    else
        error("Unsupported method: ", method)
    end
    return StateSpace(Ad, Bd, Cd, Dd, Ts), x0map
end

function rst(bplus,bminus,a,bm1,am,ao,ar=[1],as=[1] ;cont=true)

    ae      = conv(a,ar)
    be      = conv(bminus,as)
    aoam    = conv(am,ao)
    r1,s1   = dab(ae,be,aoam)

    r       = conv(conv(r1,ar),bplus)
    s       = conv(s1,as)

    bm      = conv(bminus,bm1)
    t0      = (cont ? am[end]/bm[end] : sum(am)/sum(bm))
    t       = t0*conv(ao,bm1)
    s       = s/r[1]
    t       = t/r[1]
    r       = r/r[1]

    r,s,t
end

"""
See ?rstd for the discerte case
"""
rstc(args...)=rst(args..., ;cont=true)

"""
rstd  Polynomial synthesis in discrete time.

`R,S,T=rstd(BPLUS,BMINUS,A,BM1,AM,AO,AR,AS)`

`R,S,T=rstd(BPLUS,BMINUS,A,BM1,AM,AO,AR)`

`R,S,T=rstd(BPLUS,BMINUS,A,BM1,AM,AO)`

Polynomial synthesis according to CCS ch 10 to
design a controller R(q) u(k) = T(q) r(k) - S(q) y(k)

Inputs:  BPLUS  : Part of open loop numerator
BMINUS : Part of open loop numerator
A      : Open loop denominator
BM1    : Additional zeros
AM     : Closed loop denominator
AO     : Observer polynomial
AR     : Pre-specified factor of R,
e.g integral part [1, -1]^k
AS     : Pre-specified factor of S,
e.g notch filter [1, 0, w^2]

Outputs: R,S,T  : Polynomials in controller

See function DAB how the solution to the Diophantine-
Aryabhatta-Bezout identity is chosen.

See Computer-Controlled Systems: Theory and Design, Third Edition
Karl Johan Åström, Björn Wittenmark
"""
rstd(args...)=rst(args..., ;cont=false)

"""
DAB   Solves the Diophantine-Aryabhatta-Bezout identity

`X,Y = DAB(A,B,C)`

AX + BY = C, where A, B, C, X and Y are polynomials
and deg Y = deg A - 1.

See Computer-Controlled Systems: Theory and Design, Third Edition
Karl Johan Åström, Björn Wittenmark
"""
function dab(a,b,c)

    na = length(a)
    nb = length(b)
    nc = length(c)
    ns = na - 1
    if ns < 1
        r = c/a
        s = 0
        return
    end
    nr = nc - ns
    c = nb-nr > 1 ? [zeros(nb-nr-1); c] : c
    nc = length(c)
    nr = nc - ns
    if nr < 1
        r = 0
        s = c/b
        return
    end
    b = [zeros(nr-nb+1); b]
    za = zeros(nr-1)
    zb = zeros(ns-1)
    ma = toeplitz([a; za],[a[1]; za])
    mb = toeplitz([b; zb],[b[1]; zb])
    m = [ma mb]
    if rank(m) < minimum(size(m))
        @warn("Singular problem due to common factors in A and B")
    end
    co = cond(m)
    co > 1e6 && println("dab: condition number $(co)")
    rs = (c'/(m'))'
    r = rs[1:nr]
    s = rs[nr+1:nc]
    length(s) > length(r) && @warn("Controller not casual, deg(S) > deg(R), consider increasing degree of observer polynomial")
    r,s
end

function toeplitz(c,r)
    nc = length(c)
    nr = length(r)
    A  = zeros(nc, nr)
    A[:,1] = c
    A[1,:] = r
    for i in 2:nr
        A[2:end,i] = A[1:end-1,i-1]
    end
    A
end

import Polynomials

"""
`c2d_roots2poly(ro,h)`

returns the polynomial coefficients in discrete time given a vector of roots in continuous time
"""
function c2d_roots2poly(ro,h)
    return real((Polynomials.poly(exp(ro.*h))).a[end:-1:1])
end

"""
`c2d_poly2poly(ro,h)`

returns the polynomial coefficients in discrete time given polynomial coefficients in continuous time
"""
function c2d_poly2poly(p,h)
    ro = Polynomials.roots(Polynomials.Poly(p[end:-1:1]))
    return real(Polynomials.poly(exp(ro.*h)).a[end:-1:1])
end

function c2d(G::TransferFunction, h;kwargs...)
    @assert iscontinuous(G)
    ny, nu = size(G)
    @assert (ny + nu == 2) "c2d(G::TransferFunction, h) not implemented for MIMO systems"
    sys = ss(G)
    sysd = c2d(sys, h, kwargs...)[1]
    return convert(TransferFunction, sysd)
end

"""`[y, t, x] = lsima(sys, t, r, controller, state[, x0, method])`

Calculate the time response of adaptive controller. If `x0` is ommitted,
a zero vector is used.

`controller` is a function `u[i],state = controller(state, y[1:i], u[1:i-1], r[1:i])`
Continuous time systems are discretized before simulation. By default, the
method is chosen based on the smoothness of the input signal. Optionally, the
`method` parameter can be specified as either `:zoh` or `:foh`."""
function lsima(sys::StateSpace, t::AbstractVector, r::AbstractVector, control_signal::Function,state,
    x0::VecOrMat=zeros(sys.nx, 1), method::Symbol=:zoh)
    ny, nu = size(sys)

    nx = sys.nx

    if length(x0) != nx
        error("size(x0) must match the number of states of sys")
    end

    dt = Float64(t[2] - t[1])
    if !iscontinuous(sys) || method == :zoh
        if iscontinuous(sys)
            dsys = c2d(sys, dt, :zoh)[1]
        else
            if sys.Ts != dt
                error("Time vector must match sample time for discrete system")
            end
            dsys = sys
        end
    else
        dsys, x0map = c2d(sys, dt, :foh)
    end
    n = size(t, 1)
    x = similar(r, size(sys.A, 1), n)
    u = similar(r, n)
    y = similar(r, n)
    for i=1:n
        x[:,i] = x0
        y[i] = (sys.C*x0 + sys.D*u[i])[1]

        u[i],state = control_signal(state, y[1:i], u[1:i-1], r[1:i])
        x0 = sys.A * x0 + sys.B * u[i]

    end

    return y, t, x, u
end
lsima(sys::TransferFunction, u, t,r, args...) = lsima(ss(sys), u, t,r, args...)

function indirect_str(state, y, u,uc, nb,na, lambda,bm1,am,ao,ar=[1],as=[1])
    theta, P = state
    u = [zeros(length(ao)+length(am)+1);u]
    y = [zeros(length(ao)+length(am)+1);y]
    phi = [y[end-1:-1:end-na]; u[end:-1:end-nb]]
    # compute new estimate and update covariance matrix

    K = P*phi/(lambda + phi'P*phi)
    new_theta = theta + K*(y[end] - phi'theta)
    new_P = (I - K*phi')*P/lambda
    new_P = (new_P + new_P')/2

    state = (new_theta, new_P)

    a = [1;theta[1:na]]
    b = theta[na+1:end]
    r,s,t = rstd([1],b,a,bm1,am,ao,ar,as)
    uo = r⋅u[end:-1:end-length(r)+1] + s⋅y[end-1:-1:end-length(s)] + t⋅uc[end:-1:end-length(t)+1]

    return uo,state

end

"""
`zpc(a,r,b,s)` form conv(a,r) + conv(b,s) where the lengths of the polynomials are equalized by zero-padding such that the addition can be carried out
"""
function zpconv(a,r,b,s)
    d = length(a)+length(r)-length(b)-length(s)
    if d > 0
        b = [zeros(d);b]
    end
    conv(a,r) + conv(b,s)
end


using DataEnvelopmentAnalysis
using LinearAlgebra
using Test

@testset "DataEnvelopmentAnalysis" begin

    include("dea.jl")
    include("deaadd.jl")
    include("deaddf.jl")
    include("deagdf.jl")
    include("deacost.jl")
    include("dearevenue.jl")
    include("deaprofit.jl")
    include("deaprofitability.jl")
    include("malmquist.jl")

end


sharpness(img::AbstractArray{CT,N}) where {CT<:ImageCore.NumberLike,N} = var(imfilter(img, Kernel.Laplacian(ntuple(i->true, Val(N)))))

function adaptive_thres(img::AbstractArray{CT,N}, block::NTuple{N,Int}) where {CT<:ImageCore.NumberLike,N}
    threshold = zeros(Float64, block)
    block_length = CartesianIndex(ntuple(i->ceil(Int,length(axes(img,i))/block[i]),Val(N)))
    net_s = sharpness(img)
    net_var = var(img)
    net_end = last(CartesianIndices(axes(img)))
    for i in CartesianIndices(block)
        si = CartesianIndex(ntuple(j->(i[j]-1)*block_length[j]+1,Val(N)))
        ei = min(si + block_length - _oneunit(si), net_end)
        wi = view(img, map((i,j)->i:j, si.I, ei.I)...)
        threshold[i] = 0.02 + min(sharpness(wi)/net_s*0.04,0.1) + min(var(wi)/net_var*0.04,0.1)
    end
    threshold
end

getscalar(A::AbstractArray{T,N}, i::CartesianIndex{N}, block_length::CartesianIndex{N}) where {T<:Real,N} =
    A[CartesianIndex(ntuple(j->(i[j]-1)÷block_length[j]+1, Val(N)))]

getscalar(a::Real, i...) = a

"""
    seg_img = fast_scanning(img, threshold, [diff_fn])

Segments the N-D image using a fast scanning algorithm and returns a
[`SegmentedImage`](@ref) containing information about the segments.

# Arguments:
* `img`         : N-D image to be segmented (arbitrary axes are allowed)
* `threshold`   : Upper bound of the difference measure (δ) for considering
                  pixel into same segment; an `AbstractArray` can be passed
                  having same number of dimensions as that of `img` for adaptive
                  thresholding
* `diff_fn`     : (Optional) Function that returns a difference measure (δ)
                  between the mean color of a region and color of a point

# Examples:

```jldoctest; setup = :(using ImageCore, ImageMorphology, ImageSegmentation)
julia> img = zeros(Float64, (3,3));

julia> img[2,:] .= 0.5;

julia> img[:,2] .= 0.6;

julia> seg = fast_scanning(img, 0.2);

julia> labels_map(seg)
3×3 $(Matrix{Int}):
 1  4  5
 4  4  4
 3  4  6
```

# Citation:

Jian-Jiun Ding, Cheng-Jin Kuo, Wen-Chih Hong,
"An efficient image segmentation technique by fast scanning and adaptive merging"
"""
fast_scanning(img::AbstractArray{CT,N}, block::NTuple{N,Int} = ntuple(i->4,Val(N))) where {CT<:ImageCore.NumberLike,N} =
    fast_scanning(img, adaptive_thres(img, block))

fast_scanning(img::AbstractArray{CT,N}, threshold::Union{AbstractArray,Real}, diff_fn::Function = default_diff_fn) where {CT<:Union{Colorant,Real},N} =
    fast_scanning!(fill(-1, axes(img)), img, threshold, diff_fn)

function fast_scanning!(result, img::AbstractArray{CT,N}, threshold::Union{AbstractArray,Real}, diff_fn::DF = default_diff_fn) where {CT<:Union{Colorant,Real},N,DF<:Function}
    if threshold isa AbstractArray
        ndims(img) == ndims(threshold) || error("Dimension count of image and threshold do not match")
    end

    # Neighbourhood function
    _diagmN = Diagonal([1 for i in 1:N])
    half_region::NTuple{N,CartesianIndex{N}} = ntuple(i-> CartesianIndex{N}(ntuple(j->_diagmN[j,i], Val(N))), Val(N))
    neighbourhood(x) = ntuple(i-> x-half_region[i], Val(N))

    # Required data structures
    TM = meantype(CT)
    region_means        =   Dict{Int, TM}()                                 # A map conatining (label, mean) pairs
    region_pix_count    =   Dict{Int, Int}()                                # A map conatining (label, count) pairs
    temp_labels         =   IntDisjointSets(0)                              # Disjoint set to map labels to their equivalence class
    v_neigh             =   MVector{N,Int}(undef)                           # MVector to store valid neighbours

    block_length = CartesianIndex(ntuple(i->ceil(Int,length(axes(img,i))/size(threshold,i)), Val(N)))

    for point in CartesianIndices(axes(img))
        sz = 0
        same_label = true
        prev_label = 0
        for p in neighbourhood(point)
            if checkbounds(Bool, img, p)
                root_p = find_root!(temp_labels, result[p])
                if diff_fn(region_means[root_p], img[point]) < getscalar(threshold, point, block_length)
                    if prev_label == 0
                        prev_label = root_p
                    elseif prev_label != root_p
                        same_label = false
                    end
                    sz += 1
                    v_neigh[sz] = find_root!(temp_labels, root_p)
                end
            end
        end

        # If no valid label found
        if sz == 0
            # Assign a new label
            new_label = push!(temp_labels)
            result[point] = new_label
            region_means[new_label] = img[point]
            region_pix_count[new_label] = 1

        # If all labels are same
        elseif same_label
            result[point] = prev_label
            region_pix_count[prev_label] += 1
            region_means[prev_label] += (img[point] - region_means[prev_label])/(region_pix_count[prev_label])
        else
            # Merge segments and assign to this new label
            union_label = v_neigh[1]
            for i in 1:sz
                union_label = union!(temp_labels, union_label, v_neigh[i])
            end
            result[point] = union_label
            region_pix_count[union_label] += 1
            region_means[union_label] += (img[point] - region_means[union_label])/(region_pix_count[union_label])

            for i in 1:sz
                if v_neigh[i] != union_label && haskey(region_pix_count, v_neigh[i])
                    region_pix_count[union_label] += region_pix_count[v_neigh[i]]
                    region_means[union_label] += (region_means[v_neigh[i]] - region_means[union_label])*region_pix_count[v_neigh[i]]/region_pix_count[union_label]

                    # Remove label v_neigh[i] from region_means, region_pix_count
                    delete!(region_pix_count,v_neigh[i])
                    delete!(region_means,v_neigh[i])
                end
            end
        end
    end

    for point in CartesianIndices(axes(img))
        result[point] = find_root!(temp_labels, result[point])
    end

    SegmentedImage(result, unique(temp_labels.parents), region_means, region_pix_count)
end


using LinearAlgebra

@testset "Test QXTensor constructors and basic usage" begin
    # create a QXTensor with no data
    a = QXTensor([Index(2)])
    @assert size(a) == (2,)

    # test hyper edge search
    data = Diagonal(ones(4))
    indices = [Index(4), Index(4)]
    a = QXTensor(indices, data)
    @test size(a) == (4, 4)
    @test hyperindices(a) == [indices]

    data = reshape(QXTns.Gates.cx(), (2, 2, 2 ,2))
    indices = [Index(2), Index(2), Index(2), Index(2)]
    a = QXTensor(indices, data)
    @test hyperindices(a) == [indices[[2, 4]]]
    @test hyperindices(a, all_indices=true) == [[indices[1]], indices[[2, 4]], [indices[3]]]

    data = reshape(Diagonal(ones(4)), (2, 2, 2 ,2))
    indices = [Index(2), Index(2), Index(2), Index(2)]
    a = QXTensor(indices, data)
    @test hyperindices(a) == [indices[[1, 3]], indices[[2, 4]]]
end

@testset "Test contraction of tensors with hyper indices" begin
    # we create two sets of indices and identify subsets of indices that are hyper indices
    # we then contract over these sets to ensure the results set has the expected hyper indices
    as = [Index(2), Index(2), Index(2), Index(2)]
    a_hyper_indices = [[1, 3]]
    a = QXTensor(as, a_hyper_indices)
    bs = [as[3], as[4], Index(2), Index(2)]
    b_hyper_indices = [[1, 3]]
    b = QXTensor(bs, b_hyper_indices)

    c = contract_tensors(a, b)
    @test Set(hyperindices(c)...) == Set([as[1], bs[3]])

    # now an example with two sets of hyper indices
    as = [Index(2), Index(2), Index(2), Index(2)]
    a_hyper_indices = [[1, 3], [2, 4]]
    a = QXTensor(as, a_hyper_indices)
    bs = [as[3], as[4], Index(2), Index(2)]
    b_hyper_indices = [[1, 3], [2, 4]]
    b = QXTensor(bs, b_hyper_indices)

    c = contract_tensors(a, b)
    @test Set(hyperindices(c)[1]) == Set([as[1], bs[3]])
    @test Set(hyperindices(c)[2]) == Set([as[2], bs[4]])

    # next an example where the first tensor will have a group of hyper indices remaining
    # but the second tensor won't
    as = [Index(2), Index(2), Index(2), Index(5), Index(5)]
    a_hyper_indices = [[1, 2, 3]]
    a = QXTensor(as, a_hyper_indices)
    bs = [as[1], Index(4), Index(5), as[4]]
    b_hyper_indices = [[3, 4]]
    b = QXTensor(bs, b_hyper_indices)

    c = contract_tensors(a, b)
    @test Set(hyperindices(c)[1]) == Set([as[2], as[3]])

    # next an example where the first tensor has two groups linked with group from second tensor
    as = [Index(2), Index(2), Index(2), Index(2)]
    a_hyper_indices = [[1, 2], [3, 4]]
    a = QXTensor(as, a_hyper_indices)
    bs = [as[1], as[4]]
    b_hyper_indices = [[1, 2]]
    b = QXTensor(bs, b_hyper_indices)

    c = contract_tensors(a, b)
    @test Set(hyperindices(c)[1]) == Set([as[2], as[3]])
end

@testset "Test tensor_data when considering hyperedges" begin
    # test a diagonal
    data = Diagonal(ones(4))
    indices = [Index(4), Index(4)]
    a = QXTensor(indices, data)
    @test tensor_data(a, consider_hyperindices=true) == ones(4)

    # test a rank 4 tensor with 2 sets of hyper edges
    data = reshape(Diagonal(ones(4)), (2, 2, 2 ,2))
    indices = [Index(2), Index(2), Index(2), Index(2)]
    a = QXTensor(indices, data)
    @test tensor_data(a, consider_hyperindices=true) == ones(2, 2)

    # test a 2x2x2 tensor with single group of hyper edges include all ranks
    data = zeros(2, 2, 2)
    for i in 1:2 data[i, i, i] = 1 end
    a = QXTensor([Index(2), Index(2), Index(2)], data)
    @test tensor_data(a, consider_hyperindices=true) == ones(2)
end