export ## Methods
       unitary_propagator

const liblapack = Base.liblapack_name
import Base.LinAlg: BlasChar, BlasInt, blas_int

function expm_eigen(A::Matrix, t)
    #Calculates exp(t*A) via eigenvalue decomposition and assuming Hermitian matrix
    F = eigfact(Hermitian(A))

    # V * diagm(exp(t*D)) * V'
    return scale(F[:vectors], exp(t*F[:values])) * F[:vectors]'
end

function allocate_workspace(dim::Int)
    #Allocate workspace for Hermitian eigensolver for matrices of dimension dim

    #Create a random hermitian matrix
    A = rand((dim,dim)) + 1im*rand((dim,dim))
    H = A + A'

    #Make the initial call as a workspace query with lwork=-1
        # subroutine zheevr   (
        #       character   JOBZ,
        #       character   RANGE,
        #       character   UPLO,
        #       integer     N,
        #       complex*16, dimension( lda, * )     A,
        #       integer     LDA,
        #       double precision    VL,
        #       double precision    VU,
        #       integer     IL,
        #       integer     IU,
        #       double precision    ABSTOL,
        #       integer     M,
        #       double precision, dimension( * )    W,
        #       complex*16, dimension( ldz, * )     Z,
        #       integer     LDZ,
        #       integer, dimension( * )     ISUPPZ,
        #       complex*16, dimension( * )  WORK,
        #       integer     LWORK,
        #       double precision, dimension( * )    RWORK,
        #       integer     LRWORK,
        #       integer, dimension( * )     IWORK,
        #       integer     LIWORK,
        #       integer     INFO )

    jobz = 'V' # compute both eigenvalues and eigenvectors
    range = 'A' # all eigenvalues will be found
    uplo = 'U' # upper triangle storage (but we have full matrices anyways)
    n = blas_int(dim)
    vl = 0.0 #lower limit of eigenvalues: range = 'A'
    vu = 0.0 #upper limit of eigenvalues: range = 'A'
    il = blas_int(0) #lower number of eigenvalue to find: range = 'A'
    iu = blas_int(0) #upper number of eigenvalue to find: range = 'A'
    abstol = -1.0 # precision: negative for full precision
    m = Array(BlasInt, 1) #number of non-zero eigenvalues found
    w = zeros(Float64, dim) #array for eigenvalues
    z = zeros(Complex128, (dim,dim)) #array for eigenvectors
    ldz = blas_int(dim) # number of rows in Z

    #Make initial allocation of empty work arrays
    isuppz = Array(BlasInt, 2*dim)
    work  = Array(Complex128, 1)
    lwork = blas_int(-1)
    rwork = Array(Float64, 1)
    lrwork = blas_int(-1)
    iwork = Array(BlasInt, 1)
    liwork = blas_int(-1)
    info  = Array(BlasInt, 1)

    ccall(("zheevr_", liblapack), Void,
                    (Ptr{BlasChar}, Ptr{BlasChar}, Ptr{BlasChar}, Ptr{BlasInt},
                     Ptr{Complex128}, Ptr{BlasInt}, Ptr{Float64}, Ptr{Float64},
                     Ptr{BlasInt}, Ptr{BlasInt}, Ptr{Complex128}, Ptr{BlasInt},
                     Ptr{Float64}, Ptr{Complex128}, Ptr{BlasInt}, Ptr{BlasInt},
                     Ptr{Complex128}, Ptr{BlasInt}, Ptr{Float64}, Ptr{BlasInt},
                     Ptr{BlasInt},Ptr{BlasInt}, Ptr{BlasInt}),
                    &jobz, &range, &uplo, &n,
                    A, &n, &vl, &vu,
                    &il, &iu, &abstol, m,
                    w, z, &ldz, isuppz,
                    work, &lwork, rwork, &lrwork,
                    iwork, &liwork, info)

    #Update the work arrays
    lwork = blas_int(real(work[1]))
    work = Array(Complex128, lwork)
    lrwork = blas_int(rwork[1])
    rwork = Array(Float64, lrwork)
    liwork = iwork[1]
    iwork = Array(BlasInt, liwork)

    #Return all the allocated memory
    return (jobz, range, uplo, n, vl, vu, il, iu, abstol, m, w, z, ldz, isuppz, work, lwork, rwork, lrwork, iwork, liwork, info)

end

function expm_eigen!(A::Matrix, t, jobz, range, uplo, n, vl, vu, il, iu, abstol, m, w, z, ldz, isuppz, work, lwork, rwork, lrwork, iwork, liwork, info)
    #Matrix exponential exp(t*A) for Hermitian matrix
    #Works via Hermitian eigensolver
    #This version requires preallocated workspace memory and destroys the input matrix A

    #Directly call the LAPACK function
    ccall(("zheevr_", liblapack), Void,
                    (Ptr{BlasChar}, Ptr{BlasChar}, Ptr{BlasChar}, Ptr{BlasInt},
                     Ptr{Complex128}, Ptr{BlasInt}, Ptr{Float64}, Ptr{Float64},
                     Ptr{BlasInt}, Ptr{BlasInt}, Ptr{Complex128}, Ptr{BlasInt},
                     Ptr{Float64}, Ptr{Complex128}, Ptr{BlasInt}, Ptr{BlasInt},
                     Ptr{Complex128}, Ptr{BlasInt}, Ptr{Float64}, Ptr{BlasInt},
                     Ptr{BlasInt},Ptr{BlasInt}, Ptr{BlasInt}),
                    &jobz, &range, &uplo, &n,
                    A, &n, &vl, &vu,
                    &il, &iu, &abstol, m,
                    w, z, &ldz, isuppz,
                    work, &lwork, rwork, &lrwork,
                    iwork, &liwork, info)

    #TODO: should we be checking m to make sure there are dim eigenvalues?
    return scale(z, exp(t*w)) * z'

end

function unitary_propagator(sys::CompositeQSystem, timeStep::Float64, startTime::Float64, endTime::Float64; parallelize=true)

    #Preallocate Hamiltonian memory
    Ham = zeros(Complex128, (dim(sys), dim(sys)))

    #Allocate workspace for the matrix exponential
    workspace = allocate_workspace(dim(sys))

    times = startTime:timeStep:(endTime-timeStep)

    if parallelize
        Uprop = @parallel (*) for time = times
            #a *= b expands to a = a*b
            hamiltonian_add!(Ham, sys, time)
            # expm_eigen(Ham, 1im*2pi*timeStep)
            expm_eigen!(Ham, 1im*2pi*timeStep, workspace...)
        end
    else
        Uprop = eye(dim(sys))
        for time = times
            #a *= b expands to a = a*b
            hamiltonian_add!(Ham, sys, time)
            # expm_eigen(Ham, 1im*2pi*timeStep)
            Uprop *= expm_eigen!(Ham, 1im*2pi*timeStep, workspace...)
        end
    end

    if (endTime-times[end]) > timeStep
        hamiltonian_add!(Ham, sys, times[end]+timeStep)
        Uprop *= expm_eigen!(Ham, 1im*2pi*(endTime-times[end]-timeStep), workspace...)
    end

    return Uprop'
end


#################### NaV ###############################
mutable struct Prinz_NaV{S,T} <: IonChannel
    ḡNa::S
    mNa::T
    hNa::T
    #ENa::T
end

Prinz_NaV(x) = Prinz_NaV(x, 0.,0.)

m∞(::Prinz_NaV, V) =  1.0/(1.0+exp((V+25.5)/-5.29))
h∞(::Prinz_NaV, V) =  1.0/(1.0+exp((V+48.9)/5.18))
τm(::Prinz_NaV, V) =  1.32 - 1.26/(1+exp((V+120.0)/-25.0))
τh(::Prinz_NaV, V) =  (1.34/(1.0+exp((V+62.9)/-10.0)))*(1.5+1.0/(1.0+exp((V+34.9)/3.6)))
ionic_current(::Prinz_NaV, sys::ODESystem) = sys.INa
external_params(::Prinz_NaV) = (:ENa,)

function channel_dynamics(ch::Prinz_NaV, V, Ca, D, t)
    states = @variables mNa(t) hNa(t) INa(t)
    parameters = @parameters ḡNa ENa
    eqs = [ D(mNa) ~       (1/τm(ch, V))*(m∞(ch, V) - mNa),
            D(hNa) ~       (1/τh(ch, V))*(h∞(ch, V) - hNa),
            INa ~ ḡNa*mNa^3*hNa*(ENa - V)]
    current = [eqs[3]]
    defaultmap = [mNa => ch.mNa, hNa => ch.hNa, ḡNa => ch.ḡNa]
    return eqs, states, parameters, current, defaultmap
end

#################### Slow calcium current #############################
mutable struct Prinz_CaS{S,T} <: IonChannel
    ḡCaS::S
    mCaS::T
    hCaS::T
end

Prinz_CaS(x,y) = Prinz_CaS(x,y, 0.)
Prinz_CaS(x) = Prinz_CaS(x,20., 0.)

m∞(::Prinz_CaS, V) = 1.0/(1.0+exp((V+33.0)/-8.1))
h∞(::Prinz_CaS, V) = 1.0/(1.0+exp((V+60.0)/6.2))
τm(::Prinz_CaS, V) = 2.8 + 14.0/(exp((V+27.0)/10.0) + exp((V+70.0)/-13.0))
τh(::Prinz_CaS, V) = 120.0 + 300.0/(exp((V+55.0)/9.0) + exp((V+65.0)/-16.0))

ionic_current(::Prinz_CaS, sys::ODESystem) = sys.ICaS
calcium_current(::Prinz_CaS, sys::ODESystem) = sys.ICaS
ECa(::Prinz_CaS, Ca) = (500.0)*(8.6174e-5)*(283.15)*(log(max((3000.0/Ca), 0.001)))
external_params(::Prinz_CaS) = nothing

function channel_dynamics(ch::Prinz_CaS, V, Ca, D, t)
    states = @variables mCaS(t) hCaS(t) ICaS(t)
    parameters = @parameters ḡCaS
    eqs = [ D(mCaS) ~       (1/τm(ch, V))*(m∞(ch, V) - mCaS),
            D(hCaS) ~       (1/τh(ch, V))*(h∞(ch, V) - hCaS),
            ICaS    ~       ḡCaS*mCaS^3*hCaS*(ECa(ch, Ca) - V)
            ]
    current = [eqs[3]]
    defaultmap = [mCaS => ch.mCaS, hCaS => ch.hCaS, ḡCaS => ch.ḡCaS]
    return eqs, states, parameters, current, defaultmap
end

#################### Transient calcium current ######################

mutable struct Prinz_CaT{S,T} <: IonChannel
    ḡCaT::S
    mCaT::T
    hCaT::T
end
Prinz_CaT(x) = Prinz_CaT(x, 0., 0.)
m∞(::Prinz_CaT, V) = 1.0/(1.0 + exp((V+27.1)/-7.2))
h∞(::Prinz_CaT, V) = 1.0/(1.0 + exp((V+32.1)/5.5))
τm(::Prinz_CaT, V) = 43.4 - 42.6/(1.0 + exp((V+68.1)/-20.5))
τh(::Prinz_CaT, V) = 210.0 - 179.6/(1.0 + exp((V+55.0)/-16.9))

ionic_current(::Prinz_CaT, sys::ODESystem) = sys.ICaT
calcium_current(::Prinz_CaT, sys::ODESystem) = sys.ICaT
ECa(::Prinz_CaT, Ca) = (500.0)*(8.6174e-5)*(283.15)*(log(max((3000.0/Ca), 0.001)))
external_params(::Prinz_CaT) = nothing

function channel_dynamics(ch::Prinz_CaT, V, Ca, D, t)
    states = @variables mCaT(t) hCaT(t) ICaT(t)
    parameters = @parameters ḡCaT
    eqs = [ D(mCaT) ~       (1/τm(ch, V))*(m∞(ch, V) - mCaT),
            D(hCaT) ~       (1/τh(ch, V))*(h∞(ch, V) - hCaT),
            ICaT ~ ḡCaT*mCaT^3*hCaT*(ECa(ch, Ca) - V),
            ]
    current = [eqs[3]]
    defaultmap = [mCaT => ch.mCaT, hCaT => ch.hCaT, ḡCaT => ch.ḡCaT]
    return eqs, states, parameters, current, defaultmap
end

####################  #########################
"""
A-type potassium current
"""
mutable struct Prinz_Ka{S,T} <: IonChannel
    ḡKa::S
    mKa::T
    hKa::T
    #EK::T -80
end
Prinz_Ka(x) = Prinz_Ka(x,0.,0.)

m∞(::Prinz_Ka, V) = 1.0/(1.0+exp((V+27.2)/-8.7))
h∞(::Prinz_Ka, V) = 1.0/(1.0+exp((V+56.9)/4.9))
τm(::Prinz_Ka, V) = 23.2 - 20.8/(1.0+exp((V+32.9)/-15.2))
τh(::Prinz_Ka, V) = 77.2 - 58.4/(1.0+exp((V+38.9)/-26.5))

ionic_current(::Prinz_Ka, sys::ODESystem) = sys.IKa
external_params(::Prinz_Ka) = (:EK,)

function channel_dynamics(ch::Prinz_Ka, V, Ca, D, t)
    states = @variables mKa(t) hKa(t) IKa(t)
    parameters = @parameters ḡKa EK
    eqs = [ D(mKa) ~       (1/τm(ch, V))*(m∞(ch, V) - mKa),
            D(hKa) ~       (1/τh(ch, V))*(h∞(ch, V) - hKa),
            IKa ~ ḡKa*mKa^3*hKa*(EK - V)]
    current = [eqs[3]]
    defaultmap = [mKa => ch.mKa, hKa => ch.hKa, ḡKa => ch.ḡKa]
    return eqs, states, parameters, current, defaultmap
end

################### Calcium-activated potassium current ########

mutable struct Prinz_KCa{S,T} <: IonChannel
    ḡKCa::S
    mKCa::T
    #EK::T
end

Prinz_KCa(x) = Prinz_KCa(x,0.)
m∞(::Prinz_KCa, V, Ca) = (Ca/(Ca+3.0))/(1.0+exp((V+28.3)/-12.6));
τm(::Prinz_KCa, V) = 180.6 - 150.2/(1.0+exp((V+46.0)/-22.7))
ionic_current(::Prinz_KCa, sys::ODESystem) = sys.IKCa
external_params(::Prinz_KCa) = (:EK,)

function channel_dynamics(ch::Prinz_KCa, V, Ca, D, t)
    states = @variables mKCa(t) IKCa(t)
    parameters = @parameters ḡKCa EK
    eqs = [ D(mKCa) ~       (1/τm(ch, V))*(m∞(ch, V, Ca) - mKCa),
            IKCa ~ (ḡKCa*mKCa^4)*(EK - V)]
    current = [eqs[2]]
    defaultmap = [mKCa => ch.mKCa, ḡKCa => ch.ḡKCa]
    return eqs, states, parameters, current, defaultmap
end

"""
    Delayed rectifier potassium current
"""
mutable struct Prinz_Kdr{S,T} <: IonChannel
    ḡKdr::S
    mKdr::T
    #EK::T
end
Prinz_Kdr(x) = Prinz_Kdr(x,0.)

m∞(::Prinz_Kdr, V)=  1.0/(1.0+exp((V+12.3)/-11.8));
τm(::Prinz_Kdr, V)=  14.4 - 12.8/(1.0+exp((V+28.3)/-19.2))
ionic_current(::Prinz_Kdr, sys::ODESystem) = sys.IKdr
external_params(::Prinz_Kdr) = (:EK,)

function channel_dynamics(ch::Prinz_Kdr, V, Ca, D, t)
    states = @variables mKdr(t) IKdr(t)
    parameters = @parameters ḡKdr EK
    eqs = [ D(mKdr) ~       (1/τm(ch, V))*(m∞(ch, V) - mKdr),
            IKdr ~ (ḡKdr*mKdr^4)*(EK - V)]
    current = [eqs[2]]
    defaultmap = [mKdr => ch.mKdr, ḡKdr => ch.ḡKdr]
    return eqs, states, parameters, current, defaultmap
end

"""
H current
"""
mutable struct Prinz_H{S,T} <: IonChannel
    ḡH::S
    mH::T
    # EH::T -20.
end

Prinz_H(x) = Prinz_H(x, 0.)

m∞(::Prinz_H, V) = 1.0/(1.0+exp((V+75.0)/5.5))
τm(::Prinz_H, V) = (2/( exp((V+169.7)/(-11.6)) + exp((V- 26.7)/(14.3)) ))
ionic_current(::Prinz_H, sys::ODESystem) = sys.IH
external_params(::Prinz_H) = (:EH,)

function channel_dynamics(ch::Prinz_H, V, Ca, D, t)
    states = @variables mH(t) IH(t)
    parameters = @parameters ḡH EH
    eqs = [ D(mH) ~       (1/τm(ch, V))*(m∞(ch, V) - mH),
            IH ~ ḡH*mH*(EH - V)]
    current = [eqs[2]]
    defaultmap = [mH => ch.mH, ḡH => ch.ḡH]
    return eqs, states, parameters, current, defaultmap
end

"""
leak current
"""
mutable struct Prinz_Leak{S} <: IonChannel
    ḡLeak::S
    #ELeak::S -50
end

ionic_current(::Prinz_Leak, sys::ODESystem) = sys.ILeak
external_params(::Prinz_Leak) = (:ELeak,)

function channel_dynamics(ch::Prinz_Leak, V, Ca, D, t)
    states = @variables ILeak(t)
    parameters = @parameters ḡLeak ELeak
    eqs = [ILeak ~ ḡLeak*(ELeak - V)]
    current = [eqs[1]]
    defaultmap = [ḡLeak => ch.ḡLeak]
    return eqs, states, parameters, current, defaultmap
end


module Dist

import Distributions
const Djl = Distributions
using DocStringExtensions: SIGNATURES

import ..Inference: defalg

import ..Omega
import ..Omega:lift
import ..Prim
import ..Util: *ₛ
import ..NonDet: RandVar, elemtype
import ..Causal: ReplaceRandVar
import ..Inference: FailUnsat
using Spec

# Modules with dist props
import Statistics: mean, var
import Base: minimum, maximum

# Dist Ops
export  succprob,
        support,
        failprob,
        maximum,
        minimum,
        islowerbounded,
        isupperbounded,
        isbounded,
        std,
        mean,
        median,
        mode,
        modes,
        skewness,
        kurtosis,
        isplatykurtic,
        ismesokurtic,
        isleptokurtic,
        entropy,
        prob

        # Dist Algs
export samplemean,
       sampleprob

        # Lifted distributional functions
export  succprobᵣ,
        supportᵣ,
        failprobᵣ,
        maximumᵣ,
        minimumᵣ,
        islowerboundedᵣ,
        isupperboundedᵣ,
        isboundedᵣ,
        stdᵣ,
        medianᵣ,
        modeᵣ,
        modesᵣ,
        meanᵣ,
        probᵣ,
        skewnessᵣ,
        kurtosisᵣ,
        isplatykurticᵣ,
        ismesokurticᵣ,
        isleptokurticᵣ,
        entropyᵣ,
        meanᵣ,
        varᵣ,
        samplemeanᵣ,
        sampleprobᵣ

include("interface.jl")     # Distributional operators
include("distalgs.jl")      # Algorithms that compute distritbuional operators
include("djl.jl")           # Distributions.jl interop
include("liftdistop.jl")    # Lifting for higher order
include("defaults.jl")      # Distributions.jl interop

end


__precompile__()
module SingularIntegralEquations
    using Base, Compat, BandedMatrices, ApproxFun, DualNumbers

export cauchy, cauchyintegral, stieltjes, logkernel,
       stieltjesintegral, hilbert, pseudohilbert, pseudocauchy,
       stieltjesjacobimoment, logjacobimoment

import Base: values,getindex,setindex!,*,.*,+,.+,-,.-,==,<,<=,>,
                >=,./,/,.^,^,\,∪,transpose

import BandedMatrices: bzeros

import Compat: view

import ApproxFun
import ApproxFun: bandinds,SpaceOperator,bilinearform,linebilinearform,dotu,
                  plan_transform,plan_itransform,transform,itransform,transform!,itransform!,
                  rangespace, domainspace, promotespaces, InterlaceOperator, coefficientmatrix,
                  canonicalspace, domain, space, Space, promotedomainspace, promoterangespace, AnyDomain, CalculusOperator,
                  SumSpace,PiecewiseSpace, interlace,Multiplication,ArraySpace,DiagonalArrayOperator,
                  BandedMatrix,ChebyshevDirichlet,PolynomialSpace,AbstractProductSpace,evaluate,order,
                  RealBasis,ComplexBasis,AnyBasis,UnsetSpace,BivariateFun,linesum,complexlength,
                  Fun, ProductFun, LowRankFun, mappoint, JacobiZ,
                  real, UnivariateSpace, RealUnivariateSpace, setdomain, eps, choosedomainspace, isapproxinteger, BlockOperator,
                  ConstantSpace,ReOperator,DirectSumSpace,TupleSpace, ZeroSpace,
                  DiagonalInterlaceOperator, LowRankPertOperator, LaurentDirichlet, setcanonicaldomain,
                  IntervalCurve,PeriodicCurve, reverseorientation, op_eltype, @wrapper, mobius,
                  defaultgetindex, WeightSpace, pochhammer, spacescompatible, ∞, LowRankMatrix, refactorsvd!

# we don't override for Bool and Function to make overriding below easier
# TODO: change when cauchy(f,z,s) calls cauchy(f.coefficients,space(f),z,s)

for OP in (:stieltjes,:stieltjesintegral,:pseudostieltjes)
    @eval begin
        $OP(f::Fun)=$OP(space(f),coefficients(f))
        $OP(f::Fun,z,s...)=$OP(space(f),coefficients(f),z,s...)
        $OP(f::Fun,z,s::Function)=$OP(f,z,s==+)
    end
end

hilbert(f)=Hilbert()*f
hilbert(S,f,z)=hilbert(Fun(f,S))(z)
hilbert(f::Fun,z)=hilbert(space(f),coefficients(f),z)

logkernel(f::Fun,z)=logkernel(space(f),coefficients(f),z)

cauchy(f...)=stieltjes(f...)*(im/(2π))
pseudocauchy(f...)=pseudostieltjes(f...)*(im/(2π))
cauchyintegral(u...)=stieltjesintegral(u...)*(im/(2π))

include("LinearAlgebra/LinearAlgebra.jl")
include("Operators/Operators.jl")
include("FundamentalSolutions/FundamentalSolutions.jl")
include("HypergeometricFunctions/HypergeometricFunctions.jl")

include("JacobiQWeight.jl")
include("JacobiQ.jl")
include("stieltjesmoment.jl")

include("circlecauchy.jl")
include("intervalcauchy.jl")
include("singfuncauchy.jl")

include("vectorcauchy.jl")

include("GreensFun/GreensFun.jl")

include("periodicline.jl")
include("arc.jl")
include("curve.jl")
include("asymptotics.jl")

include("fractals.jl")
include("clustertree.jl")

if isdir(Pkg.dir("TikzGraphs"))
    include("introspect.jl")
end

include("Extras/Extras.jl")

end #module


# Demonstrate/test morphological functions

using ImageProjectiveGeometry
using PyPlot
using PerceptualColourMaps

img = imread("lena.tif");
figure(10), imshow(img);
set_cmap(ColorMap("gray"))
title("Input image")

img2 = rand(512,512) .> 0.999;
figure(11), imshow(img2);
title("Dots input image")

imgd = imerode(img, "RECT", [9, 15]);
figure(12), imshow(imgd);
title("Rectangular dilate")
keypause()

imgd = imerode(img, "RECT", 21);
figure(12), imshow(imgd);
title("Rectangular erode")
keypause()

imgd = imdilate(img, "OCT", 9);
figure(12), imshow(imgd);
title("Octagonal dilate")
keypause()

imgd = imerode(img, "OCT", 9);
figure(12), imshow(imgd);
title("Octagonal erode")
keypause()

img2d = imdilate(img2, "RECT", [19, 15]);
figure(12), imshow(img2d);
title("Dots: Rectangular dilate")
keypause()

img2d = imdilate(img2, "OCT", 9);
figure(12), imshow(img2d);
title("Dots: Octagonal dilate")
keypause()

se = circularstruct(7);
img2d = imdilate(img2, se);
figure(12), imshow(img2d);
title("Dots: Circular dilate")


export UniformSlab

"""
    UniformSlab(depth::Float64)

Wrapper for a `Float64` needed to propagate PlaneWaves that distance
"""
struct UniformSlab
    depth::Float64
end

"""
    _get_propagators(kz::AbstractArray, Δz::Real; reps=1)

Calculate phases to propagate the plane wave in the +/- z direction.
Keyword arguments `reps` repeats the vector several times
"""
function _get_propagators(kz::AbstractArray, Δz::Real; reps=1)
    # fw means forward: from lower to larger z (port i to i+1)
    fw = repeat(exp.((im * Δz) * vec(kz)), reps)
    # bk means backward: from larger to lower z (port i+1 to i)
    bk = conj.(fw)
    Diagonal.((fw, bk))
end

function _sMatrix(pw::PlaneWaves{N}, slab::UniformSlab, um::UniformMedium) where N
    kz = _get_kz(pw, um)
    fw, bk = _get_propagators(kz, slab.depth; reps=N)
    zo = (Bool(0) * I)(N*length(kz))
    [
        zo bk;
        fw zo;
    ]
end

function _sBlockMatrix(pw::PlaneWaves{N}, slab::UniformSlab, um::UniformMedium)::BlockMatrix where N
    kz = _get_kz(pw, um)
    fw, bk = _get_propagators(kz, slab.depth; reps=N)
    zo = (Bool(0) * I)(N*length(kz))
    mortar(
        (zo, bk),
        (fw, zo),
    )
end

function _sBlockMap(pw::PlaneWaves{N}, slab::UniformSlab, um::UniformMedium)::BlockMap where N
    kz = _get_kz(pw, um)
    fw, bk = _get_propagators(kz, slab.depth; reps=N)
    [
        Bool(0) * I    LinearMap(bk);
        LinearMap(fw)    Bool(0) * I
    ]
end

function _sLinearMap(pw::PlaneWaves{N}, slab::UniformSlab, um::UniformMedium)::LinearMap where N
    kz = _get_kz(pw, um)
    d = length(kz)
    fw, bk = _get_propagators(kz, slab.depth; reps=N)
    LinearMap(2d*N) do x
        vcat(
            bk * x[(d*N+1):2d*N],
            fw * x[1:d*N],
        )
    end
end


#################### Monte Carlo Standard Errors ####################

function mcse(x::Vector{T}, method::Symbol=:imse; args...) where {T<:Real}
  method == :bm ? mcse_bm(x; args...) :
  method == :imse ? mcse_imse(x) :
  method == :ipse ? mcse_ipse(x) :
    throw(ArgumentError("unsupported mcse method $method"))
end

function mcse_bm(x::Vector{T}; size::Integer=100) where {T<:Real}
    n = length(x)
    m = div(n, size)
    if m < 2
        throw(
         ArgumentError("iterations are < $(2 * size) and batch size is > $(div(n, 2))")
        )
    end
    mbar = [mean(x[i * size .+ (1:size)]) for i in 0:(m - 1)]
    return sem(mbar)
end

function mcse_imse(x::Vector{T}) where {T<:Real}
    n = length(x)
    m = div(n - 2, 2)
    x_ = map(Float64, x)
    ghat = autocov(x_, [0, 1])
    Ghat = sum(ghat)
    value = -ghat[1] + 2 * Ghat
    for i in 1:m
        Ghat = min(Ghat, sum(autocov(x_, [2 * i, 2 * i + 1])))
        Ghat > 0 || break
        value += 2 * Ghat
    end
    return sqrt(value / n)
end

function mcse_ipse(x::Vector{T}) where {T<:Real}
    n = length(x)
    m = div(n - 2, 2)
    x_ = map(Float64, x)
    ghat = autocov(x_, [0, 1])
    value = ghat[1] + 2 * ghat[2]
    for i in 1:m
        Ghat = sum(autocov(x_, [2 * i, 2 * i + 1]))
        Ghat > 0 || break
        value += 2 * Ghat
    end
    return sqrt(value / n)
end


module steam_governor
using ReachabilityAnalysis
@taylorize function steam_governor!(du, u, p, t)
    local ϵ = 3.0
    local α = 1.0
    local β = 1.0

    du[1] = u[2]
    du[2] = u[3]^2 * sin(u[1]) * cos(u[1]) - sin(u[1]) - ϵ*u[2]
    du[3] = α*(cos(u[1]) - β)

    return du
end

function model(X0)
    S = @system(x' = steam_governor!(x), dim:3)
    return IVP(S, X0)
end
end # module


# MATH OPERATIONS

for op in (:sin, :cos, :tan, :atan, :asin, :acos, :log, :log1p, :log10, :sqrt,
    :exp, :expm1, :erf, :erfc, :cbrt, :lgamma, :acosh, :cosh, :asinh, :sinh, :atanh, :tanh, :factorial)
    @eval Base.($(quot(op))){T}(x::AFAbstractArray{T}) = AFArray{T}(@cxx af::($op)(x.array))
end

Base.gamma{T}(x::AFAbstractArray{T}) = AFArray{T}(@cxx af::tgamma(x))

import Base: +, -, abs, .^

# TODO: add! using +=, etc.

.^{T,S}(a::AFAbstractArray{T}, b::AFAbstractArray{S}) = AFArray{af_promote(T,S)}(af_pow(a,b))
.^{T}(a::Real, b::AFAbstractArray{T}) = AFArray{T}(af_pow(a,b))
.^{T}(a::AFAbstractArray{T}, b::Real) = AFArray{T}(af_pow(a,b))

export root
root{T,S}(a::AFAbstractArray{T}, b::AFAbstractArray{S}) = AFArray{af_promote(T,S)}(af_root(a,b))
root{T}(a::Real, b::AFAbstractArray{T}) = AFArray{T}(af_root(a,b))
root{T}(a::AFAbstractArray{T}, b::Real) = AFArray{T}(af_root(a,b))

import Base: abs, min, max

#Abs
abs{T}(a::AFAbstractArray{T}) = AFArray{T}(af_abs(a))

#Max
function max(a::AFAbstractArray, b::AFAbstractArray)
    out = af_max(a,b)
    AFArray{backend_eltype(out)}(out)
end
function max(a::AFAbstractArray, b::Real)
    if typeof(b) <: Integer
        b = Float32(b)
    end
    out = af_max(a,b)
    AFArray{backend_eltype(out)}(out)
end
max(a::Real, b::AFAbstractArray) = max(b, a)

#Min
function min(a::AFAbstractArray, b::AFAbstractArray)
    out = af_min(a, b)
    AFArray{backend_eltype(out)}(out)
end
function min(a::AFAbstractArray, b::Real)
    if typeof(b) <: Integer
        b = Float32(b)
    end
    out = af_min(a,b)
    AFArray{backend_eltype(out)}(out)
end
min(a::Real, b::AFAbstractArray) = min(b, a)

#Negation
-{T}(a::AFSubArray{T}) = AFArray{T}(icxx"0-$a;")
-{T}(a::AFArray{T}) = AFArray{T}(af_neg(a))

#Logical ops
import Base: ==, .==, .>, .<, .>=, .<=, |, .!=

#Equals
.==(a::Union{Real,AFAbstractArray}, b::Union{Real,AFAbstractArray}) = AFArray{Bool}(af_equals(a,b))

#Greater
.>(a::Union{Real,AFAbstractArray}, b::Union{Real,AFAbstractArray}) = AFArray{Bool}(af_gt(a,b))
.>=(a::Union{Real,AFAbstractArray}, b::Union{Real,AFAbstractArray}) = AFArray{Bool}(af_ge(a,b))

#Lesser
.<(a::Union{Real,AFAbstractArray}, b::Union{Real,AFAbstractArray}) = AFArray{Bool}(af_lt(a,b))
.<=(a::Union{Real,AFAbstractArray}, b::Union{Real,AFAbstractArray}) = AFArray{Bool}(af_le(a,b))

#Or
|(a::AFAbstractArray{Bool}, b::AFAbstractArray{Bool}) = AFArray{Bool}(af_bitor(a,b))

#not equals
.!=(a::AFAbstractArray, b::AFAbstractArray) = AFArray{Bool}(af_neq(a,b))

#Complex functions
import Base:complex, real, imag

complex{T}(a::AFAbstractArray{T}) = AFArray{Complex{T}}(af_complex(a))
complex{T}(a::AFAbstractArray{T}, b::AFAbstractArray{T}) = AFArray{Complex{T}}(af_complex(a,b))
real{T}(a::AFAbstractArray{Complex{T}}) = AFArray{T}(af_real(a))
imag{T}(a::AFAbstractArray{Complex{T}}) = AFArray{T}(af_imag(a))


"""
    MPO{L,T<:Number}

Matrix product operator for `L` sites with numerical entries of type `T`.

The tensors are stored with the indices `[a, m, n, b]` corresponding to

       n
       |
    a -#- b
       |
       m

where the physical indices (`m`, `n`) are between the bond indices (`a`, `b`).
The end tensors also have this structure, so they must be capped explicitly.
"""
struct MPO{L,T<:Number}
    "Tensors making up the MPO."
    tnsrs::Vector{Array{T,4}}
end

"""
    MPO(x::Array{T,4}, L)

Create an `MPO` for `L` sites with all interior sites containing the tensor
`x`. The tensor is assumed to have the usual matrix-of-operators structure,
with the first two indices being the bond (matrix) dimension and the last two
indices being the physical (operator) dimension. The first and last sites only
use the second (not last!) row and first column of `x`, respectively.
"""
function MPO(x::Array{T,4}, L) where {T}
    L >= 2 || throw(DomainError(L, "At least 2 sites."))

    tnsrs = Vector{Array{T,4}}(undef, L)
    # Row vector.
    tnsrs[1] = permutedims(x[2:2, :, :, :], (1, 3, 4, 2))
    for i in 2:(L-1)
        # Matrix.
        tnsrs[i] = permutedims(x, (1, 3, 4, 2))
    end
    # Column vector.
    tnsrs[L] = permutedims(x[:, 1:1, :, :], (1, 3, 4, 2))

    MPO{L,T}(tnsrs)
end

"""
    compress!(o::MPO, cutoff_max::Float64)

Compress `o` by throwing away singular values below `cutoff_max` at each bond.
"""
function compress!(o::MPO{L}, cutoff_max::Float64) where {L}
    bond_dimensions = Array{Int}(undef, L-1)

    for range in [1:(L-2), (L-1):-1:1]
        for site in range
            # Combine adjacent tensors:
            #
            #        n     p
            #        |     |
            #     a -#- b -#- c
            #        |     |
            #        m     o
            @tensor M[a, m, n, o, p, c] :=
                o.tnsrs[site][a, m, n, b] * o.tnsrs[site+1][b, o, p, c]
            M_mat = reshape(M, size(M, 1) * size(M, 2) * size(M, 3),
                               size(M, 4) * size(M, 5) * size(M, 6))

            U, S, V = svd(M_mat)
            # Operators are not normalized in general, so we should not expect
            # that sum(S.^2) == 1.
            trunc_total = sum(S.^2)
            trunc_len = length(S)
            trunc_cutoff = 0.0
            while (1 < trunc_len &&
                   trunc_cutoff + S[trunc_len]^2/trunc_total <= cutoff_max)
                trunc_cutoff += S[trunc_len]^2/trunc_total
                trunc_len -= 1
            end
            A = U[:, 1:trunc_len]
            B = diagm(S[1:trunc_len]) * V[:, 1:trunc_len]'
            o.tnsrs[site] = reshape(A, size(M, 1), size(M, 2), size(M, 3),
                                       trunc_len)
            o.tnsrs[site+1] = reshape(B, trunc_len,
                                         size(M, 4), size(M, 5), size(M, 6))

            bond_dimensions[site] = trunc_len
        end
    end

    mean(bond_dimensions)
end


function additional_prices(mdl::Union{Model,ModelInit})

    R,Rᴸ = mdl.Prices
    ω = (Lₛ * ( 1 + (1-ρ) * R) )^-1
    τ = (ϐ / ω) * (1 - Lₛ) / Lₛ

    return([ω,τ])

end


#== # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # #
#
# Description
#
#   Rotations from an Earth-Centered Inertial (ECI) reference frame to another
#   ECI reference frame.
#
# # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # #
#
# References
#
#   [1] Vallado, D. A (2013). Fundamentals of Astrodynamics and Applications.
#       Microcosm Press, Hawthorn, CA, USA.
#
# # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # ==#

export rECItoECI

"""
    rECEFtoECI([T,] ECIo, ECIf, JD_UTC::Number [, eop_data])
    rECEFtoECI([T,] ECIo, JD_UTCo::Number, ECIf, JD_UTCf::Number [, eop_data])

Compute the rotation from an Earth-Centered Inertial (`ECI`) reference frame to
another ECI reference frame. If the origin and destination frame contain only
one *of date* frame, then the first signature is used and `JD_UTC` is the epoch
of this frame. On the other hand, if the origin and destination frame contain
two *of date* frame[^1], e.g. TOD => MOD, then the second signature must be used
in which `JD_UTCo` is the epoch of the origin frame and `JD_UTCf` is the epoch
of the destination frame.

The rotation description that will be used is given by `T`, which can be `DCM`
or `Quaternion`. The origin ECI frame is selected by the input `ECIo` and the
destination ECI frame is selected by the input `ECIf`. The model used to compute
the rotation is specified by the selection of the origin and destination frames.
Currently, there are two models supported: IAU-76/FK5 and IAU-2006 with 2010
conventions (CIO approach only).

[^1]: TEME is an *of date* frame.

# Rotation description

The rotations that aligns the origin ECI frame with the destination ECI frame
can be described by Direction Cosine Matrices or Quaternions. This is selected
by the parameter `T`.

The possible values are:

* `DCM`: The rotation will be described by a Direction Cosine Matrix.
* `Quaternion`: The rotation will be described by a Quaternion.

If no value is specified, then it falls back to `DCM`.

# Conversion model

The model that will be used to compute the rotation is automatically inferred
given the selection of the origin and destination frames. **Notice that mixing
IAU-76/FK5 and IAU-2006/2010 frames is not supported yet.**

# ECI Frame

The supported ECI frames for both origin `ECIo` and destination `ECIf` are:

* `TEME()`: ECI will be selected as the True Equator Mean Equinox (TEME)
            reference frame.
* `TOD()`: ECI will be selected as the True of Date (TOD).
* `MOD()`: ECI will be selected as the Mean of Date (MOD).
* `J2000()`: ECI will be selected as the J2000 reference frame.
* `GCRF()`: ECI will be selected as the Geocentric Celestial Reference Frame
            (GCRF).
* `CIRS()`: ECEF will be selected as the Celestial Intermediate Reference System
            (CIRS).

# EOP Data

The conversion between the frames depends on EOP Data (see `get_iers_eop` and
`read_iers_eop`). If IAU-76/FK5 model is used, then the type of `eop_data` must
be `EOPData_IAU1980`. Otherwise, if IAU-2006/2010 model is used, then the type
of `eop_data` must be `EOPData_IAU2000A`. The following table shows the
requirements for EOP data given the selected frames.

|   Model       |   ECIo  |   ECIf  |    EOP Data   | Function Signature |
|:--------------|:--------|:--------|:--------------|:-------------------|
| IAU-76/FK5    | `GCRF`  | `J2000` | EOP IAU1980   | First              |
| IAU-76/FK5    | `GCRF`  | `MOD`   | EOP IAU1980   | First              |
| IAU-76/FK5    | `GCRF`  | `TOD`   | EOP IAU1980   | First              |
| IAU-76/FK5    | `GCRF`  | `TEME`  | EOP IAU1980   | First              |
| IAU-76/FK5    | `J2000` | `GCRF`  | EOP IAU1980   | First              |
| IAU-76/FK5    | `J2000` | `MOD`   | Not required  | First              |
| IAU-76/FK5    | `J2000` | `TOD`   | Not required  | First              |
| IAU-76/FK5    | `J2000` | `TEME`  | Not required  | First              |
| IAU-76/FK5    | `MOD`   | `GCRF`  | EOP IAU1980   | First              |
| IAU-76/FK5    | `MOD`   | `J2000` | Not required  | First              |
| IAU-76/FK5    | `MOD`   | `TOD`   | Not required  | Second             |
| IAU-76/FK5    | `MOD`   | `TEME`  | Not required  | Second             |
| IAU-76/FK5    | `TOD`   | `GCRF`  | EOP IAU1980   | First              |
| IAU-76/FK5    | `TOD`   | `J2000` | Not required  | First              |
| IAU-76/FK5    | `TOD`   | `MOD`   | Not required  | Second             |
| IAU-76/FK5    | `TOD`   | `TEME`  | Not required  | Second             |
| IAU-76/FK5    | `TEME`  | `GCRF`  | EOP IAU1980   | First              |
| IAU-76/FK5    | `TEME`  | `J2000` | Not required  | First              |
| IAU-76/FK5    | `TEME`  | `MOD`   | Not required  | Second             |
| IAU-76/FK5    | `TEME`  | `TOD`   | Not required  | Second             |
| IAU-2006/2010 | `GCRF`  | `CIRS`  | Not required¹ | First              |
| IAU-2006/2010 | `CIRS`  | `CIRS`  | Not required¹ | Second             |

`¹`: In this case, the terms that account for the free-core nutation and time
dependent effects of the Celestial Intermediate Pole (CIP) position with respect
to the GCRF will not be available, reducing the precision.

## MOD and TOD

In this function, if EOP corrections are not provided, then MOD and TOD frames
will be computed considering the original IAU-76/FK5 theory. Otherwise, the
corrected frame will be used.

# Returns

The rotation description represented by `T` that rotates the origin ECI
reference frame into alignment with the destination ECI reference frame.

# Examples

```julia-repl
julia> eop_IAU1980 = get_iers_eop(:IAU1980);

julia> rECItoECI(DCM, GCRF(), J2000(), DatetoJD(1986, 6, 19, 21, 35, 0), eop_IAU1980)
3×3 StaticArrays.SArray{Tuple{3,3},Float64,2,9}:
  1.0          -2.45469e-12   4.56602e-10
  2.45466e-12   1.0          -1.84455e-9
 -4.56602e-10   1.84455e-9    1.0

julia> rECItoECI(Quaternion, TEME(), GCRF(), DatetoJD(1986, 6, 19, 21, 35, 0), eop_IAU1980)
Quaternion{Float64}:
  + 0.9999986335698654 + 1.8300414020900853e-5.i + 0.0006653038276169474.j - 0.0015132396749411375.k

julia> rECItoECI(TOD(), DatetoJD(1986,6,19,21,35,0), TOD(), DatetoJD(1987,5,19,3,0,0), eop_IAU1980)
3×3 StaticArrays.SArray{Tuple{3,3},Float64,2,9}:
 1.0          -0.000224087  -9.73784e-5
 0.000224086   1.0          -5.79859e-6
 9.73797e-5    5.77677e-6    1.0

julia> rECItoECI(Quaternion, TOD(), JD_J2000, MOD(), JD_J2000, eop_IAU1980)
Quaternion{Float64}:
  + 0.9999999993282687 - 1.400220690336851e-5.i + 1.3473593746216003e-5.j - 3.107834312843103e-5.k

julia> rECItoECI(J2000(), TEME(), DatetoJD(1986,6,19,21,35,0))
3×3 StaticArrays.SArray{Tuple{3,3},Float64,2,9}:
  0.999995    0.0030265    0.00133055
 -0.00302645  0.999995    -3.86125e-5
 -0.00133066  3.45854e-5   0.999999

julia> eop_IAU2000A = get_iers_eop(:IAU2000A);

julia> rECItoECI(CIRS(), GCRF(), DatetoJD(1986,6,19,21,35,0), eop_IAU2000A)
3×3 StaticArrays.SArray{Tuple{3,3},Float64,2,9}:
 0.999999     3.88379e-8  -0.00133066
 7.18735e-9   1.0          3.45882e-5
 0.00133066  -3.45882e-5   0.999999

julia> rECItoECI(Quaternion, CIRS(), GCRF(), DatetoJD(1986,6,19,21,35,0), eop_IAU2000A)
Quaternion{Float64}:
  + 0.9999997785177528 + 1.7294102099105917e-5.i + 0.0006653310148723835.j + 7.912627369563795e-9.k
```
"""
@inline rECItoECI(T_ECIo::T_ECIs, T_ECIf::T_ECIs, JD_UTC::Number,
                  eop_data::EOPData_IAU1980) =
    rECItoECI(DCM, T_ECIo, T_ECIf, JD_UTC, eop_data)

@inline rECItoECI(T_ECIo::T_ECIs_IAU_2006, T_ECIf::T_ECIs_IAU_2006,
                  JD_UTC::Number, eop_data::EOPData_IAU2000A) =
    rECItoECI(DCM, T_ECIo, T_ECIf, JD_UTC, eop_data)

# Specializations for those cases in which we have two *of dates* frames.
@inline rECItoECI(T_ECIo::T_ECIs_of_date, JD_UTCo::Number,
                  T_ECIf::T_ECIs_of_date, JD_UTCf::Number,
                  eop_data::EOPData_IAU1980) =
    rECItoECI(DCM, T_ECIo, JD_UTCo, T_ECIf, JD_UTCf, eop_data)

@inline rECItoECI(T_ECIo::Val{:CIRS}, JD_UTCo::Number,
                  T_ECIf::Val{:CIRS}, JD_UTCf::Number,
                  eop_data::EOPData_IAU2000A) =
    rECItoECI(DCM, T_ECIo, JD_UTCo, T_ECIf, JD_UTCf, eop_data)

# Specializations for those cases that EOP Data is not needed.
@inline rECItoECI(T_ECIo::Val{:J2000},
                  T_ECIf::Union{Val{:MOD}, Val{:TOD}, Val{:TEME}},
                  JD_UTC::Number) =
    rECItoECI(DCM, T_ECIo, T_ECIf, JD_UTC)

@inline rECItoECI(T_ECIo::Union{Val{:MOD}, Val{:TOD}, Val{:TEME}},
                  T_ECIf::Val{:J2000},
                  JD_UTC::Number) =
    rECItoECI(DCM, T_ECIo, T_ECIf, JD_UTC)

@inline rECItoECI(T_ECIo::T_ECIs_of_date, JD_UTCo::Number,
                  T_ECIf::T_ECIs_of_date, JD_UTCf::Number) =
    rECItoECI(DCM, T_ECIo, JD_UTCo, T_ECIf, JD_UTCf)

@inline rECItoECI(T_ECIo::T_ECIs_IAU_2006, T_ECIf::T_ECIs_IAU_2006,
                  JD_UTC::Number) =
    rECItoECI(DCM, T_ECIo, T_ECIf, JD_UTC)

 @inline rECItoECI(T_ECIo::Val{:CIRS}, JD_UTCo::Number,
                   T_ECIf::Val{:CIRS}, JD_UTCf::Number) =
    rECItoECI(DCM, T_ECIo, JD_UTCo, T_ECIf, JD_UTCf)

################################################################################
#                                  IAU-76/FK5
################################################################################

#                                GCRF <=> J2000
# ==============================================================================

function rECItoECI(T::T_ROT, ::Val{:GCRF}, ::Val{:J2000}, JD_UTC::Number,
                   eop_data::EOPData_IAU1980)

    arcsec2rad = π/648000

    # Get the time in UT1 and TT.
    JD_UT1 = JD_UTCtoUT1(JD_UTC, eop_data)
    JD_TT  = JD_UTCtoTT(JD_UTC)

    # Get the EOP data related to the desired epoch.
    #
    # TODO: The difference is small, but should it be `JD_TT` or `JD_UTC`?
    δΔϵ_1980 = eop_data.dEps(JD_UTC)*arcsec2rad
    δΔψ_1980 = eop_data.dPsi(JD_UTC)*arcsec2rad

    # In this case, we need to convert GCRF back to PEF and then convert to
    # J2000, which is the same conversion from PEF to GCRF **without** the EOP
    # data.
    #
    # TODO: Can I simplify the rotation from TOD with corrections to TOD without
    # corrections?
    r_MOD_GCRF  = rGCRFtoMOD_fk5(T, JD_TT)
    r_PEF_MOD   = rMODtoPEF_fk5(T, JD_UT1, JD_TT, δΔϵ_1980, δΔψ_1980)
    r_MOD_PEF   = rPEFtoMOD_fk5(T, JD_UT1, JD_TT, 0, 0)
    r_J2000_MOD = inv_rotation(r_MOD_GCRF)

    # Compose the full rotation.
    return compose_rotation(r_MOD_GCRF, r_PEF_MOD, r_MOD_PEF, r_J2000_MOD)
end

@inline rECItoECI(T::T_ROT, T_ECIo::Val{:J2000}, T_ECIf::Val{:GCRF},
                  JD_UTC::Number, eop_data::EOPData_IAU1980) =
    inv_rotation(rECItoECI(T, T_ECIf, T_ECIo, JD_UTC, eop_data))

#                                 GCRF <=> MOD
# ==============================================================================

function rECItoECI(T::T_ROT, ::Val{:GCRF}, ::Val{:MOD}, JD_UTC::Number,
                   eop_data::EOPData_IAU1980)

    # Get the time in TT.
    JD_TT  = JD_UTCtoTT(JD_UTC)

    # Return the rotation.
    return rGCRFtoMOD_fk5(T, JD_TT)
end

@inline rECItoECI(T::T_ROT, T_ECIo::Val{:MOD}, T_ECIf::Val{:GCRF},
                  JD_UTC::Number, eop_data::EOPData_IAU1980) =
    inv_rotation(rECItoECI(T, T_ECIf, T_ECIo, JD_UTC, eop_data))

#                                 GCRF <=> TOD
# ==============================================================================

function rECItoECI(T::T_ROT, ::Val{:GCRF}, ::Val{:TOD}, JD_UTC::Number,
                   eop_data::EOPData_IAU1980)

    arcsec2rad = π/648000

    # Get the time in TT.
    JD_TT  = JD_UTCtoTT(JD_UTC)

    # Get the EOP data related to the desired epoch.
    #
    # TODO: The difference is small, but should it be `JD_TT` or `JD_UTC`?
    δΔϵ_1980 = eop_data.dEps(JD_UTC)*arcsec2rad
    δΔψ_1980 = eop_data.dPsi(JD_UTC)*arcsec2rad

    # Return the rotation.
    r_MOD_GCRF = rGCRFtoMOD_fk5(T, JD_TT)
    r_TOD_MOD  = rMODtoTOD_fk5(T, JD_TT, δΔϵ_1980, δΔψ_1980)

    # Compose the full rotation.
    return compose_rotation(r_MOD_GCRF, r_TOD_MOD)
end

@inline rECItoECI(T::T_ROT, T_ECIo::Val{:TOD}, T_ECIf::Val{:GCRF},
                  JD_UTC::Number, eop_data::EOPData_IAU1980) =
    inv_rotation(rECItoECI(T, T_ECIf, T_ECIo, JD_UTC, eop_data))

#                                GCRF <=> TEME
# ==============================================================================

function rECItoECI(T::T_ROT, ::Val{:GCRF}, ::Val{:TEME}, JD_UTC::Number,
                   eop_data::EOPData_IAU1980)

    arcsec2rad = π/648000

    # Get the time in TT.
    JD_TT  = JD_UTCtoTT(JD_UTC)

    # Get the EOP data related to the desired epoch.
    #
    # TODO: The difference is small, but should it be `JD_TT` or `JD_UTC`?
    δΔϵ_1980 = eop_data.dEps(JD_UTC)*arcsec2rad
    δΔψ_1980 = eop_data.dPsi(JD_UTC)*arcsec2rad

    # Return the rotation.
    r_MOD_GCRF = rGCRFtoMOD_fk5(T, JD_TT)
    r_TEME_MOD = rMODtoTEME(T, JD_TT, δΔϵ_1980, δΔψ_1980)

    # Compose the full rotation.
    return compose_rotation(r_MOD_GCRF, r_TEME_MOD)
end

@inline rECItoECI(T::T_ROT, T_ECIo::Val{:TEME}, T_ECIf::Val{:GCRF},
                  JD_UTC::Number, eop_data::EOPData_IAU1980) =
    inv_rotation(rECItoECI(T, T_ECIf, T_ECIo, JD_UTC, eop_data))

#                                J2000 <=> MOD
# ==============================================================================

function rECItoECI(T::T_ROT, ::Val{:J2000}, ::Val{:MOD}, JD_UTC::Number,
                   eop_data::EOPData_IAU1980)

    arcsec2rad = π/648000

    # Get the time in UT1 and TT.
    JD_UT1 = JD_UTCtoUT1(JD_UTC, eop_data)
    JD_TT  = JD_UTCtoTT(JD_UTC)

    # Get the EOP data related to the desired epoch.
    #
    # TODO: The difference is small, but should it be `JD_TT` or `JD_UTC`?
    δΔϵ_1980 = eop_data.dEps(JD_UTC)*arcsec2rad
    δΔψ_1980 = eop_data.dPsi(JD_UTC)*arcsec2rad

    # In this case, we need to convert J2000 back to PEF and then convert to
    # MOD. This is necessary because we need to apply EOP corrections to convert
    # to MOD and just a `rGCRFtoMOD_fk5` would yield a frame according to the
    # original IAU-76/FK5 theory.
    #
    # TODO: Can I simplify this rotation?
    r_MOD_J2000 = rGCRFtoMOD_fk5(T, JD_TT)
    r_PEF_MOD   = rMODtoPEF_fk5(T, JD_UT1, JD_TT, 0, 0)
    r_MOD_PEF   = rPEFtoMOD_fk5(T, JD_UT1, JD_TT, δΔϵ_1980, δΔψ_1980)

    # Compose the full rotation.
    return compose_rotation(r_MOD_J2000, r_PEF_MOD, r_MOD_PEF)
end

function rECItoECI(T::T_ROT, ::Val{:J2000}, ::Val{:MOD}, JD_UTC::Number)
    # Get the time in TT.
    JD_TT  = JD_UTCtoTT(JD_UTC)

    # Compute and return the rotation.
    r_MOD_J2000 = rGCRFtoMOD_fk5(T, JD_TT)

    return r_MOD_J2000
end

@inline rECItoECI(T::T_ROT, T_ECIo::Val{:MOD}, T_ECIf::Val{:J2000},
                  JD_UTC::Number, eop_data::EOPData_IAU1980) =
    inv_rotation(rECItoECI(T, T_ECIf, T_ECIo, JD_UTC, eop_data))

@inline rECItoECI(T::T_ROT, T_ECIo::Val{:MOD}, T_ECIf::Val{:J2000},
                  JD_UTC::Number) =
    inv_rotation(rECItoECI(T, T_ECIf, T_ECIo, JD_UTC))

#                                J2000 <=> TOD
# ==============================================================================

function rECItoECI(T::T_ROT, ::Val{:J2000}, ::Val{:TOD}, JD_UTC::Number,
                   eop_data::EOPData_IAU1980)

    arcsec2rad = π/648000

    # Get the time in UT1 and TT.
    JD_UT1 = JD_UTCtoUT1(JD_UTC, eop_data)
    JD_TT  = JD_UTCtoTT(JD_UTC)

    # Get the EOP data related to the desired epoch.
    #
    # TODO: The difference is small, but should it be `JD_TT` or `JD_UTC`?
    δΔϵ_1980 = eop_data.dEps(JD_UTC)*arcsec2rad
    δΔψ_1980 = eop_data.dPsi(JD_UTC)*arcsec2rad

    # In this case, we need to convert J2000 back to PEF and then convert to
    # TOD. This is necessary because we need to apply EOP corrections to convert
    # to TOD.
    #
    # TODO: Can I simplify this rotation?
    r_MOD_J2000 = rGCRFtoMOD_fk5(T, JD_TT)
    r_PEF_MOD   = rMODtoPEF_fk5(T, JD_UT1, JD_TT, 0, 0)
    r_MOD_PEF   = rPEFtoMOD_fk5(T, JD_UT1, JD_TT, δΔϵ_1980, δΔψ_1980)
    r_TOD_MOD   = rMODtoTOD_fk5(T, JD_TT, δΔϵ_1980, δΔψ_1980)

    # Compose the full rotation.
    return compose_rotation(r_MOD_J2000, r_PEF_MOD, r_MOD_PEF, r_TOD_MOD)
end

function rECItoECI(T::T_ROT, ::Val{:J2000}, ::Val{:TOD}, JD_UTC::Number)
    # Get the time in TT.
    JD_TT  = JD_UTCtoTT(JD_UTC)

    # Compute and return the composed rotation.
    r_MOD_J2000 = rGCRFtoMOD_fk5(T, JD_TT)
    r_TOD_MOD   = rMODtoTOD_fk5(T, JD_TT, 0, 0)

    return compose_rotation(r_MOD_J2000, r_TOD_MOD)
end

@inline rECItoECI(T::T_ROT, T_ECIo::Val{:TOD}, T_ECIf::Val{:J2000},
                  JD_UTC::Number, eop_data::EOPData_IAU1980) =
    inv_rotation(rECItoECI(T, T_ECIf, T_ECIo, JD_UTC, eop_data))

@inline rECItoECI(T::T_ROT, T_ECIo::Val{:TOD}, T_ECIf::Val{:J2000},
                  JD_UTC::Number) =
    inv_rotation(rECItoECI(T, T_ECIf, T_ECIo, JD_UTC))

#                                J2000 <=> TEME
# ==============================================================================

@inline rECItoECI(T::T_ROT, T_ECIo::Val{:J2000}, T_ECEFf::Val{:TEME},
                  JD_UTC::Number, eop_data::EOPData_IAU1980) =
    rECItoECI(T, T_ECIo, T_ECEFf, JD_UTC)

@inline rECItoECI(T::T_ROT, T_ECIo::Val{:TEME}, T_ECEFf::Val{:J2000},
                  JD_UTC::Number, eop_data::EOPData_IAU1980) =
    rECItoECI(T, T_ECIo, T_ECEFf, JD_UTC)

function rECItoECI(T::T_ROT, ::Val{:J2000}, ::Val{:TEME}, JD_UTC::Number)
    # Get the time in TT.
    JD_TT  = JD_UTCtoTT(JD_UTC)

    # Return the rotation.
    return rGCRFtoTEME(T, JD_TT, 0, 0)
end

@inline rECItoECI(T::T_ROT, T_ECIo::Val{:TEME}, T_ECIf::Val{:J2000},
                  JD_UTC::Number) =
    inv_rotation(rECItoECI(T, T_ECIf, T_ECIo, JD_UTC))

#                          Between MOD, TOD, and TEME
# ==============================================================================

function rECItoECI(T::T_ROT, T_ECIo::T_ECIs_of_date, JD_UTCo::Number,
                   T_ECIf::T_ECIs_of_date, JD_UTCf::Number,
                   eop_data::EOPData_IAU1980)

    # In this case, we convert origin to GCRF and then convert back to the
    # destination. This is necessary because the user may want to change the
    # epoch.
    r_GCRF_ECIo = rECItoECI(T, T_ECIo,     Val(:GCRF), JD_UTCo, eop_data)
    r_ECIf_GCRF = rECItoECI(T, Val(:GCRF), T_ECIf,     JD_UTCf, eop_data)

    # Return the full rotation.
    return compose_rotation(r_GCRF_ECIo, r_ECIf_GCRF)
end

function rECItoECI(T::T_ROT, T_ECIo::T_ECIs_of_date, JD_UTCo::Number,
                   T_ECIf::T_ECIs_of_date, JD_UTCf::Number)

    # In this case, in which we do not have EOP data, we convert origin to J2000
    # and then convert back to the destination. This is necessary because the
    # user may want to change the epoch.
    r_GCRF_ECIo = rECItoECI(T, T_ECIo,      Val(:J2000), JD_UTCo)
    r_ECIf_GCRF = rECItoECI(T, Val(:J2000), T_ECIf,      JD_UTCf)

    # Return the full rotation.
    return compose_rotation(r_GCRF_ECIo, r_ECIf_GCRF)
end

################################################################################
#                                IAU-2006/2010
################################################################################

#                                GCRF <=> CIRS
# ==============================================================================

function rECItoECI(T::T_ROT, ::Val{:GCRF}, ::Val{:CIRS}, JD_UTC::Number,
                   eop_data::EOPData_IAU2000A)

    arcsec2rad = π/648000

    # Get the time in TT.
    JD_TT = JD_UTCtoTT(JD_UTC)

    # Get the EOP data related to the desired epoch.
    dX = eop_data.dX(JD_UTC)*arcsec2rad
    dY = eop_data.dY(JD_UTC)*arcsec2rad

    # Compute and return the rotation.
    return rGCRFtoCIRS_iau2006(T, JD_TT, dX, dY)
end

@inline rECItoECI(T::T_ROT, T_ECIo::Val{:CIRS}, T_ECIf::Val{:GCRF},
                  JD_UTC::Number, eop_data::EOPData_IAU2000A) =
    inv_rotation(rECItoECI(T, T_ECIf, T_ECIo, JD_UTC, eop_data))

function rECItoECI(T::T_ROT, ::Val{:GCRF}, ::Val{:CIRS}, JD_UTC::Number)
    # Get the time in TT.
    JD_TT = JD_UTCtoTT(JD_UTC)

    # Compute and return the rotation.
    return rGCRFtoCIRS_iau2006(T, JD_TT)
end

@inline rECItoECI(T::T_ROT, T_ECIo::Val{:CIRS}, T_ECIf::Val{:GCRF},
                  JD_UTC::Number) =
    inv_rotation(rECItoECI(T, T_ECIf, T_ECIo, JD_UTC))

#                                 Between CIRS
# ==============================================================================

function rECItoECI(T::T_ROT, T_ECIo::Val{:CIRS}, JD_UTCo::Number,
                   T_ECIf::Val{:CIRS}, JD_UTCf::Number,
                   eop_data::EOPData_IAU2000A)

    # In this case, we convert origin to GCRF and then convert back to the
    # destination. This is necessary because the user may want to change the
    # epoch.
    r_GCRF_ECIo = rECItoECI(T, T_ECIo,     Val(:GCRF), JD_UTCo, eop_data)
    r_ECIf_GCRF = rECItoECI(T, Val(:GCRF), T_ECIf,     JD_UTCf, eop_data)

    # Return the full rotation.
    return compose_rotation(r_GCRF_ECIo, r_ECIf_GCRF)
end

function rECItoECI(T::T_ROT, T_ECIo::Val{:CIRS}, JD_UTCo::Number,
                   T_ECIf::Val{:CIRS}, JD_UTCf::Number)

    # In this case, we convert origin to GCRF and then convert back to the
    # destination. This is necessary because the user may want to change the
    # epoch.
    r_GCRF_ECIo = rECItoECI(T, T_ECIo,     Val(:GCRF), JD_UTCo)
    r_ECIf_GCRF = rECItoECI(T, Val(:GCRF), T_ECIf,     JD_UTCf)

    # Return the full rotation.
    return compose_rotation(r_GCRF_ECIo, r_ECIf_GCRF)
end


function build_nabla2_z(LF)

  D2jl = LF.LFz.D2jl

  Nx = LF.Nx
  Ny = LF.Ny
  Nz = LF.Nz

  Npoints = Nx*Ny*Nz

  # initialize with
  rowval = zeros(Int64,Nz*Npoints)
  colptr = zeros(Int64,Npoints+1)
  colptr[1] = 1
  nzval = zeros(Float64,Nz*Npoints)

  idx = 0
  idxCol = 1

  #print("\n")
  for ic = 1:Nx*Ny
    for colLoc = 1:Nz
      for rowLoc = 1:Nz
        idx = idx + 1
        rowGbl = rowLoc + (ic-1)*Nz
        rowval[idx] = rowGbl
        nzval[idx] = D2jl[rowLoc,colLoc]
        #println("colGbl = ", colGbl, " rowLoc = ", rowLoc, " colLoc = ", colLoc)
      end
      idxCol = idxCol + 1
      colptr[idxCol] = colptr[idxCol-1] + Nz
    end
  end

  return SparseMatrixCSC( Npoints, Npoints, colptr, rowval, nzval )

end


export KalmanSmoother, kalman_smoother

struct KalmanSmoother
    x
    P
    Po
    J
    S11
    S00
    S10
    Syy
    z
end

function kalman_smoother(m::LinearStateSpaceModel,z,u=fill([0.0],length(z)))
    kalman_smoother(kalman_filter(m,z,u),m)
end

function kalman_smoother(m::LinearStateSpaceModel,θ,kf::KalmanFilter)
    T = length(kf.xp)

    xf = kf.xf
    xp = kf.xp
    Pf = kf.Pf
    Pp = kf.Pp
    K  = kf.K

    F = state_transition_matrix(m,θ)
    H = observation_matrix(m,θ)

    x = fill(xf[end],T+1)
    P = fill(Pf[end],T+1)

    # Po[T] = P^T_{T,T-1} = (I-K[T])*F*P^{T-1}_{T-1}
    Po = fill((I-K[T]*H)*F*Pf[T],T)

    J = fill(Pf[end],T)
    S = fill(x[end]*x[end]' + P[end],T+1)

    S10 = zeros(size(Pf[end]))
    Syy = zeros(size(H*Pf[end]*H'))
    for t in T:-1:1

        if t == T
            J[t],x[t],P[t],_,S[t],_,syy = smoother_step(x[t+1],P[t+1],xf[t],Pf[t],xp[t],Pp[t],F,Pf[t],J[t],Po[t],H,kf.z[t])
            S10 += x[t+1]*x[t]' + Po[t]
            Syy += syy
        else
            J[t],x[t],P[t],Po[t],S[t],s10,syy = smoother_step(x[t+1],P[t+1],xf[t],Pf[t],xp[t],Pp[t],F,Pf[t+1],J[t+1],Po[t+1],H,kf.z[t])
            S10 += s10
            Syy += syy
        end

        #=
        # J[t] = J_{t-1}
        # x[t] = x^n_{t-1}
        # P[t] = P^n_{t-1}
        # xf[t] = x^{t-1}_{t-1}
        # Pf[t] = P^{t-1}_{t-1}
        # xp[t] = x^{t-1}_t
        # Pp[t] = P^{t-1}_t

        # J_{t-1} = P^{t-1}_{t-1}*F'*(P^{t-1}_t)^(-1)
        J[t] = Pf[t]*F'/Pp[t]

        # x^n_{t-1} = x^{t-1}_{t-1} + J_{t-1}*(x^n_t - x^{t-1}_t)
        x[t] = xf[t] + J[t]*(x[t+1]-xp[t])

        # P^n_{t-1} = P^{t-1}_{t-1} + J_{t-1}*(P^n_t - P^{t-1}_t)*J_{t-1}
        P[t] = Pf[t] + J[t]*(P[t+1]-Pp[t])*J[t]'

        # Po[t] = P^n_{t,t-1}
        # = P^{t}_{t} + J_{t}*(P^n_{t+1,t}-F*P^{t}_{t})*J_{t-1}
        if t == T
        else
                Po[t] = Pf[t+1]*J[t]' + J[t+1]*(Po[t+1]-F*Pf[t+1])*J[t]'
        end
        S[t] = x[t]*x[t]' + P[t]
        S10 += x[t+1]*x[t]' + Po[t]
        Syy += (kf.z[t].-H*x[t+1])*(kf.z[t].-H*x[t+1])' .+ H*P[t+1]*H'
        =#
    end

    S11 = sum(S[2:end])
    S00 = sum(S[1:end-1])

    KalmanSmoother(x,P,Po,J,S11,S00,S10,Syy,kf.z)
end

function kalman_smoother(kf::KalmanFilter,m::TimeVariantLinearStateSpaceModel)
    T = length(kf.xp)

    xf = kf.xf
    xp = kf.xp
    Pf = kf.Pf
    Pp = kf.Pp
    K  = kf.K

    F = m.F
    H = m.H

    x = fill(xf[end],T+1)
    P = fill(Pf[end],T+1)

    # Po[T] = P^T_{T,T-1} = (I-K[T])*F*P^{T-1}_{T-1}
    Po = fill((I-K[T]*H[end])*F[end]*Pf[T],T)

    J = fill(Pf[end],T)
    S = fill(x[end]*x[end]' + P[end],T+1)

    S10 = zeros(size(Pf[end]))
    Syy = zeros(size(H[end]*Pf[end]*H[end]'))
    for t in T:-1:1

        if t == T
            J[t],x[t],P[t],_,S[t],_,syy = smoother_step(x[t+1],P[t+1],xf[t],Pf[t],xp[t],Pp[t],F[t],Pf[t],J[t],Po[t],H[t],kf.z[t])
            S10 += x[t+1]*x[t]' + Po[t]
            Syy += syy
        else
            J[t],x[t],P[t],Po[t],S[t],s10,syy = smoother_step(x[t+1],P[t+1],xf[t],Pf[t],xp[t],Pp[t],F[t],Pf[t+1],J[t+1],Po[t+1],H[t],kf.z[t])
            S10 += s10
            Syy += syy
        end

    end

    S11 = sum(S[2:end])
    S00 = sum(S[1:end-1])

    KalmanSmoother(x,P,Po,J,S11,S00,S10,Syy,kf.z)
end

function smoother_step(x0,P0,xf,Pf,xp,Pp,F,Pf2,J2,Po2,H,z)
    # J[t] = J_{t-1}
    # x[t] = x^n_{t-1}
    # P[t] = P^n_{t-1}
    # xf[t] = x^{t-1}_{t-1}
    # Pf[t] = P^{t-1}_{t-1}
    # xp[t] = x^{t-1}_t
    # Pp[t] = P^{t-1}_t

    # J_{t-1} = P^{t-1}_{t-1}*F'*(P^{t-1}_t)^(-1)
    J = Pf*F'/Pp

    # x^n_{t-1} = x^{t-1}_{t-1} + J_{t-1}*(x^n_t - x^{t-1}_t)
    x = xf + J*(x0-xp)

    # P^n_{t-1} = P^{t-1}_{t-1} + J_{t-1}*(P^n_t - P^{t-1}_t)*J_{t-1}
    P = Pf + J*(P0-Pp)*J'

    # Po[t] = P^n_{t,t-1}
    # = P^{t}_{t} + J_{t}*(P^n_{t+1,t}-F*P^{t}_{t})*J_{t-1}
    Po = Pf2*J' + J2*(Po2-F*Pf2)*J'
    S = x*x' + P
    S10 = x0*x' + Po
    Syy = (z.-H*x0)*(z.-H*x0)' .+ H*P0*H'
    J,x,P,Po,S,S10,Syy
end


module GeneralizedGauss

using BasisFunctions, LinearAlgebra
import BasisFunctions: moment

export compute_moments,
    compute_gauss_rule, compute_gauss_rules,
    oscillatory_basis_exp, oscillatory_basis_cos, log_basis,
    jacobi_matrix, newton

import Base:
    eltype,
    length,
    size

include("rootfinding.jl")

include("sets.jl")

include("quadrule.jl")

include("representations.jl")

include("gengauss.jl")

end # module


using LinearMaps
"""
A(σ₀)δψ=-A(δσ)ψ₀
Therefore,
perturbed data: δψ=-A⁻¹(σ₀)A(δσ)ψ₀
"""
function bornmod!(pa::ParamExpt, δσ, σ0)
	updateA!(pa.paσ, δσ)
	updateA!(pa.paσ0, σ0)
	# pa.paσ.A[end,:] .= 0.0 # is it necessary? A=A(σ₀)+A(δσ), except for the last row
	qrA0=factorize(pa.paσ0.A)
	@showprogress 5 "time loop ψ\t" for it in 1:length(pa.tgrid)
		snap_in=view(pa.LP,:,:,it)
		# generate ψ0
		forwψ!(pa.ψ0,snap_in,pa.paσ0,qrA0)

		# compute A(δσ)*ψ0, store this in adjψ
		applyA!(pa.adjψ, pa.ψ0, pa.paσ; A=pa.paσ.A)

		# compute A(σ0)⁻¹ * adjψ
		# do we need to mute boundary source?! --- otherwise it seems to bug!
		applyinvA!(pa.ψ, pa.adjψ, pa.paσ0; A=qrA0, mute_boundary_source=false)

		# record
		dat_slice=view(pa.data,it,:)
		mul!(dat_slice,pa.ACQ,pa.ψ) # record
		rmul!(dat_slice, -1.0)
	end
	return nothing
end

# just want to extract the data out into δd
function Fborn_map!(δd, δσ, pa, σ0)
	bornmod!(pa, δσ, σ0)
	copyto!(δd, pa.data)
end

function Fadj_map!(δσ, δd, pa, σ0)
	updateA!(pa.paσ0, σ0)
	qrAt0=factorize(pa.paσ0.At)
	qrA0=factorize(pa.paσ0.A)
	fill!(pa.g, 0.0)
	@showprogress 10 "time loop ψ\t" for it in 1:length(pa.tgrid)
		snap_in=view(pa.LP,:,:,it)
		# generate ψ0
		forwψ!(pa.ψ0,snap_in,pa.paσ0,qrA0)

		# prepare the adjoint source
		for ir in 1:size(pa.ACQ,1)
			pa.data_misfit[ir]=δd[it+(ir-1)*length(pa.tgrid)]
		end
		# migrate the data!; using this routine will also test its consistency
		adjψ_core!(pa.adjψ, pa.adjψ2, pa.ψ0,
		      pa.adjsrc, pa.g, pa.gtemp,
		      pa.ACQ, pa.data_misfit, pa.paσ0, qrAt0)

	end
	copyto!(δσ,pa.g)
	return nothing
end

"""
Return a `LinearMap` object to perform linearized modeling and its transpose.

# Arguments
* `pa::PoissonExpt`
* `σ0::Array`
"""
function operator_Born(pa::ParamExpt, σ0)
	@assert length(σ0)==prod(length.(pa.mgrid))
	fw=(y,x)->Fborn_map!(y, x, pa, σ0)
	bk=(y,x)->Fadj_map!(y, x, pa, σ0)

	return LinearMap(fw, bk,
		  length(pa.data),  # length of output
		  length(pa.σ), # length of input
		  ismutating=true)
end


@testset "Root counts" begin

    @testset "Benchmark systems" begin
        kat10 = solve(equations(katsura(10)); show_progress = false)
        @test nfinite(kat10) == 1024
        @test nsingular(kat10) == 0
        @test nreal(kat10) == 216

        cyclic5 = solve(equations(cyclic(5)); show_progress = false)
        @test nfinite(cyclic5) == 70
        @test nreal(cyclic5) == 10
        @test nsingular(cyclic5) == 0

        cyclic7 = solve(equations(cyclic(7)); show_progress = false)
        @test nfinite(cyclic7) == 924
        @test nreal(cyclic7) == 56
        @test nsingular(cyclic7) == 0
        @test ntracked(cyclic7) == 5040
    end

    @testset "Multi-homogeneous" begin
        ## initialize the variables
        @polyvar z[1:6, 1:3]
        F = let
            p = [1, 1, 0]
            α = [
                0.9606655201575953,
                -0.12634585123792536,
                -0.821994270849398,
                -0.4612529209681106,
                0.1914597581898848,
            ]
            a = [
                -0.6508165773155992,
                -0.6252070694344862,
                1.2753203087262615,
                -0.30371050526688304,
                0.45398822665915634,
                -0.4042256983795828,
                -1.1485430048515646,
                -0.22145594990414993,
                0.9590486265536351,
            ]
            ## define the system of polynomials
            f = [z[i, :] ⋅ z[i, :] for i = 2:5]
            g = [z[i, :] ⋅ z[i+1, :] for i = 1:5]
            h = sum(a[i] .* (z[i, :] × z[i+1, :]) for i = 1:3) +
                sum(a[i+4] .* z[i, :] for i = 2:5)
            F′ = [f .- 1; g .- cos.(α); h .- p]
            ## assign values to z₁ and z₆
            [subs(f, z[1, :] => [1, 0, 0], z[6, :] => [1, 0, 0]) for f in F′]
        end
        group1 = [[z[2, :]; z[4, :]], [z[3, :]; z[5, :]]]
        @test nnonsingular(solve(
            F;
            system = FPSystem,
            seed = 141232,
            show_progress = false,
        )) == 16
        @test nnonsingular(solve(
            F;
            variable_groups = group1,
            system = FPSystem,
            seed = 12312,
            show_progress = false,
        )) == 16
    end

    @testset "Bio-chemical reaction networks" begin
        @testset "bacillus_subtilis" begin
            f = equations(PolynomialTestSystems.bacillus_subtilis())
            res = solve(f; show_progress = false, seed = 173221)
            @test nsolutions(res) == 44
            @test nsingular(res) == 0
            res = solve(f; seed = 231231, start_system = :polyhedral, show_progress = false)
            @test nsolutions(res) == 44
        end

        # These test got prepared by Torkel Loman to test the BioDiffEq integration
        @polyvar x[1:3] p[1:10]
        results1_direct = fill(0, 12)
        results1_template = fill(0, 12)
        results2_direct = fill(0, 12)
        results2_template = fill(0, 12)
        results3_direct = fill(0, 12)
        results3_template = fill(0, 12)
        results4_direct = fill(0, 12)
        results4_template = fill(0, 12)

        for i = 1:100
            pol = [
                -x[1] * x[3] * p[3] - x[1] * p[2] + p[1],
                x[1] * x[2] * x[3] * p[8] * p[9] + x[1] * x[3] * p[7] * p[8] * p[9] -
                x[2] * x[3] * p[5] * p[6] - x[2] * p[5] * p[6] * p[10] -
                x[2] * x[3] * p[4] - x[2] * p[4] * p[10],
                -x[1] * x[2] * x[3] * p[8] * p[9] - x[1] * x[3] * p[7] * p[8] * p[9] +
                x[2] * x[3] * p[5] * p[6] + x[2] * p[5] * p[6] * p[10] +
                x[2] * x[3] * p[4] + x[2] * p[4] * p[10],
                x[2] + x[3] - 1.0,
            ]
            p_vals = [0.04, 0.04, 1.0, 1.0, 10.0, 0.0, 0.04, 35.0, 0.1, 0.04]
            pol_pars = DynamicPolynomials.subs.(pol, Ref(p => p_vals))
            sol = solutions(HomotopyContinuation.solve(pol_pars, show_progress = false))
            results1_direct[length(sol)+1] += 1

            p_template = randn(ComplexF64, 10)
            f_template = DynamicPolynomials.subs.(pol, Ref(p => p_template))
            result_template = solutions(HomotopyContinuation.solve(
                f_template,
                show_progress = false,
            ))
            (length(result_template) > 0) && (sol_again = solutions(HomotopyContinuation.solve(
                pol,
                result_template,
                parameters = p,
                p₁ = p_template,
                p₀ = ComplexF64.(p_vals),
                show_progress = false,
            )))
            (length(result_template) > 0) ? (results1_template[length(sol_again)+1] += 1) :
            (results1_template[1] += 1)

            pol2 = [
                -x[1]^5 * x[2] * p[5] + x[1]^4 * x[3] * p[6] * p[8] -
                x[1] * x[2] * p[3]^4 * p[5] + x[3] * p[3]^4 * p[6] * p[8] - x[1]^5 * p[7] +
                x[1]^4 * x[3] * p[4] - x[1] * p[3]^4 * p[7] +
                x[3] * p[3]^4 * p[4] + x[1]^4 * p[1] + x[1]^4 * p[2] + p[1] * p[3]^4,
                -x[1]^5 * x[2] * p[5] - x[1] * x[2] * p[3]^4 * p[5] - x[1]^4 * x[2] * p[7] +
                x[1]^4 * x[3] * p[4] - x[2] * p[3]^4 * p[7] +
                x[3] * p[3]^4 * p[4] + x[1]^4 * p[1] + x[1]^4 * p[2] + p[1] * p[3]^4,
                x[1] * x[2] * p[5] - x[3] * p[6] * p[8] - x[3] * p[4] - x[3] * p[7],
            ]
            p2_vals = [0.005, 0.1, 2.8, 10, 100, 0.1, 0.01, 0.0]
            pol2_pars = DynamicPolynomials.subs.(pol2, Ref(p => p2_vals))
            sol2 = solutions(HomotopyContinuation.solve(pol2_pars, show_progress = false))
            results2_direct[length(sol2)+1] += 1

            p2_template = randn(ComplexF64, 8)
            f2_template = DynamicPolynomials.subs.(pol2, Ref(p => p2_template))
            solve_templ2 = HomotopyContinuation.solve(f2_template, show_progress = false)
            result2_template = solutions(solve_templ2)
            sol2_again = solutions(HomotopyContinuation.solve(
                pol2,
                result2_template,
                parameters = p[1:8],
                p₁ = p2_template,
                p₀ = ComplexF64.(p2_vals),
                precision_strategy = :adaptive_always,
                max_steps = 10_000,
                show_progress = false,
            ))
            results2_template[length(sol2_again)+1] += 1

            pol3 = pol2
            p3_vals = [0.005, 0.1, 2.8, 10, 100, 0.1, 0.01, 1.0] #Last parameter is 1 and not 0. Here there should be 3 real solutions instead of 1.
            pol3_pars = DynamicPolynomials.subs.(pol3, Ref(p => p3_vals))
            sol3 = solutions(HomotopyContinuation.solve(pol3_pars, show_progress = false))
            results3_direct[length(sol3)+1] += 1

            p3_template = randn(ComplexF64, 8)
            f3_template = DynamicPolynomials.subs.(pol3, Ref(p => p3_template))
            result3_template = solutions(HomotopyContinuation.solve(
                f3_template,
                show_progress = false,
            ))
            sol3_again = solutions(HomotopyContinuation.solve(
                pol3,
                result3_template,
                parameters = p[1:8],
                p₁ = p3_template,
                p₀ = ComplexF64.(p3_vals),
                precision_strategy = :adaptive_always,
                max_steps = 10_000,
                show_progress = false,
            ))
            results3_template[length(sol3_again)+1] += 1

            pol4 = [-x[1]^3 * p[3] - x[1] * p[2]^2 * p[3] + x[1]^2 * p[1]]
            p4_vals = [1.0, 0.2, 1.0]
            pol4_pars = DynamicPolynomials.subs.(pol4, Ref(p => p4_vals))
            sol4 = solutions(HomotopyContinuation.solve(pol4_pars, show_progress = false))
            results4_direct[length(sol4)+1] += 1

            p4_template = randn(ComplexF64, 3)
            f4_template = DynamicPolynomials.subs.(pol4, Ref(p => p4_template))
            result4_template = solutions(HomotopyContinuation.solve(
                f4_template,
                show_progress = false,
            ))
            sol4_again = solutions(HomotopyContinuation.solve(
                pol4,
                result4_template,
                parameters = p[1:3],
                p₁ = p4_template,
                p₀ = ComplexF64.(p4_vals),
                precision_strategy = :adaptive_always,
                max_steps = 10_000,
                show_progress = false,
            ))
            results4_template[length(sol4_again)+1] += 1
        end

        @test 97 ≤ results1_template[4] ≤ 100
        @test 97 ≤ results1_direct[4] ≤ 100
        @test 97 ≤ results2_direct[7] ≤ 100
        @test 97 ≤ results2_template[7] ≤ 100
        @test 97 ≤ results3_direct[7] ≤ 100
        @test 97 ≤ results3_template[7] ≤ 100
        @test 97 ≤ results4_direct[4] ≤ 100
        @test 97 ≤ results4_template[4] ≤ 100
    end
end


# Pseudosampling experiment
#
# Compares the Stein discrepancy convergence behavior of herding and
# quasi Monte Carlo pseudosampling with that of independent random sampling.
#
# Before running this script, run src/experiments/pseudosample/pseudosample.m from the
# repository base directory to generate pseudosamples.

include("experiment_utils.jl")

using MAT
using SteinDistributions: SteinUniform, gradlogdensity, supportlowerbound, supportupperbound
using SteinDiscrepancy: gsd

# Load generated pseudosamples from experiment results directory
experdir = joinpath("src", "experiments", "pseudosample", "results", "pseudosample")
samplefile = joinpath(experdir,"samples.mat")
samples = matread(samplefile)

# Select an optimization problem solver
solver = "clp"
#solver = "gurobi"
# Specify number of samples and dimension of sampled vectors
n = length(samples["samples_herding"])
# dimension of x
d = 1
# distribution sampled from
distname = "uniform"
# Independent Uniform([0.0,1.0]) with best known Stein constants
(c1,c2,c3) = (0.5,0.5,1.0)
target = SteinUniform(d)
# define gradlogp
function gradlogp(x::Array{Float64,1})
    gradlogdensity(target, x)
end
# get the support
lowerbounds = [supportlowerbound(target, 1)]
upperbounds = [supportupperbound(target, 1)]
# Sample sizes at which optimization problem will be solved
ns = 1:200
# Solve optimization problem for each sampler at each sample size
samplers = ["herding","sobol","independent"]

# Collect all objective values for interactive plotting
for sampler in samplers, i = ns
    @printf("Beginning optimization for %s, n=%d, d=%d\n", sampler, i, d)
    # Create program with data subset
    # each column represents a separate trial
    simulateddata = samples["samples_$sampler"]
    numtrials = size(simulateddata, 2)

    for trial in 1:numtrials
        # Compute Stein discrepancy for first i points in this trial
        res = gsd(points=simulateddata[1:i,trial]'',
                  gradlogdensity=gradlogp,
                  c1=c1,
                  c2=c2,
                  c3=c3,
                  supportlowerbounds=lowerbounds,
                  supportupperbounds=upperbounds,
                  solver=solver)
        println("\tn = $(i), objective = $(res.objectivevalue)")

        # Package and save results
        instance_data = Dict{Any, Any}(
            "sampler" => sampler,
            "distname" => distname,
            "n" => i,
            "trial" => trial,
            "d" => d,
            "seed" => samples["seed"],
            "X" => res.points,
            "q" => res.weights,
            "g" => res.g,
            "gprime" => res.gradg,
            "objectivevalue" => res.objectivevalue,
            "edgetime" => res.edgetime,
            "solvetime" => res.solvetime
        )

        save_json(
            instance_data;
            dir="convergence_rates-pseudosamplers-uniform",
            distname=distname,
            sampler=sampler,
            n=i,
            trial=trial,
            d=d,
            seed=samples["seed"]
        )
    end
end

@printf("COMPLETE!")


"A lazy Jacobian `J` stores a map `m` and returns `J(x) = jacobian(m, x)`."
struct LazyJacobian{T,M} <: SimpleLazyMap{T}
	map	::	M
end

LazyJacobian(m) = LazyJacobian{domaintype(m),typeof(m)}(m)
applymap(m::LazyJacobian, x) = jacobian(supermap(m), x)

mapsize(m::LazyJacobian) = mapsize(supermap(m))

Display.displaystencil(m::LazyJacobian) = ["LazyJacobian(", supermap(m), ")"]
show(io::IO, mime::MIME"text/plain", m::LazyJacobian) = composite_show(io, mime, m)

"""
    jacobian(m::AbstractMap[, x])

Return the jacobian map. The two-argument version evaluates the jacobian
at a point `x`.
"""
jacobian(m::AbstractMap) = LazyJacobian(m)
jacobian!(y, m::AbstractMap, x) = y .= jacobian(m, x)

"A `DeterminantMap` returns the determinant of the result of a given map."
struct DeterminantMap{T,M} <: SimpleLazyMap{T}
    map ::  M
end

DeterminantMap(m) = DeterminantMap{domaintype(m)}(m)
DeterminantMap{T}(m) where {T} = DeterminantMap{T,typeof(m)}(m)
DeterminantMap{T}(m::Map{T}) where {T} = DeterminantMap{T,typeof(m)}(m)
DeterminantMap{T}(m::Map{S}) where {S,T} = DeterminantMap{T}(convert(Map{T}, m))

determinantmap(m::AbstractMap) = DeterminantMap(m)

applymap(m::DeterminantMap, x) = det(supermap(m)(x))

"""
    jacdet(m::AbstractMap[, x])

Return the determinant of the jacobian as a map. The two-argument version
evaluates the jacobian determinant at a point `x`.
"""
jacdet(m::AbstractMap) = determinantmap(jacobian(m))
jacdet(m::AbstractMap, x) = det(jacobian(m, x))

"An `AbsMap` returns the absolute value of the result of a given map."
struct AbsMap{T,M} <: SimpleLazyMap{T}
    map ::  M
end

AbsMap(m) = AbsMap{domaintype(m)}(m)
AbsMap{T}(m) where {T} = AbsMap{T,typeof(m)}(m)
AbsMap{T}(m::Map{T}) where {T} = AbsMap{T,typeof(m)}(m)
AbsMap{T}(m::Map{S}) where {S,T} = AbsMap{T}(convert(Map{T}, m))

absmap(m::AbstractMap) = AbsMap(m)

applymap(m::AbsMap, x) = abs(supermap(m)(x))

"A lazy volume element evaluates to `diffvolume(m, x)` on the fly."
struct LazyDiffVolume{T,M} <: SimpleLazyMap{T}
	map	::	M
end

LazyDiffVolume(m) = LazyDiffVolume{domaintype(m)}(m)
LazyDiffVolume{T}(m) where {T} = LazyDiffVolume{T,typeof(m)}(m)
LazyDiffVolume{T}(m::Map{T}) where {T} = LazyDiffVolume{T,typeof(m)}(m)
LazyDiffVolume{T}(m::Map{S}) where {S,T} = LazyDiffVolume{T}(convert(Map{T}, m))

applymap(m::LazyDiffVolume, x) = diffvolume(supermap(m), x)

Display.displaystencil(m::LazyDiffVolume) = ["LazyDiffVolume(", supermap(m), ")"]
show(io::IO, mime::MIME"text/plain", m::LazyDiffVolume) = composite_show(io, mime, m)

"""
    diffvolume(m[, x])

Compute the differential volume (at a point `x`). If `J` is the Jacobian matrix,
possibly rectangular, then the differential volume is `sqrt(det(J'*J))`.

If the map is square, then the differential volume is the absolute value of the
Jacobian determinant.
"""
function diffvolume(m)
	if issquaremap(m)
		if mapsize(m,1) > 1
			absmap(jacdet(m))
		else
			jacdet(m)
		end
	else
		LazyDiffVolume(m)
	end
end

function diffvolume(m, x)
	if issquaremap(m)
		if mapsize(m,1) > 1
			abs(jacdet(m, x))
		else
			jacdet(m, x)
		end
	else
		jac = jacobian(m, x)
		sqrt(det(adjoint(jac)*jac))
	end
end

const NumberLike = Union{Number,UniformScaling}

"""
    to_matrix(::Type{T}, A[, b])

Convert the `A` in the affine map `A*x` or `A*x+b` with domaintype `T` to a matrix.
"""
to_matrix(::Type{T}, A) where {T} = A
to_matrix(::Type{T}, A::AbstractMatrix) where {T} = A
to_matrix(::Type{T}, A::NumberLike) where {T<:Number} = A
to_matrix(::Type{SVector{N,T}}, A::Number) where {N,T} = A * one(SMatrix{N,N,T})
to_matrix(::Type{SVector{N,T}}, A::UniformScaling) where {N,T} = A.λ * one(SMatrix{N,N,T})
to_matrix(::Type{T}, A::Number) where {T<:AbstractVector} = A * I
to_matrix(::Type{T}, A::UniformScaling) where {T<:Number} = one(T)
to_matrix(::Type{T}, A::UniformScaling) where {T<:AbstractVector} = A

to_matrix(::Type{T}, A, b) where {T} = A
to_matrix(::Type{T}, A::AbstractMatrix, b) where {T} = A
to_matrix(::Type{T}, A::Number, b::Number) where {T<:Number} = A
to_matrix(::Type{T}, A::UniformScaling, b::Number) where {T<:Number} = A.λ
to_matrix(::Type{SVector{N,T}}, A::NumberLike, b::SVector{N,T}) where {N,T} = A * one(SMatrix{N,N,T})
to_matrix(::Type{T}, A::NumberLike, b::AbstractVector) where {S,T<:AbstractVector{S}} =
    A * Array{S,2}(I, length(b), length(b))

"""
    to_vector(::Type{T}, A[, b])

Convert the `b` in the affine map `A*x` or `A*x+b` with domaintype `T` to a vector.
"""
to_vector(::Type{T}, A) where {T} = zero(T)
to_vector(::Type{T}, A::SVector{M,S}) where {T,M,S} = zero(SVector{M,S})
to_vector(::Type{T}, A::SMatrix{M,N,S}) where {T<:AbstractVector,M,N,S} = zero(SVector{M,S})
to_vector(::Type{T}, A::AbstractArray) where {T<:AbstractVector} = zeros(eltype(T),size(A,1))
to_vector(::Type{T}, A, b) where {T} = b

"Return a zero matrix of the same size as the map."
zeromatrix(m::AbstractMap) = zeromatrix(m, domaintype(m), codomaintype(m))
zeromatrix(m, ::Type{T}, ::Type{U}) where {T,U} = zeros(numtype(T),mapsize(m))
zeromatrix(m, ::Type{T}, ::Type{U}) where {T<:Number,U<:Number} = zero(promote_type(T,U))
zeromatrix(m, ::Type{T}, ::Type{U}) where {N,T<:StaticVector{N},U<:Number} =
	transpose(zero(SVector{N,eltype(T)}))
zeromatrix(m, ::Type{T}, ::Type{U}) where {N,M,T<:StaticVector{N},U<:StaticVector{M}} =
	zero(SMatrix{M,N,promote_type(eltype(T),eltype(U))})
zeromatrix(m, ::Type{T}, ::Type{U}) where {T<:Number,M,U<:StaticVector{M}} =
	zero(SVector{M,promote_type(T,eltype(U))})
zeromatrix(m, ::Type{T}, ::Type{U}) where {T<:AbstractVector,U<:Number} =
	transpose(zeros(promote_type(eltype(T),U), mapsize(m,1)))

"Return a zero vector of the same size as the codomain of the map."
zerovector(m::AbstractMap) = zerovector(m, codomaintype(m))
zerovector(m::AbstractMap, ::Type{U}) where {U} = zero(U)
zerovector(m::AbstractMap, ::Type{StaticVector{M,T}}) where {M,T} = zero(SVector{M,T})
# If the output type is a vector, the map itself should store the size information.
zerovector(m::AbstractMap, ::Type{<:AbstractVector{T}}) where {T} = zeros(T, mapsize(m,1))

"Return an identity matrix with the same size as the map."
identitymatrix(m::AbstractMap) = identitymatrix(m, codomaintype(m))
identitymatrix(m::AbstractMap, ::Type{T}) where {T} = one(T)
identitymatrix(m::AbstractMap, ::Type{<:StaticVector{N,T}}) where {N,T} = one(SMatrix{N,N,T})
identitymatrix(m::AbstractMap, ::Type{<:AbstractVector{T}}) where {T} = Diagonal{T}(ones(mapsize(m,1)))


using Lorenz96
using SoftPosit
using PyPlot

Xp = L96(Posit16,F=8.0,N=1000,n=256,Δt=0.05)
Xf = L96(Float64,F=8.0,N=1000,n=256,Δt=0.05)

##
ioff()

cmin = -10
cmax = 10

fig,(ax1,ax2) = subplots(1,2,sharex=true,sharey=true,figsize=(8,3))

ax1.imshow(Xf,vmin=cmin,vmax=cmax,interpolation="bilinear",aspect="auto",origin="lower")
ax2.pcolormesh(Xp,vmin=cmin,vmax=cmax)

ax1.set_xticks([])
ax1.set_yticks([])

xlim(0,size(Xf)[2])
ylim(0,size(Xf)[1])

ax1.set_title("Float64",loc="left")
ax2.set_title("Posit16",loc="left")

ax1.set_ylabel(L"$X_i$")
ax1.set_xlabel("time")
ax2.set_xlabel("time")

tight_layout()
savefig("figs/hovmoeller.png",dpi=200)


using Luxor, Test, Random
Random.seed!(42)

function point_on_polygon_test(fname)

    Drawing(1200, 1200, fname)
    origin()
    background("grey20")
    polys = [ngon(Point(x, y), rand(20:100), rand(3:8), rand() * 2π, vertices=true) for x in -500:200:500, y in -500:200:500]
    for pol in polys
        randomhue()
        poly(pol, :fill)

        pt1 = rand(1:length(pol))
        pt2 = mod1(pt1 + 1, length(pol))

        # all these should lie on their polygons
        pt = between(pol[pt1], pol[pt2], rand())
        flag = ispointonpoly(pt, pol)
        if flag
            sethue("black")
            circle(pt, 10, :fill)
        else
            sethue("red")
            circle(pt, 20, :fill)
        end

        # move the point, then it shouldn't
        pt = pt + (0.01, 0.01)
        flag = ispointonpoly(pt, pol)
        if flag
            sethue("red")
            circle(pt, 6, :fill)
        else
            sethue("green")
            circle(pt, 20, :stroke)
        end

    end

    @test finish() == true
    println("...finished test: output in $(fname)")
end

point_on_polygon_test("point-on-polygon.png")


@inline xorshift_rotl(x::UInt64, k::Int) = (x >>> (0x3f & -k)) | (x << (0x3f & k))
@inline xorshift_rotl(x::UInt32, k::Int) = (x >>> (0x1f & -k)) | (x << (0x1f & k))


function DECMesh(vertices, edges, triangles)
    tri = make_tri(vertices, edges, triangles)
    vint, dtri = make_dual(tri)
    vint, DECMesh(tri, dtri)
end

function make_tri(vertices, edges, triangles)
    vertices = convert(Vector{SVector{2,Float64}}, vertices)
    edges = convert(Vector{SVector{2,Int}}, edges)
    rectify_orientation!(triangles, vertices)
    triangles = convert(Vector{SVector{3,Int}}, triangles)
    return Triangulation(vertices, edges, triangles)
end

function make_dual(tri)
    vertices = tri.vertices
    edges = tri.edges
    triangles = tri.triangles

    edges_neighbors =
        [[i for i in 1:length(triangles) if edge ⊆ triangles[i]] for edge in edges]
    vertices_neighbors = [findall(x -> (i in x), edges) for i = 1:length(vertices)]

    edges_ext = findall(x -> length(x) == 1, edges_neighbors)
    edges_int = setdiff(1:length(edges), edges_ext)
    vertices_ext = findall(x -> !isempty(intersect(x, edges_ext)), vertices_neighbors)
    vertices_int = setdiff(1:length(vertices), vertices_ext)

    nvertices_ext = length(vertices_ext)

    mid_points = [mid_point(vertices[edges[edge]]...) for edge in edges_ext]
    nmid_points = length(mid_points)

    dtriangles = [circumcenter(vertices[triangle]...) for triangle in triangles]
    ndtriangles = length(dtriangles)

    append!(dtriangles, mid_points)
    append!(dtriangles, vertices[vertices_ext])

    dedges = fill([0, 0], length(edges))
    for i in edges_int
        d = edges_neighbors[i]
        rectify_orientation!(d, dtriangles[d], vertices[edges[i]])
        dedges[i] = d
    end
    j = 1
    for i in edges_ext
        d = [edges_neighbors[i][1], ndtriangles + j]
        rectify_orientation!(d, dtriangles[d], vertices[edges[i]])
        dedges[i] = d
        j += 1
    end

    dvertices = fill(Int[], length(vertices))
    for i in vertices_int
        dvertices[i] = catsort(dedges[vertices_neighbors[i]])
    end
    j = 1
    for i in vertices_ext
        dvertices[i] = catsort(dedges[vertices_neighbors[i]], ndtriangles + nmid_points + j)
        j += 1
    end
    rectify_orientation!(dvertices, dtriangles)

    return vertices_int,
    DualTriangulation(
        (ndtriangles, nmid_points, nvertices_ext),
        dtriangles,
        dedges,
        dvertices,
    )
end

function catsort(dedges, last = 0)
    vsorted = if last == 0
        fill(0, length(dedges))
    else
        fill(0, length(dedges) + 2)
    end

    uniques = reduce(union, dedges)
    graph = Dict(
        [i =>
            dedges |>
            y ->
                filter(x -> (i in x), y) |>
                x -> reduce(union, x) |> x -> setdiff(x, [i]) |> sort for i in uniques],
    )

    if last == 0
        vsorted[begin] = uniques[1]
    else
        vsorted[begin] = findfirst(v -> (length(v) == 1), graph)
        vsorted[end] = last
    end

    i = 2
    len = if last == 0
        length(uniques) - 1
    else
        length(uniques)
    end
    for _ = 1:len
        for n in graph[vsorted[i - 1]]
            if n ∉ vsorted
                vsorted[i] = n
                i += 1
                break
            end
        end
    end

    return vsorted
end


"""
    simulate(bg::Number, kappa::Float64, kern::Function, maxT::Number)

Simulate a Hawkes process between 0 and `maxT` with parameters `bg`, `kappa`, `kern`.

# Arguments

* `bg`: The background rate of the Hawkes process. Constant or positive function.
* `kappa`: The kappa parameter of the Hawkes process.
* `kern` : The kernel function of the Hawkes process.
* `maxT` : Maximum time that the Hawkes process will be simulated for.

# Notes

* kappa must be between 0 and 1 for a stable Hawkes process.

# Examples

``julia
kern_f(x) = pdf.(Distributions.Exponential(1/0.5), x)

simevents = simulate(0.5, 0.5, kern_f, 100)
``

"""
function simulate(bg, kappa::Float64, kern::Function, maxT::Number)

    childfunction(x) = kappa * kern(x)
    lambda_max = childfunction(0)
    childsimulation(x) = poisson_simulation(childfunction, maxT - x, lambda_max) .+ x

    backgroundEvents = poisson_simulation(bg, maxT)

    eventbus = backgroundEvents
    allevents = backgroundEvents

    while !isempty(eventbus)
        newevents = map(childsimulation, eventbus)
        childevents = vcat(filter(!isempty, newevents)...)
        append!(allevents, childevents)
        eventbus = childevents
    end
    sort(allevents)
end

function simulate(bg, kappa::Float64, kernD::Distributions.Distribution, maxT::Number)
    kern, kern_cdf = generate_kernel_functions(kernD)
    return simulate(bg, kappa, kern, maxT)
end

function poisson_simulation(rate::Number, maxT::Number)
    nevents = rand(Distributions.Poisson(rate*maxT))
    event_times = sort(rand(Distributions.Uniform(0, maxT), nevents))
    return event_times
end

function poisson_simulation(rate::Function, maxT::Number)
    lambda_max = 1.1*maximum(rate(range(0, maxT, length=1000)))
    poisson_simulation(rate, maxT, lambda_max)
end

function poisson_simulation(rate::Function, maxT::Number, lambda_max::Number)
    tstar = poisson_simulation(lambda_max, maxT)
    accept_prob = rate(tstar) / lambda_max
    tstar[rand(length(accept_prob)) .< accept_prob]
end


export
    AbstractTransform,
    transform,
    truncate_modes,
    inverse

"""
    AbstractTransform

## Interface

* `Base.ndims(<:AbstractTransform)`: N dims of modes
* `transform(<:AbstractTransform, 𝐱::AbstractArray)`: Apply the transform to 𝐱
* `truncate_modes(<:AbstractTransform, 𝐱_transformed::AbstractArray)`: Truncate modes that contribute to the noise
* `inverse(<:AbstractTransform, 𝐱_transformed::AbstractArray)`: Apply the inverse transform to 𝐱_transformed
"""
abstract type AbstractTransform end

include("fourier_transform.jl")


struct BipartiteMatching
    weight :: Int64
    match  :: Vector{Int64}
end

function bipartite_cardinality_matching(l_in::Vector{Int}, r_in::Vector{Int}, m, n; l_sorted=false)
    @assert length(l_in) == length(r_in)
    @assert m <= n
    l = l_in
    r = r_in
    if !l_sorted
        perm = sortperm(l_in)
        l = l_in[perm]
        r = r_in[perm]
    end

    len = length(l)
    matching_l = zeros(Int, m)
    matching_r = zeros(Int, n)

    # create initial matching
    match_len = 0
    for (li,ri) in zip(l,r)
        if matching_l[li] == 0 && matching_r[ri] == 0
            matching_l[li] = ri
            matching_r[ri] = li
            match_len += 1
        end
    end

    if match_len < m
        # creating indices be able to get edges a vertex is connected to
        # only works if l is sorted
        index_l = zeros(Int, m+1)
        last = l[1]
        c = 2
        for i = 2:len
            if l[i] != last
                index_l[last+1] = c
                last = l[i]
            end
            c += 1
        end
        index_l[l[end]+1] = c

        index_l[1] = 1

        process_nodes = zeros(Int, m+n)
        depths = zeros(Int, m+n)
        parents = zeros(Int, m+n)
        used_l = zeros(Bool, m)
        used_r = zeros(Bool, n)
        found = false

        # find augmenting path
        while match_len < m
            pend = 1
            pstart = 1
            for li in l
                # free vertex
                if matching_l[li] == 0
                    process_nodes[pstart] = li
                    depths[pstart] = 1
                    break
                end
            end

            begin
            while pstart <= pend
                node = process_nodes[pstart]
                depth = depths[pstart]

                # from left to right
                if depth % 2 == 1
                    used_l[node] = true
                    # only works if l is sorted
                    for ri=index_l[node]:index_l[node+1]-1
                        child_node = r[ri]
                        # don't use matching edge
                        if matching_r[child_node] != node && !used_r[child_node]
                            used_r[child_node] = true
                            pend += 1
                            depths[pend] = depth+1
                            process_nodes[pend] = child_node
                            parents[pend] = pstart
                        end
                    end
                else # right to left (only matching edge)
                    # if matching edge
                    match_to = matching_r[node]
                    if match_to != 0
                        if !used_l[match_to]
                            used_l[match_to] = true
                            pend += 1
                            depths[pend] = depth+1
                            process_nodes[pend] = match_to
                            parents[pend] = pstart
                        end
                    else
                        # found augmenting path
                        parent = pstart
                        last = 0
                        c = 0
                        while parent != 0
                            current = process_nodes[parent]
                            if last != 0
                                if c % 2 == 1
                                    matching_r[last] = current
                                    matching_l[current] = last
                                end
                            end
                            c += 1
                            last = current
                            parent = parents[parent]
                        end
                        # break because we found a path
                        found = true
                        break
                    end
                end
                pstart += 1
            end
            if found
                match_len += 1
                if match_len < m
                    used_l .= false
                    used_r .= false
                end
                found = false
            else
                break
            end
            end
        end
    end
    return BipartiteMatching(match_len, matching_l)
end


#Design example for a continuous-time lowpass ΔΣ ADC
using RSDeltaSigmaPort
using RSDeltaSigmaPort.EasyPlot #set, cons
import Printf: @sprintf
j=im

#==Baseband modulator (continuous-time implementation)
===============================================================================#
nlev=2
#dsm = RealDSM(order=3, OSR=100, M=nlev-1, f0=0, opt=0, Hinf=1.3, form=:FB)
#dsm = RealDSM(order=3, OSR=32, M=nlev-1, f0=0, opt=0, Hinf=1.5, form=:FB)
dsm = RealDSM(order=3, OSR=32, M=nlev-1, f0=0, opt=2, Hinf=1.5, form=:FB)
umax = 0.83

#Parameters for the continuous-time implementation:
tdac = [0 1] #DAC timing. [0 1] means zero-delay non-return-to-zero

println("\n*** $(dsm.order)th-Order Continuous-Time Lowpass Example")

@info("Performing NTF synthesis..."); flush(stdout); flush(stderr)
#-------------------------------------------------------------------------------
NTF0 = synthesizeNTF(dsm) #Optimized zero placement
#println("p:"); display(NTF0.p)
#println("z:"); display(NTF0.z)
plotcoll_NTF = documentNTF(dsm, NTF0)
println("\tdone.")
saveimage(:png, "dsexample2_NTF.png", plotcoll_NTF, AR=2/1, width=900)
displaygui(plotcoll_NTF)
displaygui(plotNTF(NTF0, dsm.OSR)) #ADDED MALaforge

@info("Performing time-domain simulations..."); flush(stdout); flush(stderr)
#-------------------------------------------------------------------------------
#Example spectrum
#ftest = 0.3 * 0.5/dsm.OSR/3 #~1/3 of the way across the passband
ftest = (0.5/dsm.OSR)/3 #~1/3 of the way across the passband
plot = plotExampleSpectrum(dsm, NTF0, ftest=ftest,N=2^12) #16
#title('Example Spectrum');
saveimage(:png, "dsexample2_PSD.png", plot, AR=2/1, width=900)
displaygui(plot)

#SQNR plot
snrinfo = calcSNRInfo(dsm, NTF=NTF0)
plot = plotSNR(snrinfo, dsm)
	set(plot, xyaxes=set(xmin=-100, ymin=0, ymax=100))
println("\tdone.")
saveimage(:png, "dsexample2_SQNR.png", plot, AR=2/1, width=900)
displaygui(plot)

@info("Mapping to continuous-time..."); flush(stdout); flush(stderr)
#-------------------------------------------------------------------------------
(ABCDc,tdac2) = realizeNTF_ct(NTF0, dsm.form, tdac)
(Ac, Bc, Cc, Dc) = partitionABCD(ABCDc)
sys_c = _ss(Ac,Bc,Cc,Dc)
println("\tdone.")
#Verify that the sampled pulse response of the CT loop filter
#matches the impulse response of the DT prototype
n_imp = 10
y = -impL1(NTF0, n_imp) #Negate impL1 to make the plot prettier
	y_max = maximum(real.(y))
plot = plotLollipop(0:n_imp, y, color=:blue)
	plot.title = "Loop filter pulse/impulse responses (negated)"
yl = floor(0.9*y_max); color=:blue
	ylw = waveform([0.5, 2], [yl, yl])
simglyph = cons(:a, glyph=set(shape=:o, size=1.5, color=color, fillcolor=color))
push!(plot,
	cons(:wfrm, ylw, simglyph, line=set(style=:solid, color=color, width=2), label=""),
	cons(:atext, "   discrete-time", x=2, y=yl, align=:bl),
)
dt = 1/16
t = collect(0:dt:n_imp)
yy = -pulse(sys_c, [0 0;tdac], dt, n_imp*1.0)
	yy = squeeze(yy)
	yyw = waveform(t, yy)
yl = floor(0.7*y_max); color=:green
	ylw = waveform([0.5, 2], [yl, yl])
push!(plot,
	#Add yy when it works
	cons(:wfrm, yyw, line=set(style=:solid, color=color, width=2), label=""),
	cons(:wfrm, ylw, line=set(style=:solid, color=color, width=2), label=""),
	cons(:atext, "   continuous-time", x=2, y=yl, align=:bl),
)
displaygui(plot)

#Map the cts system to its discrete time equivalent and check the NTF
(sys_d, Gp) = mapCtoD(sys_c, t=tdac)
ABCD = [sys_d.A sys_d.B; sys_d.C sys_d.D]
(NTF, G) = calculateTF(ABCD)
NTF = cancelPZ(NTF)
(plot_NTF, plot_fresp) = plotcoll_NTF.plotlist
plotPZ!(plot_NTF, NTF, color=:cyan)
#Also plot the STF
L0 = _zpk(sys_c[1,1])
@warn("FIXME: Again, numerical error in ss->zpk gives 0 gain")
f = range(0, stop=0.5, length=10)
G = evalTFP(L0, NTF, f)
Gw = waveform(f, dbv.(G))
push!(plot_fresp,
	cons(:wfrm, Gw, line=set(style=:solid, color=:magenta, width=2), label=""),
)
plotcoll_NTF.title = "NTF and STF"
displaygui(plotcoll_NTF)

@info("Performing dynamic range scaling..."); flush(stdout); flush(stderr)
#-------------------------------------------------------------------------------
#!!! This code assumes that the scale factors for the DT equivalent apply
#!!! to the CT system. A system with an RZ DAC will have inter-sample peaks
#!!! that exceed the values an the sampling instants.
(ABCDs, umax, S) = scaleABCD(ABCD, nlev=nlev, f=dsm.f0, xlim=1, umax=umax, N0=10^4)
S = S[1:dsm.order,1:dsm.order] #Don't worry about the extra states used in the d-t model
println("\tdone.")
println("\nScaled ABCD matrix (ABCDs):")
@show umax
display(ABCDs)

#Compute ABCDcs:
Sinv = inv(S)
Acs=S*Ac*Sinv; Bcs=S*Bc;  Ccs=Cc*Sinv
ABCDcs = [Acs Bcs; Ccs Dc]
sys_cs = _ss(Acs, Bcs, Ccs, Dc)
#ABCDcs needs to be checked with CT simulations to
# 1. Verify pulse response
# 2. Verify signal swings
println("\nABCDcs matrix:")
display(ABCDcs)

adc = Dict{Symbol, Any}(
	:order => dsm.order,
	:OSR => dsm.OSR,
	:opt => dsm.opt,
	:M => dsm.M,
	:f0 => dsm.f0,
	:NTF => NTF,
	:ABCD => ABCD,
	:umax => umax,
	:peak_snr => snrinfo.peak[2],
	:form => dsm.form,
	:ABCDc => ABCDc,
	:L0  => L0,
	:sys_c => sys_c,
	:ABCDcs => ABCDcs,
	:sys_cs => sys_cs,
)

println()
display(adc)

:END_OF_EXAMPLE


@testset "H" begin
    @testset "hx2dp" begin
        @test hx2dp("2A^3") == 672.0
        @test_throws SpiceError hx2dp("2A^3000")
        @test_throws SpiceError hx2dp("-2A^3000")
        @test_throws SpiceError hx2dp("foo")
    end
    @testset "hrmint" begin
        xvals = [-1.0, 0.0, 3.0, 5.0]
        yvals = [6.0, 3.0, 5.0, 0.0, 2210.0, 5115.0, 78180.0, 109395.0]
        answer, deriv = hrmint(xvals, yvals, 2)
        @test answer ≈ 141.0
        @test deriv  ≈ 456.0
        @test_throws ArgumentError hrmint(xvals, yvals[1:4], 2)
        @test_throws SpiceError hrmint([0.0, 0.0], yvals[1:4], 2)
    end
    @testset "halfpi" begin
        @test SPICE._halfpi() ≈ π/2
    end
end


# Assumes the space dimension and field dimension are the same.  In other words, when the
# space coordinate indices are (i,j,k), then the field has three vector components.
# Therefore, for the output field array G[i,j,k,w], we assume w = 1:3.

# The functions add the calculated values to the existing values of the output array.
# Therefore, if the derivative values themselves are desired, pass the output array
# initialized with zeros.

export apply_grad!

# Wrapper to apply the discrete gradient by default
#
# Note that when the positional arguments are SVector's, this function may call the concrete
# implementation directly instead of the next wrapper function taking ∆l⁻¹ even if the
# keyword arguments are not SVector's, because methods are dispatched based only on the
# types of positional arguments.  Therefore, we convert the keyword arguments into SVector's.
apply_grad!(G::AbsArrNumber,  # output field; in 3D, G[i,j,k,w] is w-component of G at (i,j,k)
            f::AbsArrNumber,  # input array of scalar; in 3D, f[i,j,k] is f at (i,j,k)
            ::Val{OP},  # Val(:(=)) or Val(:(+=)): set (=) or add (+=) operator to use
            isfwd::AbsVecBool,  # isfwd[w] = true|false: create ∂w by forward|backward difference
            ∆l⁻¹::Tuple{Vararg{Number}}=ntuple(x->1.0,length(isfwd)),  # ∆l⁻¹[w]: inverse of uniform distance between grid planes in w-direction
            isbloch::AbsVecBool=fill(true,length(isfwd)),  # boundary conditions in K dimensions
            e⁻ⁱᵏᴸ::AbsVecNumber=ones(length(isfwd));  # Bloch phase factors in K dimensions
            α::Number=1.0  # scale factor to multiply to result before adding it to g: g += α ∇⋅F
            ) where {OP} =
    (N = size(f); K = length(isfwd); apply_grad!(G, f, Val(OP), isfwd, fill.(∆l⁻¹,N), isbloch, e⁻ⁱᵏᴸ; α))

# Wrapper for converting AbstractVector's to SVec's
apply_grad!(G::AbsArrNumber,  # output field; in 3D, G[i,j,k,w] is w-component of G at (i,j,k)
            f::AbsArrNumber,  # input array of scalar; in 3D, f[i,j,k] is f at (i,j,k)
            ::Val{OP},  # Val(:(=)) or Val(:(+=)): set (=) or add (+=) operator to use
            isfwd::AbsVecBool,  # isfwd[w] = true|false: create ∂w by forward|backward difference
            ∆l⁻¹::NTuple{K,AbsVecNumber},  # ∆l⁻¹[w]: inverse of distances between grid planes in x-direction
            isbloch::AbsVecBool=fill(true,K),  # boundary conditions in K dimensions
            e⁻ⁱᵏᴸ::AbsVecNumber=ones(K);  # Bloch phase factors in K dimensions
            α::Number=1.0  # scale factor to multiply to result before adding it to g: g += α ∇⋅F
            ) where {K,OP} =
    apply_grad!(G, f, Val(OP), SBool{K}(isfwd), ∆l⁻¹, SBool{K}(isbloch), SVec{K}(e⁻ⁱᵏᴸ); α)

# Concrete implementation
function apply_grad!(G::AbsArrNumber{K₊₁},  # output field; in 3D, G[i,j,k,w] is w-component of G at (i,j,k)
                     f::AbsArrNumber{K},  # input array of scalar; in 3D, f[i,j,k] is f at (i,j,k)
                     ::Val{OP},  # Val(:(=)) or Val(:(+=)): set (=) or add (+=) operator to use
                     isfwd::SBool{K},  # isfwd[w] = true|false: create ∂w by forward|backward difference
                     ∆l⁻¹::NTuple{K,AbsVecNumber},  # ∆l[w]: inverse of distances between grid planes in x-direction
                     isbloch::SBool{K},  # boundary conditions in K dimensions
                     e⁻ⁱᵏᴸ::SNumber{K};  # Bloch phase factors in K dimensions
                     α::Number=1.0  # scale factor to multiply to result before adding it to g: g += α ∇⋅F
                     ) where {K,K₊₁,OP}
    @assert K₊₁==K+1

    for nw = 1:K  # direction of differentiation
        nv = nw  # component of output field generated by ∂w (index of matrix block)
        Gv = selectdim(G, K₊₁, nv)  # nv-th component of output field
        apply_∂!(Gv, f, Val(OP), nw, isfwd[nw], ∆l⁻¹[nw], isbloch[nw], e⁻ⁱᵏᴸ[nw]; α) # Gv += α ∂f/∂w
    end

    return nothing
end