text:
"""
    DMDc(X, U; B, alg)
    DMDc(X, Y, U; B, alg)

Approximates a 'LinearKoopman' with the `AbstractKoopmanAlgorithm` 'alg' from the data matrices `X` and `U` or `X`, `U` and `Y` respectively.
If only `X` is given, the data is split into `X[:, 1:end-1]` and `X[:, 2:end]`. `B` represents the matrix mapping the input onto `Y`, which may be known a priori.

# Example

```julia
alg = DMDPINV()
koopman = DMDc(X, U, alg = alg)

koopman = DMD(X[:, 1:end-1], X[:, 2:end], U, alg = alg)
```
"""
function DMDc(X::AbstractArray, U::AbstractArray; B::AbstractArray = [], alg::AbstractKoopmanAlgorithm = DMDPINV())
    return DMDc(X[:, 1:end-1], X[:, 2:end], U, B = B, alg = alg)
end

function DMDc(X::AbstractArray, Y::AbstractArray, U::AbstractArray; B::AbstractArray = [], alg::AbstractKoopmanAlgorithm = DMDPINV())
    @assert size(X)[2] .== size(Y)[2] "Provide consistent dimensions for data"
    @assert size(Y)[1] .<= size(Y)[2] "Provide consistent dimensions for data"
    @assert size(X)[2] == size(U)[2] "Provide consistent input data."

    nₓ = size(X)[1]
    nᵤ = size(U)[1]

    Ω = vcat(X, U)

    if isempty(B)
        G = alg(Ω, Y)

        A = G[:, 1:nₓ]
        B = G[:, nₓ+1:end]

    else
        A = alg(X, Y-B*U)
    end

    return LinearKoopman(A, B, Y*Ω', Ω*Ω', true)
end

"""
    gDMDc(X, Y, U; B, alg)
    gDMDc(t, X, U; B, dt, alg, fdm, itp)

Approximates a 'LinearKoopman' with the `AbstractKoopmanAlgorithm` 'alg' from the data matrices `X` and `U` or `X`, `U` and `Y` respectively.
`X` should contain the state trajectory and `Y` the differential state trajectory.`B` represents the matrix mapping the input onto `Y`, which may be known a priori.

If no measurements of the differential state is available, `gDMDc` can be called with measurement time points `t` as a first argument.
It will then create an interpolation using the interpolation method from `DataInterpolations.jl` defined in `itp`. The trajectory will then be resample
to equidistant measurements over time corresponding to the mean of `diff(t)` or `dt` if given.
The differential state measurements will be computed via 'FiniteDifferences.jl', given a `FiniteDifferenceMethod` in `fdm`.

# Example

```julia
koopman = gDMDc(X, Y, U)

fdm = backward_fdm(5,1)
itp = CubicSpline
koopman = gDMD(t, X, U, fdm = fdm, itp = itp)
```
"""
function gDMDc(X::AbstractArray, Y::AbstractArray, U::AbstractArray; B::AbstractArray = [], alg::AbstractKoopmanAlgorithm = DMDPINV())
    @assert size(X)[2] .== size(Y)[2] "Provide consistent dimensions for data"
    @assert size(Y)[1] .<= size(Y)[2] "Provide consistent dimensions for data"
    @assert size(X)[2] == size(U)[2] "Provide consistent input data."

    nₓ = size(X)[1]
    nᵤ = size(U)[1]

    Ω = vcat(X, U)

    if isempty(B)
        G = alg(Ω, Y)

        A = G[:, 1:nₓ]
        B = G[:, nₓ+1:end]

    else
        A = alg(X, Y-B*U)
    end

    return LinearKoopman(A, B, Y*Ω', Ω*Ω', false)
end

function gDMDc(t::AbstractVector, X::AbstractArray, U::AbstractArray; dt::Real = 0.0, B::AbstractArray = [], alg::DataDrivenDiffEq.AbstractKoopmanAlgorithm = DMDPINV(), fdm::FiniteDifferences.FiniteDifferenceMethod = backward_fdm(5, 1), itp = CubicSpline, itp_u = LinearInterpolation)
    @assert size(X, 2) == length(t) "Sample size must match."
    @assert test_comp = begin

        if itp ∈ [LinearInterpolation, QuadraticInterpolation] && !isa(fdm, FiniteDifferences.Backward{typeof(fdm.grid), typeof(fdm.coefs)})
            false
        else
            true
        end
    end "LinearInterpolation and QuadraticInterpolation need to a backward finite difference method."

    Δt = iszero(dt) ? mean(diff(t)) : dt
    t̂ = t[1]:Δt:t[end]

    X̂ = zeros(eltype(X), size(X, 1), length(t̂))
    Y = similar(X̂)
    for (i, xi) in enumerate(eachrow(X))
        itp_ = itp(xi, t)
        dx(t) = FiniteDifferences.fdm(fdm, itp_, t)
        X̂[i, :] .= itp_.(t̂)
        Y[i, :] .= dx.(t̂)
    end

    Û = zeros(eltype(U), size(U, 1), length(t̂))

    for (i, ui) in enumerate(eachrow(U))
        uitp_ = itp_u(ui, t)
        Û[i, :] .= uitp_.(t̂)
    end

    return gDMDc(X̂, Y, Û; B = B, alg = alg)
end


text:
using PyPlot
#X_t+1 = AX_t + b + ∑W_t

matrix_size = 2

A = [0.8 -0.2; -0.1 0.7]
Sigma = [0.5 0.4; 0.4 0.6]
S = [1 2; 3 4]

function get_next_S(S, A, Sigma)
  return A * S * A' + Sigma * Sigma'
end

function tkm(S, A, Sigma)
  Delta = S
  Deltas = []
  i = 1
  while sum(abs(Delta) .< 1e-12) != length(S)
    S2 = get_next_S(S, A, Sigma)
    Delta = S2 .- S
    push!(Deltas, sum(Delta))
    S = S2
    i += 1
  end
  println("finished by step $i")
  return S, Deltas
end

S_lim, Deltas = tkm(S, A, Sigma)
println(S_lim)
plot(Deltas)

ext: jl

max_stars_repo_path: Julia/Untitled Folder/1-sde.jl

max_issues_repo_path: Julia/Untitled Folder/1-sde.jl

max_forks_repo_path: Julia/Untitled Folder/1-sde.jl


text:
# This script can be used to simulate the onboard-Kalman filter using real recorded sensor data.
# This can be useful to find the optimal Kalman parameters (Q,R) or to just do experiments and find an optimal sensor fusion process.

using PyPlot
using JLD

function main(code::AbstractString)
    #log_path_record = "$(homedir())/open_loop/output-record-$(code).jld"
    log_path_record = "$(homedir())/simulations/output-record-$(code).jld"
    d_rec = load(log_path_record)

    imu_meas    = d_rec["imu_meas"]
    gps_meas    = d_rec["gps_meas"]
    cmd_log     = d_rec["cmd_log"]
    #cmd_pwm_log = d_rec["cmd_pwm_log"]
    vel_est     = d_rec["vel_est"]
    pos_info    = d_rec["pos_info"]

    t0      = max(imu_meas.t[1],vel_est.t[1],gps_meas.t[1],cmd_log.t[1])+0.3
    t_end   = min(imu_meas.t[end],vel_est.t[end],gps_meas.t[end],cmd_log.t[end])

    l_A = 0.125
    l_B = 0.125

    dt = 1/50
    t = t0:dt:t_end
    sz = length(t)
    y = zeros(sz,6)
    u = zeros(sz,2)

    y_gps_imu = zeros(sz,13)
    q = zeros(sz,2)

    P = zeros(7,7)
    x_est = zeros(length(t),7)
    P_gps_imu = zeros(14,14)
    x_est_gps_imu = zeros(length(t),14)

    yaw0 = imu_meas.z[t0.>imu_meas.t,6][end]
    gps_dist = zeros(length(t))

    yaw_prev = yaw0
    y_gps_imu[1,4] = 0

    qa = 1000
    qp = 1000

    #Q_gps_imu = diagm([1/6*dt^3,1/6*dt^3,1/2*dt^2,1/2*dt^2,dt,dt,dt,dt,0.001,0.001,0.001])
    #Q_gps_imu = diagm([0.01,0.01,0.1,0.1,1.0,1.0,0.1,1.0,0.01])
    #R_gps_imu = diagm([0.1,0.1,1.0,0.1,1.0,100.0,100.0])
    #Q_gps_imu = diagm([0.1,0.1,0.1,0.1,1.0,1.0,0.1,1.0,0.01,   0.01,0.01,1.0,1.0,0.1])
    #                   x, y, vx, vy, ax, ay, psi, psidot, psidrift
    Q_gps_imu = diagm([1/20*dt^5*qa,1/20*dt^5*qa,1/3*dt^3*qa,1/3*dt^3*qa,dt*qa,dt*qa,1/3*dt^3*qp,dt*qp,0.1, 0.01,0.01,0.1,0.1,0.1])
    #R_gps_imu = diagm([1.0,1.0,1.0,0.1,10.0,100.0,100.0,       1.0,1.0,0.1,0.5])
    R_gps_imu = diagm([5.0,5.0,1.0,10.0,100.0,1000.0,1000.0,     5.0,5.0,10.0,10.0, 10.0,10.0])
    #                   x, y, v, psi, psidot, ax, a_y       x, y, psi, v, v_x, v_y

    y_vel_prev = 0.0
    y_vel_est = 0.0

    for i=2:length(t)
        # Collect measurements and inputs for this iteration
        # Interpolate GPS-measurements:
        x_p = gps_meas.z[(t[i].>gps_meas.t_msg) & (t[i]-1.<gps_meas.t_msg),1][:]
        y_p = gps_meas.z[(t[i].>gps_meas.t_msg) & (t[i]-1.<gps_meas.t_msg),2][:]
        t_p = gps_meas.t[(t[i].>gps_meas.t_msg) & (t[i]-1.<gps_meas.t_msg)][:]-t0
        sl = size(x_p,1)

        # Create polynom for gps data
        if sl > 1
            c_x = [t_p.^2 t_p ones(sl,1)]\x_p
            c_y = [t_p.^2 t_p ones(sl,1)]\y_p
            x_int = [(t[i]-t0)^2 (t[i]-t0) 1] * c_x
            y_int = [(t[i]-t0)^2 (t[i]-t0) 1] * c_y
        else
            x_int = 0
            y_int = 0
        end

        y_gps = gps_meas.z[t[i].>gps_meas.t,:][end,:]
        y_gps = [x_int y_int]
        y_gps = pos_info.z[t[i].>pos_info.t,12:13][end,:]

        y_yaw = pos_info.z[t[i].>pos_info.t,14][end]
        y_yawdot = imu_meas.z[t[i].>imu_meas.t,3][end]

        att = imu_meas.z[t[i].>imu_meas.t,4:6][end,:]
        acc = imu_meas.z[t[i].>imu_meas.t,7:9][end,:]
        rot = imu_meas.z[t[i].>imu_meas.t,1:3][end,:]
        acc_f = rotMatrix('y',-att[2])*rotMatrix('x',-att[1])*acc'
        #rot_f = rotMatrix('y',-att[2])*rotMatrix('x',-att[1])*rot'
        a_x = imu_meas.z[t[i].>imu_meas.t,7][end]
        a_y = imu_meas.z[t[i].>imu_meas.t,8][end]
        a_x = acc_f[1]
        a_y = acc_f[2]
        #y_yawdot = rot_f[3]

        u[i,1] = cmd_log.z[t[i].>cmd_log.t,1][end]
        u[i,2] = cmd_log.z[t[i]-0.2.>cmd_log.t,2][end]

        #y_vel_est = vel_est.z[t[i].>vel_est.t,1][end]
        if pos_info.z[t[i].>pos_info.t,15][end] != y_vel_prev
            y_vel_est = pos_info.z[t[i].>pos_info.t,15][end]
            y_vel_prev = y_vel_est
        else
            y_vel_est = y_vel_est + dt*(u[i,1] - 0.5*x_est_gps_imu[i-1,13])
        end

        bta = atan(0.5*tan(u[i,2]))
        y_gps_imu[i,:]  = [y_gps y_vel_est y_yaw y_yawdot a_x a_y y_gps y_yaw y_vel_est y_vel_est*cos(bta) y_vel_est*sin(bta)]
        y_gps_imu[:,4]  = unwrap!(y_gps_imu[:,4])
        y_gps_imu[:,10] = unwrap!(y_gps_imu[:,10])

        args = (u[i,:],dt,l_A,l_B)

        # Calculate new estimate
        (x_est_gps_imu[i,:], P_gps_imu,q[i,1],q[i,2]) = ekf(simModel_gps_imu,x_est_gps_imu[i-1,:]',P_gps_imu,h_gps_imu,y_gps_imu[i,:]',Q_gps_imu,R_gps_imu,args)
    end

    println("Yaw0 = $yaw0")
    figure(1)
    plot(t-t0,x_est_gps_imu[:,1:2],"-*",gps_meas.t-t0,gps_meas.z,"-+",t-t0,y_gps_imu[:,1:2],"--x")
    plot(t-t0,x_est_gps_imu[:,10:11],"-x")
    #plot(pos_info.t-t0,pos_info.z[:,12:13],"-+")
    #plot(pos_info.t-t0,pos_info.z[:,6:7],"-x")
    grid("on")
    legend(["x_est","y_est","x_meas","y_meas","x_int","y_int","x_est2","y_est2"])
    title("Comparison x,y estimate and measurement")

    figure(2)
    plot(t-t0,x_est_gps_imu[:,[7,9,12,14]],imu_meas.t-t0,imu_meas.z[:,6]-yaw0)
    plot(pos_info.t-t0,pos_info.z[:,[10,16]],"-x")
    grid("on")
    legend(["psi_est","psi_drift_est","psi_est_2","psi_drift_est_2","psi_meas","psi_onboard","psi_drift_onboard"])

    figure(3)
    v = sqrt(x_est_gps_imu[:,3].^2+x_est_gps_imu[:,4].^2)
    plot(t-t0,v,"-*",t-t0,x_est_gps_imu[:,[3:4,13]],"--",vel_est.t-t0,vel_est.z[:,1])
    plot(pos_info.t-t0,pos_info.z[:,8:9],"-x")
    legend(["v_est","v_x_est","v_y_est","v_kin_est","v_meas","v_x_onboard","v_y_onboard"])
    grid("on")
    title("Velocity estimate and measurement")

    figure(4)
    plot(imu_meas.t-t0,imu_meas.z[:,7:8],"x",pos_info.t-t0,pos_info.z[:,17:18],t-t0,x_est_gps_imu[:,5:6])
    grid("on")
    legend(["a_x_meas","a_y_meas","a_x_onboard","a_y_onboard","a_x_est","a_y_est",])

    # PLOT DIRECTIONS
    figure(5)
    plot(x_est_gps_imu[:,10],x_est_gps_imu[:,11])
    grid("on")
    title("x-y-view")
    for i=1:10:size(pos_info.t,1)
        dir = [cos(pos_info.z[i,10]) sin(pos_info.z[i,10])]
        lin = [pos_info.z[i,6:7]; pos_info.z[i,6:7] + 0.1*dir]
        plot(lin[:,1],lin[:,2],"-+")
    end
    for i=1:10:size(t,1)
        bta = atan(l_A/(l_A+l_B)*tan(u[i,2]))
        dir = [cos(x_est_gps_imu[i,12]+bta) sin(x_est_gps_imu[i,12]+bta)]           # 7 = imu-yaw, 12 = model-yaw
        lin = [x_est_gps_imu[i,10:11]; x_est_gps_imu[i,10:11] + 0.1*dir]
        plot(lin[:,1],lin[:,2],"-*")
    end
    axis("equal")

    # figure(6)
    # plot(t-t0,q)
    # grid("on")
    # title("Errors")
    figure(7)
    plot(imu_meas.t-t0,imu_meas.z[:,3],pos_info.t-t0,pos_info.z[:,11],t-t0,x_est_gps_imu[:,8])
    legend(["psiDot_meas","psiDot_onboard","psiDot_est"])
    grid("on")
    nothing
end

function h_gps_imu(x,args)
    y = zeros(13)
    y[1] = x[1]                     # x
    y[2] = x[2]                     # y
    y[3] = sqrt(x[3]^2+x[4]^2)      # v
    y[4] = x[7]+x[9]                # psi
    y[5] = x[8]                     # psiDot
    y[6] = x[5]                     # a_x
    y[7] = x[6]                     # a_y
    y[8] = x[10]                    # x
    y[9] = x[11]                    # y
    y[10] = x[12]+x[14]             # psi
    y[11] = x[13]                   # v
    y[12] = x[3]                    # v_x
    y[13] = x[4]                    # v_y
    return y
end
function ekf(f, mx_k, P_k, h, y_kp1, Q, R, args)
    xDim    = size(mx_k,1)
    mx_kp1  = f(mx_k,args)
    A       = numerical_jac(f,mx_k,args)
    P_kp1   = A*P_k*A' + Q
    my_kp1  = h(mx_kp1,args)
    H       = numerical_jac(h,mx_kp1,args)
    P12     = P_kp1*H'
    q_GPS   = (y_kp1-my_kp1)[1:2]'*inv(H*P12+R)[1:2,1:2]*(y_kp1-my_kp1)[1:2]
    q_GPS2   = (y_kp1-my_kp1)[10:11]'*inv(H*P12+R)[10:11,10:11]*(y_kp1-my_kp1)[10:11]
    # if q_GPS[1] > 0.005
    #     R[1,1] = 100
    #     R[2,2] = 100
    # else
    #     R[1,1] = 1
    #     R[2,2] = 1
    # end
    # if q_GPS2[1] > 0.005
    #     R[8,8] = 100
    #     R[9,9] = 100
    # else
    #     R[8,8] = 1
    #     R[9,9] = 1
    # end
    K       = P12*inv(H*P12+R)
    mx_kp1  = mx_kp1 + K*(y_kp1-my_kp1)
    P_kp1   = (K*R*K' + (eye(xDim)-K*H)*P_kp1)*(eye(xDim)-K*H)'
    return mx_kp1, P_kp1, q_GPS[1], q_GPS2[1]
end

function simModel_gps_imu(z,args)
    zNext = copy(z)
    (u,dt,l_A,l_B) = args
    bta = atan(l_A/(l_A+l_B)*tan(u[2]))
    #zNext[1] = z[1] + dt*(cos(z[7])*z[3] - sin(z[7])*z[4]) + 1/2*dt^2*(cos(z[7])*z[5]-sin(z[7])*z[6])       # x
    #zNext[2] = z[2] + dt*(sin(z[7])*z[3] + cos(z[7])*z[4]) + 1/2*dt^2*(sin(z[7])*z[5]+cos(z[7])*z[6])       # y
    zNext[1] = z[1] + dt*(cos(z[7])*z[3] - sin(z[7])*z[4])       # x
    zNext[2] = z[2] + dt*(sin(z[7])*z[3] + cos(z[7])*z[4])       # y
    zNext[3] = z[3] + dt*(z[5]+z[8]*z[4])   # v_x
    zNext[4] = z[4] + dt*(z[6]-z[8]*z[3])   # v_y
    zNext[5] = z[5]                         # a_x
    zNext[6] = z[6]                         # a_y
    zNext[7] = z[7] + dt*z[8]               # psi
    zNext[8] = z[8]                         # psidot
    zNext[9] = z[9]                         # drift_psi
    zNext[10] = z[10] + dt*(z[13]*cos(z[12] + bta))                       # x
    zNext[11] = z[11] + dt*(z[13]*sin(z[12] + bta))                       # y
    zNext[12] = z[12] + dt*(z[13]/l_B*sin(bta))                           # psi
    zNext[13] = z[13] + dt*(u[1] - 0.5*z[13])                             # v
    zNext[14] = z[14]                                                     # psi_drift_2
    return zNext
end

function numerical_jac(f,x,args)
    y = f(x,args)
    jac = zeros(size(y,1),size(x,1))
    eps = 1e-5
    xp = copy(x)
    for i = 1:size(x,1)
        xp[i] = x[i]+eps/2.0
        yhi = f(xp,args)
        xp[i] = x[i]-eps/2.0
        ylo = f(xp,args)
        xp[i] = x[i]
        jac[:,i] = (yhi-ylo)/eps
    end
    return jac
end

function initPlot()             # Initialize Plots for export
    linewidth = 0.4
    rc("axes", linewidth=linewidth)
    rc("lines", linewidth=linewidth, markersize=2)
    #rc("font", family="")
    rc("axes", titlesize="small", labelsize="small")        # can be set in Latex
    rc("xtick", labelsize="x-small")
    rc("xtick.major", width=linewidth/2)
    rc("ytick", labelsize="x-small")
    rc("ytick.major", width=linewidth/2)
    rc("legend", fontsize="small")
    rc("font",family="serif")
    rc("font",size=8)
    rc("figure",figsize=[4.5,3])
    #rc("pgf", texsystem="pdflatex",preamble=L"""\usepackage[utf8x]{inputenc}\usepackage[T1]{fontenc}\usepackage{lmodern}""")
end

function unwrap!(p)
    length(p) < 2 && return p
    for i = 2:length(p)
        d = p[i] - p[i-1]
        if abs(d) > pi
            p[i] -= floor((d+pi) / (2*pi)) * 2pi
        end
    end
    return p
end

function rotMatrix(s::Char,deg::Float64)
    A = zeros(3,3)
    if s=='x'
        A = [1 0 0;
             0 cos(deg) sin(deg);
             0 -sin(deg) cos(deg)]
    elseif s=='y'
        A = [cos(deg) 0 -sin(deg);
             0 1 0;
             sin(deg) 0 cos(deg)]
    elseif s=='z'
        A = [cos(deg) sin(deg) 0
             -sin(deg) cos(deg) 0
             0 0 1]
    else
        warn("Wrong angle for rotation matrix")
    end
    return A
end

function plot_velocities(code::AbstractString)
    #log_path_record = "$(homedir())/open_loop/output-record-$(code).jld"
    log_path_record = "$(homedir())/simulations/output-record-$(code).jld"
    d_rec = load(log_path_record)

    vel_est     = d_rec["vel_est"]
    pos_info    = d_rec["pos_info"]
    cmd_log     = d_rec["cmd_log"]

    acc = diff(vel_est.z[:,1])./diff(vel_est.t_msg)
    acc = smooth(acc,5)
    t0 = vel_est.t[1]
    ax1=subplot(211)
    plot(vel_est.t-t0,vel_est.z)
    plot(vel_est.t_msg-t0,vel_est.z,"--")
    legend(["vel","fl","fr","bl","br"])
    grid("on")
    xlabel("t [s]")
    title("Velocity")
    subplot(212,sharex=ax1)
    plot(cmd_log.t-t0,cmd_log.z[:,1])
    plot(cmd_log.t_msg-t0,cmd_log.z[:,1],"--")
    plot(vel_est.t_msg[1:end-1]-t0,acc)
    grid("on")
    legend(["u_a","u_a_sent","acceleration"])
    title("Commands")
    xlabel("t [s]")
    ylabel("")
end

function smooth(x,n)
    y = zeros(size(x))
    for i=1:size(x,1)
        start = max(1,i-n)
        fin = min(size(x,1),start + 2*n)
        y[i,:] = mean(x[start:fin,:],1)
    end
    return y
end

max_stars_repo_name: RoyaFiroozi/barc

max_issues_repo_name: RoyaFiroozi/barc

max_forks_repo_name: RoyaFiroozi/barc


text:
using DataStructures: SortedDict

function last_element(iterable)
    # the built-in `last()` is limited to O(1) indexed access collections
    try
        return last(iterable)
    catch e
        if isa(e, MethodError)
            for elem in iterable
            end
            return elem
        end
    end
end

immutable Lookahead
    iterable
    state
    peek :: Nullable{Any}
end

function lookahead(iterable)
    state = start(iterable)
    if done(iterable, state)
        peek = nothing
    else
        peek, state = next(iterable, state)
        peek = Nullable{Any}(peek)
    end
    Lookahead(iterable, state, peek)
end

function lookahead_peek(la::Lookahead)
    get(la.peek)
end

function lookahead_done(la::Lookahead)
    isnull(la.peek)
end

function lookahead_next(la::Lookahead)
    if done(la.iterable, la.state)
        Lookahead(la.iterable, la.state, nothing)
    else
        item, state = next(la.iterable, la.state)
        Lookahead(la.iterable, state, Nullable{Any}(item))
    end
end

immutable EventSequence
    iters :: SortedDict{Symbol, Lookahead}
    last_event
end

function event_sequence_done(eseq::EventSequence)
    length(eseq.iters) == 0
end

function event_sequence(seqs)
    # Assuming:
    # - all iterables produce monotonously increasing values
    iters = SortedDict(((key => lookahead(seq)) for (key, seq) in seqs)...)
    last_event = max((last_element(seq) for (key, seq) in seqs)...)
    EventSequence(iters, last_event)
end

function event_sequence_next_until(max_val, eseq::EventSequence)
    events = [] # Array{Pair{Any, Symbol}, 1}()
    new_lookaheads = Array{Pair{Symbol, Lookahead}, 1}()

    for (key, lookahead) in eseq.iters
        while !lookahead_done(lookahead)

            val = lookahead_peek(lookahead)

            # handling this case separately to ensure values are snapped to max_val
            # when possible (this will help steppers to determine whether or not
            # to interpolate)
            if isapprox(val, max_val)
                val = max_val
            elseif val > max_val
                # leave it for the next iteration
                break
            end

            push!(events, val => key)

            lookahead = lookahead_next(lookahead)
        end

        if !lookahead_done(lookahead)
            push!(new_lookaheads, key => lookahead)
        end
    end

    grouped_events = [] # Array{Pair{Any, Array{Symbol, 1}}, 1}()

    # Group events with close enough times
    for (val, key) in sort(events)
        if length(grouped_events) == 0
            push!(grouped_events, (val, [key]))
        else
            last_val = grouped_events[end][1]
            if isapprox(val, last_val)
                push!(grouped_events[end][2], key)
            else
                push!(grouped_events, (val, [key]))
            end
        end
    end

    grouped_events, EventSequence(SortedDict(new_lookaheads), eseq.last_event)
end


text:
using Documenter, DocumenterMarkdown, DynamicHMC, Turing
using LibGit2: clone

# Include the update_homepage function.
include("make-utils.jl")

# Make paths.
examples_path = joinpath(@__DIR__, joinpath("site", "_tutorials"))
source_path = joinpath(@__DIR__, "src")
build_relative = "build"
build_path = joinpath(@__DIR__, build_relative)
site_path = joinpath(@__DIR__, "site")

# You can skip this part if you are on a metered
# connection by calling `julia make.jl no-tutorials`
if !in("no-tutorials", ARGS)
    isdir(examples_path) || mkpath(examples_path)

    # Clone TuringTurorials
    tmp_path = tempname()
    mkdir(tmp_path)
    clone("https://github.com/TuringLang/TuringTutorials", tmp_path)

    # Move to markdown folder.
    md_path = joinpath(tmp_path, "markdown")

    # Copy the .md versions of all examples.
    try
        println(md_path)
        for file in readdir(md_path)
            full_path = joinpath(md_path, file)
            target_path = joinpath(examples_path, file)
            println("Copying $full_path to $target_path")
            cp(full_path, target_path, force = true)
        end
    catch e
        # println("Markdown copy error: $e")
        rethrow(e)
    finally
        rm(tmp_path, recursive = true)
    end
else
    @info "Skipping tutorial copying."
end

# Preprocess markdown files.
src_temp = mktempdir()
cp(source_path, src_temp, force = true)
yaml_dict = preprocess_markdown(source_path)

# Build documentation
try
    makedocs(
        sitename = "Turing.jl",
        build = build_relative,
        format = Markdown(),
        checkdocs = :all
    )
catch e
    # Put back the original files in the event of an error.
    cp(src_temp, source_path, force = true)
    rm(src_temp, recursive = true)
    rethrow(e)
end

# Postprocess markdown files (put the YAML headers back in)
cp(src_temp, source_path, force = true)
rm(src_temp, recursive = true)
postprocess_markdown(build_path, yaml_dict, original = source_path)

@info "YAML DICT"
for i in keys(yaml_dict)
    @info i
end

# Copy the built files to the site directory.
filecopy_deep(build_path, site_path)

if !in(ARGS, "no-publish")
    # Define homepage update function.
    page_update = update_homepage(
        "github.com/TuringLang/Turing.jl.git",
        "gh-pages",
        "site"
    )
else
    @info "Skipping publishing."
end


text:
function check_case_sensitivity(prefix::Prefix)
    all_ok = true

    function check_set(root, list)
        lowered = Set()
        for f in list
            lf = lowercase(f)
            if lf in lowered
                @warn("$(relpath(joinpath(root, f), prefix.path)) causes a case-sensitivity ambiguity!")
                all_ok = false
            end
            push!(lowered, lf)
        end
    end

    for (root, dirs, files) in walkdir(prefix.path)
        check_set(root, dirs)
        check_set(root, files)
    end

    return all_ok
end

function check_absolute_paths(prefix::Prefix, all_files::Vector; silent::Bool = false)
    # Finally, check for absolute paths in any files.  This is not a "fatal"
    # offense, as many files have absolute paths.  We want to know about it
    # though, so we'll still warn the user.
    for f in all_files
        try
            file_contents = String(read(f))
            if occursin(prefix.path, file_contents)
                if !silent
                    @warn("$(relpath(f, prefix.path)) contains an absolute path")
                end
            end
        catch
            if !silent
                @warn("Skipping abspath scanning of $(f), as we can't open it")
            end
        end
    end

    return true
end

function ensure_executability(oh::ObjectHandle; verbose::Bool=false, silent::Bool=false)
    old_mode = filemode(path(oh))
    # Execution permissions only for users who can read the file
    read_mask = (old_mode & 0o444) >> 2
    # Check whether the file has executable permission for all
    if old_mode & read_mask != read_mask
        if verbose
            @info "Making $(path(oh)) executable"
        end
        try
            # Add executable permission for all users that can read the file
            chmod(path(oh), old_mode | read_mask)
        catch e
            if isa(e, InterruptException)
                rethrow(e)
            end
            if !silent
                @warn "$(path(oh)) could not be made executable!" exception=(e, catch_backtrace())
            end
        end
    end
    return true
end


text:
@testset "ProgressThreads tests" begin
    threads = Threads.nthreads()
    println("Testing Progress() with Threads.@threads across $threads threads")
    (Threads.nthreads() == 1) && @info "Threads.nthreads() == 1, so Threads.@threads test is suboptimal"
    n = 20 #per thread
    threadsUsed = Int[]
    vals = ones(n*threads)
    p = ProgressMeter.Progress(n*threads)
    Threads.@threads for i = 1:(n*threads)
        !in(Threads.threadid(), threadsUsed) && push!(threadsUsed, Threads.threadid())
        vals[i] = 0
        sleep(0.1)
        ProgressMeter.next!(p)
    end
    @test !any(vals .== 1) #Check that all elements have been iterated
    @test length(threadsUsed) == threads #Ensure that all threads are used

    println("Testing ProgressUnknown() with Threads.@threads across $threads threads")
    trigger = 100.0
    prog = ProgressMeter.ProgressUnknown("Attepts at exceeding trigger:")
    vals = Float64[]
    threadsUsed = Int[]
    Threads.@threads for _ in 1:1000
        !in(Threads.threadid(), threadsUsed) && push!(threadsUsed, Threads.threadid())
        push!(vals, rand())
        valssum = sum(vals)
        if sum(vals) <= trigger
            ProgressMeter.next!(prog)
        elseif !prog.done
            ProgressMeter.finish!(prog)
            break
        else
            break
        end
        sleep(0.1*rand())
    end
    @test sum(vals) > trigger
    @test length(threadsUsed) == threads #Ensure that all threads are used

    println("Testing ProgressThresh() with Threads.@threads across $threads threads")
    thresh = 1.0
    prog = ProgressMeter.ProgressThresh(thresh, "Minimizing:")
    vals = fill(300.0, 1)
    threadsUsed = Int[]
    Threads.@threads for _ in 1:100000
        !in(Threads.threadid(), threadsUsed) && push!(threadsUsed, Threads.threadid())
        push!(vals, -rand())
        valssum = sum(vals)
        if valssum > thresh
            ProgressMeter.update!(prog, valssum)
        else
            ProgressMeter.finish!(prog)
            break
        end
        sleep(0.1*rand())
    end
    @test sum(vals) <= thresh
    @test length(threadsUsed) == threads #Ensure that all threads are used

    @static if VERSION >= v"1.3.0-rc1" #Threads.@spawn not available before 1.3
        if (Threads.nthreads() > 1)
            threads = Threads.nthreads() - 1
            println("Testing Progress() with Threads.@spawn across $threads threads")
            n = 20 #per thread
            tasks = Vector{Task}(undef, threads)
            threadsUsed = Int[]
            vals = ones(n*threads)
            p = ProgressMeter.Progress(n*threads)
            for t in 1:threads
                tasks[t] = Threads.@spawn for i in 1:n
                    !in(Threads.threadid(), threadsUsed) && push!(threadsUsed, Threads.threadid())
                    vals[(n*(t-1)) + i] = 0
                    sleep(0.05 + (rand()*0.1))
                    ProgressMeter.next!(p)
                end
            end
            wait.(tasks)
            @test !any(vals .== 1) #Check that all elements have been iterated
            #@test length(threadsUsed) == threads #Ensure that all threads are used (unreliable for @spawn)
        else
            @info "Threads.nthreads() == 1, so Threads.@spawn tests cannot be meaningfully tested"
        end
    end
end


text:
module StateSpaceSimulation

using Modia
@usingModiaPlot

# Second order system:
#     w = 1.0
#     D = 0.1
#     k = 2.0
#     der(x1) = x2
#     der(x2) = -w^2*x1 - 2*D*w*x2 + w^2*u
#          y  = k*x1
SecondOrder1 = Model(
    w = 20.0,
    D =  0.1,
    k =  2.0,
    sys = Modia.StateSpace | Map(A = :([  0        1;
                                       -w^2  -2*D*w]),
                                 B = :([0; w^2]),
                                 C = :([k 0]),
                                 D = :(zeros(1)),
                                 x = Var(init = zeros(2)) ),
    equations = :[sys.u = 1.0]
)

secondOrder1 = @instantiateModel(SecondOrder1, unitless=true)
simulate!(secondOrder1, merge=Map(D=0.3),
          requiredFinalStates = [0.9974089572681231, 0.011808652820321723])
plot(secondOrder1, [("sys.u", "sys.y"); "sys.x"], figure = 1)

SecondOrder2 = Model(
    sys = Modia.StateSpace | Map(   A = [   0.0    1.0;
                                         -100.0  -12.0],
                                    B = [0; 400.0],
                                    C = [2.0 0],
                                    D = zeros(1,1),
                                    x = Var(init = zeros(2))),
    equations = :[sys.u = 1.0]
)

secondOrder2 = @instantiateModel(SecondOrder2, unitless=true)
simulate!(secondOrder2, Tsit5(), merge=Map(sys=Map(A = [0.0 1.0; -400.0  -12.0])),
          requiredFinalStates = [0.9974089572681231, 0.011808652820321723])
plot(secondOrder2, [("sys.u", "sys.y"); "sys.x"], figure = 2)

simulate!(secondOrder2, Tsit5(), merge=Map(sys=Map(x=[1.0,1.0])),
          logParameters=true, logStates=true,
          requiredFinalStates = [1.0000295203337868, 0.0022367686372974493])
plot(secondOrder2, [("sys.u", "sys.y"); "sys.x"], figure = 3)
end


text:
using Clp: ClpSolver
using SparseArrays: spzeros

@testset "Minimum-cost flow" begin

    # bipartite oriented + source & sink
    # source 5, sink 6, v1 {1, 2} v2 {3, 4}
    g = lg.DiGraph(6)
    lg.add_edge!(g, 5, 1)
    lg.add_edge!(g, 5, 2)
    lg.add_edge!(g, 3, 6)
    lg.add_edge!(g, 4, 6)
    lg.add_edge!(g, 1, 3)
    lg.add_edge!(g, 1, 4)
    lg.add_edge!(g, 2, 3)
    lg.add_edge!(g, 2, 4)
    w = zeros(6,6)
    w[1,3] = 10.
    w[1,4] = 5.
    w[2,3] = 2.
    w[2,4] = 2.
    # v2 -> sink have demand of one
    demand = spzeros(6,6)
    demand[3,6] = 1
    demand[4,6] = 1
    capacity = ones(6,6)

    flow = mincost_flow(g, capacity, demand, w, ClpSolver(), 5, 6)
    @test flow[5,1] == 1
    @test flow[5,2] == 1
    @test flow[3,6] == 1
    @test flow[4,6] == 1
    @test flow[1,4] == 1
    @test flow[2,4] == 0
    @test flow[2,3] == 1
    @test flow[1,3] == 0
    @test sum(diag(flow)) == 0

    # flow conservation property
    for n1 = 1:4
        @test sum(flow[n1,:]) ≈ sum(flow[:,n1])
    end

    # no demand => null flow
    d2 = spzeros(6,6)
    flow = mincost_flow(g, capacity, d2, w, ClpSolver(), 5, 6)
    for idx in 1:6
        for jdx in 1:6
            @test flow[idx,jdx] ≈ 0.0
        end
    end

    # graph without sink or source
    g = lg.DiGraph(6)
    # create circuit
    for s in 1:5
        lg.add_edge!(g, s, s+1)
    end
    lg.add_edge!(g, 6, 1)
    lg.add_edge!(g, 2, 5) # shortcut

    capacity = ones(6,6)
    demand = spzeros(6,6)
    demand[1,2] = 1
    costs = ones(6,6)
    flow = mincost_flow(g, capacity, demand, costs, ClpSolver())
    active_flows = [(1,2), (2,5), (5,6),(6,1)]
    for s in 1:6
        for t in 1:6
            if (s,t) in active_flows
                @test flow[s,t] ≈ 1.
            else
                @test flow[s,t] ≈ 0.
            end
        end
    end
    # flow conservation property
    for n1 = 1:6
        @test sum(flow[n1,:]) ≈ sum(flow[:,n1])
    end
    # higher short-circuit cost
    costs[2,5] = 10.
    flow = mincost_flow(g, capacity, demand, costs, ClpSolver())
    active_flows = [(1,2),(2,3),(3,4),(4,5),(5,6),(6,1)]
    for s in 1:6
        for t in 1:6
            if (s,t) in active_flows
                @test flow[s,t] ≈ 1.
            else
                @test flow[s,t] ≈ 0.
            end
        end
    end
        # flow conservation property
    for n1 = 1:6
        @test sum(flow[n1,:]) ≈ sum(flow[:,n1])
    end
end


text:
using TextAnalysis, JSON #, MultivariateStats, Clustering
using DataFrames

# Read in the text file

function read_json(file)
    open(file, "r") do f
        global inDict
        inDict = JSON.parse(f)
    end
    return inDict
end

textPath = string(pwd(), "/texts/isaiah.json")

isaiahjson = read_json(textPath)

isaiahtext = isaiahjson["text"]

# Want array as unified text for TextAnalysis
blockstring = ""
#for chapter in 1:length(isaiahtext)
for chapter in 24:27
    for verse in 1:length(isaiahtext[chapter])
        # print(string(chapter,":",verse," "))
        # print(isaiahtext[chapter][verse])
        global blockstring = string(blockstring," ",isaiahtext[chapter][verse])
    end
end

# Create a Corpus to Analyze

strgdc = StringDocument(blockstring)

# See the token before the cleaing
tokens(strgdc)

prepare!(strgdc, strip_punctuation)
prepare!(strgdc, strip_stopwords)
prepare!(strgdc, strip_case)
stem!(strgdc)

tokens(strgdc) # Now see it after the cleaning

ngrmdc = NGramDocument(blockstring)

crps = Corpus([strgdc])

lexicon(crps)
update_lexicon!(crps)
a = lexicon(crps)

# Get a DF for analysis and exploration
df_a = convert(DataFrame, a)

# Pivot to be more readable
colnames=names(df_a)
df_a[!, :id] = 1:nrow(df_a)
df_a = stack(df_a, colnames)

# Sort Descending
sort!(df_a, [:value], rev=true)

standardize!(crps, StringDocument)

update_lexicon!(crps)
update_inverse_index!(crps)

m = DocumentTermMatrix(crps)

dtm(m, :dense)


text:
using Test
using Permutations

@testset "Permutation" begin
    p = Permutation(7:-1:1)
    i = Permutation(7)
    @test p * p == i
    @test p == inv(p)
    a = [2, 3, 1, 6, 7, 8, 5, 4, 9]
    p = Permutation(a)
    @test order(p) == 6
    @test p^6 == Permutation(9)
    @test p^-5 == p
    @test p.data == a

    q = Permutation([1, 5, 3, 9, 4, 8, 6, 7, 2])
    @test p * q != q * p
    @test q[2] == 5
    @test q(2) == 5

    P = Matrix(p)
    Q = Matrix(q)
    @test P * Q == Matrix(p * q)
    @test P' == Matrix(inv(p))

    row = [1, 3, 5, 2, 4, 6]
    p = Permutation(row)
    @test length(p) == 6
    @test p[1] == 1
    @test length(longest_increasing(p)) == 4
    @test length(longest_decreasing(p)) == 2
    @test length(fixed_points(p)) == 2
    @test sign(p) == -1
    @test hash(p) == hash(p.data)

    M = two_row(p)
    @test M[2, :] == row

    @test Permutation(6, 1) == Permutation(6)

    p = RandomPermutation(10)
    @test p * inv(p) == Permutation(10)
    @test p' * p == Permutation(10)
    @test reverse(reverse(p)) == p

    a, b, c, d = Permutation([2, 1, 3, 4]),
    Permutation([1, 2, 4, 3]),
    Permutation([1, 3, 2, 4]),
    Permutation([2, 1, 3, 4])

    @test Matrix(a * b * c * d) == Matrix(a) * Matrix(b) * Matrix(c) * Matrix(d)

    p = RandomPermutation(12)
    d = dict(p)
    for k = 1:12
        @test p(k) == d[k]
    end

    p = RandomPermutation(12)
    pp = p * p
    q = sqrt(pp)
    @test q * q == pp

end

@testset "Cycles" begin
    p = Permutation(10)
    @test length(cycles(p)) == 10
    p = Permutation([2, 3, 4, 5, 1])
    @test length(cycles(p)) == 1

    for i = 1:10
        p = RandomPermutation(20)
        cp = cycles(p)
        @test Permutation(cp) == p
    end
end

@testset "Transposition function" begin
    p = Transposition(10, 3, 6)
    @test p == inv(p)
end

@testset "Matrix conversion" begin
    p = RandomPermutation(10)
    M = Matrix(p)
    q = Permutation(M)
    @test p == q
end

@testset "CoxeterDecomposition" begin
    n = 10
    p = RandomPermutation(n)
    c = CoxeterDecomposition(p)
    @test p == Permutation(c)

    @test inv(c) * c == CoxeterDecomposition(n, Int[])
    @test inv(c) == c'

    @test CoxeterDecomposition(5, [3, 4, 1]) == CoxeterDecomposition(5, [1, 3, 4])
    @test CoxeterDecomposition(5, [2, 1, 3, 4, 1]) == CoxeterDecomposition(5, [2, 3, 4])
    @test CoxeterDecomposition(5, [1, 3, 4, 3, 4, 3, 4, 1]) ==
          CoxeterDecomposition(5, Int[])

    @test Permutation(CoxeterDecomposition(5, Int[])).data ≈ 1:5
    @test Permutation(CoxeterDecomposition(5, [1])).data == [2; 1; 3:5]
    @test inv(CoxeterDecomposition(6, [1, 3, 5])) == CoxeterDecomposition(6, [1, 3, 5])
    @test CoxeterDecomposition(RandomPermutation(100)) isa CoxeterDecomposition # check for stack overflow
end

@testset "PermGen" begin
    X = PermGen(4)
    @test length(X) == factorial(4)

    d = [[2, 3, 4], [1, 3, 4], [1, 2, 4], [1, 2, 3]]
    X = PermGen(d)
    @test sum(length(fixed_points(p)) for p in X) == 0

    X = DerangeGen(5)
    @test length(collect(X)) == 44
end


text:
import Juno: PlotPane

@init setdisplay(Media.Graphical, PlotPane())

@require Images begin
  @render PlotPane img::Images.Image begin
    HTML() do io
      print(io, """<img src="data:image/png;base64,""")
      print(io, stringmime(MIME"image/png"(), img))
      print(io, "\" />")
    end
  end
end

@require Vega begin
  @render Inline v::Vega.VegaVisualization Text("Vega Visualisation")
  @render Clipboard v::Vega.VegaVisualization Text("Vega Visualisation")
end


text:
using Geodesics

include("../types/types.jl")
include("../setting/constants.jl")
include("../utils/readfiles.jl")
include("../utils/kdtree.jl")
include("../utils/parse_commandline_profile.jl")

function generate_profile_points(hnpts, vnpts, lon1, lat1, dep1, lon2, lat2, dep2)
    # if cut vertically
    is_cut_vertically = true
    if lon1 == lon2
        is_cut_vertically = false
    end

    # init rθϕ_new
    ngll_new = hnpts * vnpts
    xyz_new = zeros(Float64, 3, ngll_new)
    deplatlon_new = zeros(Float64, 3, ngll_new)

    # fill in deplatlon_new
    if is_cut_vertically
        for (vindex, dep) in enumerate(range(dep1, stop = dep2, length = vnpts))
            for (hindex, (lon, lat)) in enumerate(zip(range(lon1, stop = lon2, length = hnpts), range(lat1, stop = lat2, length = hnpts)))
                id = (vindex - 1) * hnpts + hindex
                deplatlon_new[:,id] = [dep,lat,lon]
            end
        end
    else
        lonnpts = hnpts
        latnpts = vnpts
        for (lonindex, lon) in enumerate(range(lon1, stop = lon2, length = lonnpts))
            for (latindex, lat) in enumerate(range(lat1, stop = lat2, length = latnpts))
                id = (lonindex - 1) * latnpts + latindex
                deplatlon_new[:,id] = [dep1,lat,lon]
            end
        end
    end

    # convert dep,lat,lon to x,y,z
    for id in 1:ngll_new
        (dep,lat,lon)=deplatlon_new[:,id]
        R_EARTH_KM = 6371.e0

        # normalized r
        r=(R_EARTH_KM-dep)/R_EARTH_KM
        θ=90-lat
        ϕ=lon

        # get x,y,z
        z=r*cosd(θ)
        h=r*sind(θ)
        x=h*cosd(ϕ)
        y=h*sind(ϕ)

        xyz_new[:,id]=[x,y,z]
    end

    return xyz_new
end

function run_interp( command_args::Dict{String,Any})
    # * init some variables
    nproc_old = command_args["nproc_old"]
    old_mesh_dir = command_args["old_mesh_dir"]
    old_model_dir = command_args["old_model_dir"]
    model_tags = command_args["model_tags"]
    output_file = command_args["output_file"]
    region=command_args["region"]
    npts=command_args["npts"]
    (lon1,lat1,lon2,lat2,dep1,dep2)=parse.(Float64,split(region,"/"))
    (hnpts,vnpts)=parse.(Int64,split(npts,"/"))

    # * init some variables
    mesh_old = sem_mesh_data()

    # * parse model tags
    model_names = String.(split(model_tags, ","))
    nmodel = length(model_names)

    @info "# nmodel=$(nmodel)"
    @info "# model names=$(model_names)"

    # * interpolate old mesh/model onto new mesh

    # typical element size at surface for old mesh
    # read from constants.jl for old mesh
    typical_size = deg2rad(max(ANGULAR_WIDTH_XI_IN_DEGREES_VAL / NEX_XI_VAL, ANGULAR_WIDTH_ETA_IN_DEGREES_VAL / NEX_ETA_VAL)) * R_UNIT_SPHERE
    max_search_dist = 10.0 * typical_size
    max_misloc = typical_size / 4.0

    # get xyz_new
    xyz_new=generate_profile_points(hnpts, vnpts, lon1, lat1, dep1, lon2, lat2, dep2)

    ngll_new = size(xyz_new)[2]
    idoubling_new = zeros(Int64, ngll_new)
    idoubling_new.=IFLAG_DUMMY

    # * initialize variables for interpolation
    location_1slice = Array{sem_mesh_location}(undef, ngll_new)
    stat_final = zeros(Int64, ngll_new)
    misloc_final = zeros(Float64, ngll_new)

    stat_final .= -1
    misloc_final .= HUGEVAL

    model_interp = zeros(nmodel, ngll_new)

    # * loop each slices of the old mesh
    flag = true

    # * loop for all points in xyz_new
    for iproc_old = 0:nproc_old - 1
        @info "# iproc_old=$(iproc_old)"
        # read old mesh slice
        mesh_old = sem_mesh_read(old_mesh_dir, iproc_old)
        nspec_old = mesh_old.nspec
        # test if the new and old mesh slices are separated apart
        min_dist = HUGEVAL
        for ispec = 1:nspec_old
            iglob = mesh_old.ibool[MIDX,MIDY,MIDZ,ispec]
            old_x=mesh_old.xyz_glob[1,iglob]
            old_y=mesh_old.xyz_glob[2,iglob]
            old_z=mesh_old.xyz_glob[3,iglob]
            dist_this_spec=sqrt(minimum(@. (xyz_new[1,:]-old_x)^2+(xyz_new[2,:]-old_y)^2+(xyz_new[3,:]-old_z)^2))
            min_dist=min(min_dist,dist_this_spec)
        end
        if min_dist>max_search_dist
            @info "#$(iproc_old) slices too far away, skip"
            continue
        end

        # read old model
        model_gll_old = zeros(Float64, nmodel, NGLLX, NGLLY, NGLLZ, nspec_old)
        sem_io_read_gll_file_n!(old_model_dir, iproc_old, model_names, nmodel, model_gll_old)

        # locate points in this mesh slice
        nnearest = 10
        location_1slice = sem_mesh_locate_kdtree2!(mesh_old, ngll_new, xyz_new, idoubling_new, nnearest, max_search_dist, max_misloc, iproc_old)

        for igll = 1:ngll_new
            if (stat_final[igll] == 1 && location_1slice[igll].stat == 1)
                @info "# multi-located, $(xyz_new[:,igll])"
                continue
            end
            # for point located inside one element in the first time or closer to one element than located before
            if location_1slice[igll].stat == 1 || (location_1slice[igll].stat == 0 && location_1slice[igll].misloc < misloc_final[igll])
                for imodel = 1:nmodel
                    model_interp[imodel,igll] = sum(location_1slice[igll].lagrange .* model_gll_old[imodel,:,:,:,location_1slice[igll].eid])
                end
                stat_final[igll] = location_1slice[igll].stat
                misloc_final[igll] = location_1slice[igll].misloc
            end
        end
    end

    # * write out gll files for this new mesh slice
    is_cut_vertically = true
    if lon1 == lon2
        is_cut_vertically = false
    end

    open(output_file,"w") do io
        if is_cut_vertically
            a, f = Geodesics.EARTH_R_MAJOR_WGS84, Geodesics.F_WGS84;
            for (vindex, dep) in enumerate(range(dep1, stop = dep2, length = vnpts))
                for (hindex, (lon, lat)) in enumerate(zip(range(lon1, stop = lon2, length = hnpts), range(lat1, stop = lat2, length = hnpts)))
                    id = (vindex - 1) * hnpts + hindex
                    dist, az, baz = Geodesics.inverse(deg2rad.((lon1, lat1, lon, lat))..., a, f)
                    if size(model_interp)[1]==1
                        write(io,"$lon $lat $dep $(model_interp[1,id]) $(dist/1000.0) \n")
                    else
                        write(io,"$lon $lat $dep $(model_interp[:,id]) $(dist/1000.0) \n")
                    end
                end
            end
        else
            lonnpts = hnpts
            latnpts = vnpts
            for (lonindex, lon) in enumerate(range(lon1, stop = lon2, length = lonnpts))
                for (latindex, lat) in enumerate(range(lat1, stop = lat2, length = latnpts))
                    id = (lonindex - 1) * latnpts + latindex
                    if size(model_interp)[1]==1
                        write(io,"$lon $lat $dep1 $(model_interp[1,id]) \n")
                    else
                        write(io,"$lon $lat $dep1 $(model_interp[:,id]) \n")
                    end
                end
            end
        end
    end
end

function main()
    command_args = parse_commandline()
    run_interp(  command_args)
end

main()

max_stars_repo_name: ziyixi/SeisScripts

max_issues_repo_name: ziyixi/SeisScripts

max_forks_repo_name: ziyixi/SeisScripts


text:
using SparseArrays
using Random
using SpatialEcology
using Distances
using StableRNGs
using Test

@testset "ComMatrix" begin
    rng = StableRNG(1337)

    datb = sprand(rng, Bool, 12,8,0.9)

    dati = sprand(rng, 8,13,0.9).*100
    dati = round.(Int, dati)

    datf = sprand(rng, 11,9,0.9)

    cmb = ComMatrix(datb)
    @test cmb isa ComMatrix{Bool}
    cmi = ComMatrix(dati,
        speciesnames = ["sp1", "sp2", "sp3", "sp4", "sp5", "sp6", "sp7", "sp8"],
        sitenames = [:a, :b, :c, :d, :e, :f, :g, :h, :i, :j, :k, :l, :m])
    @test cmi isa ComMatrix{<:Union{Int64, Int32}}
    cmf = ComMatrix(datf, sitecolumns = false)

    @test ComMatrix(Matrix(datf)).occurrences == ComMatrix(datf).occurrences

    @test occupancy(cmb) == [6, 6, 8, 7, 5, 8, 7, 8, 6, 7, 7, 8]
    @test occupancy(cmi) == [11, 13, 13, 10, 12, 12, 11, 11]
    @test occupancy(cmf) == [9, 10, 11, 11, 9, 9, 11, 9, 11]

    @test richness(cmb) == [9, 10, 12, 11, 11, 9, 11, 10]
    @test richness(cmi) == [8, 8, 7, 7, 7, 7, 6, 7, 8, 6, 8, 7, 7]
    @test richness(cmf) == [8, 9, 9, 8, 8, 7, 9, 8, 9, 8, 7]

    @test nsites(cmb) == 8
    @test nsites(cmi) == 13
    @test nsites(cmf) == 11

    @test nspecies(cmb) == 12
    @test nspecies(cmi) == 8
    @test nspecies(cmf) == 9

    @test occurring(cmb) == 1:12
    @test occurring(cmi) == 1:8
    @test occurring(cmf) == 1:9

    @test occupied(cmb) == 1:8
    @test occupied(cmi) == 1:13
    @test occupied(cmf) == 1:11

    @test noccurring(cmb) == 12
    @test noccurring(cmi) == 8
    @test noccurring(cmf) == 9

    @test noccupied(cmb) == 8
    @test noccupied(cmi) == 13
    @test noccupied(cmf) == 11

    @test getspecies(cmb, 3) == [true, true, true, true, true, true, true, true]
    @test getspecies(cmb, "species2") == [false, true, true, false, true, true, true, true]
    @test getspecies(cmi, 2) == [10, 61, 48, 6, 11, 69, 11, 36, 14, 91, 28, 93, 58]
    gcm = getspecies(cmf, 6)
    @test gcm[3] == 0.9426149574692801
    @test gcm isa SubArray

    @test speciesnames(cmi)[8] == "sp8"
    @test speciesnames(cmf)[3] == "species3"

    @test sitenames(cmi)[4] == "d"
    @test sitenames(cmb)[5] == "site5"

    @test length(sitenames(cmf)) == nsites(cmf)
    @test length(sitenames(cmi)) == nsites(cmi)
    @test length(sitenames(cmb)) == nsites(cmb)

    @test length(speciesnames(cmf)) == nspecies(cmf)
    @test length(speciesnames(cmi)) == nspecies(cmi)
    @test length(speciesnames(cmb)) == nspecies(cmb)

    @test sitetotals(cmb) == [9, 10, 12, 11, 11, 9, 11, 10]
    @test sitetotals(cmb) == richness(cmb)
    @test sitetotals(cmi) == [336, 520, 259, 301, 264, 243, 285, 450, 400, 280, 431, 470, 424]
    @test length(sitetotals(cmf)) == nsites(cmf)
    @test sitetotals(cmf)[5] ≈ 3.985356922989832

    @test speciestotals(cmb) == occupancy(cmb)
    @test speciestotals(cmi) == [572, 536, 703, 537, 729, 549, 514, 523]
    @test length(speciestotals(cmf)) == nspecies(cmf)
    @test speciestotals(cmf)[8] ≈ 5.171424049687359

    @test size(cmb) == (12, 8)
    @test size(cmi, 1) == 8

    @test cooccurring(cmb, 1, 3) == [true, false, true, true, true, false, true, true]
    @test cooccurring(cmf, [8, 3]) == [true, true, true, true, true, false, true, true, true, false, true]

    dist = pairwise(BrayCurtis(), view(cmi, sites=1:2))
    @test round.(dist, digits=1) == [0.0 0.3; 0.3 0.0]
  # getindex and setindex are to do

end


text:
# -*- encoding: utf-8 -*-
#
# The MIT License (MIT)
#
# Copyright © 2021 Matteo Foglieni and Riccardo Gervasoni
#

"""
     PCG(state::UInt64 = UInt64(42), inc::UInt64 = UInt64(54))

A mutable struct for the Permuted Congruential Generator (PCG)
which is a uniform pseudo-random number generator.

## Parameters

- `state::UInt64 = UInt64(42)` : initial state number
- `inc::UInt64 = UInt64(54)` : initial sequence number

## References

Melissa E. O’Neill (2014), ["PCG: A Family of Simple Fast
Space-Efficient Statistically Good Algorithms
for Random Number Generation"](https://www.pcg-random.org/paper.html)

See also: [`random(pcg::PCG, ::Type{UInt32})`](@ref),
[`random(pcg::PCG, ::Type{Float64})`](@ref)
"""
mutable struct PCG
     state::UInt64
     inc::UInt64

     function PCG(init_state::UInt64 = UInt64(42), init_seq::UInt64 = UInt64(54))
          # 64-bit
          self = new(UInt64(0), (init_seq << UInt64(1)) | UInt64(1))

          random(self, UInt32)

          # 64-bit
          self.state += init_state

          random(self, UInt32)
          return self
     end
end

function get_state(pcg::PCG)
     value = pcg.state
     return Int128(value)
end

function get_inc(pcg::PCG)
     value = pcg.inc
     return Int128(value)
end

"""
     random(pcg::PCG, ::Type{UInt32}) :: UInt32

Return a new random UInt32 number and advance PCG's internal state.

This function is based on the paper of Melissa E. O’Neill (2014),
where the Permuted Congruential Generator (PCG) family of random
number generators is defined and explained.

## References

Melissa E. O’Neill (2014), ["PCG: A Family of Simple Fast
Space-Efficient Statistically Good Algorithms
for Random Number Generation"](https://www.pcg-random.org/paper.html)

See also: [`random(pcg::PCG, ::Type{Float64})`](@ref), [`PCG`](@ref)
"""
function random(pcg::PCG, ::Type{UInt32})
     # 64-bit
     oldstate = pcg.state

     # 64-bit
     pcg.state = UInt64(oldstate * UInt64(6364136223846793005) + pcg.inc) # % typemax(UInt64)

     # 32-bit
     xorshifted = UInt32( ((oldstate >> UInt64(18)) ⊻ oldstate) >> UInt64(27) & typemax(UInt32))

     # 32-bit
     rot = oldstate >> UInt64(59)

     # 32-bit
     return UInt32( ((xorshifted >> rot) | (xorshifted << ((-rot) & 31))))  # % typemax(UInt32)
end

"""
     random(pcg::PCG, ::Type{Float64}) :: Float64

Returns a `Float64` random number inside `[0,1]` interval obtained
with a PCG Uniform Pseudo-random Number Generator.

It calls the `random(pcg::PCG, ::Type{UInt32})` function, which
returns a `UInt32` random number, and divides it with ` typemax(UInt32)`.

See also: [`random(pcg::PCG, ::Type{UInt32})`](@ref), [`PCG`](@ref)
"""
random(pcg::PCG, ::Type{Float64}) = random(pcg, UInt32) / typemax(UInt32)

"""
     random(pcg::PCG) :: Float64

Returns a `Float64` random number inside `[0,1]` interval obtained
with a PCG Uniform Pseudo-random Number Generator.

It calls the `random(pcg::PCG, ::Type{Float64})` function.

See also: [`random(pcg::PCG, ::Type{Float64})`](@ref),
[`random(pcg::PCG, ::Type{UInt32})`](@ref), [`PCG`](@ref)
"""
random(pcg::PCG) = random(pcg, Float64)


text:
#!/usr/bin/env julia

#=
 Licensing Information: You are free to use or extend these projects for
 education or reserach purposes provided that (1) you retain this notice
 and (2) you provide clear attribution to UC Berkeley, including a link
 to http://barc-project.com

 Attibution Information: The barc project ROS code-base was developed
 at UC Berkeley in the Model Predictive Control (MPC) lab by Jon Gonzales
 (jon.gonzales@berkeley.edu). The cloud services integation with ROS was developed
 by Kiet Lam  (kiet.lam@berkeley.edu). The web-server app Dator was
 based on an open source project by Bruce Wootton
=#

using RobotOS
@rosimport barc.msg: ECU, Encoder, Ultrasound, Z_KinBkMdl
@rosimport data_service.msg: TimeData
@rosimport geometry_msgs.msg: Vector3
@rosimport ackermann_msgs.msg: AckermannDrive, AckermannDriveStamped
@rosimport nav_msgs.msg: Odometry
rostypegen()
using barc.msg
using data_service.msg
using geometry_msgs.msg
using ackermann_msgs.msg
using nav_msgs.msg
using JuMP
using Ipopt

# define model parameters
L_a     = 0.125         # distance from CoG to front axel
L_b     = 0.125         # distance from CoG to rear axel
dt      = 0.1           # time step of system
a_max   = 1             # maximum acceleration

# preview horizon
N       = 5

# define targets [generic values]
x_ref   = 10
y_ref   = 0

# define decision variables
# states: position (x,y), yaw angle, and velocity
# inputs: acceleration, steering angle
println("Creating kinematic bicycle model ....")
mdl     = Model(solver = IpoptSolver(print_level=3))
@defVar( mdl, x[1:(N+1)] )
@defVar( mdl, y[1:(N+1)] )
@defVar( mdl, psi[1:(N+1)] )
@defVar( mdl, v[1:(N+1)] )
@defVar( mdl, a[1:N] )
@defVar( mdl, d_f[1:N] )

# define objective function
@setNLObjective(mdl, Min, (x[N+1] - x_ref)^2 + (y[N+1] - y_ref)^2 )

# define constraints
# define system dynamics
# Reference: R.Rajamani, Vehicle Dynamics and Control, set. Mechanical Engineering Series,
#               Spring, 2011, page 26
@defNLParam(mdl, x0     == 0); @addNLConstraint(mdl, x[1]     == x0);
@defNLParam(mdl, y0     == 0); @addNLConstraint(mdl, y[1]     == y0);
@defNLParam(mdl, psi0   == 0); @addNLConstraint(mdl, psi[1]   == psi0 );
@defNLParam(mdl, v0     == 0); @addNLConstraint(mdl, v[1]     == v0);
@defNLExpr(mdl, bta[i = 1:N], atan( L_a / (L_a + L_b) * tan(d_f[i]) ) )
for i in 1:N
    @addNLConstraint(mdl, x[i+1]    == x[i]      + dt*(v[i]*cos( psi[i] + bta[i] ))  )
    @addNLConstraint(mdl, y[i+1]    == y[i]      + dt*(v[i]*sin( psi[i] + bta[i] ))  )
    @addNLConstraint(mdl, psi[i+1]  == psi[i]    + dt*(v[i]/L_b * sin(bta[i]))  )
    @addNLConstraint(mdl, v[i+1]    == v[i]      + dt*(a[i])  )
    @addConstraint(mdl, 0 <=  a[i] <= a_max  )
end

# status update
println("initial solve ...")
solve(mdl)
println("finished initial solve!")

function SE_callback(msg::Odometry)
    # update mpc initial condition
    setValue(x0,    msg.pose.pose.position.x)
    setValue(y0,    msg.pose.pose.position.y)
    setValue(psi0,  msg.pose.pose.orientation)
    setValue(v0,    msg.twist.linear.z)
end

function main()
    # initiate node, set up publisher / subscriber topics
    init_node("mpc")
    pub = Publisher("ecu", AckermannDriveStamped, queue_size=10)
    sub  = Subscriber("state_estimate", Odometry, SE_callback, queue_size=10)
    loop_rate = Rate(10)

    while ! is_shutdown()
        # run mpc, publish command
        solve(mdl)

        # get optimal solutions
        a_opt   = getValue(a[1])
        d_f_opt = getValue(d_f[1])
        cmd = AckermannDriveStamped()
        cmd.drive.steering_angle = d_f_opt
        cmd.drive.speed = a_opt
        # publish commands
        publish(pub, cmd)
        rossleep(loop_rate)
    end
end

if ! isinteractive()
    main()
end


text:
# # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # #
#
# Desription
# ==============================================================================
#
#   Tests related to conversion from Euler angles to direction cosine matrices.
#
# # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # #

# File: ./src/conversions/angle_to_dcm.jl
# =======================================

# Functions: angle_to_dcm
# -----------------------

@testset "Angle about an axis => DCM (Float64)" begin
    # Sample an angle to use in all rotations.
    ang = _rand_ang()

    # Rotations
    # =========

    # X Axis
    # ------

    # Create a DCM that rotates about X axis.
    dcm = angle_to_dcm(ang, :X)
    @test eltype(dcm) === Float64

    # Create a vector that does not have X component.
    v = SVector(0, randn(), randn())

    # Rotate the reference using the DCM.
    v_r = dcm * v
    @test eltype(v_r) === Float64

    # Get the sine of the angle between the representations.
    sin_ang = ((v_r × v) / norm(v)^2)[1]

    # Test the angle between the two representations.
    @test abs(sin(ang) - sin_ang) ≈ 0 atol = 1e-12

    # Y Axis
    # ------

    # Create a DCM that rotates about Y axis.
    dcm = angle_to_dcm(ang, :Y)
    @test eltype(dcm) === Float64

    # Create a vector that does not have Y component.
    v = SVector(randn(), 0, randn())

    # Rotate the reference using the DCM.
    v_r = dcm * v
    @test eltype(v_r) === Float64

    # Get the sine of the angle between the representations.
    sin_ang = ((v_r × v) / norm(v)^2)[2]

    # Test the angle between the two representations.
    @test abs(sin(ang) - sin_ang) ≈ 0 atol = 1e-12

    # Z Axis
    # ------

    # Create a DCM that rotates about Z axis.
    dcm = angle_to_dcm(ang, :Z)
    @test eltype(dcm) === Float64

    # Create a vector that does not have Z component.
    v = SVector(randn(), randn(), 0)

    # Rotate the reference using the DCM.
    v_r = dcm * v
    @test eltype(v_r) === Float64

    # Get the sine of the angle between the representations.
    sin_ang = ((v_r × v) / norm(v)^2)[3]

    # Test the angle between the two representations.
    @test abs(sin(ang) - sin_ang) ≈ 0 atol = 1e-12
end

@testset "Angle about an axis => DCM (Float32)" begin
    # Sample an angle to use in all rotations.
    ang = _rand_ang(Float32)

    # Rotations
    # =========

    # X Axis
    # ------

    # Create a DCM that rotates about X axis.
    dcm = angle_to_dcm(ang, :X)
    @test eltype(dcm) === Float32

    # Create a vector that does not have X component.
    v = SVector{3, Float32}(0, randn(), randn())

    # Rotate the reference using the DCM.
    v_r = dcm * v
    @test eltype(v_r) === Float32

    # Get the sine of the angle between the representations.
    sin_ang = ((v_r × v) / norm(v)^2)[1]

    # Test the angle between the two representations.
    @test abs(sin(ang) - sin_ang) ≈ 0 atol = 1e-6

    # Y Axis
    # ------

    # Create a DCM that rotates about Y axis.
    dcm = angle_to_dcm(ang, :Y)
    @test eltype(dcm) === Float32

    # Create a vector that does not have Y component.
    v = SVector{3, Float32}(randn(), 0, randn())

    # Rotate the reference using the DCM.
    v_r = dcm * v
    @test eltype(v_r) === Float32

    # Get the sine of the angle between the representations.
    sin_ang = ((v_r × v) / norm(v)^2)[2]

    # Test the angle between the two representations.
    @test abs(sin(ang) - sin_ang) ≈ 0 atol = 1e-6

    # Z Axis
    # ------

    # Create a DCM that rotates about Z axis.
    dcm = angle_to_dcm(ang, :Z)
    @test eltype(dcm) === Float32

    # Create a vector that does not have Z component.
    v = SVector{3, Float32}(randn(), randn(), 0)

    # Rotate the reference using the DCM.
    v_r = dcm * v
    @test eltype(v_r) === Float32

    # Get the sine of the angle between the representations.
    sin_ang = ((v_r × v) / norm(v)^2)[3]

    # Test the angle between the two representations.
    @test abs(sin(ang) - sin_ang) ≈ 0 atol = 1e-6
end

@testset "Angle about an axis => DCM (Errors)" begin
    @test_throws ArgumentError angle_to_dcm(0, :A)
end

@testset "Euler angles => DCM (Float64)" begin
    T = Float64

    # Two rotations
    # ==========================================================================

    for rot_seq in valid_rot_seqs_2angles
        # Sample Euler angles.
        θ₁ = _rand_ang(T)
        θ₂ = _rand_ang(T)

        # Convert to DCM.
        D = angle_to_dcm(θ₁, θ₂, rot_seq)
        @test eltype(D) === T

        # Create the DCM by composing the rotations.
        rot_seq_str = string(rot_seq)
        D₁ = angle_to_dcm(θ₁, Symbol(rot_seq_str[1]))
        D₂ = angle_to_dcm(θ₂, Symbol(rot_seq_str[2]))
        De = D₂ * D₁

        # Compare.
        @test D ≈ De
    end

    # Three rotations
    # ==========================================================================

    for rot_seq in valid_rot_seqs
        # Sample Euler angles.
        ea = EulerAngles(_rand_ang(T), _rand_ang(T), _rand_ang(T), rot_seq)

        # Convert to DCM.
        D = angle_to_dcm(ea)
        @test eltype(D) === T

        # Create the DCM by composing the rotations.
        rot_seq_str = string(rot_seq)
        D₁ = angle_to_dcm(ea.a1, Symbol(rot_seq_str[1]))
        D₂ = angle_to_dcm(ea.a2, Symbol(rot_seq_str[2]))
        D₃ = angle_to_dcm(ea.a3, Symbol(rot_seq_str[3]))
        De = D₃ * D₂ * D₁

        # Compare.
        @test D ≈ De
    end
end

@testset "Euler angles => DCM (Float32)" begin
    T = Float32

    # Two rotations
    # ==========================================================================

    for rot_seq in valid_rot_seqs_2angles
        # Sample Euler angles.
        θ₁ = _rand_ang(T)
        θ₂ = _rand_ang(T)

        # Convert to DCM.
        D = angle_to_dcm(θ₁, θ₂, rot_seq)
        @test eltype(D) === T

        # Create the DCM by composing the rotations.
        rot_seq_str = string(rot_seq)
        D₁ = angle_to_dcm(θ₁, Symbol(rot_seq_str[1]))
        D₂ = angle_to_dcm(θ₂, Symbol(rot_seq_str[2]))
        De = D₂ * D₁

        # Compare.
        @test D ≈ De
    end

    # Three rotations
    # ==========================================================================

    for rot_seq in valid_rot_seqs
        # Sample Euler angles.
        ea = EulerAngles(_rand_ang(T), _rand_ang(T), _rand_ang(T), rot_seq)

        # Convert to DCM.
        D = angle_to_dcm(ea)
        @test eltype(D) === T

        # Create the DCM by composing the rotations.
        rot_seq_str = string(rot_seq)
        D₁ = angle_to_dcm(ea.a1, Symbol(rot_seq_str[1]))
        D₂ = angle_to_dcm(ea.a2, Symbol(rot_seq_str[2]))
        D₃ = angle_to_dcm(ea.a3, Symbol(rot_seq_str[3]))
        De = D₃ * D₂ * D₁

        # Compare.
        @test D ≈ De
    end
end

@testset "Euler angles => DCM (Promotion)" begin
    # Check if promotion is working as intended.
    dcm = angle_to_dcm(Int64(1), 0.0f0, Float64(0))
    @test eltype(dcm) === Float64

    dcm = angle_to_dcm(Int64(1), 0.0f0, 0.0f0)
    @test eltype(dcm) === Float32

    dcm = angle_to_dcm(Int64(1), Int32(0), Float16(0))
    @test eltype(dcm) === Float16
end

@testset "Euler angles => DCM (Errors)" begin
    @test_throws ArgumentError angle_to_dcm(1, :V)
    @test_throws ArgumentError angle_to_dcm(1, 2, :ZZ)
    @test_throws ArgumentError angle_to_dcm(1, 2, 3, :ZZY)
end

# Functions: smallangle_to_dcm
# ----------------------------

@testset "Small Euler angles => DCM (Float64)" begin
    T = Float64

    # Sample three small angles.
    θx = _rand_ang(T) * T(0.001)
    θy = _rand_ang(T) * T(0.001)
    θz = _rand_ang(T) * T(0.001)

    # Create the rotation matrix without normalization.
    D = smallangle_to_rot(θx, θy, θz, normalize = false)
    @test eltype(D) === T

    # Expected result.
    De = DCM(
          1, +θz, -θy,
        -θz,   1, +θx,
        +θy, -θx,   1
    )'

    @test De ≈ D

    # Create the rotation matrix with normalization.
    D = smallangle_to_rot(θx, θy, θz, normalize = true)
    @test eltype(D) === T

    @test orthonormalize(De) ≈ D
end

@testset "Small Euler angles => DCM (Float32)" begin
    T = Float32

    # Sample three small angles.
    θx = _rand_ang(T) * T(0.001)
    θy = _rand_ang(T) * T(0.001)
    θz = _rand_ang(T) * T(0.001)

    # Create the rotation matrix without normalization.
    D = smallangle_to_rot(θx, θy, θz, normalize = false)
    @test eltype(D) === T

    # Expected result.
    De = DCM(
          1, +θz, -θy,
        -θz,   1, +θx,
        +θy, -θx,   1
    )'

    @test De ≈ D

    # Create the rotation matrix with normalization.
    D = smallangle_to_rot(θx, θy, θz, normalize = true)
    @test eltype(D) === T

    @test orthonormalize(De) ≈ D
end

@testset "Small Euler angles => DCM (Promotion)" begin
    # Check if promotion is working as intended.
    dcm = smallangle_to_dcm(Int64(1), 0.0f0, Float64(0))
    @test eltype(dcm) === Float64

    dcm = smallangle_to_dcm(Int64(1), 0.0f0, 0.0f0)
    @test eltype(dcm) === Float32

    dcm = smallangle_to_dcm(Int64(1), Int32(0), Float16(0))
    @test eltype(dcm) === Float16
end


text:
function Base.fill(val::Node, dims::Vararg{Union{Integer,AbstractUnitRange}})
    return invoke(fill, Tuple{Node, Vararg{Any}}, val, dims...)
end

function Base.fill(val::Node, dims::Tuple{Vararg{Integer}})
    return invoke(fill, Tuple{Node, Vararg{Any}}, val, dims)
end


text:
@parameters t
D = Differential(t)

@testset "0-junction equations" begin
    model = BondGraph(:RC)
    C = Component(:C)
    R = Component(:R)
    zero_law = EqualEffort()

    add_node!(model, [R, C, zero_law])
    connect!(model, R, zero_law)
    connect!(model, zero_law, C)

    @test numports(zero_law) == 2

    @variables E[1:2](t) F[1:2](t)
    @test cr(zero_law) == [
        0 ~ F[1] + F[2],
        0 ~ E[1] - E[2]
    ]
end

@testset "1-junction equations" begin
    c1 = new(:C,:C1)
    c2 = new(:R,:R1)
    c3 = new(:I,:I1)
    j = EqualFlow()

    bg = BondGraph()
    add_node!(bg, [c1, c2, c3, j])
    connect!(bg, c1, j)
    connect!(bg, j, c2)
    connect!(bg, j, c3)

    @test numports(j) == 3
    @test length(j.weights) == 3
    @test j.weights == [1,-1,-1]

    @variables E[1:3](t) F[1:3](t)
    @test cr(j) == [
        0 ~ E[1] - E[2] - E[3],
        0 ~ F[1] + F[2],
        0 ~ F[1] + F[3],
    ]
end

function rlc()
    r = new(:R)
    l = new(:I)
    c = new(:C)
    kvl = EqualEffort(name=:kvl)

    bg = BondGraph()
    add_node!(bg, [c, l, kvl, r])

    connect!(bg, r, kvl)
    connect!(bg, l, kvl)
    connect!(bg, c, kvl)
    return bg
end

@testset "RC circuit" begin
    r = new(:R)
    c = new(:C)
    bg = BondGraph(:RC)
    add_node!(bg, [c, r])
    connect!(bg, r, c)

    sys = ODESystem(bg)
    eqs = ModelingToolkit.equations(sys)
    @test length(eqs) == 1

    (C,R) = sys.ps
    x = sys.states[1]
    e1 = eqs[1]
    e2 = D(x) ~ -x/C/R
    @test e1.lhs == e2.lhs
    @test expand(e1.rhs) == e2.rhs
end

@testset "RLC circuit" begin
    bg = rlc()
    eqs = BondGraphs.equations(bg)
    @test length(eqs) == 2

    sys = ODESystem(bg)
    eqs = ModelingToolkit.equations(sys)
    (C,L,R) = sys.ps
    (qC,pL) = sys.states
    e1 = D(qC) ~ -pL/L - qC/C/R
    e2 = D(pL) ~ qC/C

    @test expand(eqs[1].rhs) == e1.rhs
    @test eqs[2].rhs == e2.rhs
end

@testset "Chemical reaction A ⇌ B" begin
    A = new(:ce,:A)
    B = new(:ce,:B)
    re = new(:re,:r)
    bg = BondGraph()

    add_node!(bg,[A,B,re])
    connect!(bg, A, re; dstportindex=1)
    connect!(bg, re, B; srcportindex=2)
    sys = ODESystem(bg)
    eqs = ModelingToolkit.equations(sys)

    (xA,xB) = sys.states
    (KA,KB,r) = sys.ps
    e1 = D(xA) ~ -r*(KA*xA - KB*xB)
    e2 = D(xB) ~ r*(KA*xA - KB*xB)

    @test eqs[1].rhs == e1.rhs
    @test eqs[2].rhs == e2.rhs
end

@testset "Chemical reaction A ⇌ B + C, C ⇌ D" begin
    C_A = new(:ce,:A)
    C_B = new(:ce,:B)
    C_C = new(:ce,:C)
    C_D = new(:ce,:D)
    re1 = new(:re,:r1)
    re2 = new(:re,:r2)
    common_C = EqualEffort()
    BC = EqualFlow()

    bg = BondGraph()
    add_node!(bg,[C_A,C_B,C_C,C_D,re1,re2,common_C,BC])
    connect!(bg,C_A,re1; dstportindex=1)
    connect!(bg,re1,BC; srcportindex=2)
    connect!(bg,BC,C_B)
    connect!(bg,BC,common_C)
    connect!(bg,common_C,C_C)
    connect!(bg,common_C,re2; dstportindex=1)
    connect!(bg,re2,C_D; srcportindex=2)

    sys = ODESystem(bg)
    eqs = ModelingToolkit.equations(sys)

    (xA,xB,xC,xD) = sys.states
    (KA,KB,KC,KD,r1,r2) = sys.ps
    e1 = D(xA) ~ -r1*(KA*xA - KB*xB*KC*xC)
    e2 = D(xB) ~ r1*(KA*xA - KB*xB*KC*xC)
    e3 = D(xC) ~ r1*(KA*xA - KB*xB*KC*xC) - r2*(KC*xC - KD*xD)
    e4 = D(xD) ~ r2*(KC*xC - KD*xD)

    @test eqs[1].rhs == e1.rhs
    @test eqs[2].rhs == e2.rhs
    @test eqs[3].rhs == e3.rhs
    @test eqs[4].rhs == e4.rhs
end

max_stars_repo_name: jedforrest/BondGraphs

max_issues_repo_name: jedforrest/BondGraphs

max_forks_repo_name: jedforrest/BondGraphs


text:
W,(WW,EW,FW) = Lar.cuboid([1,1], true)
square = (W,EW)

square = Lar.Struct([  Lar.r(pi/4),  square  ])
squares = Lar.Struct([  square,  Lar.t(√2/2, 0),  square  ])

V,EV = Lar.struct2lar(squares)
VV = [[k] for k=1:size(V,2)]
GL.VIEW(GL.numbering(.4)((V,[VV, EV])));

model = CAGD.Model(V);
CAGD.addModelCells!(model, 1, EV);

# Planar Arrangment

A_model = CAGD.planar_arrangement(model);

A_model.G
Matrix(A_model.T[1])
Matrix(A_model.T[2])

# Display Models

if todisplay
    VV = [[k] for k=1:size(A_model.G, 2)];
    triangulated_faces = CAGD.triangulate2D(A_model);
    FVs = convert(Array{Lar.Cells}, triangulated_faces);

    GL.VIEW(GL.GLExplode(A_model.G, FVs, 1.0, 1.0, 1.0, 99, 1));
    GL.VIEW(GL.GLExplode(A_model.G, FVs, 1.2, 1.2, 1.2, 99, 1));
end

innerpoints = Lar.internalpoints2d(A_model.G', A_model.T[1], A_model.T[2][1:end, :])
# associate internal points to 2-cells
listOfModels = Lar.evalStruct(squares)
inputfacenumbers = [length(listOfModels[k][2]) for k=1:length(listOfModels)]
# test input data for containment of reference points
boolmatrix = BitArray(undef, length(innerpoints), length(listOfModels))
containmenttest = Lar.testinternalpoint2d(listOfModels)
for (k,point) in enumerate(innerpoints) # k runs on columns
    cells = containmenttest(point) # contents of columns
    #println(k," ",faces)
    for l in cells
        boolmatrix[k,l] = 1
    end
end

sq1 = boolmatrix[:, 1]
sq2 = boolmatrix[:, 2]

sqXor = sq1 .⊻ sq2

if todisplay
    GL.VIEW(GL.GLExplode(A_model.G, FVs[sparse(sqXor).nzind], 1.0, 1.0, 1.0, 99, 1));
end


text:
module TestBazykin85
include("preamble_plots.jl")

using DiffEqBase: ODEProblem
using StaticArrays: SVector

using Bifurcations: resolved_points
using Bifurcations.BifurcationsBase: contkind, HopfCont
using Bifurcations.Codim2: NormalizingAS, BackReferencingAS
using Bifurcations.Examples.Bazykin85

# Bifurcation points calculated in some version of Bifurcations.jl
KNOWN_POINTS = Dict(
    # Bazykin85.Bazykin85Param => points
    Bazykin85.Bazykin85Param(
        ϵ = 0.01,
    ) => [
        (Codim2.PointTypes.cusp,
         [32.5936, 20.4743, 0.956835, 0.290633, 0.901233, 0.00356842]),
        (Codim2.PointTypes.bogdanov_takens,
         [42.688, 21.6306, 0.993354, 0.1151, 0.860704, 0.00605879]),
        (Codim2.PointTypes.bogdanov_takens,
         [6.26577, 3.60155, 0.932668, 0.360736, 0.453624, 0.175128]),
        (Codim2.PointTypes.cusp,
         [25.8231, 2.44209, 0.999458, 0.0329046, 0.0887673, 2.80236]),
    ],
)

@testset "smoke Bazykin85 codim-2 (ϵ=$ϵ, $(nameof(ASType)))" for
        ϵ in [0.01, 0.001],
        ASType in [NormalizingAS, BackReferencingAS]

    prob = Bazykin85.make_prob(
        Bazykin85.Bazykin85Param(
            ϵ = ϵ,
        ),
        phase_space = (SVector(0.0, 0.0),  # u_min
                       SVector(Inf, Inf)), # u_max
    )
    ode = prob.p.de_prob

    codim1_solver = init(prob)
    solve!(codim1_solver)

    resolved = resolved_points(codim1_solver)
    if ϵ > 0.001
        @test length(resolved) == 5
    else
        @test_broken length(resolved) == 5
    end

    sn_point, = sort!(
        special_intervals(codim1_solver, Codim1.PointTypes.saddle_node),
        by=p->p.u0[1])
    hopf_point, = special_intervals(codim1_solver, Codim1.PointTypes.hopf)

    hopf_solver1 = nothing
    sn_solver1 = nothing
    for point in [sn_point, hopf_point]
        codim2_prob = BifurcationProblem(
            point,
            codim1_solver,
            (@lens _.δ),
            (0.0, 10.0),
            augmented_system = ASType(),
        )
        codim2_solver = init(
            codim2_prob;
            # h0 = 0.001,
            nominal_angle_rad = 0.01,
            max_samples = 1000,
        )
        solve!(codim2_solver)

        @testset "show" begin
            smoke_test_solver_show(codim2_solver)
        end

        @testset "plot" begin
            smoke_test_solver_plot(codim2_solver)
        end

        @testset "resolve $p" for p in [Codim2.PointTypes.bogdanov_takens,
                                        Codim2.PointTypes.fold_hopf,
                                        Codim2.PointTypes.hopf_hopf]
            @test_nothrow resolved_points(codim2_solver, p)
        end

        if contkind(codim2_solver) isa HopfCont
            hopf_solver1 = codim2_solver
        else
            sn_solver1 = codim2_solver
        end
    end

    @testset "switch to saddle-node" begin
        @assert hopf_solver1 !== nothing
        # Find the right-most Bogdanov-Takens bifurcation:
        point = first(sort(
            special_intervals(hopf_solver1,
                           Codim2.PointTypes.bogdanov_takens);
            by = p -> p.u0[end - 1],  # α
            rev = true))
        sn_prob = BifurcationProblem(
            point, hopf_solver1;
            augmented_system = ASType(),
        )
        sn_solver2 = init(sn_prob)
        @test_nothrow solve!(sn_solver2)
    end

    @testset "switch to hopf" begin
        @assert sn_solver1 !== nothing
        # Find the right-most Bogdanov-Takens bifurcation:
        point = first(sort(
            special_intervals(sn_solver1,
                           Codim2.PointTypes.bogdanov_takens);
            by = p -> p.u0[end - 1],  # α
            rev = true))
        hopf_prob = BifurcationProblem(
            point, sn_solver1;
            augmented_system = ASType(),
        )
        hopf_solver2 = init(hopf_prob)
        @test_nothrow solve!(hopf_solver2)

        bpoints = special_intervals(hopf_solver2, Codim2.PointTypes.bautin)
        @test length(bpoints) == 1
        @test_nothrow resolved_points(hopf_solver2, Codim2.PointTypes.bautin)
    end

    if haskey(KNOWN_POINTS, ode.p)
        desired_points = KNOWN_POINTS[ode.p]
        @testset "known points" begin
            # TODO: do this for all solvers (hopf_solver1 etc.)
            uniquified = sort(
                uniquify_points(resolved_points(sn_solver1)),
                by = point -> point.u[end],
            )
            @test length(uniquified) == length(desired_points)
            for (actual, desired) in zip(uniquified, desired_points)
                type_desired, u_desired = desired
                if ASType == NormalizingAS
                    @test actual.u ≈ u_desired  rtol=1e-4
                else
                    # Compare DS state and parameters; ignore the eigenvector
                    # TODO: don't hard-code offsets:
                    @test actual.u[1:2] ≈ u_desired[1:2]  rtol=1e-4
                    @test actual.u[end-1:end] ≈ u_desired[end-1:end]  rtol=1e-4
                end
                @test actual.point_type == type_desired
            end
        end
    end
end

end  # module


text:
using ParamLevelSet
using jInv.Mesh
using SparseArrays
using LinearAlgebra
using Test

plotting = false;
if plotting
	using jInvVis
	using PyPlot
end

println("~~~~~~~~~~~~~~~~Testing Mesh Free RBFs.~~~~~~~~~~~~~~~~~~")

n = [32,32,32];
Mesh = getRegularMesh([0.0;3.0;0.0;3.0;0.0;3.0],n);
alpha = [1.5;2.5;-2.0;-1.0];
beta = [2.5;2.0;-1.5;2.5];
Xs = [0.5 0.5 0.5 ; 2.0 2.0 2.0; 1.2 2.3 1.5; 2.2 1.5 2.0];
m = wrapTheta(alpha,beta,Xs);

u, = MeshFreeParamLevelSetModelFunc(Mesh,m;computeJacobian=0);
if plotting
	figure()
	u = reshape(u,tuple(Mesh.n...));
	plotModel(u)
end

sigmaH = getDefaultHeaviside();

us, = MeshFreeParamLevelSetModelFunc(Mesh,m;computeJacobian=0,sigma = sigmaH);
if plotting
	figure()
	plotModel(us)
end

bf = 1;
dm = 0.01*randn(length(alpha)*5);

hh = 1.0;
u0,JBuilder = MeshFreeParamLevelSetModelFunc(Mesh,m;computeJacobian=1,bf = bf);
J0 = getSparseMatrix(JBuilder);
for k=0:10
	hhh = (0.5^k)*hh;
	ut = MeshFreeParamLevelSetModelFunc(Mesh,m+ hhh*dm;computeJacobian=0,bf = bf)[1];
	println("norm(ut-u0): ",norm(ut[:]-u0[:]),", norm(ut - u0 - J0*dm): ",norm(ut[:] - u0[:] - J0*hhh*dm));
end

println("With Heaviside func");

hh = 1.0;

u0,JBuilder = MeshFreeParamLevelSetModelFunc(Mesh,m;computeJacobian=1,sigma=sigmaH,bf = bf);
J0 = getSparseMatrix(JBuilder);

for k=0:10
	hhh = (0.5^k)*hh;
    ut = MeshFreeParamLevelSetModelFunc(Mesh,m + hhh*dm;computeJacobian=0,sigma=sigmaH,bf = bf)[1];
	println("norm(ut-u0): ",norm(ut[:]-u0[:]),", norm(ut - u0 - J0*dm): ",norm(ut[:] - u0[:] - J0*hhh*dm));
end


text:
using Distributions

struct NoisyRB{L,D,W} <: RigidBody{D}
    model::L
    noise::W
end

@inline Base.size(model::NoisyRB) = size(model.model)

function NoisyRB(model::L, noise::W) where {L<:RigidBody{D}, W<:Distribution{Multivariate,Continuous}} where D
    NoisyRB{L,D,W}(model, noise)
end

function dynamics(model::NoisyRB, x::SVector, u::SVector)
    r,q,v,ω = parse_state(model.model, x)

    # Get noise
    w = rand(model.noise)
    F_ = @SVector [w[1], w[2], w[3]]
    τ_ = @SVector [w[4], w[5], w[6]]

    # Original dynamics
    F,τ = wrenches(model.model, x, u)
    M = mass_matrix(model.model, x, u)
    J = inertia(model.model, x, u)
    Jinv = inertia_inv(model.model, x, u)

    # Add noise wrench
    F += F_
    τ += τ_

    xdot = v
    qdot = kinematics(q,ω)
    vdot = M\F
    ωdot = Jinv*(τ - ω × (J*ω))

    build_state(model, xdot, qdot, vdot, ωdot)
end

@inline state_diff_size(model::NoisyRB) = state_diff_size(model.model)

@inline state_diff(model::NoisyRB, x::SVector, x0::SVector) =
    state_diff(model.model, x, x0)

@inline state_diff_jacobian(model::NoisyRB, x::SVector) =
    state_diff_jacobian(model.model, x)

@inline TrajectoryOptimization.∇²differential(model::NoisyRB, x::SVector, dx::SVector) =
    TrajectoryOptimization.∇²differential(model.model, x, dx)

@inline TrajectoryOptimization.inverse_map_jacobian(model::NoisyRB, x::SVector) =
    TrajectoryOptimization.inverse_map_jacobian(model.model, x)

@inline TrajectoryOptimization.inverse_map_∇jacobian(model::NoisyRB, x::SVector, b::SVector) =
    TrajectoryOptimization.inverse_map_∇jacobian(model.model, x, b)


text:
import Markdown: latex, latexinline

# Remove comments that can occur inside a line
function latexinline(io, comment::WeaveMarkdown.Comment)
    write(io, "")
end

function latex(io::IO, comment::WeaveMarkdown.Comment)
    for line in split(comment.text, r"\r\n|\n")
        write(io, "% $line\n")
    end
end


text:
mutable struct EqualityConstraint{T,N,Nc,Cs} <: AbstractConstraint{T,N}
    id::Int64
    name::String
    active::Bool

    constraints::Cs
    parentid::Union{Int64,Nothing}
    childids::SVector{Nc,Int64}
    inds::SVector{Nc,SVector{2,Int64}} # indices for minimal coordinates, assumes joints

    λsol::Vector{SVector{N,T}}

    function EqualityConstraint(data...; name::String="")
        jointdata = Tuple{AbstractJoint,Int64,Int64}[]
        for info in data
            if info[1] isa AbstractJoint
                push!(jointdata, info)
            else
                for subinfo in info
                    push!(jointdata, subinfo)
                end
            end
        end

        T = getT(jointdata[1][1])# .T

        parentid = jointdata[1][2]
        childids = Int64[]
        constraints = AbstractJoint{T}[]
        inds = Vector{Int64}[]
        N = 0
        for set in jointdata
            push!(constraints, set[1])
            @assert set[2] == parentid
            push!(childids, set[3])

            Nset = length(set[1])
            if isempty(inds)
                push!(inds, [1;3-Nset])
            else
                push!(inds, [last(inds)[2]+1;last(inds)[2]+3-Nset])
            end
            N += Nset
        end
        constraints = Tuple(constraints)
        Nc = length(constraints)

        λsol = [zeros(T, N) for i=1:2]

        new{T,N,Nc,typeof(constraints)}(getGlobalID(), name, true, constraints, parentid, childids, inds, λsol)
    end
end

function setPosition!(mechanism, eqc::EqualityConstraint, xθ; iter::Bool = true)
    if !iter
        _setPosition!(mechanism, eqc, xθ)
    else
        currentvals = minimalCoordinates(mechanism)
        _setPosition!(mechanism, eqc, xθ)
        for id in recursivedirectchildren!(mechanism.graph, eqc.id)
            component = getcomponent(mechanism, id)
            if component isa EqualityConstraint
                _setPosition!(mechanism, component, currentvals[id])
            end
        end
    end

    return
end

# TODO make zero alloc
# TODO currently assumed constraints are in order and only joints which is the case unless very low level constraint setting
function _setPosition!(mechanism, eqc::EqualityConstraint{T,N,Nc}, xθ) where {T,N,Nc}
    @assert length(xθ)==3*Nc-N
    n = Int64(Nc/2)
    body1 = getbody(mechanism, eqc.parentid)
    for i = 1:n
        body2 = getbody(mechanism, eqc.childids[i])
        Δx = getPositionDelta(eqc.constraints[i], body1, body2, xθ[SUnitRange(eqc.inds[i][1],eqc.inds[i][2])])
        Δq = getPositionDelta(eqc.constraints[i+1], body1, body2, xθ[SUnitRange(eqc.inds[i+1][1],eqc.inds[i+1][2])])

        p1, p2 = eqc.constraints[i].vertices
        setPosition!(body1, body2; p1 = p1, p2 = p2, Δx = Δx, Δq = Δq)
    end
    return
end

# TODO make zero alloc
# TODO currently assumed constraints are in order and only joints which is the case unless very low level constraint setting
function setVelocity!(mechanism, eqc::EqualityConstraint{T,N,Nc}, vω) where {T,N,Nc}
    @assert length(vω)==3*Nc-N
    n = Int64(Nc/2)
    body1 = getbody(mechanism, eqc.parentid)
    for i = 1:n
        body2 = getbody(mechanism, eqc.childids[i])
        Δv = getVelocityDelta(eqc.constraints[i], body1, body2, vω[SUnitRange(eqc.inds[i][1],eqc.inds[i][2])])
        Δω = getVelocityDelta(eqc.constraints[i+1], body1, body2, vω[SUnitRange(eqc.inds[i+1][1],eqc.inds[i+1][2])])

        p1, p2 = eqc.constraints[i].vertices
        setVelocity!(body1, body2; p1 = p1, p2 = p2, Δv = Δv, Δω = Δω)
    end
    return
end

# TODO make zero alloc
function setForce!(mechanism, eqc::EqualityConstraint{T,N,Nc}, Fτ::AbstractVector) where {T,N,Nc}
    @assert length(Fτ)==3*Nc-N
    for i = 1:Nc
        setForce!(eqc.constraints[i], Fτ[SUnitRange(eqc.inds[i][1],eqc.inds[i][2])])
    end
    return
end

@generated function minimalCoordinates(mechanism, eqc::EqualityConstraint{T,N,Nc}) where {T,N,Nc}
    vec = [:(minimalCoordinates(eqc.constraints[$i], getbody(mechanism, eqc.parentid), getbody(mechanism, eqc.childids[$i]))) for i = 1:Nc]
    return :(svcat($(vec...)))
end

@generated function minimalVelocities(mechanism, eqc::EqualityConstraint{T,N,Nc}) where {T,N,Nc}
    vec = [:(minimalVelocities(eqc.constraints[$i], getbody(mechanism, eqc.parentid), getbody(mechanism, eqc.childids[$i]))) for i = 1:Nc]
    return :(svcat($(vec...)))
end

@inline function GtλTof!(mechanism, body::Body, eqc::EqualityConstraint)
    isactive(eqc) && (body.state.d -= zerodimstaticadjoint(∂g∂ʳpos(mechanism, eqc, body.id)) * eqc.λsol[2])
    return
end

@generated function g(mechanism, eqc::EqualityConstraint{T,N,Nc}) where {T,N,Nc}
    vec = [:(g(eqc.constraints[$i], getbody(mechanism, eqc.parentid), getbody(mechanism, eqc.childids[$i]), mechanism.Δt)) for i = 1:Nc]
    return :(svcat($(vec...)))
end
@generated function gc(mechanism, eqc::EqualityConstraint{T,N,Nc}) where {T,N,Nc}
    vec = [:(g(eqc.constraints[$i], getbody(mechanism, eqc.parentid), getbody(mechanism, eqc.childids[$i]))) for i = 1:Nc]
    return :(svcat($(vec...)))
end

@inline function ∂g∂ʳpos(mechanism, eqc::EqualityConstraint, id::Integer)
    id == eqc.parentid ? (return ∂g∂ʳposa(mechanism, eqc, id)) : (return ∂g∂ʳposb(mechanism, eqc, id))
end

@inline function ∂g∂ʳvel(mechanism, eqc::EqualityConstraint, id::Integer)
    id == eqc.parentid ? (return ∂g∂ʳvela(mechanism, eqc, id)) : (return ∂g∂ʳvelb(mechanism, eqc, id))
end

@generated function ∂g∂ʳposa(mechanism, eqc::EqualityConstraint{T,N,Nc}, id::Integer) where {T,N,Nc}
    vec = [:(∂g∂ʳposa(eqc.constraints[$i], getbody(mechanism, id), getbody(mechanism, eqc.childids[$i]))) for i = 1:Nc]
    return :(vcat($(vec...)))
end
@generated function ∂g∂ʳposb(mechanism, eqc::EqualityConstraint{T,N,Nc}, id::Integer) where {T,N,Nc}
    vec = [:(∂g∂ʳposb(eqc.constraints[$i], getbody(mechanism, eqc.parentid), getbody(mechanism, id))) for i = 1:Nc]
    return :(vcat($(vec...)))
end

@generated function ∂g∂ʳvela(mechanism, eqc::EqualityConstraint{T,N,Nc}, id::Integer) where {T,N,Nc}
    vec = [:(∂g∂ʳvela(eqc.constraints[$i], getbody(mechanism, id), getbody(mechanism, eqc.childids[$i]), mechanism.Δt)) for i = 1:Nc]
    return :(vcat($(vec...)))
end
@generated function ∂g∂ʳvelb(mechanism, eqc::EqualityConstraint{T,N,Nc}, id::Integer) where {T,N,Nc}
    vec = [:(∂g∂ʳvelb(eqc.constraints[$i], getbody(mechanism, eqc.parentid), getbody(mechanism, id), mechanism.Δt)) for i = 1:Nc]
    return :(vcat($(vec...)))
end

@generated function ∂Fτ∂ua(mechanism, eqc::EqualityConstraint{T,N,Nc}, id) where {T,N,Nc}
    vec = [:(∂Fτ∂ua(eqc.constraints[$i], getbody(mechanism, id), getbody(mechanism, eqc.childids[$i]))) for i = 1:Nc]
    return :(hcat($(vec...)))
end
@generated function ∂Fτ∂ub(mechanism, eqc::EqualityConstraint{T,N,Nc}, id) where {T,N,Nc}
    vec = [:(∂Fτ∂ub(eqc.constraints[$i], getbody(mechanism, eqc.parentid), getbody(mechanism, id))) for i = 1:Nc]
    return :(hcat($(vec...)))
end

@inline function applyFτ!(eqc::EqualityConstraint{T,N,Nc}, mechanism, clear::Bool=true) where {T,N,Nc}
    for i=1:Nc
        applyFτ!(eqc.constraints[i], getbody(mechanism, eqc.parentid), getbody(mechanism, eqc.childids[i]), clear)
    end
    return
end

# Derivatives NOT accounting for quaternion specialness

function ∂g∂posc(mechanism, eqc::EqualityConstraint, id::Integer)
    id == eqc.parentid ? (return ∂g∂posac(mechanism, eqc, id)) : (return ∂g∂posbc(mechanism, eqc, id))
end

function ∂g∂posac(mechanism, eqc::EqualityConstraint{T,N,Nc}, id::Integer) where {T,N,Nc}
    vec = [hcat(∂g∂posac(eqc.constraints[i], getbody(mechanism, id), getbody(mechanism, eqc.childids[i]))) for i = 1:Nc]
    return vcat(vec...)
end
function ∂g∂posbc(mechanism, eqc::EqualityConstraint{T,N,Nc}, id::Integer) where {T,N,Nc}
    vec = [hcat(∂g∂posbc(eqc.constraints[i], getbody(mechanism, eqc.parentid), getbody(mechanism, id))) for i = 1:Nc]
    return vcat(vec...)
end


text:
include("datastructs.jl")
include("transforms/transforms.jl")

mutable struct GP
    trainingData::AbstractTrainingData #x, Fx, y, p (dimension of each covariate vector), dimension (dimension of each location vector)
    testingData::AbstractTestingData
    n::Int64 #number of points in kernel system, if 0 then uninitialized
    p::Int64
    g:: AbstractTransform #transform family, e.g. BoxCox(), Identity()
    k::AbstractCorrelation
    function new(trainingData::AbstractTrainingData, testingData::AbstractTestingData, g:: AbstractTransform, k::AbstractCorrelation)

    end
end

function predict_y(GP, x_test, y_test)

end

#gp = GP(train, test, IdentityTransform(), Gaussian())

max_stars_repo_name: dbindel/btg

max_issues_repo_name: dbindel/btg

max_forks_repo_name: dbindel/btg


text:
using LogCompose, Test, Logging, LoggingExtras, LoggingExtrasCompose

function test()
    config = joinpath(@__DIR__, "testapp.toml")
    file1 = "testapp1.log"
    file2 = "testapp2.log"
    rm(file1; force=true)
    rm(file2; force=true)

    let logger = LogCompose.logger(config, "file1"; section="loggers")
        with_logger(logger) do
            @info("testfile1")
        end
    end

    let logger = LogCompose.logger(config, "file2"; section="loggers")
        with_logger(logger) do
            @info("testfile2")
        end
    end

    let logger = LogCompose.logger(config, "tee"; section="loggers")
        with_logger(logger) do
            @info("testtee")
        end
    end

    @test isfile(file1)
    @test isfile(file2)

    log_file_contents = readlines(file1)
    @test findfirst("testfile1", log_file_contents[1]) !== nothing
    @test findfirst("testtee", log_file_contents[3]) !== nothing

    log_file_contents = readlines(file2)
    @test findfirst("testfile2", log_file_contents[1]) !== nothing
    @test findfirst("testtee", log_file_contents[3]) !== nothing

    try
        rm(file1; force=true)
        rm(file2; force=true)
    catch ex
        # ignore errors due to file handles being open on Windows
    end
end

test()


text:
module TkIntcode

export intcode, start

using OffsetArrays

mutable struct Intcode
    code::AbstractArray{Int}
    ip::Int
    input
    output
end

# Handling jump instructions
abstract type Jump end
struct JumpNext <: Jump end
struct JumpFar <: Jump
    location
end

# Default code parser
function read_code(s::AbstractString)
    code = parse.(Int, split(s, ","))
    return OffsetArray(code, 0:length(code)-1)
end

# Intcode constructor
intcode(s::AbstractString, input, output) = Intcode(read_code(s), 0, input, output)

# I/O support
read_input(c::Channel) = take!(c)
write_output(c::Channel, data) = put!(c, data)

read_input(io::IO) = parse(Int,readline(io))
write_output(io::IO, data) = println(io, data)

read_input(f::Function) = f()
write_output(f::Function, data) = f(data)

# Start program
function start(ic::Intcode)
    while true
        i = ic.ip
        instruction = ic.code[i]
        op = instruction % 100      # rightmost two digits
        opmode = instruction ÷ 100  # shift right by two digits
        # println("ic $(ic.id): executing op=$op opmode=$opmTode i=$i")
        if op in (1,2,7,8)
            execute!(Val(op), Val(opmode), ic.code, ic.code[i+1], ic.code[i+2], ic.code[i+3])
            ic.ip += 4
        elseif op == 3
            execute!(Val(op), Val(opmode), ic, ic.input, ic.code, ic.code[i+1])
            ic.ip += 2
        elseif op == 4
            execute!(Val(op), Val(opmode), ic, ic.output, ic.code, ic.code[i+1])
            ic.ip += 2
        elseif op in (5,6)
            j = execute!(Val(op), Val(opmode), ic.code, ic.code[i+1], ic.code[i+2])
            if j isa JumpFar
                ic.ip = j.location
            else
                ic.ip += 3
            end
        elseif op == 99
            break
        else
            println("Unknown op code $op")
        end
    end
end

execute!(::Val{1}, ::Val{000}, code, p1, p2, p3) = code[p3] = code[p1] + code[p2]
execute!(::Val{1}, ::Val{001}, code, p1, p2, p3) = code[p3] = p1       + code[p2]
execute!(::Val{1}, ::Val{010}, code, p1, p2, p3) = code[p3] = code[p1] + p2
execute!(::Val{1}, ::Val{011}, code, p1, p2, p3) = code[p3] = p1       + p2

execute!(::Val{2}, ::Val{000}, code, p1, p2, p3) = code[p3] = code[p1] * code[p2]
execute!(::Val{2}, ::Val{001}, code, p1, p2, p3) = code[p3] = p1       * code[p2]
execute!(::Val{2}, ::Val{010}, code, p1, p2, p3) = code[p3] = code[p1] * p2
execute!(::Val{2}, ::Val{011}, code, p1, p2, p3) = code[p3] = p1       * p2

function execute!(::Val{3}, ::Val{000}, ic, channel, code, p1)
    code[p1] = read_input(channel)
end

function execute!(::Val{4}, ::Val{000}, ic, channel, code, p1)
    write_output(channel, code[p1])
end

function execute!(::Val{4}, ::Val{001}, ic, channel, code, p1)
    write_output(channel, p1)
end

execute!(::Val{5}, ::Val{000}, code, p1, p2) = code[p1] != 0 ? JumpFar(code[p2]) : JumpNext()
execute!(::Val{5}, ::Val{001}, code, p1, p2) = p1 != 0       ? JumpFar(code[p2]) : JumpNext()
execute!(::Val{5}, ::Val{010}, code, p1, p2) = code[p1] != 0 ? JumpFar(p2)       : JumpNext()
execute!(::Val{5}, ::Val{011}, code, p1, p2) = p1 != 0 ?       JumpFar(p2)       : JumpNext()

execute!(::Val{6}, ::Val{000}, code, p1, p2) = code[p1] == 0 ? JumpFar(code[p2]) : JumpNext()
execute!(::Val{6}, ::Val{001}, code, p1, p2) = p1 == 0       ? JumpFar(code[p2]) : JumpNext()
execute!(::Val{6}, ::Val{010}, code, p1, p2) = code[p1] == 0 ? JumpFar(p2)       : JumpNext()
execute!(::Val{6}, ::Val{011}, code, p1, p2) = p1 == 0       ? JumpFar(p2)       : JumpNext()

execute!(::Val{7}, ::Val{000}, code, p1, p2, p3) = code[p3] = code[p1] < code[p2] ? 1 : 0
execute!(::Val{7}, ::Val{001}, code, p1, p2, p3) = code[p3] = p1       < code[p2] ? 1 : 0
execute!(::Val{7}, ::Val{010}, code, p1, p2, p3) = code[p3] = code[p1] < p2       ? 1 : 0
execute!(::Val{7}, ::Val{011}, code, p1, p2, p3) = code[p3] = p1       < p2       ? 1 : 0

execute!(::Val{8}, ::Val{000}, code, p1, p2, p3) = code[p3] = code[p1] == code[p2] ? 1 : 0
execute!(::Val{8}, ::Val{001}, code, p1, p2, p3) = code[p3] = p1       == code[p2] ? 1 : 0
execute!(::Val{8}, ::Val{010}, code, p1, p2, p3) = code[p3] = code[p1] == p2       ? 1 : 0
execute!(::Val{8}, ::Val{011}, code, p1, p2, p3) = code[p3] = p1       == p2       ? 1 : 0

end # module


text:
module BenchFindfirst

import Random
using BenchmarkTools
using ThreadsX

Random.seed!(1234)

n = 2^20

suite = BenchmarkGroup()

xs0 = rand(n)
for percent in [0, 10, 20, 30, 40, 50]
    xs = copy(xs0)
    xs[max(1, floor(Int, length(xs) * percent / 100))] = -1
    s = suite["$(percent)%"] = BenchmarkGroup()
    s["tx"] = @benchmarkable(ThreadsX.findfirst(==(-1), $xs))
    s["tx-noterm"] = @benchmarkable(ThreadsX.findfirst(==(-1), $xs, stoppable = false))
    s["tx-seq"] = @benchmarkable(ThreadsX.findfirst(==(-1), $xs, basesize = typemax(Int)))
    s["base"] = @benchmarkable(findfirst(==(-1), $xs))
end

end  # module
BenchFindfirst.suite