text:
"""
    mmg_3dof_model!(dX, X, p, t)

MMG 3DOF model on DifferentialEquations.ODEProblem. Update `dX`.

# Arguments
- `dX`: [du, dv, dr, dδ, dn_p]
- `X`: the initial state values. [`u`, `v`, `r`, `δ`, `n_p`].
- `p`: ρ and the basic & maneuvering parameters and δ & n_p spline info.
    - ρ
    - L_pp
    - B
    - d
    - x_G
    - D_p
    - m
    - I_zG
    - A_R
    - η
    - m_x
    - m_y
    - J_z
    - f_α
    - ϵ
    - t_R
    - x_R
    - a_H
    - x_H
    - γ_R_minus
    - γ_R_plus
    - l_R
    - κ
    - t_P
    - w_P0
    - x_P
    - k_0
    - k_1
    - k_2
    - R_0_dash
    - X_vv_dash
    - X_vr_dash
    - X_rr_dash
    - X_vvvv_dash
    - Y_v_dash
    - Y_r_dash
    - Y_vvv_dash
    - Y_vvr_dash
    - Y_vrr_dash
    - Y_rrr_dash
    - N_v_dash
    - N_r_dash
    - N_vvv_dash
    - N_vvr_dash
    - N_vrr_dash
    - N_rrr_dash
    - spl_δ
    - spl_n_p
- `t`: the time.
"""
function mmg_3dof_model!(dX, X, p, t)
    u, v, r, δ, n_p = X
    ρ,
    L_pp,
    B,
    d,
    x_G,
    D_p,
    m,
    I_zG,
    A_R,
    η,
    m_x,
    m_y,
    J_z,
    f_α,
    ϵ,
    t_R,
    x_R,
    a_H,
    x_H,
    γ_R_minus,
    γ_R_plus,
    l_R,
    κ,
    t_P,
    w_P0,
    x_P,
    k_0,
    k_1,
    k_2,
    R_0_dash,
    X_vv_dash,
    X_vr_dash,
    X_rr_dash,
    X_vvvv_dash,
    Y_v_dash,
    Y_r_dash,
    Y_vvv_dash,
    Y_vvr_dash,
    Y_vrr_dash,
    Y_rrr_dash,
    N_v_dash,
    N_r_dash,
    N_vvv_dash,
    N_vvr_dash,
    N_vrr_dash,
    N_rrr_dash,
    spl_δ,
    spl_n_p = p

    U = sqrt(u^2 + (v - r * x_G)^2)

    β = 0.0
    if U == 0.0
        β = 0.0
    else
        β = asin(-(v - r * x_G) / U)
    end

    v_dash = 0.0
    if U == 0.0
        v_dash = 0.0
    else
        v_dash = v / U
    end

    r_dash = 0.0
    if U == 0.0
        r_dash = 0.0
    else
        r_dash = r * L_pp / U
    end

    w_P = w_P0 * exp(-4.0 * (β - x_P * r_dash)^2)

    J = 0.0
    if n_p == 0.0
        J = 0.0
    else
        J = (1 - w_P) * u / (n_p * D_p)
    end
    K_T = k_0 + k_1 * J + k_2 * J^2
    β_R = β - l_R * r_dash
    γ_R = γ_R_minus

    if β_R < 0.0
        γ_R = γ_R_minus
    else
        γ_R = γ_R_plus
    end

    v_R = U * γ_R * β_R

    u_R = 0.0
    if J == 0.0
        u_R = sqrt(η * (κ * ϵ * 8.0 * k_0 * n_p^2 * D_p^4 / pi)^2)
    else
        u_R =
            u *
            (1.0 - w_P) *
            ϵ *
            sqrt(η * (1.0 + κ * (sqrt(1.0 + 8.0 * K_T / (pi * J^2)) - 1))^2 + (1 - η))
    end

    U_R = sqrt(u_R^2 + v_R^2)
    α_R = δ - atan(v_R, u_R)
    F_N = 0.5 * A_R * ρ * f_α * (U_R^2) * sin(α_R)

    X_H = (
        0.5 *
        ρ *
        L_pp *
        d *
        (U^2) *
        (
            -R_0_dash +
            X_vv_dash * (v_dash^2) +
            X_vr_dash * v_dash * r_dash +
            X_rr_dash * (r_dash^2) +
            X_vvvv_dash * (v_dash^4)
        )
    )
    X_R = -(1.0 - t_R) * F_N * sin(δ)
    X_P = (1.0 - t_P) * ρ * K_T * n_p^2 * D_p^4
    Y_H = (
        0.5 *
        ρ *
        L_pp *
        d *
        (U^2) *
        (
            Y_v_dash * v_dash +
            Y_r_dash * r_dash +
            Y_vvv_dash * (v_dash^3) +
            Y_vvr_dash * (v_dash^2) * r_dash +
            Y_vrr_dash * v_dash * (r_dash^2) +
            Y_rrr_dash * (r_dash^3)
        )
    )
    Y_R = -(1 + a_H) * F_N * cos(δ)
    N_H = (
        0.5 *
        ρ *
        (L_pp^2) *
        d *
        (U^2) *
        (
            N_v_dash * v_dash +
            N_r_dash * r_dash +
            N_vvv_dash * (v_dash^3) +
            N_vvr_dash * (v_dash^2) * r_dash +
            N_vrr_dash * v_dash * (r_dash^2) +
            N_rrr_dash * (r_dash^3)
        )
    )
    N_R = -(x_R + a_H * x_H) * F_N * cos(δ)
    dX[1] = du = ((X_H + X_R + X_P) + (m + m_y) * v * r + x_G * m * (r^2)) / (m + m_x)
    dX[2] =
        dv =
            (
                (x_G^2) * (m^2) * u * r - (N_H + N_R) * x_G * m +
                ((Y_H + Y_R) - (m + m_x) * u * r) * (I_zG + J_z + (x_G^2) * m)
            ) / ((I_zG + J_z + (x_G^2) * m) * (m + m_y) - (x_G^2) * (m^2))
    dX[3] = dr = (N_H + N_R - x_G * m * (dv + u * r)) / (I_zG + J_z + (x_G^2) * m)
    dX[4] = dδ = derivative(spl_δ, t)
    dX[5] = dn_p = derivative(spl_n_p, t)
end

"""
Basic parameters of target ship for MMG 3DOF simulation.

# Arguments
- `L_pp`: L_pp
- `B`:
- `d`:
- `x_G`:
- `D_p`:
- `m`:
- `I_zG`:
- `A_R`:
- `η`:
- `m_x`:
- `m_y`:
- `J_z`:
- `f_α`:
- `ϵ`:
- `t_R`:
- `x_R`:
- `a_H`:
- `x_H`:
- `γ_R_minus`:
- `γ_R_plus`:
- `l_R`:
- `κ`:
- `t_P`:
- `w_P0`:
- `x_P`:
"""
@with_kw mutable struct Mmg3DofBasicParams{T}
    L_pp::T
    B::T
    d::T
    x_G::T
    D_p::T
    m::T
    I_zG::T
    A_R::T
    η::T
    m_x::T
    m_y::T
    J_z::T
    f_α::T
    ϵ::T
    t_R::T
    x_R::T
    a_H::T
    x_H::T
    γ_R_minus::T
    γ_R_plus::T
    l_R::T
    κ::T
    t_P::T
    w_P0::T
    x_P::T
end

"""
Maneuvering parameters of target ship for MMG 3DOF simulation.

# Arguments
- `k_0::T`
- `k_1::T`
- `k_2::T`
- `R_0_dash::T`
- `X_vv_dash::T`
- `X_vr_dash::T`
- `X_rr_dash::T`
- `X_vvvv_dash::T`
- `Y_v_dash::T`
- `Y_r_dash::T`
- `Y_vvv_dash::T`
- `Y_vvr_dash::T`
- `Y_vrr_dash::T`
- `Y_rrr_dash::T`
- `N_v_dash::T`
- `N_r_dash::T`
- `N_vvv_dash::T`
- `N_vvr_dash::T`
- `N_vrr_dash::T`
- `N_rrr_dash::T`
"""
@with_kw mutable struct Mmg3DofManeuveringParams{T}
    k_0::T
    k_1::T
    k_2::T
    R_0_dash::T
    X_vv_dash::T
    X_vr_dash::T
    X_rr_dash::T
    X_vvvv_dash::T
    Y_v_dash::T
    Y_r_dash::T
    Y_vvv_dash::T
    Y_vvr_dash::T
    Y_vrr_dash::T
    Y_rrr_dash::T
    N_v_dash::T
    N_r_dash::T
    N_vvv_dash::T
    N_vvr_dash::T
    N_vrr_dash::T
    N_rrr_dash::T
end

"""
    mmg_3dof_simulate(time_list, n_p_list, δ_list, basic_params, maneuvering_params, [, u0, v0, r0, ρ, algorithm, reltol, abstol]) -> u, v, r, δ, n_p

Returns the MMG 3DOF simulation results including the lists of time, u, v, r, δ, n_p.
This function has the same logic of `ShipMMG.simulate()`.

# Arguments
- `basic_params::Mmg3DofBasicParams`: the basic parameters of target ship.
- `maneuvering_params::Mmg3DofManeuveringParams`: the maneuvering parameters of target ship.
- `time_list`: the list of simulatino time.
- `δ_list`: the list of rudder angle [rad].
- `n_p_list`: the list of propeller rps.
- `u0=0.0`: the initial x (surge) velocity.
- `v0=0.0`: the initial y (sway) velocity.
- `r0=0.0`: the initial rate of turn [rad/s].
- `ρ=1025.0`: the seawater density [kg/m^3].
- `algorithm=Tsit5()`: the parameter of DifferentialEquations.ODEProblem.solve()
- `reltol=1e-8`: the parameter of DifferentialEquations.ODEProblem.solve()
- `abstol=1e-8`: the parameter of DifferentialEquations.ODEProblem.solve()

# Examples
KVLCC2_L7 turning test.

```julia-rep1
julia> basic_params, maneuvering_params = get_KVLCC2_L7_params();
julia> duration = 200; # [s]
julia> max_δ_rad = 35 * pi / 180.0;  # [rad]
julia> n_const = 17.95;  # [rps]
julia> sampling = duration * 10;
julia> time_list = range(0.00, stop = duration, length = sampling);
julia> δ_rad_list = max_δ_rad .* ones(Float64, sampling);
julia> n_p_list = n_const .* ones(Float64, sampling);
julia> mmg_results = mmg_3dof_simulate(
    basic_params,
    maneuvering_params,
    time_list,
    δ_rad_list,
    n_p_list,
    u0 = 2.29 * 0.512,
    v0 = 0.0,
    r0 = 0.0,
);
```
"""
function mmg_3dof_simulate(
    basic_params::Mmg3DofBasicParams,
    maneuvering_params::Mmg3DofManeuveringParams,
    time_list,
    δ_list,
    n_p_list;
    u0 = 0.0,
    v0 = 0.0,
    r0 = 0.0,
    ρ = 1025.0,
    algorithm = Tsit5(),
    reltol = 1e-8,
    abstol = 1e-8,
)
    @unpack L_pp,
    B,
    d,
    x_G,
    D_p,
    m,
    I_zG,
    A_R,
    η,
    m_x,
    m_y,
    J_z,
    f_α,
    ϵ,
    t_R,
    x_R,
    a_H,
    x_H,
    γ_R_minus,
    γ_R_plus,
    l_R,
    κ,
    t_P,
    w_P0,
    x_P = basic_params

    @unpack k_0,
    k_1,
    k_2,
    R_0_dash,
    X_vv_dash,
    X_vr_dash,
    X_rr_dash,
    X_vvvv_dash,
    Y_v_dash,
    Y_r_dash,
    Y_vvv_dash,
    Y_vvr_dash,
    Y_vrr_dash,
    Y_rrr_dash,
    N_v_dash,
    N_r_dash,
    N_vvv_dash,
    N_vvr_dash,
    N_vrr_dash,
    N_rrr_dash = maneuvering_params
    simulate(
        L_pp,
        B,
        d,
        x_G,
        D_p,
        m,
        I_zG,
        A_R,
        η,
        m_x,
        m_y,
        J_z,
        f_α,
        ϵ,
        t_R,
        x_R,
        a_H,
        x_H,
        γ_R_minus,
        γ_R_plus,
        l_R,
        κ,
        t_P,
        w_P0,
        x_P,
        k_0,
        k_1,
        k_2,
        R_0_dash,
        X_vv_dash,
        X_vr_dash,
        X_rr_dash,
        X_vvvv_dash,
        Y_v_dash,
        Y_r_dash,
        Y_vvv_dash,
        Y_vvr_dash,
        Y_vrr_dash,
        Y_rrr_dash,
        N_v_dash,
        N_r_dash,
        N_vvv_dash,
        N_vvr_dash,
        N_vrr_dash,
        N_rrr_dash,
        time_list,
        δ_list,
        n_p_list,
        u0 = u0,
        v0 = v0,
        r0 = r0,
        ρ = ρ,
        algorithm = algorithm,
        reltol = reltol,
        abstol = abstol,
    )
end

"""
    mmg_3dof_simulate(time_list, n_p_list, δ_list, L_pp, B, d, x_G, D_p, m, I_zG, A_R, η, m_x, m_y, J_z, f_α, ϵ, t_R, x_R, a_H, x_H, γ_R_minus, γ_R_plus, l_R, κ, t_P, w_P0, x_P, k_0, k_1, k_2, R_0_dash, X_vv_dash, X_vr_dash, X_rr_dash, X_vvvv_dash, Y_v_dash, Y_r_dash, Y_vvv_dash, Y_vvr_dash, Y_vrr_dash, Y_rrr_dash, N_v_dash, N_r_dash, N_vvv_dash, N_vvr_dash, N_vrr_dash, N_rrr_dash, [, u0, v0, r0, ρ, algorithm, reltol, abstol]) -> u, v, r, δ, n_p

Returns the MMG 3DOF simulation results including the lists of time, u, v, r, δ, n_p.
This function has the same logic of `ShipMMG.mmg_3dof_simulate()`.

# Arguments
- `L_pp`: L_pp
- `B`:
- `d`:
- `x_G`:
- `D_p`:
- `m`:
- `I_zG`:
- `A_R`:
- `η`:
- `m_x`:
- `m_y`:
- `J_z`:
- `f_α`:
- `ϵ`:
- `t_R`:
- `x_R`:
- `a_H`:
- `x_H`:
- `γ_R_minus`:
- `γ_R_plus`:
- `l_R`:
- `κ`:
- `t_P`:
- `w_P0`:
- `x_P`:
- `k_0`
- `k_1`
- `k_2`
- `R_0_dash`
- `X_vv_dash`
- `X_vr_dash`
- `X_rr_dash`
- `X_vvvv_dash`
- `Y_v_dash`
- `Y_r_dash`
- `Y_vvv_dash`
- `Y_vvr_dash`
- `Y_vrr_dash`
- `Y_rrr_dash`
- `N_v_dash`
- `N_r_dash`
- `N_vvv_dash`
- `N_vvr_dash`
- `N_vrr_dash`
- `N_rrr_dash`
- `time_list`: the list of simulatino time.
- `δ_list`: the list of rudder angle [rad].
- `n_p_list`: the list of propeller rps.
- `u0=0.0`: the initial x (surge) velocity.
- `v0=0.0`: the initial y (sway) velocity.
- `r0=0.0`: the initial rate of turn [rad/s].
- `ρ=1025.0`: the seawater density [kg/m^3].
- `algorithm=Tsit5()`: the parameter of DifferentialEquations.ODEProblem.solve()
- `reltol=1e-8`: the parameter of DifferentialEquations.ODEProblem.solve()
- `abstol=1e-8`: the parameter of DifferentialEquations.ODEProblem.solve()
"""
function simulate(
    L_pp,
    B,
    d,
    x_G,
    D_p,
    m,
    I_zG,
    A_R,
    η,
    m_x,
    m_y,
    J_z,
    f_α,
    ϵ,
    t_R,
    x_R,
    a_H,
    x_H,
    γ_R_minus,
    γ_R_plus,
    l_R,
    κ,
    t_P,
    w_P0,
    x_P,
    k_0,
    k_1,
    k_2,
    R_0_dash,
    X_vv_dash,
    X_vr_dash,
    X_rr_dash,
    X_vvvv_dash,
    Y_v_dash,
    Y_r_dash,
    Y_vvv_dash,
    Y_vvr_dash,
    Y_vrr_dash,
    Y_rrr_dash,
    N_v_dash,
    N_r_dash,
    N_vvv_dash,
    N_vvr_dash,
    N_vrr_dash,
    N_rrr_dash,
    time_list,
    δ_list,
    n_p_list;
    u0 = 0.0,
    v0 = 0.0,
    r0 = 0.0,
    ρ = 1025.0,
    algorithm = Tsit5(),
    reltol = 1e-8,
    abstol = 1e-8,
)
    spl_δ = Spline1D(time_list, δ_list)
    spl_n_p = Spline1D(time_list, n_p_list)

    X0 = [u0; v0; r0; δ_list[1]; n_p_list[1]]
    p = [
        ρ,
        L_pp,
        B,
        d,
        x_G,
        D_p,
        m,
        I_zG,
        A_R,
        η,
        m_x,
        m_y,
        J_z,
        f_α,
        ϵ,
        t_R,
        x_R,
        a_H,
        x_H,
        γ_R_minus,
        γ_R_plus,
        l_R,
        κ,
        t_P,
        w_P0,
        x_P,
        k_0,
        k_1,
        k_2,
        R_0_dash,
        X_vv_dash,
        X_vr_dash,
        X_rr_dash,
        X_vvvv_dash,
        Y_v_dash,
        Y_r_dash,
        Y_vvv_dash,
        Y_vvr_dash,
        Y_vrr_dash,
        Y_rrr_dash,
        N_v_dash,
        N_r_dash,
        N_vvv_dash,
        N_vvr_dash,
        N_vrr_dash,
        N_rrr_dash,
        spl_δ,
        spl_n_p,
    ]
    prob = ODEProblem(mmg_3dof_model!, X0, (time_list[1], time_list[end]), p)
    sol = solve(prob, algorithm, reltol = reltol, abstol = abstol)
    sol_timelist = sol(time_list)
    results = hcat(sol_timelist.u...)
    u = results[1, :]
    v = results[2, :]
    r = results[3, :]
    δ = results[4, :]
    n_p = results[5, :]
    u, v, r, δ, n_p
end

"""
    mmg_3dof_zigzag_test(basic_params, maneuvering_params, time_list, n_p_list, target_δ_rad, target_ψ_rad_deviation, [, u0, v0, r0, ψ0, δ0, δ_rad_rate, algorithm, reltol, abstol]) -> final_u_list, final_v_list, final_r_list, final_ψ_list, final_δ_list

Returns the MMG 3DOF zigzag simulation results.

# Arguments
- `basic_params::Mmg3DofBasicParams`: the basic parameters of target ship.
- `maneuvering_params::Mmg3DofManeuveringParams`: the maneuvering parameters of target ship.
- `time_list`: the list of simulatino time.
- `n_p_list`: the list of propeller rps.
- `target_δ_rad`: target rudder angle of zigzag test.
- `target_ψ_rad_deviation`: target azimuth deviation of zigzag test.
- `u0=0.0`: the initial x (surge) velocity.
- `v0=0.0`: the initial y (sway) velocity.
- `r0=0.0`: the initial rate of turn [rad/s].
- `δ0=0.0`: the initial rudder angle.
- `δ_rad_rate=10.0*π/180`: the change rate of rudder angle [rad/s].
- `ρ=1025.0`: the seawater density [kg/m^3].
- `algorithm=Tsit5()`: the parameter of DifferentialEquations.ODEProblem.solve()
- `reltol=1e-8`: the parameter of DifferentialEquations.ODEProblem.solve()
- `abstol=1e-8`: the parameter of DifferentialEquations.ODEProblem.solve()

# Examples
KVLCC2_L7 zigzag test.

```julia-rep1
julia> ρ = 1025.0;
julia> basic_params, maneuvering_params = get_KVLCC2_L7_params();
julia> target_δ_rad = 20.0 * π / 180.0
julia> target_ψ_rad_deviation = 20.0 * π / 180.0
julia> start_time_second = 0.00
julia> time_second_interval = 0.01
julia> end_time_second = 80.00
julia> time_list = start_time_second:time_second_interval:end_time_second
julia> n_const = 17.95  # [rps]
julia> n_p_list = n_const * ones(Float64, length(time_list))
julia> δ_list, u_list, v_list, r_list, ψ_list = mmg_3dof_zigzag_test(
    basic_params,
    maneuvering_params,
    time_list
    n_p_list,
    target_δ_rad,
    target_ψ_rad_deviation,
);
```
"""
function mmg_3dof_zigzag_test(
    basic_params::Mmg3DofBasicParams,
    maneuvering_params::Mmg3DofManeuveringParams,
    time_list,
    n_p_list,
    target_δ_rad,
    target_ψ_rad_deviation;
    u0 = 0.0,
    v0 = 0.0,
    r0 = 0.0,
    ψ0 = 0.0,
    δ0 = 0.0,
    δ_rad_rate = 10.0 * π / 180,
    ρ = 1025.0,
    algorithm = Tsit5(),
    reltol = 1e-8,
    abstol = 1e-8,
)
    target_ψ_rad_deviation = abs(target_ψ_rad_deviation)

    final_δ_list = zeros(length(time_list))
    final_u_list = zeros(length(time_list))
    final_v_list = zeros(length(time_list))
    final_r_list = zeros(length(time_list))
    final_ψ_list = zeros(length(time_list))

    next_stage_index = 1
    target_δ_rad = -target_δ_rad  # for changing in while loop
    ψ = ψ0
    while next_stage_index < length(time_list)
        target_δ_rad = -target_δ_rad
        start_index = next_stage_index

        # Make delta list
        δ_list = zeros(length(time_list) - start_index + 1)
        if start_index == 1
            δ_list[1] = δ0
            u0 = u0
            v0 = v0
            r0 = r0
        else
            δ_list[1] = final_δ_list[start_index-1]
            u0 = final_u_list[start_index-1]
            v0 = final_v_list[start_index-1]
            r0 = final_r_list[start_index-1]
        end

        for i = (start_index+1):length(time_list)
            Δt = time_list[i] - time_list[i-1]
            if target_δ_rad > 0
                δ = δ_list[i-start_index] + δ_rad_rate * Δt
                if δ >= target_δ_rad
                    δ = target_δ_rad
                end
                δ_list[i-start_index+1] = δ
            elseif target_δ_rad <= 0
                δ = δ_list[i-start_index] - δ_rad_rate * Δt
                if δ <= target_δ_rad
                    δ = target_δ_rad
                end
                δ_list[i-start_index+1] = δ
            end
        end

        u, v, r, δ, n_p = mmg_3dof_simulate(
            basic_params,
            maneuvering_params,
            time_list[start_index:end],
            δ_list,
            n_p_list[start_index:end],
            u0 = u0,
            v0 = v0,
            r0 = r0,
            ρ = ρ,
            algorithm = algorithm,
            reltol = reltol,
            abstol = abstol,
        )
        x_dummy, y_dummy, ψ_list =
            calc_position(time_list[start_index:end], u, v, r, x0 = 0.0, y0 = 0.0, ψ0 = ψ)
        # get finish index
        target_ψ_rad = ψ0 + target_ψ_rad_deviation
        if target_δ_rad < 0
            target_ψ_rad = ψ0 - target_ψ_rad_deviation
        end
        over_index = findfirst(e -> e > target_ψ_rad, ψ_list)
        if target_δ_rad < 0
            over_index = findfirst(e -> e < target_ψ_rad, ψ_list)
        end
        next_stage_index = length(time_list)
        if isnothing(over_index)
            final_δ_list[start_index:next_stage_index] = δ_list
            final_u_list[start_index:next_stage_index] = u
            final_v_list[start_index:next_stage_index] = v
            final_r_list[start_index:next_stage_index] = r
            final_ψ_list[start_index:next_stage_index] = ψ_list
        else
            ψ = ψ_list[over_index]
            next_stage_index = over_index + start_index - 1
            final_δ_list[start_index:next_stage_index-1] = δ_list[begin:over_index-1]
            final_u_list[start_index:next_stage_index-1] = u[begin:over_index-1]
            final_v_list[start_index:next_stage_index-1] = v[begin:over_index-1]
            final_r_list[start_index:next_stage_index-1] = r[begin:over_index-1]
            final_ψ_list[start_index:next_stage_index-1] = ψ_list[begin:over_index-1]
        end
    end
    final_u_list, final_v_list, final_r_list, final_ψ_list, final_δ_list
end

function estimate_mmg_approx_lsm(
    data::ShipData,
    basic_params::Mmg3DofBasicParams,
    k_0,
    k_1,
    k_2;
    ρ = 1025.0,
)
    @unpack time, u, v, r, x, y, ψ, δ, n_p = data
    @unpack L_pp,
    B,
    d,
    x_G,
    D_p,
    m,
    I_zG,
    A_R,
    η,
    m_x,
    m_y,
    J_z,
    f_α,
    ϵ,
    t_R,
    x_R,
    a_H,
    x_H,
    γ_R_minus,
    γ_R_plus,
    l_R,
    κ,
    t_P,
    w_P0,
    x_P = basic_params

    # -------------------------------------
    # calculating basic info
    U = sqrt.(u .^ 2 + (v - r .* x_G) .^ 2)

    β = U .^ 0
    for i = 1:length(time)
        if isapprox(U[i], 0.0) == false
            β[i] = asin(-(v[i] - r[i] * x_G) / U[i])
        end
    end

    v_dash = U .^ 0
    for i = 1:length(time)
        if isapprox(U[i], 0.0) == false
            v_dash[i] = v[i] / U[i]
        end
    end

    r_dash = U .^ 0
    for i = 1:length(time)
        if isapprox(U[i], 0.0) == false
            r_dash[i] = r[i] * L_pp / U[i]
        end
    end

    w_P = w_P0 * exp.(.-4.0 * (β - r_dash .* x_P) .^ 2)

    J = U .^ 0
    for i = 1:length(time)
        if isapprox(n_p[i], 0.0) == false
            J[i] = (1.0 - w_P[i]) * u[i] / (n_p[i] * D_p)
        end
    end

    K_T = J .^ 2 .* k_2 + J .* k_1 .+ k_0
    β_R = β - r_dash .* l_R

    γ_R = U .^ 0
    for i = 1:length(time)
        if β_R[i] < 0.0
            γ_R[i] = γ_R_minus
        else
            γ_R[i] = γ_R_plus
        end
    end

    v_R = U .* γ_R .* β_R

    u_R = U .^ 0
    for i = 1:length(time)
        if J[i] == 0.0
            u_R[i] = sqrt(η * (κ * ϵ * 8.0 * k_0 * n_p[i]^2 * D_p^4 / pi)^2)
        else
            u_R[i] =
                u[i] *
                (1.0 - w_P[i]) *
                ϵ *
                sqrt(
                    η * (1.0 + κ * (sqrt(1.0 + 8.0 * K_T[i] / (pi * J[i]^2)) - 1.0))^2 +
                    (1.0 - η),
                )
        end
    end

    U_R = sqrt.(u_R .^ 2 + v_R .^ 2)
    α_R = δ - atan.(v_R, u_R)
    F_N = 0.5 * A_R * ρ * f_α * (U_R .^ 2) .* sin.(α_R)
    X_R = -(1.0 - t_R) .* F_N .* sin.(δ)
    X_P = (1.0 - t_P) * ρ .* K_T .* n_p .^ 2 .* D_p^4
    Y_R = -(1.0 + a_H) .* F_N .* cos.(δ)
    N_R = -(x_R + a_H * x_H) .* F_N .* cos.(δ)
    # -------------------------------------

    # -------------------------------------
    spl_u = Spline1D(time, u)
    spl_v = Spline1D(time, v)
    spl_r = Spline1D(time, r)
    f = (spl, x) -> derivative(spl, x)
    du = f(spl_u, time)
    dv = f(spl_v, time)
    dr = f(spl_r, time)

    # calculating parameters
    Ax = hcat(
        -v_dash .^ 0,
        v_dash .^ 2,
        v_dash .* r_dash,
        r_dash .^ 2,
        (v_dash .^ 2) .* (r_dash .^ 2),
    )
    bx =
        ((m + m_x) .* du - (m + m_y) .* v .* r - x_G * m .* r .^ 2 - X_R - X_P) ./
        (0.5 * ρ * L_pp * d .* (U .^ 2))
    R_0_dash, X_vv_dash, X_vr_dash, X_rr_dash, X_vvvv_dash = Ax \ bx

    Ay = hcat(
        v_dash,
        r_dash,
        v_dash .^ 3,
        (v_dash .^ 2) .* (r_dash),
        (v_dash) .* (r_dash .^ 2),
        (r_dash .^ 3),
    )
    by =
        ((m + m_y) .* dv - (m + m_x) .* u .* r + x_G * m .* dr - Y_R) ./
        (0.5 * ρ * L_pp * d .* (U .^ 2))
    Y_v_dash, Y_r_dash, Y_vvv_dash, Y_vvr_dash, Y_vrr_dash, Y_rrr_dash = Ay \ by

    An = hcat(
        v_dash,
        r_dash,
        v_dash .^ 3,
        (v_dash .^ 2) .* (r_dash),
        (v_dash) .* (r_dash .^ 2),
        (r_dash .^ 3),
    )
    bn =
        ((I_zG + x_G^2 * m + J_z) .* dr + x_G * m .* (dv .+ u .* r) - N_R) ./
        (0.5 * ρ * L_pp .^ 2 * d .* (U .^ 2))
    N_v_dash, N_r_dash, N_vvv_dash, N_vvr_dash, N_vrr_dash, N_rrr_dash = An \ bn

    R_0_dash,
    X_vv_dash,
    X_vr_dash,
    X_rr_dash,
    X_vvvv_dash,
    Y_v_dash,
    Y_r_dash,
    Y_vvv_dash,
    Y_vvr_dash,
    Y_vrr_dash,
    Y_rrr_dash,
    N_v_dash,
    N_r_dash,
    N_vvv_dash,
    N_vvr_dash,
    N_vrr_dash,
    N_rrr_dash
end

function estimate_mmg_approx_lsm_time_window_sampling(
    data::ShipData,
    window_size::Int,
    basic_params::Mmg3DofBasicParams,
    k_0,
    k_1,
    k_2;
    ρ = 1025.0,
)
    n_samples = length(data.time) - window_size
    R_0_dash_samples = zeros(n_samples)
    X_vv_dash_samples = zeros(n_samples)
    X_vr_dash_samples = zeros(n_samples)
    X_rr_dash_samples = zeros(n_samples)
    X_vvvv_dash_samples = zeros(n_samples)
    Y_v_dash_samples = zeros(n_samples)
    Y_r_dash_samples = zeros(n_samples)
    Y_vvv_dash_samples = zeros(n_samples)
    Y_vvr_dash_samples = zeros(n_samples)
    Y_vrr_dash_samples = zeros(n_samples)
    Y_rrr_dash_samples = zeros(n_samples)
    N_v_dash_samples = zeros(n_samples)
    N_r_dash_samples = zeros(n_samples)
    N_vvv_dash_samples = zeros(n_samples)
    N_vvr_dash_samples = zeros(n_samples)
    N_vrr_dash_samples = zeros(n_samples)
    N_rrr_dash_samples = zeros(n_samples)
    for i = 1:n_samples
        sample_data = ShipData(
            data.time[i:i+window_size],
            data.u[i:i+window_size],
            data.v[i:i+window_size],
            data.r[i:i+window_size],
            data.x[i:i+window_size],
            data.y[i:i+window_size],
            data.ψ[i:i+window_size],
            data.δ[i:i+window_size],
            data.n_p[i:i+window_size],
        )
        R_0_dash_samples[i],
        X_vv_dash_samples[i],
        X_vr_dash_samples[i],
        X_rr_dash_samples[i],
        X_vvvv_dash_samples[i],
        Y_v_dash_samples[i],
        Y_r_dash_samples[i],
        Y_vvv_dash_samples[i],
        Y_vvr_dash_samples[i],
        Y_vrr_dash_samples[i],
        Y_rrr_dash_samples[i],
        N_v_dash_samples[i],
        N_r_dash_samples[i],
        N_vvv_dash_samples[i],
        N_vvr_dash_samples[i],
        N_vrr_dash_samples[i],
        N_rrr_dash_samples[i] =
            estimate_mmg_approx_lsm(sample_data, basic_params, k_0, k_1, k_2, ρ = ρ)
    end
    R_0_dash_samples,
    X_vv_dash_samples,
    X_vr_dash_samples,
    X_rr_dash_samples,
    X_vvvv_dash_samples,
    Y_v_dash_samples,
    Y_r_dash_samples,
    Y_vvv_dash_samples,
    Y_vvr_dash_samples,
    Y_vrr_dash_samples,
    Y_rrr_dash_samples,
    N_v_dash_samples,
    N_r_dash_samples,
    N_vvv_dash_samples,
    N_vvr_dash_samples,
    N_vrr_dash_samples,
    N_rrr_dash_samples
end

function create_model_for_mcmc_sample_mmg(
    data::ShipData,
    basic_params::Mmg3DofBasicParams,
    k_0,
    k_1,
    k_2;
    ρ = 1025.0,
    σ_u_prior_dist = Uniform(0.00, 0.20),
    σ_v_prior_dist = Uniform(0.00, 0.20),
    σ_r_prior_dist = Uniform(0.00, 0.20),
    R_0_dash_prior_dist = Uniform(-2.0, 2.0),
    X_vv_dash_prior_dist = Uniform(-2.0, 2.0),
    X_vr_dash_prior_dist = Uniform(-2.0, 2.0),
    X_rr_dash_prior_dist = Uniform(-2.0, 2.0),
    X_vvvv_dash_prior_dist = Uniform(-2.0, 2.0),
    Y_v_dash_prior_dist = Uniform(-2.0, 2.0),
    Y_r_dash_prior_dist = Uniform(-2.0, 2.0),
    Y_vvv_dash_prior_dist = Uniform(-2.0, 2.0),
    Y_vvr_dash_prior_dist = Uniform(-2.0, 2.0),
    Y_vrr_dash_prior_dist = Uniform(-2.0, 2.0),
    Y_rrr_dash_prior_dist = Uniform(-2.0, 2.0),
    N_v_dash_prior_dist = Uniform(-2.0, 2.0),
    N_r_dash_prior_dist = Uniform(-2.0, 2.0),
    N_vvv_dash_prior_dist = Uniform(-2.0, 2.0),
    N_vvr_dash_prior_dist = Uniform(-2.0, 2.0),
    N_vrr_dash_prior_dist = Uniform(-2.0, 2.0),
    N_rrr_dash_prior_dist = Uniform(-2.0, 2.0),
    solver = Tsit5(),
    abstol = 1e-6,
    reltol = 1e-3,
)
    time_obs = data.time
    u_obs = data.u
    v_obs = data.v
    r_obs = data.r
    δ_obs = data.δ
    n_p_obs = data.n_p

    @unpack L_pp,
    B,
    d,
    x_G,
    D_p,
    m,
    I_zG,
    A_R,
    η,
    m_x,
    m_y,
    J_z,
    f_α,
    ϵ,
    t_R,
    x_R,
    a_H,
    x_H,
    γ_R_minus,
    γ_R_plus,
    l_R,
    κ,
    t_P,
    w_P0,
    x_P = basic_params

    # create sytem model
    spl_δ = Spline1D(time_obs, δ_obs)
    spl_n_p = Spline1D(time_obs, n_p_obs)
    function MMG!(dX, X, p, t)
        u, v, r, δ, n_p = X
        R_0_dash,
        X_vv_dash,
        X_vr_dash,
        X_rr_dash,
        X_vvvv_dash,
        Y_v_dash,
        Y_r_dash,
        Y_vvv_dash,
        Y_vvr_dash,
        Y_vrr_dash,
        Y_rrr_dash,
        N_v_dash,
        N_r_dash,
        N_vvv_dash,
        N_vvr_dash,
        N_vrr_dash,
        N_rrr_dash = p

        U = sqrt(u^2 + (v - r * x_G)^2)

        β = 0.0
        if U == 0.0
            β = 0.0
        else
            β = asin(-(v - r * x_G) / U)
        end

        v_dash = 0.0
        if U == 0.0
            v_dash = 0.0
        else
            v_dash = v / U
        end

        r_dash = 0.0
        if U == 0.0
            r_dash = 0.0
        else
            r_dash = r * L_pp / U
        end

        w_P = w_P0 * exp(-4.0 * (β - x_P * r_dash)^2)

        J = 0.0
        if n_p == 0.0
            J = 0.0
        else
            J = (1 - w_P) * u / (n_p * D_p)
        end
        K_T = k_0 + k_1 * J + k_2 * J^2
        β_R = β - l_R * r_dash
        γ_R = γ_R_minus

        if β_R < 0.0
            γ_R = γ_R_minus
        else
            γ_R = γ_R_plus
        end

        v_R = U * γ_R * β_R

        u_R = 0.0
        if J == 0.0
            u_R = sqrt(η * (κ * ϵ * 8.0 * k_0 * n_p^2 * D_p^4 / pi)^2)
        else
            u_R =
                u *
                (1.0 - w_P) *
                ϵ *
                sqrt(η * (1.0 + κ * (sqrt(1.0 + 8.0 * K_T / (pi * J^2)) - 1))^2 + (1 - η))
        end

        U_R = sqrt(u_R^2 + v_R^2)
        α_R = δ - atan(v_R, u_R)
        F_N = 0.5 * A_R * ρ * f_α * (U_R^2) * sin(α_R)

        X_H = (
            0.5 *
            ρ *
            L_pp *
            d *
            (U^2) *
            (
                -R_0_dash +
                X_vv_dash * (v_dash^2) +
                X_vr_dash * v_dash * r_dash +
                X_rr_dash * (r_dash^2) +
                X_vvvv_dash * (v_dash^4)
            )
        )
        X_R = -(1.0 - t_R) * F_N * sin(δ)
        X_P = (1.0 - t_P) * ρ * K_T * n_p^2 * D_p^4
        Y_H = (
            0.5 *
            ρ *
            L_pp *
            d *
            (U^2) *
            (
                Y_v_dash * v_dash +
                Y_r_dash * r_dash +
                Y_vvv_dash * (v_dash^3) +
                Y_vvr_dash * (v_dash^2) * r_dash +
                Y_vrr_dash * v_dash * (r_dash^2) +
                Y_rrr_dash * (r_dash^3)
            )
        )
        Y_R = -(1 + a_H) * F_N * cos(δ)
        N_H = (
            0.5 *
            ρ *
            (L_pp^2) *
            d *
            (U^2) *
            (
                N_v_dash * v_dash +
                N_r_dash * r_dash +
                N_vvv_dash * (v_dash^3) +
                N_vvr_dash * (v_dash^2) * r_dash +
                N_vrr_dash * v_dash * (r_dash^2) +
                N_rrr_dash * (r_dash^3)
            )
        )
        N_R = -(x_R + a_H * x_H) * F_N * cos(δ)
        dX[1] = du = ((X_H + X_R + X_P) + (m + m_y) * v * r + x_G * m * (r^2)) / (m + m_x)
        dX[2] =
            dv =
                (
                    (x_G^2) * (m^2) * u * r - (N_H + N_R) * x_G * m +
                    ((Y_H + Y_R) - (m + m_x) * u * r) * (I_zG + J_z + (x_G^2) * m)
                ) / ((I_zG + J_z + (x_G^2) * m) * (m + m_y) - (x_G^2) * (m^2))
        dX[3] = dr = (N_H + N_R - x_G * m * (dv + u * r)) / (I_zG + J_z + (x_G^2) * m)
        dX[4] = dδ = derivative(spl_δ, t)
        dX[5] = dn_p = derivative(spl_n_p, t)
    end

    R_0_dash_start = 0.022
    X_vv_dash_start = -0.040
    X_vr_dash_start = 0.002
    X_rr_dash_start = 0.011
    X_vvvv_dash_start = 0.771
    Y_v_dash_start = -0.315
    Y_r_dash_start = 0.083
    Y_vvv_dash_start = -1.607
    Y_vvr_dash_start = 0.379
    Y_vrr_dash_start = -0.391
    Y_rrr_dash_start = 0.008
    N_v_dash_start = -0.137
    N_r_dash_start = -0.049
    N_vvv_dash_start = -0.030
    N_vvr_dash_start = -0.294
    N_vrr_dash_start = 0.055
    N_rrr_dash_start = -0.013

    p = [
        R_0_dash_start,
        X_vv_dash_start,
        X_vr_dash_start,
        X_rr_dash_start,
        X_vvvv_dash_start,
        Y_v_dash_start,
        Y_r_dash_start,
        Y_vvv_dash_start,
        Y_vvr_dash_start,
        Y_vrr_dash_start,
        Y_rrr_dash_start,
        N_v_dash_start,
        N_r_dash_start,
        N_vvv_dash_start,
        N_vvr_dash_start,
        N_vrr_dash_start,
        N_rrr_dash_start,
    ]

    u0 = 2.29 * 0.512
    v0 = 0.0
    r0 = 0.0
    X0 = [u_obs[1]; v_obs[1]; r_obs[1]; δ_obs[1]; n_p_obs[1]]
    prob1 = ODEProblem(MMG!, X0, (time_obs[1], time_obs[end]), p)

    # create probabilistic model
    @model function fitMMG(time_obs, obs, prob1)
        σ_u ~ σ_u_prior_dist
        σ_v ~ σ_v_prior_dist
        σ_r ~ σ_r_prior_dist
        R_0_dash ~ R_0_dash_prior_dist
        X_vv_dash ~ X_vv_dash_prior_dist
        X_vr_dash ~ X_vr_dash_prior_dist
        X_rr_dash ~ X_rr_dash_prior_dist
        X_vvvv_dash ~ X_vvvv_dash_prior_dist
        Y_v_dash ~ Y_v_dash_prior_dist
        Y_r_dash ~ Y_r_dash_prior_dist
        Y_vvv_dash ~ Y_vvv_dash_prior_dist
        Y_vvr_dash ~ Y_vvr_dash_prior_dist
        Y_vrr_dash ~ Y_vrr_dash_prior_dist
        Y_rrr_dash ~ Y_rrr_dash_prior_dist
        N_v_dash ~ N_v_dash_prior_dist
        N_r_dash ~ N_r_dash_prior_dist
        N_vvv_dash ~ N_vvv_dash_prior_dist
        N_vvr_dash ~ N_vvr_dash_prior_dist
        N_vrr_dash ~ N_vrr_dash_prior_dist
        N_rrr_dash ~ N_rrr_dash_prior_dist

        p = [
            R_0_dash,
            X_vv_dash,
            X_vr_dash,
            X_rr_dash,
            X_vvvv_dash,
            Y_v_dash,
            Y_r_dash,
            Y_vvv_dash,
            Y_vvr_dash,
            Y_vrr_dash,
            Y_rrr_dash,
            N_v_dash,
            N_r_dash,
            N_vvv_dash,
            N_vvr_dash,
            N_vrr_dash,
            N_rrr_dash,
        ]
        prob = remake(prob1, p = p)
        sol = solve(prob, solver, abstol = abstol, reltol = reltol)
        predicted = sol(time_obs)
        for i = 1:length(predicted)
            obs[1][i] ~ Normal(predicted[i][1], σ_u) # u
            obs[2][i] ~ Normal(predicted[i][2], σ_v) # v
            obs[3][i] ~ Normal(predicted[i][3], σ_r) # r
        end
    end

    return fitMMG(time_obs, [u_obs, v_obs, r_obs], prob1)
end

function create_model_for_mcmc_sample_mmg(
    data::ShipData,
    basic_params::Mmg3DofBasicParams,
    k_0,
    k_1,
    k_2,
    σ_u,
    σ_v,
    σ_r;
    ρ = 1025.0,
    R_0_dash_prior_dist = Uniform(-2.0, 2.0),
    X_vv_dash_prior_dist = Uniform(-2.0, 2.0),
    X_vr_dash_prior_dist = Uniform(-2.0, 2.0),
    X_rr_dash_prior_dist = Uniform(-2.0, 2.0),
    X_vvvv_dash_prior_dist = Uniform(-2.0, 2.0),
    Y_v_dash_prior_dist = Uniform(-2.0, 2.0),
    Y_r_dash_prior_dist = Uniform(-2.0, 2.0),
    Y_vvv_dash_prior_dist = Uniform(-2.0, 2.0),
    Y_vvr_dash_prior_dist = Uniform(-2.0, 2.0),
    Y_vrr_dash_prior_dist = Uniform(-2.0, 2.0),
    Y_rrr_dash_prior_dist = Uniform(-2.0, 2.0),
    N_v_dash_prior_dist = Uniform(-2.0, 2.0),
    N_r_dash_prior_dist = Uniform(-2.0, 2.0),
    N_vvv_dash_prior_dist = Uniform(-2.0, 2.0),
    N_vvr_dash_prior_dist = Uniform(-2.0, 2.0),
    N_vrr_dash_prior_dist = Uniform(-2.0, 2.0),
    N_rrr_dash_prior_dist = Uniform(-2.0, 2.0),
    solver = Tsit5(),
    abstol = 1e-6,
    reltol = 1e-3,
)
    time_obs = data.time
    u_obs = data.u
    v_obs = data.v
    r_obs = data.r
    δ_obs = data.δ
    n_p_obs = data.n_p

    @unpack L_pp,
    B,
    d,
    x_G,
    D_p,
    m,
    I_zG,
    A_R,
    η,
    m_x,
    m_y,
    J_z,
    f_α,
    ϵ,
    t_R,
    x_R,
    a_H,
    x_H,
    γ_R_minus,
    γ_R_plus,
    l_R,
    κ,
    t_P,
    w_P0,
    x_P = basic_params

    # create sytem model
    spl_δ = Spline1D(time_obs, δ_obs)
    spl_n_p = Spline1D(time_obs, n_p_obs)
    function MMG!(dX, X, p, t)
        u, v, r, δ, n_p = X
        R_0_dash,
        X_vv_dash,
        X_vr_dash,
        X_rr_dash,
        X_vvvv_dash,
        Y_v_dash,
        Y_r_dash,
        Y_vvv_dash,
        Y_vvr_dash,
        Y_vrr_dash,
        Y_rrr_dash,
        N_v_dash,
        N_r_dash,
        N_vvv_dash,
        N_vvr_dash,
        N_vrr_dash,
        N_rrr_dash = p

        U = sqrt(u^2 + (v - r * x_G)^2)

        β = 0.0
        if U == 0.0
            β = 0.0
        else
            β = asin(-(v - r * x_G) / U)
        end

        v_dash = 0.0
        if U == 0.0
            v_dash = 0.0
        else
            v_dash = v / U
        end

        r_dash = 0.0
        if U == 0.0
            r_dash = 0.0
        else
            r_dash = r * L_pp / U
        end

        w_P = w_P0 * exp(-4.0 * (β - x_P * r_dash)^2)

        J = 0.0
        if n_p == 0.0
            J = 0.0
        else
            J = (1 - w_P) * u / (n_p * D_p)
        end
        K_T = k_0 + k_1 * J + k_2 * J^2
        β_R = β - l_R * r_dash
        γ_R = γ_R_minus

        if β_R < 0.0
            γ_R = γ_R_minus
        else
            γ_R = γ_R_plus
        end

        v_R = U * γ_R * β_R

        u_R = 0.0
        if J == 0.0
            u_R = sqrt(η * (κ * ϵ * 8.0 * k_0 * n_p^2 * D_p^4 / pi)^2)
        else
            u_R =
                u *
                (1.0 - w_P) *
                ϵ *
                sqrt(η * (1.0 + κ * (sqrt(1.0 + 8.0 * K_T / (pi * J^2)) - 1))^2 + (1 - η))
        end

        U_R = sqrt(u_R^2 + v_R^2)
        α_R = δ - atan(v_R, u_R)
        F_N = 0.5 * A_R * ρ * f_α * (U_R^2) * sin(α_R)

        X_H = (
            0.5 *
            ρ *
            L_pp *
            d *
            (U^2) *
            (
                -R_0_dash +
                X_vv_dash * (v_dash^2) +
                X_vr_dash * v_dash * r_dash +
                X_rr_dash * (r_dash^2) +
                X_vvvv_dash * (v_dash^4)
            )
        )
        X_R = -(1.0 - t_R) * F_N * sin(δ)
        X_P = (1.0 - t_P) * ρ * K_T * n_p^2 * D_p^4
        Y_H = (
            0.5 *
            ρ *
            L_pp *
            d *
            (U^2) *
            (
                Y_v_dash * v_dash +
                Y_r_dash * r_dash +
                Y_vvv_dash * (v_dash^3) +
                Y_vvr_dash * (v_dash^2) * r_dash +
                Y_vrr_dash * v_dash * (r_dash^2) +
                Y_rrr_dash * (r_dash^3)
            )
        )
        Y_R = -(1 + a_H) * F_N * cos(δ)
        N_H = (
            0.5 *
            ρ *
            (L_pp^2) *
            d *
            (U^2) *
            (
                N_v_dash * v_dash +
                N_r_dash * r_dash +
                N_vvv_dash * (v_dash^3) +
                N_vvr_dash * (v_dash^2) * r_dash +
                N_vrr_dash * v_dash * (r_dash^2) +
                N_rrr_dash * (r_dash^3)
            )
        )
        N_R = -(x_R + a_H * x_H) * F_N * cos(δ)
        dX[1] = du = ((X_H + X_R + X_P) + (m + m_y) * v * r + x_G * m * (r^2)) / (m + m_x)
        dX[2] =
            dv =
                (
                    (x_G^2) * (m^2) * u * r - (N_H + N_R) * x_G * m +
                    ((Y_H + Y_R) - (m + m_x) * u * r) * (I_zG + J_z + (x_G^2) * m)
                ) / ((I_zG + J_z + (x_G^2) * m) * (m + m_y) - (x_G^2) * (m^2))
        dX[3] = dr = (N_H + N_R - x_G * m * (dv + u * r)) / (I_zG + J_z + (x_G^2) * m)
        dX[4] = dδ = derivative(spl_δ, t)
        dX[5] = dn_p = derivative(spl_n_p, t)
    end

    R_0_dash_start = 0.022
    X_vv_dash_start = -0.040
    X_vr_dash_start = 0.002
    X_rr_dash_start = 0.011
    X_vvvv_dash_start = 0.771
    Y_v_dash_start = -0.315
    Y_r_dash_start = 0.083
    Y_vvv_dash_start = -1.607
    Y_vvr_dash_start = 0.379
    Y_vrr_dash_start = -0.391
    Y_rrr_dash_start = 0.008
    N_v_dash_start = -0.137
    N_r_dash_start = -0.049
    N_vvv_dash_start = -0.030
    N_vvr_dash_start = -0.294
    N_vrr_dash_start = 0.055
    N_rrr_dash_start = -0.013

    p = [
        R_0_dash_start,
        X_vv_dash_start,
        X_vr_dash_start,
        X_rr_dash_start,
        X_vvvv_dash_start,
        Y_v_dash_start,
        Y_r_dash_start,
        Y_vvv_dash_start,
        Y_vvr_dash_start,
        Y_vrr_dash_start,
        Y_rrr_dash_start,
        N_v_dash_start,
        N_r_dash_start,
        N_vvv_dash_start,
        N_vvr_dash_start,
        N_vrr_dash_start,
        N_rrr_dash_start,
    ]

    u0 = 2.29 * 0.512
    v0 = 0.0
    r0 = 0.0
    X0 = [u_obs[1]; v_obs[1]; r_obs[1]; δ_obs[1]; n_p_obs[1]]
    prob1 = ODEProblem(MMG!, X0, (time_obs[1], time_obs[end]), p)

    # create probabilistic model
    @model function fitMMG(time_obs, obs, prob1)
        R_0_dash ~ R_0_dash_prior_dist
        X_vv_dash ~ X_vv_dash_prior_dist
        X_vr_dash ~ X_vr_dash_prior_dist
        X_rr_dash ~ X_rr_dash_prior_dist
        X_vvvv_dash ~ X_vvvv_dash_prior_dist
        Y_v_dash ~ Y_v_dash_prior_dist
        Y_r_dash ~ Y_r_dash_prior_dist
        Y_vvv_dash ~ Y_vvv_dash_prior_dist
        Y_vvr_dash ~ Y_vvr_dash_prior_dist
        Y_vrr_dash ~ Y_vrr_dash_prior_dist
        Y_rrr_dash ~ Y_rrr_dash_prior_dist
        N_v_dash ~ N_v_dash_prior_dist
        N_r_dash ~ N_r_dash_prior_dist
        N_vvv_dash ~ N_vvv_dash_prior_dist
        N_vvr_dash ~ N_vvr_dash_prior_dist
        N_vrr_dash ~ N_vrr_dash_prior_dist
        N_rrr_dash ~ N_rrr_dash_prior_dist

        p = [
            R_0_dash,
            X_vv_dash,
            X_vr_dash,
            X_rr_dash,
            X_vvvv_dash,
            Y_v_dash,
            Y_r_dash,
            Y_vvv_dash,
            Y_vvr_dash,
            Y_vrr_dash,
            Y_rrr_dash,
            N_v_dash,
            N_r_dash,
            N_vvv_dash,
            N_vvr_dash,
            N_vrr_dash,
            N_rrr_dash,
        ]
        prob = remake(prob1, p = p)
        sol = solve(prob, solver, abstol = abstol, reltol = reltol)
        predicted = sol(time_obs)
        for i = 1:length(predicted)
            obs[1][i] ~ Normal(predicted[i][1], σ_u) # u
            obs[2][i] ~ Normal(predicted[i][2], σ_v) # v
            obs[3][i] ~ Normal(predicted[i][3], σ_r) # r
        end
    end

    return fitMMG(time_obs, [u_obs, v_obs, r_obs], prob1)
end

max_stars_repo_name: mitsuyukiLab/JuliaShipMMG

max_issues_repo_name: mitsuyukiLab/JuliaShipMMG


text:
# # Foundations of Bilevel Programming: Example 2
# This example is from the book _Foundations of Bilevel Programming_ by Stephan
# Dempe, Chapter 3.2, Page 25. [url](https://www.springer.com/gp/book/9781402006319)
# Moving the bound on x to lower level

# Model of the problem
# First level
# ```math
# \min 3x + y,\\
# \notag s.t.\\
# x \leq 5,\\
# y \leq 8,\\
# y \geq 0,\\
# ```
# Second level
# ```math
# \min -x,\\
# \notag s.t.\\
# x + y <= 8,\\
# 4x + y >= 8,\\
# 2x + y <= 13,\\
# 2x - y <= 0,\\
# x <= 5\\
# ```

using BilevelJuMP
using Ipopt
using JuMP
using Test

model = BilevelModel(Ipopt.Optimizer, mode = BilevelJuMP.ProductMode(1e-9))

# First we need to define all of the variables in the upper and lower problems:

@variable(Upper(model), y, start = 8 / 15)
@variable(Lower(model), x, start = 3.5 * 8 / 15)

# Then we can add the objective and constraints of the upper problem:
# Upper level objective function
@objective(Upper(model), Min, 3x + y)

# Upper level constraints
@constraints(Upper(model), begin
    u1, x <= 5
    u2, y <= 8
    u3, y >= 0
end)

# Followed by the objective and constraints of the lower problem:
# Lower level objective function
@objective(Lower(model), Min, -x)

# Lower level constraints
@constraint(Lower(model), l1,  x +  y <= 8)
@constraint(Lower(model), l2, 4x +  y >= 8)
@constraint(Lower(model), l3, 2x +  y <= 13)
@constraint(Lower(model), l4, 2x - 7y <= 0)

# !!! tip
#     You can use the singular `@constraint` macro or the plural `@constraints`!

# We can also set hints for the variables associated with the problems.

# In this example, we know the duals on the lower constraints are in
# the set `[-15, 15]`:
for c in [l1, l2, l3, l4]
    BilevelJuMP.set_dual_upper_bound(c, 15)
    BilevelJuMP.set_dual_lower_bound(c, -15)
end

# While we think the primal variables are in `[-10, 6]` for `x` and `[-1, 9]`
# for `y`. These hints are optional. But supplying them (e.g., from domain
# knowledge) can be helpful for the solver.

BilevelJuMP.set_primal_lower_bound_hint(x, -10)
BilevelJuMP.set_primal_upper_bound_hint(x, 6)
BilevelJuMP.set_primal_lower_bound_hint(y, -1)
BilevelJuMP.set_primal_upper_bound_hint(y, 9)

# Now we can solve the problem and verify the solution again that reported by
# Dempe.

optimize!(model)

# Automated testing

@test objective_value(model) ≈ 3 * (3.5 * 8 / 15) + (8 / 15) atol=1e-4
@test BilevelJuMP.lower_objective_value(model) ≈ -3.5 * 8 / 15 atol=1e-4
@test objective_value(Lower(model)) ≈ -3.5 * 8 / 15 atol=1e-4
@test value(x) ≈ 3.5 * 8 / 15 atol=1e-4
@test value(y) ≈ 8 / 15 atol=1e-4
@test value(u1) ≈ 3.5 * 8 / 15 atol=1e-4
@test value(l1) ≈ 4.5 * 8 / 15 atol=1e-4
@test dual(l1) ≈ [0] atol=1e-4
@test dual(l3) ≈ [0] atol=1e-4

# TODO: why are these commented out?    #src
# @test dual(l2) #≈ [0] atol=atol       #src
# @test dual(l4) #≈ [0] atol=atol       #src

ext: jl


text:
# Qualified names → objects

function getthing(mod::Module, name::Vector, default = nothing)
  thing = mod
  for sym in name
    sym = Symbol(sym)
    if isdefined(thing, sym) && !Base.isdeprecated(mod, sym)
      thing = getfield(thing, sym)
    else
      return default
    end
  end
  return thing
end

getthing(mod::Module, name::AbstractString, default = nothing) =
  name == "" ?
    default :
    @as x name split(x, ".", keepempty=false) map(Symbol, x) getthing(mod, x, default)

getthing(mod::Module, ::Nothing, default) = default

getthing(mod::Module, x, default) = error("can't get $x from a module")

getthing(args...) = getthing(Main, args...)

# include_string with line numbers

function Base.include_string(mod, s::AbstractString, fname::AbstractString, line::Integer)
  include_string(mod, "\n"^(line-1)*s, fname)
end

function getmodule(parent::Union{Nothing, Module}, mod::String)
  parent ≠ nothing && (mod = string(parent, ".", mod))
  getmodule(mod)
end

function getmodule(mod::String)
  mods = split(mod, '.')
  if length(mods) == 1
    getpackage(mod)
  else
    getsubmod(mods)
  end
end

function getpackage(mod)
  inds = filter(x -> x.name==mod, collect(keys(Base.loaded_modules)))
  if length(inds) == 1
    return get(Base.loaded_modules, first(inds), Main)
  elseif length(inds) == 0
    return nothing
  else
    @warn "no support for multiple packages with the same name yet (conflicting packages: $mod)"
    return get(Base.loaded_modules, first(inds), Main)
  end
end

function getsubmod(mods)
  mod = getpackage(popfirst!(mods))
  mod == nothing && (return nothing)
  for submod in mods
    submod = Symbol(submod)
    if isdefined(mod, submod) && !Base.isdeprecated(mod, submod)
      s = getfield(mod, submod)
      if s isa Module
        mod = s
      else
        return nothing
      end
    else
      return nothing
    end
  end
  mod
end

# Get the current module for a file/pos

function getmodule(code::AbstractString, pos; filemod = nothing)
  codem = codemodule(code, pos)
  modstr = (codem != "" && filemod != nothing) ? "$filemod.$codem" :
           codem == "" ? filemod : codem
  getmodule(modstr)
end


text:
#i64o-masks.jl

#masking and related operations on superints.

#generates top and bottom masks corresponding to a certain fsize
doc"""
`Unums.mask_top` returns an UInt64 with the 'top mask' of a particular fsize.
The top mask labels the bits which are part of the unum representation.  Note
that this is only valid for single UInt64s.  For ArrayNums, you will need the
Unums.mask_top! function.
Passing an Int64 to `mask_top(FSS::Int64)` automatically detects it to generate
a mask for the FSS in general.
"""
mask_top(fsize::UInt16) = reinterpret(UInt64, -9223372036854775808 >> fsize)
mask_top(FSS::Int64) = mask_top(max_fsize(FSS))
mask_top(::UInt64, fsize::UInt16) = reinterpret(UInt64, -9223372036854775808 >> fsize)

doc"""
`Unums.mask_bot` returns an UInt64 with the 'bottom mask' of a particular fsize.
The bottom mask labels the bits which are going to be thrown away and are not
part of the unum representation and are there only because of the padding
scheme.   Note that this is only valid for single UInt64s.  For ArrayNums, you
will need the Unums.mask_bot! function.
Passing an Int64 to `mask_top(FSS::Int64)` automatically detects it to generate
a mask for the FSS in general.
"""
mask_bot(fsize::UInt16) = ~reinterpret(UInt64, -9223372036854775808 >> fsize)
mask_bot(FSS::Int64) = mask_bot(max_fsize(FSS))
mask_bot(::UInt64, fsize::UInt16) = ~reinterpret(UInt64, -9223372036854775808 >> fsize)

doc"""
`Unums.mask_top!` fills an array with the 'top mask' of a particular fsize.  The
top mask labels the bits which are part of the unum representation.
"""
function mask_top!{FSS}(n::ArrayNum{FSS}, fsize::UInt16)
  middle_spot = div(fsize, 0x0040) + 1
  middle_cell = mask_top(fsize % 0x0040)
  for idx = 1:__cell_length(FSS)
    @inbounds n.a[idx] = (idx < middle_spot) * f64
    @inbounds n.a[idx] += (idx == middle_spot) * middle_cell
  end
  return n
end

#generates frac_mask_top!{ESS,FSS}(Unum{ESS,FSS})
@fracproc mask_top fsize

const bot_array = [z64, z64, f64]
doc"""
`Unums.mask_bot!` fills an array with the 'bottom mask' of a particular fsize.
The bottom mask labels the bits which are going to be thrown away and are not
part of the unum representation and are there only because of the padding
scheme.
"""
function mask_bot!{FSS}(n::ArrayNum{FSS}, fsize::UInt16)
  middle_spot = div(fsize, 0x0040) + 1
  middle_cell = mask_bot(fsize % 0x0040)
  for idx = 1:__cell_length(FSS)
    @inbounds n.a[idx] = (idx > middle_spot) * f64
    @inbounds n.a[idx] += (idx == middle_spot) * middle_cell
  end
  return n
end

@fracproc mask_bot fsize

doc"""
`Unums.fill_mask!` takes two arraynums and fills the first one with the UInt64 AND
operation with each other UInt64.
"""
function fill_mask!{FSS}(n::ArrayNum{FSS}, m::ArrayNum{FSS})
  for idx = 1:__cell_length(FSS)
    @inbounds n.a[idx] = n.a[idx] & m.a[idx]
  end
  n
end

doc"""
`Unums.bottom_bit` returns the bottom bit of a fraction with a given fsize.
"""
bottom_bit(fsize::UInt16) = reinterpret(UInt64, -9223372036854775808 >>> fsize)
bottom_bit(FSS::Int64) = bottom_bit(max_fsize(FSS))
bottom_bit(::UInt64, fsize::UInt16) = reinterpret(UInt64, -9223372036854775808 >>> fsize)
bottom_bit(::UInt64) = reinterpret(UInt64, -9223372036854775808 >>> fsize)

doc"""
`Unums.bottom_bit!` returns the bottom bit of a fraction with a given fsize.
If no fsize is provided, it returns a zero arraynum with the lowest bit set.
"""
function bottom_bit!{FSS}(n::ArrayNum{FSS}, fsize::UInt16)
  middle_cell = div(fsize, 0x0040) + 1
  middle_word = t64 >> (fsize % 0x0040)
  for idx = 1:__cell_length(FSS)
    @inbounds n.a[idx] = (idx == middle_cell) * middle_word
  end
  return n
end

function bottom_bit!{FSS}(n::ArrayNum{FSS})
  l = __cell_length(FSS)
  for idx = 1:(l-1)
    @inbounds n.a[idx] = z64
  end
  @inbounds n.a[l] = o64
  return n
end

@fracproc bottom_bit fsize
frac_bottom_bit!{ESS,FSS}(x::UnumSmall{ESS,FSS}) = (x.fraction = bottom_bit(FSS); return x)
frac_bottom_bit!{ESS,FSS}(x::UnumLarge{ESS,FSS}) = (bottom_bit!(x.fraction); return x)

max_stars_repo_name: REX-Computing/unumjl

max_issues_repo_name: REX-Computing/unumjl

max_forks_repo_name: REX-Computing/unumjl


text:
draw(PNG("square-rotate.png", 140, 100),
     Paper("lightgrey"),
     Axes(centred=false)) do
    square(1, align=7, rotate=45)
end


text:
module test_24_Reuse

using ModiaPlot

include("test_base/test_24_Reuse.jl")

end


text:
""" Visualize raw and semantic flow graphs.
"""
module Visualization

using Compat

using Catlab.WiringDiagrams, Catlab.Graphics
import Catlab.Graphics.WiringDiagramLayouts: box_label, wire_label
using ..Doctrine
using ..RawFlowGraphs
import ..Serialization: text_label

# Raw flow graphs
#################

# FIXME: These methods use language-specific attributes. Perhaps there should
# be some standardization across languages.

function box_label(::MIME"text/plain", node::RawNode)
  lang = node.language
  get_first(lang, ["slot", "qual_name", "function", "kind"], "")
end

function wire_label(::MIME"text/plain", port::RawPort)
  lang = port.language
  get_first(lang, ["qual_name", "class"], "")
end

function get_first(collection, keys, default)
  if isempty(keys); return default end
  get(collection, splice!(keys, 1)) do
    get_first(collection, keys, default)
  end
end

# Semantic flow graphs
######################

box_label(::MIME"text/plain", f::Monocl.Hom) = text_label(f)
wire_label(::MIME"text/plain", A::Monocl.Ob) = text_label(A)

end

max_stars_repo_name: IBM/semanticflowgraph

max_issues_repo_name: epatters/semanticflowgraph

max_forks_repo_name: IBM/semanticflowgraph


text:
# Completely factorized variational inference

using NumericExtensions

const ϵ = 1.0e-64

# update variational paramter `θ` based on eq. (9a), (9b)
function _updateE!(fhmm::FHMM,
                   Y::AbstractMatrix,
                   θ::Array{Float64, 3},   # shape (K, M, T)
                   C⁻¹::Matrix{Float64},    # shape (D, D)
                   logP::Array{Float64, 3}, # shape (K, K, M)
                   Δ::Matrix{Float64},     # shape (K, M)
                   Ỹ::Array{Float64, 3}     # shape (D, T)
                   )
    const D, T = size(Y)
    # alias
    W = fhmm.W

    for m=1:fhmm.M
        Δ[:,m] = diag(W[:,:,m]'*C⁻¹*W[:,:,m])
    end

    # Eq. (9b)
    ΣWθ = zeros(Float64, D)
    for m=1:fhmm.M
        for t=1:T
            fill!(ΣWθ, 0)
            for l=1:fhmm.M
                if m != l
                    ΣWθ += W[:,:,l] * θ[:,l,t]
                end
            end
            Ỹ[:,m,t] = Y[:,t] - ΣWθ
        end
    end

    # Eq. (9a)
    for m=1:fhmm.M
        WᵗC⁻¹ = W[:,:,m]'C⁻¹
        for t=2:T-1
            θ[:,m,t] = softmax(WᵗC⁻¹*Ỹ[:,m,t] - 0.5Δ[:,m]
                + logP[:,:,m]*θ[:,m,t-1]
                + logP[:,:,m]'*θ[:,m,t+1])
        end
        # special cases
        θ[:,m,1] = softmax(WᵗC⁻¹*Ỹ[:,m,1] - 0.5Δ[:,m]
                + logP[:,:,m]'*θ[:,m,2])
        θ[:,m,T] = softmax(WᵗC⁻¹*Ỹ[:,m,T] - 0.5Δ[:,m]
                + logP[:,:,m]*θ[:,m,T-1])
    end

    @assert !any(isnan(θ))

    return θ
end

# update variational parameters until convergence
function updateE!(fhmm::FHMM,
                  Y::AbstractMatrix,
                  θ::Array{Float64, 3}; # shape (K, M, T)
                  maxiter::Int=10,
                  tol::Float64=1.0e-4,
                  verbose::Bool=false)
    const D, T = size(Y)

    C⁻¹::Matrix{Float64} = fhmm.C^-1
    logP::Array{Float64, 3} = log(fhmm.P + ϵ)
    Δ = Array(Float64, fhmm.K, fhmm.M)
    Ỹ = Array(Float64, D, fhmm.M, T)

    θᵗ = copy(θ)
    for iter=1:maxiter
        # update expectations
        _updateE!(fhmm, Y, θᵗ, C⁻¹, logP, Δ, Ỹ)

        # check if converged
        diff = vecnorm(θᵗ-θ)
        if diff < tol
            if verbose
                println("$(diff): converged at #$(iter).")
            end
            break
        end

        # update θ with new one
        θ = copy(θᵗ)
    end

    return θ
end

# M-step
function updateM!(fhmm::FHMM,
                  Y::AbstractMatrix,
                  θ::Array{Float64, 3};
                  fzero::Bool=false)
    const D, T = size(Y)

    # Eq. (A.4) update initial state prob.
    fhmm.π[:,:] = θ[:,:,1]

    # Eq. (A.5) update transition matrices.
    for m=1:fhmm.M
        for i=1:fhmm.K
            for j=1:fhmm.K
                fhmm.P[i,j,m] = sum([θ[i,m,t]*θ[j,m,t-1] for t=2:T])
            end
        end
        θᵐ = reshape(θ[:,m,1:T-1], fhmm.K, T-1)
        # avoid zero division
        fhmm.P[:,:,m] = fhmm.P[:,:,m] ./ (sum(θᵐ, 2) + ϵ)'
    end

    # Eq. (A.3) update observation means.
    θʳ = reshape(θ, fhmm.K*fhmm.M, T) # slight allocations
    fhmm.W[:,:,:] = reshape((Y*θʳ') * pinv(θʳ*θʳ'), D, fhmm.K, fhmm.M)

    # sparseness contraint for minimum norm col of W
    # ** not showen in the original paper
    if fzero
        for m=1:fhmm.M
            minidx = indmin([norm(fhmm.W[:,k,m]) for k=1:fhmm.K])
            fhmm.W[:,minidx,m] .*= 0.99
        end
    end

    # Eq. (A.7) update C
    s = zeros(D, D)
    for t=1:T
        for m=1:fhmm.M
            s += fhmm.W[:,:,m]*θ[:,m,t]*Y[:,t]'
        end
    end
    fhmm.C[:,:] = 1/T * (Y*Y' - s)

    nothing
end

function randθ(K::Int, M::Int, T::Int)
    return softmax(rand(K, M, T), 1)
end

# TODO derive correct lower bound
function bound(fhmm::FHMM,
               Y::AbstractMatrix,
               θ::Array{Float64, 3})
    const D, T = size(Y)
    likelihood::Float64 = 0.0
    logθ::Array{Float64, 3} = log(θ + ϵ)
    logP::Array{Float64, 3} = log(fhmm.P + ϵ)

    for t=1:T
        for m=1:fhmm.M
            likelihood += (θ[:,m,t]'logθ[:,m,t])[1]
        end
    end

    likelihood -= T*D*log(pi)
    d = det(fhmm.C)
    if d > 0
        likelihood -= 0.5T*log(d)
    end

    C⁻¹::Matrix{Float64} = fhmm.C^-1
    for t=1:T
        ws = sum([fhmm.W[:,:,m]*θ[:,m,t] for m=1:fhmm.M])
        likelihood -= (0.5*(Y[:,t] - ws)'*C⁻¹*(Y[:,t] - ws))[1]
    end

    for m=1:fhmm.M
        likelihood += (θ[:,m,1]'log(fhmm.π[:,m] + ϵ))[1]
    end

    for t=2:T
        for m=1:fhmm.M
            likelihood += (θ[:,m,t]'logP[:,:,m]*θ[:,m,t-1])[1]
        end
    end

    @assert !isnan(likelihood)

    return likelihood
end

# Result of completely factorized variational (CFV) inference
immutable CFVInferenceResult
    likelihoods::Vector{Float64}
    θ::Array{Float64, 3}
end

function fit!(fhmm::FHMM,
              Y::AbstractMatrix; # Y: observation matrix, shape (D, T)
              maxiter::Int=100,
              tol::Float64=1.0e-5,
              fzero::Bool=false,
              verbose::Bool=false)
    const D, T = size(Y)

    # initialize variational parameter
    # means of state variables, shape (K, M, T)
    θ = randθ(fhmm.K, fhmm.M, T)

    likelihood::Vector{Float64} = zeros(1)

    # Roop of EM algorithm
    for iter=1:maxiter
        # compute bound
        score = bound(fhmm, Y, θᵗ)

        # update expectations
        θ = updateE!(fhmm, Y, θ, verbose=verbose)

        # update parameters of FHMM
        updateM!(fhmm, Y, θ, fzero=fzero)

        improvement = (score - likelihood[end]) / abs(likelihood[end])

        if verbose
            println("#$(iter): bound $(likelihood[end])
                    improvement: $(improvement)")
        end

        # check if converged
        #=
        if improvement < 1.0e-7
            if verbose
                println("#$(iter) converged")
            end
            break
        end
        =#

        push!(likelihood, score)
    end

    # remove initial zero
    shift!(likelihood)

    return CFVInferenceResult(likelihood, θ)
end


text:
module TestUtils

using IterTools
using ParameterHandling
using Test

using ParameterHandling: AbstractParameter, value

# Handles equality of scalars, functions, tuples or arbitrary types
function default_equality(a::T, b::T; kwargs...) where {T}
    vals = fieldvalues(a)

    # If we don't have any fields then we're probably dealing with scalars
    if isempty(vals)
        # Only call isapprox for numbers, otherwise we fallback to ==
        return T <: Number ? isapprox(a, b; kwargs...) : a == b
    else
        return all(t -> default_equality(t...; kwargs...), zip(vals, fieldvalues(b)))
    end
end

# Handles extracting elements from arrays.
# Needed because fieldvalues(a) are empty, but we may need to recurse depending on
# the element type
function default_equality(a::T, b::T; kwargs...) where {T<:AbstractArray}
    return all(t -> default_equality(t...; kwargs...), zip(a, b))
end

# Handles extracting values for any dictionary types
function default_equality(a::T, b::T; kwargs...) where {T<:AbstractDict}
    return all(t -> default_equality(t...; kwargs...), zip(values(a), values(b)))
end

# NOTE: May want to make the equality function a kwarg in the future.
function test_flatten_interface(x::T; check_inferred::Bool=true) where {T}
    @testset "flatten($T)" begin
        # Checks default eltype still works and ensure that
        # basic functionality is implemented.
        v, unflatten = flatten(x)
        @test typeof(v) === Vector{Float64}
        @test default_equality(x, unflatten(v))
        @test unflatten(v) isa T

        # Check that everything infers properly.
        check_inferred && @inferred flatten(x)

        # Test with different precisions
        @testset "Float64" begin
            _v, _unflatten = flatten(Float64, x)
            @test typeof(_v) === Vector{Float64}
            @test _v == v
            @test default_equality(x, unflatten(_v))
            @test _unflatten(_v) isa T

            # Check that everything infers properly.
            check_inferred && @inferred flatten(Float64, x)
        end
        @testset "Float32" begin
            _v, _unflatten = flatten(Float32, x)
            @test typeof(_v) === Vector{Float32}
            @test default_equality(x, _unflatten(_v); atol=1e-5)
            @test _unflatten(_v) isa T

            # Check that everything infers properly.
            check_inferred && @inferred flatten(Float32, x)
        end
        @testset "Float16" begin
            _v, _unflatten = flatten(Float16, x)
            @test typeof(_v) === Vector{Float16}
            @test default_equality(x, _unflatten(_v); atol=1e-2)
            @test _unflatten(_v) isa T

            # Check that everything infers properly.
            check_inferred && @inferred flatten(Float16, x)
        end
    end

    return nothing
end

function test_parameter_interface(x; check_inferred::Bool=true)

    # Parameters need to be flatten-able.
    test_flatten_interface(x; check_inferred=check_inferred)

    # Run this to make sure that it doesn't error.
    value(x)

    if check_inferred
        @inferred value(x)
    end
    return nothing
end

end # module


text:
using Test
using StableRNGs
using ComplexMixtures, PDBTools
using Random
const CM = ComplexMixtures

include("./namd.jl")
include("./namd_chemfiles.jl")
include("./gromacs.jl")
include("./pdb.jl")
include("./merge.jl")


text:
# Geometry which displays points at given (x, y) positions.
immutable PointGeometry <: Gadfly.GeometryElement
end

const point = PointGeometry

function element_aesthetics(::PointGeometry)
    [:x, :y, :size, :color]
end

# Generate a form for a point geometry.
#
# Args:
#   geom: point geometry.
#   theme: the plot's theme.
#   aes: aesthetics.
#
# Returns:
#   A compose Form.
#
function render(geom::PointGeometry, theme::Gadfly.Theme, aes::Gadfly.Aesthetics)
    Gadfly.assert_aesthetics_defined("Geom.point", aes, :x, :y)
    Gadfly.assert_aesthetics_equal_length("Geom.point", aes,
                                          element_aesthetics(geom)...)

    default_aes = Gadfly.Aesthetics()
    default_aes.color = PooledDataArray(RGBA{Float32}[theme.default_color])
    default_aes.size = Measure[theme.default_point_size]
    aes = inherit(aes, default_aes)

    lw_hover_scale = 10
    lw_ratio = theme.line_width / aes.size[1]

    aes_x, aes_y = concretize(aes.x, aes.y)

    ctx = compose!(
        context(),
        circle(aes.x, aes.y, aes.size),
        fill(aes.color),
        linewidth(theme.highlight_width))

    if aes.color_key_continuous != nothing && aes.color_key_continuous
        compose!(ctx,
            stroke(map(theme.continuous_highlight_color, aes.color)))
    else
        compose!(ctx,
            stroke(map(theme.discrete_highlight_color, aes.color)),
            svgclass([svg_color_class_from_label(escape_id(aes.color_label([c])[1]))
                      for c in aes.color]))
    end

    return compose!(context(order=4), svgclass("geometry"), ctx)
end


text:
# ------------------------------------------------------------------
# Licensed under the ISC License. See LICENCE in the project root.
# ------------------------------------------------------------------

"""
    alr(c)

Additive log-ratio transformation of composition `c`.
"""
alr(c::Composition{D}) where {D} = log.(c.parts[1:D-1] ./ c.parts[D])

"""
    alrinv(x)

Inverse alr for coordinates `x`.
"""
alrinv(x) = Composition(𝓒(vcat(exp.(x), 1.)))

"""
    clr(c)

Centered log-ratio transformation of composition `c`.
"""
clr(c::Composition{D}) where {D} = log.(c.parts ./ geomean(c.parts))

"""
    clrinv(x)

Inverse clr for coordinates `x`.
"""
clrinv(x) = Composition(𝓒(exp.(x)))

"""
    ilr(c)

Isometric log-ratio transformation of composition `c`.
"""
function ilr(c::Composition{D}) where {D}
  # TODO
end

"""
    ilrinv(x)

Inverse ilr for coordinates `x`.
"""
function ilrinv(x)
  # TODO
end


text:
# This file is a part of JuliaFEM.
# License is MIT: see https://github.com/JuliaFEM/Materials.jl/blob/master/LICENSE

@with_kw mutable struct IdealPlasticDriverState <: AbstractMaterialState
    time :: Float64 = zero(Float64)
    strain :: SymmetricTensor{2,3} = zero(SymmetricTensor{2,3,Float64})
end

@with_kw struct IdealPlasticParameterState <: AbstractMaterialState
    youngs_modulus :: Float64 = zero(Float64)
    poissons_ratio :: Float64 = zero(Float64)
    yield_stress :: Float64 = zero(Float64)
end

@with_kw struct IdealPlasticVariableState <: AbstractMaterialState
    stress :: SymmetricTensor{2,3} = zero(SymmetricTensor{2,3,Float64})
    plastic_strain :: SymmetricTensor{2,3} = zero(SymmetricTensor{2,3,Float64})
    cumeq :: Float64 = zero(Float64)
    jacobian :: SymmetricTensor{4,3} = zero(SymmetricTensor{4,3,Float64})
end

@with_kw mutable struct IdealPlastic <: AbstractMaterial
    drivers :: IdealPlasticDriverState = IdealPlasticDriverState()
    ddrivers :: IdealPlasticDriverState = IdealPlasticDriverState()
    variables :: IdealPlasticVariableState = IdealPlasticVariableState()
    variables_new :: IdealPlasticVariableState = IdealPlasticVariableState()
    parameters :: IdealPlasticParameterState = IdealPlasticParameterState()
    dparameters :: IdealPlasticParameterState = IdealPlasticParameterState()
end

function integrate_material!(material::IdealPlastic)
    p = material.parameters
    v = material.variables
    dd = material.ddrivers
    d = material.drivers

    E = p.youngs_modulus
    nu = p.poissons_ratio
    mu = E/(2.0*(1.0+nu))
    lambda = E*nu/((1.0+nu)*(1.0-2.0*nu))
    R0 = p.yield_stress

    # @unpack strain, time = d
    dstrain = dd.strain
    dtime = dd.time
    @unpack stress, plastic_strain, cumeq, jacobian = v

    jacobian = isotropic_elasticity_tensor(lambda, mu)
    stress += dcontract(jacobian, dstrain)
    seff_dev = dev(stress)
    stress_v = sqrt(1.5)*norm(seff_dev)
    f = stress_v - R0

    if f>0.0
        n = 1.5*seff_dev/stress_v
        dp = 1.0/(3.0*mu)*(stress_v - R0)
        plastic_strain += dp*n
        cumeq += dp

        stress -= dcontract(jacobian, dp*n)
        delta(i,j) = i==j ? 1.0 : 0.0
        II = SymmetricTensor{4,3}((i,j,k,l) -> 0.5*(delta(i,k)*delta(j,l)+delta(i,l)*delta(j,k)))
        P = II - 1.0/3.0*SymmetricTensor{4,3}((i,j,k,l) -> delta(i,j)*delta(k,l))
        EE = II + dp*dcontract(jacobian, 1.5*P/R0 - otimes(n,n)/R0)
        ED = dcontract(inv(EE),jacobian)
        jacobian = ED - otimes(dcontract(ED, n), dcontract(n, ED))/dcontract(dcontract(n, ED), n)
    end
    variables_new = IdealPlasticVariableState(stress=stress,
                                              plastic_strain=plastic_strain,
                                              cumeq=cumeq,
                                              jacobian=jacobian)
    material.variables_new = variables_new
end


text:
# -*- coding: utf-8 -*-
import Revise
import PyCall
import Revise
import GLRenderer as GL
import ThreeDP3 as T
import Images as I
import MiniGSG as S
import NearestNeighbors as NN
import PoseComposition: Pose, IDENTITY_POSE, IDENTITY_ORN
using Gen
import Rotations as R
import DataStructures as DS
using Printf
import MeshCatViz

# +
YCB_DIR = "/home/nishadg/mcs/ThreeDVision.jl/data/ycbv2"
world_scaling_factor = 100.0
id_to_cloud, id_to_shift, id_to_box  = T.load_ycbv_models_adjusted(YCB_DIR, world_scaling_factor);
all_ids = sort(collect(keys(id_to_cloud)));
id_to_mesh = Dict()
resolution = 0.5

for id in all_ids
    cloud = id_to_cloud[id]
    v,n,f = GL.mesh_from_voxelized_cloud(GL.voxelize(cloud, resolution), resolution)
    id_to_mesh[id] = (v,n,f)
end
# -

p = PyCall.pyimport("pybullet")
np = PyCall.pyimport("numpy")
pybullet_data = PyCall.pyimport("pybullet_data")

p.connect(p.GUI, options="--background_color_red=1.0 --background_color_green=1.0 --background_color_blue=1.0")
p.setAdditionalSearchPath(pybullet_data.getDataPath())

# +
id = 4
filepath = "/tmp/$(id).obj"

(v,n,f) = id_to_mesh[id]
@show collect(size.([v,n,f]))
open(filepath, "w") do file
    for ver in eachrow(v)
        @printf(file, "v %.4f %.4f %.4f\n", ver[1],ver[2],ver[3])
    end
    for ver in eachrow(f)
        @printf(file, "f %d %d %d\n", (ver .+ 1)...)
    end
end

parent_box = S.Box(40.0,40.0,0.1)
parent_pose = Pose(0.0, 0.0, 0.0)
parent_face = :top

# +
child_face = :right
child_box = id_to_box[id]
(x,y,ang) = 0.0,0.0,(2*π - π/3)
contact = S.ShapeContact(parent_face, Real[], child_face, Real[], S.PlanarContact(x,y,ang))

pose = parent_pose * S.getRelativePoseFromContact(parent_box, child_box, contact);
rot = R.QuatRotation(pose.orientation)
color = I.colorant"red"

# +
p.resetSimulation()
planeOrn = [0,0,0,1]#p.getQuaternionFromEuler([0.3,0,0])
planeId = p.loadURDF("plane.urdf", [0,0,-1.0],planeOrn)
obj_path = "/tmp/$(id).obj"
aruco_cube_shape_id = p.createVisualShape(
        shapeType=p.GEOM_MESH,
        rgbaColor=[color.r,color.g,color.b,1.0],
        fileName=obj_path)
aruco_cube_collision_id = p.createCollisionShape(
        shapeType=p.GEOM_MESH,
        fileName=obj_path)
aruco_id = p.createMultiBody(
    baseVisualShapeIndex=aruco_cube_shape_id,
    baseCollisionShapeIndex=aruco_cube_collision_id,
    basePosition=np.array(pose.pos),
    baseOrientation=[rot.q.v1,rot.q.v2,rot.q.v3,rot.q.s],
    baseMass = 0.5,
)
gravZ=-10
p.setGravity(0, 0, gravZ)
p.setTimeStep(0.1)

p.setRealTimeSimulation(1)

# -

p.disconnect()

p.getBasePositionAndOrientation(aruco_id)

while Bool(p.isConnected())
  p.setGravity(0,0,gravZ)
  sleep(1/240.)
end

# +

@gen function slam_model(T, wall_colors)
    map ~ map_prior(wall_colors)

    prev_pose = nothing
    for t in 1:T
        if t == 1
            pose = {(t,:pose)} ~ pose_uniform(map)
        else
            pose = {(t,:pose)} ~ motion_model(map, prev_pose)
        end
        prev_pose = pose

        rgb_render, depth_render = render(map, pose)
        {(t,:obs_rgb)} = color_likelihood(rgb_render)
        {(t,:obs_depth)} = depth_likelihood(depth_render)
    end
end

# -

import Revise
import PyCall
import GLRenderer as GL
import ThreeDP3 as T
import Images as I
import MiniGSG as S
import NearestNeighbors as NN
import PoseComposition: Pose, IDENTITY_POSE, IDENTITY_ORN
using Gen
import Rotations as R
import DataStructures as DS
import MeshCatViz

import MeshCatViz
MeshCatViz.setup_visualizer()

YCB_DIR = "/home/nishadg/mcs/ThreeDVision.jl/data/ycbv2"
world_scaling_factor = 100.0
id_to_cloud, id_to_shift, id_to_box  = T.load_ycbv_models_adjusted(YCB_DIR, world_scaling_factor);
all_ids = sort(collect(keys(id_to_cloud)));

IDX =400
@show T.get_ycb_scene_frame_id_from_idx(YCB_DIR,IDX)
gt_poses, ids, rgb_image, gt_depth_image, cam_pose, original_camera = T.load_ycbv_scene_adjusted(
    YCB_DIR, IDX, world_scaling_factor, id_to_shift
);
camera = T.scale_down_camera(original_camera, 6)
@show camera
img = I.colorview(I.Gray, gt_depth_image ./ maximum(gt_depth_image))
I.colorview(I.RGB, permutedims(Float64.(rgb_image)./255.0, (3,1,2)))

# +
resolution = 0.5

renderer = GL.setup_renderer(camera, GL.DepthMode())
for id in all_ids
    cloud = id_to_cloud[id]
    v,n,f = GL.mesh_from_voxelized_cloud(GL.voxelize(cloud, resolution), resolution)
    GL.load_object!(renderer, v, f)
end
depth_image = GL.gl_render(
    renderer, ids, gt_poses, T.IDENTITY_POSE)
depth_image[depth_image .== 50000.0] .= 200.0

function get_cloud_from_ids_and_poses(ids, poses, camera_pose)
    depth_image = GL.gl_render(renderer, ids, poses, camera_pose)
    cloud = GL.flatten_point_cloud(GL.depth_image_to_point_cloud(depth_image, camera))
    cloud = cloud[:,cloud[3,:] .< (camera.far - 1.0)]
    if size(cloud)[2] == 0
        cloud = zeros(3,1)
    end
    cloud
end

img = I.colorview(I.Gray, depth_image ./ maximum(depth_image))
# -

import PyCall
p = PyCall.pyimport("pybullet")
np = PyCall.pyimport("numpy")

p.connect(p.GUI, options="--background_color_red=1.0 --background_color_green=1.0 --background_color_blue=1.0")

soup_can_dir = "/home/nishadg/mcs/ThreeDVision.jl/data/ycbv2/models/005_tomato_soup_can"
aruco_cube_shape_id = p.createVisualShape(
        shapeType=p.GEOM_MESH,
        fileName=joinpath(soup_can_dir, "textured_simple.obj"))
aruco_cube_collision_id = p.createCollisionShape(
        shapeType=p.GEOM_MESH,
        fileName=joinpath(soup_can_dir, "textured_simple.obj"))
aruco_id = p.createMultiBody(
    baseVisualShapeIndex=aruco_cube_shape_id,
    baseCollisionShapeIndex=aruco_cube_collision_id,
    basePosition=np.array([0,0,0]),
    baseOrientation=[0,0,0,1],
)

get_cloud_func(id,p) = T.move_points_to_frame_b(T.voxelize(get_cloud_from_ids_and_poses(
                                [id], [p], cam_pose),0.5), cam_pose)

blobs = T.get_entities_from_assignment(voxelized_obs_cloud,
    T.dbscan_cluster(voxelized_obs_cloud, radius=0.8))

obs_blob = blobs[2]
MeshCatViz.reset_visualizer()
MeshCatViz.viz(obs_blob)

# +
parent_box = table_box
parent_pose = Pose(0.0, 0.0, -0.5)
parent_face = :top

c1_tree = NN.KDTree(obs_blob)
id_to_face_cloud_and_score =  DS.DefaultDict{Any,Vector{Any}}(()->[])

for id in all_ids
    print(id," ")
    for child_face in S.BOX_SURFACE_IDS
        child_box = id_to_box[id]
        (x,y,ang) = T.icp_project_to_planar_contact(
            obs_blob, p -> get_cloud_func(id,p), parent_pose, parent_box, parent_face,
            child_box, child_face,
            0.0,0.0,0.0;
            c1_tree= c1_tree, outer_iterations=2
        )

        contact = S.ShapeContact(parent_face, Real[], child_face, Real[], S.PlanarContact(x,y,ang))
        p = parent_pose * S.getRelativePoseFromContact(parent_box, child_box, contact)
        c = get_cloud_func(id,p)
        score = Gen.logpdf(
            T.uniform_mixture_from_template,
            obs_blob, c, 0.01, 0.5*2,(-1000.0, 1000.0, -1000.0,1000.0,-1000.0,1000.0)
        )

        push!(id_to_face_cloud_and_score[id], (face=child_face, cloud=c, score=score, pose=p, contact_params=(x,y,ang)))
    end
#     println(size(id_to_face_cloud_and_score[id]))
    sort!(id_to_face_cloud_and_score[id],by=x->x.score,rev=true)
end

# +

c = id_to_face_cloud_and_score[14][1].cloud

MeshCatViz.reset_visualizer()
MeshCatViz.viz(obs_blob; color=I.colorant"black", channel_name=:obs_cloud)
MeshCatViz.viz(c; color=I.colorant"red", channel_name=:gen_cloud)

# +
scores = Dict([id => id_to_face_cloud_and_score[id][1].score for id in all_ids]...)
best_score_id = argmax(scores)
@show best_score_id
c = id_to_face_cloud_and_score[best_score_id][1].cloud

MeshCatViz.reset_visualizer()
MeshCatViz.viz(obs_blob; color=I.colorant"black", channel_name=:obs_cloud)
MeshCatViz.viz(c; color=I.colorant"red", channel_name=:gen_cloud)

# +

id_and_id_to_face_cloud_and_score =  DS.DefaultDict{Any,Vector{Any}}(()->[])

for id_1 in all_ids
    print(id_1," ")

    data = id_to_face_cloud_and_score[id_1][1]

    unexplained_idxs = T.get_unexplained_points(obs_blob, data.cloud)
    unexplained_points = obs_blob[:, unexplained_idxs]

    c1_tree = NN.KDTree(unexplained_points)

    parent_pose = data.pose
    parent_face = T.opposite_face(data.face)
    parent_box = id_to_box[id_1]

    for id in all_ids
        for child_face in S.BOX_SURFACE_IDS
            child_box = id_to_box[id]

            (x,y,ang) = T.icp_project_to_planar_contact(
                unexplained_points, p -> get_cloud_func(id,p), parent_pose, parent_box, parent_face,
                child_box, child_face,
                0.0,0.0,0.0;
                c1_tree= c1_tree
            )

            contact = S.ShapeContact(parent_face, Real[], child_face, Real[], S.PlanarContact(x,y,ang))
            p = parent_pose * S.getRelativePoseFromContact(parent_box, child_box, contact)
            c = get_cloud_func(id,p)
            score = Gen.logpdf(
                T.uniform_mixture_from_template,
                unexplained_points, c, 0.01, 0.5*2,(-1000.0, 1000.0, -1000.0,1000.0,-1000.0,1000.0)
            )

            push!(id_and_id_to_face_cloud_and_score[(id_1,id)],
                (face=child_face, cloud=c, score=score, pose=p, contact_params=(x,y,ang)))
        end
        sort!(id_and_id_to_face_cloud_and_score[(id_1,id)],by=x->x.score,rev=true)
    end
end

# -

id_and_id_to_score = Dict()
for id_1 in all_ids
    for id in all_ids
        pose_1 = id_to_face_cloud_and_score[id_1][1].pose
        pose_2 = id_and_id_to_face_cloud_and_score[(id_1,id)][1].pose

        c = T.move_points_to_frame_b(T.voxelize(get_cloud_from_ids_and_poses(
                                [id_1,id], [pose_1, pose_2], cam_pose), 0.5), cam_pose)

        score = Gen.logpdf(
            T.uniform_mixture_from_template,
            obs_blob, c, 0.01, 0.5*2,(-1000.0, 1000.0, -1000.0,1000.0,-1000.0,1000.0)
        )
        id_and_id_to_score[(id_1,id)] = score
    end
end

# +
(id_1,id) = argmax(id_and_id_to_score)
@show (id_1,id)
pose_1 = id_to_face_cloud_and_score[id_1][1].pose
pose_2 = id_and_id_to_face_cloud_and_score[(id_1,id)][1].pose

c = T.move_points_to_frame_b(T.voxelize(get_cloud_from_ids_and_poses(
                        [id_1,id], [pose_1, pose_2], cam_pose),0.5), cam_pose)
MeshCatViz.viz(obs_blob; color=I.colorant"black", channel_name=:obs_cloud)
MeshCatViz.viz(c; color=I.colorant"red", channel_name=:gen_cloud)

# +
id_1 = 13
    data = id_to_face_cloud_and_score[id_1][1]

    unexplained_idxs = T.get_unexplained_points(obs_blob, data.cloud)
    unexplained_points = obs_blob[:, unexplained_idxs]

    c1_tree = NN.KDTree(unexplained_points)

    parent_pose = data.pose
    parent_face = T.opposite_face(data.face)
    parent_box = id_to_box[id_1]

id = 3
child_face=:left
(x,y,ang) = T.icp_project_to_planar_contact(
    unexplained_points, p -> get_cloud_func(id,p), parent_pose, parent_box, parent_face,
    child_box, child_face,
    0.0,0.0,0.0;
    c1_tree= c1_tree, outer_iterations=5, iterations=10
)

contact = S.ShapeContact(parent_face, Real[], child_face, Real[], S.PlanarContact(x,y,ang))
p = parent_pose * S.getRelativePoseFromContact(parent_box, child_box, contact)
c = get_cloud_func(id,p)

MeshCatViz.reset_visualizer()
MeshCatViz.viz(unexplained_points; color=I.colorant"black", channel_name=:obs_cloud)
MeshCatViz.viz(c; color=I.colorant"red", channel_name=:gen_cloud)

# +

c = id_and_id_to_face_cloud_and_score[(13,3)][1].cloud

MeshCatViz.reset_visualizer()
MeshCatViz.viz(obs_blob; color=I.colorant"black", channel_name=:obs_cloud)
MeshCatViz.viz(c; color=I.colorant"red", channel_name=:gen_cloud)
# -

unexplained_idxs = T.get_unexplained_points(obs_blob, data.cloud)
unexplained_points = obs_blob[:, unexplained_idxs]
MeshCatViz.viz(unexplained_points; color=I.colorant"red", channel_name=:gen_cloud)

pair = (13,:right)
@show id_and_face_to_score[pair]
c = id_and_face_to_cloud[pair]
c = id_and_face_to_cloud[best_pair]

MeshCatViz.reset_visualizer()
MeshCatViz.viz(obs_blob; color=I.colorant"black", channel_name=:obs_cloud)
MeshCatViz.viz(c; color=I.colorant"red", channel_name=:gen_cloud)

ids

clouds = [[
    let
        p = T.icp_object_pose(IDENTITY_POSE, obs_blob,
                              p-> T.move_points_to_frame_b(T.voxelize(get_cloud_from_ids_and_poses(
                                [id], [p], cam_pose),0.5), cam_pose), true)

        c = T.move_points_to_frame_b(T.voxelize(
                get_cloud_from_ids_and_poses([id], [p], cam_pose), 0.5), cam_pose)
    end
    for _ in 1:10
] for id in all_ids];
scores = [
    maximum([
        Gen.logpdf(T.uniform_mixture_from_template,obs_blob, c, 0.01, 0.5*2,(-1000.0, 1000.0, -1000.0,1000.0,-1000.0,1000.0))
        for c in x
    ])
    for x in clouds
]

best_id = argmax(scores)
@show best_id
c = clouds[best_id][1]
MeshCatViz.viz(c; color=I.colorant"red", channel_name=:h1)
MeshCatViz.viz(obs_blob; color=I.colorant"black", channel_name=:h2)

ids

ids

c

MeshCatViz.reset_visualizer()
MeshCatViz.viz(T.get_obs_cloud(trace); color=I.colorant"black", channel_name=:obs_cloud)
MeshCatViz.viz(T.get_gen_cloud(trace); color=I.colorant"red", channel_name=:gen_cloud)

traces = [trace];

for i in 1:length(ids)
    if !T.isFloating(T.get_structure(trace),i)
        continue
    end
    pose_addr = T.floating_pose_addr(i)
    for _ in 1:50
        trace, acc = T.drift_move(trace, pose_addr, 0.5, 10.0)
        if acc traces = push!(traces, trace) end

        trace, acc = T.drift_move(trace, pose_addr, 1.0, 100.0)
        if acc traces = push!(traces, trace) end
        trace, acc = T.drift_move(trace, pose_addr, 1.5, 1000.0)
        if acc traces = push!(traces, trace) end
        trace, acc = T.drift_move(trace, pose_addr, 0.1, 1000.0)
        if acc traces = push!(traces, trace) end
        trace, acc = T.drift_move(trace, pose_addr, 0.1, 100.0)
        if acc traces = push!(traces, trace) end
        trace, acc = T.drift_move(trace, pose_addr, 0.5, 5000.0)
        if acc traces = push!(traces, trace) end
        trace, acc = T.pose_flip_move(trace, pose_addr, 1, 1000.0)
        if acc traces = push!(traces, trace) end
        trace, acc = T.pose_flip_move(trace, pose_addr, 2, 1000.0)
        if acc traces = push!(traces, trace) end
        trace, acc = T.pose_flip_move(trace, pose_addr, 3, 1000.0)
        if acc traces = push!(traces, trace) end
    end
end

CRAZY_POSE = Pose(-100.0 * ones(3), T.IDENTITY_ORIENTATION)
for i in 1:length(ids)
    if !T.isFloating(T.get_structure(trace),i)
        continue
    end
    addr = T.floating_pose_addr(i)

    t, = Gen.update(trace, Gen.choicemap(T.floating_pose_addr(i) => CRAZY_POSE))
    valid = T.get_unexplained_obs_cloud(t)

    p = T.icp_object_pose(trace[addr], trace[:obs][:,valid],
                          p-> get_cloud_from_ids_and_poses(
                            [ids[i]], [p], cam_pose), false)
    for _ in 1:3
        trace, acc = T.pose_mixture_move(trace, addr, [trace[addr], p], [0.5, 0.5], 0.001, 5000.0)
        if acc traces = push!(traces, trace) end
    end
end

MeshCatViz.reset_visualizer()
MeshCatViz.viz(T.get_obs_cloud(trace); color=I.colorant"black", channel_name=:obs_cloud)
MeshCatViz.viz(T.get_gen_cloud(trace); color=I.colorant"red", channel_name=:gen_cloud)

depth_image = GL.gl_render(
    renderer, ids, T.get_poses(trace)[1:end-1], cam_pose)
depth_image[depth_image .== 50000.0] .= 200.0
img = I.colorview(I.Gray, depth_image ./ maximum(depth_image))

# +
renderer_texture = GL.setup_renderer(original_camera, GL.TextureMode())
obj_paths = T.load_ycb_model_obj_file_paths(YCB_DIR)
texture_paths = T.load_ycb_model_texture_file_paths(YCB_DIR)
for id in all_ids
    v,n,f,t = renderer_texture.gl_instance.load_obj_parameters(
        obj_paths[id]
    )
    v = v * world_scaling_factor
    v .-= id_to_shift[id]'

    GL.load_object!(renderer_texture, v, n, f, t,
        texture_paths[id]
    )
end

# -

rgb_image, depth_image = GL.gl_render(
    renderer_texture, ids, T.get_poses(trace)[1:end-1], cam_pose)
I.colorview(I.RGBA, permutedims(rgb_image,(3,1,2)))


text:
fb(i::Integer) = "Fizz" ^ (i % 3 == 0) * "Buzz" ^ (i % 5 == 0) * dec(i) ^ (i % 3 != 0 && i % 5 != 0)
for i in 1:100 println(fb(i)) end

max_stars_repo_name: ethansaxenian/RosettaDecode

max_issues_repo_name: ethansaxenian/RosettaDecode

max_forks_repo_name: ethansaxenian/RosettaDecode


text:
export read_data, read_data!

function read_data_seisio!(S::SeisData, filestr::String, memmap::Bool, v::Integer)
  S_in = rseis(filestr, memmap=memmap)
  L = length(S_in)
  for i in 1:L
    if typeof(S_in[i]) == SeisChannel
      push!(S, S_in[i])
      break
    elseif typeof(S_in[i]) <: GphysChannel
      C = convert(SeisChannel, S_in[i])
      push!(S, C)
      break
    elseif typeof(S_in[i]) == SeisData
      append!(S, S_in[i])
      break
    elseif typeof(S_in[i]) <: GphysData
      S1 = convert(SeisData, S_in[i])
      append!(S, S1)
      break
    elseif typeof(S_in[i]) == SeisEvent
      (v > 0) && @warn(string("Obj ", i, " is type SeisEvent: only reading :data field"))
      S1 = convert(SeisData, getfield(S_in[i], :data))
      append!(S, S1)
      break
    else
      (v > 0) && @warn(string("Obj ", i, " skipped (Type incompatible with read_data)"))
    end
  end
  return nothing
end

@doc """
    S = read_data(fmt, filestr [, keywords])
    read_data!(S, fmt, filestr [, keywords])

Read data in file format `fmt` matching file pattern `filestr` into SeisData object `S`.

    S = read_data(filestr [, keywords])
    read_data!(S, filestr [, keywords])

Read from files matching file pattern `filestr` into SeisData object `S`.
Calls `guess(filestr)` to identify the file type based on the first file
matching pattern `filestr`. Much slower than manually specifying file type.

* Formats: ah1, ah2, bottle, geocsv, geocsv.slist, lennartz, mseed, passcal, suds, sac, segy, seisio, slist, uw, win32
* Keywords: cf, full, jst, memmap, nx_add, nx_new, strict, swap, v, vl

This function is fully described in the official documentation at https://seisio.readthedocs.io/ in section **Time-Series Files**.

See also: `SeisIO.KW`, `get_data`, `guess`, `rseis`
""" read_data!
function read_data!(S::GphysData, fmt::String, fpat::Union{String, Array{String,1}};
  cf      ::String  = "",                 # win32 channel info file
  full    ::Bool    = false,              # full SAC/SEGY hdr
  jst     ::Bool    = true,               # are sample times JST (UTC+9)?
  ll      ::UInt8   = 0x00,               # auto-set location field in :id?
  memmap  ::Bool    = false,              # use mmap? (DANGEROUS)
  nx_add  ::Int64   = KW.nx_add,          # append nx_add to overfull channels
  nx_new  ::Int64   = KW.nx_new,          # new channel samples
  strict  ::Bool    = false,              # strict channel matching
  swap    ::Bool    = false,              # do byte swap?
  v       ::Integer = KW.v,               # verbosity level
  vl      ::Bool    = false               # verbose logging
  )

  # Variables for tracking changes
  N             = S.n
  fpat_is_array = isa(fpat, Array{String, 1})
  fmt_is_seisio = (fmt == "seisio")
  opt_strings   = ""
  last_src      = zeros(Int64, S.n)
  nx            = Array{Int64, 1}(undef, S.n)
  for i in 1:S.n
    nx[i] = length(S.x[i])
  end

  if fpat_is_array
    one_file = false
    files = String[]
    for f in fpat
      ff = abspath(f)
      if safe_isfile(ff)
        push!(files, ff)
      else
        append!(files, ls(ff))
      end
    end
  else
    filestr = abspath(fpat)
    one_file = safe_isfile(filestr)
    if one_file == false
      files = ls(filestr)
    end
  end

  if fmt == "sac"
    if one_file
      read_sac_file!(S, filestr, full, memmap, strict)
    else
      for (j, fname) in enumerate(files)
        read_sac_file!(S, fname, full, memmap, strict)
        track_src!(S, j, nx, last_src)
      end
    end

  elseif fmt_is_seisio
    if one_file
      read_data_seisio!(S, filestr, memmap, v)
    else
      for (j, fname) in enumerate(files)
        read_data_seisio!(S, fname, memmap, v)
        track_src!(S, j, nx, last_src)
      end
    end

  elseif ((fmt == "miniseed") || (fmt == "mseed"))
    setfield!(BUF, :swap, swap)
    if one_file
      read_mseed_file!(S, filestr, nx_new, nx_add, memmap, strict, v)
    else
      for (j, fname) in enumerate(files)
        read_mseed_file!(S, fname, nx_new, nx_add, memmap, strict, v)
        track_src!(S, j, nx, last_src)
      end
    end
    opt_strings = string("swap = ", swap,
                          ", nx_new = ", nx_new,
                          ", nx_add = ", nx_add)

# ============================================================================
# Data formats that aren't SAC, SEISIO, or SEED begin here and are alphabetical
# by first KW

  elseif fmt == "ah1"
    if one_file
      read_ah1!(S, filestr, full, memmap, strict, v)
    else
      for (j, fname) in enumerate(files)
        read_ah1!(S, fname, full, memmap, strict, v)
        track_src!(S, j, nx, last_src)
      end
    end
    opt_strings = string("full = ", full)

  elseif fmt == "ah2"
    if one_file
      read_ah2!(S, filestr, full, memmap, strict, v)
    else
      for (j, fname) in enumerate(files)
        read_ah2!(S, fname, full, memmap, strict, v)
        track_src!(S, j, nx, last_src)
      end
    end
    opt_strings = string("full = ", full)

  elseif fmt == "bottle"
    read_bottle!(S, filestr, nx_new, nx_add, memmap, strict, v)
    opt_strings = string("nx_new = ", nx_new,
                              ", nx_add = ", nx_add)

  elseif fmt in ("geocsv", "geocsv.tspair", "geocsv", "geocsv.slist")
    tspair = (fmt == "geocsv" || fmt == "geocsv.tspair")
    if one_file
      read_geocsv_file!(S, filestr, tspair, memmap)
    else
      for (j, fname) in enumerate(files)
        read_geocsv_file!(S, fname, tspair, memmap)
        track_src!(S, j, nx, last_src)
      end
    end

  elseif fmt == "segy" || fmt == "passcal"
    passcal = fmt == "passcal"

    if one_file
      read_segy_file!(S, filestr, ll, passcal, memmap, full, swap, strict)
    else
      for (j, fname) in enumerate(files)
        read_segy_file!(S, fname, ll, passcal, memmap, full, swap, strict)
        track_src!(S, j, nx, last_src)
      end
    end
    opt_strings = string("full = ", full,
                          ", ll = ", ll,
                          ", swap = ", swap)

  elseif fmt == "slist" || fmt == "lennartz"
    lennartz = fmt == "lennartz"
    if one_file
      read_slist!(S, filestr, lennartz, memmap, strict, v)
    else
      for (j, fname) in enumerate(files)
        read_slist!(S, fname, lennartz, memmap, strict, v)
        track_src!(S, j, nx, last_src)
      end
    end

  elseif fmt == "suds"
    if one_file
      append!(S, SUDS.read_suds(filestr, memmap=memmap, full=full, v=v))
    else
      for (j, fname) in enumerate(files)
        append!(S, SUDS.read_suds(fname, memmap=memmap, full=full, v=v))
        track_src!(S, j, nx, last_src)
      end
    end
    opt_strings, string("full = ", full)

  elseif fmt == "uw"
    if one_file
      UW.uwdf!(S, filestr, full, memmap, strict, v)
    else
      for (j, fname) in enumerate(files)
        UW.uwdf!(S, fname, full, memmap, strict, v)
        track_src!(S, j, nx, last_src)
      end
    end
    opt_strings = string("full = ", full)

  elseif fmt == "win32" || fmt =="win"
    if isa(fpat, Array{String, 1})
      for (j, f) in enumerate(fpat)
        readwin32!(S, f, cf, jst, nx_new, nx_add, memmap, strict, v)

        # here the list of files is already expanded, so this is accurate
        track_src!(S, j, nx, last_src)
      end
    else
      readwin32!(S, filestr, cf, jst, nx_new, nx_add, memmap, strict, v)
    end
    opt_strings = string("cf = \"", abspath(cf), "\"",
                              ", jst = ", jst,
                              ", nx_new = ", nx_new,
                              ", nx_add = ", nx_add)

  else
    error("Unknown file format!")
  end

  # ===================================================================
  # logging
  if !fmt_is_seisio
    if isempty(opt_strings)
      opt_strings = string("v = ", v, ", vl = ", vl)
    else
      opt_strings *= string(", v = ", v, ", vl = ", vl)
    end

    # Update all channels
    to_note = Int64[]
    if fpat_is_array
      for i in 1:S.n
        if last_src[i] > 0
          S.src[i] = files[last_src[i]]
          push!(to_note, i)
        end
      end
    else

      # Update existing channels first
      for i in 1:N
        if length(S.x[i]) > nx[i]
          S.src[i] = filestr
          push!(to_note, i)
        end
      end

      # Do new channels
      chan_view = view(S.src, N+1:S.n)
      fill!(chan_view , filestr)

      # Note new source
      append!(to_note, collect(N+1:S.n))

      # note filestr used in read
      if vl == false
          fread_note!(S, to_note, "read_data!", fmt, filestr, opt_strings)

      # For verbose logging, note all files used in the read
      elseif one_file == false
        files = ls(filestr)
      end
    end

    # For verbose logging, any changed channel logs all files to :notes
    if vl && (one_file == false)
      for f in files
        fread_note!(S, to_note, "read_data!", fmt, f, opt_strings)
      end
    end
  end

  # ===================================================================
  return nothing
end

@doc (@doc read_data!)
function read_data(fmt::String, filestr::Union{String, Array{String, 1}};
  full    ::Bool    = false,              # full SAC/SEGY hdr
  cf      ::String  = "",                 # win32 channel info file
  jst     ::Bool    = true,               # are sample times JST (UTC+9)?
  ll      ::UInt8   = 0x00,               # auto-set location field in :id?
  memmap  ::Bool    = false,              # use mmap? (DANGEROUS)
  nx_add  ::Int64   = KW.nx_add,          # append nx_add to overfull channels
  nx_new  ::Int64   = KW.nx_new,          # new channel samples
  strict  ::Bool    = false,              # strict channel matching
  swap    ::Bool    = false,              # do byte swap?
  v       ::Integer = KW.v,               # verbosity level
  vl      ::Bool    = false               # verbose logging
  )

  S = SeisData()
  read_data!(S, fmt, filestr,
    full    = full,
    cf      = cf,
    jst     = jst,
    ll      = ll,
    memmap  = memmap,
    nx_add  = nx_add,
    nx_new  = nx_new,
    strict  = strict,
    swap    = swap,
    v       = v,
    vl      = vl
    )
  return S
end

function read_data(filestr::Union{String, Array{String, 1}};
  full    ::Bool    = false,              # full SAC/SEGY hdr
  cf      ::String  = "",                 # win32 channel info file
  jst     ::Bool    = true,               # are sample times JST (UTC+9)?
  ll      ::UInt8   = 0x00,               # auto-set location field in :id?
  memmap  ::Bool    = false,              # use mmap? (DANGEROUS)
  nx_add  ::Int64   = KW.nx_add,          # append nx_add to overfull channels
  nx_new  ::Int64   = KW.nx_new,          # new channel samples
  strict  ::Bool    = false,              # strict channel matching
  v       ::Integer = KW.v,               # verbosity level
  vl      ::Bool    = false               # verbose logging
  )

  if isa(filestr, String)
    if safe_isfile(filestr)
      g = guess(filestr)
    else
      files = ls(filestr)
      g = guess(files[1])
    end
  else
    g = guess(filestr[1])
  end
  S = SeisData()
  read_data!(S, g[1], filestr,
    full    = full,
    cf      = cf,
    jst     = jst,
    ll      = ll,
    memmap  = memmap,
    nx_add  = nx_add,
    nx_new  = nx_new,
    strict  = strict,
    swap    = g[2],
    v       = v,
    vl      = vl
    )
  return S
end

function read_data!(S::GphysData, filestr::Union{String, Array{String, 1}};
  full    ::Bool    = false,              # full SAC/SEGY hdr
  cf      ::String  = "",                 # win32 channel info file
  jst     ::Bool    = true,               # are sample times JST (UTC+9)?
  ll      ::UInt8   = 0x00,               # auto-set location field in :id?
  memmap  ::Bool    = false,              # use mmap? (DANGEROUS)
  nx_add  ::Int64   = KW.nx_add,          # append nx_add to overfull channels
  nx_new  ::Int64   = KW.nx_new,          # new channel samples
  strict  ::Bool    = false,              # strict channel matching
  v       ::Integer = KW.v,               # verbosity level
  vl      ::Bool    = false               # verbose logging
  )

  if isa(filestr, String)
    if safe_isfile(filestr)
      g = guess(filestr)
    else
      files = ls(filestr)
      g = guess(files[1])
    end
  else
    g = guess(filestr[1])
  end

  read_data!(S, g[1], filestr,
    full    = full,
    cf      = cf,
    jst     = jst,
    ll      = ll,
    memmap  = memmap,
    nx_add  = nx_add,
    nx_new  = nx_new,
    strict  = strict,
    swap    = g[2],
    v       = v,
    vl      = vl
    )
  return S
end