function rebin2D(df::DataFrame, _prec::Float64 = 0.001, _dx = 0.001)
    df_rebinned = DataFrame(E1 = Float64[],
                            minE = Float64[], maxE = Float64[],
                            minG = Float64[], maxG = Float64[],
                            a = Float64[], b = Float64[])

    for e in unique(df.E1)
        df_rebinned = vcat(df_rebinned, rebin1D(df[df.E1 .== e, 2], df[df.E1 .== e, 3], e, _prec, _dx))
    end

    return df_rebinned
end

function rebin1D(_x::Vector{Float64}, _y::Vector{Float64}, _E1::Float64, _prec::Float64 = 0.001, _dx = 0.001)
    e0=1 #initialize index of the first point in fit
    ef=2 #initialize index of the second point in fit

    a,b = get_line_params(0.0, 0.0, _x[e0], _y[e0])                 # first approximation is made from origin to first point in spectrum

    df_rebinned = DataFrame(E1 = _E1,                               # initialized DataFrame with first approximation
                            minE = 0.0, maxE = _x[e0],
                            minG = 0.0, maxG = _y[e0],
                            a = a, b = b )

    if length(_x) == 1                                              # if there is only one point in the vector, first point is approximated from (0,0) to (x1,y1)
        a,b = get_line_params(_x[e0], _y[e0], _x[e0] + _dx, 0.0)    # second line is approximated from (x1,y1) to (x1+dx, 0) creating a triangle with area equal to
                                                                    # rectangle with sides dx and y1 (dx is the step in x-direction...we assume x-array is evenly spaced)
        push!(df_rebinned, [_E1,
                            _x[e0], _x[e0] + _dx,
                            _y[e0], 0.0     ,
                            a     , b       ] )
        return df_rebinned
    end

    a,b = get_line_params(_x[e0], _y[e0], _x[ef], _y[ef])           # first fit points 1,2
    Δy  = abs( (get_line_point(_x[ef], a, b) - _y[ef]) / _y[ef] )   # Δy of points 1,2

    for _ in 1:length(_x)

        while Δy <= _prec && ef < length(_x)
            Δy = abs((get_line_point(_x[ef], a, b) - _y[ef])/_y[ef])
            ef += 1
        end

        if ef != length(_x)
            a, b = get_line_params(_x[e0], _y[e0], _x[ef-1], _y[ef-1])

            push!(df_rebinned, [_E1,
                                _x[e0], _x[ef-1],                   # refit line with previous point
                                _y[e0], _y[ef-1],                   # unless it's last point of the series
                                a     , b       ] )
        else
            a, b = get_line_params(_x[e0], _y[e0], _x[ef], _y[ef])
            push!(df_rebinned, [_E1,
                                _x[e0], _x[ef],
                                _y[e0], _y[ef],                     # for last point line stops at the endpoint
                                a     , b       ] )
        end

        if ef >= length(_x)                                         # if the next point is more than the size of data, check ends
            break
        end

        e0  = ef -1                                                 # last point from fit becomes new point in next fit
        a,b = get_line_params(_x[e0], _y[e0], _x[ef], _y[ef])       # find new line from the next set of points
        Δy  = abs((get_line_point(_x[ef], a, b) - _y[ef])/_y[ef])   # find new Δy

    end

    a, b = get_line_params(_x[ef], _y[ef], _x[ef]+_dx, 0.0)       # finally to add very last approximation by going from (xf, yf) to (xf+dx, 0.0)
    push!(df_rebinned, [_E1,
                        _x[ef], _x[ef]+_dx,
                        _y[ef], 0.0         ,                       # for last point line stops at the endpoint
                        a     , b           ] )

    return df_rebinned
end

function normalize2D!(df_rebinned::DataFrame, thickness = 0.001) # CHANGE THIS, NO NEED TO PREALLOCATE df_normed. JUST CHANGE THE INPUT DF and RETURN IT!
    volume_rebinned = get_total_volume(df_rebinned, thickness)

    df_normed = select(df_rebinned,
                    :E1            => :E1,
                    :minE          => :minE,
                    :maxE          => :maxE,
                    :minG=> ByRow(g->g/volume_rebinned) => :minG,
                    :maxG=> ByRow(g->g/volume_rebinned) => :maxG,
                    :a   => ByRow(a->a/volume_rebinned) => :a,
                    :b   => ByRow(b->b/volume_rebinned) => :b,
                    )

    return df_normed  #return select(df_rebinned,
                    #  :E1            => :E1,
                    #  :minE          => :minE,
                    #  :maxE          => :maxE,
                    #  :minG=> ByRow(g->g/volume_rebinned) => :minG,
                    #  :maxG=> ByRow(g->g/volume_rebinned) => :maxG,
                    #  :a   => ByRow(a->a/volume_rebinned) => :a,
                    #  :b   => ByRow(b->b/volume_rebinned) => :b,
                    #  )
end

"""
#### function ```get_cdf(df::DataFrame, thickness::Real = 0.001)```
<br>

    Description of ```get_cdf```
    ------------------------------
Calculate CDF for pdf = a + bx. CDF is cummulative density function defined as integral from -inf to x of pdf dx.

"""
function get_cdf(df::DataFrame, thickness::Real = 0.001)
    n = nrow(df)
    cdf = Vector{Float64}(undef, n)
    cdf[1] = get_integral_linear(df[1,2], df[1,3], df[1,6], df[1,7])*thickness

    for i in 2:n
        cdf[i] = cdf[i-1] + get_integral_linear(df[i,2], df[i,3], df[i,6], df[i,7])*thickness
    end
    return cdf
end

function get_cdf!(df::DataFrame, thickness::Real = 0.001)
    n = nrow(df)
    cdf = Vector{Float64}(undef, n)
    cdf[1] = get_integral_linear(df[1,2], df[1,3], df[1,6], df[1,7])*thickness

    for i in 2:n
        cdf[i] = cdf[i-1] + get_integral_linear(df[i,2], df[i,3], df[i,6], df[i,7])*thickness
    end

    @transform! df :cdf = cdf
end


using Convex
using Mosek, MosekTools
using Optim

using Transducers
using BenchmarkTools

function f(x, u)
    0.5 * (-x'*x + sum(dot(^)(u,2)))
end

function convex_solver(x, u_guess, ulim)
    m = length(u_guess)
    u = Convex.Variable(m)
    problem = minimize(f(x, u))
    problem.constraints += [u >= ulim[1]]
    problem.constraints += [u <= ulim[2]]
    solve!(problem, Mosek.Optimizer(); silent_solver=true)
    u.value, problem.optval
end

function ipnewton_solver(x, u_guess, ulim)
    dfc = TwiceDifferentiableConstraints(ulim[1], ulim[2])
    obj(u) = f(x, u)
    res = Optim.optimize(obj, dfc, u_guess, IPNewton())
    res.minimizer, res.minimum
end

@testset "solver" begin
    for N in [1, 10, 100]
        @show n, m = N, N
        d = 100
        xs = 1:d |> Map(i -> rand(n)) |> collect
        us = 1:d |> Map(i -> rand(m)) |> collect
        ulim = (-2*ones(m), 2*ones(m))
        # case 1: convex solver
        println("convex solver")
        @btime convex_solver(rand($n), rand($m), $ulim)
        println("non-convex solver (ipnewton)")
        @btime ipnewton_solver(rand($n), rand($m), $ulim)
    end
    nothing
end


abstract type AbstractImplicitSurface end

mutable struct ImplicitResult{T<:CSGBuilding.AbstractImplicitSurface, F<:Real}
    surface::T
    value::F
    signint::Int
end

value(result::ImplicitResult) = result.value

function normal(result::ImplicitResult, coords)
    return result.signint * normal(result.surface, coords)
end

Base.show(io::IO, result::ImplicitResult) =
    print(io, "ImplicitResult: $(result.surface), $(result.value)")

Base.min(x::ImplicitResult, y::ImplicitResult) = ifelse(isless(x.value, y.value), x, y)

Base.max(x::ImplicitResult, y::ImplicitResult) = ifelse(isless(y.value, x.value), x, y)

## sphere

struct ImplicitSphere{T<:Real} <: AbstractImplicitSurface
    center::SVector{3,T}
    radius::T
end

function ImplicitSphere(center::AbstractArray, radius)
    T = promote_type(eltype(center), typeof(radius))
    nc = convert(SVector{3, T}, center)
    return ImplicitSphere(nc, convert(T, radius))
end

Base.show(io::IO, surface::ImplicitSphere) = print(io, "ImplSphere: R$(surface.radius)")
_name(surface::ImplicitSphere) = "Sphere"

function evaluate(surface::ImplicitSphere, coords)
    val = norm(coords-surface.center) - surface.radius
    return ImplicitResult(surface, val, 1)
end

function normal(surface::ImplicitSphere, coords)
    dist = coords-surface.center
    if ! isapprox(norm(dist), 0)
        return normalize(dist)
    else
        return convert(typeof(coords), [0,0,0])
    end
end

## plane

struct ImplicitPlane{T<:Real} <: AbstractImplicitSurface
    point::SVector{3,T}
    normal::SVector{3,T}
end

function ImplicitPlane(point::AbstractArray, normal::AbstractArray)
    T = promote_type(eltype(point), eltype(normal))
    np = convert(SVector{3, T}, point)
    nn = convert(SVector{3, T}, normal)
    return ImplicitPlane(np, nn)
end

Base.show(io::IO, surface::ImplicitPlane) = print(io, "ImplPlane: n$(surface.normal)")
_name(surface::ImplicitPlane) = "Plane"

function evaluate(surface::ImplicitPlane, coords)
    val = dot(coords-surface.point, surface.normal)
    return ImplicitResult(surface, val, 1)
end

normal(surface::ImplicitPlane, coords) = surface.normal

"""
    issame(x::AbstractImplicitSurface, y::AbstractImplicitSurface, params)

For any non-`ImplicitPlane` return false.
"""
issame(x::AbstractImplicitSurface, y::AbstractImplicitSurface, params) = false

"""
    issame(pl1::ImplicitPlane, pl2::ImplicitPlane, params)

Are the two planes the same? Parameters `α_conv, ϵ_conv`.
"""
function issame(pl1::ImplicitPlane, pl2::ImplicitPlane, params)
    @unpack α_conv, ϵ_conv = params
    abs(dot(pl1.normal, pl2.normal)) < α_conv && return false
    d = pl1.point-pl2.point
    abs(dot(d, pl1.normal)) > ϵ_conv && return false
    return true
end

## cylinder

struct ImplicitCylinder{T<:Real} <: AbstractImplicitSurface
    axis::SVector{3,T}
    center::SVector{3,T}
    radius::T
end

function ImplicitCylinder(axis::AbstractArray, center::AbstractArray, radius)
    T = promote_type(eltype(axis), eltype(center), typeof(radius))
    na = convert(SVector{3, T}, axis)
    nc = convert(SVector{3, T}, center)
    return ImplicitCylinder(na, nc, convert(T, radius))
end

"""
    vectorfromline(p, d, q)

Compute the vector directing from a line to `q` point.
The line is defined by one point `p` and it's direction vector `d`.
"""
vectorfromline(p, d, q) = q-p-d*dot(q-p, d)

function evaluate(surface::ImplicitCylinder, coords)
    c = surface.center
    a = surface.axis
    val = norm(vectorfromline(c, a, coords)) - surface.radius
    return ImplicitResult(surface, val, 1)
end

function normal(surface::ImplicitCylinder, coords)
    c = surface.center
    a = surface.axis
    tnorm = vectorfromline(c, a, coords)
    if ! isapprox(norm(tnorm), 0)
        return normalize(tnorm)
    else
        return convert(typeof(coords), [0,0,0])
    end
end

function Base.show(io::IO, surface::ImplicitCylinder)
    return print(io, "ImplCylinder: R$(surface.radius)")
end
_name(surface::ImplicitCylinder) = "Cylinder"

## cone

struct ImplicitCone{T<:Real} <: AbstractImplicitSurface
    apex::SVector{3,T}
    axis::SVector{3,T}
    opang::T
end

function ImplicitCone(apex::AbstractArray, axis::AbstractArray, opang)
    T = promote_type(eltype(apex), eltype(axis), typeof(opang))
    nap = convert(SVector{3, T}, apex)
    nax = convert(SVector{3, T}, axis)
    return ImplicitCone(nap, nax, convert(T, radius))
end

function evaluate(surface::ImplicitCone, coords)
    d, _ = project2cone(surface, coords)
    return ImplicitResult(surface, d, 1)
end

function normal(surface::ImplicitCone, coords)
    _, n = project2cone(surface, coords)
    return n
end

function Base.show(io::IO, surface::ImplicitCone)
    return print(io, "ImplCone: op:$(rad2deg(surface.opang)°)")
end
_name(surface::ImplicitCone) = "Cone"

## translational

struct ImplicitTranslational <: AbstractImplicitSurface
    coordframe
    contour
    # center of gravity
    center
    # normal of the contour is parallel to the direction
    # towards the center of the contour?
    # == should flip the computed normals to direct outwards?
    # this is used in e.g. CSGBuilding
    # true means, that the computed normals must be turned to direct outside
    outwards::Int
    # should the computed normal be flipped to match the measured points
    # this is used in this package to ensure that in/outwards is correct
    # true means that computed normals must be turned to match the measured points
    flipnormal::Int
end

function evaluate(surface::ImplicitTranslational, coords)
    proj = project2sketchplane([coords], surface.coordframe)
    d, _, _ = dn2shape_outw(proj[1], surface)
    return ImplicitResult(surface, d, 1)
end

function normal(surface::ImplicitTranslational, coords)
    proj = project2sketchplane([coords], surface.coordframe)
    _, n, _ = dn2shape_outw(proj[1], surface)
    return n
end

function Base.show(io::IO, surface::ImplicitTranslational)
    return print(io, "ImplTransl")
end
_name(surface::ImplicitTranslational) = "Transl"


#############################################################################
# prox_poly.jl
# compute the prox of indicator functions of associated polyhedra
#############################################################################

export prox

function prox(p::AssocPoly, w::AbstractArray, Tol::Float64 = 1e-3; solver::String = "fujishige_wolfe")
  if solver == "card_inc_fix"
    return  card_inc_fix(p.F, w)
  elseif solver == "fujishige_wolfe"
    return fujishige_wolfe(p, w, Tol)
  elseif solver == "frank_wolfe_away"
    x = Variable(p.F.setvariables[1].cardinality)
    g = norm(x)
    prob_perm = SCOPEminimize(g, x in p)
    return frank_wolfe_away(prob_perm, verbose = false)
  elseif solver == "cutting_plane"
    x = Variable(p.F.setvariables[1].cardinality)
    q = Problem(:minimize, 0.5*norm(x)^2)
    if isa(p, BasePoly)
      f = lovasz(p.F, x)
    elseif isa(p, SymPoly)
      f = lovasz(p.F, abs(x))
    else
      error("The cutting plane method is not applicable to $(typeof(p)).")
    end
    primal_solution = cutting_plane(q, f)
    return -primal_solution                           # the solution we get here is the negative
  else
    error("This method is not supported.")
  end
end

prox(C::SetConstraint{AssocPoly}, w::AbstractArray, Tol::Float64 = 1e-3) = prox(p, w, Tol)


@testset "Convolution methods" begin
    function conv_test(psf, img, img_out, dims, s)
        otf = fft(psf, dims)
        otf_r = rfft(psf, dims)
        otf_p, conv_p = plan_conv_r(psf, dims)
        @testset "$s" begin
            @test img_out ≈ conv_psf(img, psf, dims)
            @test img_out ≈ conv_otf(img, otf, dims)
            @test img_out ≈ conv_otf_r(img, otf_r, dims)
            @test img_out ≈ conv_p(img, otf_p)
        end
    end

    N = 5
    psf = zeros((N, N))
    psf[1, 1] = 1
    img = randn((N, N))
    conv_test(psf, img, img, [1,2], "Convolution random image with delta peak")

    N = 5
    psf = abs.(randn((N, N, 2)))
    img = randn((N, N, 2))
    dims = [1, 2]
    img_out = conv_psf(img, psf, dims)
    conv_test(psf, img, img_out, dims, "Convolution with random 3D PSF and random 3D image over 2D dimensions")

    N = 5
    psf = abs.(randn((N, N, N, N, N)))
    img = randn((N, N, N, N, N))
    dims = [1, 2, 3, 4]
    img_out = conv_psf(img, psf, dims)
    conv_test(psf, img, img_out, dims, "Convolution with random 5D PSF and random 5D image over 4 Dimensions")

    N = 5
    psf = abs.(zeros((N, N, N, N, N)))
    for i = 1:N
        psf[1,1,1,1, i] = 1
    end
    img = randn((N, N, N, N, N))
    dims = [1, 2, 3, 4]
    img_out = conv_psf(img, psf, dims)
    conv_test(psf, img, img, dims, "Convolution with 5D delta peak and random 5D image over 4 Dimensions")

    function conv_test_complex(psf, img, img_out, dims, s)
        otf = fft(psf, dims)
        @testset "$s" begin
            @test img_out ≈ conv(img, psf, dims, real_res=false)
            @test img_out ≈ conv_v_ft(img, otf, dims, real_res=false)
        end
    end

    N = 5
    psf = abs.(zeros((N, N, N, N, N)))
    for i = 1:N
        psf[1,1,1,1, i] = 1
    end
    img = randn((N, N, N, N, N))
    img = img .+ exp.(1im .* img)
    dims = [1, 2, 3, 4]
    img_out = conv(img, psf, dims, real_res=false)
    conv_test_complex(psf, img, img, dims, "Complex Convolution with 5D delta peak and random 5D image over 4 Dimensions")

    N = 5
    psf = zeros((N, N))
    psf[1, 1] = 1
    img = randn((N, N))
    img = img .+ exp.(1im .* img)
    conv_test_complex(psf, img, img, [1,2], "Complex Convolution random image with delta peak")

    N = 5
    psf = abs.(randn((N, N, 2)))
    img = randn((N, N, 2))
    img = img .+ exp.(1im .* img)
    dims = [1, 2]
    img_out = conv(img, psf, dims, real_res=false)
    conv_test_complex(psf, img, img_out, dims, "Complex Convolution with random 3D PSF and random 3D image over 2D dimensions")

    N = 5
    psf = abs.(randn((N, N, N, N, N)))
    img = randn((N, N, N, N, N))
    img = img .+ exp.(1im .* img)
    dims = [1, 2, 3, 4]
    img_out = conv(img, psf, dims, real_res=false)
    conv_test_complex(psf, img, img_out, dims, "Complex Convolution with random 5D PSF and random 5D image over 4 Dimensions")

    @testset "dims argument nothing" begin
        N = 5
        psf = abs.(randn((N, N, N, N, N)))
        img = randn((N, N, N, N, N))
        dims = [1,2,3,4,5]
        @test conv(psf, img) ≈ conv(psf, img, dims)
        @test conv_v_ft(img, fft(psf, dims)) ≈ conv_v_ft(img, fft(psf, dims), dims)
    end

    @testset "Test shift argument" begin
        N = 10
        img = randn((N, N))
        psf = abs.(randn((N, N)))

        @test conv(img, psf, shift=true) ≈ conv(img, ifftshift(psf))
        @test conv_psf(img, psf, shift=true) ≈ conv(img, ifftshift(psf))
        @test conv_psf(img, psf, shift=true) ≈ conv(img, ifftshift(psf))
    end

    @testset "Check types" begin
        N = 10
        img = randn((N, N))
        psf = abs.(randn((N, N)))

        @test conv_psf(img, psf, shift=true) ≈ conv(img, ifftshift(psf))
        @test typeof(conv_psf(img, psf, shift=true)) != typeof(conv(img, ifftshift(psf)))
        @test typeof(conv_psf(img, psf, shift=true)) == typeof(conv(img, ifftshift(psf), real_res=true))
    end
end


#=
Part of this work is derived from astropy/photutils. The relevant derivations
are considered under a BSD 3-clause license. =#

"""
    EllipticalAperture(x, y, a, b, θ)
    EllipticalAperture(position, a, b, θ)

An elliptical aperture with semi-major axis `a`, semi-minor axis `b`, and position angle `θ`. `a` and `b` must be ≥ 0, `θ` is measured in degrees counter-clockwise the standard x-axis.

# Examples
```jldoctest
julia> ap = EllipticalAperture(0, 0, 4, 2, 35)
EllipticalAperture(0, 0, a=4, b=2, θ=35°)
```
"""
struct EllipticalAperture{T <: Number} <: AbstractAperture
    x::T
    y::T
    a::T
    b::T
    theta::T

    function EllipticalAperture(x::T, y::T, a::T, b::T, theta::T) where T <: Number
        a < 0 && error("Invalid axis a=$a. a must be greater than or equal to 0")
        b < 0 && error("Invalid axis b=$b. b must be greater than or equal to 0")
        new{T}(x, y, a, b, mod(theta, 360))
    end
end

EllipticalAperture(x, y, a, b, theta) = EllipticalAperture(promote(x, y, a, b, theta)...)
EllipticalAperture(center::AbstractVector, a, b, theta) = EllipticalAperture(center..., a, b, theta)

function Base.show(io::IO, e::EllipticalAperture)
    print(io, "EllipticalAperture($(e.x), $(e.y), a=$(e.a), b=$(e.b), θ=$(e.theta)°)")
end

function oblique_coefficients(a, b, theta)
    sintheta, costheta = sincos(deg2rad(theta))
    a2 = a^2
    b2 = b^2
    cxx = costheta^2 / a2 + sintheta^2 / b2
    cyy = sintheta^2 / a2 + costheta^2 / b2
    cxy = 2 * costheta * sintheta * (1 / a2 - 1 / b2)
    return cxx, cyy, cxy
end

function bbox(e::EllipticalAperture)

    sintheta, costheta = sincos(deg2rad(e.theta))

    t = atan((-e.b * tand(e.theta)) / e.a)

    sint, cost = sincos(t)
    xmin = e.x + e.a * cost * costheta - e.b * sint * sintheta
    xmax = xmin

    for n in -2:2
        sint, cost = sincos(t + n * pi)
        xmin = min(xmin, e.x + e.a * cost * costheta - e.b * sint * sintheta)
    end

    for n in -2:2
        sint, cost = sincos(t + n * pi)
        xmax = max(xmax, e.x + e.a * cost * costheta - e.b * sint * sintheta)
    end

    t = atan((e.b * cotd(e.theta)) / e.a)

    sint, cost = sincos(t)
    ymin = e.y + e.b * sint * costheta + e.a * cost * sintheta
    ymax = ymin

    for n in -2:2
        sint, cost = sincos(t + n * pi)
        ymin = min(ymin, e.y + e.b * sint * costheta + e.a * cost * sintheta)
    end

    for n in -2:2
        sint, cost = sincos(t + n * pi)
        ymax = max(ymax, e.y + e.b * sint * costheta + e.a * cost * sintheta)
    end

    xmin = floor(Int, xmin)
    xmax = ceil(Int, xmax)
    ymin = floor(Int, ymin)
    ymax = ceil(Int, ymax)

    return xmin, xmax, ymin, ymax
end

function mask(e::EllipticalAperture; method = :center)
    bounds = edges(e)
    ny, nx = size(e)
    return elliptical_overlap(bounds..., nx, ny, e.a, e.b, e.theta, method = method)
end

#######################################################

"""
    EllipticalAnnulus(x, y, a_in, a_out, b_out, θ)
    EllipticalAnnulus(position, a_in, a_out, b_out, θ)
An elliptical annulus with inner semi-major axis `a_in`, outer semi-major axis `a_out`, outer semi-minor axis `b_out`, and position angle `θ`.
`a_out` ≥ `a_in` ≥ 0 and `b_out` must be ≥ 0, `θ` is measured in degrees counter-clockwise the standard x-axis.

`b_in` will automatically be calculated from `(a_in / a_out) * b_out`. Note this may cause a type instability.
# Examples
```jldoctest
julia> ap = EllipticalAnnulus(0, 0, 4, 10, 5, 45)
EllipticalAnnulus(0.0, 0.0, a_in=4.0, a_out=10.0, b_in=2.0, b_out=5.0, θ=45.0°)
```
"""
struct EllipticalAnnulus{T <: Number} <: AbstractAperture
    x::T
    y::T
    a_in::T
    b_in::T
    a_out::T
    b_out::T
    theta::T

    function EllipticalAnnulus(x::T, y::T, a_in::T, b_in::T, a_out::T, b_out::T, theta::T) where T <: Number
        0 ≤ a_in ≤ a_out || error("Invalid axis a_in=$a_in or a_out=$a_out. a_out must be greater than a_in which must be greater than or equal to 0")
        0 ≤ b_in ≤ b_out || error("Invalid axis b_in=$b_in or b_out=$b_out. b_out must be greater than b_in which must be greater than or equal to 0")
        new{T}(x, y, a_in, b_in, a_out, b_out, mod(theta, 360))
    end
end

EllipticalAnnulus(x, y, a_in, a_out, b_out, theta) = EllipticalAnnulus(promote(x, y, a_in, a_in / a_out * b_out, a_out, b_out, theta)...)
EllipticalAnnulus(center::AbstractVector, a_in, a_out, b_out, theta) = EllipticalAnnulus(center..., a_in, a_out, b_out, theta)

function Base.show(io::IO, e::EllipticalAnnulus)
    print(io, "EllipticalAnnulus($(e.x), $(e.y), a_in=$(e.a_in), a_out=$(e.a_out), b_in=$(e.b_in), b_out=$(e.b_out), θ=$(e.theta)°)")
end

function bbox(e::EllipticalAnnulus)

    sintheta, costheta = sincos(deg2rad(e.theta))

    t = atan((-e.b_out * tand(e.theta)) / e.a_out)

    sint, cost = sincos(t)
    xmin = e.x + e.a_out * cost * costheta - e.b_out * sint * sintheta
    xmax = xmin

    for n in -2:2
        sint, cost = sincos(t + n * pi)
        xmin = min(xmin, e.x + e.a_out * cost * costheta - e.b_out * sint * sintheta)
    end

    for n in -2:2
        sint, cost = sincos(t + n * pi)
        xmax = max(xmax, e.x + e.a_out * cost * costheta - e.b_out * sint * sintheta)
    end

    t = atan((e.b_out * cotd(e.theta)) / e.a_out)

    sint, cost = sincos(t)
    ymin = e.y + e.b_out * sint * costheta + e.a_out * cost * sintheta
    ymax = ymin

    for n in -2:2
        sint, cost = sincos(t + n * pi)
        ymin = min(ymin, e.y + e.b_out * sint * costheta + e.a_out * cost * sintheta)
    end

    for n in -2:2
        sint, cost = sincos(t + n * pi)
        ymax = max(ymax, e.y + e.b_out * sint * costheta + e.a_out * cost * sintheta)
    end

    xmin = floor(Int, xmin)
    xmax = ceil(Int, xmax)
    ymin = floor(Int, ymin)
    ymax = ceil(Int, ymax)

    return xmin, xmax, ymin, ymax
end

function mask(e::EllipticalAnnulus; method = :exact)
    bounds = edges(e)
    ny, nx = size(e)
    out = elliptical_overlap(bounds..., nx, ny, e.a_out, e.b_out, e.theta, method = method)
    out .-= elliptical_overlap(bounds..., nx, ny, e.a_in, e.b_in, e.theta, method = method)

    return out
end


using Test
using SumOfSquares
using DynamicPolynomials

# Inspired from https://github.com/jump-dev/SumOfSquares.jl/issues/79
function concave_then_convex_cubic_test(optimizer, config::MOIT.TestConfig,
                                        MCT::Type{<:MOI.AbstractVectorSet})
    atol = config.atol
    rtol = config.rtol

    model = _model(optimizer)

    @polyvar x
    @variable(model, p, Poly(monomials(x, 0:3)))
    cone = ConvexPolyInnerCone{MCT}()
    cref_convex  = @constraint(model,  p in cone, domain = (@set x >= 0))
    cref_concave = @constraint(model, -p in cone, domain = (@set x <= 0))
    @constraint(model, p(x =>  2) ==  8)
    @constraint(model, p(x => -2) == -8)
    @constraint(model, p(x =>  1) ==  1)
    @constraint(model, p(x => -1) == -1)

    optimize!(model)

    @test termination_status(model) == MOI.OPTIMAL

    @test primal_status(model) == MOI.FEASIBLE_POINT
    @test value(p) ≈ x^3 atol=atol rtol=rtol
    @test_throws SumOfSquares.ValueNotSupported value(cref_convex)
    @test_throws SumOfSquares.ValueNotSupported value(cref_concave)

    # The monomials contain the variables created for the Hessian so we cannot
    # check them easily
    for μ in [dual(cref_convex), dual(cref_concave)]
        @test μ isa AbstractMeasure{Float64}
        @test length(moments(μ)) == 2
    end
    for μ in [moments(cref_convex), moments(cref_concave)]
        @test μ isa AbstractMeasure{Float64}
        @test length(moments(μ)) == 7
    end
end

function sos_concave_then_convex_cubic_test(optimizer, config)
    concave_then_convex_cubic_test(optimizer, config,
                                   MOI.PositiveSemidefiniteConeTriangle)
end
sd_tests["sos_concave_then_convex_cubic"] = sos_concave_then_convex_cubic_test
function sdsos_concave_then_convex_cubic_test(optimizer, config)
    concave_then_convex_cubic_test(optimizer, config,
                                   SumOfSquares.ScaledDiagonallyDominantConeTriangle)
end
soc_tests["sdsos_concave_then_convex_cubic"] = sdsos_concave_then_convex_cubic_test
function dsos_concave_then_convex_cubic_test(optimizer, config)
    concave_then_convex_cubic_test(optimizer, config,
                                   SumOfSquares.DiagonallyDominantConeTriangle)
end
linear_tests["dsos_concave_then_convex_cubic"] = dsos_concave_then_convex_cubic_test


#src # Adding a new equation: nonconservative linear advection
using Test: @test #src

# If you want to use Trixi for your own research, you might be interested in
# a new physics model that is not present in Trixi.jl. In this tutorial,
# we will implement the nonconservative linear advection equation in a periodic domain
# ```math
# \left\{
# \begin{aligned}&\partial_t u(t,x) + a(x) \partial_x u(t,x) = 0 \\
# &u(0,x)=\sin(x) \\
# &u(t,-\pi)=u(t,\pi)
# \end{}
# \right.
# ```
# where $a(x) = 2 + \cos(x)$. The analytic solution is
# ```math
# u(t,x)=-\sin \left(2 \tan ^{-1}\left(\sqrt{3} \tan \left(\frac{\sqrt{3} t}{2}-\tan ^{-1}\left(\frac{1}{\sqrt{3}}\tan \left(\frac{x}{2}\right)\right)\right)\right)\right)
# ```
# In Trixi.jl, such a mathematical model
# is encoded as a subtype of [`Trixi.AbstractEquations`](@ref).

# ## Basic setup

# Since there is no native support for variable coefficients, we need to transform the PDE to the following system:
#```math
# \left\{
# \begin{aligned}&\partial_t \begin{pmatrix}u(t,x)\\a(t,x) \end{pmatrix} +\begin{pmatrix} a(t,x) \partial_x u(t,x) \\ 0 \end{pmatrix} = 0 \\
# &u(0,x)=\sin(x) \\
# &a(0,x)=2+\cos(x) \\
# &u(t,-\pi)=u(t,\pi)
# \end{}
# \right.
#```

## Define new physics
using Trixi
using Trixi: AbstractEquations, get_node_vars
import Trixi: varnames, default_analysis_integrals, flux, max_abs_speed_naive,
              have_nonconservative_terms

## Since there is no native support for variable coefficients, we use two
## variables: one for the basic unknown `u` and another one for the coefficient `a`
struct NonconservativeLinearAdvectionEquation <: AbstractEquations{1 #= spatial dimension =#,
                                                                   2 #= two variables (u,a) =#}
end

varnames(::typeof(cons2cons), ::NonconservativeLinearAdvectionEquation) = ("scalar", "advection_velocity")

default_analysis_integrals(::NonconservativeLinearAdvectionEquation) = ()

## The conservative part of the flux is zero
flux(u, orientation, equation::NonconservativeLinearAdvectionEquation) = zero(u)

## Calculate maximum wave speed for local Lax-Friedrichs-type dissipation
function max_abs_speed_naive(u_ll, u_rr, orientation::Integer, ::NonconservativeLinearAdvectionEquation)
    _, advection_velocity_ll = u_ll
    _, advection_velocity_rr = u_rr

    return max(abs(advection_velocity_ll), abs(advection_velocity_rr))
end

## We use nonconservative terms
have_nonconservative_terms(::NonconservativeLinearAdvectionEquation) = Val(true)

## This "nonconservative numerical flux" implements the nonconservative terms.
## In general, nonconservative terms can be written in the form
##   g(u) ∂ₓ h(u)
## Thus, a discrete difference approximation of this nonconservative term needs
## - `u mine`:  the value of `u` at the current position (for g(u))
## - `u_other`: the values of `u` in a neighborhood of the current position (for ∂ₓ h(u))
function flux_nonconservative(u_mine, u_other, orientation,
                              equations::NonconservativeLinearAdvectionEquation)
    _, advection_velocity = u_mine
    scalar, _             = u_other

    return SVector(advection_velocity * scalar, zero(scalar))
end

# The implementation of nonconservative terms uses a single "nonconservative flux"
# function `flux_nonconservative`. It will basically be applied in a loop of the
# form
# ```julia
# du_m(D, u) = sum(D[m, l] * flux_nonconservative(u[m], u[l], 1, equations)) # orientation 1: x
# ```
# where `D` is the derivative matrix and `u` contains the nodal solution values.

# Now, we can run a simple simulation using a DGSEM discretization.

## Create a simulation setup
using Trixi
using OrdinaryDiffEq

equation = NonconservativeLinearAdvectionEquation()

## You can derive the exact solution for this setup using the method of
## characteristics
function initial_condition_sine(x, t, equation::NonconservativeLinearAdvectionEquation)
    x0 = -2 * atan(sqrt(3) * tan(sqrt(3) / 2 * t - atan(tan(x[1] / 2) / sqrt(3))))
    scalar = sin(x0)
    advection_velocity = 2 + cos(x[1])
    SVector(scalar, advection_velocity)
end

## Create a uniform mesh in 1D in the interval [-π, π] with periodic boundaries
mesh = TreeMesh(-Float64(π), Float64(π), # min/max coordinates
                initial_refinement_level=4, n_cells_max=10^4)

## Create a DGSEM solver with polynomials of degree `polydeg`
## Remember to pass a tuple of the form `(conservative_flux, nonconservative_flux)`
## as `surface_flux` and `volume_flux` when working with nonconservative terms
volume_flux  = (flux_central, flux_nonconservative)
surface_flux = (flux_lax_friedrichs, flux_nonconservative)
solver = DGSEM(polydeg=3, surface_flux=surface_flux,
               volume_integral=VolumeIntegralFluxDifferencing(volume_flux))

## Setup the spatial semidiscretization containing all ingredients
semi = SemidiscretizationHyperbolic(mesh, equation, initial_condition_sine, solver)

## Create an ODE problem with given time span
tspan = (0.0, 1.0)
ode = semidiscretize(semi, tspan)

## Set up some standard callbacks summarizing the simulation setup and computing
## errors of the numerical solution
summary_callback = SummaryCallback()
analysis_callback = AnalysisCallback(semi, interval=50)
callbacks = CallbackSet(summary_callback, analysis_callback)

## OrdinaryDiffEq's `solve` method evolves the solution in time and executes
## the passed callbacks
sol = solve(ode, Tsit5(), abstol=1.0e-6, reltol=1.0e-6,
            save_everystep=false, callback=callbacks)

## Print the timer summary
summary_callback()

## Plot the numerical solution at the final time
using Plots: plot
plot(sol)

# You see a plot of the final solution.

# We can check whether everything fits together by refining the grid and comparing
# the numerical errors. First, we look at the error using the grid resolution
# above.

error_1 = analysis_callback(sol).l2 |> first
@test isapprox(error_1, 0.0002961027497) #src
# Next, we increase the grid resolution by one refinement level and run the
# simulation again.

mesh = TreeMesh(-Float64(π), Float64(π), # min/max coordinates
                initial_refinement_level=5, n_cells_max=10^4)

semi = SemidiscretizationHyperbolic(mesh, equation, initial_condition_sine, solver)

tspan = (0.0, 1.0)
ode = semidiscretize(semi, tspan);

summary_callback = SummaryCallback()
analysis_callback = AnalysisCallback(semi, interval=50)
callbacks = CallbackSet(summary_callback, analysis_callback);

sol = solve(ode, Tsit5(), abstol=1.0e-6, reltol=1.0e-6,
            save_everystep=false, callback=callbacks);
summary_callback()

#nb #-
error_2 = analysis_callback(sol).l2 |> first
@test isapprox(error_2, 1.860295931682964e-5, rtol=0.05) #src
#-
error_1 / error_2
@test isapprox(error_1 / error_2, 15.916970234784808, rtol=0.05) #src
# As expected, the new error is roughly reduced by a factor of 16, corresponding
# to an experimental order of convergence of 4 (for polynomials of degree 3).

# ## Summary of the code

# Here is the complete code that we used (without the callbacks since these
# create a lot of unnecessary output in the doctests of this tutorial).
# In addition, we create the `struct` inside the new module `NonconservativeLinearAdvection`.
# That ensures that we can re-create `struct`s defined therein without having to
# restart Julia.

# Define new physics
module NonconservativeLinearAdvection

using Trixi
using Trixi: AbstractEquations, get_node_vars
import Trixi: varnames, default_analysis_integrals, flux, max_abs_speed_naive,
              have_nonconservative_terms

## Since there is not yet native support for variable coefficients, we use two
## variables: one for the basic unknown `u` and another one for the coefficient `a`
struct NonconservativeLinearAdvectionEquation <: AbstractEquations{1 #= spatial dimension =#,
                                                                   2 #= two variables (u,a) =#}
end

varnames(::typeof(cons2cons), ::NonconservativeLinearAdvectionEquation) = ("scalar", "advection_velocity")

default_analysis_integrals(::NonconservativeLinearAdvectionEquation) = ()

## The conservative part of the flux is zero
flux(u, orientation, equation::NonconservativeLinearAdvectionEquation) = zero(u)

## Calculate maximum wave speed for local Lax-Friedrichs-type dissipation
function max_abs_speed_naive(u_ll, u_rr, orientation::Integer, ::NonconservativeLinearAdvectionEquation)
    _, advection_velocity_ll = u_ll
    _, advection_velocity_rr = u_rr

    return max(abs(advection_velocity_ll), abs(advection_velocity_rr))
end

## We use nonconservative terms
have_nonconservative_terms(::NonconservativeLinearAdvectionEquation) = Val(true)

## This "nonconservative numerical flux" implements the nonconservative terms.
## In general, nonconservative terms can be written in the form
##   g(u) ∂ₓ h(u)
## Thus, a discrete difference approximation of this nonconservative term needs
## - `u mine`:  the value of `u` at the current position (for g(u))
## - `u_other`: the values of `u` in a neighborhood of the current position (for ∂ₓ h(u))
function flux_nonconservative(u_mine, u_other, orientation,
                              equations::NonconservativeLinearAdvectionEquation)
    _, advection_velocity = u_mine
    scalar, _            = u_other

    return SVector(advection_velocity * scalar, zero(scalar))
end

end # module

## Create a simulation setup
import .NonconservativeLinearAdvection
using Trixi
using OrdinaryDiffEq

equation = NonconservativeLinearAdvection.NonconservativeLinearAdvectionEquation()

## You can derive the exact solution for this setup using the method of
## characteristics
function initial_condition_sine(x, t, equation::NonconservativeLinearAdvection.NonconservativeLinearAdvectionEquation)
    x0 = -2 * atan(sqrt(3) * tan(sqrt(3) / 2 * t - atan(tan(x[1] / 2) / sqrt(3))))
    scalar = sin(x0)
    advection_velocity = 2 + cos(x[1])
    SVector(scalar, advection_velocity)
end

## Create a uniform mesh in 1D in the interval [-π, π] with periodic boundaries
mesh = TreeMesh(-Float64(π), Float64(π), # min/max coordinates
                initial_refinement_level=4, n_cells_max=10^4)

## Create a DGSEM solver with polynomials of degree `polydeg`
## Remember to pass a tuple of the form `(conservative_flux, nonconservative_flux)`
## as `surface_flux` and `volume_flux` when working with nonconservative terms
volume_flux  = (flux_central, NonconservativeLinearAdvection.flux_nonconservative)
surface_flux = (flux_lax_friedrichs, NonconservativeLinearAdvection.flux_nonconservative)
solver = DGSEM(polydeg=3, surface_flux=surface_flux,
               volume_integral=VolumeIntegralFluxDifferencing(volume_flux))

## Setup the spatial semidiscretization containing all ingredients
semi = SemidiscretizationHyperbolic(mesh, equation, initial_condition_sine, solver)

## Create an ODE problem with given time span
tspan = (0.0, 1.0)
ode = semidiscretize(semi, tspan);

## Set up some standard callbacks summarizing the simulation setup and computing
## errors of the numerical solution
summary_callback = SummaryCallback()
analysis_callback = AnalysisCallback(semi, interval=50)
callbacks = CallbackSet(summary_callback, analysis_callback);

## OrdinaryDiffEq's `solve` method evolves the solution in time and executes
## the passed callbacks
sol = solve(ode, Tsit5(), abstol=1.0e-6, reltol=1.0e-6,
            save_everystep=false);

## Plot the numerical solution at the final time
using Plots: plot
plot(sol);


using .TaylorSeries, FFTW
import FFTW.fft!;

function get_diff(x::AbstractArray,n::Integer)
	return TaylorSeries.getcoeff.(differentiate.(x,n),0)
end
function fft!(x::AbstractArray{T}) where {T <: AbstractSeries{cpx}} where {cpx <: Complex{num}} where {num <: Number}
	Xcomplex=[el[0] for el in x]
    planned_fft = plan_fft!(similar(Xcomplex))
    Xcomplex .= planned_fft * Xcomplex
	idx=1:get_order(x[1])
	ders=[get_diff(x,i) for i in idx];
	for i in idx
		ders[i] .= planned_fft * ders[i]
	end
	for i in 1:length(x)
		ders2=[ders[order][i] for order in idx]
		@views x[i]=Taylor1([Xcomplex[i],ders2...])
	end
    nothing
end


module HeatCapacity

# This implements different methods to specify heat capacity
#
# If you want to add a new method here, feel free to do so.
# Remember to also export the function name in GeoParams.jl (in addition to here)

using Parameters, LaTeXStrings, Unitful
using ..Units
using GeoParams: AbstractMaterialParam, AbstractMaterialParamsStruct
import Base.show

abstract type AbstractHeatCapacity <: AbstractMaterialParam end

export  ComputeHeatCapacity,                # calculation routines
        ComputeHeatCapacity!,               # in-place routine
        ConstantHeatCapacity,               # constant
        T_HeatCapacity_Whittacker           # T-dependent heat capacity

# Constant Heat Capacity -------------------------------------------------------
"""
    ConstantHeatCapacity(cp=1050J/mol/kg)

Set a constant heat capacity:
```math
    cp  = cst
```
where ``cp`` is the thermal heat capacity [``J/kg/K``].
"""
@with_kw_noshow mutable struct ConstantHeatCapacity <: AbstractHeatCapacity
    equation::LaTeXString   =   L"cp = cst"
    cp::GeoUnit             =   1050J/kg/K                # heat capacity
end

# Calculation routine
function ComputeHeatCapacity(P, T, s::ConstantHeatCapacity)
    @unpack cp   = s
    if length(T)>1
        return Value(cp)*ones(size(T))
    else
        return Value(cp)
    end

end

# Calculation routine
function ComputeHeatCapacity!(P, T, s::ConstantHeatCapacity)
    @unpack cp   = s
    if length(T)>1
        return Value(cp)*ones(size(T))
    else
        return Value(cp)
    end

end

function ComputeHeatCapacity!(cp_array::AbstractArray{<:AbstractFloat,N},P::AbstractArray{<:AbstractFloat,N},T::AbstractArray{<:AbstractFloat,N}, s::ConstantHeatCapacity) where N
    @unpack cp   = s

    cp_array .= NumValue(cp)

    return nothing
end

# Print info
function show(io::IO, g::ConstantHeatCapacity)
    print(io, "Constant heat capacity: cp=$(g.cp.val)")
end
#-------------------------------------------------------------------------

# Temperature dependent heat capacity -------------------------------
"""
    T_HeatCapacity_Whittacker()

Sets a temperature-dependent heat capacity following the parameterization of Whittacker et al. (2009), Nature:
```math
    Cp = (a + b T - c/T^2)/m
```

where ``Cp`` is the heat capacity [``J/kg/K``], and ``a,b,c`` are parameters that dependent on the temperature `T`:
- a = 199.50 J/mol/K    if T<= 846 K
- a = 199.50 J/mol/K    if T> 846 K
- b = 0.0857J/mol/K^2   if T<= 846 K
- b = 0.0323J/mol/K^2   if T> 846 K
- c = 5e6J/mol*K        if T<= 846 K
- c = 47.9e-6J/mol*K    if T> 846 K
- molmass =   0.22178kg/mol

Note that this is slightly different than the equation in the manuscript, as Cp is in J/kg/K (rather than ``J/mol/K`` as in eq.3/4 of the paper)
"""
@with_kw_noshow mutable struct T_HeatCapacity_Whittacker <: AbstractHeatCapacity
    # Note: the resulting curve was visually compared with Fig. 2 of the paper
    equation::LaTeXString   =   L"cp = (a + b*T - c/T^2)/m"
    a0::GeoUnit             =   199.5J/mol/K                # prefactor for low T       (T<= 846 K)
    a1::GeoUnit             =   229.32J/mol/K               # prefactor for high T      (T>  846 K)
    b0::GeoUnit             =   0.0857J/mol/K^2             # linear term for low T     (T<= 846 K)
    b1::GeoUnit             =   0.0323J/mol/K^2             # linear term for high T    (T>  846 K)
    c0::GeoUnit             =   5e6J/mol*K                  # quadratic term for low T  (T<= 846 K)
    c1::GeoUnit             =   47.9e-6J/mol*K              # quadratic term for high T (T>  846 K)
    molmass::GeoUnit        =   0.22178kg/mol               # average molar mass
    Tcutoff::GeoUnit        =   846K                        # cutoff temperature
end

# Calculation routine
function ComputeHeatCapacity(P,T, s::T_HeatCapacity_Whittacker)
    @unpack a0,a1,b0,b1,c0,c1, molmass, Tcutoff   = s

    cp = zeros(size(T)).*Value(a0)./Value(molmass)

    for i in eachindex(T)
        if T[i] <= Value(Tcutoff)
            a,b,c = Value(a0),Value(b0),Value(c0)
        else
            a,b,c = Value(a1),Value(b1),Value(c1)
        end

        cp[i] = (a + b*T[i] - c/T[i]^2)/molmass
    end

    return cp
end

"""
    ComputeHeatCapacity!(cp_array::AbstractArray{<:AbstractFloat,N}, T::AbstractArray{<:AbstractFloat,N}, s::T_HeatCapacity_Whittacker) where N

Computes T-dependent heat capacity in-place
"""
function ComputeHeatCapacity!(cp_array::AbstractArray{<:AbstractFloat,N},T::AbstractArray{<:AbstractFloat,N}, s::T_HeatCapacity_Whittacker) where N
    @unpack a0,a1,b0,b1,c0,c1, molmass, Tcutoff   = s
    a0 = NumValue(a0)
    a1 = NumValue(a1)
    b0 = NumValue(b0)
    b1 = NumValue(b1)
    c0 = NumValue(c0)
    c1 = NumValue(c1)
    molmass = NumValue(molmass)
    Tcutoff = NumValue(Tcutoff)

    ind             =   (T .<= Tcutoff)
    T_local         =   view(T, ind )
    cp_array[ind]   =   (a0 .+ b0*T_local - c0./T_local.^2)/molmass

    ind             =   (T .> Tcutoff)
    T_local         =   view(T, ind )
    cp_array[ind]   =   (a1 .+ b1*T_local - c1./T_local.^2)/molmass

    return nothing
end

"""
    ComputeHeatCapacity!(cp_array::AbstractArray{<:AbstractFloat,N},P::AbstractArray{<:AbstractFloat,N},T::AbstractArray{<:AbstractFloat,N}, s::T_HeatCapacity_Whittacker) where N

Computes T-dependent heat capacity in-place
"""
function ComputeHeatCapacity!(cp_array::AbstractArray{<:AbstractFloat,N},P::AbstractArray{<:AbstractFloat,N},T::AbstractArray{<:AbstractFloat,N}, s::T_HeatCapacity_Whittacker) where N

    ComputeHeatCapacity!(cp_array,T, s)

    return nothing
end

# Print info
function show(io::IO, g::T_HeatCapacity_Whittacker)
    print(io, "T-dependent heat capacity: cp/$(g.molmass.val)=$(g.a0.val) + $(g.b0.val)*T - $(g.c0.val)/T^2 (for T<=$(g.Tcutoff.val)); ");
    print(io, " cp/$(g.molmass.val)=$(g.a1.val) + $(g.b1.val)*T - $(g.c1.val)/T^2 (for T>$(g.Tcutoff.val)) \n");
end
#-------------------------------------------------------------------------

#-------------------------------------------------------------------------
# Heat capacity from phase diagram

# to be implemented - see density implementation

#-------------------------------------------------------------------------

# Help info for the calculation routines
"""
    Cp = ComputeHeatCapacity(P, T, s:<AbstractHeatCapacity)

Returns the heat capacity `Cp` at any temperature `T` and pressure `P` using any of the heat capacity laws implemented.

Currently available:
- ConstantHeatCapacity
- T\\_HeatCapacity_Whittacker

# Example
Using dimensional units
```julia
julia> T  = (250:100:1250)*K;
julia> cp = T_HeatCapacity_Whittacker()
julia> Cp = ComputeHeatCapacity(0,T,cp)
11-element Vector{Unitful.Quantity{Float64, 𝐋² 𝚯⁻¹ 𝐓⁻², Unitful.FreeUnits{(kg⁻¹, J, K⁻¹), 𝐋² 𝚯⁻¹ 𝐓⁻², nothing}}}:
  635.4269997294616 J kg⁻¹ K⁻¹
  850.7470171764261 J kg⁻¹ K⁻¹
  962.0959598489883 J kg⁻¹ K⁻¹
 1037.5420433770641 J kg⁻¹ K⁻¹
 1097.3517921966488 J kg⁻¹ K⁻¹
 1149.2745563671706 J kg⁻¹ K⁻¹
 1157.7915050948404 J kg⁻¹ K⁻¹
 1172.3554874197264 J kg⁻¹ K⁻¹
 1186.9194697445964 J kg⁻¹ K⁻¹
  1201.483452069455 J kg⁻¹ K⁻¹
 1216.0474343943067 J kg⁻¹ K⁻¹
```

"""
ComputeHeatCapacity()

"""
    Cp = ComputeHeatCapacity(T::Any, s::AbstractHeatCapacity)

Computes heat capacity if only temperature (and not pressure) is specified
"""
ComputeHeatCapacity(T::Any, s::AbstractHeatCapacity) =  ComputeHeatCapacity(0,T, s)

"""
    Cp = ComputeHeatCapacity(s::ConstantHeatCapacity)

Returns heat capacity if we are sure that we will only employ constant heat capacity in the simulation
"""
ComputeHeatCapacity(s::ConstantHeatCapacity) =  ComputeHeatCapacity(0,0, s)

"""
    ComputeHeatCapacity!(Cp::AbstractArray{<:AbstractFloat}, Phases::AbstractArray{<:Integer}, P::AbstractArray{<:AbstractFloat},T::AbstractArray{<:AbstractFloat}, MatParam::AbstractArray{<:AbstractMaterialParamsStruct})

In-place computation of heat capacity `Cp` for the whole domain and all phases, in case a vector with phase properties `MatParam` is provided, along with `P` and `T` arrays.
This assumes that the `Phase` of every point is specified as an Integer in the `Phases` array.

"""
function ComputeHeatCapacity!(Cp::AbstractArray{<:AbstractFloat, N}, Phases::AbstractArray{<:Integer, N}, P::AbstractArray{<:AbstractFloat, N},T::AbstractArray{<:AbstractFloat, N}, MatParam::AbstractArray{<:AbstractMaterialParamsStruct, 1}) where N

    for i = 1:length(MatParam)

        if !isnothing(MatParam[i].HeatCapacity)
            # Create views into arrays (so we don't have to allocate)
            ind = Phases .== i;
            cp_local    =   view(Cp, ind )
            P_local     =   view(P , ind )
            T_local     =   view(T , ind )

            ComputeHeatCapacity!(cp_local, P_local, T_local, MatParam[i].HeatCapacity[1] )
        end

    end

end

"""
    ComputeHeatCapacity!(Cp::AbstractArray{<:AbstractFloat}, Phases::AbstractArray{<:Integer}, T::AbstractArray{<:AbstractFloat}, MatParam::AbstractArray{<:AbstractMaterialParamsStruct})

In-place computation of heat capacity `Cp` for the whole domain and all phases, in case a vector with phase properties `MatParam` is provided, along with `P` and `T` arrays.
This assumes that the `Phase` of every point is specified as an Integer in the `Phases` array.

"""
function ComputeHeatCapacity!(Cp::AbstractArray{<:AbstractFloat, N}, Phases::AbstractArray{<:Integer, N}, T::AbstractArray{<:AbstractFloat, N}, MatParam::AbstractArray{<:AbstractMaterialParamsStruct, 1}) where N

    for i = 1:length(MatParam)
        if !isnothing(MatParam[i].HeatCapacity)
            # Create views into arrays (so we don't have to allocate)
            ind = Phases .== i;
            cp_local    =   view(Cp, ind )
            T_local     =   view(T , ind )

            ComputeHeatCapacity!(cp_local, T_local, MatParam[i].HeatCapacity[1] )
        end

    end

end

"""
    ComputeDensity!(Cp::AbstractArray{<:AbstractFloat,N}, PhaseRatios::AbstractArray{<:AbstractFloat, M}, P::AbstractArray{<:AbstractFloat,N},T::AbstractArray{<:AbstractFloat,N}, MatParam::AbstractArray{<:AbstractMaterialParamsStruct})

In-place computation of heat capacity `Cp` for the whole domain and all phases, in case a vector with phase properties `MatParam` is provided, along with `P` and `T` arrays.
This assumes that the `PhaseRatio` of every point is specified as an Integer in the `PhaseRatios` array, which has one dimension more than the data arrays (and has a phase fraction between 0-1)

"""
function ComputeHeatCapacity!(Cp::AbstractArray{<:AbstractFloat, N}, PhaseRatios::AbstractArray{<:AbstractFloat, M}, P::AbstractArray{<:AbstractFloat, N},T::AbstractArray{<:AbstractFloat, N}, MatParam::AbstractArray{<:AbstractMaterialParamsStruct, 1}) where {N,M}

    if M!=(N+1)
        error("The PhaseRatios array should have one dimension more than the other arrays")
    end

    Cp .= 0.0;
    for i = 1:length(MatParam)

        Cp_local   = zeros(size(Cp))
        Fraction    = selectdim(PhaseRatios,M,i);
        if (maximum(Fraction)>0.0) & (!isnothing(MatParam[i].HeatCapacity))

            ComputeHeatCapacity!(Cp_local, P, T, MatParam[i].HeatCapacity[1] )

            Cp .= Cp .+ Cp_local.*Fraction
        end

    end

end

end


using BEAST, CompScienceMeshes, MieSeries

Θ = range(0, stop=π, length=100)
Φ = 0.0
P = [ [cos(ϕ)*sin(θ), sin(ϕ)*sin(θ), cos(θ)] for θ in Θ for ϕ in Φ]

## Solve the scatering problem using BEM
Γ = readmesh(joinpath(dirname(@__FILE__),"sphere2.in"))
X = raviartthomas(Γ)

ϵ = μ = 1.0
ω = 1.0; κ = ω * √(ϵ*μ)
t = Maxwell3D.singlelayer(wavenumber=κ)
E = Maxwell3D.planewave(direction=ẑ, polarization=x̂, wavenumber=κ)
e = (n × E) × n

@hilbertspace j; @hilbertspace k
efie = @discretise t[k,j]==e[k]  j∈X k∈X
u = gmres(efie)

A = Maxwell3D.farfield(wavenumber=κ)
farbem = 1/(4pi)* norm.(potential(A, P, u, X))

## Solve the scattering problem by computing the Mie series
radius = 1.0; ϵ = μ = 1.0
sphere = PECSphere(radius, ϵ, μ)
exc = excite(sphere, ω, :x)
farmie = [norm(collect(farfield(sphere, exc, p)[1:3])) for p in P]

# Compare
@show norm(farmie - farbem) / norm(farmie)


# ============================================================
# Theories
# ============================================================

export prim_conserve, conserve_prim, mixture_prim_conserve, mixture_conserve_prim
export advection_flux, burgers_flux, euler_flux, euler_jacobi, em_coefficients
export gauss_moments, mixture_gauss_moments, discrete_moments
export moments_conserve, diatomic_moments_conserve, mixture_moments_conserve
export pdf_slope, mixture_pdf_slope, moments_conserve_slope, mixture_moments_conserve_slope
export flux_conserve!, pressure, stress, heat_flux
export maxwellian, energy_maxwellian, maxwellian!, mixture_maxwellian, mixture_maxwellian!
export shakhov, shakhov!, rykov!
export reduce_distribution, full_distribution
export ref_vhs_vis, vhs_collision_time, rykov_zr
export aap_hs_collision_time, aap_hs_prim, aap_hs_diffeq!
export shift_pdf!
export hs_boltz_kn, kernel_mode, fsm_kernel, boltzmann_fft, boltzmann_fft!
export boltzmann_ode!, boltzmann_nuode!, bgk_ode!
export chapman_enskog
export heat_capacity_ratio, sound_speed

include("theory_continuum.jl")
include("theory_plasma.jl")
include("theory_maxwellian.jl")
include("theory_shakhov.jl")
include("theory_diatomic.jl")
include("theory_atom.jl")
include("theory_pdf.jl")
include("theory_fsm.jl")
include("theory_moments_pure.jl")
include("theory_moments_mixture.jl")
include("theory_thermo.jl")
include("theory_particle.jl")


function get_auc(x::Vector{T}, y::Vector{T}) where {T}
  AUC=zero(T)
  for i in 2:length(x)
    dx = x[i] - x[i-1]
    dy = y[i] - y[i-1]
    AUC += ( (dx*y[i-1]) + (0.5*dx*dy) )
  end
  return AUC
end


using HCIToolbox
using LossFunctions
using Optim
using PSFModels: Gaussian

# generative model
function model(X::AbstractVector{T}) where T
    position = @view X[1:2] # x, y position
    fwhm     = X[3]         # fwhm
    amp      = exp(X[4])    # amplitude
    return amp * Gaussian{T}(position, fwhm)
end

# objective function
function loss(X::AbstractVector{T}, target::SubArray) where T
    # cheap way to enforce positivity
    all(>(0), X) || return T(Inf)
    # get generative model
    m = model(X)
    # l2-distance loss (χ² loss) (LossFunctions.jl)
    stamp = @view m[target.indices...]
    return value(L2DistLoss(), target, stamp, AggMode.Sum())
end

"""
    gaussian_fit(frame, guess=center(frame))

Fit a 2D Gaussian PSF profile to `frame` centered around `guess` and return the best fitting parameters `[x, y, FWHM, log(Amp)]`.
"""
function gaussian_fit(frame::AbstractMatrix{T}, guess=center(frame); sub_size=100) where T
    # crop target view for speed
    half_size = sub_size ÷ 2
    indsx = round(Int, guess[1]-half_size):round(Int, guess[1]+half_size)
    indsy = round(Int, guess[2]-half_size):round(Int, guess[2]+half_size)
    target = @view frame[indsy, indsx]
    X0 = T[guess..., 7.5, log(maximum(target))]
    res = optimize(P -> loss(P, target), X0, LBFGS(); autodiff=:forward)
    # turn best-fit location into index
    return Optim.minimizer(res)
end

"""
    gaussian_fit_offset(frame, guess=center(frame))

Fit a 2D Gaussian PSF profile to `frame` centered around `guess` and return the offset from the center. Combine this with `HCIToolbox.shift_frame` for registering frames.
"""
function gaussian_fit_offset(frame::AbstractMatrix{T}, args...; kwargs...) where T
    Xbest = gaussian_fit(frame, args...; kwargs...)
    cy, cx = center(frame)
    dx = cx - Xbest[1]
    dy = cy - Xbest[2]
    return dx, dy
end


# distritbutions
@rule typeof(+)(:in2, Marginalisation) (m_out::UnivariateNormalDistributionsFamily, m_in1::UnivariateNormalDistributionsFamily) = begin
    min1, vin1 = mean_var(m_in1)
    mout, vout = mean_var(m_out)
    return NormalMeanVariance(mout - min1, vout + vin1)
end

@rule typeof(+)(:in2, Marginalisation) (m_out::MultivariateNormalDistributionsFamily, m_in1::MultivariateNormalDistributionsFamily) = begin
    min1, vin1 = mean_cov(m_in1)
    mout, vout = mean_cov(m_out)
    return MvNormalMeanCovariance(mout - min1, vout + vin1)
end

# PointMass
@rule typeof(+)(:in2, Marginalisation) (m_out::PointMass, m_in1::PointMass) = PointMass(mean(m_out) - mean(m_in1))

@rule typeof(+)(:in2, Marginalisation) (m_out::PointMass, m_in1::NormalMeanPrecision) = begin
    min1, win1 = mean_precision(m_in1)
    return NormalMeanPrecision(mean(m_out) - min1, win1)
end

@rule typeof(+)(:in2, Marginalisation) (m_out::NormalMeanPrecision, m_in1::PointMass) = begin
    mout, wout = mean_precision(m_out)
    return NormalMeanPrecision(mout - mean(m_in1), wout)
end

@rule typeof(+)(:in2, Marginalisation) (m_out::PointMass, m_in1::MvNormalMeanPrecision) = begin
    min1, win1 = mean_precision(m_in1)
    return MvNormalMeanPrecision(mean(m_out) - min1, win1)
end

@rule typeof(+)(:in2, Marginalisation) (m_out::MvNormalMeanPrecision, m_in1::PointMass) = begin
    mout, wout = mean_precision(m_out)
    return MvNormalMeanPrecision(mout - mean(m_in1), wout)
end

@rule typeof(+)(:in2, Marginalisation) (m_out::PointMass, m_in1::UnivariateNormalDistributionsFamily) = begin
    min1, vin1 = mean_var(m_in1)
    return NormalMeanVariance(mean(m_out) - min1, vin1)
end

@rule typeof(+)(:in2, Marginalisation) (m_out::UnivariateNormalDistributionsFamily, m_in1::PointMass) = begin
    mout, vout = mean_var(m_out)
    return NormalMeanVariance(mout - mean(m_in1), vout)
end

@rule typeof(+)(:in2, Marginalisation) (m_out::PointMass, m_in1::MultivariateNormalDistributionsFamily) = begin
    min1, vin1 = mean_cov(m_in1)
    return MvNormalMeanCovariance(mean(m_out) - min1, vin1)
end

@rule typeof(+)(:in2, Marginalisation) (m_out::MultivariateNormalDistributionsFamily, m_in1::PointMass) = begin
    mout, vout = mean_cov(m_out)
    return MvNormalMeanCovariance(mout - mean(m_in1), vout)
end


mutable struct MWSingleLayerField3D{K}
    wavenumber::K
end

"""
    MWSingleLayerField3D(wavenumber = error())

Create the single layer near field operator, for use with `potential`.
"""
MWSingleLayerField3D(;wavenumber = error("Missing arg: `wavenumber`")) = MWSingleLayerField3D(wavenumber)

quaddata(op::MWSingleLayerField3D,rs,els) = quadpoints(rs,els,(2,))
quadrule(op::MWSingleLayerField3D,refspace,p,y,q,el,qdata) = qdata[1,q]

function kernelvals(op::MWSingleLayerField3D,y,p)

        k = op.wavenumber
        r = y - cartesian(p)
        R = norm(r)

        kr = k * R
        expn = exp(-im * k * R)
        green = expn / (4pi*R)
        gradgreen = -(im*k + 1/R) * green / R * r

        KernelValsMaxwell3D(im*k, r, R, green, gradgreen)
end

function integrand(op::MWSingleLayerField3D, krn, y, fp, p)

    j = fp[1]
    ρ = fp[2]

    #κ = krn.
    γ = krn.gamma
    G = krn.green
    ∇G = krn.gradgreen

    -γ*G*j + ∇G*ρ/γ
end


using Parameters, ForwardDiff

struct DSGE_Model{data <: AbstractArray, alfa <: Float64}
    "Data"
    data::data
    "Alpha"
    alfa::alfa
end

function (problem::DSGE_Model)(pars)
    @unpack alfa, data = problem
    @unpack bet, epsilon, theta, sig, s2, phi, phi_pi, phi_y, rho_v = pars
    log_p_bet(bet) = logpdf(prior_bet,bet)
    log_p_epsilon(epsilon) = logpdf(prior_epsilon,epsilon)
    log_p_theta(theta) = logpdf(prior_theta,theta)
    log_p_sig(sig) = logpdf(prior_sig,sig)
    log_p_s2(s2) = logpdf(prior_s2,s2)
    log_p_phi(phi) = logpdf(prior_phi,phi)
    log_p_phi_pi(phi_pi) = logpdf(prior_phi_pi,phi_pi)
    log_p_phi_y(phi_y) = logpdf(prior_phi_y,phi_y)
    log_p_rho_v(rho_v) = logpdf(prior_rho_v,rho_v)
    llh,dll = log_like_dsge([alfa,bet,epsilon,theta,sig,s2,phi,phi_pi,phi_y,rho_v],data)
    llh = llh + log_p_bet(bet) + log_p_epsilon(epsilon) + log_p_theta(theta) + log_p_sig(sig) + log_p_s2(s2) + log_p_phi(phi) + log_p_phi_pi(phi_pi) + log_p_phi_y(phi_y) + log_p_rho_v(rho_v)
    dll = dll + [0 ForwardDiff.derivative(log_p_bet,bet) ForwardDiff.derivative(log_p_epsilon,epsilon) ForwardDiff.derivative(log_p_theta,theta) ForwardDiff.derivative(log_p_sig,sig) ForwardDiff.derivative(log_p_s2,s2) ForwardDiff.derivative(log_p_phi,phi) ForwardDiff.derivative(log_p_phi_pi,phi_pi) ForwardDiff.derivative(log_p_phi_y,phi_y) ForwardDiff.derivative(log_p_rho_v,rho_v)]

    return llh,dll
end

function problem_transform(p::DSGE_Model)
    as((bet = as_unit_interval, epsilon=as_positive_real,theta = as_unit_interval, sig = as_positive_real, s2 = as_positive_real, phi = as_positive_real, phi_pi = as_positive_real, phi_y = as_positive_real, rho_v = as_unit_interval))
end


abstract type AbstractPrimRec end

struct Succ <: AbstractPrimRec end

(::Succ)(xs::Int...)::Int = xs[1] + 1

const succ = Succ()

struct Zero <: AbstractPrimRec end

(::Zero)(xs::Int...)::Int = 0

const zro = Zero()

struct Proj{T} <: AbstractPrimRec end

function (::Proj{T})(xs::Int...)::Int where {T}; xs[T] end

struct Comb <: AbstractPrimRec
    f::AbstractPrimRec
    gs::Tuple{Vararg{T where T<:AbstractPrimRec}}
    Comb(f, gs...) = new(f, gs)
end

(c::Comb)(xs::Int...)::Int = c.f((g(xs...) for g in c.gs)...)

struct PrimRec <: AbstractPrimRec
    basic::AbstractPrimRec
    step::AbstractPrimRec
end

(pr::PrimRec)(xs::Int...)::Int = begin
    t = xs[1]
    y = xs[2:end]
    cur = pr.basic(y...)
    for i in 0:(t - 1)
        cur = pr.step(cur, i, y...)
    end
    cur
end

(f::AbstractPrimRec)(gs::AbstractPrimRec...) = Comb(f, gs...)


using Test

import sbp
using sbp.Operator: read_operator, to_matrix
using sbp.Grid:grid_xp, grid_xm
using sbp.Test:test_first_derivative, test_quadrature, test_interpolation,
        test_first_derivative_sbp, test_interpolation_sbp
include("operators.jl")

using . OP2019: build_operators, load_description

m = 10

desc = load_description()
xp, xm, h, Dp, Dm, Hp, Hm, Pp, Pm = build_operators(desc, m)

@time begin

@testset "Dp_42" begin
        ml = size(desc.Dp.left, 1)
        mr = size(desc.Dp.right, 1)
        test_first_derivative(Dp, xp, xm, 4, 2, ml, mr)
end

@testset "Dm_42" begin
        ml = size(desc.Dm.left, 1)
        mr = size(desc.Dm.right, 1)
        test_first_derivative(Dm, xm, xp, 4, 2, ml, mr)
end

@testset "Pp_42" begin
        ml = size(desc.Pp.left, 1)
        mr = size(desc.Pp.right, 1)
        test_interpolation(Pp, xp, xm, 3, 1, ml, mr)
end

@testset "Pm_42" begin
        ml = size(desc.Pm.left, 1)
        mr = size(desc.Pm.right, 1)
        test_interpolation(Pm, xm, xp, 3, 1, ml, mr)
end

@testset "Hp_42" begin
        test_quadrature(Hp, xp, 3)

end

@testset "Hm_42" begin
        test_quadrature(Hm, xm, 3)

end

@testset "D_SBP" begin
        B = 0*Dp
        B[1,1] = -1
        B[end,end] = 1
        test_first_derivative_sbp(Hp, Dp, Hm, Dm, B)
end

@testset "P_SBP" begin
        test_interpolation_sbp(Hp, Pp, Hm, Pm)
end

end


module Transforms
	using Base.Threads
	using Interpolations
	using StaticArrays
	using LinearAlgebra
	using QuadGK
	using ProgressMeter

	export line_segment, interpolate_array, line_segment_2D, LineFilterTransform, OrientationFilterTransform

	function interpolate_array(array::AbstractArray, interp_scheme::BSpline)
		interpolant = interpolate(array, interp_scheme)
		interpolant_padded = extrapolate(interpolant, 0)

		return interpolant_padded
	end

	function line_segment(i::Integer, j::Integer, θ::T, S::SVector{N, T}) where {N, T<:Real}

		x = SVector{N, T}(i+s*cos(θ) for s in S)
		y = SVector{N, T}(j+s*sin(θ) for s in S)

		return x, y
	end

	function line_segment(i::Integer, j::Integer, k::Integer, θ::T, ϕ::T, S::SVector{N, T}) where {N, T<:Real}

		x = SVector{N, T}(i+s*sin(θ)*cos(ϕ) for s in S)
		y = SVector{N, T}(j+s*sin(θ)*sin(ϕ) for s in S)
		z = SVector{N, T}(k+s*cos(θ) for s in S)

		return x, y, z
	end

	function LFT_point(i, j, Θ, array_interpolant, quad_points, quad_weights)

		function LFT_integrand(θ)
			line_seg = line_segment(i, j, θ, quad_points)
			array_vals = SVector([array_interpolant(c...) for c in zip(line_seg...)])
			return array_vals ⋅ quad_weights
		end

		LFT_θ = map(LFT_integrand, Θ)
		max_val, ind = findmax(LFT_θ)

		return max_val, Θ[ind]
	end

	function LFT_point_faster(i, j, Θ, array_interpolant, quad_points, quad_weights)

		function LFT_integrand(θ)
			sθ, cθ = sincos(θ)
			sum = zero(θ)
			for (s, w) in zip(quad_points, quad_weights)
				f = array_interpolant(i+s*cθ, j+s*sθ)
				sum = muladd(f, w, sum)
			end
			return sum
		end

		LFT_θ = map(LFT_integrand, Θ)
		max_val, ind = findmax(LFT_θ)

		return max_val, Θ[ind]
	end

	function LFT_point(i, j, k, angles, array_interpolant, quad_points, quad_weights)

		function LFT_integrand(angle)
			line_seg = line_segment(i, j, k, angle..., quad_points)
			array_vals = SVector([array_interpolant(c...) for c in zip(line_seg...)])
			return array_vals ⋅ quad_weights
		end

		LFT_angles = map(LFT_integrand, angles)
		max_val, ind = findmax(LFT_angles)

		return max_val, angles[ind][1], angles[ind][2]
	end

	function LFT_point_faster(i, j, k, angles, array_interpolant, quad_points, quad_weights)

		function LFT_integrand(angle)
			sθ, cθ = sincos(angle[1])
			sϕ, cϕ = sincos(angle[2])
			sθcϕ, sθsϕ = sθ*cϕ, sθ*sϕ
			sum = zero(angle[1])

			for (s, w) in zip(quad_points, quad_weights)
				f = array_interpolant(i+s*sθcϕ, j+s*sθsϕ, k+s*cθ)
				sum = muladd(f, w, sum)
			end
			return sum
		end

		LFT_angles = map(LFT_integrand, angles)
		max_val, ind = findmax(LFT_angles)

		return max_val, angles[ind][1], angles[ind][2]
	end

	function LFT_point(i, j, k, angles, array_interpolant, quad_points, quad_weights)

		function LFT_integrand(angle)
			line_seg = line_segment(i, j, k, angle..., quad_points)
			array_vals = SVector([array_interpolant(c...) for c in zip(line_seg...)])
			return array_vals ⋅ quad_weights
		end

		LFT_angles = map(LFT_integrand, angles)
		max_val, ind = findmax(LFT_angles)

		return max_val, angles[ind][1], angles[ind][2]
	end

	function LineFilterTransform(array::AbstractArray{T, 2}, N::Integer, r::Integer, Nr::Integer; interp_scheme::BSpline = BSpline(Constant())) where {T<:Real}
		array_interpolant = interpolate_array(array, interp_scheme)
		intensity, orientation = zero(array), zero(array)

		angles = SVector{N, T}(LinRange(-π/2, π/2, N))
		quad_points, quad_weights = gauss(Nr, -r, r)
		quad_points = SVector{Nr, T}(quad_points)
		quad_weights = SVector{Nr, T}(quad_weights)

		p = Progress(length(array))
		@threads for ind in CartesianIndices(array)
			intensity[ind], orientation[ind] = @fastmath LFT_point_faster(ind.I..., angles, array_interpolant, quad_points, quad_weights)
			next!(p)
		end

		return intensity, orientation
	end

	function LineFilterTransform(array::AbstractArray{T, 3}, N::Integer, r::Integer, Nr::Integer; interp_scheme::BSpline = BSpline(Constant())) where {T<:Real}
		array_interpolant = interpolate_array(array, interp_scheme)
		intensity, orientation = zero(array), zeros(T, size(array)..., 2)

		angles = SVector{2N^2, Tuple{T, T}}(collect(Iterators.product(LinRange(-π/2, π/2, N), LinRange(-π, π, 2N))))
		quad_points, quad_weights = gauss(Nr, -r, r)
		quad_points = SVector{Nr, T}(quad_points)
		quad_weights = SVector{Nr, T}(quad_weights)

		p = Progress(length(array))
		@threads for ind in CartesianIndices(array)
			intensity[ind], orientation[ind,1], orientation[ind,2] = @fastmath LFT_point_faster(ind.I..., angles, array_interpolant, quad_points, quad_weights)
			next!(p)
		end

		return intensity, orientation
	end

	function OFT_point(i, j, Α, intensity_field, orientation_field, quad_points, quad_weights)

		function OFT_integrand(α)
			line_seg = line_segment(i, j, α, quad_points)
			ρ_line = SVector([intensity_field(c...) for c in zip(line_seg...)])
			θ_line = SVector([orientation_field(c...) for c in zip(line_seg...)])

			orientation_functional = ρ_line.*cos.(2(θ_line .- α))
			return orientation_functional ⋅ quad_weights
		end

		OFT_α = map(OFT_integrand, Α)
		ind = argmax(abs.(OFT_α))

		return OFT_α[ind]
	end

	function OFT_point_faster(i, j, Α, intensity_field, orientation_field, quad_points, quad_weights)

		function OFT_integrand(α)
			sα, cα = sincos(α)

			sum = zero(α)
			@inbounds for (s, w) in zip(quad_points, quad_weights)
				x, y = i+s*cα, j+s*sα
				ρ, θ = intensity_field(x, y), orientation_field(x, y)
				f = ρ*cos(2(θ-α))

				sum = muladd(f, w, sum)
			end

			return sum
		end

		OFT_α = map(OFT_integrand, Α)
		ind = argmax(abs.(OFT_α))

		return OFT_α[ind]
	end

	function OFT_point(i, j, k, angles, intensity_field, θ_field, ϕ_field, quad_points, quad_weights)

		function OFT_integrand(angle)
			line_seg = line_segment(i, j, k, angle..., quad_points)
			ρ_line = SVector([intensity_field(c...) for c in zip(line_seg...)])
			θ_line = SVector([θ_field(c...) for c in zip(line_seg...)])
			ϕ_line = SVector([ϕ_field(c...) for c in zip(line_seg...)])

			α₁, α₂ = angle
			inner = sin.(θ_line)*sin(α₁).*cos.(ϕ_line.-α₂).+cos.(θ_line)*cos(α₁)
			orientation_functional = ρ_line.*(2inner.^2 .- 1)
			return orientation_functional ⋅ quad_weights
		end

		OFT_angles = map(OFT_integrand, angles)
		ind = argmax(abs.(OFT_angles))

		return OFT_angles[ind]
	end

	function OFT_point_faster(i, j, k, angles, intensity_field, θ_field, ϕ_field, quad_points, quad_weights)

		function OFT_integrand(angle)
			sα₁, cα₁ = sincos(angle[1])
			sα₂, cα₂ = sincos(angle[2])
			sα₁cα₂, sα₁sα₂ = sα₁*cα₂, sα₁*sα₂
			sum = zero(angle[1])
			for (s, w) in zip(quad_points, quad_weights)
				x, y, z = i+s*sα₁cα₂, j+s*sα₁sα₂, k+s*cα₁
				ρ, θ, ϕ = intensity_field(x, y, z), θ_field(x, y, z), ϕ_field(x, y, z)
				sθ, cθ = sincos(θ)
				aligned = sθ*sα₁*cos(ϕ-angle[2]) + cθ*cα₁
				f = ρ*(2aligned^2-1)

				sum = muladd(f, w, sum)
			end

			return sum
		end

		OFT_angles = map(OFT_integrand, angles)
		ind = argmax(abs.(OFT_angles))

		return OFT_angles[ind]
	end

	function OrientationFilterTransform(LFT_intensity::AbstractArray{T, 2}, LFT_orientation::AbstractArray{T, 2}, N::Integer, r::Integer, Nr::Integer, interp_scheme::BSpline = BSpline(Constant())) where {T<:Real}
		ρ = interpolate_array(LFT_intensity, interp_scheme)
		θ = interpolate_array(LFT_orientation, interp_scheme)
		OFT = zero(LFT_intensity)

		angles = SVector{N, T}(LinRange(-π/2, π/2, N))
		quad_points, quad_weights = gauss(Nr, -r, r)
		quad_points = SVector{Nr, T}(quad_points)
		quad_weights = SVector{Nr, T}(quad_weights)

		p = Progress(length(LFT_intensity))
		@threads for ind in CartesianIndices(LFT_intensity)
			OFT[ind] = @fastmath OFT_point_faster(ind.I..., angles, ρ, θ, quad_points, quad_weights)
			next!(p)
		end

		return OFT
	end

	function OrientationFilterTransform(LFT_intensity::AbstractArray{T, 3}, LFT_orientation::AbstractArray{T, 4}, N::Integer, r::Integer, Nr::Integer, interp_scheme::BSpline = BSpline(Constant())) where {T<:Real}
		ρ = interpolate_array(LFT_intensity, interp_scheme)
		θ = interpolate_array(LFT_orientation[:,:,:, 1], interp_scheme)
		ϕ = interpolate_array(LFT_orientation[:,:,:, 2], interp_scheme)
		OFT = zero(LFT_intensity)

		angles = SVector{2N^2, Tuple{T, T}}(collect(Iterators.product(LinRange(-π/2, π/2, N), LinRange(-π, π, 2N))))
		quad_points, quad_weights = gauss(Nr, -r, r)
		quad_points = SVector{Nr, T}(quad_points)
		quad_weights = SVector{Nr, T}(quad_weights)

		p = Progress(length(LFT_intensity))
		@threads for ind in CartesianIndices(LFT_intensity)
			OFT[ind] = @fastmath OFT_point_faster(ind.I..., angles, ρ, θ, ϕ, quad_points, quad_weights)
			next!(p)
		end

		return OFT
	end
end


function integrate(m1, m2, p0, e0)
    """
    General purpose integrator for eqn(5.14) of Peters, 1964 [1].

    [1] https://ui.adsabs.harvard.edu/abs/1964PhRv..136.1224P/abstract

    Params:
        m1 -- The mass of the first star  (Solar Mass)
        m2 -- The mass of the second star (Solar Mass)
        p0 -- The initial period          (days)
        e0 -- The initial eccentricity

    Returns:
        tC -- The coalescence time        (seconds)
    """
    Solar_Mass   = 1.989e30    # kg
    Solar_Radius = 696340000.0 # m
               G = 6.67e-11    # m^3 kg^-1 s^-2
               c = 299792458.0 # m/s

    convert(Float64, m1)
    convert(Float64, m2)
    convert(Float64, p0)
    convert(Float64, e0)

    ##########################################
    ############ BEGIN UNIT CHECK ############
    ##########################################
    m1 = m1 * Solar_Mass   # Kilograms     ###
    m2 = m2 * Solar_Mass   # Kilograms     ###
    p0 = p0 * 24 * 60 * 60 # Seconds       ###
    e0 = e0                # Dimensionless ###
    ##########################################
    ############# END UNIT CHECK #############
    ##########################################

    # Use Kepler's Third Law to compute the semimajor axis, in meters
    a0 = (p0^2.0 * (G * (m1+m2)) / (4.0 * pi^2.0))^(1.0/3.0) # meters

    # compute the constant c0
    c0 = a0 * (1.0-e0^2.0) * e0^(-12.0/19.0) * (1.0+(121.0 / 304.0 * e0^2.0))^(-870.0/2299.0) # meters

    # Compute the constant beta
    beta = 64.0 / 5.0 * G^3.0 * m1 * m2 * (m1 + m2) / c^5.0
    # meters^4 / s

    # Circular binary coalescence time
    tC = a0^4.0 / (4.0*beta)

    if e0 != 0
        if e0 < 0.01
            # Low eccentricity - see eqn after eqn(5.14) of Peters, 1964
            tC = c0^4.0 * e0^(48.0/19.0) / (4.0*beta)
        elseif e0 > 0.99
            # High eccentricity - see eqn after eqn after eqn(5.14) of Peters, 1964.
            tC = tC * ((768.0 / 425.0) * (1.0-e0^2.0)^3.5)
        else
            # Medium eccentricity - see eqn(5.14) of Peters, 1964
            e = 0.0
            de = e0 / 10000.0
            summand = 0.0

            while e < e0
                this_integral  = de * e^(29.0/19.0)
                this_integral *= (1.0 + (121.0/304.0) * e^2.0)^(1181.0/2299.0)
                this_integral /= (1.0 - e^2.0)^(1.5)

                summand += this_integral
                e       += de

                convert(Float64, e)
            end

            tC = (12.0/19.0) * (c0^4.0 / beta) * summand
        end
    end

    return tC
end


module RandomMatrix

using StatsBase: sample, Weights
import Distributions: mean, var, rand
using Distributions
using Random
using LinearAlgebra
import Polynomials.fit as polyfit
using SparseArrays: spzeros
using Plots: heatmap, cgrad

const S = Union{Tuple,AbstractArray,Distribution,DataType}

export
     randDiagonal, randTriangular, randMatrix,
     randHermitian, randSymmetric, GOE, GUE,
     randStochastic,
     randToeplitz, randHankel,
     randUnitary, randOrthogonal , Haar, COE, CUE, randPermutation

include("MatrixModels.jl")

include("StructuredMatrices.jl")

include("AdjacencyMatrices.jl")

include("SamplingMatrix.jl")

include("Transformations.jl")

include("Spacing.jl")

include("Distributions.jl")

include("Math.jl")

include("Plots.jl")
end # module


struct EstimateTable
    name::Vector{String}
    estimate::Vector{Float64}
    se::Vector{Float64}
    df::Vector{Float64}
    t::Vector{Float64}
    pval::Vector{Float64}
    level::Vector{Float64}
    cil::Vector{Float64}
    ciu::Vector{Float64}
end
"""
    estimate(lmm, l::AbstractVector; level = 0.95, name = "Estimate")

Estimate table for l vector. Satter DF used.
"""
function estimate(lmm, l::AbstractVector; level = 0.95, name = "Estimate")
    est  = coef(lmm)'*l
    se   = sqrt(mulαtβα(l, vcov(lmm)))
    df   = dof_satter(lmm, l)
    t    = abs(est/se)
    pval = ccdf(TDist(df), t)*2
    d = se*quantile(TDist(df), 1-(1-level)/2)
    cil = est - d
    ciu = est + d
    EstimateTable([name], [est], [se], [df], [t], [pval], [level], [cil], [ciu])
end

function Base.show(io::IO, et::EstimateTable)
    println(io, "  Estiamte")
    mx = metida_table(et.name,  et.estimate, et.se, et.df, et.t, et.pval, et.level, et.cil, et.ciu; names = (:Name, :Estimate, :SE, :DF, :t, :pval, :Level, :LCI, :UCI))
    PrettyTables.pretty_table(io, mx; tf = PrettyTables.tf_compact)
end


import Base.getindex
import ControlSystems.numeric_type

"""
    G = LQG(sys::AbstractStateSpace, Q1, Q2, R1, R2; qQ=0, qR=0, M = I, N = I)

Return an LQG object that describes the closed control loop around the process `sys=ss(A,B,C,D)`
where the controller is of LQG-type. The controller is specified by weight matrices `Q1,Q2`
that penalizes state deviations and control signal variance respectively, and covariance
matrices `R1,R2` which specify state drift and measurement covariance respectively.

`qQ` and `qR` can be set to incorporate loop transfer recovery, i.e.,
```julia
L = lqr(A, B, Q1+qQ*C'C, Q2)
K = kalman(A, C, R1+qR*B*B', R2)
```
Increasing `qQ` will add more cost in output direction, e.g., encouraging the use of cheap control, while
increasing `qR` adds fictious dynamics noise, makes the observer faster in the direction we control.

`M` is a matrix that defines the controlled variables `z`, i.e., the variables for which you provide reference signals. If no `M` is provided, the default is to consider all state variables of the system as controlled. The definitions of `z` and `y` are given below
```
y = C*x
z = M*x
```
`size(M, 1)` determines the size of the `Q1` matrix you need to supply.

`N` is a matrix that defines how the dynamics noise `v` enters the system, i.e. If no `N` is provided, the default is to consider all state variables being affected by independent noise components. The definition of `v` is given below
```
x′ = A*x + B*u + N*v
```
`size(N, 2)` determines the size of the `R1` matrix you need to supply.

# Example

```julia
s = tf("s")
P = [1/(s+1) 2/(s+2); 1/(s+3) 1/(s-1)]
sys = ss(P)
eye(n) = Matrix{Float64}(I,n,n) # For convinience

qQ = 1
qR = 1
Q1 = 10eye(4)
Q2 = 1eye(2)
R1 = 1eye(6)
R2 = 1eye(2)

G = LQG(sys, Q1, Q2, R1, R2, qQ=qQ, qR=qR)

Gcl = G.cl
T = G.T
S = G.S
sigmaplot([S,T],exp10.(range(-3, stop=3, length=1000)))
stepplot(Gcl)
```

"""
struct LQGProblem
    sys::ExtendedStateSpace
    Q1::AbstractMatrix
    Q2::AbstractMatrix
    R1::AbstractMatrix
    R2::AbstractMatrix
    qQ::Real
    qR::Real
end

ControlSystems.isdiscrete(l::LQGProblem) = ControlSystems.isdiscrete(l.sys)
ControlSystems.iscontinuous(l::LQGProblem) = ControlSystems.iscontinuous(l.sys)

function LQGProblem(
    sys::AbstractStateSpace,
    Q1::AbstractVecOrMat,
    Q2::AbstractVecOrMat,
    R1::AbstractVecOrMat,
    R2::AbstractVecOrMat;
    qQ = 0,
    qR = 0,
    kwargs...,
)
    sys isa ExtendedStateSpace || (sys = ExtendedStateSpace(sys))
    @unpack B1, C1, C2 = sys
    Q1 = Q1 isa AbstractVector ? diagm(Q1) : Q1
    Q2 = Q2 isa AbstractVector ? diagm(Q2) : Q2
    R1 = R1 isa AbstractVector ? diagm(R1) : R1
    R2 = R2 isa AbstractVector ? diagm(R2) : R2
    size(Q1, 1) == size(C1,1) || throw(ArgumentError("The size of Q1 is determined by C1, not by the state."))
    size(R2, 1) == size(C2,1) || throw(ArgumentError("The size of R2 is determined by C2, not by the state."))
    size(R1, 1) == size(B1,2) || throw(ArgumentError("The size of R1 is determined by B1, not by the state."))
    LQGProblem(sys, Q1, Q2, R1, R2, qQ, qR; kwargs...)
end

"""
    LQGProblem(P::ExtendedStateSpace)

If only an `ExtendedStateSpace` system is provided, the system `P` is assumed to correspond to the H₂ optimal control problem with
```
C1'C1    = Q1
D12'D12  = Q2

B1*B1'   = R1
D21*D21' = R2
```
and an `LQGProblem` with the above covariance matrices is returned. The system
description in the returned LQGProblem will have `B1 = C1 = I`.
See Ch. 13 in Robust and optimal control for reference.
"""
function LQGProblem(P::ExtendedStateSpace)
    @unpack A, B1, B2, C1, C2, D11, D12, D21, D22 = P
    # all(iszero, D22) || throw(ArgumentError("Non-zero D22 not handled."))
    all(iszero, D11) || throw(ArgumentError("Non-zero D11 not handled."))
    all(iszero, D12'C1) || throw(ArgumentError("D12'C1 should be 0"))
    all(iszero, D21*B1') || throw(ArgumentError("D21*B1' should be 0"))
    Q1 = C1'C1
    Q2 = D12'D12
    R1 = B1*B1'
    R2 = D21*D21'
    B1 = I(P.nx)
    C1 = I(P.nx)

    P = ss(A, B1, B2, C1, C2; D22, Ts = P.timeevol)
    # P = ss(A, B1, B2, C1, C2; Ts = P.timeevol)
    LQGProblem(P, Q1, Q2, R1, R2)
end

function ControlSystems.kalman(l::LQGProblem)
    @unpack A, C2, B1, R1, qR, B2, R2 = l
    fun = isdiscrete(l) ? dkalman : kalman
    K = fun(A, C2, Hermitian(B1*R1*B1' + qR * B2 * B2'), R2)
end

function ControlSystems.lqr(l::LQGProblem)
    @unpack A, B2, C1, Q1, qQ, C2, Q2 = l
    fun = isdiscrete(l) ? dlqr : lqr
    L = fun(A, B2, Hermitian(C1'Q1*C1 + qQ * C2'C2), Q2)
end

"""
    ControlSystems.lqr(P::AbstractStateSpace, Q1::AbstractMatrix, Q2::AbstractMatrix, Q3::AbstractMatrix)

Calculate the feedback gain of the LQR cost function augmented with control differences
```math
x'Q1*x + u'Q2*u + Δu'Q3*Δu
Δu = u(k) - u(k-1)
```
"""
function ControlSystems.lqr(P::AbstractStateSpace, Q1::AbstractMatrix, Q2::AbstractMatrix, Q3::AbstractMatrix)
    Pd = add_input_differentiator(P)
    S = zeros(Pd.nx, P.nu)
    S[P.nx+1:end, :] = -Q3
	X, _, L = MatrixEquations.ared(Pd.A, Pd.B, Q2+Q3, cat(Q1, Q3, dims=(1,2)), S) # ME has cost matrices reversed
    L[:, 1:P.nx]
end

"""
    ControlSystems.dare(P::AbstractStateSpace, Q1::AbstractMatrix, Q2::AbstractMatrix, Q3::AbstractMatrix)

Solve the discrete-time algebraic Riccati equation for a LQR cost augmented with control differences
```math
x'Q1*x + u'Q2*u + Δu'Q3*Δu
Δu = u(k) - u(k-1)
```
"""
function ControlSystems.dare(P::AbstractStateSpace, Q1::AbstractMatrix, Q2::AbstractMatrix, Q3::AbstractMatrix)
    # The reference cited in MatrixEquations.ared, W.F. Arnold, III and A.J. Laub, Generalized Eigenproblem Algorithms and Software for Algebraic Riccati Equations
    # defines the cost function as x'Q1x + u'Q2u + 2x'Su.
    # The Δu term expands to u+'Q2u + u'Q2u - 2u Q3 u+, so the factor 2 is already accounted for
    Pd = add_input_differentiator(P)
    S = zeros(Pd.nx, P.nu)
    S[P.nx+1:end, :] = -Q3
	X, _, L = MatrixEquations.ared(Pd.A, Pd.B, Q2+Q3, cat(Q1, Q3, dims=(1,2)), S) # ME has cost matrices reversed
    X[1:P.nx, 1:P.nx]
end

function static_gain_compensation(l::LQGProblem, L = lqr(l))
    @unpack A, C1, B1, B2, D11, D12  = l
    pinv(D12 - (C1 - D12*L) * inv(A - B2*L) * B2)
end

function static_gain_compensation(A, B, C, D, L)
    pinv(D - (C - D*L) * inv(A - B*L) * B) # if D is nonzero
    # pinv(C * ((B * L - A) \ B))
end

"""
    extended_controller(K::AbstractStateSpace)

Takes a controller and returns an `ExtendedStateSpace` version which has augmented input `[r; y]` and output `y` (`z` output is 0-dim).
"""
function extended_controller(K::AbstractStateSpace)
    nx,nu,ny = K.nx, K.nu, K.ny
    A,B,C,D = ssdata(K)
    @error("This has not been verified")

    B1 = zeros(nx, nx) # dynamics not affected by r
    B2 = B # input y
    D21 = C#   K*r
    C2 = -C # - K*y
    C1 = zeros(0, nx)
    ss(A, B1, B2, C1, C2; D21, Ts = K.timeevol)
end

"""
    extended_controller(l::LQGProblem, L = lqr(l), K = kalman(l))

Returns an expression for the controller that is obtained when state-feedback `u = -L(xᵣ-x̂)` is combined with a Kalman filter with gain `K` that produces state estimates x̂. The controller is an instance of `ExtendedStateSpace` where `C2 = -L, D21 = L` and `B2 = K`.

The returned system has *inputs* `[xᵣ; y]` and outputs the control signal `u`. If a reference model `R` is used to generate state references `xᵣ`, the controller from `e = ry - y -> u` is given by
```julia
Ce = extended_controller(l)
Ce = named_ss(Ce; x = :xC, y = :u, u = [R.y; :y^l.ny]) # Name the inputs of Ce the same as the outputs of `R`.
connect([R, Ce]; u1 = R.y, y1 = R.y, w1 = [:ry^l.ny, :y^l.ny], z1=[:u])
```

Since the negative part of the feedback is built into the returned system, we have
```
C = observer_controller(l)
Ce = extended_controller(l)
system_mapping(Ce) == -C
````
"""
function extended_controller(l::LQGProblem, L = lqr(l), K = kalman(l))
    A,B,C,D = ssdata(system_mapping(l))
    Ac = A - B*L - K*C + K*D*L # 8.26b
    nx = l.nx
    B1 = zeros(nx, nx) # dynamics not affected by r
    B2 = K # input y
    D21 = L #   L*xᵣ # should be D21?
    C2 = -L # - L*x̂
    C1 = zeros(0, nx)
    ss(Ac, B1, B2, C1, C2; D21, Ts = l.timeevol)
end

"""
    observer_controller(l::LQGProblem, L = lqr(l), K = kalman(l))

Returns an expression for the feedback controller `u = Cy` that is obtained when state-feedback `u = -Lx̂` is combined with a Kalman filter with gain `K` that produces state estimates x̂.

Note: the transfer function returned is only a representation of the controller in the simple setting described above, e.g., it is not valid if the actual input contains anything that is not produced by a pure feedback from observed states. To obtain a controller that takes references into account, see `extended_controller`.
"""
function ControlSystems.observer_controller(l::LQGProblem, L::AbstractMatrix = lqr(l), K::AbstractMatrix = kalman(l))
    A,B,C,D = ssdata(system_mapping(l))
    Ac = A - B*L - K*C + K*D*L # 8.26b
    Bc = K
    Cc = L
    Dc = 0
    ss(Ac, Bc, Cc, Dc, l.timeevol)
end

function ff_controller(l::LQGProblem, L = lqr(l), K = kalman(l))
    Ae,Be,Ce,De = ssdata(system_mapping(l))
    Ac = Ae - Be*L - K*Ce + K*De*L # 8.26c
    Bc = Be*static_gain_compensation(l, L)
    Cc = L
    Dc = 0
    return 1 - ss(Ac, Bc, Cc, Dc, l.timeevol)
end

"""
    closedloop(l::LQGProblem, L = lqr(l), K = kalman(l))

Closed-loop system as defined in Glad and Ljung eq. 8.28

The return value will be the closed loop from reference only, other disturbance signals (B1) are ignored. See `feedback` for a more advanced option.
"""
function closedloop(l::LQGProblem, L = lqr(l), K = kalman(l))
    # todo: reimplement as lft
    P = system_mapping(l)
    @unpack A, B2, C2, C1 = l
    n = P.nx
    # Lr = pinv(C1 * ((P.B * L[:, 1:n] - P.A) \ P.B))
    Lr = static_gain_compensation(l, L[:, 1:n])
    # Lr = (D - (C - D*L) * inv(A - B*L) * B)
    if any(!isfinite, Lr) || all(iszero, Lr)
        @warn "Could not compensate for static gain automatically." Lr
        Lr = 1
    end
    Acl = [A-B2*L B2*L; zero(A) A-K*C2] # 8.28
    BLr = B2 * Lr # QUESTION: should be B1 here? Glad Ljung has B2
    Bcl = [BLr; zero(BLr)]
    Ccl = [C1 zero(C1)]
    syscl = ss(Acl, Bcl, Ccl, 0, l.timeevol)
end

# function closedloop(l::LQGProblem)
#     K = observer_controller(l)
#     Ke = extended_controller(K)
#     lft(l.sys, Ke)
# end

system_mapping(l::LQGProblem) = system_mapping(l.sys)

performance_mapping(l::LQGProblem) = performance_mapping(l.sys)

function Base.getproperty(G::LQGProblem, s::Symbol)
    if s ∈ fieldnames(LQGProblem)
        return getfield(G, s)
    end
    return getproperty(G.sys, s)
    error("No property named $s")
end

sensdoc = """
```
         ▲
         │e₁
         │  ┌─────┐
d₁────+──┴──►  P  ├─────┬──►e₄
      │-    └─────┘     │
      │                 │
      │     ┌─────┐     │
 e₂◄──┴─────┤  C  ◄──┬──+───d₂
            └─────┘  │
                     │e₃
                     ▼
```
- Input  sensitivity is the transfer function from d₁ to e₁,                 (I + CP)⁻¹
- Output sensitivity is the transfer function from d₂ to e₂,                 (I + PC)⁻¹
- Input  complementary sensitivity is the transfer function from d₁ to e₂, CP(I + CP)⁻¹
- Output complementary sensitivity is the transfer function from d₂ to e₄, PC(I + PC)⁻¹
"""

"""
See [`output_sensitivity`](@ref)
$sensdoc
"""
function sensitivity(args...)# Sensitivity function
    return output_sensitivity(args...)
end

"""
See [`output_comp_sensitivity`](@ref)
$sensdoc
"""
function comp_sensitivity(args...) # Complementary sensitivity function
    return output_comp_sensitivity(args...)
end

G_PS(P, C) = P*input_sensitivity(P, C)
function G_PS(l::LQGProblem) # Load disturbance to output
    return system_mapping(l) * input_sensitivity(l)
end

G_CS(P, C) = C*output_sensitivity(P, C)
function G_CS(l::LQGProblem) # Noise to control signal
    return observer_controller(l) * output_sensitivity(l)
end

loopgain(P,C) = P*C
function loopgain(l::LQGProblem)
    return system_mapping(l)*observer_controller(l)
end

function returndifference(l::LQGProblem)
    PC = loopgain(L)
    p = size(l.C2, 1)
    return ss(Matrix{numeric_type(PC)}(I, p, p), l.timeevol) + PC
end

function stabilityrobustness(l::LQGProblem)
    PC = loopgain(L)
    p = size(l.C2, 1)
    return ss(Matrix{numeric_type(PC)}(I, p, p), l.timeevol) + inv(PC)
end

"""
    input_sensitivity(P, C)
    input_sensitivity(l::LQGProblem)

Transfer function from load disturbance to total plant input.
- "Input" signifies that the transfer function is from the input of the plant.
$sensdoc
"""
function input_sensitivity(P,C)
    T = feedback(C * P)
    ss(I(noutputs(T)), P.timeevol) - T
end
input_sensitivity(l::LQGProblem) = input_sensitivity(system_mapping(l), observer_controller(l))

"""
    output_sensitivity(P, C)
    output_sensitivity(l::LQGProblem)

Transfer function from measurement noise / reference to control signal.
- "output" signifies that the transfer function is from the output of the plant.
$sensdoc
"""
function output_sensitivity(P,C)
    PC = P*C
    S = feedback(ss(Matrix{numeric_type(PC)}(I, ninputs(PC), ninputs(PC)), P.timeevol), PC)
    S.C .*= -1
    S.B .*= -1
    S
end
output_sensitivity(l::LQGProblem) = output_sensitivity(system_mapping(l), observer_controller(l))

"""
    input_comp_sensitivity(P,C)
    input_comp_sensitivity(l::LQGProblem)

Transfer function from load disturbance to control signal.
- "Input" signifies that the transfer function is from the input of the plant.
- "Complimentary" signifies that the transfer function is to an output (in this case controller output)
$sensdoc
"""
function input_comp_sensitivity(P,C)
    T = feedback(C * P)
end
input_comp_sensitivity(l::LQGProblem) = input_comp_sensitivity(system_mapping(l), observer_controller(l))

"""
    output_comp_sensitivity(P,C)
    output_comp_sensitivity(l::LQGProblem)

Transfer function from measurement noise / reference to plant output.
- "output" signifies that the transfer function is from the output of the plant.
- "Complimentary" signifies that the transfer function is to an output (in this case plant output)
$sensdoc
"""
function output_comp_sensitivity(P,C)
    S = output_sensitivity(P,C)
    ss(I(noutputs(S)), P.timeevol) - S
end
output_comp_sensitivity(l::LQGProblem) = output_comp_sensitivity(system_mapping(l), observer_controller(l))

plot(G::LQGProblem) = gangoffourplot(G)

function gangoffour(l::LQGProblem)
    sensitivity(l), G_PS(l), G_CS(l), comp_sensitivity(l)
end

function ControlSystems.gangoffourplot(l::LQGProblem, args...; sigma = true, kwargs...)
    S,D,N,T = gangoffour(l)
    bp = (args...; kwargs...) -> sigma ? sigmaplot(args...; kwargs...) : bodeplot(args...; plotphase=false, kwargs...)
    f1 = bp(S, args...; show=false, title="S = 1/(1+PC)", kwargs...)
    Plots.hline!([1], l=(:black, :dash), primary=false)
    try
        mag, freq = hinfnorm(S)
        isfinite(mag) && isfinite(freq) && (freq>0) && Plots.scatter!([freq], [mag], label="Mₛ = $(round(mag, digits=2))")
    catch
    end
    f2 = bodeplot(D, args...; show=false, title="D = P/(1+PC)", plotphase=false, kwargs...)
    Plots.hline!([1], l=(:black, :dash), primary=false)
    f3 = bodeplot(N, args...; show=false, title="N = C/(1+PC)", plotphase=false, kwargs...)
    f4 = bp(T, args...; show=false, title="T = PC/(1+PC)", kwargs...)
    Plots.hline!([1], l=(:black, :dash), primary=false)
    try
        mag, freq = hinfnorm(T)
        isfinite(mag) && isfinite(freq) && (freq>0) && Plots.scatter!([freq], [mag], label="Mₜ = $(round(mag, digits=2))")
    catch
    end
    Plots.plot(f1,f2,f3,f4)
end

"""
    gangoffourplot2(P, C, args...; sigma = true, kwargs...)

Plot the gang-of-four. If the closed-loop is MIMO, output sensitivity functions will be shown.

$sensdoc
"""
function gangoffourplot2(P, C, args...; sigma = true, kwargs...)
    S,D,N,T = gangoffour2(P,C)
    bp = (args...; kwargs...) -> sigma ? sigmaplot(args...; kwargs...) : bodeplot(args...; plotphase=false, kwargs...)
    f1 = bp(S, args...; show=false, title="S = 1/(1+PC)", kwargs...)
    # Plots.hline!([1], l=(:black, :dash), primary=false)
    Plots.hline!([1.0 1.1 1.2], l=(:dash, [:green :orange :red]), sp=1, lab=["1.0" "1.1" "1.2"], ylims=(-3,1.8))
    f2 = bodeplot(D, args...; show=false, title="D = P/(1+PC)", plotphase=false, kwargs...)
    Plots.hline!([1], l=(:black, :dash), primary=false)
    f3 = bodeplot(N, args...; show=false, title="N = C/(1+PC)", plotphase=false, kwargs...)
    f4 = bp(T, args...; show=false, title="T = PC/(1+PC)", ylims=(-3,1.8), kwargs...)
    Plots.hline!([1], l=(:black, :dash), primary=false)
    Plots.plot(f1,f2,f3,f4, ticks=:default, ylabel="", legend=:bottomright)
end

function gangofsevenplot(P, C, F, args...; sigma = true, ylabel="", layout=4, kwargs...)
    S,D,CS,T = gangoffour2(P,C)
    RY = T*F
    RU = CS*F
    RE = S*F
    bp = (args...; kwargs...) -> sigma ? sigmaplot!(args...; kwargs...) : bodeplot!(args...; plotphase=false, kwargs...)
    Plots.plot(; layout, ticks=:default, xscale=:log10, link=:both)
    bp(S, args...; show=false, title="S = 1/(1+PC)", sp=1, kwargs...)
    # Plots.hline!([1], l=(:black, :dash), primary=false, sp=1)
    Plots.hline!([1.0 1.1 1.2], l=(:dash, [:green :orange :red]), sp=1, lab=["1.0" "1.1" "1.2"], ylims=(-3,1.8))
    bodeplot!(D, args...; show=false, title="PS = P/(1+PC)", plotphase=false, sp=2, kwargs...)
    Plots.hline!([1], l=(:black, :dash), primary=false, sp=2)
    bodeplot!(CS, args...; show=false, title="CS = C/(1+PC)", plotphase=false, sp=3, kwargs...)
    Plots.hline!([1], l=(:black, :dash), primary=false, sp=3)
    bp(T, args...; show=false, title="T = PC/(1+PC)", sp=4, kwargs...)
    Plots.hline!([1], l=(:black, :dash), primary=false, sp=4)

    bp(RE, args...; show=false, title="S = 1/(1+PC)", lab="SF = r->e", l=(:dash, :red), sp=1, kwargs...)
    bp(RY, args...; show=false, title="T = PC/(1+PC)", lab="TF = r->y", l=(:dash, :green), sp=4, kwargs...)
    bp(RU, args...; show=false, title="CS = C/(1+PC)", lab="CSF = r->u", l=(:dash, :blue), sp=3, kwargs...)

    # Plots.plot(f1,f2,f3,f4,f7,f6,f5; ticks=:default, layout)
end

"""
    S, PS, CS, T = gangoffour2(P::LTISystem, C::LTISystem)

Return the gang-of-four. If the closed-loop is MIMO, output sensitivity functions will be shown.

$sensdoc
"""
function gangoffour2(P::LTISystem, C::LTISystem)
    S = output_sensitivity(P, C)
    PS = G_PS(P, C)
    CS = G_CS(P, C)
    T = output_comp_sensitivity(P, C)
    return S, PS, CS, T
end