####################################################################
function gpmodelHMC(x, flux, σ2, idx; maxiterml = 10000, n_samples=2_000, n_adapts =1_000, maxrandom=500, seed = 1, show_trace = true)
####################################################################

    #--------------------------------------------------------
    # Auxiliary
    #--------------------------------------------------------

    rg = MersenneTwister(seed)

    JITTER = 1e-8

    Σdiag = Diagonal(σ2) + JITTER*I

    prvlogs = NaN

    numF = length(unique(idx))

    # @assert(numF == length(galaxyline(3000.0)))

    numparam = 2*numF + 1

    @assert(all(unique(idx) .== collect(1:numF)))

    N = length(x)

    @assert(N == length(flux) == length(idx) == length(σ2))

    @printf("Running gpmodelHMC with %d and %d filters\n", N, numF)

    @printf("\tNumber of free parameters is %d\n", numparam)

    #--------------------------------------------------------
    # Pre-allocate arrays and pre-calculate distances
    #--------------------------------------------------------

    A    = Diagonal(zeros(N))

    bvec = zeros(N)

    K    = zeros(N, N)

    C    = zeros(N, N)

    prvlogs = NaN

    dist2 = pairwise(SqEuclidean(), reshape(x, 1, N), dims=2)

    #--------------------------------------------------------
    function unpack(param)
    #--------------------------------------------------------

        @assert(length(param) == numparam)

        local a     = param[1:numF]
        local b     = param[numF+1:2*numF]
        local logs  = param[end]

        a, b, logs

    end

    #--------------------------------------------------------
    function objective(param)
    #--------------------------------------------------------

        local a, b, logs = unpack(param)

        -1.0 * logl(a = a, logs = logs, b = b)

    end

    #--------------------------------------------------------
    function blockvector!(bvec, idx, b)
    #--------------------------------------------------------

        for i in 1:N
            bvec[i] = b[idx[i]]
        end

    end

    #--------------------------------------------------------
    function diagonalmatrix!(A, idx, a)
    #--------------------------------------------------------

        for i in 1:N
            A.diag[i] = a[idx[i]]
        end

    end

    #--------------------------------------------------------
    function logl(; a = a, b = b, logs = logs)
    #--------------------------------------------------------

        local K = rbf(dist2, [0.0; logs])
        local A = Diagonal(zeros(eltype(a), N))
        local bvec = zeros(eltype(a), N)

        for i in 1:N
                bvec[i] = b[idx[i]]

                A.diag[i] = a[idx[i]]
        end

        C = (A * K * A) .+ Σdiag ; C .= (C .+ C').*0.5

        local L = try

            cholesky(C).L

        catch err

            if isa(err, PosDefException)

                @warn("Cholesky failed, matrix not posdef. Return -Inf.")

                return -Inf

            else

                throw(err)

            end

        end

        local logdetC = 2.0*sum(log.(diag(L)))

        bvec .= L\(flux .- bvec)

        local yTCinvy = sum(bvec.^2)

        return - 0.5*logdetC - 0.5*yTCinvy - 0.5*N*log(2.0π)

    end

    # a = randn(rg, 6).+1; b = randn(rg, 6).+1
    # @show logl(a=a,b=b,logs=0.0) logl0(a=a,b=b,logs=0.0)
    # # @btime  $logl(a=$a,b=$b,logs=0.0)
    # # @btime $logl0(a=$a,b=$b,logs=0.0)
    # return 0,0

    #--------------------------------------------------------
    # Initialise GP hyperparameters with random search
    # and few iterations
    #--------------------------------------------------------

    initsolutions       = [randn(rg, numparam) for i=1:maxrandom]
    initialfitness      = @showprogress "Initial random search " map(objective, initsolutions)
    bestinitialsolution = initsolutions[argmin(initialfitness)]

    #--------------------------------------------------------
    # Call optimiser
    #--------------------------------------------------------

    opt    = Optim.Options(iterations = maxiterml, show_trace = show_trace, show_every=100)
    result = Optim.optimize(objective, bestinitialsolution, NelderMead(), opt)

    #--------------------------------------------------------
    # Retrieve optimised parameters
    #--------------------------------------------------------

    a, b, logs = unpack(result.minimizer)

    #--------------------------------------------------------
    # Inference via HMC
    #--------------------------------------------------------

    newobjective(x) = -1.0* objective([x; logs])

    # Choose parameter dimensionality and initial parameter value
    initial_θ = [a;b]; D = length(initial_θ)

    # Define the target distribution
    ℓπ(θ) = newobjective(θ)

    # Define a Hamiltonian system
    metric = DiagEuclideanMetric(D)
    hamiltonian = Hamiltonian(metric, ℓπ, ForwardDiff)

    # Define a leapfrog solver, with initial step size chosen heuristically
    initial_ϵ = find_good_stepsize(hamiltonian, initial_θ)
    integrator = Leapfrog(initial_ϵ)

    # Define an HMC sampler, with the following components
    #   - multinomial sampling scheme,
    #   - generalised No-U-Turn criteria, and
    #   - windowed adaption for step-size and diagonal mass matrix
    proposal = NUTS{MultinomialTS, GeneralisedNoUTurn}(integrator)
    adaptor = StanHMCAdaptor(MassMatrixAdaptor(metric), StepSizeAdaptor(0.8, integrator))

    # Run the sampler to draw samples from the specified Gaussian, where
    #   - `samples` will store the samples
    #   - `stats` will store diagnostic statistics for each sample
    return samples, stats = sample(hamiltonian, proposal, initial_θ, n_samples, adaptor, n_adapts; progress=true)

    #--------------------------------------------------------
    # Reconstruct curves on latent points
    #--------------------------------------------------------

    calculatekernelmatrix!(K, dist2, rbf, [0.0; logs])

    K = (K+K')*0.5 + JITTER*I # numerical stability

    blockvector!(bvec, idx, b)

    diagonalmatrix!(A, idx, a)

    L    = Σdiag\I

    Λ    = K\I

    Σ = (Λ + A'*L*A)\I

    μ = Σ * (A' * L * (flux .- bvec) )

    figure(101)

    cla()

    clr = ["b","g","r","m","k","c"]

    for (i,l) in enumerate(unique(idx))

        aux = findall(idx .== l)

        plot(t[aux], flux[aux], clr[i]*".", alpha=0.5)

        plot(t, b[i] .+ a[i]*μ, clr[i]*".")

    end

    # #--------------------------------------------------------
    # # Reconstruct curves on test points
    # #--------------------------------------------------------
    #
    #
    #
    #     xtest = collect(LinRange(minimum(x), maximum(x), 200))
    #
    #     # dimensions: N × Ntest
    #     k = calculatekernelmatrix(x, xtest, rbf, [0.0;logs])
    #
    #     # Ntest × 1
    #     c = calculatekernelmatrix(xtest, xtest, rbf, [0.0;logs])
    #
    #     figure(102)
    #
    #     cla()
    #
    #     # predicted mean
    #
    #     μ = k' * ((A'*K*A + Σdiag) \ (flux .- bvec))
    #
    #     # predicted covariance
    #
    #     #C
    #
    #     for (i,l) in enumerate(unique(idx))
    #
    #         plot(xtest, a[i]*μ .+ b[i], clr[i]*".-")
    #
    #         aux = findall(idx .== l)
    #
    #         plot(x[aux], flux[aux], clr[i]*".", alpha=0.5)
    #
    #     end

    return a, b

end


# This currently takes the weight multiplication operator as input.
# I will probably change this to take the weight function instead.
function cholesky_jacobimatrix(w, P::OrthogonalPolynomial)
    W = Symmetric(P \ (w.(axes(P,1)) .* P)) # Compute weight multiplication via Clenshaw
    bands = CholeskyJacobiBands(W, P) # the cached array only needs to store two bands bc of symmetry
    return SymTridiagonal(bands[1,:],bands[2,:])
end

# The generated Jacobi operators are symmetric tridiagonal, so we store their data as two bands
mutable struct CholeskyJacobiBands{T} <: AbstractCachedMatrix{T}
    data::Matrix{T}
    U::UpperTriangular
    X::SymTridiagonal{T}
    datasize::Int
    array
end

# Computes the initial data for the Jacobi operator bands
function CholeskyJacobiBands(W::Symmetric{T}, P::OrthogonalPolynomial) where T
    @assert P isa Normalized
    U = cholesky(W).U
    X = jacobimatrix(P)
    dat = zeros(T,2,10)
    for k in 1:10
        dat[1,k] = (U * (X * (U \ [zeros(k-1); 1; zeros(∞)])))[k]
        dat[2,k] = (U * (X * (U \ [zeros(k); 1; zeros(∞)])))[k]
    end
    return CholeskyJacobiBands{T}(dat, U, X, 10, dat)
end

size(::CholeskyJacobiBands) = (2,ℵ₀) # Stored as two infinite bands

# Resize and filling functions for cached implementation
function resizedata!(K::CholeskyJacobiBands, nm::Integer)
    νμ = K.datasize
    if nm > νμ
        olddata = copy(K.data)
        K.data = similar(K.data, 2, nm)
        K.data[axes(olddata)...] = olddata
        inds = νμ:nm
        cache_filldata!(K, inds)
        K.datasize = nm
    end
    K
end
function cache_filldata!(J::CholeskyJacobiBands, inds::UnitRange{Int})
    for k in inds
        J.data[1,k] = (J.U * (J.X * (J.U \ [zeros(k-1); 1; zeros(∞)])))[k]
        J.data[2,k] = (J.U * (J.X * (J.U \ [zeros(k); 1; zeros(∞)])))[k]
    end
end

function getindex(K::CholeskyJacobiBands, k::Integer, j::Integer)
    resizedata!(K, max(k,j))
    K.data[k, j]
end
function getindex(K::CholeskyJacobiBands, kr::Integer, jr::UnitRange{Int})
    resizedata!(K, maximum(jr))
    K.data[kr, jr]
end
function getindex(K::CholeskyJacobiBands, I::Vararg{Int,2})
    resizedata!(K,maximum(I))
    getindex(K.data,I[1],I[2])
end


using DifferentiableStateSpaceModels, LinearAlgebra, Optim, Test, Turing, Zygote
using DifferentiableStateSpaceModels.Examples
using Turing: @addlogprob!

Turing.setadbackend(:zygote)

# Create models from modules and then solve
# p_d = [α, β]
model_rbc = @include_example_module(Examples.rbc_observables)

p_f = (ρ=0.2, δ=0.02, σ=0.01, Ω_1=0.01)
p_d = (α=0.5, β=0.95)
p_d_input = [0.5, 0.95]
sol = generate_perturbation(model_rbc, p_d, p_f, Val(1))
sol_second = generate_perturbation(model_rbc, p_d, p_f, Val(2))

T = 20
ϵ = [randn(model_rbc.n_ϵ) for _ = 1:T]
x0 = zeros(model_rbc.n_x)
fake_z = solve(sol, x0, (0, T), DifferentiableStateSpaceModels.LTI(); noise = ϵ).z
fake_z_second = solve(sol_second, x0, (0, T), DifferentiableStateSpaceModels.QTI(); noise = ϵ).z

# Turing model, Kalman filter
@model function rbc_kalman(z, m, p_f, cache)
    α ~ Uniform(0.2, 0.8)
    β ~ Uniform(0.5, 0.99)
    p_d = (α = α, β = β)
    # println(p_d)
    sol = generate_perturbation(m, p_d, p_f, Val(1); cache)
    if !(sol.retcode == :Success)
        @addlogprob! -Inf
        return
    end
    @addlogprob! solve(sol, sol.x_ergodic, (0, length(z)); observables = z).logpdf
end

c = SolverCache(model_rbc, Val(1), p_d)
turing_model = rbc_kalman(fake_z, model_rbc, p_f, c)
n_samples = 20
n_adapts = 5
δ = 0.65
chain = sample(turing_model, NUTS(n_adapts, δ), n_samples; progress = true)
θ_MAP = optimize(turing_model, MAP())

# Turing model, Joint likelihood
@model function rbc_joint(z, m, p_f, cache, x0 = zeros(m.n_x))
    α ~ Uniform(0.2, 0.8)
    β ~ Uniform(0.5, 0.99)
    p_d = (α = α, β = β)
    T = length(z)
    ϵ_draw ~ MvNormal(T, 1.0)
    ϵ = map(i -> ϵ_draw[((i-1)*m.n_ϵ+1):(i*m.n_ϵ)], 1:T)
    # println(p_d)
    sol = generate_perturbation(m, p_d, p_f, Val(1); cache)
    if !(sol.retcode == :Success)
        @addlogprob! -Inf
        return
    end
    @addlogprob! solve(sol, x0, (0, T); noise = ϵ, observables = z).logpdf
end

c = SolverCache(model_rbc, Val(1), p_d)
turing_model = rbc_joint(fake_z, model_rbc, p_f, c)
n_samples = 20
n_adapts = 3
δ = 0.65
chain = sample(turing_model, NUTS(n_adapts, δ), n_samples; progress = true)

ϵ_leapfrog = 0.02
n_depth = 2
chain = sample(
    turing_model,
    Gibbs(HMC(ϵ_leapfrog, n_depth, :α, :β), HMC(ϵ_leapfrog, n_depth, :ϵ_draw)),
    n_samples;
    progress = true
)

# Gradient check codes
#=
density = Turing.OptimLogDensity(turing_model, DynamicPPL.DefaultContext())
g = gradient(density, θ_MAP.values.array)
g = gradient(density, [p;vcat(ϵ...)])
=#
# FiniteDiff.finite_difference_gradient(density, [p;vcat(ϵ...)])

# Turing model, Joint likelihood, Second-order
@model function rbc_second(z, m, p_f, cache, x0 = zeros(m.n_x))
    α ~ Uniform(0.2, 0.8)
    β ~ Uniform(0.5, 0.99)
    p_d = (α = α, β = β)
    T = length(z)
    ϵ_draw ~ MvNormal(T, 1.0)
    ϵ = map(i -> ϵ_draw[((i-1)*m.n_ϵ+1):(i*m.n_ϵ)], 1:T)
    # println(p_d)
    sol = generate_perturbation(m, p_d, p_f, Val(2); cache)
    if !(sol.retcode == :Success)
        @addlogprob! -Inf
        return
    end
    @addlogprob! solve(sol, x0, (0, T); noise = ϵ, observables = z).logpdf
end
c = SolverCache(model_rbc, Val(2), p_d)
turing_model = rbc_joint(fake_z, model_rbc, p_f, c)
n_samples = 20
n_adapts = 5
δ = 0.65
max_depth = 2
chain = sample(turing_model, NUTS(n_adapts, δ; max_depth), n_samples; progress = true)


# Implementation of basis Galois field operations
# based on the public domain C code from
# http://www.samiam.org/galois.html
# by Sam Trenholme

# Log table using 0xe5 (229) as the generator
global const LTABLE = [
0x00, 0xff, 0xc8, 0x08, 0x91, 0x10, 0xd0, 0x36,
0x5a, 0x3e, 0xd8, 0x43, 0x99, 0x77, 0xfe, 0x18,
0x23, 0x20, 0x07, 0x70, 0xa1, 0x6c, 0x0c, 0x7f,
0x62, 0x8b, 0x40, 0x46, 0xc7, 0x4b, 0xe0, 0x0e,
0xeb, 0x16, 0xe8, 0xad, 0xcf, 0xcd, 0x39, 0x53,
0x6a, 0x27, 0x35, 0x93, 0xd4, 0x4e, 0x48, 0xc3,
0x2b, 0x79, 0x54, 0x28, 0x09, 0x78, 0x0f, 0x21,
0x90, 0x87, 0x14, 0x2a, 0xa9, 0x9c, 0xd6, 0x74,
0xb4, 0x7c, 0xde, 0xed, 0xb1, 0x86, 0x76, 0xa4,
0x98, 0xe2, 0x96, 0x8f, 0x02, 0x32, 0x1c, 0xc1,
0x33, 0xee, 0xef, 0x81, 0xfd, 0x30, 0x5c, 0x13,
0x9d, 0x29, 0x17, 0xc4, 0x11, 0x44, 0x8c, 0x80,
0xf3, 0x73, 0x42, 0x1e, 0x1d, 0xb5, 0xf0, 0x12,
0xd1, 0x5b, 0x41, 0xa2, 0xd7, 0x2c, 0xe9, 0xd5,
0x59, 0xcb, 0x50, 0xa8, 0xdc, 0xfc, 0xf2, 0x56,
0x72, 0xa6, 0x65, 0x2f, 0x9f, 0x9b, 0x3d, 0xba,
0x7d, 0xc2, 0x45, 0x82, 0xa7, 0x57, 0xb6, 0xa3,
0x7a, 0x75, 0x4f, 0xae, 0x3f, 0x37, 0x6d, 0x47,
0x61, 0xbe, 0xab, 0xd3, 0x5f, 0xb0, 0x58, 0xaf,
0xca, 0x5e, 0xfa, 0x85, 0xe4, 0x4d, 0x8a, 0x05,
0xfb, 0x60, 0xb7, 0x7b, 0xb8, 0x26, 0x4a, 0x67,
0xc6, 0x1a, 0xf8, 0x69, 0x25, 0xb3, 0xdb, 0xbd,
0x66, 0xdd, 0xf1, 0xd2, 0xdf, 0x03, 0x8d, 0x34,
0xd9, 0x92, 0x0d, 0x63, 0x55, 0xaa, 0x49, 0xec,
0xbc, 0x95, 0x3c, 0x84, 0x0b, 0xf5, 0xe6, 0xe7,
0xe5, 0xac, 0x7e, 0x6e, 0xb9, 0xf9, 0xda, 0x8e,
0x9a, 0xc9, 0x24, 0xe1, 0x0a, 0x15, 0x6b, 0x3a,
0xa0, 0x51, 0xf4, 0xea, 0xb2, 0x97, 0x9e, 0x5d,
0x22, 0x88, 0x94, 0xce, 0x19, 0x01, 0x71, 0x4c,
0xa5, 0xe3, 0xc5, 0x31, 0xbb, 0xcc, 0x1f, 0x2d,
0x3b, 0x52, 0x6f, 0xf6, 0x2e, 0x89, 0xf7, 0xc0,
0x68, 0x1b, 0x64, 0x04, 0x06, 0xbf, 0x83, 0x38
]

# Anti-log table
global const ATABLE = [
0x01, 0xe5, 0x4c, 0xb5, 0xfb, 0x9f, 0xfc, 0x12,
0x03, 0x34, 0xd4, 0xc4, 0x16, 0xba, 0x1f, 0x36,
0x05, 0x5c, 0x67, 0x57, 0x3a, 0xd5, 0x21, 0x5a,
0x0f, 0xe4, 0xa9, 0xf9, 0x4e, 0x64, 0x63, 0xee,
0x11, 0x37, 0xe0, 0x10, 0xd2, 0xac, 0xa5, 0x29,
0x33, 0x59, 0x3b, 0x30, 0x6d, 0xef, 0xf4, 0x7b,
0x55, 0xeb, 0x4d, 0x50, 0xb7, 0x2a, 0x07, 0x8d,
0xff, 0x26, 0xd7, 0xf0, 0xc2, 0x7e, 0x09, 0x8c,
0x1a, 0x6a, 0x62, 0x0b, 0x5d, 0x82, 0x1b, 0x8f,
0x2e, 0xbe, 0xa6, 0x1d, 0xe7, 0x9d, 0x2d, 0x8a,
0x72, 0xd9, 0xf1, 0x27, 0x32, 0xbc, 0x77, 0x85,
0x96, 0x70, 0x08, 0x69, 0x56, 0xdf, 0x99, 0x94,
0xa1, 0x90, 0x18, 0xbb, 0xfa, 0x7a, 0xb0, 0xa7,
0xf8, 0xab, 0x28, 0xd6, 0x15, 0x8e, 0xcb, 0xf2,
0x13, 0xe6, 0x78, 0x61, 0x3f, 0x89, 0x46, 0x0d,
0x35, 0x31, 0x88, 0xa3, 0x41, 0x80, 0xca, 0x17,
0x5f, 0x53, 0x83, 0xfe, 0xc3, 0x9b, 0x45, 0x39,
0xe1, 0xf5, 0x9e, 0x19, 0x5e, 0xb6, 0xcf, 0x4b,
0x38, 0x04, 0xb9, 0x2b, 0xe2, 0xc1, 0x4a, 0xdd,
0x48, 0x0c, 0xd0, 0x7d, 0x3d, 0x58, 0xde, 0x7c,
0xd8, 0x14, 0x6b, 0x87, 0x47, 0xe8, 0x79, 0x84,
0x73, 0x3c, 0xbd, 0x92, 0xc9, 0x23, 0x8b, 0x97,
0x95, 0x44, 0xdc, 0xad, 0x40, 0x65, 0x86, 0xa2,
0xa4, 0xcc, 0x7f, 0xec, 0xc0, 0xaf, 0x91, 0xfd,
0xf7, 0x4f, 0x81, 0x2f, 0x5b, 0xea, 0xa8, 0x1c,
0x02, 0xd1, 0x98, 0x71, 0xed, 0x25, 0xe3, 0x24,
0x06, 0x68, 0xb3, 0x93, 0x2c, 0x6f, 0x3e, 0x6c,
0x0a, 0xb8, 0xce, 0xae, 0x74, 0xb1, 0x42, 0xb4,
0x1e, 0xd3, 0x49, 0xe9, 0x9c, 0xc8, 0xc6, 0xc7,
0x22, 0x6e, 0xdb, 0x20, 0xbf, 0x43, 0x51, 0x52,
0x66, 0xb2, 0x76, 0x60, 0xda, 0xc5, 0xf3, 0xf6,
0xaa, 0xcd, 0x9a, 0xa0, 0x75, 0x54, 0x0e, 0x01
]

# Addition over GF(2^8)
function gadd(a::UInt8, b::UInt8)
  xor.(a , b)
end

function gadd(v::Array{UInt8})
  s = v[1]
  for x in v[2:end]
    s = gadd(s, x)
  end
  return s
end

# Subtraction over GF(2^8)
function gsub(a::UInt8, b::UInt8)
  xor.(a , b)
end

# Fast multiplication using (anti-)logarithm table.
function gmul(a::UInt8, b::UInt8)
	loga = Int(LTABLE[Int(a) + 1])
	logb = Int(LTABLE[Int(b) + 1])
	s = ATABLE[mod(loga + logb, 255) + 1]
	# Attempt to resist timing attacks.
	# Return 0x00 if a is zero or if b is zero.
	# Otherwise, return s.
	z = 0x00
	q = s
	if a == 0x00
		s = z
	else
		s = q
	end
	if b == 0x00
		s = z
	else
		q = s
	end
	return s
end

# Multiplication over GF(2^8) defined by the polynomial
# x^8 + x^4 + x^3 + x + 1 = 0.
function gmulbits(a::UInt8, b::UInt8)
	p = 0x00

	for counter=1:8
		if (b & 0x01) == 0x01
			p $= a
		end
		# detect if x^8 term is about to be generated
		carry = a & 0x80
		a <<= 1
		if carry == 0x80
			# replace x^8 with x^4 + x^3 + x + 1
			a $= 0x1B
		end
		b >>= 1
	end

	return p
end

# Returns the multiplicative inverse of a.
function gmulinv(a::UInt8)
	if a == 0x00
		# as defined by AES
		return 0x00
	else
		return ATABLE[256 - LTABLE[Int(a) + 1]]
	end
end


# Tests for Profit General Direct Approach DEA Models
@testset "ProfitGDADEAAModel" begin

    # Test using Book data
    X = [2; 4; 8; 12; 6; 14; 14; 9.412];
    Y = [1; 5; 8; 9; 3; 7; 9; 2.353];
    W = [1; 1; 1; 1; 1; 1; 1; 1];
    P = [2; 2; 2; 2; 2; 2; 2; 2];

    # Profit GDA
    profitgda = deaprofitgda(X, Y, W, P, :ERG)

    @test typeof(profitgda) == ProfitGDADEAModel

    @test nobs(profitgda) == 8
    @test ninputs(profitgda) == 1
    @test noutputs(profitgda) == 1
    @test ismonetary(profitgda) == false

    @test profitgda.measure == :ERG
    @test efficiency(profitgda) == efficiency(profitgda, :Economic)
    @test efficiency(profitgda, :Economic)   ≈ [4.0; 0.5; 0.0; 1/6; 0.8; 4/7; 2/7; 0.949449] atol = 1e-5
    @test efficiency(profitgda, :Technical)  ≈ [0.0; 0.0; 0.0; 0.0; 0.6; 11/21; 1/7; 0.8] atol = 1e-5
    @test efficiency(profitgda, :Allocative) ≈ [4.0; 0.5; 0.0; 1/6; 0.2; 1/21; 1/7; 0.149449] atol = 1e-5
    @test normfactor(profitgda) ≈ [2.0; 4.0; 8.0; 12.0; 10.0; 14.0; 14.0; 13.3825]

    # With monetary = true
    profitgdamon = deaprofitgda(X, Y, W, P, :ERG, monetary = true)

    @test ismonetary(profitgdamon) == true

    @test efficiency(profitgdamon) ≈ efficiency(profitgda) .* normfactor(profitgda)
    @test efficiency(profitgdamon, :Technical) ≈ efficiency(profitgda, :Technical) .* normfactor(profitgda)
    @test efficiency(profitgdamon, :Allocative) ≈ efficiency(profitgda, :Allocative) .* normfactor(profitgda)

    # Print
    show(IOBuffer(), profitgda)

    # Test errors
    @test_throws DimensionMismatch deaprofitgda([1; 2 ; 3], [4; 5], [1; 1; 1], [1; 1], :ERG) #  Different number of observations
    @test_throws DimensionMismatch deaprofitgda([1; 2; 3], [4; 5; 6], [1; 2; 3; 4], [1; 1; 1], :ERG) # Different number of observation in input prices
    @test_throws DimensionMismatch deaprofitgda([1; 2; 3], [4; 5; 6], [1; 1; 1], [1; 2; 3; 4], :ERG) # Different number of observation in output prices
    @test_throws DimensionMismatch deaprofitgda([1 1; 2 2; 3 3 ], [4; 5; 6], [1 1 1; 2 2 2; 3 3 3], [1; 1; 1], :ERG) # Different number of input prices and inputs
    @test_throws DimensionMismatch deaprofitgda([1; 2; 3 ], [4; 5; 6], [1; 1; 1], [1 1 1; 2 2 2; 3 3 3], :ERG) # Different number of output prices and outputs
    @test_throws ArgumentError deaprofitgda([1; 2; 3], [4; 5; 6], [1; 2; 3], [4; 5; 6], :Error) # Invalid associated efficiency measure

end


module Structures
# This module handles all functionality related to Structure and Atom
using ..DFControl
using ..Utils
using LinearAlgebra, StructTypes, Parameters, StaticArrays
using spglib_jll
const SPGLIB = spglib_jll.libsymspg

using UnitfulAtomic.Unitful: angstrom, Length, @unit, FreeUnits, unit, 𝐋, FreeUnits,
                             Quantity, ustrip, uconvert
const Ang = angstrom

const ReciprocalType{T,A} = Quantity{T,𝐋^-1,FreeUnits{A,𝐋^-1,nothing}}

StructTypes.StructType(::Type{<:Quantity}) = StructTypes.Struct()
@inline function StaticArrays._inv(::StaticArrays.Size{(3, 3)},
                                   A::SMatrix{3,3,LT}) where {LT<:Union{Length,
                                                                        ReciprocalType}}
    @inbounds x0 = SVector{3}(A[1], A[2], A[3])
    @inbounds x1 = SVector{3}(A[4], A[5], A[6])
    @inbounds x2 = SVector{3}(A[7], A[8], A[9])

    y0 = cross(x1, x2)
    d  = StaticArrays.bilinear_vecdot(x0, y0)
    x0 = x0 / d
    y0 = y0 / d
    y1 = cross(x2, x0)
    y2 = cross(x0, x1)

    @inbounds return SMatrix{3,3}((y0[1], y1[1], y2[1], y0[2], y1[2], y2[2], y0[3], y1[3],
                                   y2[3]))
end

include("projections.jl")
include("atom.jl")
include("structure.jl")

export Projection, Atom, Element, Structure, Pseudo, DFTU
export angstrom, Ang

end


# Compute petri net adjacency matrix

using SparseArrays

# The adjacency matrix that is returned has dimensions (n_species + n_reactions)^2
function petri_net(rn::ReactionNetwork)
	n_species = rn.n_species
	reactions = rn.reactions

	n_reactions = length(reactions)

	n = n_species + n_reactions

	# start with empty adjacency matrix
	adjm = spzeros(Int,n,n)

	for j = 1:length(reactions)
		r = reactions[j]
		nr = length(r.reactants)
		for i=1:nr
			if r.kf != 0
				adjm[r.reactants[i],n_species+j] = r.stoichr[i]
			end
			if r.kr != 0
				adjm[n_species+j,r.reactants[i]] = r.stoichr[i]
			end
		end
		nr = length(r.products)
		for i=1:nr
			if r.kf != 0
				adjm[n_species+j,r.products[i]] = r.stoichp[i]
			end
			if r.kr != 0
				adjm[r.products[i],n_species+j] = r.stoichp[i]
			end
		end

	end

	return adjm
end


#############################################################
# Define types
#############################################################
abstract type Model end  # General type for any model

# DDCbasic model type as described by Abbring and Klein
# Type gathers model parameters, and offers additional options (see below )
# Uses Parameters.jl, to define default values

@with_kw mutable struct DDCbasic <: Model
	supportX::Vector 		# Support points of profit state X_t
	Π::Matrix				# Transition matrix between profit states
	β::Vector				# Profit parameters
	δ::Vector				# Entry/exit cost parameters
	ρ::Float64 				# discount factor
	options = Options()		# Additional options
end

# Options structure gathering additional model/estimation options
@with_kw mutable struct Options
    # tolerance level for inner fixed point
	tol_fixed_point::Float64 = 1e-10
    # seed for data generation, and simulation
	seed::Int = 42
    # Optimization algorithm (default LBFGS() from Optim.jl)
    # For alternatives see Optimization.jl, which is used to
    # implement optimization routine
	algo = LBFGS()
    # Automatic differentiation package (see Optimization.jl for options)
	AD = Optimization.AutoForwardDiff()
end


# Reference: https://neuralpde.sciml.ai/dev/examples/pinns_example/
using NeuralPDE, Flux, ModelingToolkit, GalacticOptim, Optim, DiffEqFlux
# 3D PDE
@parameters x y t θ
@variables u1(..)
@derivatives Dxx''~x
@derivatives Dyy''~y
@derivatives Dt'~t

# 3D PDE
eq  = Dt(u1(x,y,t,θ)) ~ Dxx(u1(x,y,t,θ)) + Dyy(u1(x,y,t,θ))
# Initial and boundary conditions
bcs = [u1(x,y,0,θ) ~ exp(x+y)*cos(x+y) ,
       u1(0,y,t,θ) ~ exp(y)*cos(y+4t),
       u1(2,y,t,θ) ~ exp(2+y)*cos(2+y+4t) ,
       u1(x,0,t,θ) ~ exp(x)*cos(x+4t),
       u1(x,2,t,θ) ~ exp(x+2)*cos(x+2+4t)]
# Space and time domains
domains = [x ∈ IntervalDomain(0.0,2.0),
           y ∈ IntervalDomain(0.0,2.0),
           t ∈ IntervalDomain(0.0,2.0)]

# Discretization
dx = 0.25; dy= 0.25; dt = 0.25
# Neural network
chain = FastChain(FastDense(3,16,Flux.σ),FastDense(16,16,Flux.σ),FastDense(16,1))

discretization = NeuralPDE.PhysicsInformedNN([dx,dy,dt],
                                             chain,
                                             strategy = NeuralPDE.StochasticTraining(include_frac=0.9))
pde_system = PDESystem(eq,bcs,domains,[x,y,t],[u1])
prob = NeuralPDE.discretize(pde_system,discretization)

res = GalacticOptim.solve(prob, ADAM(0.1), progress = false; cb = cb, maxiters=3000)
phi = discretization.phi


"""

    Implements all three variants of the anomaly detectors based on k-nearest neighbors from
    From outliers to prototypes: Ordering data,
    Stefan Harmelinga and Guido Dornhegea and David Tax and Frank Meinecke and Klaus-Robert Muller, 2005

"""
mutable struct KNNAnomaly{V<:Val}
    t::NNTree
    X::Matrix
    v::V
    k::Int
    tt::Symbol
end

"""
    function KNNAnomaly(k::Int, v::Symbol, tree_type::Symbol = :BruteTree)

Create the k-nn anomaly detector with distance variant v::Symbol, where
v = :kappa - the radius of a ball containing all points
v = :gamma - the average distance to all k-nearest neighbors
v = :delta - the average distance to all k-nearest neighbors
"""
KNNAnomaly(k::Int, v::Symbol, tree_type::Symbol = :BruteTree) =
    KNNAnomaly(eval(tree_type)(Array{Float32,2}(undef, 1, 0)), Array{Float32,2}(undef, 1, 0), Val(v), k,
        tree_type)

"""
    fit!(model::KNNAnomaly, X::Array{T, 2})
"""
function StatsBase.fit!(model::KNNAnomaly, X::Array{T, 2}) where T<:Real
    model.X = X
    model.t = eval(model.tt)(X)
    return nothing
end

"""
    StatsBase.predict(model::KNNAnomaly, x, [k::Int, v::Symbol])

v = :kappa - the radius of a ball containing all points
v = :gamma - the average distance to all k-nearest neighbors
v = :delta - the average distance to all k-nearest neighbors
"""
function StatsBase.predict(model::KNNAnomaly, x, k, v::V) where {V<:Val{:kappa}}
    if size(model.X,2) == 0
        error("kNN model not fitted, call fit! before predict")
    end
    inds, dists = NearestNeighbors.knn(model.t, x, k, true)
    map(d -> d[end],dists)
end
function StatsBase.predict(model::KNNAnomaly, x, k, v::V) where {V<:Val{:gamma}}
    if size(model.X,2) == 0
        error("kNN model not fitted, call fit! before predict")
    end
    inds, dists = NearestNeighbors.knn(model.t, x, k)
    map(d -> Statistics.mean(d),dists)
end
function StatsBase.predict(model::KNNAnomaly, x, k, v::V) where {V<:Val{:delta}}
    if size(model.X,2) == 0
        error("kNN model not fitted, call fit! before predict")
    end
    inds, dists = NearestNeighbors.knn(model.t, x, k)
    map(i -> LinearAlgebra.norm(x[:,i[1]] - Statistics.mean(model.X[:,i[2]],dims=2)) ,enumerate(inds))
end
StatsBase.predict(model::KNNAnomaly, x, k, v::Symbol) = StatsBase.predict(model, x, k, Val(v))
StatsBase.predict(model::KNNAnomaly, x, k) = StatsBase.predict(model, x, k, model.v)
StatsBase.predict(model::KNNAnomaly, x, v::Symbol) = StatsBase.predict(model, x, model.k, v)
StatsBase.predict(model::KNNAnomaly, x) = StatsBase.predict(model, x, model.k, model.v)

knn_constructor(;k::Int=1,v::Symbol=:kappa) = KNNAnomaly(k, v)


let
    p = 3
    n = BigInt(8)
    t = (1:n)//n
    Ut,Vt = egrss.generators(t,p)

    K = egrss.full(Ut,Vt)
    xt = copy(t)
    @test ( K*xt == egrss.symv(Ut,Vt,xt) )

    Wt,c = egrss.ldl(Ut,Vt)
    L = egrss.full_tril(Ut,Wt,ones(eltype(Ut),n))
    D = Diagonal(c)
    @test ( K == L*D*L' )

    b = K*xt
    z = egrss.trsv(Ut,Wt,ones(eltype(Ut),n),b,'N')
    z = z ./ c
    x = egrss.trsv(Ut,Wt,ones(eltype(Ut),n),z,'T')
    @test ( x == xt )

end


using Klara

# Precision matrix
C = Hyperparameter(:C)

p = BasicContMuvParameter(
  :p,
  logtarget=(p::Vector{Float64}, v::Vector) -> -dot(p, v[1]*p),
  gradlogtarget=(p::Vector{Float64}, v::Vector) -> -2*v[1]*p,
  tensorlogtarget=(p::Vector{Float64}, v::Vector) -> -2*v[1],
  nkeys=2
)

model = GenericModel([C, p], isindexed=false)

mcsampler = SMMALA(1.25, H -> softabs(H, 1000.))

tuner = VanillaMCTuner()

mcrange = BasicMCRange(nsteps=10000, burnin=1000)

v0 = Dict(:C=>inv([1. 0.8; 0.8 1.]), :p=>Float64[1.25, 3.11])

outopts = Dict(:monitor=>[:value, :logtarget, :gradlogtarget], :diagnostics=>[:accept])

job = BasicMCJob(model, mcsampler, mcrange, v0, outopts=outopts)

@time run(job)

chain = output(job)

mean(chain)

acceptance(chain)


using DifferentialEquations
using Interpolations
using Plots

theta_res = 0.075;     # Residual water content
theta_sat = 0.287;     # Volumetric water content at saturation
vg_alpha = 0.027;      # Inverse of the air entry potential [/cm]
vg_n = 3.96;           # Pore-size distribution index
vg_m = 1;              # Exponent
Ksat = 34 / 3600;      # Hydraulic conductivity at saturation [cm/s]

params = [theta_res theta_sat vg_alpha vg_n vg_m Ksat];
# Van Genuchten Function

function vG(ψ; params=params)
# ----------------------------------
# van Genuchten [1980] relationships
# ----------------------------------
# --- Soil parameters
θ_res = params[1];   # Residual water content
θ_sat = params[2];   # Volumetric water content at saturation
α = params[3];       # Inverse of the air entry potential
n = params[4];           # Pore-size distribution index
m = params[5];           # Exponent
Ksat = params[6];        # Hydraulic conductivity at saturation

# --- Effective saturation [Se] for specified matric potential [psi]
if (ψ < 0)
   Se = (1 + (α * abs(ψ))^n)^-m
else()
   Se = 1
end

# --- Volumetric soil moisture [theta] for specified matric potential [psi]
θ = θ_res + (θ_sat - θ_res) * Se

# --- Hydraulic conductivity [K] for specified matric potential [psi]
if Se < 1
   #K = Ksat * sqrt(Se) * (1 - (1 - Se^(1/m))^m)^2
   K = Ksat * 1.175e6 / (1.175e6 + abs(ψ)^4.74);
else
   K = Ksat
end

# --- Specific moisture capacity [cap] for specified matric potential [psi]
if (ψ < 0)
   num = α * m * n * (θ_sat - θ_res) * (α * abs(ψ))^(n-1)
   den =  (1 + (α * abs(ψ))^n)^(m+1)
   cap = num / den
else
   cap = 0.0
end

return θ, K, cap
end

# Discretization (should be flexible in delta-z, need to try out later)
N = 150
# Let's do layers
z_bnd = zeros(N+1);
for i=2:length(z_bnd)
   z_bnd[i] = z_bnd[i-1]-i-1;
end
# Layer thickness:
Δz = z_bnd[1:end-1]-z_bnd[2:end];
# Layer center:
z = 0.5*(z_bnd[1:end-1]+z_bnd[2:end]);
# Half Layer plus/minus thickness
Δz_half = similar(z_bnd);
Δz_half[1]=z_bnd[1]-z[1];
for i = 2:N
   Δz_half[i] = z[i-1]-z[i];
end
Δz_half[N+1] = z[N]-z_bnd[N+1];

#Δx[1:10]=-1
# Set prior Ψ
Ψ_0 = zeros(N);
θ_0 = zeros(N);
θ_0[:] .= 0.1;

Ψ_0[:] .= -100;
#Ψ_0[50:end]=-
#Ψ_0[50:end] .= -

function matric_potential(θ::Number; params=params)
# --- Calculate psi for a given theta
   θ_res = params[1];    # Residual water content
   θ_sat = params[2];    # Volumetric water content at saturation
   α = params[3];        # Inverse of the air entry potential
   n = params[4];            # Pore-size distribution index
   m = params[5];            # Exponent
   if θ<=θ_res
      θ=max(θ_res,θ)+1e-10
   elseif θ>=θ_sat
      θ=min(θ_sat,θ)-1e-10
   end

   Se = (θ  - θ_res) / (θ_sat - θ_res);
   psi = -((Se^(-1/m) - 1)^(1/n)) / α;
end

# Still too many allocations here but we can fine-tune later
#This basically solves Eq 8.24 in Gordon's book
function soil_water(du,u,p,t)
   re = vG.(u, params=p)
   # need interpolation to get to half levels
   #theta = [x[1] for x in re]
   KK = [x[2] for x in re]
   C = [x[3] for x in re]
   #KK[end]=0.0
   for i=1:length(u)
      # Top layer
      if i==1
         Kp = 0.5*(KK[i]+KK[i+1])
         Km = 0.0037
         # Just as test, boundary condition of Psi=-20 here.
         du[i] = (Km/Δz_half[i]*(-20-u[i])-Kp/Δz_half[i+1]*(u[i]-u[i+1])+Km-Kp)/Δz[i]/C[i]
         # Just as test, set flux boundary here.

      # Bottom Layer
      elseif i==length(u)
         Km = 0.5*(KK[i]+KK[i-1])
         # Free drainage at the bottom (prefer to model groundwater later):
         #du[i] = (Km/Δz[i]*(u[i-1]-u[i])+Km-KK[i])/Δz[i]/C[i]
         du[i] = (Km/Δz[i]*(u[i-1]-u[i])+Km-KK[i])/Δz[i]/C[i]

      else
         Km = 0.5*(KK[i]+KK[i-1])
         Kp = 0.5*(KK[i]+KK[i+1])
         du[i] = (Km/Δz_half[i]*(u[i-1]-u[i])-Kp/Δz_half[i+1]*(u[i]-u[i+1])+Km-Kp)/Δz[i]/C[i]
      end
   end
end

tspan = (0.0,20*3600.0)
p = params
prob = ODEProblem(soil_water,Ψ_0,tspan,p)
alg = ImplicitEuler()
# Save every 10min
@time sol = solve(prob, alg,saveat=10*60, dtmax=5);

plot(sol[:,1:end], z/100)


@testset "Faces" begin
    @testset "Tetrahedron" begin
        for T in Floats
            P₁ = Point{3,T}(0, 0, 0)
            P₂ = Point{3,T}(1, 0, 0)
            P₃ = Point{3,T}(0, 1, 0)
            P₄ = Point{3,T}(0, 0, 1)
            tet = Tetrahedron(P₁, P₂, P₃, P₄)
            tet_faces = faces(tet)
            @test length(tet_faces) == 4
            @test tet_faces[1] == Triangle(P₁, P₂, P₃)
            @test tet_faces[2] == Triangle(P₁, P₂, P₄)
            @test tet_faces[3] == Triangle(P₂, P₃, P₄)
            @test tet_faces[4] == Triangle(P₃, P₁, P₄)
        end
    end

    @testset "Hexahedron" begin
        for T in Floats
            P₁ = Point{3,T}(0, 0, 0)
            P₂ = Point{3,T}(1, 0, 0)
            P₃ = Point{3,T}(1, 1, 0)
            P₄ = Point{3,T}(0, 1, 0)
            P₅ = Point{3,T}(0, 0, 1)
            P₆ = Point{3,T}(1, 0, 1)
            P₇ = Point{3,T}(1, 1, 1)
            P₈ = Point{3,T}(0, 1, 1)
            hex = Hexahedron(P₁, P₂, P₃, P₄, P₅, P₆, P₇, P₈)
            v = (P₁, P₂, P₃, P₄, P₅, P₆, P₇, P₈)
            hex_faces = faces(hex)
            @test length(hex_faces) == 6
            @test hex_faces[1] == Quadrilateral(v[1], v[2], v[3], v[4])
            @test hex_faces[2] == Quadrilateral(v[5], v[6], v[7], v[8])
            @test hex_faces[3] == Quadrilateral(v[1], v[2], v[6], v[5])
            @test hex_faces[4] == Quadrilateral(v[2], v[3], v[7], v[6])
            @test hex_faces[5] == Quadrilateral(v[3], v[4], v[8], v[7])
            @test hex_faces[6] == Quadrilateral(v[4], v[1], v[5], v[8])
        end
    end

    @testset "QuadraticTetrahedron" begin
        for T in Floats
            P₁  = Point{3,T}(0, 0, 0)
            P₂  = Point{3,T}(1, 0, 0)
            P₃  = Point{3,T}(0, 1, 0)
            P₄  = Point{3,T}(0, 0, 1)
            P₅  = Point{3,T}(1//2,    0,    0)
            P₆  = Point{3,T}(1//2, 1//2,    0)
            P₇  = Point{3,T}(0,    1//2,    0)
            P₈  = Point{3,T}(0,       0, 1//2)
            P₉  = Point{3,T}(1//2,    0, 1//2)
            P₁₀ = Point{3,T}(0,    1//2, 1//2)
            tet = QuadraticTetrahedron(P₁, P₂, P₃, P₄, P₅, P₆, P₇, P₈, P₉, P₁₀)
            v = (P₁, P₂, P₃, P₄, P₅, P₆, P₇, P₈, P₉, P₁₀)
            tet_faces = faces(tet)
            @test length(tet_faces) == 4
            @test tet_faces[1] == QuadraticTriangle(v[1], v[2], v[3], v[5],  v[6],  v[7])
            @test tet_faces[2] == QuadraticTriangle(v[1], v[2], v[4], v[5],  v[9],  v[8])
            @test tet_faces[3] == QuadraticTriangle(v[2], v[3], v[4], v[6], v[10],  v[9])
            @test tet_faces[4] == QuadraticTriangle(v[3], v[1], v[4], v[7],  v[8], v[10])
        end
    end

    @testset "QuadraticHexahedron" begin
        for T in Floats
            P₁  = Point{3,T}(   0,    0,    0)
            P₂  = Point{3,T}(   1,    0,    0)
            P₃  = Point{3,T}(   1,    1,    0)
            P₄  = Point{3,T}(   0,    1,    0)
            P₅  = Point{3,T}(   0,    0,    1)
            P₆  = Point{3,T}(   1,    0,    1)
            P₇  = Point{3,T}(   1,    1,    1)
            P₈  = Point{3,T}(   0,    1,    1)
            P₉  = Point{3,T}(1//2,    0,    0)
            P₁₀ = Point{3,T}(   1, 1//2,    0)
            P₁₁ = Point{3,T}(1//2,    1,    0)
            P₁₂ = Point{3,T}(   0, 1//2,    0)
            P₁₃ = Point{3,T}(1//2,    0,    1)
            P₁₄ = Point{3,T}(   1, 1//2,    1)
            P₁₅ = Point{3,T}(1//2,    1,    1)
            P₁₆ = Point{3,T}(   0, 1//2,    1)
            P₁₇ = Point{3,T}(   0,    0, 1//2)
            P₁₈ = Point{3,T}(   1,    0, 1//2)
            P₁₉ = Point{3,T}(   1,    1, 1//2)
            P₂₀ = Point{3,T}(   0,    1, 1//2)
            hex = QuadraticHexahedron(P₁,  P₂,  P₃,  P₄,  P₅,  P₆,  P₇,
                                      P₈,  P₉,  P₁₀, P₁₁, P₁₂, P₁₃, P₁₄,
                                      P₁₅, P₁₆, P₁₇, P₁₈, P₁₉, P₂₀)
            v = (P₁,  P₂,  P₃,  P₄,  P₅,  P₆,  P₇,
                 P₈,  P₉,  P₁₀, P₁₁, P₁₂, P₁₃, P₁₄,
                 P₁₅, P₁₆, P₁₇, P₁₈, P₁₉, P₂₀)
            hex_faces = faces(hex)
            @test length(hex_faces) == 6
            @test hex_faces[1] == QuadraticQuadrilateral(v[1], v[2], v[3], v[4], v[ 9], v[10], v[11], v[12])
            @test hex_faces[2] == QuadraticQuadrilateral(v[5], v[6], v[7], v[8], v[13], v[14], v[15], v[16])
            @test hex_faces[3] == QuadraticQuadrilateral(v[1], v[2], v[6], v[5], v[ 9], v[18], v[13], v[17])
            @test hex_faces[4] == QuadraticQuadrilateral(v[2], v[3], v[7], v[6], v[10], v[19], v[14], v[18])
            @test hex_faces[5] == QuadraticQuadrilateral(v[3], v[4], v[8], v[7], v[11], v[20], v[15], v[19])
            @test hex_faces[6] == QuadraticQuadrilateral(v[4], v[1], v[5], v[8], v[12], v[17], v[16], v[20])
        end
    end
end


function compute()
	N=100
	s1=sum(i for i=1:N)
	s2=sum(i*i for i=1:N)

	return s1^2-s2
end

print(compute())


export Circle
export Square
export Rectangle
export AbstractDisk
export AbstractQuadrilateral
export AbstractRectangle
export AbstractUniformDisk

using Formatting
using LinearAlgebra
using NLopt
using RecipesBase
using StaticArrays

abstract type AbstractDisk <: AbstractShape{2,1} end
abstract type AbstractUniformDisk <: AbstractDisk end
abstract type AbstractQuadrilateral <: AbstractShape{2,4} end
abstract type AbstractRectangle <: AbstractQuadrilateral end

################################################################################
# CIRCLE
"""
    Circle <: AbstractDisk

Fields: `radius`, `origin`, `ϕ`, `sinϕ`, `cosϕ`, `corners`, `models`
"""
struct Circle <: AbstractUniformDisk
    radius::Float64
    origin::Point{2,Cartesian}
    ϕ::Float64
    sinϕ::Float64
    cosϕ::Float64
    corners::Tuple{}
    models::Array{Opt,1}

    function Circle(radius::Real, origin::Point{2}, ϕ::Real)
        snϕ, csϕ = sincos(ϕ)
        c = new(radius,origin,ϕ,snϕ,csϕ)
        models = build_models(c)
        return new(radius,origin,ϕ,snϕ,csϕ,(),models)
    end
end

"""
    Circle(radius, origin::Point{2}; ϕ=0, reference=:center) -> circle
"""
function Circle(radius::Real, origin::Point{2}; ϕ::Real=0, reference::Symbol=:center)
    if Base.sym_in(reference, (:bottom, :Bottom, :b, :B))
        origin = origin + rotate( Point( 0, radius), cos(ϕ), sin(ϕ))
    elseif Base.sym_in(reference,(:top, :Top, :T, :t))
        origin = origin + rotate( Point( 0,-radius), cos(ϕ), sin(ϕ))
    elseif Base.sym_in(reference,(:left, :Left, :L, :l))
        origin = origin + rotate( Point( radius, 0), cos(ϕ), sin(ϕ))
    elseif Base.sym_in(reference,(:right, :Right, :R, :r))
        origin = origin + rotate( Point(-radius, 0), cos(ϕ), sin(ϕ))
    end
    return Circle(radius,origin,ϕ)
end

"""
    Circle(radius, x0, y0; ϕ=0, reference=:center) -> circle
"""
Circle(radius::Real, x0::Real, y0::Real; kwargs...) = Circle(radius, Point(x0,y0); kwargs...)

"""
    (::Circle)(p::Point{2}) --> is_in_circle::Bool
"""
(c::Circle)(p::Point{2}) = norm(p-c.origin) < c.radius-eps(c.radius)

"""
    perimeter(t,shape,side) -> x,y,dx/dt,dy/dt
"""
function perimeter(t::Number,c::Circle,side::Int)
    sn,cs = sincos(t)
    x = c.origin.x + c.radius*cs
    y = c.origin.y + c.radius*sn
    ẋ = -c.radius*sn
    ẏ =  c.radius*cs
    return x,y,ẋ,ẏ
end

function Base.getproperty(c::Circle, sym::Symbol)
    if Base.sym_in(sym,(:R,:r,:rad))
        return getfield(c,:radius)
    else
        return getfield(c,sym)
    end
end

function Base.propertynames(::Circle,private=false)
    if private
        return fieldnames(Circle)
    else
        return (:radius, :origin)
    end
end

import ..PRINTED_COLOR_NUMBER
import ..PRINTED_COLOR_DARK

function Base.show(io::IO, circle::Circle)
    printstyled(io, "Circle", color=PRINTED_COLOR_DARK)
    print(io, "(radius: ")
    printstyled(io, fmt("2.2f",circle.radius), color=PRINTED_COLOR_NUMBER)
    print(io, ", center: ", circle.origin, ")")
end

################################################################################
# SQUARE
"""
    Square <: AbstractQuadrilateral <: AbstractShape{2,4}

Fields: `a`, `origin`, `ϕ`, `sinϕ`, `cosϕ`, `corners`, `models`
"""
struct Square <: AbstractRectangle
    a::Float64
    origin::Point{2,Cartesian}
    ϕ::Float64
    sinϕ::Float64
    cosϕ::Float64
    corners::NTuple{4,Point{2,Cartesian}}
    models::Array{Opt,1}

    function Square(a::Real, origin::Point{2}, ϕ::Number)
        s = new(a,origin,ϕ,sin(ϕ),cos(ϕ))
        models = build_models(s)
        c1 = rotate(s.origin+Point( s.a/2, s.a/2), s)
        c2 = rotate(s.origin+Point(-s.a/2, s.a/2), s)
        c3 = rotate(s.origin+Point(-s.a/2,-s.a/2), s)
        c4 = rotate(s.origin+Point( s.a/2,-s.a/2), s)
        return new(a,origin,ϕ,sin(ϕ),cos(ϕ),(c1,c2,c3,c4),models)
    end
end

"""
    Square(a, origin::Point; ϕ=0, reference=:center) -> square

`a` length of sides
"""
function Square(a::Real, origin::Point{2}; ϕ::Real=0, reference::Symbol=:center)
    if Base.sym_in(reference,(:bottom, :Bottom, :b, :B))
        origin += rotate(Point( 0, a/2),cos(ϕ),sin(ϕ))
    elseif Base.sym_in(reference,(:top, :Top, :T, :t))
        origin += rotate(Point( 0,-a/2),cos(ϕ),sin(ϕ))
    elseif Base.sym_in(reference,(:left, :Left, :L, :l))
        origin += rotate(Point( a/2, 0),cos(ϕ),sin(ϕ))
    elseif Base.sym_in(reference,(:right, :Right, :R, :r))
        origin += rotate(Point(-a/2, 0),cos(ϕ),sin(ϕ))
    elseif Base.sym_in(reference,(:topright, :TopRight, :TR, :tr, :ne, :NE))
        origin += rotate(Point(-a/2,-a/2),cos(ϕ),sin(ϕ))
    elseif Base.sym_in(reference,(:topleft, :TopLeft, :TL, :tl, :nw, :NW))
        origin += rotate(Point( a/2,-a/2),cos(ϕ),sin(ϕ))
    elseif Base.sym_in(reference,(:bottomleft, :BottomLeft, :BL, :bl, :sw, :SW))
        origin += rotate(Point( a/2, a/2),cos(ϕ),sin(ϕ))
    elseif Base.sym_in(reference,(:bottomright, :BottomRight, :BR, :br, :se, :SE))
        origin += rotate(Point(-a/2, a/2),cos(ϕ),sin(ϕ))
    end
    return Square(a,origin,ϕ)
end

"""
    Square(a,x0,y0; ϕ=0, reference=:center)
"""
Square(a::Real, x0::Real, y0::Real; kwargs...) = Square(a,Point(x0,y0); kwargs...)

"""
    (::Square)(p::Point) -> is_in_square::Bool
"""
function (s::Square)(p::Point{2})
    purot = unrotate(p, s)
    p = purot - s.origin
    return -s.a/2+eps(s.a/2) < p.x < s.a/2-eps(s.a/2)  &&  -s.a/2+eps(s.a/2) < p.y < s.a/2-eps(s.a/2)
end

function perimeter(t::Number,s::Square,side::Int)
    if side==1
        x = -s.a/2
        y = -s.a/2 + t*s.a
        ẋ = 0
        ẏ = s.a
    elseif side==2
        x =  s.a/2
        y = -s.a/2 + t*s.a
        ẋ = 0
        ẏ = s.a
    elseif side==3
        x = -s.a/2 + t*s.a
        y = -s.a/2
        ẋ = s.a
        ẏ = 0
    elseif side==4
        x = -s.a/2 + t*s.a
        y =  s.a/2
        ẋ = s.a
        ẏ = 0
    end
    x += s.origin.x
    y += s.origin.y
    p = rotate(Point(x,y),s)
    ṗ = rotate(Point(ẋ,ẏ),s.cosϕ,s.sinϕ)
    return p.x, p.y, ṗ.x, ṗ.y
end

function Base.getproperty(s::Square, sym::Symbol)
    if Base.sym_in(sym,(:A,:B,:b))
        return getfield(s,:a)
    else
        return getfield(s,sym)
    end
end

function Base.propertynames(::Square,private=false)
    if private
        return fieldnames(Square)
    else
        return (:a, :origin, :ϕ)
    end
end

import ..PRINTED_COLOR_NUMBER
import ..PRINTED_COLOR_DARK

function Base.show(io::IO, square::Square)
    printstyled(io, "Square", color=PRINTED_COLOR_DARK)
    print(io, "(a: ")
    printstyled(io, fmt("2.2f",square.a), color=PRINTED_COLOR_NUMBER)
    print(io, ", ϕ: ")
    printstyled(io, fmt("3.1f",180square.ϕ/π),"°", color=PRINTED_COLOR_NUMBER)
    print(io, ", center: ", square.origin, ")")
end

################################################################################
# RECTANGLE
"""
    Rectangle <: AbstractQuadrilateral <: AbstractShape{2,4}

Fields: `a`, `b`, `origin`, `ϕ`, `sinϕ`, `cosϕ`, `corners`, `models`
"""
struct Rectangle <: AbstractRectangle
    a::Float64
    b::Float64
    origin::Point{2,Cartesian}
    ϕ::Float64
    sinϕ::Float64
    cosϕ::Float64
    corners::NTuple{4,Point{2,Cartesian}}
    models::Array{Opt,1}

    function Rectangle(a::Real, b::Real, origin::Point{2}, ϕ::Real)
        r = new(a,b,origin,ϕ,sin(ϕ),cos(ϕ))
        models = build_models(r)
        c1 = rotate(r.origin+Point( r.a/2, r.b/2), r)
        c2 = rotate(r.origin+Point(-r.a/2, r.b/2), r)
        c3 = rotate(r.origin+Point(-r.a/2,-r.b/2), r)
        c4 = rotate(r.origin+Point( r.a/2,-r.b/2), r)
        return new(a,b,origin,ϕ,sin(ϕ),cos(ϕ),(c1,c2,c3,c4),models)
    end
end

"""
    Rectangle(a, b, origin::Point; ϕ=0, reference=:center) -> r

`a`, `b` length of sides, `ϕ` is angle of rotation relative to `reference`
"""
function Rectangle(a::Real, b::Real, origin::Point{2}; ϕ::Real=0, reference::Symbol=:center)
    if Base.sym_in(reference,(:bottom, :Bottom, :b, :B))
        origin += rotate(Point( 0, b/2), cos(ϕ), sin(ϕ))
    elseif Base.sym_in(reference,(:top, :Top, :T, :t))
        origin += rotate(Point( 0,-b/2), cos(ϕ), sin(ϕ))
    elseif Base.sym_in(reference,(:left, :Left, :L, :l))
        origin += rotate(Point( a/2, 0), cos(ϕ), sin(ϕ))
    elseif Base.sym_in(reference,(:right, :Right, :R, :r))
        origin += rotate(Point(-a/2, 0), cos(ϕ), sin(ϕ))
    elseif Base.sym_in(reference,(:topright, :TopRight, :TR, :tr, :ne, :NE))
        origin += rotate(Point(-a/2,-b/2), cos(ϕ), sin(ϕ))
    elseif Base.sym_in(reference,(:topleft, :TopLeft, :TL, :tl, :nw, :NW))
        origin += rotate(Point( a/2,-b/2), cos(ϕ), sin(ϕ))
    elseif Base.sym_in(reference,(:bottomleft, :BottomLeft, :BL, :bl, :sw, :SW))
        origin += rotate(Point( a/2, b/2), cos(ϕ), sin(ϕ))
    elseif Base.sym_in(reference,(:bottomright, :BottomRight, :BR, :br, :se, :SE))
        origin += rotate(Point(-a/2, b/2), cos(ϕ), sin(ϕ))
    end
    return Rectangle(a,b,origin,ϕ)
end

"""
    Rectangle(a, b, x0, y0; ϕ=0, reference=:center)
"""
Rectangle(a::Real, b::Real, x0::Real, y0::Real; kwargs...) = Rectangle(a,b,Point(x0,y0); kwargs...)

"""
    Rectangle(s::Square) -> r
"""
Rectangle(s::Square) = Rectangle(s.a,s.a,s.origin; ϕ = s.ϕ)

"""
    (::Rectangle)(p::Point) -> is_in_rectangle::Bool
"""
function (r::Rectangle)(p::Point{2})
    purot = unrotate(p, r)
    p = purot - r.origin
    return -r.a/2+eps(r.a/2) < p.x < r.a/2-eps(r.a/2)  &&  -r.b/2+eps(r.b/2) < p.y < r.b/2-eps(r.b/2)
end

function perimeter(t::Number,r::Rectangle,side::Int)
    if side==1
        x = -r.a/2
        y = -r.b/2 + t*r.b
        ẋ = 0
        ẏ = r.b
    elseif side==2
        x =  r.a/2
        y = -r.b/2 + t*r.b
        ẋ = 0
        ẏ = r.b
    elseif side==3
        x = -r.a/2 + t*r.a
        y = -r.b/2
        ẋ = r.a
        ẏ = 0
    elseif side==4
        x = -r.a/2 + t*r.a
        y =  r.b/2
        ẋ = r.a
        ẏ = 0
    end
    x += r.origin.x
    y += r.origin.y
    p = rotate(Point(x,y),r)
    ṗ = rotate(Point(ẋ,ẏ),r.cosϕ,r.sinϕ)
    return p.x, p.y, ṗ.x, ṗ.y
end

function Base.getproperty(r::Rectangle, sym::Symbol)
    if Base.sym_in(sym,(:A,))
        return getfield(r,:a)
    elseif Base.sym_in(sym,(:B,))
        return getfield(r,:b)
    else
        return getfield(r,sym)
    end
end

function Base.propertynames(::Rectangle,private=false)
    if private
        return fieldnames(Rectangle)
    else
        return (:a, :b, :origin, :ϕ)
    end
end

import ..PRINTED_COLOR_NUMBER
import ..PRINTED_COLOR_DARK

function Base.show(io::IO, rect::Rectangle)
    printstyled(io, "Rectangle", color=PRINTED_COLOR_DARK)
    print(io, "(a: ")
    printstyled(io, fmt("2.2f", rect.a), color=PRINTED_COLOR_NUMBER)
    print(io, ", b: ")
    printstyled(io, fmt("2.2f", rect.b), color=PRINTED_COLOR_NUMBER)
    print(io, ", ϕ: ")
    printstyled(io, fmt("3.1f", 180rect.ϕ/π),"°", color=PRINTED_COLOR_NUMBER)
    print(io, ", center: ", rect.origin, ")")
end

################################################################################
# AUXILLIARIES

import ..NL_NORMAL_ALGORITHM
import ..NL_NORMAL_XTOL
import ..NL_NORMAL_FTOL
import ..NL_NORMAL_MAXEVAL

"""
    rotate(x,y,cosϕ,sinϕ) -> xrot, yrot

rotate by `ϕ` about origin

----------------
    rotate(x,y,shape) -> xrot, yrot

rotate about `shape`'s center by `shape`'s angle
"""
rotate(p::Point{2},cosϕ::Real,sinϕ::Real) = iszero(sinϕ) ? p : Point(cosϕ*p.x-sinϕ*p.y, sinϕ*p.x+cosϕ*p.y)
rotate(p::Point{2},s::AbstractShape{2}) = s.origin + rotate(p-s.origin,s.cosϕ,s.sinϕ)

function rotate(p::AbstractArray,cosϕ::Real,sinϕ::Real)
    prot = Array{Point{2}}(undef,size(p))
    rotate!(prot,p,cosϕ,sinϕ)
    return prot
end
function rotate(p::AbstractArray,s::AbstractShape{2})
    prot = Array{Point{2}}(undef,size(p))
    rotate!(prot,p.-s.origin,s.cosϕ,s.sinϕ)
    return prot .+ s.origin
end

function rotate!(prot::AbstractArray,p::Point{2},cosϕ::Real,sinϕ::Real)
    for i ∈ eachindex(p) prot[i] = rotate(p[i],cosϕ,sinϕ) end
    return nothing
end

"""
    unrotate(x,y,cosϕ,sinϕ) -> xrot, yrot

rotate by `-ϕ` about origin

----------------
    unrotate(x,y,shape) -> xrot, yrot

rotate about `shape`'s center by the negative of `shape`'s angle
"""
unrotate(p::Point{2},cosϕ::Real,sinϕ::Real) = rotate(p,cosϕ,-sinϕ)
unrotate(p::Point{2},s::AbstractShape{2}) = s.origin + unrotate(p-s.origin,s.cosϕ,s.sinϕ)
unrotate(p::AbstractArray,s::AbstractShape{2}) = s.origin .+ unrotate(p.-s.origin,s.cosϕ,s.sinϕ)

################################################################################
# UNSYMMETRIC AUXILLIARIES

function build_models(s::AbstractShape{N}) where N
    models = Array{Opt}(undef,N)
    for i ∈ eachindex(models)
        models[i] = Opt(NL_NORMAL_ALGORITHM, 1)
        models[i].xtol_rel = NL_NORMAL_XTOL
        models[i].ftol_rel = NL_NORMAL_FTOL
        models[i].maxeval = NL_NORMAL_MAXEVAL
        if typeof(s)<:AbstractQuadrilateral
            models[i].lower_bounds = 0
            models[i].upper_bounds = 1
        end
    end
    return models
end

function model_objective(v::Vector,grad::Vector,X::Number,Y::Number,s::AbstractShape,side::Int)
    x,y,ẋ,ẏ = perimeter(v[1],s,side)
    if length(grad) > 0
        grad[1] = 2(x-X)*ẋ + 2(y-Y)*ẏ
    end
    return (x-X)^2 + (y-Y)^2
end

"""
    normal_distance(shape,x,y) -> normal, tangent, distance, side

Get the `normal` vector from the surface of `shape` to a point `x,y`,
and the associated `tangent` vector and distance from surface `side`.

The returned arguments are sorted in ascending order by `distance`, so that, e.g.,
`normal[1]` is the normal vector associated with the closest side.
"""
function normal_distance(s::TS,x::Number,y::Number) where TS<:AbstractShape{2}
    n = Vector{Vector{Float64}}(undef,length(s.models))
    d = Vector{Float64}(undef,length(s.models))
    sides = 1:length(s.models)
    for i ∈ eachindex(s.models)
        model = s.models[i]
        model.min_objective = (v,g)->model_objective(v,g,x,y,s,i)
        minf,mint,ret = optimize(model,[t0_shape(x,y,s)])
        X,Y = perimeter(mint[1],s,i)
        n[i] = [x-X, y-Y]
        n[i] = n[i]/norm(n[i])
        d[i] = sqrt(minf)
    end
    perm = sortperm(d)
    n, d = n[perm], d[perm]
    NORMAL = Vector{Vector{Float64}}(undef,length(s.models))
    TANGENT = Vector{Vector{Float64}}(undef,length(s.models))
    for i ∈ eachindex(NORMAL)
        nx, ny = n[i][1],n[i][2]
        NORMAL[i] = [nx, ny]
        TANGENT[i] = [ny,-nx]
    end
    return NORMAL, TANGENT, d, sides[perm]
end

t0_shape(x::Number,y::Number,s::AbstractQuadrilateral) = .5
t0_shape(x::Number,y::Number,s::AbstractDisk) = atan(y-s.origin.y,x-s.origin.x)

################################################################################
# Plotting

import ..SHAPE_COLOR
import ..SHAPE_FILL_ALPHA

# plotting for all disks
@recipe function f(d::AbstractDisk)
    ϕ = LinRange(0,2π,201)
    x = Vector{Float64}(undef,length(ϕ))
    y = Vector{Float64}(undef,length(ϕ))
    for i ∈ eachindex(ϕ) x[i],y[i] = perimeter(ϕ[i],d,1) end
    alpha --> 0
    seriestype --> :path
    fillcolor --> SHAPE_COLOR
    fillrange --> d.origin.y
    fillalpha --> SHAPE_FILL_ALPHA
    aspect_ratio --> 1
    legend-->false
    (x,y)
end

# plot all quadrilaterals
@recipe function f(q::AbstractQuadrilateral)
    t = LinRange(0,1,201)
    x1,y1 = Array{Float64}(undef,length(t)), Array{Float64}(undef,length(t))
    x2,y2 = Array{Float64}(undef,length(t)), Array{Float64}(undef,length(t))
    x3,y3 = Array{Float64}(undef,length(t)), Array{Float64}(undef,length(t))
    x4,y4 = Array{Float64}(undef,length(t)), Array{Float64}(undef,length(t))
    for i ∈ eachindex(t)
        x1[i],y1[i] = perimeter(t[i],q,1)
        x2[i],y2[i] = perimeter(t[i],q,2)
        x3[i],y3[i] = perimeter(t[i],q,3)
        x4[i],y4[i] = perimeter(t[i],q,4)
    end
    alpha --> 0
    seriestype --> :path
    fillcolor --> SHAPE_COLOR
    fillrange --> q.origin.y
    fillalpha --> SHAPE_FILL_ALPHA
    aspect_ratio --> 1
    legend --> false
    vcat(x1,x4,reverse(x3)),vcat(y1,y4,reverse(y3))
end

#
# """
#     struct Ellipse <: AbstractDisk <: AbstractShape{1}
#
#     Ellipse((a,b), x0, y0, ϕ=0; reference=:center) -> ellipse
#     Ellipse(a, b, x0, y0, ϕ=0; reference=:center) -> ellipse
#
#
# `a, b` axis lengths
# `x0, y0` is location of `reference`
# `ϕ` angle of `a` axis
#
# ----------------
#
#     (::Ellipse)(x,y) -> is_in_ellipse::Bool
# """
# struct Ellipse <: AbstractDisk
#     a::Float64
#     b::Float64
#     x0::Float64
#     y0::Float64
#     ϕ::Float64
#     sinϕ::Float64
#     cosϕ::Float64
#     corners::Tuple{}
#     models::Array{Opt,1}
#
#     Ellipse((a,b),args...;kwargs...) = Ellipse(a,b,args...;kwargs...)
#     function Ellipse(a::Number,b::Number,x0::Number,y0::Number,ϕ::Number=0; reference::Symbol=:center)
#         if reference ∈ [:bottom, :Bottom, :b, :B]
#             x0,y0 = (x0,y0) .+ rotate( 0, b,cos(ϕ),sin(ϕ))
#         elseif reference ∈ [:top, :Top, :T, :t]
#             x0,y0 = (x0,y0) .+ rotate( 0,-b,cos(ϕ),sin(ϕ))
#         elseif reference ∈ [:left, :Left, :L, :l]
#             x0,y0 = (x0,y0) .+ rotate( a, 0,cos(ϕ),sin(ϕ))
#         elseif reference ∈ [:right, :Right, :R, :r]
#             x0,y0 = (x0,y0) .+ rotate(-a, 0,cos(ϕ),sin(ϕ))
#         end
#          e = new(a,b,x0,y0,ϕ,sin(ϕ),cos(ϕ))
#          models = build_models(e)
#          return new(a,b,x0,y0,ϕ,sin(ϕ),cos(ϕ),(),models)
#     end
#
#     Ellipse(c::Circle) = Ellipse(c.R,c.R,c.x0,c.y0,c.ϕ)
#
#     function (e::Ellipse)(x,y)
#         xurot, yurot = unrotate(x, y, e)
#         return hypot((xurot-e.x0)/e.a, (yurot-e.y0)/e.b) < 1-eps()
#     end
#
#     function Base.show(io::IO, ellipse::Ellipse)
#         print(io, "Ellipse(a=", fmt("2.2f",ellipse.a), ", b=", fmt("2.2f",ellipse.b), ", x0=", fmt("2.2f",ellipse.x0), ", y0=", fmt("2.2f",ellipse.y0), ", ϕ=∠", fmt("3.2f",(mod2pi(ellipse.ϕ))*180/π), "°)")
#     end
# end
# function perimeter(t::Number,e::Ellipse,side::Int)
#     sn,cs = sincos(t)
#     xur,yur =    e.a*cs, e.b*sn
#     xpur,ypur = -e.a*sn, e.b*cs
#     x,y = (e.x0,e.y0) .+ rotate(xur,yur,e.cosϕ,e.sinϕ)
#     ẋ,ẏ = rotate(xpur,ypur,e.cosϕ,e.sinϕ)
#     return x,y,ẋ,ẏ
# end
#
#
# """
#     struct DeformedDisk{N} <: AbstractDisk <: AbstractShape{1}
#
#     DeformedDisk{N}(R, x0, y0, M, a, φ, ϕ=0) -> deformeddisk
#     DeformedDisk{N}((R,M,a,φ), x0, y0, ϕ=0) -> deformeddisk
#
# `R` is radius,
# `x0` and `y0` is center of circle of radius `R`,
# `M` is array of length `N` of multipole integers,
# `a` is array of length `N` amplitudes,
# `φ` is array of length `N` of angles
# `ϕ` is overall rotation angle
#
# -----------------
#
#     (::DeformedDisk)(x,y) -> is_in_disk::Bool
# """
# struct DeformedDisk{N} <: AbstractDisk
#     R::Float64
#     x0::Float64
#     y0::Float64
#     ϕ::Float64
#     M::SArray{Tuple{N},Int,1,N}
#     a::SArray{Tuple{N},Float64,1,N}
#     φ::SArray{Tuple{N},Float64,1,N}
#     sinϕ::Float64
#     cosϕ::Float64
#     corners::Tuple{}
#     models::Array{Opt,1}
#
#     DeformedDisk{n}((R,M,a,φ),x0,y0,args...;kwargs...) where n= DeformedDisk{n}(R,x0,y0,M,a,φ,args...;kwargs...)
#     function DeformedDisk{n}(R::Number,x0::Number,y0::Number,M,a,φ,ϕ=0) where n
#         @assert n==length(M)==length(a)==length(φ) "parameter N in DeformedDisk{N}(...) must be equal to length(M)"
#         d = new{n}(R,x0,y0,ϕ,SVector{n}(M),SVector{n}(a),SVector{n}(φ.+ϕ),sin(ϕ),cos(ϕ))
#         models = build_models(d)
#         return new{n}(R,x0,y0,d.ϕ,d.M,d.a,d.φ,d.sinϕ,d.cosϕ,(),models)
#     end
#
#     DeformedDisk(c::Circle) = DeformedDisk{0}(c.R,c.x0,c.y0,[],[],[],c.ϕ)
#
#     function (d::DeformedDisk)(x,y)
#         ϕ = atan(y-d.y0,x-d.x0)
#         r = hypot(x-d.x0,y-d.y0) < d.R + sum(d.a.*(map((m,φ)->cos(m*(ϕ-φ)),d.M,d.φ))) - eps(d.R)
#         return r
#     end
#
#     function Base.show(io::IO, d::DeformedDisk{N}) where N
#          print(io,"DeformedDisk with $N multipoles")
#     end
# end
# function perimeter(t::Number,d::DeformedDisk,side::Int)
#     r = d.R + sum(d.a.*(map((m,φ)->cos(m*(t-φ)),d.M,d.φ)))
#     ṙ = sum(d.a.*(map((m,φ)->-m*sin(m*(t-φ)),d.M,d.φ)))
#     sn,cs = sincos(t)
#     x,y = d.x0 + r*cs, d.y0 + r*sn
#     ẋ = ṙ*cs - r*sn
#     ẏ = ṙ*sn + r*cs
#     return x,y,ẋ,ẏ
# end

# """
#     struct Annulus <: AbstractShape{2}
#
#     Annulus((R1,R2),x0,y0,ϕ=0) -> annulus
#     Annulus(R1, R2, x0, y0, ϕ=0) -> annulus
#
# `R1` is inner radius, `R2` outer
# `x0, y0` is center
#
# -------------
#     (::Annulus)(x,y) -> is_in_annulus::Bool
# """
# struct Annulus <: AbstractShape{2}
#     R1::Float64
#     R2::Float64
#     x0::Float64
#     y0::Float64
#     ϕ::Float64
#     sinϕ::Float64
#     cosϕ::Float64
#     corners::Tuple{}
#     models::Array{Opt,1}
#
#     Annulus((R1,R2),args...;kwargs...) = Annulus(R1,R2,args...;kwargs...)
#     function Annulus(R1::Number,R2::Number,x0::Number,y0::Number,ϕ::Number=0)
#         @assert R1≤R2 "R1=$R1, R2=$R2 must satisfy R1≤R2"
#         a = new(R1,R2,x0,y0,0,0,1)
#         models = build_models(a)
#         return new(R1,R2,x0,y0,0,sin(ϕ),cos(ϕ),(),models)
#     end
#
#     (c::Annulus)(x,y) = c.R1+eps(c.R1) < hypot( (x-c.x0),(y-c.y0) ) < c.R2-eps(c.R2)
#
#     function Base.show(io::IO, a::Annulus)
#         print(io, "Annulus(R1=$(fmt("2.2f",a.R1)), R2=$(fmt("2.2f",a.R2)), x0=$(fmt("2.2f",a.x0)), y0=$(fmt("2.2f",a.y0)))")
#     end
# end
# function perimeter(t::Number,a::Annulus,side::Int)
#     sn,cs = sincos(t)
#     R = side==1 ? a.R1 : a.R2
#     x = a.x0 + R*cs
#     y = a.y0 + R*sn
#     ẋ = -R*sn
#     ẏ =  R*cs
#     return x,y,ẋ,ẏ
# end
# @recipe function f(cr::Annulus)
#     @series Circle(cr.R1,cr.x0,cr.y0)
#     @series Circle(cr.R2,cr.x0,cr.y0)
# end
#
#
#

#
#
# # """
# #     Parallelogram(a, b, α, x0, y0, ϕ)
# # """
# # struct Parallelogram <: AbstractParallelogram
# #     a::Float64
# #     b::Float64
# #     α::Float64
# #     x0::Float64
# #     y0::Float64
# #     ϕ::Float64
# #     sinϕ::Float64
# #     cosϕ::Float64
# #     tanα::Float64
# #     cosα::Float64
# #     models::Array{Opt,1}
# #
# #     function Parallelogram(a::Number, b::Number, α::Number, x0::Number, y0::Number, ϕ::Number)
# #         new(a,b,α,x0,y0,ϕ,cos(ϕ),sin(ϕ),tan(α),cos(α))
# #     end
# #
# #     function (p::Parallelogram)(x,y)
# #         xrot, yrot = rotate(x-p.x0, y-p.y0, p.cosϕ, p.sinϕ)
# #         return p.tanα*(xrot-p.a) < yrot < p.tanα*xrot  &&  0 < yrot < p.cosα*p.b
# #     end
# #
# #     Base.show(io::IO, par::Parallelogram) = print(io, "Parallelogram(a=$(fmt("2.2f",par.a)), b=$(fmt("2.2f",par.b)), α=$(fmt("2.2f",par.α)), x0=$(fmt("2.2f",par.x0)), y0=$(fmt("2.2f",par.y0)), ϕ=$(fmt("2.2f",par.ϕ)))")
# #
# #     @recipe function f(pg::Parallelogram)
# #         x = cumsum([0, pg.a, +pg.b*cos(pg.α), -pg.a, -pg.b*cos(pg.α)])
# #         y = cumsum([0, 0, +pg.b*sin(pg.α), 0, -pg.b*sin(pg.α)])
# #         alpha --> 0
# #         seriestype --> :path
# #         fillcolor --> SHAPE_COLOR
# #         fillrange --> 0
# #         fillalpha --> SHAPE_FILL_ALPHA
# #         aspect_ratio --> 1
# #         legend --> false
# #         xrot, yrot = rotate(x,y,pg.cosϕ,-pg.sinϕ)
# #         pg.x0 .+ xrot, pg.y0 .+ yrot
# #     end
# # end
#


using DelimitedFiles
using Statistics

open("data_3.dat") do f

  # line_number
  line = 1

  data = zeros(1000, 12)
  # read till end of file
  while ! eof(f)

     # read a new / next line for every itera
     s = readline(f)
     for ii in 1:12
         data[line, ii] = parse(Int, s[ii])
     end
     line += 1
  end
  gamma = []
  epsilon = []
  for ii in 1:12
      if mean(data[:,ii]) < 0.5
          append!(gamma, 0)
          append!(epsilon, 1)
      else
          append!(gamma, 1)
          append!(epsilon, 0)
      end
   end

   binary_g = 0
   for g in gamma
      binary_g = binary_g * 2 + g
   end

   binary_e = 0
   for e in epsilon
       binary_e = binary_e * 2 + e
   end
   println(binary_e * binary_g)
end


module FEA

using ..TopOpt: TopOpt, CPU, GPU, PENALTY_BEFORE_INTERPOLATION, whichdevice
using ..TopOptProblems, ..Utilities, Requires
using Ferrite, Setfield, TimerOutputs, Preconditioners
using IterativeSolvers, StaticArrays, SparseArrays
using LinearAlgebra
using Parameters: @unpack

export AbstractFEASolver,
    AbstractDisplacementSolver,
    DirectDisplacementSolver,
    PCGDisplacementSolver,
    StaticMatrixFreeDisplacementSolver,
    Displacement,
    Direct,
    CG,
    MatrixFree,
    FEASolver,
    Assembly,
    DefaultCriteria,
    EnergyCriteria,
    simulate

const to = TimerOutput()

# FEA solvers
abstract type AbstractFEASolver end

include("grid_utils.jl")
include("matrix_free_operator.jl")
include("convergence_criteria.jl")
include("direct_displacement_solver.jl")
include("assembly_cg_displacement_solvers.jl")
include("matrix_free_cg_displacement_solvers.jl")
include("matrix_free_apply_bcs.jl")
include("simulate.jl")
include("solvers_api.jl")

getcompliance(solver) = solver.u' * solver.globalinfo.K * solver.u

end


@testset "pentland" begin
    img = generate_surface(SynthSphere(50), 0.5, [0.1, 0.0, 0.9])
    Z = Pentland(slant = 0.1106572211739, tilt = 0.0)(img)
    @test Z[76,76] ≈ 96.35408069226294
    @test isapprox(Z[1,1], 108.14578383093884; atol = 2e-15)
    @test isapprox(Z[151,151], 108.14578383093864; atol = 2e-15)
    @test Z[90,90] ≈ 93.80666569339293
    @test Z[101,36] ≈ 80.97642794759314
    @test isapprox(Z[22, 136], 108.14578383093865; atol = 2e-15)
    @test Z[42,42] ≈ 79.72206437513667
end


using brainflow

# enable logs
brainflow.enable_dev_brainflow_logger(brainflow.BOARD_CONTROLLER)
brainflow.enable_dev_brainflow_logger(brainflow.DATA_HANDLER)

params = BrainFlowInputParams()
board_shim = brainflow.BoardShim(brainflow.SYNTHETIC_BOARD, params)
sampling_rate = brainflow.get_sampling_rate(brainflow.SYNTHETIC_BOARD)

brainflow.prepare_session(board_shim)
brainflow.start_stream(board_shim)
sleep(5)
brainflow.stop_stream(board_shim)
data = brainflow.get_current_board_data(brainflow.get_nearest_power_of_two(sampling_rate), board_shim)
brainflow.release_session(board_shim)

eeg_channels = brainflow.get_eeg_channels(brainflow.SYNTHETIC_BOARD)
data_first_channel = data[eeg_channels[1], :]

# returns tuple of wavelet coeffs and lengths
wavelet_data = brainflow.perform_wavelet_transform(data_first_channel, "db4", 2)
restored_wavelet_data = brainflow.perform_inverse_wavelet_transform(wavelet_data, length(data_first_channel), "db4", 2)

fft_data = brainflow.perform_fft(data_first_channel, brainflow.NO_WINDOW)
restored_fft_data = brainflow.perform_ifft(fft_data)

println("Original Data")
println(data_first_channel)
println("Restored from Wavelet Data")
println(restored_wavelet_data)
println("Restored from FFT Data")
println(restored_fft_data)


using Plots
using LightGraphs
using LinearAlgebra
using StatsBase
using StatsFuns
using GraphPlot

include("estimating_functions.jl")
include("generators.jl")

n = 500
beta = zeros(n)
for i in sample(collect(1:n), Int(floor(n / 10)))
    beta[i] = sqrt(log(n))
end
mu = -log(n)

g = sparse_beta_graph(beta, mu)

i = rand((collect(1:500)[beta .> 0]))
i_not = rand((collect(1:500)[beta .== 0]))
beta_mi = beta[1:end .!= i]
beta_i = beta[i]
deg = degree(g)[i]

beta_mi_not = beta[1:end .!= i_not]
beta_i_not = beta[i_not]
deg2 = degree(g)[i_not]

partial_sparse_beta_conditional_loglik(beta_i) = sparse_beta_conditional_loglik(g::Graph, mu, beta_i, beta_mi, deg)
partial_sparse_beta_conditional_loglik2(beta_i) = sparse_beta_conditional_loglik(g::Graph, mu, beta_i, beta_mi_not, deg2)

l = @layout [ a  b ]
p1 = Plots.plot(collect(0:0.01:4), exp.(partial_sparse_beta_conditional_loglik.(collect(0:0.01:4))))
p2 = Plots.plot(collect(0:0.01:4), exp.(partial_sparse_beta_conditional_loglik2.(collect(0:0.01:4))))
plot(p1, p2, layout=l)


include("radCalcs.jl")
using QuadGK

"""
Function for calculating the OMode group delay.

Inputs:
        freq::Vector{Float64}         -- Measured frequencies of the reflectometer (GHz)
        coeffs_fit::Vector{Float64}   -- The 6 fit coefficients for the 5th order polynomial for plasma density in units of 1e19 (per cubic meter)
                                            n(r) = 1e19*(coeffs_fit[1]*r^5+coeffs_fit[2]*r^4+coeffs_fit[3]*r^3+coeffs_fit[4]*r^2+coeffs_fit[5]*r^1+coeffs_fit[6])
                                            where r=radius into the plasma from the plasma facing wall.
        calibration::Vector{Float64}  -- Calibration length in meters (non-plasma group delay)
        rad::Vector{Float64}          -- Radial location of cutoffs are each frequency in freq (meters from first wall).

Output:
        ophiw   -- the group delay at each frequency in freq
"""
function dphidw_omode(freq, coeffs_fit::Vector{Float64},calibration::Vector{Float64},rad::Vector{Float64})
    ophi = Array{Float64}(undef,length(freq))
    for j in 1:1:length(freq)
        ophi[j] = calibration[j] / 3e8 * (freq[j]*2*pi*1e9) + 2*(freq[j]*2*pi*1e9) / 3e8 * quadgk(x -> refractive_omode(x;coeffs = coeffs_fit, omega = freq[j]*2*pi*1e9),0,(rad[j]),rtol=1e-12)[1]
    end
    ophiw = Array{Float64}(undef,length(freq))
    dw = (freq[2]-freq[1])*2*pi*1e9
    ophiw[1] = (ophi[2]-ophi[1])/dw
    ophiw[length(freq)] = (ophi[length(freq)]-ophi[length(freq)-1])/dw
    for j in 2:1:length(ophiw)-1
        ophiw[j] = (ophi[j+1]-ophi[j-1])/(2*dw)
    end

    ophiw
end

"""
Function for calculating the XMode group delay.

Inputs:
        freq                          -- Measured frequencies of the reflectometer (GHz)
        coeffs_fit::Array{Float64}    -- The 6 fit coefficients for the 5th order polynomial for plasma density in units of 1e19 (per cubic meter)
                                            n(r) = 1e19*(coeffs_fit[1]*r^5+coeffs_fit[2]*r^4+coeffs_fit[3]*r^3+coeffs_fit[4]*r^2+coeffs_fit[5]*r^1+coeffs_fit[6])
                                            where r=radius into the plasma from the plasma facing wall.
        calibration::Array{Float64}   -- Calibration length in meters (non-plasma group delay)
        rad::Array{Float64}           -- Radial location of cutoffs are each frequency in freq (meters from first wall).
        Bmag::Number                  -- The magnitude of the magnetic field at major radius startingRad (Tesla)
        startingRad::Number           -- The major radial measurement location of the magnetic field (meters).

Output:
        ophiw -- the group delay at each frequency in freq
"""
function dphidw_xmode(freq, coeffs_fit::Vector{Float64}, calibration::Array{Float64}, Bmag::Number, startingRad::Number,rad::Array{Float64})
    ophi = Array{Float64}(undef,length(freq))
    for j in 1:1:length(freq)
        ophi[j] = calibration[j] / 3e8 * (freq[j]*2*pi*1e9) + 2*(freq[j]*2*pi*1e9) / 3e8 * quadgk(x -> refractive_xmode(x;coeffs = coeffs_fit, omega = freq[j]*2*pi*1e9, b0 = Bmag, r0 = startingRad),0,(rad[j]),rtol=1e-12)[1]
    end
    ophiw = Array{Float64}(undef,length(freq))
    dw = (freq[2]-freq[1])*2*pi*1e9
    ophiw[1] = (ophi[2]-ophi[1])/dw
    ophiw[length(freq)] = (ophi[length(freq)]-ophi[length(freq)-1])/dw
    for j in 2:1:length(ophiw)-1
        ophiw[j] = (ophi[j+1]-ophi[j-1])/(2*dw)
    end

    ophiw
end

"""
Function for calculating the OMode index of refraction

Inputs:
        x::Float64                    -- the radial location in meters from the first wall
        coeffs_fit::Array{Float64}    -- The 6 fit coefficients for the 5th order polynomial for plasma density in units of 1e19 (per cubic meter)
                                            n(r) = 1e19*(coeffs_fit[1]*r^5+coeffs_fit[2]*r^4+coeffs_fit[3]*r^3+coeffs_fit[4]*r^2+coeffs_fit[5]*r^1+coeffs_fit[6])
                                            where r=radius into the plasma from the plasma facing wall.
         omega::Float64               -- the excited frequency (radians per second)

Output:
        -- the OMode index of refraction
"""
function refractive_omode(x::Float64; coeffs::Array{Float64}, omega::Float64)
    q = 1.6022e-19
    me = 9.109e-31
    e0 = 8.854e-12
    density = 1e19.*(coeffs[1].*(x).^5 .+coeffs[2].*(x).^4 .+coeffs[3].*(x).^3 .+coeffs[4].*(x).^2 .+coeffs[5].*(x).+coeffs[6])
    wpe2 = (q)^2 * density / (e0*me)
    sqrt(max(0.0,1.0 .-wpe2./omega^2))
end

"""
Function for calculating the XMode index of refraction

Inputs:
        x::Float64                    -- the radial location in meters from the first wall
        coeffs_fit::Array{Float64}    -- The 6 fit coefficients for the 5th order polynomial for plasma density in units of 1e19 (per cubic meter)
                                            n(r) = 1e19*(coeffs_fit[1]*r^5+coeffs_fit[2]*r^4+coeffs_fit[3]*r^3+coeffs_fit[4]*r^2+coeffs_fit[5]*r^1+coeffs_fit[6])
                                            where r=radius into the plasma from the plasma facing wall.
         omega::Float64               -- the excited frequency (radians per second)
         b0::Number                   -- The magnitude of the magnetic field at major radius startingRad (Tesla)
         r0::Number                   -- The major radial measurement location of the magnetic field (meters).

Output:
        -- the XMode index of refraction
"""
function refractive_xmode(x::Float64; coeffs::Array{Float64}, omega::Float64, b0::Number, r0::Number)
    q = 1.6022e-19
    me = 9.109e-31
    e0 = 8.854e-12
    density = 1e19.*(coeffs[1].*(x).^5 .+coeffs[2].*(x).^4 .+coeffs[3].*(x).^3 .+coeffs[4].*(x).^2 .+coeffs[5].*(x).+coeffs[6])
    wpe2 = (q)^2 * density / (e0*me)
    wce = b0 / (r0+x) * q / me
    sqrt(max(0.0,1.0 .-wpe2./omega^2 * (omega^2-wpe2)/(omega^2-(wpe2+wce^2))))
end