#=
exp_xform.jl

see also MIRT/time/exp_xform.jl

2019-12-08 Connor Martin
Based on exp_xform_mex.c
Copyright 2004-9-23, Jeff Fessler, University of Michigan
=#

export exp_xform

"""
    exp_xform(x, u, v ; mode::Symbol = :matrix)

in:
* `x [N L]` possibly complex vector(s)
* `u [D N]` possibly complex vectors
* `v [D M]` possibly complex vectors
* `mode::Symbol` `:matrix` (default) | `:element` | `:row` | `:column`

out:
* `y [M L]` typically complex vector
 `y[m,l] = sum_n x[n,l] exp(-sum(u[:,n] .* v[:,m]))`

Iterates through subsets of the ML matrix designated by `:mode`
(i.e. row, column, element, or just computing the entire matrix)
This is the 'slow' 'exact' transform model for MRI.

Output type will depend on input types.
"""
function exp_xform(x::AbstractMatrix{<:Number},
        u::AbstractMatrix{<:Number},
        v::AbstractMatrix{<:Number}
        ; mode::Symbol = :matrix)

    T = promote_type(eltype(u), eltype(v), eltype(x), ComplexF32)

    N = size(u,2)
    M = size(v,2)
    out = zeros(T, M, size(x,2))

    if (mode === :matrix)
        return exp.(-(transpose(v) * u)) * x # [M D] * [D N] * [N L]

    elseif (mode === :element)
        # avoid generating intermediate steps by iterating thru N and M
        # dot the columns
        for n = 1:N
            for m = 1:M
                t = transpose(u[:,n]) * v[:,m] #calculate one spot in the array
                out[m,:] .+= (exp.(-t) .* x[n,:])
            end
        end
        return out

    elseif (mode === :row) # iterate through rows of the large matrix
        for m = 1:M
            t = transpose(v[:,m]) * u # [1 D] * [D N] -> [1 N]
            # m == 1 && @show size(t), size(exp.(-t) * x)
            # product is a 2d matrix (row), but should be a column vector:
            out[m,:] .+= (exp.(-t) * x)[1,:]
        end
        return out

    elseif (mode === :column)
        for n = 1:N
            t = transpose(v) * u[:,n] # [M D] * [D .] -> [M]
            # n == 1 && @show size(t), size(x[n,:])
            out .+= (exp.(-t) * transpose(x[n,:])) # outer product [M 1] * [1 L]
        end
        return out

    else
        throw("Invalid mode parameter $mode")
    end
end

# handle "vector" case where L=1
exp_xform(x::AbstractVector{<:Number},
        u::AbstractMatrix{<:Number},
        v::AbstractMatrix{<:Number}
        ; kwargs...) = exp_xform(reshape(x, :, 1), u, v ; kwargs...)[:,1]


function rbc()
    ∞ = Inf
    @variables α, β, ρ, δ, σ
    @variables t::Integer, k(..), z(..), c(..), q(..)
    x = [k, z]
    y = [c, q]
    p = [α, β, ρ, δ, σ]

    H = [1 / c(t) - (β / c(t + 1)) * (α * exp(z(t + 1)) * k(t + 1)^(α - 1) + (1 - δ)),
         c(t) + k(t + 1) - (1 - δ) * k(t) - q(t), q(t) - exp(z(t)) * k(t)^α,
         z(t + 1) - ρ * z(t)]

    steady_states = [k(∞) ~ (((1 / β) - 1 + δ) / α)^(1 / (α - 1)), z(∞) ~ 0,
                     c(∞) ~ (((1 / β) - 1 + δ) / α)^(α / (α - 1)) -
                            δ * (((1 / β) - 1 + δ) / α)^(1 / (α - 1)),
                     q(∞) ~ (((1 / β) - 1 + δ) / α)^(α / (α - 1))]

    steady_states_iv = [k(∞) ~ (((1 / β) - 1 + δ) / α)^(1 / (α - 1)), z(∞) ~ 0,
                        c(∞) ~ (((1 / β) - 1 + δ) / α)^(α / (α - 1)) -
                               δ * (((1 / β) - 1 + δ) / α)^(1 / (α - 1)),
                        q(∞) ~ (((1 / β) - 1 + δ) / α)^(α / (α - 1))]

    n_ϵ = 1
    n_x = length(x)
    n_y = length(y)
    n_p = length(p)
    Γ = reshape([σ], n_ϵ, n_ϵ)
    η = reshape([0; -1], n_x, n_ϵ) # η is n_x * n_ϵ matrix

    return H, (; t, x, y, p, steady_states, steady_states_iv, Γ, η), "rbc"
end

function rbc_multiple_shocks()
    H, nt = rbc()
    σ = nt.Γ[1, 1]
    Γ = [σ 0; σ σ]
    η = [0 -1; 0 -1]
    return H, merge(nt, (; Γ, η)), "rbc_multiple_shocks"
end

function rbc_reorder_ss()
    H, nt = rbc()
    steady_states = reverse(nt.steady_states)
    return H, merge(nt, (; steady_states)), "rbc_reorder_ss"
end

function rbc_solve_steady_state()
    H, nt = rbc()
    steady_states = nothing
    return H, merge(nt, (; steady_states)), "rbc_solve_steady_state"
end

function rbc_solve_steady_state_different_iv()
    H, nt = rbc()

    ∞ = Inf
    @variables α, β, ρ, δ, σ
    @variables t::Integer, k(..), z(..), c(..), q(..)
    x = [k, z]
    y = [c, q]
    p = [α, β, ρ, δ, σ]

    steady_states_iv = [
        k(∞) ~ 0.1,# i.e., not (((1 / β) - 1 + δ) / α)^(1 / (α - 1)),
        z(∞) ~ 0.01,
        c(∞) ~ (((1 / β) - 1 + δ) / α)^(α / (α - 1)) -
                δ * (((1 / β) - 1 + δ) / α)^(1 / (α - 1)),
        q(∞) ~ (((1 / β) - 1 + δ) / α)^(α / (α - 1)),
    ]

    steady_states = nothing

    return H, merge(nt, (; steady_states, steady_states_iv)),
           "rbc_solve_steady_state_different_iv"
end

# Add observation equation with a single source of measurement error
function rbc_observables()
    H, nt = rbc()

    n_x = length(nt.x)
    n_y = length(nt.y)
    n_z = 2 # number of observables
    Q = zeros(n_z, n_x + n_y)
    Q[1, 1] = 1.0
    Q[2, 3] = 1.0

    @variables Ω_1
    Ω = [Ω_1, Ω_1]
    p = [nt.p; Ω_1]
    return H, merge(nt, (; p, Ω, Q)), "rbc_observables"
end

# Add observation equation with two parameters in the source of measurement error
function rbc_observables_separate_variance()
    H, nt = rbc()

    n_x = length(nt.x)
    n_y = length(nt.y)
    n_z = 2 # number of observables
    Q = zeros(n_z, n_x + n_y)
    Q[1, 1] = 1.0
    Q[2, 3] = 1.0

    @variables Ω_1, Ω_2
    Ω = [Ω_1, Ω_2]
    p = [nt.p; Ω_1; Ω_2]
    return H, merge(nt, (; p, Ω, Q)), "rbc_observables_separate_variance"
end


module PrimeFactorsKata

function factorsOf(x)
    factors = Int[]
    divisor = 2
    while x > 1
        while x % divisor == 0
            append!(factors, divisor)
            x = convert(Int, x / divisor)
        end
        divisor += 1
    end
    return factors
end

export factorsOf

end # module


using RungeKutta: name, order, nstages, coefficients, weights, nodes

@testset "$(rpad("Partitioned Tableaus",80))" begin

    @test typeof(PartitionedTableau(:PRK4, TableauRK4(), TableauRK4())) <: PartitionedTableau
    @test PartitionedTableau(:PRK4, TableauRK4(), TableauRK4()) == PartitionedTableau(:PRK4, TableauRK4())

    @test typeof(PartitionedTableauGauss(1)) <: PartitionedTableau
    @test order(PartitionedTableauGauss(1)) == 2
    @test nstages(PartitionedTableauGauss(1)) == 1
    @test issymplectic(PartitionedTableauGauss(1))
    @test issymmetric(PartitionedTableauGauss(1))
    @test all(all.(check_symmetry(PartitionedTableauGauss(1))))

    @test !isexplicit(PartitionedTableauGauss(1))
    @test  isimplicit(PartitionedTableauGauss(1))
    @test !isdiagnonallyimplicit(PartitionedTableauGauss(1))
    @test  isfullyimplicit(PartitionedTableauGauss(1))

    @test typeof(PartitionedTableauGauss(2)) <: PartitionedTableau
    @test order(PartitionedTableauGauss(2)) == 4
    @test nstages(PartitionedTableauGauss(2)) == 2
    @test issymplectic(PartitionedTableauGauss(2))
    @test issymmetric(PartitionedTableauGauss(2))
    @test all(all.(check_symmetry(PartitionedTableauGauss(2))))

    @test !isexplicit(PartitionedTableauGauss(2))
    @test  isimplicit(PartitionedTableauGauss(2))
    @test !isdiagnonallyimplicit(PartitionedTableauGauss(2))
    @test  isfullyimplicit(PartitionedTableauGauss(2))

    @test typeof(TableauLobattoIIIAIIIB(2)) <: PartitionedTableau
    @test order(TableauLobattoIIIAIIIB(2)) == 2
    @test nstages(TableauLobattoIIIAIIIB(2)) == 2
    @test issymplectic(TableauLobattoIIIAIIIB(2))
    @test issymmetric(TableauLobattoIIIAIIIB(2))

    @test  isexplicit(TableauLobattoIIIAIIIB(2))
    @test !isimplicit(TableauLobattoIIIAIIIB(2))
    @test !isdiagnonallyimplicit(TableauLobattoIIIAIIIB(2))
    @test !isfullyimplicit(TableauLobattoIIIAIIIB(2))

    @test typeof(TableauLobattoIIIAIIIB(3)) <: PartitionedTableau
    @test order(TableauLobattoIIIAIIIB(3)) == 4
    @test nstages(TableauLobattoIIIAIIIB(3)) == 3
    @test issymplectic(TableauLobattoIIIAIIIB(3))
    @test issymmetric(TableauLobattoIIIAIIIB(3))

    @test !isexplicit(TableauLobattoIIIAIIIB(3))
    @test  isimplicit(TableauLobattoIIIAIIIB(3))
    @test !isdiagnonallyimplicit(TableauLobattoIIIAIIIB(3))
    @test  isfullyimplicit(TableauLobattoIIIAIIIB(3))

    @test typeof(TableauLobattoIIIBIIIA(2)) <: PartitionedTableau
    @test order(TableauLobattoIIIBIIIA(2)) == 2
    @test nstages(TableauLobattoIIIBIIIA(2)) == 2
    @test issymplectic(TableauLobattoIIIBIIIA(2))
    @test issymmetric(TableauLobattoIIIBIIIA(2))

    @test  isexplicit(TableauLobattoIIIBIIIA(2))
    @test !isimplicit(TableauLobattoIIIBIIIA(2))
    @test !isdiagnonallyimplicit(TableauLobattoIIIBIIIA(2))
    @test !isfullyimplicit(TableauLobattoIIIBIIIA(2))

    @test typeof(TableauLobattoIIIBIIIA(3)) <: PartitionedTableau
    @test order(TableauLobattoIIIBIIIA(3)) == 4
    @test nstages(TableauLobattoIIIBIIIA(3)) == 3
    @test issymplectic(TableauLobattoIIIBIIIA(3))
    @test issymmetric(TableauLobattoIIIBIIIA(3))

    @test !isexplicit(TableauLobattoIIIBIIIA(3))
    @test  isimplicit(TableauLobattoIIIBIIIA(3))
    @test !isdiagnonallyimplicit(TableauLobattoIIIBIIIA(3))
    @test  isfullyimplicit(TableauLobattoIIIBIIIA(3))

    @test typeof(TableauLobattoIIICIIIC̄(2)) <: PartitionedTableau
    @test order(TableauLobattoIIICIIIC̄(2)) == 2
    @test nstages(TableauLobattoIIICIIIC̄(2)) == 2
    @test  issymplectic(TableauLobattoIIICIIIC̄(2))
    @test !issymmetric(TableauLobattoIIICIIIC̄(2))

    @test !isexplicit(TableauLobattoIIICIIIC̄(2))
    @test  isimplicit(TableauLobattoIIICIIIC̄(2))
    @test !isdiagnonallyimplicit(TableauLobattoIIICIIIC̄(2))
    @test  isfullyimplicit(TableauLobattoIIICIIIC̄(2))

    @test typeof(TableauLobattoIIICIIIC̄(3)) <: PartitionedTableau
    @test order(TableauLobattoIIICIIIC̄(3)) == 4
    @test nstages(TableauLobattoIIICIIIC̄(3)) == 3
    @test  issymplectic(TableauLobattoIIICIIIC̄(3))
    @test !issymmetric(TableauLobattoIIICIIIC̄(3))

    @test !isexplicit(TableauLobattoIIICIIIC̄(3))
    @test  isimplicit(TableauLobattoIIICIIIC̄(3))
    @test !isdiagnonallyimplicit(TableauLobattoIIICIIIC̄(3))
    @test  isfullyimplicit(TableauLobattoIIICIIIC̄(3))

    @test typeof(TableauLobattoIIIC̄IIIC(2)) <: PartitionedTableau
    @test order(TableauLobattoIIIC̄IIIC(2)) == 2
    @test nstages(TableauLobattoIIIC̄IIIC(2)) == 2
    @test  issymplectic(TableauLobattoIIIC̄IIIC(2))
    @test !issymmetric(TableauLobattoIIIC̄IIIC(2))

    @test !isexplicit(TableauLobattoIIIC̄IIIC(2))
    @test  isimplicit(TableauLobattoIIIC̄IIIC(2))
    @test !isdiagnonallyimplicit(TableauLobattoIIIC̄IIIC(2))
    @test  isfullyimplicit(TableauLobattoIIIC̄IIIC(2))

    @test typeof(TableauLobattoIIIC̄IIIC(3)) <: PartitionedTableau
    @test order(TableauLobattoIIIC̄IIIC(3)) == 4
    @test nstages(TableauLobattoIIIC̄IIIC(3)) == 3
    @test  issymplectic(TableauLobattoIIIC̄IIIC(3))
    @test !issymmetric(TableauLobattoIIIC̄IIIC(3))

    @test !isexplicit(TableauLobattoIIIC̄IIIC(3))
    @test  isimplicit(TableauLobattoIIIC̄IIIC(3))
    @test !isdiagnonallyimplicit(TableauLobattoIIIC̄IIIC(3))
    @test  isfullyimplicit(TableauLobattoIIIC̄IIIC(3))

end


using Test
using FinDiff2018
using Plots

#=
Piecewise Linear Continuous Functions

	We approximate functions by a piecewise linear continuous approximation,
	with a regular grid spacing
=#

fsinpi = samplePLC(Float64, Float64, 4, sinpi)
#fsinpi2 = samplePLC(Float64, Float64, 4, sinpi)
#@show fsinpi

#@test (fsinpi + fsinpi2) == (fsinpi2 + fsinpi)

(evaluate(fsinpi, 0.3333), sinpi(0.3333))
xs = collect(range(0, stop=1, length=1001))
#@show xs

plot(sinpi.(xs))
plot!([evaluate(fsinpi, x) for x in xs])

rm("C:/Julia/FinDiff2018/test/Plot1_sinPLC.png", force=true)
savefig("Plot1_sinPLC.png")

fsinpi2 = refine(fsinpi)
plot([evaluate(fsinpi, x) for x in xs])
plot!([evaluate(fsinpi2, x)+0.1 for x in xs])

rm("C:/Julia/FinDiff2018/test/Plot1_errorPLC.png", force=true)
savefig("Plot1_errorPLC.png")

############################################################
##			Advection Equation
############################################################
s0 = initialGaussian(18)
plot([evaluate(s0.u, x) for x in xs])

rm("C:/Julia/FinDiff2018/test/Plot2_AdStateInitGuassian.png", force=true)
savefig("Plot2_AdStateInitGuassian.png")

r0 = rhsAdvection(s0)
plot([evaluate(r0.u, x) for x in xs])

rm("C:/Julia/FinDiff2018/test/Plot2_RHSadvection.png", force=true)
savefig("Plot2_RHSadvection.png")

############################################################
##			Time evolution
############################################################
s1 = euler(rhsAdvection, 0.1, s0)

plot([evaluate(s0.u, x) for x in xs])
plot!([evaluate(s1.u, x) for x in xs])

rm("C:/Julia/FinDiff2018/test/Plot3_EulerRHSadvection.png", force=true)
savefig("Plot3_EulerRHSadvection.png")

sol1 = solveAdvection(1.0, 100, 0.5);
sol2 = solveAdvection(1.0, 200, 0.5);

function plotSolution(sol, every)
    plot!([evaluate(sol1.states[1].u, x) for x in xs])
    for state in sol1.states[every:every:end]
        plot!([evaluate(state.u, x) for x in xs])
    end
    plot!()
end

plot()
plotSolution(sol1, 10)

rm("C:/Julia/FinDiff2018/test/Plot3_solveAdvection.png", force=true)
savefig("Plot3_solveAdvection.png")

############################################################
##			Discretization Error, Convergence
############################################################

sol1 = solveAdvection(1.0, 100, 0.5);
sol2 = solveAdvection(1.0, 200, 0.5);
sol3 = solveAdvection(1.0, 400, 0.5);

@show (sol1.states[2].time, sol2.states[3].time, sol3.states[5].time)

ue1 = refine(sol1.states[2].u) + (-1.0) * sol2.states[3].u
ue2 = refine(sol2.states[3].u) + (-1.0) * sol3.states[5].u;

plot([evaluate(ue1, x) for x in xs])
plot!([evaluate(ue2, x) for x in xs])

rm("C:/Julia/FinDiff2018/test/Plot4_descreteError_Convergence.png", force=true)
savefig("Plot4_descreteError_Convergence.png")


using MLJ
import RDatasets: dataset
using PrettyPrinting
import DataFrames: DataFrame, select, Not
DTC = @load DecisionTreeClassifier pkg=DecisionTree

carseats = dataset("ISLR", "Carseats")

first(carseats, 3) |> pretty

High = ifelse.(carseats.Sales .<= 8, "No", "Yes") |> categorical;
carseats[!, :High] = High;

X = select(carseats, Not([:Sales, :High]))
y = carseats.High;

HotTreeClf = OneHotEncoder() |> DTC()

mdl = HotTreeClf
mach = machine(mdl, X, y)
fit!(mach);

ypred = predict_mode(mach, X)
misclassification_rate(ypred, y)

train, test = partition(eachindex(y), 0.5, shuffle=true, rng=333)
fit!(mach, rows=train)
ypred = predict_mode(mach, rows=test)
misclassification_rate(ypred, y[test])

r_mpi = range(mdl, :(decision_tree_classifier.max_depth), lower=1, upper=10)
r_msl = range(mdl, :(decision_tree_classifier.min_samples_leaf), lower=1, upper=50)

tm = TunedModel(
    model=mdl,
    ranges=[r_mpi, r_msl],
    tuning=Grid(resolution=8),
    resampling=CV(nfolds=5, rng=112),
    operation=predict_mode,
    measure=misclassification_rate
)
mtm = machine(tm, X, y)
fit!(mtm, rows=train)

ypred = predict_mode(mtm, rows=test)
misclassification_rate(ypred, y[test])

fitted_params(mtm).best_model.decision_tree_classifier

DTR = @load DecisionTreeRegressor pkg=DecisionTree

boston = dataset("MASS", "Boston")

y, X = unpack(boston, ==(:MedV))

train, test = partition(eachindex(y), 0.5, shuffle=true, rng=551);

scitype(X)

X = coerce(X, autotype(X, rules=(:discrete_to_continuous,)))

dtr_model = DTR()
dtr = machine(dtr_model, X, y)

fit!(dtr, rows=train)

ypred = MLJ.predict(dtr, rows=test)
round(rms(ypred, y[test]), sigdigits=3)

r_depth = range(dtr_model, :max_depth, lower=2, upper=20)

tm = TunedModel(
    model=dtr_model,
    ranges=[r_depth],
    tuning=Grid(resolution=10),
    resampling=CV(nfolds=5, rng=1254),
    measure=rms
)

mtm = machine(tm, X, y)

fit!(mtm, rows=train)

ypred = MLJ.predict(mtm, rows=test)
round(rms(ypred, y[test]), sigdigits=3)

RFR = @load RandomForestRegressor pkg=ScikitLearn

rf_mdl = RFR()
rf = machine(rf_mdl, X, y)
fit!(rf, rows=train)

ypred = MLJ.predict(rf, rows=test)
round(rms(ypred, y[test]), sigdigits=3)

XGBR = @load XGBoostRegressor

xgb_mdl = XGBR(num_round=10, max_depth=10)
xgb = machine(xgb_mdl, X, y)
fit!(xgb, rows=train)

ypred = MLJ.predict(xgb, rows=test)
round(rms(ypred, y[test]), sigdigits=3)

# This file was generated using Literate.jl, https://github.com/fredrikekre/Literate.jl


using MarkovModelAndMMs.MarkovStuff
using MarkovModelAndMMs.MMs
using MarkovModelAndMMs.CostFunctions
using LinearAlgebra
using Test

let m = MarkovProcess([-1//2 1//4 1//4; 3//4 -1//1 1//4; 1//2 1//6 -2//3]),
	c = MarkovChain([0 1//2 1//2; 3//4 0 1//4; 3//4 1//4 0]),
	λ = [1//2, 1//1, 2//3]
	@test MarkovProcess(c, λ).G == m.G
	@test imbeddedchain(m).P == c.P
end


# # Welford's algorithm
#
# * Welford, B. P. (1962). "Note on a method for calculating corrected sums of squares and products". Technometrics. 4 (3): 419–420. doi:10.2307/1266577. JSTOR 1266577
# * [Welford's online algorithm - Algorithms for calculating variance - Wikipedia](https://en.wikipedia.org/wiki/Algorithms_for_calculating_variance#Welford's_online_algorithm)

using Transducers

using Transducers: R_, start, next, complete, inner, xform, wrap, unwrap, wrapping

using Measurements

export Mean

struct Mean <: Transducer
end

function Transducers.start(rf::R_{Mean}, result)
    # @info "Transducers.start" result
    private_state = (0, 0.0, 0.0)
    return wrap(rf, private_state, start(inner(rf), result))
end

function Transducers.next(rf::R_{Mean}, result, input)
    # @info "Transducers.next" result input
    wrapping(rf, result) do st, iresult
        (n, μ, M2) = st
        n += 1
        δ = input - μ
        μ += δ/n
        δ2 = input - μ
        M2 += δ*δ2
        stderr = sqrt(M2)/n
        iinput = μ ± stderr
        iresult = next(inner(rf), iresult, iinput)
        return (n, μ, M2), iresult
    end
end

function Transducers.complete(rf::R_{Mean}, result)
    # @info "Transducers.complete" result
    _private_state, inner_result = unwrap(rf, result)
    return complete(inner(rf), inner_result)
end


include("evaluate_lambda.jl")

using PyPlot

α = [-0.5;0.5]
L = round.(Int, exp10.(range(0, stop=4, length=50)))
λlap = [-ℓ*(ℓ+1) for ℓ in L]

for i = 1:length(α)
    δ1 = 0.01
    λ1 = evaluate_lambda(L[end]+1, α[i], δ1)[L.+1]
    δ2 = 0.1
    λ2 = evaluate_lambda(L[end]+1, α[i], δ2)[L.+1]
    δ3 = 1.0
    λ3 = evaluate_lambda(L[end]+1, α[i], δ3)[L.+1]
    δ4 = 2.0
    λ4 = evaluate_lambda(L[end]+1, α[i], δ4)[L.+1]

    clf()
    loglog(L, -λlap, "-k", L, -λ1, ".k", L, -λ2, "xk", L, -λ3, "+k", L, -λ4, "--k")
    xlabel("\$\\ell\$"); ylabel("Numerical Evaluation of \$\\lambda_\\delta(\\ell)\$ at \$\\alpha=$(α[i])\$"); grid()
    legend(["\$-\\lambda_0(\\ell)\$","\$-\\lambda_{$(δ1)}(\\ell)\$","\$-\\lambda_{$(δ2)}(\\ell)\$","\$-\\lambda_{$(δ3 == Int(δ3) ? Int(δ3) : δ3)}(\\ell)\$","\$-\\lambda_{$(δ4 == Int(δ4) ? Int(δ4) : δ4)}(\\ell)\$"])
    savefig("plots/plot_spectrum$(i).pdf")
end


#could be written more efficiently
#but is not meant to be evaluated repeatedly like the likelihood
#so not a priority

function viterbi(model::DynamicDiscreteModel,data::Array{Int,1})
	T=length(data)
	dx=size(model.mu,1)

	memory=Array(Int,dx,T-1)

	model.psi[:]=model.mu[:,data[1]]/sum(model.mu[:,data[1]])
	for t=2:T
		mm=reshape(model.m[:,data[t-1],:,data[t]],(dx,dx))
		for jx=1:dx
			model.psi[jx],memory[jx,t-1]=findmax(model.psi.*mm[:,jx])
		end
		model.psi[:]=model.psi/sum(model.psi)
	end
	viterbipath=Array(Int,T)
	viterbipath[T]=indmax(model.psi)
	for t=T-1:-1:1
		viterbipath[t]=memory[viterbipath[t+1],t]
	end
	viterbipath
end

#wrapper for panel data
function viterbi(model::DynamicDiscreteModel,data::Array{Array,1})
	n=length(data)
	viterbipaths=Array(Array{Int,1},n)
	for i=1:n
		viterbipaths[i]=viterbi(model,data[i])
	end
	viterbipaths
end


using Test
using LogicCircuits
using ProbabilisticCircuits
using DataFrames: DataFrame

include("../helper/gpu.jl")

@testset "Likelihood" begin
    # Uses a PC with 4 variables, and tests 3 of the configurations to
    # match with python. Also tests all probabilities sum up to 1.

    prob_circuit = zoo_psdd("little_4var.psdd");
    @test prob_circuit isa ProbCircuit;

    # Step 1. Check Probabilities for 3 samples
    data = DataFrame(BitArray([0 0 0 0; 0 1 1 0; 0 0 1 1]));
    true_prob = [0.07; 0.03; 0.13999999999999999]

    calc_prob = EVI(prob_circuit, data)
    calc_prob = exp.(calc_prob)

    @test true_prob ≈ calc_prob atol=1e-7;

    # Step 2. Add up all probabilities and see if they add up to one
    N = 4;
    data_all = generate_data_all(N)

    calc_prob_all = EVI(prob_circuit, data_all)
    calc_prob_all = exp.(calc_prob_all)
    sum_prob_all = sum(calc_prob_all)

    @test 1 ≈ sum_prob_all atol = 1e-7;

    cpu_gpu_agree_approx(data_all) do d
        EVI(prob_circuit, d)
    end

    # make sure log-likelihoods are -Inf when the input is not satisfied
    alltrue = multiply(pos_literals(ProbCircuit,4))
    @test all(EVI(alltrue, data) .== -Inf)

    cpu_gpu_agree(data) do d
        EVI(alltrue, d)
    end

end

@testset "Bagging models' likelihood" begin
    dfb = DataFrame(BitMatrix([true true; true true; true true; true true]))
    r = fully_factorized_circuit(ProbCircuit,num_features(dfb))
    # bag_dfb = bagging_dataset(dfb; num_bags = 2, frac_examples = 1.0)
    bag_dfb = Array{DataFrame}(undef, 2)
    bag_dfb[1] = dfb[[2, 1, 3, 4], :]
    bag_dfb[2] = dfb[[4, 3, 2, 1], :]

    r = compile(SharedProbCircuit, r, 2)

    estimate_parameters(r, bag_dfb; pseudocount = 1.0)

    ll = log_likelihood_per_instance(r, dfb)
    @test ll[1] ≈ -0.2107210 atol = 1e-6
    @test ll[2] ≈ -0.2107210 atol = 1e-6
    @test ll[3] ≈ -0.2107210 atol = 1e-6
    @test ll[4] ≈ -0.2107210 atol = 1e-6

    @test log_likelihood(r, dfb) ≈ -0.8428841 atol = 1e-6

    @test log_likelihood_avg(r, dfb) ≈ -0.2107210 atol = 1e-6
end


"""
Some statistical functions, mainly related to (weighted) median computations.
"""
module Stats

import ..GraphIdx: Const, Vec, Ones

"""
    permcumsum(w, pi)

Return the cumulative sums over `w` in order `pi`.
"""
function permcumsum(w::Array{W}, pi::Vector{I}) where {W, I<:Integer}
    local acc::W = 0
    for j in pi
        acc += w[j]
        w[j] = acc
    end
    return w
end

permcumsum(w::Const{W}, pi::Vector{I}) where {W, I<:Integer} =
    pi

permcumsum(w::Ones{W}, pi::Vector{I}) where {W, I<:Integer} =
    pi

"""
    weighted_median(x, [w, [∂ ]])

Compute a generalized weighted order statistics.
Hereby, we minimize
```math
\\min_{t} \\sum_{i} w_i |t - x_i| + f(t)
```
whereby `∂` is the derivative of the convex function ``f``
(by default constant `0` for ordinary weighted median).

Return the interval ``[a, b]`` containing the minimizer (if unique ``a = b``).

!!! todo

    Compare the sorting to binary search and median strategies.
    Maybe use [SIMD for median](https://gist.github.com/jw3126/3a3c65009e96af2c4fcc96f701bd4913).

"""
weighted_median(x::Vector{X}) where {X} =
    weighted_median(x, Ones{X}())

function weighted_median(
    x::Vector{X},
    ww::W,
    ∂::F = _ -> 0,
    pi::Vector{Int} = Int[],
)::Tuple{X,X} where {X, W, F <: Function}
    if ww isa Array
        @assert length(x) == length(ww)
    end
    local n = length(x)
    resize!(pi, n)
    sortperm!(pi, x)

    # Precompute  wsum(i) = sum(-w[j] for j < i) + sum(+w[j] for j > i) + ∂(x[i])
    local w = permcumsum(ww, pi)

    local wsum0 = -w[pi[end]]
    w0(i::Int) = i == 1 ? w[pi[1]] : w[pi[i]] - w[pi[i-1]]
    wsum(i::Int) = wsum0 + ∂(x[pi[i]]) + w[pi[i]] + (i == 1 ? 0 : w[pi[i-1]])

    # sum of abs "derivative" if t < x[first] (and x is sorted)
    local first::Int, last::Int = 1, length(x)
    while last - first > 1
        local mid::Int = (first + last) ÷ 2
        if wsum(mid) < 0
            first = mid
        else
            last = mid
        end
    end
    # @show first, last, wsum(first), wsum(last)
    if wsum(first) + w0(first) > 0
        return x[pi[first]], x[pi[first]]
    elseif wsum(last) - w0(last) < 0
        return x[pi[last]], x[pi[last]]
    else
        return x[pi[first]], x[pi[last]]
    end
end

end


using OrthogonalPolynomialsQuasi, BandedMatrices, ArrayLayouts, Test
import OrthogonalPolynomialsQuasi: recurrencecoefficients, PaddedLayout

@testset "Lanczos" begin
    @testset "Legendre" begin
        P = Legendre();
        w = P * [1; zeros(∞)];
        Q = LanczosPolynomial(w);
        @test Q.data.W[1:10,1:10] isa BandedMatrix

        Q̃ = Normalized(P);
        A,B,C = recurrencecoefficients(Q);
        Ã,B̃,C̃ = recurrencecoefficients(Q̃);
        @test @inferred(A[1:10]) ≈ Ã[1:10] ≈ [A[k] for k=1:10]
        @test @inferred(B[1:10]) ≈ B̃[1:10] ≈ [B[k] for k=1:10]
        @test @inferred(C[2:10]) ≈ C̃[2:10] ≈ [C[k] for k=2:10]

        @test A[1:10] isa Vector{Float64}
        @test B[1:10] isa Vector{Float64}
        @test C[1:10] isa Vector{Float64}

        @test Q[0.1,1:10] ≈ Q̃[0.1,1:10]

        R = P \ Q
        @test R[1:10,1:10] ≈ (P \ Q̃)[1:10,1:10]
        @test (Q'Q)[1:10,1:10] ≈ I

        # Q'Q == I => Q*sqrt(M) = P

        x = axes(P,1)
        X = Q' * (x .* Q)
        X̃ = Q̃' * (x .* Q̃)
        @test X[1:10,1:10] ≈ X̃[1:10,1:10]
    end

    @testset "other weight" begin
        P = Normalized(Legendre())
        x = axes(P,1)

        Q = LanczosPolynomial(exp.(x))
        R = P \ Q
        @test MemoryLayout(R[:,2]) isa PaddedLayout

        A,B,C = recurrencecoefficients(Q)
        @test A[1] ≈ 1.903680130866564 # emperical from Mathematica
        @test B[1] ≈ -0.5959190532652192
        @test A[2] ≈ 1.9150612001588696
        @test B[2] ≈ 0.0845629033308663
        @test C[2] ≈ 1.005978456731134

        @test Q[0.1,1] ≈ (Q * [1; zeros(∞)])[0.1] ≈ P[0.1,:]'*R[:,1] ≈ 0.6522722316024658
        @test Q[0.1,2] ≈ (Q * [0; 1; zeros(∞)])[0.1] ≈ P[0.1,:]'*R[:,2] ≈ -0.26452968200597243
        @test Q[0.1,3] ≈ (Q * [zeros(2); 1; zeros(∞)])[0.1] ≈ P[0.1,:]'*R[:,3] ≈ -0.7292002638736375
        @test Q[0.1,5] ≈ 0.7576999562707534 # emperical

        Q = LanczosPolynomial(  1 ./ (2 .+ x));
        R = P \ Q
        @test norm(R[1,3:10]) ≤ 1E-14

        Q = LanczosPolynomial(  1 ./ (2 .+ x).^2);
        R = P \ Q
        @test norm(R[1,4:10]) ≤ 1E-14

        # polys
        Q = LanczosPolynomial( 2 .+ x);
        R = P \ Q;
        @test norm(inv(R)[1,3:10]) ≤ 1E-14

        w = P * (P \ (1 .+ x))
        Q = LanczosPolynomial(w)
        @test Q[0.1,5] ≈ Normalized(Jacobi(0,1))[0.1,5] ≈ 0.742799258138176

        Q = LanczosPolynomial( 1 .+ x.^2);
        R = P \ Q;
        @test norm(inv(R)[1,4:10]) ≤ 1E-14
    end

    @testset "Expansion" begin
        P = Legendre();
        x = axes(P,1)
        w = P * [1; zeros(∞)];
        Q = LanczosPolynomial(w);
        R = Normalized(P) \ Q
        @test R * [1; 2; 3; zeros(∞)] ≈ [R[1:3,1:3] * [1,2,3]; zeros(∞)]
        @test R \ [1; 2; 3; zeros(∞)] ≈ [1; 2; 3; zeros(∞)]
        @test (Q * (Q \ (1 .- x.^2)))[0.1] ≈ (1-0.1^2)
    end

    @testset "Jacobi via Lanczos" begin
        P = Legendre(); x = axes(P,1)
        w = P * (P \ (1 .- x.^2))
        Q = LanczosPolynomial(w)
        A,B,C = recurrencecoefficients(Q)

        @test @inferred(Q[0.1,1]) ≈ sqrt(3)/sqrt(4)
        @test Q[0.1,2] ≈ 2*0.1 * sqrt(15)/sqrt(16)
    end

    @testset "Singularity" begin
        T = Chebyshev(); wT = WeightedChebyshev()
        x = axes(T,1)

        w = wT * [1; zeros(∞)];
        Q = LanczosPolynomial(w)
        @test Q[0.1,1:10] ≈ Normalized(T)[0.1,1:10]

        @test (Q'*(ChebyshevTWeight() .* Q))[1:10,1:10] ≈ I
        @test (Q'*(w .* Q))[1:10,1:10] ≈ I
    end

    @testset "BigFloat" begin
        P = Legendre{BigFloat}()
        x = axes(P,1)
        w = P * (P \ exp.(x))
        W = P \ (w .* P)
        v = [[1,2,3]; zeros(BigFloat,∞)];
        Q = LanczosPolynomial(w)

        x̃ = BigFloat(1)/10
        @test Q[x̃,1] ≈ 0.652272231602465791008015756161075576539994569266308567422055126278763683344388252
        @test Q[x̃,2] ≈ -0.26452968200597244253463861322599173806126678155361307561211048667577270734771616
        @test Q[x̃,3] ≈ -0.72920026387366053084159259908849371062183891778315602761397748592062615496583854

        X = Q \ (x .* Q)
        # empirical test
        @test X[5,5] ≈ -0.001489975039238321407179828331585356464766466154894764141171294038822525312179884

        @test (Q*[1; 2; 3; zeros(BigFloat,∞)])[0.1] ≈ -2.0643879240304606865860392675563890314480557471903856666440983048346601962485597
        @test 0.1*(Q*[1; 2; 3; zeros(BigFloat,∞)])[0.1] ≈ (Q * (X * [1; 2; 3; zeros(BigFloat,∞)]))[0.1]
    end

    @testset "Mixed Jacobi" begin
        P = Jacobi(1/2,0)
        x = axes(P,1)

        w = @. sqrt(1-x)
        Q = LanczosPolynomial(w, P)
        @test Q[0.1,1:10] ≈ Normalized(P)[0.1,1:10]

        w = @. exp(x) * sqrt(1-x)
        Q = LanczosPolynomial(w, P)
        # emperical from Julia
        @test Q[0.1,10] ≈ 0.5947384257847858
    end

    @testset "Mapped" begin
        x = Inclusion(0..1)
        w = @. sqrt(1 - x^2)
        Q = LanczosPolynomial(w, jacobi(1/2,0,0..1))
        # emperical from Julia
        @test Q[0.1,10] ≈ -0.936819626414421

        @test (Q * (Q \ exp.(x)))[0.1] ≈ exp(0.1)
        @test (Q[:,1:20] \ exp.(x)) ≈ (Q \ exp.(x))[1:20]

        @testset "Mapped Conversion" begin
            P₊ = jacobi(0,1/2,0..1)
            x = axes(P₊,1)
            y = @.(sqrt(x)*sqrt(2-x))
            U = LanczosPolynomial(y, P₊)
            @test P₊ ≠ U
            R = P₊ \ U
            @test U[0.1,5] ≈ (P₊ * R * [zeros(4); 1; zeros(∞)])[0.1]
        end
    end

    @testset "orthogonality (#68)" begin
        α = -0.5
        β = -0.5
        w = JacobiWeight(α,β)
        p = LanczosPolynomial(w)
        x = axes(w,1)
        b = 2
        wP = WeightedJacobi(α+1,β+1)
        ϕw = wP * (Jacobi(α+1,β+1) \ (b .- x))
        pϕ = LanczosPolynomial(ϕw)
        @test (pϕ.P' * (ϕw .* pϕ.P))[1:3,1:3] ≈ [2 -0.5 0; -0.5 2 -0.5; 0 -0.5 2]
        @test (pϕ' * (ϕw .* pϕ))[1:5,1:5] ≈ I

        ϕw = JacobiWeight(α+1,β+1) .* (b .- x)
        pϕ = LanczosPolynomial(ϕw)
        @test (pϕ.P' * (ϕw .* pϕ.P))[1:3,1:3] ≈ [2 -0.5 0; -0.5 2 -0.5; 0 -0.5 2]
        @test (pϕ' * (ϕw .* pϕ))[1:5,1:5] ≈ I
    end
end


using Unitful

import Unitful:
    nm, μm, mm, cm, m, km,
    mg, g, kg,
    ps, ns, μs, ms, s, minute, hr, d, yr, Hz, kHz, MHz, GHz,
    eV,
    μJ, mJ, J,
	μW, mW, W

	"""
	GaussianLaser

	Representation of a Gaussian laser defined by a laser, the location of the waist (z0) and the waist radius (w0)

	# Fields
	- `laser::Laser`       : A Laser
	- `w0::Unitful.Length` : radius of the waist
	- `z0::Unitful.Length` : location of the waist (computed from w0)
	- `I0::typeof(1.0mW/cm^2)` : Intensity of the beam at (0,0) (computed from w0)
	- `θ0::Real ` : divergence of the beam  (computed from w0)

	"""
	struct GaussianLaser
		laser::Laser
		w0::Unitful.Length
		z0::Unitful.Length
		I0::typeof(1.0mW/cm^2)
		γ0::typeof(1.0Hz/cm^2)
		θ0::Real

		function GaussianLaser(laser, w0)
			z0 = π * w0^2 / laser.λ
			I0 = 2 * laser.P / (π * w0^2)
			nγ = uconvert(Hz, laser.P / photon_energy(laser.λ))
			γ0 = 2 * nγ / (π * w0^2)
			θ0 = w0/z0
			new(laser, w0, z0, I0, γ0, θ0)
		end
	end

	"""
	W(gl::GaussianLaser)

returns the beam width: ``W(z) = W_0 \\sqrt{1 + (z/z_0)^2}``

# Fields
	- `gl::GaussianLaser`       : A gaussian Laser

"""
function W(gl::GaussianLaser)
	Wz(z::Unitful.Length) = gl.w0 * sqrt(1.0 + (z/gl.z0)^2)
	return Wz
end

"""
R(gl::GaussianLaser)

returns the beam radius of curvature: ``R(z) = z ( 1 + (z_0/z)^2) ``

# Fields
- `gl::GaussianLaser`       : A gaussian Laser

"""
function R(gl::GaussianLaser)
	Rz(z::Unitful.Length) = z * (1.0 + (gl.z0/z)^2)
	return Rz
end

"""
I(gl::GaussianLaser)

returns the beam Intensity: ``I(\\rho, z) = I_0 ( W_0 / W(z))^2 \\exp{-2 \\rho^2/W^2(z)}  ``

# Fields
- `gl::GaussianLaser`       : A gaussian Laser

"""
function I(gl::GaussianLaser)
	Wz = W(gl)
	Irz(ρ::Unitful.Length, z::Unitful.Length) = gl.I0 * (gl.w0 / Wz(z))^2 * exp(-2.0 * ρ^2/Wz(z)^2)
	return Irz
end

"""
	spot_size(gl::GaussianLaser)

returns 2*w0 that is the diameter of the beam waist, also called spot size

# Fields

- `gl::GaussianLaser` : A gaussian laser

"""
spot_size(gl::GaussianLaser)  = 2 * gl.w0

"""
	angular_divergence(gl::GaussianLaser)

returns 2θ that is the beam angular divergence

# Fields

- `gl::GaussianLaser` : A gaussian laser

"""
angular_divergence(gl::GaussianLaser)  = 2 * gl.θ0

"""
	depth_of_focus(gl::GaussianLaser)

returns 2z0

# Fields

- `gl::GaussianLaser` : A gaussian laser

"""
depth_of_focus(gl::GaussianLaser)  = 2 * gl.z0

"""
	propagate_paralel_beam(laser::Laser, obj::Objective)

returns a gaussian laser with parameter w0 resulting from focusing the paralell laser beam
filling the entrance pupil of the objective.

# Fields

- `laser::Laser` : A  laser filling the entrance pupil of the objective (assuming parallell rays)
- `obj::Objective` : An objective, used to focuse the laser in the sample.

"""
function propagate_paralell_beam(laser::Laser, obj::Objective)
	w0 = laser.λ / (π * obj.NA)
	GaussianLaser(laser, w0)
end


# furnsh("src/kernels/de440s.bsp")
# furnsh("src/kernels/naif0012.tls")

"""
    rot2inert(rv, θ, μ; origin=:barycenter)

Convert state vector `rv = [r; v]` {NON; NON} from rotating (synodic) frame to inertial
frame in the normalized CR3BP given time `θ` {NON} and mass parameter `μ` {NON}.
"""
function rot2inert(rv, θ, μ; origin=:barycenter)
    if origin == :barycenter
    elseif origin == :prim
        rv -= [-μ, 0, 0, 0, 0, 0]
    elseif origin == :sec
        rv -= [1-μ, 0, 0, 0, 0, 0]
    end
    cθ, sθ = cos(θ), sin(θ)
    A =  [cθ -sθ  0;
          sθ  cθ  0;
           0   0  1]
    B = [-sθ -cθ  0;
          cθ -sθ  0;
           0   0  0]
    C = [A zeros(3,3);
         B A]
    return C*rv
end

"""
    rot2inert(rv, θ, sys::System; origin=:barycenter)

Convert state vector `rv = [r; v]` {NON; NON} from rotating (synodic) frame to inertial
frame in the normalized CR3BP given time `θ` {NON} and CR3BP system `sys`.
"""
rot2inert(rv, θ, sys::System; origin=:barycenter) = rot2inert(rv, θ, sys.μ, origin=origin)

"""
    rot2inert(rv, θ, p::Array; origin=:barycenter)

Convert state vector `rv = [r; v]` {NON; NON} from rotating (synodic) frame to inertial
frame in the non-normalized CR3BP given time `θ` {NON} and p = [μ₁;μ₂;d]`
{km³/s²; km³/s²; km}, which contains the gravitational parameters of the first and second
primary bodies as well as the distance between them.
"""
function rot2inert(rv, θ, p::Array; origin=:barycenter)
    μ₁,μ₂,d = p # parameters

    if origin == :barycenter
    elseif origin == :prim
        rv -= [-d*μ₂/(μ₁ + μ₂), 0, 0, 0, 0, 0]
    elseif origin == :sec
        rv -= [d*(1-μ₂/(μ₁ + μ₂)), 0, 0, 0, 0, 0]
    end

    ωₛ = sqrt((μ₁ + μ₂)/d^3);
    cθ, sθ = cos(ωₛ*θ), sin(ωₛ*θ)
    A = [cθ -sθ  0;
         sθ  cθ  0;
          0   0  1]
    B = [-sθ -cθ  0;
          cθ -sθ  0;
           0   0  0]
    C = [A zeros(3,3);
         B A]
    return C*rv
end

"""
    rot2inert!(rv, θ, μ)

In-place version of rot2inert(rv, θ, μ)
"""
function rot2inert!(rv, θ, μ; origin=:barycenter)
    rv[1:6] = rot2inert(rv, θ, μ, origin=origin)
    return nothing
end

"""
    rot2inert!(rv, θ, sys::System)

In-place version of rot2inert(rv, θ, sys::System)
"""
function rot2inert!(rv, θ, sys::System; origin=:barycenter)
    rv[1:6] = rot2inert(rv, θ, sys, origin=origin)
    return nothing
end

"""
    rot2inert!(rv, θ, p::Array)

In-place version of rot2inert(rv, θ, p::Array)
"""
function rot2inert!(rv, θ, p::Array; origin=0)
    rv[1:6] = rot2inert(rv, θ, p, origin=origin)
    return nothing
end

"""
    inert2rot(rv, θ, μ)

Inertial frame to rotating (synodic) frame
"""
function inert2rot(rv, t, μ; origin=:barycenter)
    ct, st = cos(t), sin(t)
    A =  [ct  st  0;
         -st  ct  0;
           0   0  1]
    # A = rotz(-t)
    B = [-st  ct  0;
         -ct -st  0;
           0   0  0]
    C = [A zeros(3,3);
         B A]
    if origin == :barycenter
    elseif origin == :prim
        rv += [-μ, 0, 0, 0, 0, 0]
    elseif origin == :sec
        rv += [1-μ, 0, 0, 0, 0, 0]
    end
    return C*rv
end

"""
    inert2rot(rv, θ, sys::System)

Inertial frame to rotating (synodic) frame
"""
inert2rot(rv, t, sys::System; origin=:barycenter) = inert2rot(rv, t, sys.μ, origin=origin)

"""
    inert2rot(rv, θ, p::Array)

Inertial frame to rotating (synodic) frame
"""
function inert2rot(rv, t, p::Array; origin=:barycenter)
# function S2I!(rv,t,p::Array)
    μ₁,μ₂,d = p # parameters

    ωₛ = sqrt((μ₁ + μ₂)/d^3);
    ct, st = cos(ωₛ*t), sin(ωₛ*t)
    A = [ct  st  0;
        -st  ct  0;
          0   0  1]
    # A = rotz(ωₛ*t)
    B = [-st  ct  0;
         -ct -st  0;
           0   0  0]
    C = [A zeros(3,3);
         B A]

    if origin == :barycenter
    elseif origin == :prim
        rv += [-d*μ₂/(μ₁ + μ₂), 0, 0, 0, 0, 0]
    elseif origin == :sec
        rv += [d*(1-μ₂/(μ₁ + μ₂)), 0, 0, 0, 0, 0]
    end

    return C*rv
end

"""
    inert2rot!(rv, t, μ)

In-place version of `inert2rot(rv, t, μ)`.
"""
function inert2rot!(rv, t, μ; origin=:barycenter)
    # function inert2rot!(rv, t)
    rv[1:6] = inert2rot(rv, t, μ, origin=origin)
    return nothing
end

"""
    inert2rot!(rv, t, sys::System)

In-place version of `inert2rot(rv, t, sys::System)`.
"""
function inert2rot!(rv, t, sys::System; origin=:barycenter)
    rv[1:6] = inert2rot(rv, t, sys, origin=origin)
    return nothing
end

"""
    inert2rot!(rv, t, p::Array)

In-place version of `inert2rot(rv, t, p::Array)`.
"""
function inert2rot!(rv, t, p::Array; origin=0)
    rv[1:6] = inert2rot(rv, t, p, origin=origin)
    return nothing
end

"""
    ecef2eci(rv_ecef, θ; ω=7.292115373194e-5, ang_unit::Symbol=:rad)

Convert state vector `rv = [r; v]` {km; km/s} from Earth-Centered Earth-Fixed (ECEF) frame
to Earth-Centered Inertial (ECI) frame given Greenwich Mean Sidereal Time (GMST) `θ` {rad}.
"""
function ecef2eci(rv_ecef, θ; ω=7.292115373194e-5, ang_unit::Symbol=:rad)
    if ang_unit == :deg
        θ = deg2rad(θ)
    elseif ang_unit == :rad
    else
        error("ang_unit should be :rad or :deg")
    end

    R = rotz(θ)
    O = zeros(3,3)
    R_dot = ω*[-sin(θ) -cos(θ) 0;
                cos(θ) -sin(θ) 0;
                     0       0 0]

    rv_eci = [R O; R_dot R]*rv_ecef

    return rv_eci
end

"""
    ecef2eci(rv_ecef, θ; ω=7.292115373194e-5, ang_unit::Symbol=:rad)

Convert state vector `rv = [r; v]` {km; km/s} from Earth-Centered Inertial (ECI) frame to
Earth-Centered Earth-Fixed (ECEF) frame given Greenwich Mean Sidereal Time (GMST) `θ` {rad}.
"""
function eci2ecef(rv_eci, θ; ω=7.292115373194e-5, ang_unit::Symbol=:rad)
    if ang_unit == :deg
        θ = deg2rad(θ)
    elseif ang_unit == :rad
    else
        error("ang_unit should be :rad or :deg")
    end

    R = rotz(-θ)
    O = zeros(3,3)
    R_dot = ω*[sin(-θ) cos(-θ) 0;
              -cos(-θ) sin(-θ) 0;
                     0       0 0]

    rv_ecef = [R O; R_dot R]*rv_eci

    return rv_ecef
end

"""
    eci2sci(rv_eci, rv_sun_eci; ε=23.43929)
"""
function eci2sci(rv_eci, rv_sun_eci; ε=23.439292)
    R = rotx(-ε)
    O = zeros(3,3)
    rv_sci = [R O; O R]*(rv_eci-rv_sun_eci)
    return rv_sci
end

"""
    sci2eci(rv_sci, rv_sun_eci; ε=23.439292)
"""
function sci2eci(rv_sci, rv_sun_eci; ε=23.439292)
    R = rotx(ε)
    O = zeros(3,3)
    rv_eci = [R O; O R]*rv_sci + rv_sun_eci
    return rv_eci
end

"""
    enu2ecef(rv_enu, ϕ, λ, h=0; geodetic=true, ang_unit=:deg)
Converts state from ENU frame to inertial frame
"""
function enu2ecef(rv_enu, ϕ, λ, h=0; geodetic=true, ang_unit::Symbol=:deg, e_earth=0.0818, r_earth=6.378136e3)
    d = h + r_earth # [km] radius of the earth plus altitude
    if geodetic # geodetic latitude
        if ang_unit == :rad
            N = r_earth/sqrt(1-(e_earth^2)*sin(λ)^2)
        elseif ang_unit == :deg
            N = r_earth/sqrt(1-(e_earth^2)*sind(λ)^2)
        else
            error("ang_unit should be :deg or :rad")
        end
        r_radar_ecef = latlon2cart(ϕ, λ, 1, ang_unit=ang_unit)
        geodetic_factors = [N + h; N + h; N*(1-e_earth^2) + h]
        r_radar_ecef = r_radar_ecef.*geodetic_factors
    else # geocentric latitude and longitude
        r_radar_ecef = latlon2cart(ϕ, λ, d, ang_unit=ang_unit)
    end

    R = rotlatlon(ϕ, λ, ang_unit=ang_unit)

    r_ecef = R*rv_enu[1:3] + r_radar_ecef
    v_ecef = R*rv_enu[4:6]
    rv_ecef = [r_ecef; v_ecef]
    return rv_ecef
end

"""
    ecef2enu(rv_ecef, ϕ, λ, h=0; geodetic=true, ang_unit::Symbol=:deg, e_earth=0.0818, r_earth=6.378136e3)
Converts state from ENU frame to inertial frame
"""
function ecef2enu(rv_ecef, ϕ, λ, h=0; geodetic=true, ang_unit::Symbol=:deg, e_earth=0.0818, r_earth=6.378136e3)
    d = h + r_earth # [km] radius of the earth plus altitude

    R = rotlatlon(ϕ, λ, ang_unit=ang_unit)

    if geodetic # geodetic latitude
        if ang_unit == :rad
            N = r_earth/sqrt(1-(e_earth^2)*sin(λ)^2)
        elseif ang_unit == :deg
            N = r_earth/sqrt(1-(e_earth^2)*sind(λ)^2)
        else
            error("ang_unit should be :deg or :rad")
        end
        r_radar_ecef = latlon2cart(ϕ, λ, 1, ang_unit=ang_unit)
        geodetic_factors = [N + h; N + h; N*(1-e_earth^2) + h]
        r_radar_ecef = r_radar_ecef.*geodetic_factors
    else # geocentric latitude and longitude
        r_radar_ecef = latlon2cart(ϕ, λ, d, ang_unit=ang_unit)
    end
    r_enu = R'*(rv_ecef[1:3] - r_radar_ecef)
    v_enu = R'*rv_ecef[4:6]
    rv_enu = [r_enu; v_enu]
    return rv_enu
end

"""
    latlon2cart(ϕ, λ, d; ang_unit::Symbol=:deg)
"""
function latlon2cart(ϕ, λ, d; ang_unit::Symbol=:deg)
    if ang_unit == :rad
        r = d*[cos(ϕ)*cos(λ), cos(ϕ)*sin(λ), sin(ϕ)]
    elseif ang_unit == :deg
        r = d*[cosd(ϕ)*cosd(λ), cosd(ϕ)*sind(λ), sind(ϕ)]
    else
        error("ang_unit should be :deg or :rad")
    end
    return r
end

"""
    cart2latlon(r::Array)
"""
function cart2latlon(r::Array, ang_unit::Symbol=:deg)
    x,y,z = r
    xy = sqrt(x^2 + y^2)
    d = norm(r)
    if ang_unit == :rad
        ϕ = asin(z/d)
        λ = atan(y,x)
    elseif ang_unit == :deg
        ϕ = asind(z/d)
        λ = atand(y,x)
    else
        error("ang_unit should be :deg or :rad")
    end
    return ϕ, λ, d
end
# y   r
# |  /
# | /  lon
# |/------- x

function eci2CR3BP()
end

function CR3BP2eci()
end

function CR3BP2cart()
end

function sci2CR3BP()
end

function CR3BP2sci()
end

"""
    dimensionalize(rv, sys::System)

Add dimensions to normalized state vector `rv`
"""
function dimensionalize(rv, sys::System)
    return vcat(rv[1:3]*sys.RUNIT, rv[4:6]*sys.VUNIT)
end

"""
    dimensionalize!(rv, sys::System)

In-place version of dimensionalize(rv, sys::System)
"""
function dimensionalize!(rv, sys::System)
    rv[1:6] = dimensionalize(rv,sys)
    return nothing
end

"""
    nondimensionalize(rv, sys::System)

Normalize state vector `rv`
"""
function nondimensionalize(rv, sys::System)
    return vcat(rv[1:3]/sys.RUNIT, rv[4:6]/sys.VUNIT)
end

"""
    nondimensionalize!(rv, sys::System)

In-place version of nondimensionalize(rv, sys::System)
"""
function nondimensionalize!(rv, sys::System)
    rv[1:6] = nondimensionalize(rv,sys)
    return nothing
end


module LagrangianDofBasesTests

using Test

using Gridap.TensorValues
using Gridap.Fields
using Gridap.ReferenceFEs

x1 = Point(0,0)
x2 = Point(1,0)
x3 = Point(0,1)
x4 = Point(1,1)
x = [x1,x2,x3,x4]
np = length(x)

db = LagrangianDofBasis(Float64,x)

@test db.nodes === x
@test db.node_and_comp_to_dof == collect(1:np)
@test db.dof_to_node == collect(1:np)
@test db.dof_to_comp == fill(1,np)

v = 3.0
f = GenericField(x->v*x[1])
fx = evaluate(f,x)
test_dof_array(db,f,fx)

ndof = 8
b = fill(f,ndof)
bx = evaluate(b,x)
test_dof_array(db,b,bx)

T = VectorValue{2,Float64}
db = LagrangianDofBasis(T,x)
@test db.nodes === x
@test db.node_and_comp_to_dof == VectorValue{2,Int}[(1, 5), (2, 6), (3, 7), (4, 8)]
@test db.dof_to_node == [1, 2, 3, 4, 1, 2, 3, 4]
@test db.dof_to_comp == [1, 1, 1, 1, 2, 2, 2, 2]

v = VectorValue(1,2)
f = GenericField(x->v*x[1])
dbf = [0, 1, 0, 1, 0, 2, 0, 2]

test_dof_array(db,f,dbf)

ndof = 8
b = fill(f,ndof)
bx = evaluate(b,x)
dbb = [
  0 0 0 0 0 0 0 0; 1 1 1 1 1 1 1 1; 0 0 0 0 0 0 0 0; 1 1 1 1 1 1 1 1;
  0 0 0 0 0 0 0 0; 2 2 2 2 2 2 2 2; 0 0 0 0 0 0 0 0; 2 2 2 2 2 2 2 2]

test_dof_array(db,b,dbb)

end # module


"""
    Mattsson2014

Coefficients of the SBP operators given in
  Mattsson (2014)
  Diagonal-norm summation by parts operators for fiite difference approximations
    of third and fourth derivatives.
  Journal of Computational Physics 274, pp. 432-454.
"""
struct Mattsson2014 <: SourceOfCoefficients end

function Base.show(io::IO, ::Mattsson2014)
    print(io,
        "  Mattsson (2014) \n",
        "  Diagonal-norm summation by parts operators for fiite difference approximations\n",
        "    of third and fourth derivatives. \n",
        "  Journal of Computational Physics 274, pp. 432-454. \n")
end

function first_derivative_coefficients(source::Mattsson2014, order::Int, T=Float64, parallel=Val{:serial}())
    if order == 2
        left_boundary = (
            # d1
            DerivativeCoefficientRow{T,1,2}(SVector(T(-1),
                                                    T(1) )),
        )
        right_boundary = .- left_boundary
        upper_coef = SVector(T(1//2))
        central_coef = zero(T)
        lower_coef = -upper_coef
        left_weights = SVector(T(1//2))
        right_weights = left_weights
        left_boundary_derivatives = Tuple{}()
        right_boundary_derivatives = left_boundary_derivatives

        DerivativeCoefficients(left_boundary, right_boundary,
                                left_boundary_derivatives, right_boundary_derivatives,
                                lower_coef, central_coef, upper_coef,
                                left_weights, right_weights, parallel, 1, order, source)
    elseif order == 4
        left_boundary = (
            # d1
            DerivativeCoefficientRow{T,1,6}(SVector(T(-50400//35809),
                                                    T(526249//322281),
                                                    T(-75733//1933686),
                                                    T(-50767//322281),
                                                    T(-4417//71618),
                                                    T(31850//966843) )),
            # d2
            DerivativeCoefficientRow{T,1,6}(SVector(T(-526249//1077057),
                                                    T(0),
                                                    T(1421209//3231171),
                                                    T(16657//239346),
                                                    T(-16934//1077057),
                                                    T(-33059//6462342) )),
            # d3
            DerivativeCoefficientRow{T,1,6}(SVector(T(75733//5541372),
                                                    T(-1421209//2770686),
                                                    T(0),
                                                    T(631187//1385343),
                                                    T(400139//5541372),
                                                    T(-8789//307854) )),
            # d4
            DerivativeCoefficientRow{T,1,6}(SVector(T(50767//811962),
                                                    T(-16657//180436),
                                                    T(-631187//1217943),
                                                    T(0),
                                                    T(496403//811962),
                                                    T(-308533//4871772) )),
            # d5
            DerivativeCoefficientRow{T,1,7}(SVector(T(4417//211146),
                                                    T(16934//950157),
                                                    T(-400139//5700942),
                                                    T(-496403//950157),
                                                    T(0),
                                                    T(1805647//2850471),
                                                    T(-2800//35191) )),
            # d6
            DerivativeCoefficientRow{T,1,8}(SVector(T(-31850//2713743),
                                                    T(33059//5427486),
                                                    T(8789//301527),
                                                    T(308533//5427486),
                                                    T(-1805647//2713743),
                                                    T(0),
                                                    T(22400//33503),
                                                    T(-2800//33503) )),
        )
        right_boundary = .- left_boundary
        upper_coef = SVector(T(2//3), T(-1//12))
        central_coef = zero(T)
        lower_coef = -upper_coef
        left_weights = SVector( T(35809//100800),
                                T(13297//11200),
                                T(5701//5600),
                                T(45109//50400),
                                T(35191//33600),
                                T(33503//33600) )
        right_weights = left_weights
        left_boundary_derivatives = Tuple{}()
        right_boundary_derivatives = left_boundary_derivatives

        DerivativeCoefficients(left_boundary, right_boundary,
                                left_boundary_derivatives, right_boundary_derivatives,
                                lower_coef, central_coef, upper_coef,
                                left_weights, right_weights, parallel, 1, order, source)
    elseif order == 6
        left_boundary = (
            # d1
            DerivativeCoefficientRow{T,1,8}(SVector(T(-508032//318365),
                                                    T(113221347//55519750),
                                                    T(-3338172//27759875),
                                                    T(-1002751721//2731571700),
                                                    T(-46815789//455261950),
                                                    T(1228638//9105239),
                                                    T(7120579//166559250),
                                                    T(-10874619//350201500) )),
            # d2
            DerivativeCoefficientRow{T,1,8}(SVector(T(-4642075227//10228748530),
                                                    T(0),
                                                    T(69095487//265681780),
                                                    T(32352081//146124979),
                                                    T(28592150//438374937),
                                                    T(-137946309//1461249790),
                                                    T(-720387//44961532),
                                                    T(538846039//30686245590) )),
            # d3
            DerivativeCoefficientRow{T,1,8}(SVector(T(45621684//696580885),
                                                    T(-23031829//36186020),
                                                    T(0),
                                                    T(28368209//39804622),
                                                    T(-9693137//39804622),
                                                    T(3868089//30618940),
                                                    T(-2468403//199023110),
                                                    T(-1686470//139316177) )),
            # d4
            DerivativeCoefficientRow{T,1,8}(SVector(T(1002751721//11591207628),
                                                    T(-32352081//137990567),
                                                    T(-85104627//275981134),
                                                    T(0),
                                                    T(17499453//42458636),
                                                    T(13059537//275981134),
                                                    T(4924918//413971701),
                                                    T(-29088585//1931867938) )),
            # d5
            DerivativeCoefficientRow{T,1,8}(SVector(T(46815789//1188140954),
                                                    T(-28592150//254601633),
                                                    T(29079411//169734422),
                                                    T(-17499453//26112988),
                                                    T(0),
                                                    T(112822635//169734422),
                                                    T(-9119079//84867211),
                                                    T(103152839//7128845724) )),
            # d6
            DerivativeCoefficientRow{T,1,9}(SVector(T(-2047730//52061009),
                                                    T(45982103//371864350),
                                                    T(-3868089//57209900),
                                                    T(-4353179//74372870),
                                                    T(-7521509//14874574),
                                                    T(0),
                                                    T(474569879//743728700),
                                                    T(-138109864//1301525225),
                                                    T(4032//260045) )),
            # d7
            DerivativeCoefficientRow{T,1,10}(SVector(T(-291943739//21305233950),
                                                    T(720387//31216460),
                                                    T(7405209//1014534950),
                                                    T(-4924918//304360485),
                                                    T(9119079//101453495),
                                                    T(-1423709637//2029069900),
                                                    T(0),
                                                    T(5309800707//7101744650),
                                                    T(-108864//709465),
                                                    T(12096//709465) )),
            # d8
            DerivativeCoefficientRow{T,1,11}(SVector(T(10874619//1121684300),
                                                    T(-538846039//21872843850),
                                                    T(1011882//145818959),
                                                    T(5817717//291637918),
                                                    T(-103152839//8749137540),
                                                    T(414329592//3645473975),
                                                    T(-5309800707//7290947950),
                                                    T(0),
                                                    T(762048//1019713),
                                                    T(-762048//5098565),
                                                    T(84672//5098565) )),
        )
        right_boundary = .- left_boundary
        upper_coef = SVector(T(3//4), T(-3//20), T(1//60))
        central_coef = zero(T)
        lower_coef = -upper_coef
        left_weights = SVector( T(318365//1016064),
                                T(145979//103680),
                                T(139177//241920),
                                T(964969//725760),
                                T(593477//725760),
                                T(52009//48384),
                                T(141893//145152),
                                T(1019713//1016064) )
        right_weights = left_weights
        left_boundary_derivatives = Tuple{}()
        right_boundary_derivatives = left_boundary_derivatives

        DerivativeCoefficients(left_boundary, right_boundary,
                                left_boundary_derivatives, right_boundary_derivatives,
                                lower_coef, central_coef, upper_coef,
                                left_weights, right_weights, parallel, 1, order, source)
    else
        throw(ArgumentError("Order $order not implemented/derived."))
    end
end

function second_derivative_coefficients(source::Mattsson2014, order::Int, T=Float64, parallel=Val{:serial}())
    if order == 2
        left_boundary = (
            # d1
            DerivativeCoefficientRow{T,1,3}(SVector(T(1),
                                                    T(-2),
                                                    T(1) )),
        )
        right_boundary = left_boundary
        upper_coef = SVector(T(1))
        central_coef = T(-2)
        lower_coef = upper_coef
        left_weights = SVector(T(1//2))
        right_weights = left_weights
        left_boundary_derivatives = (
            # first derivative
            DerivativeCoefficientRow{T,1,3}(SVector(T(-3//2),
                                                    T(2),
                                                    T(-1//2) )),
        )
        right_boundary_derivatives = (
            -left_boundary_derivatives[1],
        )

        DerivativeCoefficients(left_boundary, right_boundary,
                                left_boundary_derivatives, right_boundary_derivatives,
                                lower_coef, central_coef, upper_coef,
                                left_weights, right_weights, parallel, 2, order, source)
    elseif order == 4
        left_boundary = (
            # d1
            DerivativeCoefficientRow{T,1,6}(SVector(T(8027765//3867372),
                                                    T(-1690702//322281),
                                                    T(8240267//1933686),
                                                    T(-1030334//966843),
                                                    T(-6817//143236),
                                                    T(21380//966843) )),
            # d2
            DerivativeCoefficientRow{T,1,6}(SVector(T(1030898//1077057),
                                                    T(-23780195//12924684),
                                                    T(2540018//3231171),
                                                    T(26257//239346),
                                                    T(-12268//3231171),
                                                    T(-119459//12924684) )),
            # d3
            DerivativeCoefficientRow{T,1,6}(SVector(T(75467//5541372),
                                                    T(1270009//1385343),
                                                    T(-558115//307854),
                                                    T(1111174//1385343),
                                                    T(551339//5541372),
                                                    T(-8789//461781) )),
            # d4
            DerivativeCoefficientRow{T,1,6}(SVector(T(-61567//1217943),
                                                    T(26257//180436),
                                                    T(1111174//1217943),
                                                    T(-5126635//2435886),
                                                    T(464003//405981),
                                                    T(-222133//4871772) )),
            # d5
            DerivativeCoefficientRow{T,1,7}(SVector(T(-6817//422292),
                                                    T(-12268//2850471),
                                                    T(551339//5700942),
                                                    T(928006//950157),
                                                    T(-25370795//11401884),
                                                    T(3568094//2850471),
                                                    T(-2800//35191) )),
            # d6
            DerivativeCoefficientRow{T,1,8}(SVector(T(21380//2713743),
                                                    T(-119459//10854972),
                                                    T(-17578//904581),
                                                    T(-222133//5427486),
                                                    T(3568094//2713743),
                                                    T(-9063745//3618324),
                                                    T(44800//33503),
                                                    T(-2800//33503) )),
        )
        right_boundary = left_boundary
        upper_coef = SVector(T(4//3), T(-1//12))
        central_coef = T(-5//2)
        lower_coef = upper_coef
        left_weights = SVector( T(35809//100800),
                                T(13297//11200),
                                T(5701//5600),
                                T(45109//50400),
                                T(35191//33600),
                                T(33503//33600) )
        right_weights = left_weights
        left_boundary_derivatives = (
            # first derivative
            DerivativeCoefficientRow{T,1,4}(SVector(T(-11/6),
                                                    T(3),
                                                    T(-3//2),
                                                    T(1//3) )),
        )
        right_boundary_derivatives = (
            -left_boundary_derivatives[1],
        )

        DerivativeCoefficients(left_boundary, right_boundary,
                                left_boundary_derivatives, right_boundary_derivatives,
                                lower_coef, central_coef, upper_coef,
                                left_weights, right_weights, parallel, 2, order, source)
    elseif order == 6
        left_boundary = (
            # d1
            DerivativeCoefficientRow{T,1,8}(SVector(T(24055498439//8194715100),
                                                    T(-9925742373//1138154875),
                                                    T(983223468//103468625),
                                                    T(-18528585641//4097357550),
                                                    T(16391451//22207900),
                                                    T(13725804//227630975),
                                                    T(355447739//20486787750),
                                                    T(-24731721//2276309750) )),
            # d2
            DerivativeCoefficientRow{T,1,8}(SVector(T(270821931//300845545),
                                                    T(-42416226217//26302496220),
                                                    T(685962357//1461249790),
                                                    T(2160993//8595587),
                                                    T(59905900//1315124811),
                                                    T(-158509509//2922499580),
                                                    T(-9667431//1461249790),
                                                    T(634102039//92058736770) )),
            # d3
            DerivativeCoefficientRow{T,1,8}(SVector(T(-2479644//63325535),
                                                    T(228654119//199023110),
                                                    T(-16197861//7237204),
                                                    T(24739409//19902311),
                                                    T(-7878737//39804622),
                                                    T(1917829//18093010),
                                                    T(-7508403//398046220),
                                                    T(-41036//10716629) )),
            # d4
            DerivativeCoefficientRow{T,1,8}(SVector(T(-1092927401//17386811442),
                                                    T(36736881//137990567),
                                                    T(74218227//137990567),
                                                    T(-7780367599//4967660412),
                                                    T(210256089//275981134),
                                                    T(1500627//25089194),
                                                    T(1246172//95531931),
                                                    T(-37555785//3863735876) )),
            # d5
            DerivativeCoefficientRow{T,1,8}(SVector(T(-54436269//2376281908),
                                                    T(59905900//763804899),
                                                    T(-23636211//169734422),
                                                    T(210256089//169734422),
                                                    T(-7116321131//3055219596),
                                                    T(8176215//6528247),
                                                    T(-7304679//84867211),
                                                    T(84101639//10693268586) )),
            # d6
            DerivativeCoefficientRow{T,1,9}(SVector(T(4575268//260305045),
                                                    T(-52836503//743728700),
                                                    T(1917829//33805850),
                                                    T(500209//6761170),
                                                    T(545081//572099),
                                                    T(-324760747//148745740),
                                                    T(461969879//371864350),
                                                    T(-11753624//118320475),
                                                    T(2688//260045) )),
            # d7
            DerivativeCoefficientRow{T,1,10}(SVector(T(355447739//63915701850),
                                                    T(-9667431//1014534950),
                                                    T(-22525209//2029069900),
                                                    T(1246172//70237035),
                                                    T(-7304679//101453495),
                                                    T(1385909637//1014534950),
                                                    T(-48284442317//18261629100),
                                                    T(5288632707//3550872325),
                                                    T(-108864//709465),
                                                    T(8064//709465) )),
            # d8
            DerivativeCoefficientRow{T,1,11}(SVector(T(-24731721//7290947950),
                                                    T(634102039//65618531550),
                                                    T(-123108//56084215),
                                                    T(-7511157//583275836),
                                                    T(84101639//13123706310),
                                                    T(-35260872//331406725),
                                                    T(5288632707//3645473975),
                                                    T(-70820489957//26247412620),
                                                    T(1524096//1019713),
                                                    T(-762048//5098565),
                                                    T(56448//5098565) )),
        )
        right_boundary = left_boundary
        upper_coef = SVector(T(3//2), T(-3//20), T(1//90))
        central_coef = T(-49//18)
        lower_coef = upper_coef
        left_weights = SVector( T(318365//1016064),
                                T(145979//103680),
                                T(139177//241920),
                                T(964969//725760),
                                T(593477//725760),
                                T(52009//48384),
                                T(141893//145152),
                                T(1019713//1016064) )
        right_weights = left_weights
        left_boundary_derivatives = (
            # first derivative
            DerivativeCoefficientRow{T,1,5}(SVector(T(-25/12),
                                                    T(4),
                                                    T(-3),
                                                    T(4//3),
                                                    T(-1//4) )),
        )
        right_boundary_derivatives = (
            -left_boundary_derivatives[1],
        )

        DerivativeCoefficients(left_boundary, right_boundary,
                                left_boundary_derivatives, right_boundary_derivatives,
                                lower_coef, central_coef, upper_coef,
                                left_weights, right_weights, parallel, 2, order, source)
    else
        throw(ArgumentError("Order $order not implemented/derived."))
    end
end

function third_derivative_coefficients(source::Mattsson2014, order::Int, T=Float64, parallel=Val{:serial}())
    if order == 2
        left_boundary = (
            # d1
            DerivativeCoefficientRow{T,1,4}(SVector(T(1//4),
                                                    T(-5//8),
                                                    T(1//2),
                                                    T(-1//8) )),
            # d2
            DerivativeCoefficientRow{T,1,4}(SVector(T(-11//16),
                                                    T(2),
                                                    T(-31//16),
                                                    T(5//8) )),
            # d3
            DerivativeCoefficientRow{T,1,5}(SVector(T(-1//2),
                                                    T(15//16),
                                                    T(1//8),
                                                    T(-17//16),
                                                    T(1//2) )),
        )
        right_boundary = .- left_boundary
        upper_coef = SVector(T(-1), T(1//2))
        central_coef = T(0)
        lower_coef = -upper_coef
        left_weights = SVector(T(1//2))
        right_weights = left_weights
        left_boundary_derivatives = (
            # first derivative
            DerivativeCoefficientRow{T,1,3}(SVector(T(-3//2),
                                                    T(2),
                                                    T(-1//2) )),
            # second derivative
            DerivativeCoefficientRow{T,1,3}(SVector(T(1),
                                                    T(-2),
                                                    T(1) )),
        )
        right_boundary_derivatives = (
            -left_boundary_derivatives[1],
            left_boundary_derivatives[2]
        )

        DerivativeCoefficients(left_boundary, right_boundary,
                                left_boundary_derivatives, right_boundary_derivatives,
                                lower_coef, central_coef, upper_coef,
                                left_weights, right_weights, parallel, 3, order, source)
    elseif order == 4
        left_boundary = (
            # d1
            DerivativeCoefficientRow{T,1,6}(SVector(T(-32200//35809),
                                                    T(188187//71618),
                                                    T(-90183//35809),
                                                    T(27988//35809),
                                                    T(-801//35809),
                                                    T(2205//71618) )),
            # d2
            DerivativeCoefficientRow{T,1,6}(SVector(T(-96329//79782),
                                                    T(50400//13297),
                                                    T(-163046//39891),
                                                    T(63583//39891),
                                                    T(-1337//26594),
                                                    T(-1567//39891) )),
            # d3
            DerivativeCoefficientRow{T,1,6}(SVector(T(-11939//34206),
                                                    T(5923//17103),
                                                    T(6300//5701),
                                                    T(-32543//17103),
                                                    T(29083//34206),
                                                    T(-284//5701) )),
            # d4
            DerivativeCoefficientRow{T,1,7}(SVector(T(5606//45109),
                                                    T(-89949//90218),
                                                    T(72429//45109),
                                                    T(2800//45109),
                                                    T(-77319//45109),
                                                    T(95517//90218),
                                                    T(-6300//45109) )),
            # d5
            DerivativeCoefficientRow{T,1,8}(SVector(T(267//35191),
                                                    T(4011//70382),
                                                    T(-29083//35191),
                                                    T(51546//35191),
                                                    T(0),
                                                    T(-108271//70382),
                                                    T(33600//35191),
                                                    T(-4200//35191) )),
            # d6
            DerivativeCoefficientRow{T,1,9}(SVector(T(-735//67006),
                                                    T(1567//33503),
                                                    T(1704//33503),
                                                    T(-31839//33503),
                                                    T(108271//67006),
                                                    T(0),
                                                    T(-54600//33503),
                                                    T(33600//33503),
                                                    T(-4200//33503) )),
        )
        right_boundary = .- left_boundary
        upper_coef = SVector(T(-13//8), T(1), T(-1//8))
        central_coef = T(0)
        lower_coef = -upper_coef
        left_weights = SVector( T(35809//100800),
                                T(13297//11200),
                                T(5701//5600),
                                T(45109//50400),
                                T(35191//33600),
                                T(33503//33600) )
        right_weights = left_weights
        left_boundary_derivatives = (
            # first derivative
            DerivativeCoefficientRow{T,1,4}(SVector(T(-11/6),
                                                    T(3),
                                                    T(-3//2),
                                                    T(1//3) )),
            # second derivative
            DerivativeCoefficientRow{T,1,4}(SVector(T(2),
                                                    T(-5),
                                                    T(4),
                                                    T(-1) )),
        )
        right_boundary_derivatives = (
            -left_boundary_derivatives[1],
            left_boundary_derivatives[2]
        )

        DerivativeCoefficients(left_boundary, right_boundary,
                                left_boundary_derivatives, right_boundary_derivatives,
                                lower_coef, central_coef, upper_coef,
                                left_weights, right_weights, parallel, 3, order, source)
    elseif order == 6
        left_boundary = (
            # d1
            DerivativeCoefficientRow{T,1,8}(SVector(T(-151704//63673),
                                                    T(15270676769//1821047800),
                                                    T(-443349971//41387450),
                                                    T(2063356637//364209560),
                                                    T(-39300617//45526195),
                                                    T(-11473393//364209560),
                                                    T(-38062741//455261950),
                                                    T(40315779//1821047800) )),
            # d2
            DerivativeCoefficientRow{T,1,8}(SVector(T(-13333381409//8182998824),
                                                    T(829440//145979),
                                                    T(-8702160983//1168999832),
                                                    T(1321219979//292249958),
                                                    T(-1463113021//1168999832),
                                                    T(1240729//20874997),
                                                    T(102110955//1168999832),
                                                    T(-50022767//2045749706) )),
            # d3
            DerivativeCoefficientRow{T,1,8}(SVector(T(14062931//75990642),
                                                    T(-1261072297//477655464),
                                                    T(1088640//139177),
                                                    T(-4530616889//477655464),
                                                    T(602572103//119413866),
                                                    T(-116503713//159218488),
                                                    T(-17846623//59706933),
                                                    T(343537955//3343588248) )),
            # d4
            DerivativeCoefficientRow{T,1,8}(SVector(T(661223855//7727471752),
                                                    T(-214194059//275981134),
                                                    T(1209539129//1103924536),
                                                    T(645120//964969),
                                                    T(-2321979501//1103924536),
                                                    T(327603877//275981134),
                                                    T(-175223717//1103924536),
                                                    T(1353613//965933969) )),
            # d5
            DerivativeCoefficientRow{T,1,9}(SVector(T(-91064195//594070477),
                                                    T(632843581//678937688),
                                                    T(-446896583//169734422),
                                                    T(2045223021//678937688),
                                                    T(22680//593477),
                                                    T(-1804641793//678937688),
                                                    T(311038417//169734422),
                                                    T(-1932566239//4752563816),
                                                    T(21168//593477) )),
            # d6
            DerivativeCoefficientRow{T,1,10}(SVector(T(11473393//1249464216),
                                                    T(-8685103//111559305),
                                                    T(116503713//297491480),
                                                    T(-327603877//223118610),
                                                    T(1804641793//892474440),
                                                    T(0),
                                                    T(-1760949511//892474440),
                                                    T(2105883973//1561830270),
                                                    T(-72576//260045),
                                                    T(7056//260045) )),
            # d7
            DerivativeCoefficientRow{T,1,11}(SVector(T(38062741//1420348930),
                                                    T(-20422191//162325592),
                                                    T(17846623//101453495),
                                                    T(175223717//811627960),
                                                    T(-311038417//202906990),
                                                    T(1760949511//811627960),
                                                    T(0),
                                                    T(-1081094773//516490520),
                                                    T(1022112//709465),
                                                    T(-217728//709465),
                                                    T(21168//709465) )),
            # d8
            DerivativeCoefficientRow{T,1,12}(SVector(T(-40315779//5832758360),
                                                    T(50022767//1458189590),
                                                    T(-68707591//1166551672),
                                                    T(-1353613//729094795),
                                                    T(1932566239//5832758360),
                                                    T(-2105883973//1458189590),
                                                    T(1081094773//530250760),
                                                    T(0),
                                                    T(-10329984//5098565),
                                                    T(7154784//5098565),
                                                    T(-1524096//5098565),
                                                    T(148176//5098565) )),
        )
        right_boundary = .- left_boundary
        upper_coef = SVector(T(-61//30), T(169//120), T(-3//10), T(7//240))
        central_coef = T(0)
        lower_coef = -upper_coef
        left_weights = SVector( T(318365//1016064),
                                T(145979//103680),
                                T(139177//241920),
                                T(964969//725760),
                                T(593477//725760),
                                T(52009//48384),
                                T(141893//145152),
                                T(1019713//1016064) )
        right_weights = left_weights
        left_boundary_derivatives = (
            # first derivative
            DerivativeCoefficientRow{T,1,5}(SVector(T(-25//12),
                                                    T(4),
                                                    T(-3),
                                                    T(4//3),
                                                    T(-1//4) )),
            # second derivative
            DerivativeCoefficientRow{T,1,5}(SVector(T(35//12),
                                                    T(-26//3),
                                                    T(19//2),
                                                    T(-14//3),
                                                    T(11//12) )),
        )
        right_boundary_derivatives = (
            -left_boundary_derivatives[1],
            left_boundary_derivatives[2]
        )

        DerivativeCoefficients(left_boundary, right_boundary,
                                left_boundary_derivatives, right_boundary_derivatives,
                                lower_coef, central_coef, upper_coef,
                                left_weights, right_weights, parallel, 3, order, source)
    else
        throw(ArgumentError("Order $order not implemented/derived."))
    end
end

function fourth_derivative_coefficients(source::Mattsson2014, order::Int, T=Float64, parallel=Val{:serial}())
    if order == 2
        left_boundary = (
            # d1
            DerivativeCoefficientRow{T,1,4}(SVector(T(8//5),
                                                    T(-24//5),
                                                    T(24//5),
                                                    T(-8//5) )),
            # d2
            DerivativeCoefficientRow{T,1,4}(SVector(T(-2//5),
                                                    T(6//5),
                                                    T(-6//5),
                                                    T(2//5) )),
            # d3
            DerivativeCoefficientRow{T,1,5}(SVector(T(2//5),
                                                    T(-11//5),
                                                    T(21//5),
                                                    T(-17//5),
                                                    T(1) )),
            # d4
            DerivativeCoefficientRow{T,1,6}(SVector(T(1//5),
                                                    T(2//5),
                                                    T(-17//5),
                                                    T(-29//5),
                                                    T(-4),
                                                    T(1) )),
        )
        right_boundary = left_boundary
        upper_coef = SVector(T(-4), T(1))
        central_coef = T(6)
        lower_coef = upper_coef
        left_weights = SVector(T(1//2))
        right_weights = left_weights
        left_boundary_derivatives = (
            # first derivative
            DerivativeCoefficientRow{T,1,3}(SVector(T(-3//2),
                                                    T(2),
                                                    T(-1//2) )),
            # second derivative
            DerivativeCoefficientRow{T,1,3}(SVector(T(1),
                                                    T(-2),
                                                    T(1) )),
            # third derivative
            DerivativeCoefficientRow{T,1,4}(SVector(T(-1),
                                                    T(3),
                                                    T(-3),
                                                    T(1) )),
        )
        right_boundary_derivatives = (
            -left_boundary_derivatives[1],
            left_boundary_derivatives[2],
            -left_boundary_derivatives[3],
        )

        DerivativeCoefficients(left_boundary, right_boundary,
                                left_boundary_derivatives, right_boundary_derivatives,
                                lower_coef, central_coef, upper_coef,
                                left_weights, right_weights, parallel, 4, order, source)
    elseif order == 4
        left_boundary = (
            # d1
            DerivativeCoefficientRow{T,1,6}(SVector(T(-242219//644562),
                                                    T(881057//644562),
                                                    T(-183673//107427),
                                                    T(220981//322281),
                                                    T(109057//644562),
                                                    T(-29273//214854) )),
            # d2
            DerivativeCoefficientRow{T,1,6}(SVector(T(578657//2154114),
                                                    T(-703457//718038),
                                                    T(1327457//1077057),
                                                    T(-544543//1077057),
                                                    T(-79457//718038),
                                                    T(204257//2154114) )),
            # d3
            DerivativeCoefficientRow{T,1,6}(SVector(T(219527//307854),
                                                    T(-2754943//923562),
                                                    T(2216981//461781),
                                                    T(-559673//153927),
                                                    T(1141057//923562),
                                                    T(-120619//923562) )),
            # d4
            DerivativeCoefficientRow{T,1,7}(SVector(T(69781//811962),
                                                    T(665057//811962),
                                                    T(-584873//135327),
                                                    T(2995381//405981),
                                                    T(-4614143//811962),
                                                    T(172109//90218),
                                                    T(-8400//45109) )),
            # d5
            DerivativeCoefficientRow{T,1,8}(SVector(T(8389//146178),
                                                    T(-79457//633438),
                                                    T(1141057//950157),
                                                    T(-4614143//950157),
                                                    T(557127//70382),
                                                    T(-11293343//1900314),
                                                    T(67200//35191),
                                                    T(-5600//35191) )),
            # d6
            DerivativeCoefficientRow{T,1,9}(SVector(T(-29273//603054),
                                                    T(204257//1809162),
                                                    T(-120619//904581),
                                                    T(172109//100509),
                                                    T(-11293343//1809162),
                                                    T(16787381//1809162),
                                                    T(-218400//33503),
                                                    T(67200//33503),
                                                    T(-5600//33503) )),
        )
        right_boundary = left_boundary
        upper_coef = SVector(T(-13//2), T(2), T(-1//6))
        central_coef = T(28//3)
        lower_coef = upper_coef
        left_weights = SVector( T(35809//100800),
                                T(13297//11200),
                                T(5701//5600),
                                T(45109//50400),
                                T(35191//33600),
                                T(33503//33600) )
        right_weights = left_weights
        left_boundary_derivatives = (
            # first derivative
            DerivativeCoefficientRow{T,1,4}(SVector(T(-11/6),
                                                    T(3),
                                                    T(-3//2),
                                                    T(1//3) )),
            # second derivative
            DerivativeCoefficientRow{T,1,4}(SVector(T(2),
                                                    T(-5),
                                                    T(4),
                                                    T(-1) )),
            # third derivative
            DerivativeCoefficientRow{T,1,4}(SVector(T(-1),
                                                    T(3),
                                                    T(-3),
                                                    T(1) )),
        )
        right_boundary_derivatives = (
            -left_boundary_derivatives[1],
            left_boundary_derivatives[2],
            -left_boundary_derivatives[3],
        )

        DerivativeCoefficients(left_boundary, right_boundary,
                                left_boundary_derivatives, right_boundary_derivatives,
                                lower_coef, central_coef, upper_coef,
                                left_weights, right_weights, parallel, 4, order, source)
    elseif order == 6
        left_boundary = (
            # d1
            DerivativeCoefficientRow{T,1,8}(SVector(T(37567391168//53948541075),
                                                    T(-95834307667//35965694050),
                                                    T(252350074//65392171),
                                                    T(-58232913019//21579416430),
                                                    T(4040770588//3596569405),
                                                    T(-15248255797//35965694050),
                                                    T(4832196698//53948541075),
                                                    T(134156001//7193138810) )),
            # d2
            DerivativeCoefficientRow{T,1,8}(SVector(T(29125918379//23087746682),
                                                    T(-1255810938848//242421340161),
                                                    T(1289206067431//161614226774),
                                                    T(-431078362378//80807113387),
                                                    T(494586219497//484842680322),
                                                    T(31446420748//80807113387),
                                                    T(-21701585799//161614226774),
                                                    T(334788562//242421340161) )),
            # d3
            DerivativeCoefficientRow{T,1,8}(SVector(T(1308658570//3001630359),
                                                    T(-88210933529//66035867898),
                                                    T(13622370452//11005977983),
                                                    T(-27138341627//66035867898),
                                                    T(23881355534//33017933949),
                                                    T(-26412188989//22011955966),
                                                    T(21399717536//33017933949),
                                                    T(-928716467//9433695414) )),
            # d4
            DerivativeCoefficientRow{T,1,8}(SVector(T(110582060185//457852701306),
                                                    T(-22954806538//76308783551),
                                                    T(-180184675067//152617567102),
                                                    T(678091654628//228926350653),
                                                    T(-378329435643//152617567102),
                                                    T(69519106966//76308783551),
                                                    T(-98928859751//457852701306),
                                                    T(4720003312//76308783551) )),
            # d5
            DerivativeCoefficientRow{T,1,9}(SVector(T(1870177580//46931567683),
                                                    T(-21945155863//281589406098),
                                                    T(45403496174//46931567683),
                                                    T(-384706366203//93863135366),
                                                    T(974238057544//140794703049),
                                                    T(-520477408939//93863135366),
                                                    T(99162460006//46931567683),
                                                    T(-99640101991//281589406098),
                                                    T(21168//593477) )),
            # d6
            DerivativeCoefficientRow{T,1,10}(SVector(T(-15248255797//123384591330),
                                                    T(31446420748//61692295665),
                                                    T(-26412188989//41128197110),
                                                    T(69519106966//61692295665),
                                                    T(-520477408939//123384591330),
                                                    T(155376599432//20564098555),
                                                    T(-772894368601//123384591330),
                                                    T(21159425698//8813185095),
                                                    T(-96768//260045),
                                                    T(7056//260045) )),
            # d7
            DerivativeCoefficientRow{T,1,11}(SVector(T(690313814//24044478315),
                                                    T(-21701585799//112207565470),
                                                    T(21399717536//56103782735),
                                                    T(-98928859751//336622696410),
                                                    T(99162460006//56103782735),
                                                    T(-772894368601//112207565470),
                                                    T(1826861184956//168311348205),
                                                    T(-915425403107//112207565470),
                                                    T(2044224//709465),
                                                    T(-290304//709465),
                                                    T(21168//709465) )),
            # d8
            DerivativeCoefficientRow{T,1,12}(SVector(T(134156001//23039395522),
                                                    T(334788562//172795466415),
                                                    T(-6501015269//115196977610),
                                                    T(4720003312//57598488805),
                                                    T(-99640101991//345590932830),
                                                    T(148115979886//57598488805),
                                                    T(-915425403107//115196977610),
                                                    T(1952118169516//172795466415),
                                                    T(-41319936//5098565),
                                                    T(14309568//5098565),
                                                    T(-2032128//5098565),
                                                    T(148176//5098565) )),
        )
        right_boundary = left_boundary
        upper_coef = SVector(T(-122//15), T(169//60), T(-2//5), T(7//240))
        central_coef = T(91//8)
        lower_coef = upper_coef
        left_weights = SVector( T(318365//1016064),
                                T(145979//103680),
                                T(139177//241920),
                                T(964969//725760),
                                T(593477//725760),
                                T(52009//48384),
                                T(141893//145152),
                                T(1019713//1016064) )
        right_weights = left_weights
        left_boundary_derivatives = (
        # first derivative
        DerivativeCoefficientRow{T,1,5}(SVector(T(-25//12),
                                                T(4),
                                                T(-3),
                                                T(4//3),
                                                T(-1//4) )),
        # second derivative
        DerivativeCoefficientRow{T,1,5}(SVector(T(35//12),
                                                T(-26//3),
                                                T(19//2),
                                                T(-14//3),
                                                T(11//12) )),
        # third derivative
        DerivativeCoefficientRow{T,1,5}(SVector(T(-5//2),
                                                T(9),
                                                T(-12),
                                                T(7),
                                                T(-3//2) )),
        )
        right_boundary_derivatives = (
            -left_boundary_derivatives[1],
            left_boundary_derivatives[2],
            -left_boundary_derivatives[3],
        )

        DerivativeCoefficients(left_boundary, right_boundary,
                                left_boundary_derivatives, right_boundary_derivatives,
                                lower_coef, central_coef, upper_coef,
                                left_weights, right_weights, parallel, 4, order, source)
    else
        throw(ArgumentError("Order $order not implemented/derived."))
    end
end