text:
function assemble_dim(iop::IntegralOperator;compress=Matrix,location=:onsurface)
    X    = target_surface(iop)
    Y    = source_surface(iop)
    pde  = iop.kernel.op
    T    = kernel_type(iop)
    if T === SingleLayer()
        singlelayer_dim(pde,X,Y;compress,location)
    elseif T === DoubleLayer()
        doublelayer_dim(pde,X,Y;compress,location)
    elseif T === AdjointDoubleLayer()
        adjointdoublelayer_dim(pde,X,Y;compress,location)
    elseif T === HyperSingular()
        hypersingular_dim(pde,X,Y;compress,location)
    else
        notimplemented()
    end
end

function single_doublelayer_dim(pde,X,Y=X;compress=Matrix,location=:onsurface)
    msg = "unrecognized value for kw `location`: received $location.
           Valid options are `:onsurface`, `:inside` and `:outside`."
    σ = location === :onsurface ? -0.5 : location === :inside ? 0 : location === :outside ? -1 : error(msg)
    Sop  = SingleLayerOperator(pde,X,Y)
    Dop  = DoubleLayerOperator(pde,X,Y)
    # convert to a possibly more efficient format
    S = compress(Sop)
    D = compress(Dop)
    basis,γ₁_basis = _basis_dim(Sop)
    γ₀B,γ₁B,R      = _auxiliary_quantities_dim(Sop,S,D,basis,γ₁_basis,σ)
    # compute corrections
    δS = _singular_weights_dim(Sop,γ₀B,γ₁B,R)
    δD = _singular_weights_dim(Dop,γ₀B,γ₁B,R)
    # add corrections to the dense part
    axpy!(true,δS,S)
    axpy!(true,δD,D)
    return S,D
end
singlelayer_dim(args...;kwargs...) = single_doublelayer_dim(args...;kwargs...)[1]
doublelayer_dim(args...;kwargs...) = single_doublelayer_dim(args...;kwargs...)[2]

function adjointdoublelayer_hypersingular_dim(pde,X,Y=X;compress=Matrix,location=:onsurface)
    msg = "unrecognized value for kw `location`: received $location.
    Valid options are `:onsurface`, `:inside` and `:outside`."
    σ = location === :onsurface ? -0.5 : location === :inside ? 0 : location === :outside ? -1 : error(msg)
    Kop  = AdjointDoubleLayerOperator(pde,X,Y)
    Hop  = HyperSingularOperator(pde,X,Y)
    # convert to a possibly more efficient compress
    K = compress(Kop)
    H = compress(Hop)
    basis,γ₁_basis = _basis_dim(Kop)
    γ₀B,γ₁B,R      = _auxiliary_quantities_dim(Kop,K,H,basis,γ₁_basis,σ)
    # compute corrections
    δK = _singular_weights_dim(Kop,γ₀B,γ₁B,R)
    δH = _singular_weights_dim(Hop,γ₀B,γ₁B,R)
    # add corrections to the dense part
    axpy!(true,δK,K)
    axpy!(true,δH,H)
    return K,H
end
adjointdoublelayer_dim(args...;kwargs...)  = adjointdoublelayer_hypersingular_dim(args...;kwargs...)[1]
hypersingular_dim(args...;kwargs...)        = adjointdoublelayer_hypersingular_dim(args...;kwargs...)[2]

function singular_weights_dim(iop::IntegralOperator,compress=Matrix)
    X,Y,op = iop.X, iop.Y, iop.kernel.op
    σ = X == Y ? -0.5 : 0.0
    #
    basis,γ₁_basis = _basis_dim(iop)
    Op1, Op2       = _auxiliary_operators_dim(iop,compress)
    γ₀B,γ₁B,R      = _auxiliary_quantities_dim(iop,Op1,Op2,basis,γ₁_basis,σ)
    Sp = _singular_weights_dim(iop,γ₀B,γ₁B,R)
    return Sp # a sparse matrix
end

function _auxiliary_quantities_dim(iop,Op1,Op2,basis,γ₁_basis,σ)
    T           = eltype(iop)
    kernel,X,Y  = iop.kernel, iop.X, iop.Y
    op          = kernel.op
    m,n         = length(X),length(Y)
    nbasis      = length(basis)
    # compute matrix of basis evaluated on Y
    ynodes   = qnodes(Y)
    ynormals = qnormals(Y)
    γ₀B     = Matrix{T}(undef,length(ynodes),nbasis)
    γ₁B     = Matrix{T}(undef,length(ynodes),nbasis)
    for k in 1:nbasis
        for i in 1:length(ynodes)
            γ₀B[i,k] = basis[k](ynodes[i])
            γ₁B[i,k] = γ₁_basis[k](ynodes[i],ynormals[i])
        end
    end
    # integrate the basis over Y
    xnodes   = qnodes(X)
    xnormals = qnormals(X)
    R        = Op1*γ₁B - Op2*γ₀B
    # analytic correction for on-surface evaluation of Greens identity
    if kernel_type(iop) isa Union{SingleLayer,DoubleLayer}
        if σ !== 0
            for k in 1:nbasis
                for i in 1:length(xnodes)
                    R[i,k] += σ*basis[k](xnodes[i])
                end
            end
        end
    elseif kernel_type(iop) isa Union{AdjointDoubleLayer,HyperSingular}
        if σ !== 0
            for k in 1:nbasis
                for i in 1:length(xnodes)
                    R[i,k] += σ*γ₁_basis[k](xnodes[i],xnormals[i])
                end
            end
        end
    end
    return γ₀B, γ₁B, R
end

function _auxiliary_operators_dim(iop,compress)
    X,Y,op = iop.X, iop.Y, iop.kernel.op
    T = eltype(iop)
    # construct integral operators required for correction
    if kernel_type(iop) isa Union{SingleLayer,DoubleLayer}
        Op1 = IntegralOperator{T}(SingleLayerKernel(op),X,Y) |> compress
        Op2 = IntegralOperator{T}(DoubleLayerKernel(op),X,Y) |> compress
    elseif kernel_type(iop) isa Union{AdjointDoubleLayer,HyperSingular}
        Op1 = IntegralOperator{T}(AdjointDoubleLayerKernel(op),X,Y) |> compress
        Op2 = IntegralOperator{T}(HyperSingularKernel(op),X,Y) |> compress
    end
    return Op1,Op2
end

function _basis_dim(iop)
    op = iop.kernel.op
    xs = _source_gen(iop)
    basis     = [y->SingleLayerKernel(op)(x,y) for x in xs]
    γ₁_basis  = [(y,ny)->transpose(DoubleLayerKernel(op)(x,y,ny)) for x in xs]
    return basis,γ₁_basis
end

function _singular_weights_dim(iop::IntegralOperator,γ₀B,γ₁B,R)
    X,Y = iop.X, iop.Y
    T = eltype(iop)
    num_basis = size(γ₀B,2)
    a,b = combined_field_coefficients(iop)
    # we now have the residue R. For the correction we need the coefficients.
    dict_near = near_interaction_list(qnodes(X),Y;atol=0)
    Is = Int[]
    Js = Int[]
    Vs = T[]
    for (E,list_near) in dict_near
        el2qnodes = elt2dof(Y,E)
        num_qnodes, num_els   = size(el2qnodes)
        M                     = Matrix{T}(undef,2*num_qnodes,num_basis)
        @assert length(list_near) == num_els
        for n in 1:num_els
            j_glob                = @view el2qnodes[:,n]
            M[1:num_qnodes,:]     = @view γ₀B[j_glob,:]
            M[num_qnodes+1:end,:] = @view γ₁B[j_glob,:]
            # distinguish scalar and vectorial case
            if T <: Number
                F                     = qr(M)
            elseif T <: SMatrix
                F                     = qr!(blockmatrix_to_matrix(M))
            else
                error("unknown element type T=$T")
            end
            for (i,_) in list_near[n]
                if T <: Number
                    tmp = ((R[i:i,:])/F.R)*adjoint(F.Q)
                elseif T <: SMatrix
                    tmp_scalar  = (blockmatrix_to_matrix(R[i:i,:])/F.R)*adjoint(F.Q)
                    tmp  = matrix_to_blockmatrix(tmp_scalar,T)
                else
                    error("unknown element type T=$T")
                end
                w    = axpby!(a,view(tmp,1:num_qnodes),b,view(tmp,(num_qnodes+1):(2*num_qnodes)))
                append!(Is,fill(i,num_qnodes))
                append!(Js,j_glob)
                append!(Vs,w)
            end
        end
    end
    Sp = sparse(Is,Js,Vs,size(iop)...)
    return Sp
end

function _source_gen(iop::IntegralOperator,kfactor=5)
    Y      =  iop.Y
    nquad  = 0
    for (E,tags) in elt2dof(Y)
        nquad = max(nquad,size(tags,1))
    end
    nbasis = 3*nquad
    # construct source basis
    return _source_gen(iop,nbasis;kfactor)
end

function _source_gen(iop,nsources;kfactor)
    N      = ambient_dimension(iop)
    Y      = iop.Y
    pts    = qnodes(Y)
    # create a bounding box
    bbox   = bounding_box(pts)
    xc     = center(bbox)
    d      = diameter(bbox)
    if N == 2
        xs = _circle_sources(;nsources,center=xc,radius=kfactor*d/2)
    elseif N == 3
        xs = _sphere_sources_lebedev(;nsources,center=xc,radius=kfactor*d/2)
    else
        error("dimension must be 2 or 3. Got $N")
    end
    return xs
end

function _sphere_sources_lebedev(;nsources, radius=10, center=SVector(0.,0.,0.))
    lpts = lebedev_points(nsources)
    Xs = SVector{3,Float64}[]
    for pt in lpts
        push!(Xs,radius*pt .+ center)
    end
    return Xs
end

function _circle_sources(;nsources, radius=10, center=SVector(0.,0.))
    par   = (s) -> center .+ radius .* SVector(cospi(2 * s[1]), sinpi(2 * s[1]))
    x,_   = Integration._trapezoidalP(nsources)
    Xs    = SVector{2,Float64}[]
    for pt in x
        push!(Xs,par(pt))
    end
    return Xs
end

max_stars_repo_name: WaveProp/WaveProp

max_issues_repo_name: WaveProp/WaveProp

max_forks_repo_name: WaveProp/WaveProp


text:
import Printf: @sprintf
import LinearAlgebra: dot, norm
import Dates
import MPI

import ClimateMachine
import ClimateMachine.DGMethods.FVReconstructions: FVConstant, FVLinear
import ClimateMachine.DGMethods.NumericalFluxes:
    RusanovNumericalFlux,
    CentralNumericalFluxSecondOrder,
    CentralNumericalFluxGradient
import ClimateMachine.DGMethods: DGFVModel, init_ode_state
import ClimateMachine.GenericCallbacks:
    EveryXWallTimeSeconds, EveryXSimulationSteps
import ClimateMachine.MPIStateArrays: MPIStateArray, euclidean_distance
import ClimateMachine.Mesh.Grids:
    DiscontinuousSpectralElementGrid, EveryDirection
import ClimateMachine.Mesh.Topologies: StackedBrickTopology
import ClimateMachine.ODESolvers: LSRK54CarpenterKennedy, solve!, gettime
import ClimateMachine.VTK: writevtk, writepvtu

if !@isdefined integration_testing
    const integration_testing = parse(
        Bool,
        lowercase(get(ENV, "JULIA_CLIMA_INTEGRATION_TESTING", "false")),
    )
end
const output = parse(Bool, lowercase(get(ENV, "JULIA_CLIMA_OUTPUT", "false")))

include("advection_diffusion_model.jl")

struct Pseudo1D{n, α} <: AdvectionDiffusionProblem end

function init_velocity_diffusion!(
    ::Pseudo1D{n, α},
    aux::Vars,
    geom::LocalGeometry,
) where {n, α}
    # Direction of flow is n with magnitude α
    aux.advection.u = α * n
end

function initial_condition!(
    ::Pseudo1D{n, α},
    state,
    aux,
    localgeo,
    t,
) where {n, α}
    ξn = dot(n, localgeo.coord)
    state.ρ = sin((ξn - α * t) * pi)
end
inhomogeneous_data!(::Val{0}, P::Pseudo1D, x...) = initial_condition!(P, x...)

function do_output(mpicomm, vtkdir, vtkstep, dgfvm, Q, Qe, model, testname)
    ## Name of the file that this MPI rank will write
    filename = @sprintf(
        "%s/%s_mpirank%04d_step%04d",
        vtkdir,
        testname,
        MPI.Comm_rank(mpicomm),
        vtkstep
    )

    statenames = flattenednames(vars_state(model, Prognostic(), eltype(Q)))
    exactnames = statenames .* "_exact"

    writevtk(filename, Q, dgfvm, statenames, Qe, exactnames)

    ## Generate the pvtu file for these vtk files
    if MPI.Comm_rank(mpicomm) == 0
        ## Name of the pvtu file
        pvtuprefix = @sprintf("%s/%s_step%04d", vtkdir, testname, vtkstep)

        ## Name of each of the ranks vtk files
        prefixes = ntuple(MPI.Comm_size(mpicomm)) do i
            @sprintf("%s_mpirank%04d_step%04d", testname, i - 1, vtkstep)
        end

        writepvtu(
            pvtuprefix,
            prefixes,
            (statenames..., exactnames...),
            eltype(Q),
        )

        @info "Done writing VTK: $pvtuprefix"
    end
end

function test_run(
    mpicomm,
    ArrayType,
    fvmethod,
    dim,
    topl,
    N,
    timeend,
    FT,
    dt,
    n,
    α,
    vtkdir,
    outputtime,
)

    grid = DiscontinuousSpectralElementGrid(
        topl,
        FloatType = FT,
        DeviceArray = ArrayType,
        polynomialorder = N,
    )
    bcs = (InhomogeneousBC{0}(),)
    model = AdvectionDiffusion{dim}(Pseudo1D{n, α}(), bcs, diffusion = false)
    dgfvm = DGFVModel(
        model,
        grid,
        fvmethod,
        RusanovNumericalFlux(),
        CentralNumericalFluxSecondOrder(),
        CentralNumericalFluxGradient();
        direction = EveryDirection(),
    )

    Q = init_ode_state(dgfvm, FT(0))

    lsrk = LSRK54CarpenterKennedy(dgfvm, Q; dt = dt, t0 = 0)

    eng0 = norm(Q)
    @info @sprintf """Starting
    norm(Q₀) = %.16e""" eng0

    # Set up the information callback
    starttime = Ref(Dates.now())
    cbinfo = EveryXWallTimeSeconds(60, mpicomm) do (s = false)
        if s
            starttime[] = Dates.now()
        else
            energy = norm(Q)
            @info @sprintf(
                """Update
                simtime = %.16e
                runtime = %s
                norm(Q) = %.16e""",
                gettime(lsrk),
                Dates.format(
                    convert(Dates.DateTime, Dates.now() - starttime[]),
                    Dates.dateformat"HH:MM:SS",
                ),
                energy
            )
        end
    end
    callbacks = (cbinfo,)
    if ~isnothing(vtkdir)
        # Create vtk dir
        mkpath(vtkdir)

        vtkstep = 0
        # Output initial step
        do_output(
            mpicomm,
            vtkdir,
            vtkstep,
            dgfvm,
            Q,
            Q,
            model,
            "advection_diffusion",
        )

        # Setup the output callback
        cbvtk = EveryXSimulationSteps(floor(outputtime / dt)) do
            vtkstep += 1
            Qe = init_ode_state(dgfvm, gettime(lsrk))
            do_output(
                mpicomm,
                vtkdir,
                vtkstep,
                dgfvm,
                Q,
                Qe,
                model,
                "advection_diffusion",
            )
        end
        callbacks = (callbacks..., cbvtk)
    end

    solve!(Q, lsrk; timeend = timeend, callbacks = callbacks)

    # Print some end of the simulation information
    engf = norm(Q)
    Qe = init_ode_state(dgfvm, FT(timeend))

    engfe = norm(Qe)
    errf = euclidean_distance(Q, Qe)
    @info @sprintf """Finished
    norm(Q)                 = %.16e
    norm(Q) / norm(Q₀)      = %.16e
    norm(Q) - norm(Q₀)      = %.16e
    norm(Q - Qe)            = %.16e
    norm(Q - Qe) / norm(Qe) = %.16e
    """ engf engf / eng0 engf - eng0 errf errf / engfe
    errf
end

using Test
let
    ClimateMachine.init()
    ArrayType = ClimateMachine.array_type()

    mpicomm = MPI.COMM_WORLD

    base_num_elem = 4

    expected_result = Dict()
    expected_result[2, 1, Float64, FVConstant()] = 1.0404261715459338e-01
    expected_result[2, 2, Float64, FVConstant()] = 5.5995868545685376e-02
    expected_result[2, 3, Float64, FVConstant()] = 2.9383695610072275e-02
    expected_result[2, 4, Float64, FVConstant()] = 1.5171779426843507e-02

    expected_result[2, 1, Float64, FVLinear()] = 8.3196657944903635e-02
    expected_result[2, 2, Float64, FVLinear()] = 3.9277132273521774e-02
    expected_result[2, 3, Float64, FVLinear()] = 1.7155773433218020e-02
    expected_result[2, 4, Float64, FVLinear()] = 7.6525022056819231e-03

    expected_result[3, 1, Float64, FVConstant()] = 9.6785620234054362e-02
    expected_result[3, 2, Float64, FVConstant()] = 5.3406412788842651e-02
    expected_result[3, 3, Float64, FVConstant()] = 2.8471535157235807e-02
    expected_result[3, 4, Float64, FVConstant()] = 1.4846239937398318e-02

    expected_result[3, 1, Float64, FVLinear()] = 8.5860120005258181e-02
    expected_result[3, 2, Float64, FVLinear()] = 4.2844889694123235e-02
    expected_result[3, 3, Float64, FVLinear()] = 1.9302295207100174e-02
    expected_result[3, 4, Float64, FVLinear()] = 8.6084633401356733e-03

    expected_result[2, 1, Float32, FVConstant()] = 1.0404255986213684e-01
    expected_result[2, 2, Float32, FVConstant()] = 5.5995877832174301e-02
    expected_result[2, 3, Float32, FVConstant()] = 2.9383875429630280e-02
    expected_result[2, 4, Float32, FVConstant()] = 1.5171864069998264e-02

    expected_result[2, 1, Float32, FVLinear()] = 8.3196602761745453e-02
    expected_result[2, 2, Float32, FVLinear()] = 3.9277125149965286e-02
    expected_result[2, 3, Float32, FVLinear()] = 1.7155680805444717e-02
    expected_result[2, 4, Float32, FVLinear()] = 7.6521718874573708e-03

    expected_result[3, 1, Float32, FVConstant()] = 9.6785508096218109e-02
    expected_result[3, 2, Float32, FVConstant()] = 5.3406376391649246e-02
    expected_result[3, 3, Float32, FVConstant()] = 2.8471505269408226e-02
    expected_result[3, 4, Float32, FVConstant()] = 1.4849635772407055e-02

    expected_result[3, 1, Float32, FVLinear()] = 8.5860058665275574e-02
    expected_result[3, 2, Float32, FVLinear()] = 4.2844854295253754e-02
    expected_result[3, 3, Float32, FVLinear()] = 1.9302234053611755e-02
    expected_result[3, 4, Float32, FVLinear()] = 8.6139924824237823e-03

    @testset "$(@__FILE__)" begin
        for FT in (Float64, Float32)
            numlevels =
                integration_testing ||
                ClimateMachine.Settings.integration_testing ?
                4 : 1
            result = zeros(FT, numlevels)
            for dim in 2:3
                N = (ntuple(j -> 4, dim - 1)..., 0)
                n =
                    dim == 2 ? SVector{3, FT}(1 / sqrt(2), 1 / sqrt(2), 0) :
                    SVector{3, FT}(1 / sqrt(3), 1 / sqrt(3), 1 / sqrt(3))
                α = FT(1)
                connectivity = dim == 2 ? :face : :full

                for fvmethod in (FVConstant(), FVLinear())
                    @info @sprintf """Configuration
                                      FT                = %s
                                      ArrayType         = %s
                                      FV Reconstruction = %s
                                      dims              = %d
                                      """ FT ArrayType fvmethod dim
                    for l in 1:numlevels
                        Ne = 2^(l - 1) * base_num_elem
                        brickrange = (
                            ntuple(
                                j -> range(FT(-1); length = Ne + 1, stop = 1),
                                dim - 1,
                            )...,
                            range(FT(-1); length = N[1] * Ne + 1, stop = 1),
                        )
                        periodicity = ntuple(j -> false, dim)
                        bc = ntuple(j -> (1, 1), dim)
                        topl = StackedBrickTopology(
                            mpicomm,
                            brickrange;
                            periodicity = periodicity,
                            boundary = bc,
                            connectivity = connectivity,
                        )
                        dt = (α / 4) / (Ne * max(1, maximum(N))^2)

                        timeend = FT(1 // 4)
                        outputtime = timeend

                        dt = outputtime / ceil(Int64, outputtime / dt)

                        vtkdir =
                            output ?
                            "vtk_advection" *
                            "_poly$(N)" *
                            "_dim$(dim)_$(ArrayType)_$(FT)" *
                            "_level$(l)" :
                            nothing
                        result[l] = test_run(
                            mpicomm,
                            ArrayType,
                            fvmethod,
                            dim,
                            topl,
                            N,
                            timeend,
                            FT,
                            dt,
                            n,
                            α,
                            vtkdir,
                            outputtime,
                        )
                        @test result[l] ≈
                              FT(expected_result[dim, l, FT, fvmethod])
                    end
                    @info begin
                        msg = ""
                        for l in 1:(numlevels - 1)
                            rate = log2(result[l]) - log2(result[l + 1])
                            msg *= @sprintf(
                                "\n  rate for level %d = %e\n",
                                l,
                                rate
                            )
                        end
                        msg
                    end
                end
            end
        end
    end
end

nothing

ext: jl


text:
# libsndfile has the ability to define a virtual IO interface where you provide
# callbacks for read, write, etc, and whenever the library wants to perform
# these operations it calls your functions. See http://www.mega-nerd.com/libsndfile/api.html#open_virtual
# for details.

# Here we define a set of functions that work with general IO types, and we
# have a parametric VIRTUAL_IO function that specializes these functions for a
# particular IO type, and creates a struct of the resulting c-callable function
# pointers.

# we don't in general know the stream length
function virtual_get_filelen(userdata)::sf_count_t
    io = unsafe_pointer_to_objref(userdata)
    # io is a LengthIO, which is a wrapper for a generic IO that also supports
    # the `length` function
    length(io)
end

function virtual_seek(offset, whence, userdata)::sf_count_t
    io = unsafe_pointer_to_objref(userdata)
    if whence == SF_SEEK_SET
        seek(io, offset)
        offset
    elseif whence == SF_SEEK_CUR
        cur = position(io)
        skip(io, offset)
        cur+offset
    elseif whence == SF_SEEK_END
        seekend(io)
        skip(io, offset)
        position(io)
    else
        throw(ArgumentError("Got `whence` value of $whence. Expected 0, 1, or 2"))
    end
end

function virtual_read(dest, count, userdata)::sf_count_t
    io = unsafe_pointer_to_objref(userdata)
    read = readbytes!(io, unsafe_wrap(Array, Ptr{UInt8}(dest), count))

    read
end

function virtual_write(src, count, userdata)::sf_count_t
    io = unsafe_pointer_to_objref(userdata)
    unsafe_write(io, src, count)
    count
end
function virtual_tell(userdata)::sf_count_t
    io = unsafe_pointer_to_objref(userdata)
    position(io)
end

# this contains a collection of function pointers that libsndfile uses to
# read and write data in a buffer
struct SF_VIRTUAL_IO
    get_filelen::Ptr{Cvoid}
    seek::Ptr{Cvoid}
    read::Ptr{Cvoid}
    write::Ptr{Cvoid}
    tell::Ptr{Cvoid}
end

# make a struct of function pointers where the userdata argument is a pointer of
# the specified type
function SF_VIRTUAL_IO(::Type{T}) where T<:IO
    SF_VIRTUAL_IO(
        @cfunction(virtual_get_filelen, sf_count_t, (Ptr{T}, )),
        @cfunction(virtual_seek,        sf_count_t, (sf_count_t, Int32, Ptr{T})),
        @cfunction(virtual_read,        sf_count_t, (Ptr{Cvoid}, sf_count_t, Ptr{T})),
        @cfunction(virtual_write,       sf_count_t, (Ptr{Cvoid}, sf_count_t, Ptr{T})),
        @cfunction(virtual_tell,        sf_count_t, (Ptr{T}, ))
    )
end


text:
"""
  module JCRHF
The module required for computation of the wave function using the *Restricted
Hartree-Fock* (RHF) method in a Self-Consistent Field (SCF) calculation. This
module will be used often, as the RHF wave function is often the zeroth-order
wave function for closed-shell systems.
"""
module JCRHF

#== RHF energy module ==#
Base.include(@__MODULE__,"energy/Energy.jl")
export Energy

#== RHF gradient module ==#
Base.include(@__MODULE__,"gradient/Gradient.jl")
export Gradient

#== RHF properties module ==#
Base.include(@__MODULE__,"properties/Properties.jl")
export Properties

end


text:
##### types.jl #####
# Created by Alexander Dowling

##############
##### Functions and Types for setup

mutable struct IrreducibleDegenerateSet
	c::Int64
	nElements::Float64
	lambda::Array{Float64,1}
	y::Array{Float64,1}
end

mutable struct DegenSettings
	includeBounds::Bool
	includeWeaklyActive::Bool
	removeFixedVar::Bool
	onlyCandidateSearch::Bool
	epsiActive::Float64
	epsiLambda::Float64
	lambdaM::Float64
	mySolver
	printSmallestSVD::Float64
end

function DegenSettings(mySolver)
	return DegenSettings(false, true, true, true, 1E-6, 1E-6, 1.0E5, mySolver, 0.0)
end

mutable struct DegenData

# Point to analyze
	x::Array{Float64,1}

# Jacobian expressed as a sparse matrix
	J_active

#	NLP evaluator
	d

# Full Jacobian (pull entries for bounds, if appropriate per settings)
	J::Array{Float64,1}
	iR::Array{Int64,1}
	jC::Array{Int64,1}

# Constraint information
	gLB::Array{Float64,1}
	gUB::Array{Float64,1}
	g::Array{Float64,1}

# Maps from active Jacobian to full Jacobian
	gMap::Array{Int64,1}
	bMap::Array{Int64,1}
	vMap::Array{Int64,1}

# Size info
	nLambda::Int64
	nVarActive::Int64
	nVar::Int64
	nConstr::Int64

end

function DegenData()
	return DegenData(Float64[],
						nothing,
						nothing,
						Float64[],
						Int64[],
						Int64[],
						Float64[],
						Float64[],
						Float64[],
						Int64[],
						Int64[],
						Int64[],
						0,
						0,
						0,
						0)
end

function DegenData(m::Model, f=stdout, status::Symbol=:Unknown)

	dd = DegenData()

# Select point to analyze
	dd.x = get_x(m, status)

	dd.nVar = length(dd.x)

# Evaluate constraint bounds
	(dd.gLB, dd.gUB) = JuMP.constraintbounds(m)

	if(!m.internalModelLoaded)
		JuMP.build(m)
	end

	if(typeof(m.internalModel) == MathProgBase.SolverInterface.NonlinearToLPQPBridge)
		processNonlinearModel!(m, dd, f)

	# TO DO: Update processLinearModel! to work with quadratic constraints
	elseif(typeof(m.internalModel) <: MathProgBase.SolverInterface.AbstractLinearQuadraticModel)
		# processLinearModel!(m, dd, f)
		processNonlinearModel!(m, dd, f)

	elseif(typeof(m.internalModel) <: MathProgBase.SolverInterface.AbstractNonlinearModel)
		processNonlinearModel!(m, dd, f)

	else
		println("Warning: Conic models are not yet supported!")
	end

	dd.nConstr = MathProgBase.numconstr(m)

	return dd

end

function get_x(m::Model, status::Symbol = :Unknown)
	if(m.internalModel === nothing || status == :InfeasibleOrUnbounded || status == :Unsolved || status == :Unknown)
		x = m.colVal
	else
	# Note: getsolution will throw an error if model is infeasible or unbounded with Gurobi
		x = MathProgBase.getsolution(m.internalModel)
	end

	return x
end

function processNonlinearModel!(m::Model, dd::DegenData, f=stdout)

# Initialize NLP evaluatorSetup

	print(f,"Setting up NLP evaluator... ")
	t0 = time()
	dd.d = JuMP.NLPEvaluator(m)
	tm = time() - t0
	println(f, string(tm, " seconds"))

	print(f,"Initializing... ")
	t0 = time()
	MathProgBase.initialize(dd.d, [:ExprGraph, :Jac])
	tm = time() - t0
	println(f, string(tm," seconds"))

# Evaluate constraints
	n = MathProgBase.numconstr(m)

	dd.g = zeros(n)
	MathProgBase.eval_g(dd.d,dd.g,dd.x)

# Query Jacobian structure

	# Rows: Constraints
	# Columns: Variables

	print(f,"Determining Jacobian structure... ")
	t0 = time()
	(dd.iR, dd.jC) = MathProgBase.jac_structure(dd.d)
	tm = time() - t0
	println(f,string(tm," seconds"))

	dd.J = zeros(length(dd.iR))

# Query Jacobian at point
	print(f,"Evaluating Jacobian... ")
	t0 = time()
	MathProgBase.eval_jac_g(dd.d,dd.J,dd.x)
	tm = time() - t0
	println(f,string(tm, " seconds"))

	return nothing

end

# TODO: Update to work with quadratic models.
function processLinearModel!(m::Model, dd::DegenData, f=stdout)

	# Jacobian as a SparseMatrixCSC
	if(!m.internalModelLoaded)
		JuMP.build(m)
	end
	A = MathProgBase.getconstrmatrix(internalmodel(m))

	# Translate to row, column, element form
	(dd.iR, dd.jC, dd.J) = findnz(A)

	# Get RHS of constraints
	# dd.g = MathProgBase.getconstrsolution(internalmodel(m))
	dd.g = A*dd.x

	return nothing

end


text:
# Copyright (c) 2017: Miles Lubin and contributors
# Copyright (c) 2017: Google Inc.
#
# Use of this source code is governed by an MIT-style license that can be found
# in the LICENSE.md file or at https://opensource.org/licenses/MIT.

"""
    abstract type AbstractBridge <: MOI.Bridges.AbstractBridge end

Subtype of [`MathOptInterface.Bridges.AbstractBridge`](@ref) for objective
bridges.
"""
abstract type AbstractBridge <: MOI.Bridges.AbstractBridge end

"""
    supports_objective_function(
        BT::Type{<:MOI.Bridges.Objective.AbstractBridge},
        F::Type{<:MOI.AbstractScalarFunction},
    )::Bool

Return a `Bool` indicating whether the bridges of type `BT` support bridging
objective functions of type `F`.

## Implementation notes

 * This method depends only on the type of the inputs, not the runtime values.
 * There is a default fallback, so you need only implement this method For
   objective functions that the bridge implements.
"""
function supports_objective_function(
    ::Type{<:AbstractBridge},
    ::Type{<:MOI.AbstractScalarFunction},
)
    return false
end

"""
    concrete_bridge_type(
        BT::Type{<:MOI.Bridges.Objective.AbstractBridge},
        F::Type{<:MOI.AbstractScalarFunction},
    )::Type

Return the concrete type of the bridge supporting objective functions of type
`F`.

This function can only be called if `MOI.supports_objective_function(BT, F)` is
`true`.
"""
function concrete_bridge_type(
    ::Type{BT},
    ::Type{<:MOI.AbstractScalarFunction},
) where {BT}
    return BT
end

function concrete_bridge_type(
    b::MOI.Bridges.AbstractBridgeOptimizer,
    F::Type{<:MOI.AbstractScalarFunction},
)
    return concrete_bridge_type(MOI.Bridges.bridge_type(b, F), F)
end

"""
    bridge_objective(
        BT::Type{<:MOI.Bridges.Objective.AbstractBridge},
        model::MOI.ModelLike,
        func::MOI.AbstractScalarFunction,
    )::BT

Bridge the objective function `func` using bridge `BT` to `model` and returns
a bridge object of type `BT`.

## Implementation notes

 * The bridge type `BT` must be a concrete type, that is, all the type
   parameters of the bridge must be set.
"""
function bridge_objective(
    ::Type{<:AbstractBridge},
    ::MOI.ModelLike,
    func::MOI.AbstractScalarFunction,
)
    return throw(
        MOI.UnsupportedAttribute(MOI.ObjectiveFunction{typeof(func)}()),
    )
end

function MOI.set(
    ::MOI.ModelLike,
    ::MOI.ObjectiveSense,
    bridge::AbstractBridge,
    ::MOI.OptimizationSense,
)
    return throw(
        ArgumentError(
            "Objective bridge of type `$(typeof(bridge))` does not support " *
            "modifying the objective sense. As a workaround, set the sense " *
            "to `MOI.FEASIBILITY_SENSE` to clear the objective function and " *
            "bridges.",
        ),
    )
end

function MOI.get(
    ::MOI.ModelLike,
    ::MOI.ObjectiveFunction,
    bridge::AbstractBridge,
)
    return throw(
        ArgumentError(
            "ObjectiveFunction bridge of type `$(typeof(bridge))` does not" *
            " support getting the objective function.",
        ),
    )
end

function MOI.delete(::MOI.ModelLike, bridge::AbstractBridge)
    return throw(
        ArgumentError(
            "`MOI.delete` not implemented for `ObjectiveFunction` bridges of " *
            "type `$(typeof(bridge))`",
        ),
    )
end


text:
# This file was generated by the Julia Swagger Code Generator
# Do not modify this file directly. Modify the swagger specification instead.

mutable struct IoK8sApimachineryPkgApisMetaV1LabelSelector <: SwaggerModel
    matchExpressions::Any # spec type: Union{ Nothing, Vector{IoK8sApimachineryPkgApisMetaV1LabelSelectorRequirement} } # spec name: matchExpressions
    matchLabels::Any # spec type: Union{ Nothing, Dict{String, String} } # spec name: matchLabels

    function IoK8sApimachineryPkgApisMetaV1LabelSelector(;matchExpressions=nothing, matchLabels=nothing)
        o = new()
        validate_property(IoK8sApimachineryPkgApisMetaV1LabelSelector, Symbol("matchExpressions"), matchExpressions)
        setfield!(o, Symbol("matchExpressions"), matchExpressions)
        validate_property(IoK8sApimachineryPkgApisMetaV1LabelSelector, Symbol("matchLabels"), matchLabels)
        setfield!(o, Symbol("matchLabels"), matchLabels)
        o
    end
end # type IoK8sApimachineryPkgApisMetaV1LabelSelector

const _property_map_IoK8sApimachineryPkgApisMetaV1LabelSelector = Dict{Symbol,Symbol}(Symbol("matchExpressions")=>Symbol("matchExpressions"), Symbol("matchLabels")=>Symbol("matchLabels"))
const _property_types_IoK8sApimachineryPkgApisMetaV1LabelSelector = Dict{Symbol,String}(Symbol("matchExpressions")=>"Vector{IoK8sApimachineryPkgApisMetaV1LabelSelectorRequirement}", Symbol("matchLabels")=>"Dict{String, String}")
Base.propertynames(::Type{ IoK8sApimachineryPkgApisMetaV1LabelSelector }) = collect(keys(_property_map_IoK8sApimachineryPkgApisMetaV1LabelSelector))
Swagger.property_type(::Type{ IoK8sApimachineryPkgApisMetaV1LabelSelector }, name::Symbol) = Union{Nothing,eval(Base.Meta.parse(_property_types_IoK8sApimachineryPkgApisMetaV1LabelSelector[name]))}
Swagger.field_name(::Type{ IoK8sApimachineryPkgApisMetaV1LabelSelector }, property_name::Symbol) =  _property_map_IoK8sApimachineryPkgApisMetaV1LabelSelector[property_name]

function check_required(o::IoK8sApimachineryPkgApisMetaV1LabelSelector)
    true
end

function validate_property(::Type{ IoK8sApimachineryPkgApisMetaV1LabelSelector }, name::Symbol, val)
end


text:
println("yo", "lo")
println("Hello, world!")
println("string ", 42)
println(true, false)
print(4, 2)
println(42)
print(true)
print(42)
print(false)
println()
println("Hello, world!")
println("Hello", ",", " ", "world", "!")
println(42, "newline\nafter newline\t", nothing)
println("\\\\")
println("\\\"")

max_stars_repo_name: gabriel-doriath-dohler/pjulia

max_issues_repo_name: gabriel-doriath-dohler/pjulia

max_forks_repo_name: gabriel-doriath-dohler/pjulia


text:
using GaussianProcesses
using GPR
using Optim
using ConstrainedDynamicsVis
using ConstrainedDynamics
using LineSearches
using Statistics
using JSON

function experimentCPMin(config, id)
    traindfs, testdfs = config["datasets"]  # Each thread operates on its own dataset -> no races
    traindf = traindfs.df[id][shuffle(1:nrow(traindfs.df[id]))[1:config["trainsamples"]], :]
    testdf = testdfs.df[id][shuffle(1:nrow(testdfs.df[id]))[1:config["testsamples"]], :]
    mechanism = cartpole(1, Δt=0.01, threadlock=config["mechanismlock"])[2]

    xtrain_old = reduce(hcat, [max2mincoordinates(CState(x), mechanism) for x in traindf.sold])
    xtrain_curr = [max2mincoordinates(CState(x), mechanism) for x in traindf.scurr]
    ytrain = [[s[id] for s in xtrain_curr] for id in [2,4]]  # v12, ω21
    xtest_old = [max2mincoordinates(CState(x),mechanism) for x in testdf.sold]
    xtest_future = [CState(x) for x in testdf.sfuture]
    # intentionally not converting xtest_future since final comparison is done in maximal coordinates

    stdx = std(xtrain_old, dims=2)
    stdx[stdx .== 0] .= 1000
    params = [100., (50 ./(stdx))...]
    params = params .+ (5rand(length(params)) .- 0.999) .* params

    predictedstates = Vector{CState{Float64,2}}()
    gps = Vector{GPE}()
    for yi in ytrain
        kernel = SEArd(log.(params[2:end]), log(params[1]))
        gp = GP(xtrain_old, yi, MeanZero(), kernel)
        GaussianProcesses.optimize!(gp, LBFGS(linesearch = BackTracking(order=2)), Optim.Options(time_limit=10.))
        push!(gps, gp)
    end

    for i in 1:length(xtest_old)
        predictedstate = predictdynamicsmin(mechanism, "CP", gps, xtest_old[i], config["simsteps"])
        push!(predictedstates, predictedstate)
    end
    return predictedstates, xtest_future, params
end


text:
#####
##### Hybrid mesh
#####

struct ExtrudedFiniteDifferenceSpace{
    S <: Staggering,
    H <: AbstractSpace,
    M,
    G,
} <: AbstractSpace
    staggering::S
    horizontal_space::H
    vertical_mesh::M
    center_local_geometry::G
    face_local_geometry::G
end

const CenterExtrudedFiniteDifferenceSpace =
    ExtrudedFiniteDifferenceSpace{CellCenter}

const FaceExtrudedFiniteDifferenceSpace =
    ExtrudedFiniteDifferenceSpace{CellFace}

function ExtrudedFiniteDifferenceSpace{S}(
    space::ExtrudedFiniteDifferenceSpace,
) where {S <: Staggering}
    ExtrudedFiniteDifferenceSpace(
        S(),
        space.horizontal_space,
        space.vertical_mesh,
        space.center_local_geometry,
        space.face_local_geometry,
    )
end

local_geometry_data(space::CenterExtrudedFiniteDifferenceSpace) =
    space.center_local_geometry

local_geometry_data(space::FaceExtrudedFiniteDifferenceSpace) =
    space.face_local_geometry

function ExtrudedFiniteDifferenceSpace(
    horizontal_space::H,
    vertical_space::V,
) where {H <: AbstractSpace, V <: FiniteDifferenceSpace}
    staggering = vertical_space.staggering
    vertical_mesh = vertical_space.mesh
    center_local_geometry =
        product_geometry.(
            horizontal_space.local_geometry,
            vertical_space.center_local_geometry,
        )
    face_local_geometry =
        product_geometry.(
            horizontal_space.local_geometry,
            vertical_space.face_local_geometry,
        )
    return ExtrudedFiniteDifferenceSpace(
        staggering,
        horizontal_space,
        vertical_mesh,
        center_local_geometry,
        face_local_geometry,
    )
    return nothing
end

quadrature_style(space::ExtrudedFiniteDifferenceSpace) =
    space.horizontal_space.quadrature_style

topology(space::ExtrudedFiniteDifferenceSpace) = space.horizontal_space.topology

slab(space::ExtrudedFiniteDifferenceSpace, v, h) =
    slab(space.horizontal_space, v, h)

column(space::ExtrudedFiniteDifferenceSpace, i, j, h) = FiniteDifferenceSpace(
    space.staggering,
    space.vertical_mesh,
    column(space.center_local_geometry, i, j, h),
    column(space.face_local_geometry, i, j, h),
)

nlevels(space::CenterExtrudedFiniteDifferenceSpace) =
    size(space.center_local_geometry, 4)

nlevels(space::FaceExtrudedFiniteDifferenceSpace) =
    size(space.face_local_geometry, 4)

left_boundary_name(space::ExtrudedFiniteDifferenceSpace) =
    propertynames(space.vertical_mesh.boundaries)[1]

right_boundary_name(space::ExtrudedFiniteDifferenceSpace) =
    propertynames(space.vertical_mesh.boundaries)[2]

function blockmat(
    a::Geometry.Axis2Tensor{
        FT,
        Tuple{Geometry.Cartesian1Axis, Geometry.Covariant1Axis},
        SMatrix{1, 1, FT, 1},
    },
    b::Geometry.Axis2Tensor{
        FT,
        Tuple{Geometry.Cartesian3Axis, Geometry.Covariant3Axis},
        SMatrix{1, 1, FT, 1},
    },
) where {FT}
    A = Geometry.components(a)
    B = Geometry.components(b)
    Geometry.AxisTensor(
        (Geometry.Cartesian13Axis(), Geometry.Covariant13Axis()),
        SMatrix{2, 2}(A[1, 1], zero(FT), zero(FT), B[1, 1]),
    )
end

function blockmat(
    a::Geometry.Axis2Tensor{
        FT,
        Tuple{Geometry.Cartesian12Axis, Geometry.Covariant12Axis},
        SMatrix{2, 2, FT, 4},
    },
    b::Geometry.Axis2Tensor{
        FT,
        Tuple{Geometry.Cartesian3Axis, Geometry.Covariant3Axis},
        SMatrix{1, 1, FT, 1},
    },
) where {FT}
    A = Geometry.components(a)
    B = Geometry.components(b)
    Geometry.AxisTensor(
        (Geometry.Cartesian123Axis(), Geometry.Covariant123Axis()),
        SMatrix{3, 3}(
            A[1, 1],
            A[1, 2],
            zero(FT),
            A[2, 1],
            A[2, 2],
            zero(FT),
            zero(FT),
            zero(FT),
            B[1, 1],
        ),
    )
end

function product_geometry(
    horizontal_local_geometry::Geometry.LocalGeometry,
    vertical_local_geometry::Geometry.LocalGeometry,
)
    coordinates = Geometry.product_coordinates(
        horizontal_local_geometry.coordinates,
        vertical_local_geometry.coordinates,
    )
    J = horizontal_local_geometry.J * vertical_local_geometry.J
    WJ = horizontal_local_geometry.WJ * vertical_local_geometry.WJ
    ∂x∂ξ =
        blockmat(horizontal_local_geometry.∂x∂ξ, vertical_local_geometry.∂x∂ξ)
    ∂ξ∂x = inv(∂x∂ξ)
    return Geometry.LocalGeometry(coordinates, J, WJ, ∂x∂ξ, ∂ξ∂x)
end

function eachslabindex(cspace::CenterExtrudedFiniteDifferenceSpace)
    h_iter = eachslabindex(cspace.horizontal_space)
    Nv = size(cspace.center_local_geometry, 4)
    return Iterators.product(1:Nv, h_iter)
end
function eachslabindex(fspace::FaceExtrudedFiniteDifferenceSpace)
    h_iter = eachslabindex(fspace.horizontal_space)
    Nv = size(fspace.face_local_geometry, 4)
    return Iterators.product(1:Nv, h_iter)
end


text:
# Loss functions.
module LossFunctions

using Statistics
using GradientBoost.Util

export LossFunction,
       loss,
       negative_gradient,
       minimizing_scalar,
       LeastSquares,
       LeastAbsoluteDeviation,
       BinomialDeviance

# Loss function.
#@ abstract LossFunction
abstract type LossFunction end
# Calculates loss.
#
# @param lf Loss function.
# @param y True response.
# @param y_pred Approximated response.
# @return Loss.
loss(lf::LossFunction, y, y_pred) = err_must_be_overriden()

# Calculates negative gradient of loss function.
#
# @param lf Loss function.
# @param y True response.
# @param y_pred Approximated response.
# @return Vector of negative gradient residuals.
negative_gradient(lf::LossFunction, y, y_pred) = err_must_be_overriden()

# Finds scalar value c that minimizes loss L(y, c).
#
# @param lf Loss function.
# @param y True response.
# @return Scalar value.
minimizing_scalar(lf::LossFunction, y) = err_must_be_overriden()

# LeastSquares
struct LeastSquares <: LossFunction end

function loss(lf::LeastSquares, y, y_pred)
  mean((y .- y_pred) .^ 2.0)
end

function negative_gradient(lf::LeastSquares, y, y_pred)
  y .- y_pred
end

function minimizing_scalar(lf::LeastSquares, y)
  mean(y)
end

# LeastAbsoluteDeviation
struct LeastAbsoluteDeviation <: LossFunction end

function loss(lf::LeastAbsoluteDeviation, y, y_pred)
  mean(abs(y .- y_pred))
end

function negative_gradient(lf::LeastAbsoluteDeviation, y, y_pred)
  sign(y .- y_pred)
end

function minimizing_scalar(lf::LeastAbsoluteDeviation, y)
  median(y)
end

# Binomial Deviance (Two Classes {0,1})
struct BinomialDeviance <: LossFunction end

function loss(lf::BinomialDeviance, y, y_pred)
  -2.0 .* mean(y .* y_pred .- log(1.0 .+ exp(y_pred)))
end

function negative_gradient(lf::BinomialDeviance, y, y_pred)
  y .- 1.0 ./ (1.0 .+ exp(-y_pred))
end

function minimizing_scalar(lf::BinomialDeviance, y)
  y_sum = sum(y)
  y_length = length(y)
  log(y_sum / (y_length - y_sum))
end

end # module


text:
export
        SceneOverlay,
        TextOverlay,
        Overwash,

        NeighborsOverlay,
        CarFollowingStatsOverlay,
        MOBILOverlay,
        CollisionOverlay,
        MarkerDistOverlay,
        HistogramOverlay,

        TextParams,
        drawtext

abstract type SceneOverlay end

function render(scene::EntityFrame{S,D,I}, roadway::R, overlays::AbstractVector{O};
    canvas_width::Int=DEFAULT_CANVAS_WIDTH,
    canvas_height::Int=DEFAULT_CANVAS_HEIGHT,
    rendermodel::RenderModel=RenderModel(),
    cam::Camera=SceneFollowCamera(),
    car_colors::Dict{I,Colorant}=Dict{I,Colorant}(),
    ) where {S,D,I,O<:SceneOverlay,R}

    s = CairoRGBSurface(canvas_width, canvas_height)
    ctx = creategc(s)
    clear_setup!(rendermodel)

    render!(rendermodel, roadway)
    render!(rendermodel, scene, car_colors=car_colors)

    for overlay in overlays
        render!(rendermodel, overlay, scene, roadway)
    end

    camera_set!(rendermodel, cam, scene, roadway, canvas_width, canvas_height)

    render(rendermodel, ctx, canvas_width, canvas_height)
    s
end

mutable struct TextParams
    size::Int
    color::Colorant
    x::Int
    y_start::Int
    y_jump::Int

    function TextParams(;
        size::Int=12,
        color::Colorant=colorant"white",
        x::Int=20,
        y_start::Int=20,
        y_jump::Int=round(Int, size*1.2),
        )

        retval = new()
        retval.size = size
        retval.color = color
        retval.x = x
        retval.y_start = y_start
        retval.y_jump = y_jump
        retval
    end
end
function drawtext(text::AbstractString, y::Int, rendermodel::RenderModel, t::TextParams; incameraframe::Bool=false)
    add_instruction!(rendermodel, render_text, (text, t.x, y, t.size, t.color), incameraframe=incameraframe)
    y + t.y_jump
end

@with_kw mutable struct TextOverlay <: SceneOverlay
    text::Vector{String}
    color::Colorant = colorant"white"
    font_size::Int = 10 # [pix]
    pos::VecE2 = VecE2(10, font_size)
    line_spacing::Float64 = 1.5 # multiple of font_size
    incameraframe=false
end
function render!(rendermodel::RenderModel, overlay::TextOverlay, scene::EntityFrame{S,D,I}, roadway::R) where {S,D,I,R}
    x = overlay.pos.x
    y = overlay.pos.y
    y_jump = overlay.line_spacing*overlay.font_size
    for line in overlay.text
        add_instruction!(rendermodel, render_text, (line, x, y, overlay.font_size, overlay.color), incameraframe=overlay.incameraframe)
        y += y_jump
    end
    rendermodel
end

# method for new interface
function render!(rendermodel::RenderModel, overlay::TextOverlay, scene)
    x = overlay.pos.x
    y = overlay.pos.y
    y_jump = overlay.line_spacing*overlay.font_size
    for line in overlay.text
        add_instruction!(rendermodel, render_text, (line, x, y, overlay.font_size, overlay.color), incameraframe=overlay.incameraframe)
        y += y_jump
    end
    rendermodel
end

mutable struct Overwash <: SceneOverlay
    color::Colorant
end
function render!(rendermodel::RenderModel, overlay::Overwash, scene::EntityFrame{S,D,I}, roadway::R) where {S,D,I,R}
    add_instruction!(rendermodel, render_paint, (overlay.color,))
    rendermodel
end

"""
    HistogramOverlay

Display a bar at the specified position `pos`, the bar is of size `width`, `height` and is filled up to a given proportion of its height.
The fill proportion is set using `val`, it should be a number between 0 and 1. If it is 0, the bar is not filled, if it is 1 it is filled to the top.
"""
@with_kw mutable struct HistogramOverlay <: SceneOverlay
    pos::VecE2{Float64} = VecE2(0.,0.)
    incameraframe::Bool = true
    label::String = "histogram"
    val::Float64 = 0.5 # should be between 0 and 1
    width::Float64 = 2.
    height::Float64 = 5.
    fill_color::Colorant = colorant"blue"
    line_color::Colorant = colorant"white"
    font_size::Int64 = 15 # [pix]
    label_pos::VecSE2{Float64} = pos + VecSE2(0., -height/2)
end

function AutoViz.render!(rendermodel::RenderModel, overlay::HistogramOverlay, scene::Scene, roadway::R) where R
    # render value
    add_instruction!(rendermodel, render_rect, (overlay.pos.x, overlay.pos.y, overlay.width, overlay.val*overlay.height,overlay.fill_color, true, false), incameraframe=overlay.incameraframe)
    # render histogram outline
    add_instruction!(rendermodel, render_rect, (overlay.pos.x, overlay.pos.y, overlay.width, overlay.height, overlay.line_color), incameraframe=overlay.incameraframe)
     # label
    add_instruction!(rendermodel, render_text, (overlay.label, overlay.label_pos.x, overlay.label_pos.y, overlay.font_size, overlay.line_color), incameraframe=overlay.incameraframe)
end


text:
#InspectDR: Base functionnality and types for Cairo layer
#-------------------------------------------------------------------------------

#=TODO
-Fix glyph size for "constant area" (appearance).  Currently, algorithms use
 "constant bounding box" to avoid repeating complex math (ex: h=sqrt(a^2+b^2)).
 Solution: Create glyph object that takes Vector{Point2D} - or something
 similar.  That way, complex math is done only once per curve.
=#

#==Constants
===============================================================================#

#==Main types
===============================================================================#
#2D plot canvas; basic info:
type PCanvas2D <: PlotCanvas
	ctx::CairoContext
	bb::BoundingBox #Entire canvas
	graphbb::BoundingBox #Graph portion
	ext::PExtents2D #Extents of graph portion
	xf::Transform2D #Transform used to render data
end
PCanvas2D(ctx, bb, graphbb, ext) =
	PCanvas2D(ctx, bb, graphbb, ext, Transform2D(ext, graphbb))

immutable CGlyph{GTYPE}; end #Dispatchable glyph object
CGlyph(gtype::Symbol) = CGlyph{gtype}()

#==Basic rendering
===============================================================================#

#Reset context to known state:
function _reset(ctx::CairoContext)
	Cairo.set_source(ctx, COLOR_BLACK)
	Cairo.set_line_width(ctx, 1);
	Cairo.set_dash(ctx, Float64[], 0)
end

#Clears a rectangle-shaped area with a solid color
function clear(ctx::CairoContext, bb::BoundingBox, color::Colorant=COLOR_WHITE)
	Cairo.set_source(ctx, color)
	Cairo.rectangle(ctx, bb)
	Cairo.fill(ctx)
end

#Apply transform to Cairo (when easier to do so):
function setxfrm(ctx::CairoContext, xf::Transform2D)
	xorig = xf.x0*xf.xs
	yorig = xf.y0*xf.ys
	Cairo.translate(ctx, xorig, yorig)
	Cairo.scale(ctx, xf.xs, xf.ys)
end

#==Rendering Glyphs
===============================================================================#

function getglyphcolor(glyph::GlyphAttributes, line::LineAttributes)
	c = glyph.color
	if nothing == c
		c = line.color
	end
	return c
end

function getglyphfill(glyph::GlyphAttributes)
	fill = glyph.fillcolor
	if COLOR_TRANSPARENT == fill #Small optimization
		fill = nothing
	end
	return fill
end

#Conditionnaly render fill (preserve path for stroke)
renderfill(ctx::CairoContext, fill::Void) = nothing
function renderfill(ctx::CairoContext, fill::Colorant)
	Cairo.save(ctx) #-----
	Cairo.set_source(ctx, fill)
	Cairo.fill_preserve(ctx)
	Cairo.restore(ctx) #-----
end

function drawglyph{T}(ctx::CairoContext, ::CGlyph{T}, pt::Point2D, size::DReal, fill)
	warn("Glyph shape not supported: $T")
end

#= Supported:
	:square, :diamond,
	:uarrow, :darrow, :larrow, :rarrow, #usually triangles
	:cross, :+, :diagcross, :x,
	:circle, :o, :star, :*,
=#

function drawglyph(ctx::CairoContext, ::CGlyph{:circle}, pt::Point2D, size::DReal, fill)
	radius = size/2
	Cairo.arc(ctx, pt.x, pt.y, radius, 0, 2pi)
	renderfill(ctx, fill)
	Cairo.stroke(ctx)
end
drawglyph(ctx::CairoContext, ::CGlyph{:o}, pt::Point2D, size::DReal, fill) =
	drawglyph(ctx, CGlyph{:circle}(), pt, size, fill)

function drawglyph(ctx::CairoContext, ::CGlyph{:square}, pt::Point2D, size::DReal, fill)
	elen = size #full edge length
	hlen = elen/2 #halflength
	Cairo.rectangle(ctx, pt.x-hlen, pt.y-hlen, elen, elen)
	renderfill(ctx, fill)
	Cairo.stroke(ctx)
end

function drawglyph(ctx::CairoContext, ::CGlyph{:cross}, pt::Point2D, size::DReal, fill)
	elen = size*sqrt(2) #full edge length
	hlen = elen/2 #halflength
	Cairo.move_to(ctx, pt.x, pt.y-hlen)
	Cairo.rel_line_to(ctx, 0, elen)
	Cairo.stroke(ctx)
	Cairo.move_to(ctx, pt.x-hlen, pt.y)
	Cairo.rel_line_to(ctx, elen, 0)
	Cairo.stroke(ctx)
end
drawglyph(ctx::CairoContext, ::CGlyph{:+}, pt::Point2D, size::DReal, fill) =
	drawglyph(ctx, CGlyph{:cross}(), pt, size, fill)

function drawglyph(ctx::CairoContext, ::CGlyph{:diagcross}, pt::Point2D, size::DReal, fill)
	elen = size #full edge length
	hlen = elen/2 #halflength
	Cairo.move_to(ctx, pt.x-hlen, pt.y-hlen)
	Cairo.rel_line_to(ctx, elen, elen)
	Cairo.stroke(ctx)
	Cairo.move_to(ctx, pt.x-hlen, pt.y+hlen)
	Cairo.rel_line_to(ctx, elen, -elen)
	Cairo.stroke(ctx)
end
drawglyph(ctx::CairoContext, ::CGlyph{:x}, pt::Point2D, size::DReal, fill) =
	drawglyph(ctx, CGlyph{:diagcross}(), pt, size, fill)

function drawglyph(ctx::CairoContext, ::CGlyph{:star}, pt::Point2D, size::DReal, fill)
	elen = size*sqrt(2) #full edge length
	hlen = elen/2 #halflength

	Cairo.move_to(ctx, pt.x, pt.y-hlen)
	Cairo.rel_line_to(ctx, 0, elen)
	Cairo.stroke(ctx)
	Cairo.move_to(ctx, pt.x-hlen, pt.y)
	Cairo.rel_line_to(ctx, elen, 0)
	Cairo.stroke(ctx)

	elen = size #full edge length
	hlen = elen/2 #halflength

	Cairo.move_to(ctx, pt.x-hlen, pt.y-hlen)
	Cairo.rel_line_to(ctx, elen, elen)
	Cairo.stroke(ctx)
	Cairo.move_to(ctx, pt.x-hlen, pt.y+hlen)
	Cairo.rel_line_to(ctx, elen, -elen)
	Cairo.stroke(ctx)
end
drawglyph(ctx::CairoContext, ::CGlyph{:*}, pt::Point2D, size::DReal, fill) =
	drawglyph(ctx, CGlyph{:star}(), pt, size, fill)

function drawglyph(ctx::CairoContext, ::CGlyph{:diamond}, pt::Point2D, size::DReal, fill)
	elen = size #full edge length
	hlen = elen/2 #halflength
	Cairo.move_to(ctx, pt.x, pt.y-hlen)
	Cairo.rel_line_to(ctx, hlen, hlen)
	Cairo.rel_line_to(ctx, -hlen, hlen)
	Cairo.rel_line_to(ctx, -hlen, -hlen)
	Cairo.close_path(ctx)
	renderfill(ctx, fill)
	Cairo.stroke(ctx)
end

#dir=1: up, -1: down
function drawglyph_varrow(ctx::CairoContext, pt::Point2D, size::DReal, dir::Int, fill)
	elen = size #full edge length
	hlen = elen/2 #halflength
	dlen = dir*elen #directional edge
	Cairo.move_to(ctx, pt.x, pt.y-dir*hlen)
	Cairo.rel_line_to(ctx, hlen, dlen)
	Cairo.rel_line_to(ctx, -elen, 0)
	Cairo.close_path(ctx)
	renderfill(ctx, fill)
	Cairo.stroke(ctx)
end

drawglyph(ctx::CairoContext, ::CGlyph{:uarrow}, pt::Point2D, size::DReal, fill) =
	drawglyph_varrow(ctx, pt, size, 1, fill)
drawglyph(ctx::CairoContext, ::CGlyph{:darrow}, pt::Point2D, size::DReal, fill) =
	drawglyph_varrow(ctx, pt, size, -1, fill)

#dir=1: right, -1: left
function drawglyph_harrow(ctx::CairoContext, pt::Point2D, size::DReal, dir::Int, fill)
	elen = size #full edge length
	hlen = elen/2 #halflength
	dlen = dir*elen #directional edge
	Cairo.move_to(ctx, pt.x+dir*hlen, pt.y)
	Cairo.rel_line_to(ctx, -dlen, hlen)
	Cairo.rel_line_to(ctx, 0, -elen)
	Cairo.close_path(ctx)
	renderfill(ctx, fill)
	Cairo.stroke(ctx)
end

drawglyph(ctx::CairoContext, ::CGlyph{:larrow}, pt::Point2D, size::DReal, fill) =
	drawglyph_harrow(ctx, pt, size, -1, fill)
drawglyph(ctx::CairoContext, ::CGlyph{:rarrow}, pt::Point2D, size::DReal, fill) =
	drawglyph_harrow(ctx, pt, size, 1, fill)

#Correctly displays a glyph, given wfrm properties.
function drawglyph_safe(ctx::CairoContext, wfrm::DWaveform, pt::Point2D)
	if wfrm.glyph.shape != :none
		_glyph = CGlyph(wfrm.glyph.shape)
		setlinestyle(ctx, :solid, Float64(wfrm.line.width))
		linecolor = getglyphcolor(wfrm.glyph, wfrm.line)
		Cairo.set_source(ctx, linecolor)
		fill = getglyphfill(wfrm.glyph)
		gsize = Float64(wfrm.glyph.size)

		drawglyph(ctx, _glyph, pt, gsize, fill)
	end
end

#==Rendering base plot elements
===============================================================================#

#Render main plot annotation (titles, axis labels, ...)
#-------------------------------------------------------------------------------
function render(canvas::PCanvas2D, a::Annotation, lyt::Layout)
	const ctx = canvas.ctx
	const bb = canvas.bb
	const graph = canvas.graphbb
	const TIMESTAMP_OFFSET = 3

	xcenter = (graph.xmin+graph.xmax)/2
	ycenter = (graph.ymin+graph.ymax)/2

	#Title
	pt = Point2D(xcenter, bb.ymin+lyt.htitle/2)
	render(ctx, a.title, pt, lyt.fnttitle, align=ALIGN_HCENTER|ALIGN_VCENTER)

	#X-axis
	pt = Point2D(xcenter, bb.ymax-lyt.haxlabel/2)
	render(ctx, a.xlabel, pt, lyt.fntaxlabel, align=ALIGN_HCENTER|ALIGN_VCENTER)

	#Y-axis
	pt = Point2D(bb.xmin+lyt.waxlabel/2, ycenter)
	render(ctx, a.ylabel, pt, lyt.fntaxlabel, align=ALIGN_HCENTER|ALIGN_VCENTER, angle=-π/2)

	#Time stamp
	if lyt.showtimestamp
		pt = Point2D(bb.xmax-TIMESTAMP_OFFSET, bb.ymax-TIMESTAMP_OFFSET)
		render(ctx, a.timestamp, pt, lyt.fnttime, align=ALIGN_RIGHT|ALIGN_BOTTOM)
	end
end

#Render frame around graph
#-------------------------------------------------------------------------------
function render_graphframe(canvas::PCanvas2D)
	const ctx = canvas.ctx
	Cairo.set_source(ctx, COLOR_BLACK)
	Cairo.set_line_width(ctx, 2);
	Cairo.rectangle(ctx, canvas.graphbb)
	Cairo.stroke(ctx)
end

#==Render grid
===============================================================================#
render_vlines(ctx::CairoContext, graphbb::BoundingBox, xf::Transform2D, lyt::Layout, xlines::AbstractGridLines) = nothing
function render_vlines(ctx::CairoContext, graphbb::BoundingBox, xf::Transform2D, lyt::Layout, xlines::GridLines)
	if lyt.grid.vmajor
		setlinestyle(ctx, :dash, GRID_MAJOR_WIDTH)
		Cairo.set_source(ctx, GRID_MAJOR_COLOR)
		for xline in xlines.major
			x = ptmap(xf, Point2D(xline, 0)).x
			drawline(ctx, Point2D(x, graphbb.ymin), Point2D(x, graphbb.ymax))
		end
	end
	if lyt.grid.vminor
		setlinestyle(ctx, :dash, GRID_MINOR_WIDTH)
		Cairo.set_source(ctx, GRID_MINOR_COLOR)
		for xline in xlines.minor
			x = ptmap(xf, Point2D(xline, 0)).x
			drawline(ctx, Point2D(x, graphbb.ymin), Point2D(x, graphbb.ymax))
		end
	end
end
render_hlines(ctx::CairoContext, graphbb::BoundingBox, xf::Transform2D, lyt::Layout, ylines::AbstractGridLines) = nothing
function render_hlines(ctx::CairoContext, graphbb::BoundingBox, xf::Transform2D, lyt::Layout, ylines::GridLines)
	if lyt.grid.hmajor
		setlinestyle(ctx, :dash, GRID_MAJOR_WIDTH)
		Cairo.set_source(ctx, GRID_MAJOR_COLOR)
		for yline in ylines.major
			y = ptmap(xf, Point2D(0, yline)).y
			drawline(ctx, Point2D(graphbb.xmin, y), Point2D(graphbb.xmax, y))
		end
	end
	if lyt.grid.hminor
		setlinestyle(ctx, :dash, GRID_MINOR_WIDTH)
		Cairo.set_source(ctx, GRID_MINOR_COLOR)
		for yline in ylines.minor
			y = ptmap(xf, Point2D(0, yline)).y
			drawline(ctx, Point2D(graphbb.xmin, y), Point2D(graphbb.xmax, y))
		end
	end
end

function render_grid(canvas::PCanvas2D, lyt::Layout, grid::GridRect)
	const ctx = canvas.ctx
	Cairo.save(ctx) #-----
	render_vlines(ctx, canvas.graphbb, canvas.xf, lyt, grid.xlines)
	render_hlines(ctx, canvas.graphbb, canvas.xf, lyt, grid.ylines)
	Cairo.restore(ctx) #-----
end

#==Render ticks
===============================================================================#

#Default label:
function render_ticklabel(ctx::CairoContext, val::DReal, pt::Point2D, font::Font, align::CAlignment, fmt::TickLabelFormatting, ::AxisScale)
	tstr = ""
	if false #Old display system.
		#TODO: deprecate
		tstr = "$val"
		if length(tstr) > 7 #HACK!
			tstr = @sprintf("%0.1e", val)
		end
	else
		tstr = formatted(val, fmt.fmt, showexp=!fmt.splitexp)
	end
	render(ctx, tstr, pt, font, align=align)
end

render_ticklabel(ctx::CairoContext, val::DReal, pt::Point2D, font::Font, align::CAlignment, fmt::TickLabelFormatting, scale::AxisScale{:log10}) =
	render_power(ctx, 10, val, pt, font, align)

function render_axisscalelabel(ctx::CairoContext, pt::Point2D, font::Font, align::CAlignment, fmt::TickLabelFormatting, ::AxisScale)
	tstr = formatted_exp(fmt.fmt)
	render(ctx, tstr, pt, font, align=align)
end

#Render ticks: Well-defined GridLines
#-------------------------------------------------------------------------------
function render_xticks(ctx::CairoContext, graphbb::BoundingBox, xf::Transform2D, lyt::Layout, xlines::GridLines)
	fmt = TickLabelFormatting(lyt.xlabelformat, xlines.rnginfo)
	yframe = graphbb.ymax
	ylabel = graphbb.ymax + lyt.vlabeloffset
	for xtick in xlines.major
		x = ptmap(xf, Point2D(xtick, 0)).x
		render_ticklabel(ctx, xtick, Point2D(x, ylabel), lyt.fntticklabel, ALIGN_TOP|ALIGN_HCENTER, fmt, xlines.scale)
		drawline(ctx, Point2D(x, yframe), Point2D(x, yframe-TICK_MAJOR_LEN))
	end
	for xtick in xlines.minor
		x = ptmap(xf, Point2D(xtick, 0)).x
		drawline(ctx, Point2D(x, yframe), Point2D(x, yframe-TICK_MINOR_LEN))
	end
	if fmt.splitexp
		xlabel = graphbb.xmax + lyt.tframe
		render_axisscalelabel(ctx, Point2D(xlabel, yframe), lyt.fntticklabel, ALIGN_BOTTOM|ALIGN_LEFT, fmt, xlines.scale)
	end
end
function render_yticks(ctx::CairoContext, graphbb::BoundingBox, xf::Transform2D, lyt::Layout, ylines::GridLines)
	fmt = TickLabelFormatting(lyt.ylabelformat, ylines.rnginfo)
	xframe = graphbb.xmin
	xlabel = graphbb.xmin - lyt.hlabeloffset
	for ytick in ylines.major
		y = ptmap(xf, Point2D(0, ytick)).y
		render_ticklabel(ctx, ytick, Point2D(xlabel, y), lyt.fntticklabel, ALIGN_RIGHT|ALIGN_VCENTER, fmt, ylines.scale)
		drawline(ctx, Point2D(xframe, y), Point2D(xframe+TICK_MAJOR_LEN, y))
	end
	for ytick in ylines.minor
		y = ptmap(xf, Point2D(0, ytick)).y
		drawline(ctx, Point2D(xframe, y), Point2D(xframe+TICK_MINOR_LEN, y))
	end
	if fmt.splitexp
		ylabel = graphbb.ymin - lyt.tframe
		render_axisscalelabel(ctx, Point2D(xframe, ylabel), lyt.fntticklabel, ALIGN_BOTTOM|ALIGN_LEFT, fmt, ylines.scale)
	end
end

#Render ticks: UndefinedGridLines
#-------------------------------------------------------------------------------
function render_xticks(ctx::CairoContext, graphbb::BoundingBox, xf::Transform2D, lyt::Layout, xlines::UndefinedGridLines)
	fmt = TickLabelFormatting(NoRangeDisplayInfo())
	yframe = graphbb.ymax
	ylabel = graphbb.ymax + lyt.vlabeloffset
	for (x, xlabel) in [(graphbb.xmin, xlines.minline), (graphbb.xmax, xlines.maxline)]
		render_ticklabel(ctx, xlabel, Point2D(x, ylabel), lyt.fntticklabel, ALIGN_TOP|ALIGN_HCENTER, fmt, AxisScale{:lin}())
		drawline(ctx, Point2D(x, yframe), Point2D(x, yframe-TICK_MAJOR_LEN))
	end
end
function render_yticks(ctx::CairoContext, graphbb::BoundingBox, xf::Transform2D, lyt::Layout, ylines::UndefinedGridLines)
	fmt = TickLabelFormatting(NoRangeDisplayInfo())
	xframe = graphbb.xmin
	xlabel = graphbb.xmin - lyt.hlabeloffset
	for (y, ylabel) in [(graphbb.ymax, ylines.minline), (graphbb.ymin, ylines.maxline)]
		render_ticklabel(ctx, ylabel, Point2D(xlabel, y), lyt.fntticklabel, ALIGN_RIGHT|ALIGN_VCENTER, fmt, AxisScale{:lin}())
		drawline(ctx, Point2D(xframe, y), Point2D(xframe+TICK_MAJOR_LEN, y))
	end
end

function render_ticks(canvas::PCanvas2D, lyt::Layout, grid::GridRect)
	render_xticks(canvas.ctx, canvas.graphbb, canvas.xf, lyt, grid.xlines)
	render_yticks(canvas.ctx, canvas.graphbb, canvas.xf, lyt, grid.ylines)
end

#==High-level rendering
===============================================================================#

#Render axis labels, ticks, ...
#-------------------------------------------------------------------------------
function render_axes(canvas::PCanvas2D, lyt::Layout, grid::GridRect)
	render_graphframe(canvas)
	render_ticks(canvas, lyt, grid)
end

#Render an actual waveform
#-------------------------------------------------------------------------------
function render(canvas::PCanvas2D, wfrm::DWaveform)
	ctx = canvas.ctx
	ds = wfrm.ds

	if length(ds) < 2
		return
	end

if wfrm.line.style != :none
	Cairo.set_source(ctx, wfrm.line.color)
	setlinestyle(ctx, wfrm.line.style, Float64(wfrm.line.width))
	pt = ptmap(canvas.xf, ds[1])
	Cairo.move_to(ctx, pt.x, pt.y)
	for i in 2:length(ds)
		pt = ptmap(canvas.xf, ds[i])
		Cairo.line_to(ctx, pt.x, pt.y)
	end
#	set_line_join(ctx, Cairo.CAIRO_LINE_JOIN_MITER)
	Cairo.stroke(ctx)
end

	if :none == wfrm.glyph.shape; return; end
	_glyph = CGlyph(wfrm.glyph.shape)

	#TODO: do not draw when outside graph extents.

	#Draw symbols:
	setlinestyle(ctx, :solid, Float64(wfrm.line.width))
	linecolor = getglyphcolor(wfrm.glyph, wfrm.line)
	Cairo.set_source(ctx, linecolor)
	fill = getglyphfill(wfrm.glyph)
	gsize = Float64(wfrm.glyph.size)
	for i in 1:length(ds)
		pt = ptmap(canvas.xf, ds[i])
		drawglyph(ctx, _glyph, pt, gsize, fill)
	end

	return
end
render(canvas::PCanvas2D, wfrmlist::Vector{DWaveform}) =
	map((w)->render(canvas, w), wfrmlist)

#Last line


text:
export
    iLQRSolverOptions,
	iLQRSolver2,
    iLQRSolver

@with_kw mutable struct iLQRStats{T}
    iterations::Int = 0
    cost::Vector{T} = [0.]
    dJ::Vector{T} = [0.]
    gradient::Vector{T} = [0.]
    dJ_zero_counter::Int = 0
end

function reset!(stats::iLQRStats, N=0)
    stats.iterations = 0
    stats.cost = zeros(N)
    stats.dJ = zeros(N)
    stats.gradient = zeros(N)
    stats.dJ_zero_counter = 0
end

"""$(TYPEDEF)
Solver options for the iterative LQR (iLQR) solver.
$(FIELDS)
"""
@with_kw mutable struct iLQRSolverOptions{T} <: AbstractSolverOptions{T}
    # Options

    "Print summary at each iteration."
    verbose::Bool=false

    "Live plotting."
    live_plotting::Symbol=:off # :state, :control

    "dJ < ϵ, cost convergence criteria for unconstrained solve or to enter outerloop for constrained solve."
    cost_tolerance::T = 1.0e-4

    "gradient type: :todorov, :feedforward."
    gradient_type::Symbol = :todorov

    "gradient_norm < ϵ, gradient norm convergence criteria."
    gradient_norm_tolerance::T = 1.0e-5

    "iLQR iterations."
    iterations::Int = 300

    "restricts the total number of times a forward pass fails, resulting in regularization, before exiting."
    dJ_counter_limit::Int = 10

    "use square root method backward pass for numerical conditioning."
    square_root::Bool = false

    "forward pass approximate line search lower bound, 0 < line_search_lower_bound < line_search_upper_bound."
    line_search_lower_bound::T = 1.0e-8

    "forward pass approximate line search upper bound, 0 < line_search_lower_bound < line_search_upper_bound < ∞."
    line_search_upper_bound::T = 10.0

    "maximum number of backtracking steps during forward pass line search."
    iterations_linesearch::Int = 20

    # Regularization
    "initial regularization."
    bp_reg_initial::T = 0.0

    "regularization scaling factor."
    bp_reg_increase_factor::T = 1.6

    "maximum regularization value."
    bp_reg_max::T = 1.0e8

    "minimum regularization value."
    bp_reg_min::T = 1.0e-8

    "type of regularization- control: () + ρI, state: (S + ρI); see Synthesis and Stabilization of Complex Behaviors through Online Trajectory Optimization."
    bp_reg_type::Symbol = :control

    "additive regularization when forward pass reaches max iterations."
    bp_reg_fp::T = 10.0

    # square root backward pass options:
    "type of matrix inversion for bp sqrt step."
    bp_sqrt_inv_type::Symbol = :pseudo

    "initial regularization for square root method."
    bp_reg_sqrt_initial::T = 1.0e-6

    "regularization scaling factor for square root method."
    bp_reg_sqrt_increase_factor::T = 10.0

	bp_reg::Bool = false

    # Solver Numerical Limits
    "maximum cost value, if exceded solve will error."
    max_cost_value::T = 1.0e8

    "maximum state value, evaluated during rollout, if exceded solve will error."
    max_state_value::T = 1.0e8

    "maximum control value, evaluated during rollout, if exceded solve will error."
    max_control_value::T = 1.0e8

	static_bp::Bool = true

    log_level::Base.CoreLogging.LogLevel = InnerLoop
end

function iLQRSolverOptions(opts::Union{SolverOptions,UnconstrainedSolverOptions})
	iLQRSolverOptions(
		cost_tolerance=opts.cost_tolerance,
		iterations=opts.iterations,
		verbose=opts.verbose
	)
end

abstract type iLQRSolver{T} <: UnconstrainedSolver{T} end

struct iLQRSolver2{T,I<:QuadratureRule,L,O,n,n̄,m,L1,GT} <: iLQRSolver{T}
    # Model + Objective
    model::L
    obj::O

    # Problem info
    x0::MVector{n,T}
    xf::MVector{n,T}
    tf::T
    N::Int

    opts::iLQRSolverOptions{T}
    stats::iLQRStats{T}

    # Primal Duals
    Z::Vector{KnotPoint{T,n,m,L1}}
    Z̄::Vector{KnotPoint{T,n,m,L1}}

    # Data variables
    # K::Vector{SMatrix{m,n̄,T,L2}}  # State feedback gains (m,n,N-1)
    K::Vector{SizedMatrix{m,n̄,T,2}}  # State feedback gains (m,n,N-1)
    d::Vector{SizedVector{m,T,1}}  # Feedforward gains (m,N-1)

    D::Vector{SizedDynamicsExpansion{T,n,n̄,m}}  # discrete dynamics jacobian (block) (n,n+m+1,N)
    G::Vector{GT}                               # state difference jacobian (n̄, n)

    S::Vector{SizedExpansion{T,n,n̄,m}}      # Optimal cost-to-go expansion trajectory
    Q::Vector{SizedCostExpansion{T,n,n̄,m}}  # cost-to-go expansion trajectory
	# E::SizedExpansion{T,n,n̄,m}

	Quu_reg::SizedMatrix{m,m,T,2}
	Qux_reg::SizedMatrix{m,n̄,T,2}
    ρ::Vector{T}   # Regularization
    dρ::Vector{T}  # Regularization rate of change

    grad::Vector{T}  # Gradient

    logger::SolverLogger

end

function iLQRSolver(prob::Problem{QUAD,T}, opts=SolverOptions{T}()) where {QUAD,T}

    # Init solver statistics
    stats = iLQRStats{T}() # = Dict{Symbol,Any}(:timer=>TimerOutput())

    # Init solver results
    n,m,N = size(prob)
    n̄ = state_diff_size(prob.model)

    x0 = SVector{n}(prob.x0)
    xf = SVector{n}(prob.xf)

    Z = prob.Z
    # Z̄ = Traj(n,m,Z[1].dt,N)
    Z̄ = copy(prob.Z)

	K = [zeros(T,m,n̄) for k = 1:N-1]
    d = [zeros(T,m)   for k = 1:N-1]

	D = [SizedDynamicsExpansion{T}(n,n̄,m) for k = 1:N-1]
	if state_diff_jacobian(prob.model, x0) isa UniformScaling
		G = [I for k = 1:N]
	else
		G = [SizedMatrix{n,n̄}(zeros(n,n̄)) for k = 1:N]
	end

    S = [SizedExpansion{T}(n,n̄,m) for k = 1:N]
	if prob.model isa RigidBody
		Q = [SizedCostExpansion{T}(n,n̄,m) for k = 1:N]
	else
		Q = [SizedCostExpansion{T}(n,m) for k = 1:N]
	end

	Quu_reg = SizedMatrix{m,m}(zeros(m,m))
	Qux_reg = SizedMatrix{m,n̄}(zeros(m,n̄))
    ρ = zeros(T,1)
    dρ = zeros(T,1)

    grad = zeros(T,N-1)

    logger = default_logger(opts.verbose)
	L = typeof(prob.model)
	O = typeof(prob.obj)
	ET = typeof(E)
	GT = eltype(G)

	opts_ilqr = iLQRSolverOptions(opts)
    solver = iLQRSolver2{T,QUAD,L,O,n,n̄,m,n+m,GT}(prob.model, prob.obj, x0, xf,
		prob.tf, N, opts_ilqr, stats,
        Z, Z̄, K, d, D, G, S, Q, Quu_reg, Qux_reg, ρ, dρ, grad, logger)

    reset!(solver)
    return solver
end

# function reset!(solver::iLQRSolver2{T}, reset_stats=true) where T
#     if reset_stats
#         reset!(solver.stats, solver.opts.iterations)
#     end
#     solver.ρ[1] = 0.0
#     solver.dρ[1] = 0.0
#     return nothing
# end
#
Base.size(solver::iLQRSolver2{T,I,L,O,n,m}) where {T,I,L,O,n,m} = n,m,solver.N
# @inline get_trajectory(solver::iLQRSolver2) = solver.Z
# @inline get_objective(solver::iLQRSolver2) = solver.obj
# @inline get_model(solver::iLQRSolver2) = solver.model
# @inline get_initial_state(solver::iLQRSolver2) = solver.x0
#
# function cost(solver::iLQRSolver2, Z=solver.Z)
#     cost!(solver.obj, Z)
#     return sum(get_J(solver.obj))
# end

AbstractSolver(prob::Problem, opts::iLQRSolverOptions) = iLQRSolver(prob, opts)

function reset!(solver::iLQRSolver{T}, reset_stats=true) where T
    if reset_stats
        reset!(solver.stats, solver.opts.iterations)
    end
    solver.ρ[1] = 0.0
    solver.dρ[1] = 0.0
    return nothing
end

@inline get_trajectory(solver::iLQRSolver) = solver.Z
@inline get_objective(solver::iLQRSolver) = solver.obj
@inline get_model(solver::iLQRSolver) = solver.model
@inline get_initial_state(solver::iLQRSolver) = solver.x0

function cost(solver::iLQRSolver, Z=solver.Z)
    cost!(solver.obj, Z)
    return sum(get_J(solver.obj))
end


text:
ENV["PYTHON"]=""
Pkg.add("Conda")
using Conda
Conda.add("python==2.7.13")
Conda.add("matplotlib")
Conda.add("scikit-learn")
Pkg.add("PyCall")
Pkg.build("PyCall")


text:
module Metrics

using ..GeometricFactors

export creategrid, compute_reference_to_physical_coord_jacobian, computemetric

"""
    creategrid!(vgeo, elemtocoord, ξ)

Create a 1-D grid using `elemtocoord` (see `brickmesh`) using the 1-D
`(-1, 1)` reference coordinates `ξ` (in 1D, `ξ = ξ1`). The element grids
are filled using linear interpolation of the element coordinates.

If `Nq = length(ξ)` and `nelem = size(elemtocoord, 3)` then the preallocated
array `vgeo.x1` should be `Nq * nelem == length(x1)`.
"""
function creategrid!(
    vgeo::VolumeGeometry{Nq, <:AbstractArray, <:AbstractArray},
    e2c,
    ξ::NTuple{1, Vector{FT}},
) where {Nq, FT}
    (d, nvert, nelem) = size(e2c)
    @assert d == 1
    (ξ1,) = ξ
    x1 = reshape(vgeo.x1, (Nq..., nelem))

    # Linear blend
    @inbounds for e in 1:nelem
        for i in 1:Nq[1]
            vgeo.x1[i, e] =
                ((1 - ξ1[i]) * e2c[1, 1, e] + (1 + ξ1[i]) * e2c[1, 2, e]) / 2
        end
    end
    nothing
end

"""
    creategrid!(vgeo, elemtocoord, ξ)

Create a 2-D tensor product grid using `elemtocoord` (see `brickmesh`)
using the tuple `ξ = (ξ1, ξ2)`, composed by the 1D reference coordinates `ξ1` and `ξ2` in `(-1, 1)^2`.
The element grids are filled using bilinear interpolation of the element coordinates.

If `Nq = (length(ξ1), length(ξ2))` and `nelem = size(elemtocoord, 3)` then the
preallocated arrays `vgeo.x1` and `vgeo.x2` should be
`prod(Nq) * nelem == size(vgeo.x1) == size(vgeo.x2)`.
"""
function creategrid!(
    vgeo::VolumeGeometry{Nq, <:AbstractArray, <:AbstractArray},
    e2c,
    ξ::NTuple{2, Vector{FT}},
) where {Nq, FT}
    (d, nvert, nelem) = size(e2c)
    @assert d == 2
    (ξ1, ξ2) = ξ
    x1 = reshape(vgeo.x1, (Nq..., nelem))
    x2 = reshape(vgeo.x2, (Nq..., nelem))

    # Bilinear blend of corners
    @inbounds for (f, n) in zip((x1, x2), 1:d)
        for e in 1:nelem, j in 1:Nq[2], i in 1:Nq[1]
            f[i, j, e] =
                (
                    (1 - ξ1[i]) * (1 - ξ2[j]) * e2c[n, 1, e] +
                    (1 + ξ1[i]) * (1 - ξ2[j]) * e2c[n, 2, e] +
                    (1 - ξ1[i]) * (1 + ξ2[j]) * e2c[n, 3, e] +
                    (1 + ξ1[i]) * (1 + ξ2[j]) * e2c[n, 4, e]
                ) / 4
        end
    end
    nothing
end

"""
    creategrid!(vgeo, elemtocoord, ξ)

Create a 3-D tensor product grid using `elemtocoord` (see `brickmesh`)
using the tuple `ξ = (ξ1, ξ2, ξ3)`, composed by the 1D reference coordinates `ξ1`, `ξ2`, `ξ3` in `(-1, 1)^3`.
The element grids are filled using trilinear interpolation of the element coordinates.

If `Nq = (length(ξ1), length(ξ2), length(ξ3))` and
`nelem = size(elemtocoord, 3)` then the preallocated arrays `vgeo.x1`, `vgeo.x2`,
and `vgeo.x3` should be `prod(Nq) * nelem == size(vgeo.x1) == size(vgeo.x2) == size(vgeo.x3)`.
"""
function creategrid!(
    vgeo::VolumeGeometry{Nq, <:AbstractArray, <:AbstractArray},
    e2c,
    ξ::NTuple{3, Vector{FT}},
) where {Nq, FT}
    (d, nvert, nelem) = size(e2c)
    @assert d == 3
    (ξ1, ξ2, ξ3) = ξ
    x1 = reshape(vgeo.x1, (Nq..., nelem))
    x2 = reshape(vgeo.x2, (Nq..., nelem))
    x3 = reshape(vgeo.x3, (Nq..., nelem))

    # Trilinear blend of corners
    @inbounds for (f, n) in zip((x1, x2, x3), 1:d)
        for e in 1:nelem, k in 1:Nq[3], j in 1:Nq[2], i in 1:Nq[1]
            f[i, j, k, e] =
                (
                    (1 - ξ1[i]) * (1 - ξ2[j]) * (1 - ξ3[k]) * e2c[n, 1, e] +
                    (1 + ξ1[i]) * (1 - ξ2[j]) * (1 - ξ3[k]) * e2c[n, 2, e] +
                    (1 - ξ1[i]) * (1 + ξ2[j]) * (1 - ξ3[k]) * e2c[n, 3, e] +
                    (1 + ξ1[i]) * (1 + ξ2[j]) * (1 - ξ3[k]) * e2c[n, 4, e] +
                    (1 - ξ1[i]) * (1 - ξ2[j]) * (1 + ξ3[k]) * e2c[n, 5, e] +
                    (1 + ξ1[i]) * (1 - ξ2[j]) * (1 + ξ3[k]) * e2c[n, 6, e] +
                    (1 - ξ1[i]) * (1 + ξ2[j]) * (1 + ξ3[k]) * e2c[n, 7, e] +
                    (1 + ξ1[i]) * (1 + ξ2[j]) * (1 + ξ3[k]) * e2c[n, 8, e]
                ) / 8
        end
    end
    nothing
end

"""
    compute_reference_to_physical_coord_jacobian!(vgeo, nelem, D)

Input arguments:
- vgeo::VolumeGeometry, a struct containing the volumetric geometric factors
- D::NTuple{2,Int}, a tuple of derivative matrices, i.e., D = (D1,), where:
    - D1::DAT2, 1-D derivative operator on the device in the first dimension

Compute the Jacobian matrix, ∂x / ∂ξ, of the transformation from reference coordinates,
`ξ1`, to physical coordinates, `vgeo.x1`, for each quadrature point in element e.
"""
function compute_reference_to_physical_coord_jacobian!(
    vgeo::VolumeGeometry{Nq, <:AbstractArray, <:AbstractArray},
    nelem,
    D::NTuple{1, Matrix{FT}},
) where {Nq, FT}

    @assert Nq == map(d -> size(d, 1), D)

    T = eltype(vgeo.x1)
    (D1,) = D

    vgeo.x1ξ1 .= zero(T)

    for e in 1:nelem
        for i in 1:Nq[1]
            for n in 1:Nq[1]
                vgeo.x1ξ1[i, e] += D1[i, n] * vgeo.x1[n, e]
            end
        end
    end

    return vgeo
end

"""
    compute_reference_to_physical_coord_jacobian!(vgeo, nelem, D)

Input arguments:
- vgeo::VolumeGeometry, a struct containing the volumetric geometric factors
- D::NTuple{2,Int}, a tuple of derivative matrices, i.e., D = (D1, D2), where:
    - D1::DAT2, 1-D derivative operator on the device in the first dimension
    - D2::DAT2, 1-D derivative operator on the device in the second dimension

Compute the Jacobian matrix, ∂x / ∂ξ, of the transformation from reference coordinates,
`ξ1`, `ξ2`, to physical coordinates, `vgeo.x1`, `vgeo.x2`,
for each quadrature point in element e.
"""
function compute_reference_to_physical_coord_jacobian!(
    vgeo::VolumeGeometry{Nq, <:AbstractArray, <:AbstractArray},
    nelem,
    D::NTuple{2, Matrix{FT}},
) where {Nq, FT}

    @assert Nq == map(d -> size(d, 1), D)

    T = eltype(vgeo.x1)
    (D1, D2) = D

    x1 = reshape(vgeo.x1, (Nq..., nelem))
    x2 = reshape(vgeo.x2, (Nq..., nelem))
    x1ξ1 = reshape(vgeo.x1ξ1, (Nq..., nelem))
    x2ξ1 = reshape(vgeo.x2ξ1, (Nq..., nelem))
    x1ξ2 = reshape(vgeo.x1ξ2, (Nq..., nelem))
    x2ξ2 = reshape(vgeo.x2ξ2, (Nq..., nelem))

    x1ξ1 .= x1ξ2 .= zero(T)
    x2ξ1 .= x2ξ2 .= zero(T)

    for e in 1:nelem
        for j in 1:Nq[2], i in 1:Nq[1]
            for n in 1:Nq[1]
                x1ξ1[i, j, e] += D1[i, n] * x1[n, j, e]
                x2ξ1[i, j, e] += D1[i, n] * x2[n, j, e]
            end
            for n in 1:Nq[2]
                x1ξ2[i, j, e] += D2[j, n] * x1[i, n, e]
                x2ξ2[i, j, e] += D2[j, n] * x2[i, n, e]
            end
        end
    end

    return vgeo
end

"""
    compute_reference_to_physical_coord_jacobian!(vgeo, nelem, D)

Input arguments:
- vgeo::VolumeGeometry, a struct containing the volumetric geometric factors
- D::NTuple{3,Int}, a tuple of derivative matrices, i.e., D = (D1, D2, D3), where:
    - D1::DAT2, 1-D derivative operator on the device in the first dimension
    - D2::DAT2, 1-D derivative operator on the device in the second dimension
    - D3::DAT2, 1-D derivative operator on the device in the third dimension

Compute the Jacobian matrix, ∂x / ∂ξ, of the transformation from reference coordinates,
`ξ1`, `ξ2`, `ξ3` to physical coordinates, `vgeo.x1`, `vgeo.x2`, `vgeo.x3` for
each quadrature point in element e.
"""
function compute_reference_to_physical_coord_jacobian!(
    vgeo::VolumeGeometry{Nq, <:AbstractArray, <:AbstractArray},
    nelem,
    D::NTuple{3, Matrix{FT}},
) where {Nq, FT}

    @assert Nq == map(d -> size(d, 1), D)

    T = eltype(vgeo.x1)
    (D1, D2, D3) = D

    x1 = reshape(vgeo.x1, (Nq..., nelem))
    x2 = reshape(vgeo.x2, (Nq..., nelem))
    x3 = reshape(vgeo.x3, (Nq..., nelem))
    x1ξ1 = reshape(vgeo.x1ξ1, (Nq..., nelem))
    x2ξ1 = reshape(vgeo.x2ξ1, (Nq..., nelem))
    x3ξ1 = reshape(vgeo.x3ξ1, (Nq..., nelem))
    x1ξ2 = reshape(vgeo.x1ξ2, (Nq..., nelem))
    x2ξ2 = reshape(vgeo.x2ξ2, (Nq..., nelem))
    x3ξ2 = reshape(vgeo.x3ξ2, (Nq..., nelem))
    x1ξ3 = reshape(vgeo.x1ξ3, (Nq..., nelem))
    x2ξ3 = reshape(vgeo.x2ξ3, (Nq..., nelem))
    x3ξ3 = reshape(vgeo.x3ξ3, (Nq..., nelem))

    x1ξ1 .= x1ξ2 .= x1ξ3 .= zero(T)
    x2ξ1 .= x2ξ2 .= x2ξ3 .= zero(T)
    x3ξ1 .= x3ξ2 .= x3ξ3 .= zero(T)

    @inbounds for e in 1:nelem
        for k in 1:Nq[3], j in 1:Nq[2], i in 1:Nq[1]
            for n in 1:Nq[1]
                x1ξ1[i, j, k, e] += D1[i, n] * x1[n, j, k, e]
                x2ξ1[i, j, k, e] += D1[i, n] * x2[n, j, k, e]
                x3ξ1[i, j, k, e] += D1[i, n] * x3[n, j, k, e]
            end
            for n in 1:Nq[2]
                x1ξ2[i, j, k, e] += D2[j, n] * x1[i, n, k, e]
                x2ξ2[i, j, k, e] += D2[j, n] * x2[i, n, k, e]
                x3ξ2[i, j, k, e] += D2[j, n] * x3[i, n, k, e]
            end
            for n in 1:Nq[3]
                x1ξ3[i, j, k, e] += D3[k, n] * x1[i, j, n, e]
                x2ξ3[i, j, k, e] += D3[k, n] * x2[i, j, n, e]
                x3ξ3[i, j, k, e] += D3[k, n] * x3[i, j, n, e]
            end
        end
    end

    return vgeo
end

"""
    computemetric!(vgeo, sgeo, D)

Input arguments:
- vgeo::VolumeGeometry, a struct containing the volumetric geometric factors
- sgeo::SurfaceGeometry, a struct containing the surface geometric factors
- D::NTuple{1,Int}, tuple with 1-D derivative operator on the device

Compute the 1-D metric terms from the element grid arrays `vgeo.x1`. All the arrays
are preallocated by the user and the (square) derivative matrix `D` should be
consistent with the reference grid `ξ1` used in [`creategrid!`](@ref).

If `Nq = size(D, 1)` and `nelem = div(length(x1), Nq)` then the volume arrays
`x1`, `J`, and `ξ1x1` should all have length `Nq * nelem`.  Similarly, the face
arrays `sJ` and `n1` should be of length `nface * nelem` with `nface = 2`.
"""
function computemetric!(
    vgeo::VolumeGeometry{Nq, <:AbstractArray, <:AbstractArray},
    sgeo::SurfaceGeometry{Nfp, <:AbstractArray},
    D::NTuple{1, Matrix{FT}},
) where {Nq, Nfp, FT}

    @assert Nq == map(d -> size(d, 1), D)

    nelem = div(length(vgeo.ωJ), Nq[1])
    ωJ = reshape(vgeo.ωJ, (Nq[1], nelem))
    nface = 2
    n1 = reshape(sgeo.n1, (1, nface, nelem))
    sωJ = reshape(sgeo.sωJ, (1, nface, nelem))

    # Compute vertical Jacobian determinant, JcV, and Jacobian determinant, det(∂x/∂ξ), per quadrature point
    vgeo.JcV .= vgeo.x1ξ1
    vgeo.ωJ .= vgeo.x1ξ1

    vgeo.ξ1x1 .= 1 ./ vgeo.ωJ

    sgeo.n1[1, 1, :] .= -sign.(ωJ[1, :])
    sgeo.n1[1, 2, :] .= sign.(ωJ[Nq[1], :])
    sgeo.sωJ .= 1
    nothing
end

"""
    computemetric!(vgeo, sgeo, D)

Input arguments:
- vgeo::VolumeGeometry, a struct containing the volumetric geometric factors
- sgeo::SurfaceGeometry, a struct containing the surface geometric factors
- D::NTuple{2,Int}, a tuple of derivative matrices, i.e., D = (D1, D2), where:
    - D1::DAT2, 1-D derivative operator on the device in the first dimension
    - D2::DAT2, 1-D derivative operator on the device in the second dimension

Compute the 2-D metric terms from the element grid arrays `vgeo.x1` and `vgeo.x2`. All the
arrays are preallocated by the user and the (square) derivative matrice `D1` and
`D2` should be consistent with the reference grid `ξ1` and `ξ2` used in
[`creategrid!`](@ref).

If `Nq = (size(D1, 1), size(D2, 1))` and `nelem = div(length(vgeo.x1), prod(Nq))`
then the volume arrays `vgeo.x1`, `vgeo.x2`, `vgeo.ωJ`, `vgeo.ξ1x1`, `vgeo.ξ2x1`, `vgeo.ξ1x2`, and `vgeo.ξ2x2`
should all be of size `(Nq..., nelem)`.  Similarly, the face arrays `sgeo.sωJ`, `sgeo.n1`,
and `sgeo.n2` should be of size `(maximum(Nq), nface, nelem)` with `nface = 4`
"""
function computemetric!(
    vgeo::VolumeGeometry{Nq, <:AbstractArray, <:AbstractArray},
    sgeo::SurfaceGeometry{Nfp, <:AbstractArray},
    D::NTuple{2, Matrix{FT}},
) where {Nq, Nfp, FT}

    @assert Nq == map(d -> size(d, 1), D)
    @assert Nfp == div.(prod(Nq), Nq)

    nelem = div(length(vgeo.ωJ), prod(Nq))
    x1 = reshape(vgeo.x1, (Nq..., nelem))
    x2 = reshape(vgeo.x2, (Nq..., nelem))
    ωJ = reshape(vgeo.ωJ, (Nq..., nelem))
    JcV = reshape(vgeo.JcV, (Nq..., nelem))
    ξ1x1 = reshape(vgeo.ξ1x1, (Nq..., nelem))
    ξ2x1 = reshape(vgeo.ξ2x1, (Nq..., nelem))
    ξ1x2 = reshape(vgeo.ξ1x2, (Nq..., nelem))
    ξ2x2 = reshape(vgeo.ξ2x2, (Nq..., nelem))
    x1ξ1 = reshape(vgeo.x1ξ1, (Nq..., nelem))
    x1ξ2 = reshape(vgeo.x1ξ2, (Nq..., nelem))
    x2ξ1 = reshape(vgeo.x2ξ1, (Nq..., nelem))
    x2ξ2 = reshape(vgeo.x2ξ2, (Nq..., nelem))
    nface = 4
    n1 = reshape(sgeo.n1, (maximum(Nfp), nface, nelem))
    n2 = reshape(sgeo.n2, (maximum(Nfp), nface, nelem))
    sωJ = reshape(sgeo.sωJ, (maximum(Nfp), nface, nelem))

    for e in 1:nelem
        for j in 1:Nq[2], i in 1:Nq[1]

            # Compute vertical Jacobian determinant, JcV, per quadrature point
            JcV[i, j, e] = hypot(x1ξ2[i, j, e], x2ξ2[i, j, e])
            # Compute Jacobian determinant, det(∂x/∂ξ), per quadrature point
            ωJ[i, j, e] =
                x1ξ1[i, j, e] * x2ξ2[i, j, e] - x2ξ1[i, j, e] * x1ξ2[i, j, e]

            ξ1x1[i, j, e] = x2ξ2[i, j, e] / ωJ[i, j, e]
            ξ2x1[i, j, e] = -x2ξ1[i, j, e] / ωJ[i, j, e]
            ξ1x2[i, j, e] = -x1ξ2[i, j, e] / ωJ[i, j, e]
            ξ2x2[i, j, e] = x1ξ1[i, j, e] / ωJ[i, j, e]
        end

        # Compute surface struct field entries
        for i in 1:maximum(Nfp)
            if i <= Nfp[1]
                sgeo.n1[i, 1, e] = -ωJ[1, i, e] * ξ1x1[1, i, e]
                sgeo.n2[i, 1, e] = -ωJ[1, i, e] * ξ1x2[1, i, e]
                sgeo.n1[i, 2, e] = ωJ[Nq[1], i, e] * ξ1x1[Nq[1], i, e]
                sgeo.n2[i, 2, e] = ωJ[Nq[1], i, e] * ξ1x2[Nq[1], i, e]
            else
                sgeo.n1[i, 1:2, e] .= NaN
                sgeo.n2[i, 1:2, e] .= NaN
            end
            if i <= Nfp[2]
                sgeo.n1[i, 3, e] = -ωJ[i, 1, e] * ξ2x1[i, 1, e]
                sgeo.n2[i, 3, e] = -ωJ[i, 1, e] * ξ2x2[i, 1, e]
                sgeo.n1[i, 4, e] = ωJ[i, Nq[2], e] * ξ2x1[i, Nq[2], e]
                sgeo.n2[i, 4, e] = ωJ[i, Nq[2], e] * ξ2x2[i, Nq[2], e]
            else
                sgeo.n1[i, 3:4, e] .= NaN
                sgeo.n2[i, 3:4, e] .= NaN
            end

            for n in 1:nface
                sgeo.sωJ[i, n, e] = hypot(n1[i, n, e], n2[i, n, e])
                sgeo.n1[i, n, e] /= sωJ[i, n, e]
                sgeo.n2[i, n, e] /= sωJ[i, n, e]
            end
        end

    end

    nothing
end

"""
    computemetric!(vgeo, sgeo, D)

Input arguments:
- vgeo::VolumeGeometry, a struct containing the volumetric geometric factors
- sgeo::SurfaceGeometry, a struct containing the surface geometric factors
- D::NTuple{3,Int}, a tuple of derivative matrices, i.e., D = (D1, D2, D3), where:
    - D1::DAT2, 1-D derivative operator on the device in the first dimension
    - D2::DAT2, 1-D derivative operator on the device in the second dimension
    - D3::DAT2, 1-D derivative operator on the device in the third dimension

Compute the 3-D metric terms from the element grid arrays `vgeo.x1`, `vgeo.x2`, and `vgeo.x3`.
All the arrays are preallocated by the user and the (square) derivative matrice `D1`,
`D2`, and `D3` should be consistent with the reference grid `ξ1`, `ξ2`, and `ξ3` used in
[`creategrid!`](@ref).

If `Nq = size(D1, 1)` and `nelem = div(length(vgeo.x1), Nq^3)` then the volume arrays
`vgeo.x1`, `vgeo.x2`, `vgeo.x3`, `vgeo.ωJ`, `vgeo.ξ1x1`, `vgeo.ξ2x1`, `vgeo.ξ3x1`,
`vgeo.ξ1x2`, `vgeo.ξ2x2`, `vgeo.ξ3x2`, `vgeo.ξ1x3`,`vgeo.ξ2x3`, and `vgeo.ξ3x3`
should all be of length `Nq^3 * nelem`.  Similarly, the face
arrays `sgeo.sωJ`, `sgeo.n1`, `sgeo.n2`, and `sgeo.n3` should be of size `Nq^2 * nface * nelem` with
`nface = 6`.

The curl invariant formulation of Kopriva (2006), equation 37, is used.

Reference:
 - [Kopriva2006](@cite)
"""
function computemetric!(
    vgeo::VolumeGeometry{Nq, <:AbstractArray, <:AbstractArray},
    sgeo::SurfaceGeometry{Nfp, <:AbstractArray},
    D::NTuple{3, Matrix{FT}},
) where {Nq, Nfp, FT}

    @assert Nq == map(d -> size(d, 1), D)
    @assert Nfp == div.(prod(Nq), Nq)

    T = eltype(vgeo.x1)
    nelem = div(length(vgeo.ωJ), prod(Nq))
    x1 = reshape(vgeo.x1, (Nq..., nelem))
    x2 = reshape(vgeo.x2, (Nq..., nelem))
    x3 = reshape(vgeo.x3, (Nq..., nelem))
    ωJ = reshape(vgeo.ωJ, (Nq..., nelem))
    JcV = reshape(vgeo.JcV, (Nq..., nelem))
    ξ1x1 = reshape(vgeo.ξ1x1, (Nq..., nelem))
    ξ2x1 = reshape(vgeo.ξ2x1, (Nq..., nelem))
    ξ3x1 = reshape(vgeo.ξ3x1, (Nq..., nelem))
    ξ1x2 = reshape(vgeo.ξ1x2, (Nq..., nelem))
    ξ2x2 = reshape(vgeo.ξ2x2, (Nq..., nelem))
    ξ3x2 = reshape(vgeo.ξ3x2, (Nq..., nelem))
    ξ1x3 = reshape(vgeo.ξ1x3, (Nq..., nelem))
    ξ2x3 = reshape(vgeo.ξ2x3, (Nq..., nelem))
    ξ3x3 = reshape(vgeo.ξ3x3, (Nq..., nelem))
    x1ξ1 = reshape(vgeo.x1ξ1, (Nq..., nelem))
    x1ξ2 = reshape(vgeo.x1ξ2, (Nq..., nelem))
    x1ξ3 = reshape(vgeo.x1ξ3, (Nq..., nelem))
    x2ξ1 = reshape(vgeo.x2ξ1, (Nq..., nelem))
    x2ξ2 = reshape(vgeo.x2ξ2, (Nq..., nelem))
    x2ξ3 = reshape(vgeo.x2ξ3, (Nq..., nelem))
    x3ξ1 = reshape(vgeo.x3ξ1, (Nq..., nelem))
    x3ξ2 = reshape(vgeo.x3ξ2, (Nq..., nelem))
    x3ξ3 = reshape(vgeo.x3ξ3, (Nq..., nelem))

    nface = 6
    n1 = reshape(sgeo.n1, maximum(Nfp), nface, nelem)
    n2 = reshape(sgeo.n2, maximum(Nfp), nface, nelem)
    n3 = reshape(sgeo.n3, maximum(Nfp), nface, nelem)
    sωJ = reshape(sgeo.sωJ, maximum(Nfp), nface, nelem)

    JI2 = similar(vgeo.ωJ, Nq...)
    (yzr, yzs, yzt) = (similar(JI2), similar(JI2), similar(JI2))
    (zxr, zxs, zxt) = (similar(JI2), similar(JI2), similar(JI2))
    (xyr, xys, xyt) = (similar(JI2), similar(JI2), similar(JI2))
    # Temporary variables to compute inverse of a 3x3 matrix
    (a11, a12, a13) = (similar(JI2), similar(JI2), similar(JI2))
    (a21, a22, a23) = (similar(JI2), similar(JI2), similar(JI2))
    (a31, a32, a33) = (similar(JI2), similar(JI2), similar(JI2))

    ξ1x1 .= ξ2x1 .= ξ3x1 .= zero(T)
    ξ1x2 .= ξ2x2 .= ξ3x2 .= zero(T)
    ξ1x3 .= ξ2x3 .= ξ3x3 .= zero(T)

    fill!(n1, NaN)
    fill!(n2, NaN)
    fill!(n3, NaN)
    fill!(sωJ, NaN)

    @inbounds for e in 1:nelem
        for k in 1:Nq[3], j in 1:Nq[2], i in 1:Nq[1]

            # Compute vertical Jacobian determinant, JcV, per quadrature point
            JcV[i, j, k, e] =
                hypot(x1ξ3[i, j, k, e], x2ξ3[i, j, k, e], x3ξ3[i, j, k, e])
            # Compute Jacobian determinant, det(∂x/∂ξ), per quadrature point
            ωJ[i, j, k, e] = (
                x1ξ1[i, j, k, e] * (
                    x2ξ2[i, j, k, e] * x3ξ3[i, j, k, e] -
                    x3ξ2[i, j, k, e] * x2ξ3[i, j, k, e]
                ) +
                x2ξ1[i, j, k, e] * (
                    x3ξ2[i, j, k, e] * x1ξ3[i, j, k, e] -
                    x1ξ2[i, j, k, e] * x3ξ3[i, j, k, e]
                ) +
                x3ξ1[i, j, k, e] * (
                    x1ξ2[i, j, k, e] * x2ξ3[i, j, k, e] -
                    x2ξ2[i, j, k, e] * x1ξ3[i, j, k, e]
                )
            )

            JI2[i, j, k] = 1 / (2 * ωJ[i, j, k, e])

            yzr[i, j, k] =
                x2[i, j, k, e] * x3ξ1[i, j, k, e] -
                x3[i, j, k, e] * x2ξ1[i, j, k, e]
            yzs[i, j, k] =
                x2[i, j, k, e] * x3ξ2[i, j, k, e] -
                x3[i, j, k, e] * x2ξ2[i, j, k, e]
            yzt[i, j, k] =
                x2[i, j, k, e] * x3ξ3[i, j, k, e] -
                x3[i, j, k, e] * x2ξ3[i, j, k, e]
            zxr[i, j, k] =
                x3[i, j, k, e] * x1ξ1[i, j, k, e] -
                x1[i, j, k, e] * x3ξ1[i, j, k, e]
            zxs[i, j, k] =
                x3[i, j, k, e] * x1ξ2[i, j, k, e] -
                x1[i, j, k, e] * x3ξ2[i, j, k, e]
            zxt[i, j, k] =
                x3[i, j, k, e] * x1ξ3[i, j, k, e] -
                x1[i, j, k, e] * x3ξ3[i, j, k, e]
            xyr[i, j, k] =
                x1[i, j, k, e] * x2ξ1[i, j, k, e] -
                x2[i, j, k, e] * x1ξ1[i, j, k, e]
            xys[i, j, k] =
                x1[i, j, k, e] * x2ξ2[i, j, k, e] -
                x2[i, j, k, e] * x1ξ2[i, j, k, e]
            xyt[i, j, k] =
                x1[i, j, k, e] * x2ξ3[i, j, k, e] -
                x2[i, j, k, e] * x1ξ3[i, j, k, e]
        end

        for k in 1:Nq[3], j in 1:Nq[2], i in 1:Nq[1]
            for n in 1:Nq[1]
                ξ2x1[i, j, k, e] -= D[1][i, n] * yzt[n, j, k]
                ξ3x1[i, j, k, e] += D[1][i, n] * yzs[n, j, k]
                ξ2x2[i, j, k, e] -= D[1][i, n] * zxt[n, j, k]
                ξ3x2[i, j, k, e] += D[1][i, n] * zxs[n, j, k]
                ξ2x3[i, j, k, e] -= D[1][i, n] * xyt[n, j, k]
                ξ3x3[i, j, k, e] += D[1][i, n] * xys[n, j, k]
            end
            for n in 1:Nq[2]
                ξ1x1[i, j, k, e] += D[2][j, n] * yzt[i, n, k]
                ξ3x1[i, j, k, e] -= D[2][j, n] * yzr[i, n, k]
                ξ1x2[i, j, k, e] += D[2][j, n] * zxt[i, n, k]
                ξ3x2[i, j, k, e] -= D[2][j, n] * zxr[i, n, k]
                ξ1x3[i, j, k, e] += D[2][j, n] * xyt[i, n, k]
                ξ3x3[i, j, k, e] -= D[2][j, n] * xyr[i, n, k]
            end
            for n in 1:Nq[3]
                ξ1x1[i, j, k, e] -= D[3][k, n] * yzs[i, j, n]
                ξ2x1[i, j, k, e] += D[3][k, n] * yzr[i, j, n]
                ξ1x2[i, j, k, e] -= D[3][k, n] * zxs[i, j, n]
                ξ2x2[i, j, k, e] += D[3][k, n] * zxr[i, j, n]
                ξ1x3[i, j, k, e] -= D[3][k, n] * xys[i, j, n]
                ξ2x3[i, j, k, e] += D[3][k, n] * xyr[i, j, n]
            end
            ξ1x1[i, j, k, e] *= JI2[i, j, k]
            ξ2x1[i, j, k, e] *= JI2[i, j, k]
            ξ3x1[i, j, k, e] *= JI2[i, j, k]
            ξ1x2[i, j, k, e] *= JI2[i, j, k]
            ξ2x2[i, j, k, e] *= JI2[i, j, k]
            ξ3x2[i, j, k, e] *= JI2[i, j, k]
            ξ1x3[i, j, k, e] *= JI2[i, j, k]
            ξ2x3[i, j, k, e] *= JI2[i, j, k]
            ξ3x3[i, j, k, e] *= JI2[i, j, k]

            # Invert ∂ξk/∂xi, since the discrete curl-invariant form that we have
            # just computed, ∂ξk/∂xi, is not equal to its inverse
            a11[i, j, k] =
                ξ2x2[i, j, k, e] * ξ3x3[i, j, k, e] -
                ξ2x3[i, j, k, e] * ξ3x2[i, j, k, e]
            a12[i, j, k] =
                ξ1x3[i, j, k, e] * ξ3x2[i, j, k, e] -
                ξ1x2[i, j, k, e] * ξ3x3[i, j, k, e]
            a13[i, j, k] =
                ξ1x2[i, j, k, e] * ξ2x3[i, j, k, e] -
                ξ1x3[i, j, k, e] * ξ2x2[i, j, k, e]
            a21[i, j, k] =
                ξ2x3[i, j, k, e] * ξ3x1[i, j, k, e] -
                ξ2x1[i, j, k, e] * ξ3x3[i, j, k, e]
            a22[i, j, k] =
                ξ1x1[i, j, k, e] * ξ3x3[i, j, k, e] -
                ξ1x3[i, j, k, e] * ξ3x1[i, j, k, e]
            a23[i, j, k] =
                ξ1x3[i, j, k, e] * ξ2x1[i, j, k, e] -
                ξ1x1[i, j, k, e] * ξ2x3[i, j, k, e]
            a31[i, j, k] =
                ξ2x1[i, j, k, e] * ξ3x2[i, j, k, e] -
                ξ2x2[i, j, k, e] * ξ3x1[i, j, k, e]
            a32[i, j, k] =
                ξ1x2[i, j, k, e] * ξ3x1[i, j, k, e] -
                ξ1x1[i, j, k, e] * ξ3x2[i, j, k, e]
            a33[i, j, k] =
                ξ1x1[i, j, k, e] * ξ2x2[i, j, k, e] -
                ξ1x2[i, j, k, e] * ξ2x1[i, j, k, e]

            det =
                ξ1x1[i, j, k, e] * a11[i, j, k] +
                ξ2x1[i, j, k, e] * a12[i, j, k] +
                ξ3x1[i, j, k, e] * a13[i, j, k]

            x1ξ1[i, j, k, e] =
                1.0 / det * (
                    a11[i, j, k] * a11[i, j, k] +
                    a12[i, j, k] * a12[i, j, k] +
                    a13[i, j, k] * a13[i, j, k]
                )
            x1ξ2[i, j, k, e] =
                1.0 / det * (
                    a11[i, j, k] * a21[i, j, k] +
                    a12[i, j, k] * a22[i, j, k] +
                    a13[i, j, k] * a23[i, j, k]
                )
            x1ξ3[i, j, k, e] =
                1.0 / det * (
                    a11[i, j, k] * a31[i, j, k] +
                    a12[i, j, k] * a32[i, j, k] +
                    a13[i, j, k] * a33[i, j, k]
                )
            x2ξ1[i, j, k, e] =
                1.0 / det * (
                    a21[i, j, k] * a11[i, j, k] +
                    a22[i, j, k] * a12[i, j, k] +
                    a23[i, j, k] * a13[i, j, k]
                )
            x2ξ2[i, j, k, e] =
                1.0 / det * (
                    a21[i, j, k] * a21[i, j, k] +
                    a22[i, j, k] * a22[i, j, k] +
                    a23[i, j, k] * a23[i, j, k]
                )
            x2ξ3[i, j, k, e] =
                1.0 / det * (
                    a21[i, j, k] * a31[i, j, k] +
                    a22[i, j, k] * a32[i, j, k] +
                    a23[i, j, k] * a33[i, j, k]
                )
            x3ξ1[i, j, k, e] =
                1.0 / det * (
                    a31[i, j, k] * a11[i, j, k] +
                    a32[i, j, k] * a12[i, j, k] +
                    a33[i, j, k] * a13[i, j, k]
                )
            x3ξ2[i, j, k, e] =
                1.0 / det * (
                    a31[i, j, k] * a21[i, j, k] +
                    a32[i, j, k] * a22[i, j, k] +
                    a33[i, j, k] * a23[i, j, k]
                )
            x3ξ3[i, j, k, e] =
                1.0 / det * (
                    a31[i, j, k] * a31[i, j, k] +
                    a32[i, j, k] * a32[i, j, k] +
                    a33[i, j, k] * a33[i, j, k]
                )
        end

        # Compute surface struct field entries
        # faces 1 & 2
        for k in 1:Nq[3], j in 1:Nq[2]
            n = j + (k - 1) * Nq[2]
            sgeo.n1[n, 1, e] = -ωJ[1, j, k, e] * ξ1x1[1, j, k, e]
            sgeo.n2[n, 1, e] = -ωJ[1, j, k, e] * ξ1x2[1, j, k, e]
            sgeo.n3[n, 1, e] = -ωJ[1, j, k, e] * ξ1x3[1, j, k, e]
            sgeo.n1[n, 2, e] = ωJ[Nq[1], j, k, e] * ξ1x1[Nq[1], j, k, e]
            sgeo.n2[n, 2, e] = ωJ[Nq[1], j, k, e] * ξ1x2[Nq[1], j, k, e]
            sgeo.n3[n, 2, e] = ωJ[Nq[1], j, k, e] * ξ1x3[Nq[1], j, k, e]
            for f in 1:2
                sgeo.sωJ[n, f, e] = hypot(n1[n, f, e], n2[n, f, e], n3[n, f, e])
                sgeo.n1[n, f, e] /= sωJ[n, f, e]
                sgeo.n2[n, f, e] /= sωJ[n, f, e]
                sgeo.n3[n, f, e] /= sωJ[n, f, e]
            end
        end
        # faces 3 & 4
        for k in 1:Nq[3], i in 1:Nq[1]
            n = i + (k - 1) * Nq[1]
            sgeo.n1[n, 3, e] = -ωJ[i, 1, k, e] * ξ2x1[i, 1, k, e]
            sgeo.n2[n, 3, e] = -ωJ[i, 1, k, e] * ξ2x2[i, 1, k, e]
            sgeo.n3[n, 3, e] = -ωJ[i, 1, k, e] * ξ2x3[i, 1, k, e]
            sgeo.n1[n, 4, e] = ωJ[i, Nq[2], k, e] * ξ2x1[i, Nq[2], k, e]
            sgeo.n2[n, 4, e] = ωJ[i, Nq[2], k, e] * ξ2x2[i, Nq[2], k, e]
            sgeo.n3[n, 4, e] = ωJ[i, Nq[2], k, e] * ξ2x3[i, Nq[2], k, e]
            for f in 3:4
                sgeo.sωJ[n, f, e] = hypot(n1[n, f, e], n2[n, f, e], n3[n, f, e])
                sgeo.n1[n, f, e] /= sωJ[n, f, e]
                sgeo.n2[n, f, e] /= sωJ[n, f, e]
                sgeo.n3[n, f, e] /= sωJ[n, f, e]
            end
        end
        # faces 5 & 6
        for j in 1:Nq[2], i in 1:Nq[1]
            n = i + (j - 1) * Nq[1]
            sgeo.n1[n, 5, e] = -ωJ[i, j, 1, e] * ξ3x1[i, j, 1, e]
            sgeo.n2[n, 5, e] = -ωJ[i, j, 1, e] * ξ3x2[i, j, 1, e]
            sgeo.n3[n, 5, e] = -ωJ[i, j, 1, e] * ξ3x3[i, j, 1, e]
            sgeo.n1[n, 6, e] = ωJ[i, j, Nq[3], e] * ξ3x1[i, j, Nq[3], e]
            sgeo.n2[n, 6, e] = ωJ[i, j, Nq[3], e] * ξ3x2[i, j, Nq[3], e]
            sgeo.n3[n, 6, e] = ωJ[i, j, Nq[3], e] * ξ3x3[i, j, Nq[3], e]
            for f in 5:6
                sgeo.sωJ[n, f, e] = hypot(n1[n, f, e], n2[n, f, e], n3[n, f, e])
                sgeo.n1[n, f, e] /= sωJ[n, f, e]
                sgeo.n2[n, f, e] /= sωJ[n, f, e]
                sgeo.n3[n, f, e] /= sωJ[n, f, e]
            end
        end
    end

    nothing
end

end # module