text:
##########################################################################
# Postprocess results from the simulate function.
function postprocess(raw_results, simulation_params, plot_params)
    methods = get_methods_to_plot(simulation_params, plot_params)

    # Main independent variable
    idp_vals_length = length(simulation_params["independent_variable"][2])

    # Auxiliary independent variables
    if haskey(simulation_params, "aux_independent_variables")
        Naux = length(simulation_params["aux_independent_variables"])
        aux_idp_vals_length = length(simulation_params["aux_independent_variables"][1][2])
        for n = 2:Naux
            aux_idp_vals_length == length(simulation_params["aux_independent_variables"][n][2]) ? nothing : Lumberjack.error("Auxiliary independent variable vectors must have equal length.")
        end
    else
        Naux = 0; aux_idp_vals_length = 1
    end

    # Compact raw results into result matrices
    Ndrops = size(raw_results.simulation_results, 1); Nsim = size(raw_results.simulation_results, 2)
    results = [ method => Dict{ASCIIString, Array{Float64}}() for method in methods ]
    for method_name in methods
        for (result_param,) in plot_params["methods"][method_name]
            if isa(result_param, ASCIIString)
                result_name = result_param

                result_dimensions = size(raw_results[1, 1, 1, 1][method_name][result_name])
            else
                (calculator, calculate_from) = result_param
                result_name = string(string(calculator), "_", calculate_from)

                result_dimensions = size(raw_results[1, 1, 1, 1][method_name][calculate_from])
            end
            result_ranges = [ 1:s for s in result_dimensions ]
            results[method_name][result_name] = Array(Float64, Ndrops, Nsim, idp_vals_length, aux_idp_vals_length, result_dimensions...)
            for Ndrops_idx = 1:Ndrops; for Nsim_idx = 1:Nsim; for idp_vals_idx = 1:idp_vals_length; for aux_idp_vals_idx = 1:aux_idp_vals_length
                if isa(result_param, ASCIIString)
                    result = raw_results[Ndrops_idx, Nsim_idx, idp_vals_idx, aux_idp_vals_idx][method_name][result_name]
                else
                    result = calculator(raw_results[Ndrops_idx, Nsim_idx, idp_vals_idx, aux_idp_vals_idx][method_name][calculate_from], idp_vals_idx)
                end
                results[method_name][result_name][Ndrops_idx, Nsim_idx, idp_vals_idx, aux_idp_vals_idx, result_ranges...] = result
            end; end; end; end
        end
    end

    # Calculate statistics on result matrices
    plot_params["objective"] == :sum && (objective = (r -> sum(r, 5:6)))
    plot_params["objective"] == :mean && (objective = (r -> mean(sum(r, 6), 5)))
    plot_params["objective"] == :min && (objective = (r -> minimum(sum(r, 6), 5)))
    plot_params["objective"] == :none && (objective = (r -> r))

    results_mean = [ method => Dict{ASCIIString, Array{Float64}}() for method in methods ]
    results_var = [ method => Dict{ASCIIString, Array{Float64}}() for method in methods ]
    for method_name in methods
        for (result_param,) in plot_params["methods"][method_name]
            if isa(result_param, ASCIIString)
                result_name = result_param
            else
                (calculator, calculate_from) = result_param
                result_name = string(string(calculator), "_", calculate_from)
            end

            # mean: average over drops and sims
            results_mean[method_name][result_name] = squeeze(mean(objective(results[method_name][result_name]), 1:2), (1,2,5,6))

            # var: average over sims, estimate var over drops
            results_var[method_name][result_name] = squeeze(var(mean(objective(results[method_name][result_name]), 2), 1), (1,2,5,6))
        end
    end

    return results, results_mean, results_var
end

##########################################################################
# Plot results from the postprocess function.
function plot(processed_results, simulation_params, plot_params)
    results_mean = processed_results[2]; results_var = processed_results[3]

    fig = PyPlot.figure(;plot_params["figure"]...)
    ax = fig[:add_subplot](1, 1, 1; plot_params["axes"]...)

    plot_methods(ax, results_mean, results_var, simulation_params, plot_params;
        xvals=(haskey(plot_params, "xvals") ? plot_params["xvals"] : simulation_params["independent_variable"][2]))
    haskey(plot_params, "legend") && ax[:legend](;plot_params["legend"]...)

    if displayable("application/pdf") || displayable("image/png")
        display(fig)
    else
        open("$(simulation_params["simulation_name"])_$(plot_params["plot_name"]).pdf", "w") do file
            writemime(file, "application/pdf", fig)
        end
    end
    PyPlot.close(fig)
end

##########################################################################
# Post-process results from the simulate_precoding_convergence function.
function postprocess_precoding_convergence(raw_results, simulation_params, plot_params)
    methods = get_methods_to_plot(simulation_params, plot_params)

    # Auxiliary independent variables
    if haskey(simulation_params, "aux_independent_variables")
        Naux = length(simulation_params["aux_independent_variables"])
        aux_idp_vals_length = length(simulation_params["aux_independent_variables"][1][2])
        for n = 2:Naux
            aux_idp_vals_length == length(simulation_params["aux_independent_variables"][n][2]) ? nothing : Lumberjack.error("Auxiliary independent variable vectors must have equal length.")
        end
    else
        Naux = 0; aux_idp_vals_length = 1
    end

    # Compact raw results into result matrices
    Ndrops = size(raw_results.simulation_results, 1); Nsim = size(raw_results.simulation_results, 2)
    results = [ method => Dict{ASCIIString, Array{Float64}}() for method in methods ]
    for method_name in methods
        for (result_param,) in plot_params["methods"][method_name]
            if isa(result_param, ASCIIString)
                result_name = result_param

                result_dimensions = size(raw_results[1, 1, 1][method_name][result_name])
            else
                (calculator, calculate_from) = result_param
                result_name = string(string(calculator), "_", calculate_from)

                result_dimensions = size(raw_results[1, 1, 1][method_name][calculate_from])
            end

            result_ranges = [ 1:s for s in result_dimensions ]
            results[method_name][result_name] = Array(Float64, Ndrops, Nsim, aux_idp_vals_length, result_dimensions...)
            for Ndrops_idx = 1:Ndrops; for Nsim_idx = 1:Nsim; for aux_idp_vals_idx = 1:aux_idp_vals_length
                if isa(result_param, ASCIIString)
                    result = raw_results[Ndrops_idx, Nsim_idx, aux_idp_vals_idx][method_name][result_name]
                else
                    result = calculator(raw_results[Ndrops_idx, Nsim_idx, aux_idp_vals_idx][method_name][calculate_from])
                end
                results[method_name][result_name][Ndrops_idx, Nsim_idx, aux_idp_vals_idx, result_ranges...] = result
            end; end; end
        end
    end

    # Calculate statistics on result matrices
    plot_params["objective"] == :sum && (objective = (r -> sum(r, 4:5)))
    plot_params["objective"] == :mean && (objective = (r -> mean(sum(r, 5), 4)))
    plot_params["objective"] == :min && (objective = (r -> minimum(sum(r, 5), 4)))
    plot_params["objective"] == :none && (objective = (r -> r))

    results_mean = [ method => Dict{ASCIIString, Array{Float64}}() for method in methods ]
    results_var = [ method => Dict{ASCIIString, Array{Float64}}() for method in methods ]
    for method_name in methods
        for (result_param,) in plot_params["methods"][method_name]
            if isa(result_param, ASCIIString)
                result_name = result_param
            else
                (calculator, calculate_from) = result_param
                result_name = string(string(calculator), "_", calculate_from)
            end

            # mean: average over drops and sims
            results_mean[method_name][result_name] = transpose(squeeze(mean(objective(results[method_name][result_name]), 1:2), (1,2,4,5)))

            # var: average over sims, estimate var over drops
            results_var[method_name][result_name] = transpose(squeeze(var(mean(objective(results[method_name][result_name]), 2), 1), (1,2,4,5)))
        end
    end

    return results, results_mean, results_var
end

##########################################################################
# Post-process results from the simulate_assignment_convergence function.
function postprocess_assignment_convergence(raw_results, simulation_params, plot_params)
    methods = get_methods_to_plot(simulation_params, plot_params)

    # Auxiliary independent variables
    if haskey(simulation_params, "aux_independent_variables")
        Naux = length(simulation_params["aux_independent_variables"])
        aux_idp_vals_length = length(simulation_params["aux_independent_variables"][1][2])
        for n = 2:Naux
            aux_idp_vals_length == length(simulation_params["aux_independent_variables"][n][2]) ? nothing : Lumberjack.error("Auxiliary independent variable vectors must have equal length.")
        end
    else
        Naux = 0; aux_idp_vals_length = 1
    end

    # Compact raw results into result matrices
    Ndrops = size(raw_results.simulation_results, 1)
    results = [ method => Dict{ASCIIString, Array{Float64}}() for method in methods ]
    for method_name in methods
        for (result_param,) in plot_params["methods"][method_name]
            if isa(result_param, ASCIIString)
                result_name = result_param

                result_dimensions = size(raw_results[1, 1, 1][method_name][result_name])
            else
                (calculator, calculate_from) = result_param
                result_name = string(string(calculator), "_", calculate_from)

                result_dimensions = size(raw_results[1, 1, 1][method_name][calculate_from])
            end

            result_ranges = [ 1:s for s in result_dimensions ]
            results[method_name][result_name] = Array(Float64, Ndrops, 1, aux_idp_vals_length, result_dimensions...)
            for Ndrops_idx = 1:Ndrops; for aux_idp_vals_idx = 1:aux_idp_vals_length
                if isa(result_param, ASCIIString)
                    result = raw_results[Ndrops_idx, 1, aux_idp_vals_idx][method_name][result_name]
                else
                    result = calculator(raw_results[Ndrops_idx, 1, aux_idp_vals_idx][method_name][calculate_from])
                end
                results[method_name][result_name][Ndrops_idx, 1, aux_idp_vals_idx, result_ranges...] = result
            end; end
        end
    end

    # Calculate statistics on result matrices
    plot_params["objective"] == :sum && (objective = (r -> sum(r, 4:5)))
    plot_params["objective"] == :mean && (objective = (r -> mean(sum(r, 5), 4)))
    plot_params["objective"] == :min && (objective = (r -> minimum(sum(r, 5), 4)))
    plot_params["objective"] == :none && (objective = (r -> r))

    results_mean = [ method => Dict{ASCIIString, Array{Float64}}() for method in methods ]
    results_var = [ method => Dict{ASCIIString, Array{Float64}}() for method in methods ]
    for method_name in methods
        for (result_param,) in plot_params["methods"][method_name]
            if isa(result_param, ASCIIString)
                result_name = result_param
            else
                (calculator, calculate_from) = result_param
                result_name = string(string(calculator), "_", calculate_from)
            end

            # mean: average over drops and sims
            results_mean[method_name][result_name] = transpose(squeeze(mean(objective(results[method_name][result_name]), 1:2), (1,2,4,5)))

            # var: average over sims, estimate var over drops
            results_var[method_name][result_name] = transpose(squeeze(var(mean(objective(results[method_name][result_name]), 2), 1), (1,2,4,5)))
        end
    end

    return results, results_mean, results_var
end

##########################################################################
# Plots results from the postprocess_precoding_convergence function.
function plot_precoding_convergence(processed_results, simulation_params, plot_params; user_plots::Bool=true)
    methods = get_methods_to_plot(simulation_params, plot_params)

    results = processed_results[1]
    results_mean = processed_results[2]
    results_var = processed_results[3]

    ### SYSTEM-LEVEL OBJECTIVE ###
    fig = PyPlot.figure(;plot_params["figure"]...)
    ax = fig[:add_subplot](1, 1, 1; plot_params["axes"]...)

    if haskey(plot_params, "xvals")
        plot_methods(ax, results_mean, results_var, simulation_params, plot_params;
            xvals=plot_params["xvals"])
    else
        plot_methods(ax, results_mean, results_var, simulation_params, plot_params)
    end
    haskey(plot_params, "legend") && ax[:legend](;plot_params["legend"]...)

    if displayable("application/pdf") || displayable("image/png")
        display(fig)
    else
        open("$(simulation_params["simulation_name"])_$(plot_params["plot_name"]).pdf", "w") do file
            writemime(file, "application/pdf", fig)
        end
    end
    PyPlot.close(fig)

    if user_plots
        ### USER UTILITIES ###
        K_method_name = collect(keys(results))[1]
        K_result_name = collect(keys(results[K_method_name]))[1]
        K = size(results[K_method_name][K_result_name], 4) # ugly shit. This is really not stable.

        fig = PyPlot.figure(figsize=(6*K,3*length(methods)))
        subplot_ind = 1

        for method_name in methods
            for k = 1:K
                ax = fig[:add_subplot](length(methods), K, subplot_ind); subplot_ind += 1

                for (result_param, line_params) in plot_params["methods"][method_name]
                    if isa(result_param, ASCIIString)
                        result = results[method_name][result_param]
                    else
                        (calculator, calculate_from) = result_param
                        result = calculator(results[method_name][calculate_from])
                    end

                    ax[:plot](1:size(result, 6),
                              transpose(squeeze(mean(sum(result[:,:,:,k,:,:], 5), 1:2), (1,2,4,5)));
                              line_params...)
                end

                if k == 1
                    ax[:set_ylabel](method_name)
                end
                if subplot_ind-1 <= K
                    ax[:set_title]("User $k")
                end
                if subplot_ind-1 > (K-1)*length(methods)
                    ax[:set_xlabel]("Iteration")
                end
            end
        end

        if displayable("application/pdf") || displayable("image/png")
            display(fig)
        else
            open("$(simulation_params["simulation_name"])_$(plot_params["plot_name"])_peruser.pdf", "w") do file
                writemime(file, "application/pdf", fig)
            end
        end
        PyPlot.close(fig)

        ### STREAM UTILITIES ###
        fig = PyPlot.figure(figsize=(6*simulation_params["d"],3*K))
        subplot_ind = 1

        for k = 1:K; for n = 1:simulation_params["d"]
            ax = fig[:add_subplot](K, simulation_params["d"], subplot_ind); subplot_ind += 1

            for method_name in methods
                for (result_param, line_params) in plot_params["methods"][method_name]
                    if isa(result_param, ASCIIString)
                        result = results[method_name][result_param]
                    else
                        (calculator, calculate_from) = result_param
                        result = calculator(results[method_name][calculate_from])
                    end

                    ax[:plot](1:size(result, 6),
                              transpose(squeeze(mean(result[:,:,:,k,n,:], 1:2), (1,2,4,5)));
                              line_params...)
                end
            end

            if n == 1
                ax[:set_ylabel]("User $k")
            end
            if k == 1
                ax[:set_title]("Stream $n")
            end
            if k == K
                ax[:set_xlabel]("Iteration")
            end
        end; end

        if displayable("application/pdf") || displayable("image/png")
            display(fig)
        else
            open("$(simulation_params["simulation_name"])_$(plot_params["plot_name"])_perstream.pdf", "w") do file
                writemime(file, "application/pdf", fig)
            end
        end
        PyPlot.close(fig)
    end
end

##########################################################################
# Plots results from the postprocess_assignment_convergence function.
plot_assignment_convergence(processed_results, simulation_params, plot_params; user_plots::Bool=true) =
    plot_precoding_convergence(processed_results, simulation_params, plot_params, user_plots=user_plots)

##########################################################################
# Helper methods

# Plot lines for all methods into an axes
function plot_methods(ax, results_mean, results_var, simulation_params, plot_params; xvals=[])
    methods = get_methods_to_plot(simulation_params, plot_params)

    for method_name in methods
        for (result_param, line_params) in plot_params["methods"][method_name]
            if isa(result_param, ASCIIString)
                result_name = result_param
            else
                (calculator, calculate_from) = result_param
                result_name = string(string(calculator), "_", calculate_from)
            end

            xvals = (xvals != []) ? xvals : 1:size(results_mean[method_name][result_name], 1)
            ax[:plot](xvals, results_mean[method_name][result_name]; line_params...)

            if haskey(plot_params, "confidence_interval_zalpha2")
                for n = 1:size(results_mean[method_name][result_name], 2)
                    m = results_mean[method_name][result_name][:,n]
                    s = sqrt(results_var[method_name][result_name][:,n])
                    e = plot_params["confidence_interval_zalpha2"]*s/sqrt(simulation_params["Ndrops"])
                    ax[:fill_between](xvals, m + e, m - e; alpha=0.5, color=line_params[:color])
                end
            end
        end
    end
end

# We can only plot the methods that have been simulated (and thus specified
# in simulation_params), and those we have plotting parameters for (and
# thus specified in plot_params).
function get_methods_to_plot(simulation_params, plot_params)
    precoding_methods = get(simulation_params, "precoding_methods", Function[])
    assignment_methods = get(simulation_params, "assignment_methods", Function[])

    return intersect(map(stringify_method, union(precoding_methods, assignment_methods)), keys(plot_params["methods"]))
end


text:
function _integrate_spectrum(h::Float64, γ, λ, f) #interpreted as point mass spectrum
    denom = (λ / h + 1 / (1 + γ * f))
    1 / denom
end

#interpreted as
function _integrate_spectrum(h::Distribution, γ, λ, f)
    expectation(u->_integrate_spectrum(u, γ, λ, f), h)
end

function fixed_point_function(hs, γs, λs)
    γ = sum(γs)
    fixed_point_f = f -> f - sum(γs ./ γ .* _integrate_spectrum.(hs, γ, λs, f))
    find_zero(fixed_point_f, (0.0, 100.0))
end

function risk_formula(hs, γs, αs, λs)
    λs = min.(λs, 10_000) #hack for now until properly dealing with Infinity
    γ = sum(γs)
    fixed_pt = λs_tilde -> fixed_point_function(hs, γs, λs_tilde)
    f = fixed_pt(λs)
    ∇f = grad(central_fdm(5, 1), fixed_pt, λs)[1]
    #return ∇f
    #return γ ./ γs .* (γs .* λs - αs.^2 .* λs.^2) .* ∇f
    1 + γ * f + sum(γ ./ γs .* (γs .* λs - αs .^ 2 .* λs .^ 2) .* ∇f)
end

function r_squared(hs, γs, αs, λs)
    response_var = 1 + sum(abs2, αs)
    risk = risk_formula(hs, γs, αs, λs)
    1 - risk / response_var
end

function optimal_r_squared(αs, γs, hs)
    λs_opt = γs ./ αs .^ 2
    r_squared(hs, γs, αs, λs_opt)
end

function optimal_λs(γs, αs)
    γs ./ αs .^ 2
end

function optimal_risk(hs, γs, αs)
    λs_opt = optimal_λs(γs, αs)
    risk_formula(hs, γs, αs, λs_opt)
end

function optimal_single_λ(γs, αs)
    λ_opt = sum(γs) / sum(abs2, αs)
    λs_opt = fill(λ_opt, 2)
end

function optimal_single_λ_risk(hs, γs, αs)
    λs_opt = optimal_single_λ(γs, αs)
    risk_formula(hs, γs, αs, λs_opt)
end

function optimal_ignore_second_group_λs(γs, αs)
    λ1_opt = γs[1] * (1 + αs[2]^2) / αs[1]^2
    λs_opt = [λ1_opt; Inf]
end

function optimal_ignore_second_group_risk(hs, γs, αs)
    λs_opt = optimal_ignore_second_group_λs(γs, αs)
    risk_formula(hs, γs, αs, λs_opt)
end

ext: jl


text:
"""
FunctionBlocks
"""

export RampBlock

mutable struct RampBlock <: AbstractFunctionBlock
    inport::Vector{InPort}
    outport::Vector{OutPort}
    slope::AbstractSignalLine
    starttime::AbstractSignalLine
    initialoutput::AbstractSignalLine

    function RampBlock(::Type{Tv} = Float64; slope = 0, starttime = 0, initialoutput = 0) where Tv
        @createblock new(Vector{InPort}(), Vector{OutPort}(),
            signal(slope, Tv),
            signal(starttime, Tv),
            signal(initialoutput, Tv)
            ) 0 1
    end
end

"""
IO
"""

function Base.show(io::IO, blk::RampBlock)
    println(io, "Ramp()")
end

"""
to expr
"""

function _toexpr(blk::RampBlock)
    slope = _toexpr(blk.slope)
    starttime = _toexpr(blk.starttime)
    initialoutput = _toexpr(blk.initialoutput)
    :((t < $starttime) ? $initialoutput : $initialoutput + ($slope * (t - $starttime)))
end


text:
#

using GRAMPC
using LinearAlgebra

# Gravitational constant
const GRAV = 9.81

####################################################
# Dynamics functions
####################################################
function dyn( dx::Vector{Float64}, x::Vector{Float64}, u::Vector{Float64},
              p::Vector{Float64}, t::Float64, param::Vector{Float64} )
    dx[1] = x[2];
    dx[2] = u[1];
    dx[3] = x[4];
    dx[4] = u[2];
    dx[5] = x[6];
    dx[6] = -( ( GRAV * sin( x[5] ) + cos( x[5] ) * u[1] + 2 * x[4] * x[6] ) / x[3] );
end

# Jacobian df/dx multiplied by vector vec, i.e. (df/dx)^T*vec or vec^T*(df/dx)
function dfdx_vec( res::Vector{Float64}, x::Vector{Float64}, u::Vector{Float64},
                   p::Vector{Float64}, t::Float64, vec::Vector{Float64}, param::Vector{Float64} )
  aux1 = sin( x[5] );
  aux2 = cos( x[5] );

  res[1] = 0;
  res[2] = vec[1];
  res[3] = ( GRAV * aux1 + aux2 * u[1] + 2 * x[4] * x[6] ) * vec[6] / ( x[3]^2 );
  res[4] = vec[3] - ( 2 * x[6] * vec[6] ) / x[3];
  res[5] = -( ( (GRAV * aux2 - aux1 * u[1] ) * vec[6] ) / x[3] );
  res[6] = vec[5] - ( 2 * x[4] * vec[6] ) / x[3];
end

# Jacobian df/du multiplied by vector vec, i.e. (df/du)^T*vec or vec^T*(df/du)
function dfdu_vec( res::Vector{Float64}, x::Vector{Float64}, u::Vector{Float64},
                   p::Vector{Float64}, t::Float64, vec::Vector{Float64}, param::Vector{Float64} )
  res[1] = vec[2] - ( cos( x[5] ) * vec[6] ) / x[3];
  res[2] = vec[4];
end

####################################################
# Integral cost functions
####################################################
function lfct( t::Float64, x::Vector{Float64}, u::Vector{Float64}, p::Vector{Float64}, xdes::Vector{Float64},
               udes::Vector{Float64}, param::Vector{Float64} )
    Q = diagm( param[1:6] )
    R = diagm( param[7:8] )

    xt = x - xdes
    ut = u - udes

    return xt'*Q*xt + ut'*R*ut
end

# Gradient dl/dx
function dldx( res::Vector{Float64}, t::Float64, x::Vector{Float64}, u::Vector{Float64}, p::Vector{Float64},
               xdes::Vector{Float64}, udes::Vector{Float64}, param::Vector{Float64} )
    Q = diagm( param[1:6] )

    # We need to return a column vector, so this is actually the transpose of the gradient
    res = 2*Q'*( x - xdes )
end

# Gradient dl/du
function dldu( res::Vector{Float64}, t::Float64, x::Vector{Float64}, u::Vector{Float64}, p::Vector{Float64},
               xdes::Vector{Float64}, udes::Vector{Float64}, param::Vector{Float64} )
    R = diagm( param[7:8] )

    # We need to return a column vector, so this is actually the transpose of the gradient
    res = 2*R'*( u - udes )
end

####################################################
# Inequality constraints h(t,x(t),u(t),p,uperparam) <= 0
####################################################
function hfct( res::Vector{Float64}, t::Float64, x::Vector{Float64}, u::Vector{Float64}, p::Vector{Float64},
               param::Vector{Float64} )
    pSys = param[9:11]
    xPos = x[1] + sin( x[5] ) * x[3];

    res[1] = cos( x[5] ) * x[3] - pSys[1] * xPos^2 - pSys[2];
    res[2] = x[6] - pSys[3];
    res[3] = -x[6] - pSys[3];
end

# Jacobian dh/dx multiplied by vector vec, i.e. (dh/dx)^T*vec or vec^T*(dg/dx)
function dhdx_vec( res::Vector{Float64}, t::Float64, x::Vector{Float64}, u::Vector{Float64}, p::Vector{Float64},
                   vec::Vector{Float64}, param::Vector{Float64} )
    pSys = param[9:11]
    temp = pSys[1] * ( x[1] + sin( x[5] ) * x[3] );

    res[1] = temp * vec[1];
    res[2] = 0;
    res[3] = ( sin( x[5] ) * temp + cos( x[5] ) ) * vec[1];
    res[4] = 0;
    res[5] = ( cos( x[5] ) * x[3] * temp - sin( x[5] ) * x[3] ) * vec[1];
    res[6] = 0 + vec[2] - vec[3];
end

# Jacobian dh/du multiplied by vector vec, i.e. (dh/du)^T*vec or vec^T*(dg/du)
function dhdu_vec( res::Vector{Float64}, t::Float64, x::Vector{Float64}, u::Vector{Float64}, p::Vector{Float64},
                   vec::Vector{Float64}, param::Vector{Float64} )
  res = [0];
end

####################################################
# Setup the GRAMPC problem
####################################################
craneModel = GRAMPC.Model( states = 6, inputs = 2, ineqcon = 3 )

# Add the dynamics to the problem
set_dynamics!( craneModel, dyn, dx=dfdx_vec, du=dfdu_vec )

# Setup the cost of the problem
set_lagrange_term!( craneModel, lfct, dx=dldx, du=dldu )

# Setup the constraints for the problem
set_inequality_constraints!( craneModel, hfct, dx=dhdx_vec, du=dhdu_vec )

# Starting state
x0 = [ -2.0;
        0.0;
        2.0;
        0.0;
        0.0;
        0.0 ]

# Starting input
u0 = [ 0.0;
       0.0 ]

# Constraint tolerances
constraintTol = [ 1e-4;
                  1e-3;
                  1e-3 ]

# Initialize the problem and setup the options
setup!( craneModel, Thor=2,                        # Length of the prediction horizon
                    Nhor=20,                         # Number of system integration steps
                    dt=0.002,                         # Sampling time
                    t0=0.0,                          # Initial time
                    TerminalCost=false,              # Disable the terminal cost
                    ConstraintsAbsTol=constraintTol, # Constraint tolerances
                    x0=x0,
                    u0=u0 )

# Set the limits after initializing the problem (they are considered an option and not normal constraints)
umax = [ 2.0;
         2.0 ]
umin = [ -2.0;
         -2.0 ]

set_limits!( craneModel, :input, lb=umin, ub=umax )

# Desired state/input
xdes = [ 2.0;
         0.0;
         2.0;
         0.0;
         0.0;
         0.0 ]
udes = [ 0.0;
         0.0 ]

set_setpoint!( craneModel, input=udes, state=xdes )

# Pass in the user parameters
pSys = [ 1.0,   # Q matrix diagonal
         2.0,
         2.0,
         1.0,
         1.0,
         4.0,
         0.05,  # R matrix diagonal
         0.05,
         0.2,   # Inequality constraint parameters
         1.25,
         0.3 ]

set_user_parameters!( craneModel, pSys )

estimate_penalty_min!( craneModel )

# Solve the problem
sol = solve!( craneModel )


text:
module ReferenceStates

using Polynomials
using Unitful, UnitfulAtomic
using ..HKQM

export HarmonicEigenstate,
       harmonic_state

function hermite_polynomial(ν)
    if ν == 0
        return Polynomial([1])
    elseif ν == 1
        return Polynomial([0, 2])
    else
        p1 = Polynomial([0,2])*hermite_polynomial(ν-1)
        p2 = 2(ν-1)*hermite_polynomial(ν-2)
        return p1 - p2
    end
end

struct HarmonicEigenstate
    ν::Int
    α::Float64
    ω::Float64
    N::Float64
    hp::Polynomial{Int}
    function HarmonicEigenstate(ν::Int; ω=1)
        α = 1/sqrt(ω)
        hp = hermite_polynomial(ν)
        N = (2^ν*factorial(ν)*sqrt(π)*α)^(-1//2)
        new(ν, α, ω, N, hp)
    end
end

function (HE::HarmonicEigenstate)(v::AbstractVector)
    return prod(HE, v)
end

function (HE::HarmonicEigenstate)(x::Real)
    y = x/HE.α
    return HE.N*HE.hp(y)*exp(-0.5*y^2)
end

function harmonic_state(ceg, hx, hy=hx, hz=hx)
    r = position_operator(ceg)
    ψ = map(r-> hx(r[1])*hy(r[2])*hz(r[3]), ceg)
    return QuantumState(ceg, ψ)
end

function energy(he::HarmonicEigenstate)
    return he.ω*(he.ν+0.5)*u"hartree"
end

end  # module


text:
# Autogenerated wrapper script for OpenCVQt_jll for armv7l-linux-gnueabihf-cxx03
export libopencv_calib3d, libopencv_core, libopencv_dnn, libopencv_features2d, libopencv_flann, libopencv_gapi, libopencv_highgui, libopencv_imgcodecs, libopencv_imgproc, libopencv_ml, libopencv_objdetect, libopencv_photo, libopencv_stitching, libopencv_video, libopencv_videoio

using Qt_jll
using Libglvnd_jll
## Global variables
PATH = ""
LIBPATH = ""
LIBPATH_env = "LD_LIBRARY_PATH"
LIBPATH_default = ""

# Relative path to `libopencv_calib3d`
const libopencv_calib3d_splitpath = ["lib", "libopencv_calib3d.so"]

# This will be filled out by __init__() for all products, as it must be done at runtime
libopencv_calib3d_path = ""

# libopencv_calib3d-specific global declaration
# This will be filled out by __init__()
libopencv_calib3d_handle = C_NULL

# This must be `const` so that we can use it with `ccall()`
const libopencv_calib3d = "libopencv_calib3d.so.4.5"

# Relative path to `libopencv_core`
const libopencv_core_splitpath = ["lib", "libopencv_core.so"]

# This will be filled out by __init__() for all products, as it must be done at runtime
libopencv_core_path = ""

# libopencv_core-specific global declaration
# This will be filled out by __init__()
libopencv_core_handle = C_NULL

# This must be `const` so that we can use it with `ccall()`
const libopencv_core = "libopencv_core.so.4.5"

# Relative path to `libopencv_dnn`
const libopencv_dnn_splitpath = ["lib", "libopencv_dnn.so"]

# This will be filled out by __init__() for all products, as it must be done at runtime
libopencv_dnn_path = ""

# libopencv_dnn-specific global declaration
# This will be filled out by __init__()
libopencv_dnn_handle = C_NULL

# This must be `const` so that we can use it with `ccall()`
const libopencv_dnn = "libopencv_dnn.so.4.5"

# Relative path to `libopencv_features2d`
const libopencv_features2d_splitpath = ["lib", "libopencv_features2d.so"]

# This will be filled out by __init__() for all products, as it must be done at runtime
libopencv_features2d_path = ""

# libopencv_features2d-specific global declaration
# This will be filled out by __init__()
libopencv_features2d_handle = C_NULL

# This must be `const` so that we can use it with `ccall()`
const libopencv_features2d = "libopencv_features2d.so.4.5"

# Relative path to `libopencv_flann`
const libopencv_flann_splitpath = ["lib", "libopencv_flann.so"]

# This will be filled out by __init__() for all products, as it must be done at runtime
libopencv_flann_path = ""

# libopencv_flann-specific global declaration
# This will be filled out by __init__()
libopencv_flann_handle = C_NULL

# This must be `const` so that we can use it with `ccall()`
const libopencv_flann = "libopencv_flann.so.4.5"

# Relative path to `libopencv_gapi`
const libopencv_gapi_splitpath = ["lib", "libopencv_gapi.so"]

# This will be filled out by __init__() for all products, as it must be done at runtime
libopencv_gapi_path = ""

# libopencv_gapi-specific global declaration
# This will be filled out by __init__()
libopencv_gapi_handle = C_NULL

# This must be `const` so that we can use it with `ccall()`
const libopencv_gapi = "libopencv_gapi.so.4.5"

# Relative path to `libopencv_highgui`
const libopencv_highgui_splitpath = ["lib", "libopencv_highgui.so"]

# This will be filled out by __init__() for all products, as it must be done at runtime
libopencv_highgui_path = ""

# libopencv_highgui-specific global declaration
# This will be filled out by __init__()
libopencv_highgui_handle = C_NULL

# This must be `const` so that we can use it with `ccall()`
const libopencv_highgui = "libopencv_highgui.so.4.5"

# Relative path to `libopencv_imgcodecs`
const libopencv_imgcodecs_splitpath = ["lib", "libopencv_imgcodecs.so"]

# This will be filled out by __init__() for all products, as it must be done at runtime
libopencv_imgcodecs_path = ""

# libopencv_imgcodecs-specific global declaration
# This will be filled out by __init__()
libopencv_imgcodecs_handle = C_NULL

# This must be `const` so that we can use it with `ccall()`
const libopencv_imgcodecs = "libopencv_imgcodecs.so.4.5"

# Relative path to `libopencv_imgproc`
const libopencv_imgproc_splitpath = ["lib", "libopencv_imgproc.so"]

# This will be filled out by __init__() for all products, as it must be done at runtime
libopencv_imgproc_path = ""

# libopencv_imgproc-specific global declaration
# This will be filled out by __init__()
libopencv_imgproc_handle = C_NULL

# This must be `const` so that we can use it with `ccall()`
const libopencv_imgproc = "libopencv_imgproc.so.4.5"

# Relative path to `libopencv_ml`
const libopencv_ml_splitpath = ["lib", "libopencv_ml.so"]

# This will be filled out by __init__() for all products, as it must be done at runtime
libopencv_ml_path = ""

# libopencv_ml-specific global declaration
# This will be filled out by __init__()
libopencv_ml_handle = C_NULL

# This must be `const` so that we can use it with `ccall()`
const libopencv_ml = "libopencv_ml.so.4.5"

# Relative path to `libopencv_objdetect`
const libopencv_objdetect_splitpath = ["lib", "libopencv_objdetect.so"]

# This will be filled out by __init__() for all products, as it must be done at runtime
libopencv_objdetect_path = ""

# libopencv_objdetect-specific global declaration
# This will be filled out by __init__()
libopencv_objdetect_handle = C_NULL

# This must be `const` so that we can use it with `ccall()`
const libopencv_objdetect = "libopencv_objdetect.so.4.5"

# Relative path to `libopencv_photo`
const libopencv_photo_splitpath = ["lib", "libopencv_photo.so"]

# This will be filled out by __init__() for all products, as it must be done at runtime
libopencv_photo_path = ""

# libopencv_photo-specific global declaration
# This will be filled out by __init__()
libopencv_photo_handle = C_NULL

# This must be `const` so that we can use it with `ccall()`
const libopencv_photo = "libopencv_photo.so.4.5"

# Relative path to `libopencv_stitching`
const libopencv_stitching_splitpath = ["lib", "libopencv_stitching.so"]

# This will be filled out by __init__() for all products, as it must be done at runtime
libopencv_stitching_path = ""

# libopencv_stitching-specific global declaration
# This will be filled out by __init__()
libopencv_stitching_handle = C_NULL

# This must be `const` so that we can use it with `ccall()`
const libopencv_stitching = "libopencv_stitching.so.4.5"

# Relative path to `libopencv_video`
const libopencv_video_splitpath = ["lib", "libopencv_video.so"]

# This will be filled out by __init__() for all products, as it must be done at runtime
libopencv_video_path = ""

# libopencv_video-specific global declaration
# This will be filled out by __init__()
libopencv_video_handle = C_NULL

# This must be `const` so that we can use it with `ccall()`
const libopencv_video = "libopencv_video.so.4.5"

# Relative path to `libopencv_videoio`
const libopencv_videoio_splitpath = ["lib", "libopencv_videoio.so"]

# This will be filled out by __init__() for all products, as it must be done at runtime
libopencv_videoio_path = ""

# libopencv_videoio-specific global declaration
# This will be filled out by __init__()
libopencv_videoio_handle = C_NULL

# This must be `const` so that we can use it with `ccall()`
const libopencv_videoio = "libopencv_videoio.so.4.5"

# Inform that the wrapper is available for this platform
wrapper_available = true

"""
Open all libraries
"""
function __init__()
    # This either calls `@artifact_str()`, or returns a constant string if we're overridden.
    global artifact_dir = find_artifact_dir()

    global PATH_list, LIBPATH_list
    # Initialize PATH and LIBPATH environment variable listings
    # From the list of our dependencies, generate a tuple of all the PATH and LIBPATH lists,
    # then append them to our own.
    foreach(p -> append!(PATH_list, p), (Qt_jll.PATH_list, Libglvnd_jll.PATH_list,))
    foreach(p -> append!(LIBPATH_list, p), (Qt_jll.LIBPATH_list, Libglvnd_jll.LIBPATH_list,))

    global libopencv_calib3d_path = normpath(joinpath(artifact_dir, libopencv_calib3d_splitpath...))

    # Manually `dlopen()` this right now so that future invocations
    # of `ccall` with its `SONAME` will find this path immediately.
    global libopencv_calib3d_handle = dlopen(libopencv_calib3d_path, RTLD_LAZY | RTLD_DEEPBIND)
    push!(LIBPATH_list, dirname(libopencv_calib3d_path))

    global libopencv_core_path = normpath(joinpath(artifact_dir, libopencv_core_splitpath...))

    # Manually `dlopen()` this right now so that future invocations
    # of `ccall` with its `SONAME` will find this path immediately.
    global libopencv_core_handle = dlopen(libopencv_core_path, RTLD_LAZY | RTLD_DEEPBIND)
    push!(LIBPATH_list, dirname(libopencv_core_path))

    global libopencv_dnn_path = normpath(joinpath(artifact_dir, libopencv_dnn_splitpath...))

    # Manually `dlopen()` this right now so that future invocations
    # of `ccall` with its `SONAME` will find this path immediately.
    global libopencv_dnn_handle = dlopen(libopencv_dnn_path, RTLD_LAZY | RTLD_DEEPBIND)
    push!(LIBPATH_list, dirname(libopencv_dnn_path))

    global libopencv_features2d_path = normpath(joinpath(artifact_dir, libopencv_features2d_splitpath...))

    # Manually `dlopen()` this right now so that future invocations
    # of `ccall` with its `SONAME` will find this path immediately.
    global libopencv_features2d_handle = dlopen(libopencv_features2d_path, RTLD_LAZY | RTLD_DEEPBIND)
    push!(LIBPATH_list, dirname(libopencv_features2d_path))

    global libopencv_flann_path = normpath(joinpath(artifact_dir, libopencv_flann_splitpath...))

    # Manually `dlopen()` this right now so that future invocations
    # of `ccall` with its `SONAME` will find this path immediately.
    global libopencv_flann_handle = dlopen(libopencv_flann_path, RTLD_LAZY | RTLD_DEEPBIND)
    push!(LIBPATH_list, dirname(libopencv_flann_path))

    global libopencv_gapi_path = normpath(joinpath(artifact_dir, libopencv_gapi_splitpath...))

    # Manually `dlopen()` this right now so that future invocations
    # of `ccall` with its `SONAME` will find this path immediately.
    global libopencv_gapi_handle = dlopen(libopencv_gapi_path, RTLD_LAZY | RTLD_DEEPBIND)
    push!(LIBPATH_list, dirname(libopencv_gapi_path))

    global libopencv_highgui_path = normpath(joinpath(artifact_dir, libopencv_highgui_splitpath...))

    # Manually `dlopen()` this right now so that future invocations
    # of `ccall` with its `SONAME` will find this path immediately.
    global libopencv_highgui_handle = dlopen(libopencv_highgui_path, RTLD_LAZY | RTLD_DEEPBIND)
    push!(LIBPATH_list, dirname(libopencv_highgui_path))

    global libopencv_imgcodecs_path = normpath(joinpath(artifact_dir, libopencv_imgcodecs_splitpath...))

    # Manually `dlopen()` this right now so that future invocations
    # of `ccall` with its `SONAME` will find this path immediately.
    global libopencv_imgcodecs_handle = dlopen(libopencv_imgcodecs_path, RTLD_LAZY | RTLD_DEEPBIND)
    push!(LIBPATH_list, dirname(libopencv_imgcodecs_path))

    global libopencv_imgproc_path = normpath(joinpath(artifact_dir, libopencv_imgproc_splitpath...))

    # Manually `dlopen()` this right now so that future invocations
    # of `ccall` with its `SONAME` will find this path immediately.
    global libopencv_imgproc_handle = dlopen(libopencv_imgproc_path, RTLD_LAZY | RTLD_DEEPBIND)
    push!(LIBPATH_list, dirname(libopencv_imgproc_path))

    global libopencv_ml_path = normpath(joinpath(artifact_dir, libopencv_ml_splitpath...))

    # Manually `dlopen()` this right now so that future invocations
    # of `ccall` with its `SONAME` will find this path immediately.
    global libopencv_ml_handle = dlopen(libopencv_ml_path, RTLD_LAZY | RTLD_DEEPBIND)
    push!(LIBPATH_list, dirname(libopencv_ml_path))

    global libopencv_objdetect_path = normpath(joinpath(artifact_dir, libopencv_objdetect_splitpath...))

    # Manually `dlopen()` this right now so that future invocations
    # of `ccall` with its `SONAME` will find this path immediately.
    global libopencv_objdetect_handle = dlopen(libopencv_objdetect_path, RTLD_LAZY | RTLD_DEEPBIND)
    push!(LIBPATH_list, dirname(libopencv_objdetect_path))

    global libopencv_photo_path = normpath(joinpath(artifact_dir, libopencv_photo_splitpath...))

    # Manually `dlopen()` this right now so that future invocations
    # of `ccall` with its `SONAME` will find this path immediately.
    global libopencv_photo_handle = dlopen(libopencv_photo_path, RTLD_LAZY | RTLD_DEEPBIND)
    push!(LIBPATH_list, dirname(libopencv_photo_path))

    global libopencv_stitching_path = normpath(joinpath(artifact_dir, libopencv_stitching_splitpath...))

    # Manually `dlopen()` this right now so that future invocations
    # of `ccall` with its `SONAME` will find this path immediately.
    global libopencv_stitching_handle = dlopen(libopencv_stitching_path, RTLD_LAZY | RTLD_DEEPBIND)
    push!(LIBPATH_list, dirname(libopencv_stitching_path))

    global libopencv_video_path = normpath(joinpath(artifact_dir, libopencv_video_splitpath...))

    # Manually `dlopen()` this right now so that future invocations
    # of `ccall` with its `SONAME` will find this path immediately.
    global libopencv_video_handle = dlopen(libopencv_video_path, RTLD_LAZY | RTLD_DEEPBIND)
    push!(LIBPATH_list, dirname(libopencv_video_path))

    global libopencv_videoio_path = normpath(joinpath(artifact_dir, libopencv_videoio_splitpath...))

    # Manually `dlopen()` this right now so that future invocations
    # of `ccall` with its `SONAME` will find this path immediately.
    global libopencv_videoio_handle = dlopen(libopencv_videoio_path, RTLD_LAZY | RTLD_DEEPBIND)
    push!(LIBPATH_list, dirname(libopencv_videoio_path))

    # Filter out duplicate and empty entries in our PATH and LIBPATH entries
    filter!(!isempty, unique!(PATH_list))
    filter!(!isempty, unique!(LIBPATH_list))
    global PATH = join(PATH_list, ':')
    global LIBPATH = join(vcat(LIBPATH_list, [joinpath(Sys.BINDIR, Base.LIBDIR, "julia"), joinpath(Sys.BINDIR, Base.LIBDIR)]), ':')

end  # __init__()


text:
function _create_client_script(;
        sys_root, client_root,
        client_script, url
    )
    client_tfile = joinpath(@__DIR__, "client_script_template.jl")
    temp::String = read(client_tfile, String)
    temp = replace(temp, "REMOTE_URL" => url)
    temp = replace(temp, "CLIENT_ROOT" => client_root)
    temp = replace(temp, "SYSTEM_ROOT" => sys_root)
    src = replace(temp, "SERVER_SCRIPT_PATH" => client_script)
    write(client_script, src)
    return src
end

function _create_server_script(;
        server_root, url, deb, server_script
    )
    # SERVER_ROOT
    # DEB_MODE
    server_tfile = joinpath(@__DIR__, "server_script_template.jl")
    temp::String = read(server_tfile, String)
    temp = replace(temp, "REMOTE_URL" => url)
    temp = replace(temp, "DEB_MODE" => string(deb))
    src = replace(temp, "SERVER_ROOT" => server_root)
    write(server_script, src)
    return src
end

## ---------------------------------------------------------------
function gw_create_devland(;
        sys_root = _GW_SYSTEM_DFLT_ROOT,
        clear_repos = true,
        clear_scripts = false,
        verbose = false,
        deb = false,
        branch_name = "main"
    )

    # ---------------------------------------------------------------
    # systems
    devland = gw_devland(sys_root)
    upstream_repo, url = GitLinks.create_local_upstream(devland;
        verbose, branch_name
    )
    client_root = joinpath(devland, "client_root")
    server_root = joinpath(devland, "server_root")

    if clear_repos
        _rm(client_root)
        _rm(server_root)
    end

    @info("Setting server")
    server_gw = GitWorker(server_root, url)
    gw_setup(server_gw; verbose)

    @info("Setting client")
    client_gw = GitWorker(client_root, url)
    gw_setup(client_gw; verbose)

    # ---------------------------------------------------------------
    # scripts
    client_script = joinpath(devland, "client.jl")
    server_script = joinpath(devland, "server.jl")

    if clear_scripts || !isfile(client_script)
        _rm(client_script)
        _create_client_script(; sys_root, client_root, client_script, url)
    end

    # server script
    if clear_scripts || !isfile(server_script)
        _rm(server_script)
        _create_server_script(;server_root, url, deb, server_script)
    end

    @info("DevLand setup", client_script, server_script)

    return (;client_gw, server_gw)
end

max_stars_repo_name: josePereiro/GitWorkers

max_issues_repo_name: josePereiro/GitWorkers

max_forks_repo_name: josePereiro/GitWorkers


text:
using AdversarialDriving
using Test
using POMDPs

## Test PedestrianControl conversion functions
@test PEDESTRIAN_DISTURBANCE_DIM == 5

pc = PedestrianControl(a = (-0.1, 0.1), da = (0.2, 0.3), noise = Noise((0.5, 0.6), 0.7))
vec = PedestrianControl_to_vec(pc)
@test vec == [0.2, 0.3, 0.5, 0.6, 0.7]

pc2 = vec_to_PedestrianControl([0.2, 0.3, 0.5, 0.6, 0.7])
@test all(pc2.a .== (0, 0))
@test pc2.da == pc.da
@test pc2.noise == pc.noise

## Test BlinkerVehicleConstrol conversion functions
@test BLINKERVEHICLE_DISTURBANCE_DIM == 5

bv = BlinkerVehicleControl(a = 1., da=0.1, toggle_goal = true, toggle_blinker = false, noise = Noise((0.5, 0.6), 0.7))
v = BlinkerVehicleControl_to_vec(bv)
@test v == [0.1, 1., 0., 0.5, 0.7]

bv2 = vec_to_BlinkerVehicleControl([0.1, 0.5, -0.5, 0.5, 0.7])
@test bv2.a == 0
@test bv2.da == bv.da
@test bv2.toggle_goal == bv.toggle_goal
@test bv2.toggle_blinker == bv.toggle_blinker
@test bv2.noise.pos[1] ==  bv.noise.pos[1]
@test bv2.noise.vel ==  bv.noise.vel
@test bv2.noise.pos[2] == 0

## Test Convert_a functions

sut_agent = BlinkerVehicleAgent(get_ped_vehicle(id=1, s=5., v=15.), TIDM(ped_TIDM_template, noisy_observations = true))
adv_vehicle = BlinkerVehicleAgent(get_ped_vehicle(id=2, s=15., v=15.), TIDM(ped_TIDM_template), disturbance_model = [])
adv_ped = NoisyPedestrianAgent(get_pedestrian(id=3, s=7., v=2.0), AdversarialPedestrian())
mdp = AdversarialDrivingMDP(sut_agent, [adv_vehicle, adv_ped], ped_roadway, 0.1,)

a = [bv, pc]
avec = convert_a(AbstractArray, a, mdp)
@test avec[1:5] == v
@test avec[6:10] == vec
a2 = convert_a(Vector{Disturbance}, avec, mdp)
@test a2[1] == bv2
@test a2[2] == pc2

@test_throws ErrorException actions(mdp)

# Create two agents and construct their actions
bv1 = BlinkerVehicleAgent(left_straight(id=1), TIDM(Tint_TIDM_template))
bv2 = BlinkerVehicleAgent(right_straight(id=2), TIDM(Tint_TIDM_template))
advs = [bv1, bv2]
dist = combine_discrete(advs)
@test dist isa DiscreteActionModel

# Test action construction, probabilities and indexing
acts = dist.actions
for (a, i) in zip(acts, 1:length(acts))
    @test get_actionindex(dist, a) == i
end

BV_ACTIONS = bv1.disturbance_model.actions
@test length(acts) == 13
@test acts[1] == [BV_ACTIONS[1][1], BV_ACTIONS[1][1]]
@test acts[2] == [BV_ACTIONS[2][1], BV_ACTIONS[1][1]]
@test acts[7] == [BV_ACTIONS[7][1], BV_ACTIONS[1][1]]
@test acts[8] == [BV_ACTIONS[1][1], BV_ACTIONS[2][1]]
@test acts[13] == [BV_ACTIONS[1][1], BV_ACTIONS[7][1]]

action_prob = dist.probs
BV_ACTION_PROB = bv1.disturbance_model.probs
@test isapprox(BV_ACTION_PROB[1], action_prob[1])
aprob2 = action_prob[2]
aprob3 = action_prob[3]
@test isapprox(aprob3/aprob2, 10.)


text:
using Test
using CryptoSignatures
using CryptoGroups

function signaturetest(s,G)
    @test verify(s,G)

    # Changing message

    vs1 = DSASignature(signature.hash+1,signature.r,signature.s,signature.pubkey)
    @test verify(vs1,G)==false

    # Changing signer

    s = Signer(G)
    vs2 = DSASignature(signature.hash,signature.r,signature.s,s.pubkey)
    @test verify(vs2,G)==false

    # Tampering the signature

    vs3 = DSASignature(signature.hash,signature.r+1,signature.s,signature.pubkey)
    @test verify(vs3,G)==false

    vs4 = DSASignature(signature.hash,signature.r,signature.s+1,signature.pubkey)
    @test verify(vs4,G)==false
end

### DSA signatures

# PrimeGroup

G = CryptoGroups.MODP160Group()
s = Signer(G)

h = BigInt(hash(3434))
signature = DSASignature(h,s)

signaturetest(signature,G)

# EllipticGroup

G = CryptoGroups.Scep256k1Group()
s = Signer(G)

h = BigInt(hash(3434))
signature = DSASignature(h,s)

signaturetest(signature,G)

# Now some convinience methods
@test DSASignature{BigInt}(Dict(signature)) == signature
@test Signer{BigInt}(Dict(s),G) == s


text:
using Documenter, SearchLight
using SearchLightOracle

makedocs(
    sitename = "SearchLightOracle.jl",
    modules = [ SearchLightOracle ],
    pages = [ "Home" => "index.md",
            ],
)

deploydocs(
    repo = "github.com/FrankUrbach/SearchLightOracle.jl.git",
    target = "build",
)


text:
function initialize!(integrator, cache::Union{Rosenbrock23Cache,
                                              Rosenbrock32Cache,})
  integrator.kshortsize = 2
  @unpack k₁,k₂,fsalfirst,fsallast = cache
  integrator.fsalfirst = fsalfirst
  integrator.fsallast = fsallast
  resize!(integrator.k, integrator.kshortsize)
  integrator.k .= [k₁,k₂]
  integrator.f(integrator.fsalfirst, integrator.uprev, integrator.p, integrator.t)
end

function initialize!(integrator, cache::Union{Rosenbrock23ConstantCache,
                                              Rosenbrock32ConstantCache})
  integrator.kshortsize = 2
  integrator.k = typeof(integrator.k)(undef, integrator.kshortsize)
  integrator.fsalfirst = integrator.f(integrator.uprev, integrator.p, integrator.t)

  # Avoid undefined entries if k is an array of arrays
  integrator.fsallast = zero(integrator.fsalfirst)
  integrator.k[1] = zero(integrator.fsalfirst)
  integrator.k[2] = zero(integrator.fsalfirst)
end

@muladd function perform_step!(integrator, cache::Rosenbrock23Cache, repeat_step=false)
  @unpack t,dt,uprev,u,f,p = integrator
  @unpack k₁,k₂,k₃,du1,du2,f₁,vectmp,vectmp2,vectmp3,fsalfirst,fsallast,dT,J,W,tmp,uf,tf,linsolve_tmp,linsolve_tmp_vec,jac_config = cache
  @unpack c₃₂,d = cache.tab

  # Assignments
  sizeu  = size(u)
  mass_matrix = integrator.f.mass_matrix

  # Precalculations
  γ = dt*d
  dto2 = dt/2
  dto6 = dt/6

  calc_rosenbrock_differentiation!(integrator, cache, γ, γ, repeat_step, false)

  if DiffEqBase.has_invW(f)
    mul!(vectmp, W, linsolve_tmp_vec)
  else
    cache.linsolve(vectmp, W, linsolve_tmp_vec, !repeat_step)
  end

  recursivecopy!(k₁, reshape(vectmp, size(u)...))
  @. u = uprev + dto2*k₁
  f(f₁,u,p,t+dto2)

  if mass_matrix == I
    tmp .= k₁
  else
    mul!(tmp,mass_matrix,k₁)
  end

  @. linsolve_tmp = f₁ - tmp
  if DiffEqBase.has_invW(f)
    mul!(vectmp2, W, linsolve_tmp_vec)
  else
    cache.linsolve(vectmp2, W, linsolve_tmp_vec)
  end

  tmp2 = reshape(vectmp2, sizeu...)
  @. k₂ = tmp2 + k₁
  @. u = uprev + dt*k₂

  if integrator.opts.adaptive
    f( fsallast,  u, p, t+dt)

    if mass_matrix == I
      @. linsolve_tmp = fsallast - c₃₂*(k₂-f₁) - 2(k₁-fsalfirst) + dt*dT
    else
      @. du2 = c₃₂*k₂ + 2k₁
      mul!(du1,mass_matrix,du2)
      @. linsolve_tmp = fsallast - du1 + c₃₂*f₁ + 2fsalfirst + dt*dT
    end

    if DiffEqBase.has_invW(f)
      mul!(vectmp3, W, linsolve_tmp_vec)
    else
      cache.linsolve(vectmp3, W, linsolve_tmp_vec)
    end

    k₃ = reshape(vectmp3, sizeu...)

    @. tmp = dto6*(k₁ - 2*k₂ + k₃)
    # does not work with units - additional unitless array required!
    calculate_residuals!(tmp, tmp, uprev, u, integrator.opts.abstol, integrator.opts.reltol,integrator.opts.internalnorm)
    integrator.EEst = integrator.opts.internalnorm(tmp)
  end
end

@muladd function perform_step!(integrator, cache::Rosenbrock32Cache, repeat_step=false)
  @unpack t,dt,uprev,u,f,p = integrator
  @unpack k₁,k₂,k₃,du1,du2,f₁,vectmp,vectmp2,vectmp3,fsalfirst,fsallast,dT,J,W,tmp,uf,tf,linsolve_tmp,linsolve_tmp_vec,jac_config = cache
  @unpack c₃₂,d = cache.tab

  # Assignments
  sizeu  = size(u)
  mass_matrix = integrator.f.mass_matrix

  # Precalculations
  γ = dt*d
  dto2 = dt/2
  dto6 = dt/6

  calc_rosenbrock_differentiation!(integrator, cache, γ, γ, repeat_step, false)

  if DiffEqBase.has_invW(f)
    mul!(vectmp, W, linsolve_tmp_vec)
  else
    cache.linsolve(vectmp, W, linsolve_tmp_vec, !repeat_step)
  end

  recursivecopy!(k₁, reshape(vectmp, sizeu...))
  @. u = uprev + dto2*k₁
  f(f₁,u,p,t+dto2)

  if mass_matrix == I
    tmp .= k₁
  else
    mul!(tmp,mass_matrix,k₁)
  end

  @. linsolve_tmp = f₁ - tmp

  if DiffEqBase.has_invW(f)
    mul!(vectmp2, W, linsolve_tmp_vec)
  else
    cache.linsolve(vectmp2, W, linsolve_tmp_vec)
  end

  tmp2 = reshape(vectmp2, sizeu...)
  @. k₂ = tmp2 + k₁
  @. tmp = uprev + dt*k₂
  f( fsallast,  tmp, p, t+dt)

  if mass_matrix == I
    @. linsolve_tmp = fsallast - c₃₂*(k₂-f₁) - 2(k₁-fsalfirst) + dt*dT
  else
    @. du2 = c₃₂*k₂ + 2k₁
    mul!(du1,mass_matrix,du2)
    @. linsolve_tmp = fsallast - du1 + c₃₂*f₁ + 2fsalfirst + dt*dT
  end

  if DiffEqBase.has_invW(f)
    mul!(vectmp3, W, linsolve_tmp_vec)
  else
    cache.linsolve(vectmp3, W, linsolve_tmp_vec)
  end

  k₃ = reshape(vectmp3, sizeu...)
  @. u = uprev + dto6*(k₁ + 4k₂ + k₃)

  if integrator.opts.adaptive
    @. tmp = dto6*(k₁ - 2*k₂ + k₃)
    # does not work with units - additional unitless array required!
    calculate_residuals!(tmp, tmp, uprev, u, integrator.opts.abstol, integrator.opts.reltol,integrator.opts.internalnorm)
    integrator.EEst = integrator.opts.internalnorm(tmp)
  end
end

@muladd function perform_step!(integrator, cache::Rosenbrock23ConstantCache, repeat_step=false)
  @unpack t,dt,uprev,u,f,p = integrator
  @unpack c₃₂,d,tf,uf = cache

  # Precalculations
  γ = dt*d
  dto2 = dt/2
  dto6 = dt/6

  # Time derivative
  dT = calc_tderivative(integrator, cache)

  W = calc_W!(integrator, cache, γ, repeat_step)

  k₁ = W\(integrator.fsalfirst + γ*dT)
  f₁ = f(uprev  + dto2*k₁, p, t+dto2)

  k₂ = W\(f₁-k₁) + k₁
  u = uprev  + dt*k₂

  if integrator.opts.adaptive
    integrator.fsallast = f(u, p, t+dt)

    k₃ = W\(integrator.fsallast - c₃₂*(k₂-f₁) - 2*(k₁-integrator.fsalfirst) + dt*dT)

    utilde =  dto6*(k₁ - 2*k₂ + k₃)
    atmp = calculate_residuals(utilde, uprev, u, integrator.opts.abstol,
                               integrator.opts.reltol,integrator.opts.internalnorm)
    integrator.EEst = integrator.opts.internalnorm(atmp)
  end

  integrator.k[1] = k₁
  integrator.k[2] = k₂
  integrator.u = u
end

@muladd function perform_step!(integrator, cache::Rosenbrock32ConstantCache, repeat_step=false)
  @unpack t,dt,uprev,u,f,p = integrator
  @unpack c₃₂,d,tf,uf = cache

  # Precalculations
  γ = dt*d
  dto2 = dt/2
  dto6 = dt/6

  # Time derivative
  dT = calc_tderivative(integrator, cache)

  W = calc_W!(integrator, cache, γ, repeat_step)

  #f₀ = f(uprev, p, t)

  k₁ = W\(integrator.fsalfirst + γ*dT)
  f₁ = f(uprev  + dto2*k₁, p, t+dto2)

  k₂ = W\(f₁-k₁) + k₁
  tmp = uprev  + dt*k₂
  integrator.fsallast = f(tmp, p, t+dt)

  k₃ = W\(integrator.fsallast - c₃₂*(k₂-f₁) - 2(k₁-integrator.fsalfirst) + dt*dT)
  u = uprev  + dto6*(k₁ + 4k₂ + k₃)

  if integrator.opts.adaptive
    utilde =  dto6*(k₁ - 2k₂ + k₃)
    atmp = calculate_residuals(utilde, uprev, u, integrator.opts.abstol,
                               integrator.opts.reltol,integrator.opts.internalnorm)
    integrator.EEst = integrator.opts.internalnorm(atmp)
  end

  integrator.k[1] = k₁
  integrator.k[2] = k₂
  integrator.u = u
end

function initialize!(integrator, cache::Union{Rosenbrock33ConstantCache,
                                              Rosenbrock34ConstantCache,
                                              Rosenbrock4ConstantCache,
                                              Rosenbrock5ConstantCache})
  integrator.kshortsize = 2
  integrator.k = typeof(integrator.k)(undef, integrator.kshortsize)
  integrator.fsalfirst = integrator.f(integrator.uprev, integrator.p, integrator.t)

  # Avoid undefined entries if k is an array of arrays
  integrator.fsallast = zero(integrator.fsalfirst)
  integrator.k[1] = integrator.fsalfirst
  integrator.k[2] = integrator.fsallast
end

function initialize!(integrator, cache::Union{Rosenbrock33Cache,
                                              Rosenbrock34Cache,
                                              Rosenbrock4Cache,
                                              Rosenbrock5Cache})
  integrator.kshortsize = 2
  @unpack fsalfirst,fsallast = cache
  integrator.fsalfirst = fsalfirst
  integrator.fsallast = fsallast
  resize!(integrator.k, integrator.kshortsize)
  integrator.k .= [fsalfirst,fsallast]
  integrator.f(integrator.fsalfirst, integrator.uprev, integrator.p, integrator.t)
end

@muladd function perform_step!(integrator, cache::Rosenbrock33ConstantCache,
                               repeat_step=false)
  @unpack t,dt,uprev,u,f,p = integrator
  @unpack tf,uf = cache
  @unpack a21,a31,a32,C21,C31,C32,b1,b2,b3,btilde1,btilde2,btilde3,gamma,c2,c3,d1,d2,d3 = cache.tab

  # Precalculations
  dtC21 = C21/dt
  dtC31 = C31/dt
  dtC32 = C32/dt

  dtd1 = dt*d1
  dtd2 = dt*d2
  dtd3 = dt*d3
  dtgamma = dt*gamma

  # Time derivative
  dT = calc_tderivative(integrator, cache)

  W = calc_W!(integrator, cache, dtgamma, repeat_step, true)

  linsolve_tmp =  integrator.fsalfirst + dtd1*dT

  k1 = W\linsolve_tmp
  u = uprev  + a21*k1
  du = f(u, p, t+c2*dt)

  linsolve_tmp =  du + dtd2*dT + dtC21*k1

  k2 = W\linsolve_tmp
  u = uprev  + a31*k1 + a32*k2
  du = f(u, p, t+c3*dt)

  linsolve_tmp =  du + dtd3*dT + dtC31*k1 + dtC32*k2

  k3 = W\linsolve_tmp
  u = uprev  + b1*k1 + b2*k2 + b3*k3
  integrator.fsallast = f(u, p, t + dt)

  if integrator.opts.adaptive
    utilde =  btilde1*k1 + btilde2*k2 + btilde3*k3
    atmp = calculate_residuals(utilde, uprev, u, integrator.opts.abstol,
                               integrator.opts.reltol,integrator.opts.internalnorm)
    integrator.EEst = integrator.opts.internalnorm(atmp)
  end

  integrator.k[1] = integrator.fsalfirst
  integrator.k[2] = integrator.fsallast
  integrator.u = u
end

@muladd function perform_step!(integrator, cache::Rosenbrock33Cache, repeat_step=false)
  @unpack t,dt,uprev,u,f,p = integrator
  @unpack du,du1,du2,vectmp,vectmp2,vectmp3,vectmp4,fsalfirst,fsallast,dT,J,W,uf,tf,linsolve_tmp,linsolve_tmp_vec,jac_config = cache
  @unpack a21,a31,a32,C21,C31,C32,b1,b2,b3,btilde1,btilde2,btilde3,gamma,c2,c3,d1,d2,d3 = cache.tab

  # Assignments
  mass_matrix = integrator.f.mass_matrix
  sizeu  = size(u)
  utilde = du
  atmp = du # does not work with units - additional unitless array required!

  # Precalculations
  dtC21 = C21/dt
  dtC31 = C31/dt
  dtC32 = C32/dt

  dtd1 = dt*d1
  dtd2 = dt*d2
  dtd3 = dt*d3
  dtgamma = dt*gamma

  calc_rosenbrock_differentiation!(integrator, cache, dtd1, dtgamma, repeat_step, true)

  if DiffEqBase.has_invW(f)
    mul!(vectmp, W, linsolve_tmp_vec)
  else
    cache.linsolve(vectmp, W, linsolve_tmp_vec, !repeat_step)
  end

  k1 = reshape(vectmp, sizeu...)
  @. u = uprev + a21*k1
  f( du,  u, p, t+c2*dt)

  if mass_matrix == I
    @. linsolve_tmp = du + dtd2*dT + dtC21*k1
  else
    @. du1 = dtC21*k1
    mul!(du2,mass_matrix,du1)
    @. linsolve_tmp = du + dtd2*dT + du2
  end

  if DiffEqBase.has_invW(f)
    mul!(vectmp2, W, linsolve_tmp_vec)
  else
    cache.linsolve(vectmp2, W, linsolve_tmp_vec)
  end

  k2 = reshape(vectmp2, sizeu...)
  @. u = uprev + a31*k1 + a32*k2
  f( du,  u, p, t+c3*dt)

  if mass_matrix == I
    @. linsolve_tmp = du + dtd3*dT + dtC31*k1 + dtC32*k2
  else
    @. du1 = dtC31*k1 + dtC32*k2
    mul!(du2,mass_matrix,du1)
    @. linsolve_tmp = du + dtd3*dT + du2
  end

  if DiffEqBase.has_invW(f)
    mul!(vectmp3, W, linsolve_tmp_vec)
  else
    cache.linsolve(vectmp3, W, linsolve_tmp_vec)
  end

  k3 = reshape(vectmp3, sizeu...)
  @. u = uprev + b1*k1 + b2*k2 + b3*k3
  f( fsallast,  u, p, t + dt)

  if integrator.opts.adaptive
    @. utilde = btilde1*k1 + btilde2*k2 + btilde3*k3
    calculate_residuals!(atmp, utilde, uprev, u, integrator.opts.abstol,
                         integrator.opts.reltol,integrator.opts.internalnorm)
    integrator.EEst = integrator.opts.internalnorm(atmp)
  end
end

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

@muladd function perform_step!(integrator, cache::Rosenbrock34ConstantCache, repeat_step=false)
  @unpack t,dt,uprev,u,f,p = integrator
  @unpack tf,uf = cache
  @unpack a21,a31,a32,C21,C31,C32,C41,C42,C43,b1,b2,b3,b4,btilde1,btilde2,btilde3,btilde4,gamma,c2,c3,d1,d2,d3,d4 = cache.tab

  # Precalculations
  dtC21 = C21/dt
  dtC31 = C31/dt
  dtC32 = C32/dt
  dtC41 = C41/dt
  dtC42 = C42/dt
  dtC43 = C43/dt

  dtd1 = dt*d1
  dtd2 = dt*d2
  dtd3 = dt*d3
  dtd4 = dt*d4
  dtgamma = dt*gamma

  # Time derivative
  tf.u = uprev
  dT = ForwardDiff.derivative(tf, t)

  W = calc_W!(integrator, cache, dtgamma, repeat_step, true)

  linsolve_tmp =  integrator.fsalfirst + dtd1*dT

  k1 = W\linsolve_tmp
  u = uprev # +a21*k1 a21 == 0
  # du = f(u, p, t+c2*dt) c2 == 0 and a21 == 0 => du = f(uprev, p, t) == fsalfirst

  linsolve_tmp =  integrator.fsalfirst + dtd2*dT + dtC21*k1

  k2 = W\linsolve_tmp
  u = uprev  + a31*k1 + a32*k2
  du = f(u, p, t+c3*dt)

  linsolve_tmp =  du + dtd3*dT + dtC31*k1 + dtC32*k2

  k3 = W\linsolve_tmp
  linsolve_tmp =  du + dtd4*dT + dtC41*k1 + dtC42*k2 + dtC43*k3

  k4 = W\linsolve_tmp
  u = uprev  + b1*k1 + b2*k2 + b3*k3 + b4*k4
  integrator.fsallast = f(u, p, t + dt)

  if integrator.opts.adaptive
    utilde =  btilde1*k1 + btilde2*k2 + btilde3*k3 + btilde4*k4
    atmp = calculate_residuals(utilde, uprev, u, integrator.opts.abstol,
                               integrator.opts.reltol,integrator.opts.internalnorm)
    integrator.EEst = integrator.opts.internalnorm(atmp)
  end

  integrator.k[1] = integrator.fsalfirst
  integrator.k[2] = integrator.fsallast
  integrator.u = u
end

@muladd function perform_step!(integrator, cache::Rosenbrock34Cache, repeat_step=false)
  @unpack t,dt,uprev,u,f,p = integrator
  @unpack du,du1,du2,vectmp,vectmp2,vectmp3,vectmp4,fsalfirst,fsallast,dT,J,W,uf,tf,linsolve_tmp,linsolve_tmp_vec,jac_config = cache
  @unpack a21,a31,a32,C21,C31,C32,C41,C42,C43,b1,b2,b3,b4,btilde1,btilde2,btilde3,btilde4,gamma,c2,c3,d1,d2,d3,d4 = cache.tab

  # Assignments
  uidx = eachindex(integrator.uprev)
  sizeu  = size(u)
  mass_matrix = integrator.f.mass_matrix
  utilde = du
  atmp = du # does not work with units - additional unitless array required!

  # Precalculations
  dtC21 = C21/dt
  dtC31 = C31/dt
  dtC32 = C32/dt
  dtC41 = C41/dt
  dtC42 = C42/dt
  dtC43 = C43/dt

  dtd1 = dt*d1
  dtd2 = dt*d2
  dtd3 = dt*d3
  dtd4 = dt*d4
  dtgamma = dt*gamma

  calc_rosenbrock_differentiation!(integrator, cache, dtd1, dtgamma, repeat_step, true)

  if DiffEqBase.has_invW(f)
    mul!(vectmp, W, linsolve_tmp_vec)
  else
    cache.linsolve(vectmp, W, linsolve_tmp_vec, !repeat_step)
  end

  k1 = reshape(vectmp, sizeu...)

  #=
  a21 == 0 and c2 == 0
  so du = fsalfirst!
  @. u = uprev + a21*k1

  f(du, u, p, t+c2*dt)
  =#

  if mass_matrix == I
    @. linsolve_tmp = fsalfirst + dtd2*dT + dtC21*k1
  else
    @. du1 = dtC21*k1
    mul!(du2,mass_matrix,du1)
    @. linsolve_tmp = fsalfirst + dtd2*dT + du2
  end

  if DiffEqBase.has_invW(f)
    mul!(vectmp2, W, linsolve_tmp_vec)
  else
    cache.linsolve(vectmp2, W, linsolve_tmp_vec)
  end

  k2 = reshape(vectmp2, sizeu...)
  @. u = uprev + a31*k1 + a32*k2
  f( du,  u, p, t+c3*dt)

  if mass_matrix == I
    @. linsolve_tmp = du + dtd3*dT + dtC31*k1 + dtC32*k2
  else
    @. du1 = dtC31*k1 + dtC32*k2
    mul!(du2,mass_matrix,du1)
    @. linsolve_tmp = du + dtd3*dT + du2
  end

  if DiffEqBase.has_invW(f)
    mul!(vectmp3, W, linsolve_tmp_vec)
  else
    cache.linsolve(vectmp3, W, linsolve_tmp_vec)
  end

  k3 = reshape(vectmp3, sizeu...)

  if mass_matrix == I
    @. linsolve_tmp = du + dtd4*dT + dtC41*k1 + dtC42*k2 + dtC43*k3
  else
    @. du1 = dtC41*k1 + dtC42*k2 + dtC43*k3
    mul!(du2,mass_matrix,du1)
    @. linsolve_tmp = du + dtd4*dT + du2
  end

  if DiffEqBase.has_invW(f)
    mul!(vectmp4, W, linsolve_tmp_vec)
  else
    cache.linsolve(vectmp4, W, linsolve_tmp_vec)
  end

  k4 = reshape(vectmp4, sizeu...)
  @. u = uprev + b1*k1 + b2*k2 + b3*k3 + b4*k4
  f( fsallast,  u, p, t + dt)

  if integrator.opts.adaptive
    @. utilde = btilde1*k1 + btilde2*k2 + btilde3*k3 + btilde4*k4
    calculate_residuals!(atmp, utilde, uprev, u, integrator.opts.abstol,
                         integrator.opts.reltol,integrator.opts.internalnorm)
    integrator.EEst = integrator.opts.internalnorm(atmp)
  end
end

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

#### ROS4 type method

@muladd function perform_step!(integrator, cache::Rosenbrock4ConstantCache, repeat_step=false)
  @unpack t,dt,uprev,u,f,p = integrator
  @unpack tf,uf = cache
  @unpack a21,a31,a32,C21,C31,C32,C41,C42,C43,b1,b2,b3,b4,btilde1,btilde2,btilde3,btilde4,gamma,c2,c3,d1,d2,d3,d4 = cache.tab

  # Precalculations
  dtC21 = C21/dt
  dtC31 = C31/dt
  dtC32 = C32/dt
  dtC41 = C41/dt
  dtC42 = C42/dt
  dtC43 = C43/dt

  dtd1 = dt*d1
  dtd2 = dt*d2
  dtd3 = dt*d3
  dtd4 = dt*d4
  dtgamma = dt*gamma

  # Time derivative
  tf.u = uprev
  dT = ForwardDiff.derivative(tf, t)

  W = calc_W!(integrator, cache, dtgamma, repeat_step, true)

  linsolve_tmp =  integrator.fsalfirst + dtd1*dT

  k1 = W\linsolve_tmp
  u = uprev +a21*k1
  du = f(u, p, t+c2*dt)

  linsolve_tmp =  du + dtd2*dT + dtC21*k1

  k2 = W\linsolve_tmp
  u = uprev  + a31*k1 + a32*k2
  du = f(u, p, t+c3*dt)

  linsolve_tmp =  du + dtd3*dT + dtC31*k1 + dtC32*k2

  k3 = W\linsolve_tmp

  linsolve_tmp =  du + dtd4*dT + dtC41*k1 + dtC42*k2 + dtC43*k3

  k4 = W\linsolve_tmp
  u = uprev  + b1*k1 + b2*k2 + b3*k3 + b4*k4
  integrator.fsallast = f(u, p, t + dt)

  if integrator.opts.adaptive
    utilde =  btilde1*k1 + btilde2*k2 + btilde3*k3 + btilde4*k4
    atmp = calculate_residuals(utilde, uprev, u, integrator.opts.abstol,
                               integrator.opts.reltol,integrator.opts.internalnorm)
    integrator.EEst = integrator.opts.internalnorm(atmp)
  end

  integrator.k[1] = integrator.fsalfirst
  integrator.k[2] = integrator.fsallast
  integrator.u = u
end

@muladd function perform_step!(integrator, cache::Rosenbrock4Cache, repeat_step=false)
  @unpack t,dt,uprev,u,f,p = integrator
  @unpack du,du1,du2,vectmp,vectmp2,vectmp3,vectmp4,fsalfirst,fsallast,dT,J,W,uf,tf,linsolve_tmp,linsolve_tmp_vec,jac_config = cache
  @unpack a21,a31,a32,C21,C31,C32,C41,C42,C43,b1,b2,b3,b4,btilde1,btilde2,btilde3,btilde4,gamma,c2,c3,d1,d2,d3,d4 = cache.tab

  # Assignments
  sizeu  = size(u)
  uidx = eachindex(integrator.uprev)
  mass_matrix = integrator.f.mass_matrix
  utilde = du
  atmp = du # does not work with units - additional unitless array required!

  dtC21 = C21/dt
  dtC31 = C31/dt
  dtC32 = C32/dt
  dtC41 = C41/dt
  dtC42 = C42/dt
  dtC43 = C43/dt

  dtd1 = dt*d1
  dtd2 = dt*d2
  dtd3 = dt*d3
  dtd4 = dt*d4
  dtgamma = dt*gamma

  calc_rosenbrock_differentiation!(integrator, cache, dtd1, dtgamma, repeat_step, true)

  if DiffEqBase.has_invW(f)
    mul!(vectmp, W, linsolve_tmp_vec)
  else
    cache.linsolve(vectmp, W, linsolve_tmp_vec, !repeat_step)
  end

  k1 = reshape(vectmp, sizeu...)
  @. u = uprev + a21*k1
  f( du,  u, p, t+c2*dt)

  if mass_matrix == I
    @. linsolve_tmp = du + dtd2*dT + dtC21*k1
  else
    @. du1 = dtC21*k1
    mul!(du2,mass_matrix,du1)
    @. linsolve_tmp = du + dtd2*dT + du2
  end

  if DiffEqBase.has_invW(f)
    mul!(vectmp2, W, linsolve_tmp_vec)
  else
    cache.linsolve(vectmp2, W, linsolve_tmp_vec)
  end

  k2 = reshape(vectmp2, sizeu...)
  @. u = uprev + a31*k1 + a32*k2
  f( du,  u, p, t+c3*dt)

  if mass_matrix == I
    @. linsolve_tmp = du + dtd3*dT + dtC31*k1 + dtC32*k2
  else
    @. du1 = dtC31*k1 + dtC32*k2
    mul!(du2,mass_matrix,du1)
    @. linsolve_tmp = du + dtd3*dT + du2
  end

  if DiffEqBase.has_invW(f)
    mul!(vectmp3, W, linsolve_tmp_vec)
  else
    cache.linsolve(vectmp3, W, linsolve_tmp_vec)
  end

  k3 = reshape(vectmp3, sizeu...)

  if mass_matrix == I
    @. linsolve_tmp = du + dtd4*dT + dtC41*k1 + dtC42*k2 + dtC43*k3
  else
    @. du1 = dtC41*k1 + dtC42*k2 + dtC43*k3
    mul!(du2,mass_matrix,du1)
    @. linsolve_tmp = du + dtd4*dT + du2
  end

  if DiffEqBase.has_invW(f)
    mul!(vectmp4, W, linsolve_tmp_vec)
  else
    cache.linsolve(vectmp4, W, linsolve_tmp_vec)
  end

  k4 = reshape(vectmp4, sizeu...)
  @. u = uprev + b1*k1 + b2*k2 + b3*k3 + b4*k4
  f( fsallast,  u, p, t + dt)

  if integrator.opts.adaptive
    @. utilde = btilde1*k1 + btilde2*k2 + btilde3*k3 + btilde4*k4
    calculate_residuals!(atmp, utilde, uprev, u, integrator.opts.abstol,
                         integrator.opts.reltol,integrator.opts.internalnorm)
    integrator.EEst = integrator.opts.internalnorm(atmp)
  end
end

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

#### Rodas4 type method

function initialize!(integrator, cache::Rodas4ConstantCache)
  integrator.kshortsize = 2
  integrator.k = typeof(integrator.k)(undef, integrator.kshortsize)
  # Avoid undefined entries if k is an array of arrays
  integrator.k[1] = zero(integrator.u)
  integrator.k[2] = zero(integrator.u)
end

@muladd function perform_step!(integrator, cache::Rodas4ConstantCache, repeat_step=false)
  @unpack t,dt,uprev,u,f,p = integrator
  @unpack tf,uf = cache
  @unpack a21,a31,a32,a41,a42,a43,a51,a52,a53,a54,C21,C31,C32,C41,C42,C43,C51,C52,C53,C54,C61,C62,C63,C64,C65,gamma,c2,c3,c4,d1,d2,d3,d4 = cache.tab

  # Precalculations
  dtC21 = C21/dt
  dtC31 = C31/dt
  dtC32 = C32/dt
  dtC41 = C41/dt
  dtC42 = C42/dt
  dtC43 = C43/dt
  dtC51 = C51/dt
  dtC52 = C52/dt
  dtC53 = C53/dt
  dtC54 = C54/dt
  dtC61 = C61/dt
  dtC62 = C62/dt
  dtC63 = C63/dt
  dtC64 = C64/dt
  dtC65 = C65/dt

  dtd1 = dt*d1
  dtd2 = dt*d2
  dtd3 = dt*d3
  dtd4 = dt*d4
  dtgamma = dt*gamma

  # Time derivative
  tf.u = uprev
  dT = ForwardDiff.derivative(tf, t)

  W = calc_W!(integrator, cache, dtgamma, repeat_step, true)

  du = f(uprev, p, t)

  linsolve_tmp =  du + dtd1*dT

  k1 = W\linsolve_tmp
  u = uprev  + a21*k1
  du = f(u, p, t+c2*dt)

  linsolve_tmp =  du + dtd2*dT + dtC21*k1

  k2 = W\linsolve_tmp
  u = uprev  + a31*k1 + a32*k2
  du = f(u, p, t+c3*dt)

  linsolve_tmp =  du + dtd3*dT + (dtC31*k1 + dtC32*k2)

  k3 = W\linsolve_tmp
  u = uprev  + a41*k1 + a42*k2 + a43*k3
  du = f(u, p, t+c4*dt)

  linsolve_tmp =  du + dtd4*dT + (dtC41*k1 + dtC42*k2 + dtC43*k3)

  k4 = W\linsolve_tmp
  u = uprev  + a51*k1 + a52*k2 + a53*k3 + a54*k4
  du = f(u, p, t+dt)

  linsolve_tmp =  du + (dtC52*k2 + dtC54*k4 + dtC51*k1 + dtC53*k3)

  k5 = W\linsolve_tmp
  u = u + k5
  du = f(u, p, t+dt)

  linsolve_tmp =  du + (dtC61*k1 + dtC62*k2 + dtC65*k5 + dtC64*k4 + dtC63*k3)

  k6 = W\linsolve_tmp
  u = u + k6

  if integrator.opts.adaptive
    atmp = calculate_residuals(k6, uprev, u, integrator.opts.abstol,
                               integrator.opts.reltol,integrator.opts.internalnorm)
    integrator.EEst = integrator.opts.internalnorm(atmp)
  end

  if integrator.opts.calck
    @unpack h21,h22,h23,h24,h25,h31,h32,h33,h34,h35 = cache.tab
    integrator.k[1] =  h21*k1 + h22*k2 + h23*k3 + h24*k4 + h25*k5
    integrator.k[2] =  h31*k1 + h32*k2 + h33*k3 + h34*k4 + h35*k5
  end
  integrator.u = u
end

function initialize!(integrator, cache::Rodas4Cache)
  integrator.kshortsize = 2
  @unpack dense1,dense2 = cache
  resize!(integrator.k, integrator.kshortsize)
  integrator.k .= [dense1,dense2]
end

@muladd function perform_step!(integrator, cache::Rodas4Cache, repeat_step=false)
  @unpack t,dt,uprev,u,f,p = integrator
  @unpack du,du1,du2,vectmp,vectmp2,vectmp3,vectmp4,vectmp5,vectmp6,dT,J,W,uf,tf,linsolve_tmp,linsolve_tmp_vec,jac_config = cache
  @unpack a21,a31,a32,a41,a42,a43,a51,a52,a53,a54,C21,C31,C32,C41,C42,C43,C51,C52,C53,C54,C61,C62,C63,C64,C65,gamma,c2,c3,c4,d1,d2,d3,d4 = cache.tab

  # Assignments
  sizeu  = size(u)
  uidx = eachindex(integrator.uprev)
  mass_matrix = integrator.f.mass_matrix
  atmp = du # does not work with units - additional unitless array required!

  # Precalculations
  dtC21 = C21/dt
  dtC31 = C31/dt
  dtC32 = C32/dt
  dtC41 = C41/dt
  dtC42 = C42/dt
  dtC43 = C43/dt
  dtC51 = C51/dt
  dtC52 = C52/dt
  dtC53 = C53/dt
  dtC54 = C54/dt
  dtC61 = C61/dt
  dtC62 = C62/dt
  dtC63 = C63/dt
  dtC64 = C64/dt
  dtC65 = C65/dt

  dtd1 = dt*d1
  dtd2 = dt*d2
  dtd3 = dt*d3
  dtd4 = dt*d4
  dtgamma = dt*gamma

  calc_rosenbrock_differentiation!(integrator, cache, dtd1, dtgamma, repeat_step, true)

  if DiffEqBase.has_invW(f)
    mul!(vectmp, W, linsolve_tmp_vec)
  else
    cache.linsolve(vectmp, W, linsolve_tmp_vec, !repeat_step)
  end

  k1 = reshape(vectmp, sizeu...)
  @. u = uprev + a21*k1
  f( du,  u, p, t+c2*dt)

  if mass_matrix == I
    @. linsolve_tmp = du + dtd2*dT + dtC21*k1
  else
    @. du1 = dtC21*k1
    mul!(du2,mass_matrix,du1)
    @. linsolve_tmp = du + dtd2*dT + du2
  end

  if DiffEqBase.has_invW(f)
    mul!(vectmp2, W, linsolve_tmp_vec)
  else
    cache.linsolve(vectmp2, W, linsolve_tmp_vec)
  end

  k2 = reshape(vectmp2, sizeu...)
  @. u = uprev + a31*k1 + a32*k2
  f( du,  u, p, t+c3*dt)

  if mass_matrix == I
    @. linsolve_tmp = du + dtd3*dT + (dtC31*k1 + dtC32*k2)
  else
    @. du1 = dtC31*k1 + dtC32*k2
    mul!(du2,mass_matrix,du1)
    @. linsolve_tmp = du + dtd3*dT + du2
  end

  if DiffEqBase.has_invW(f)
    mul!(vectmp3, W, linsolve_tmp_vec)
  else
    cache.linsolve(vectmp3, W, linsolve_tmp_vec)
  end

  k3 = reshape(vectmp3, sizeu...)
  @. u = uprev + a41*k1 + a42*k2 + a43*k3
  f( du,  u, p, t+c4*dt)

  if mass_matrix == I
    @. linsolve_tmp = du + dtd4*dT + (dtC41*k1 + dtC42*k2 + dtC43*k3)
  else
    @. du1 = dtC41*k1 + dtC42*k2 + dtC43*k3
    mul!(du2,mass_matrix,du1)
    @. linsolve_tmp = du + dtd4*dT + du2
  end

  if DiffEqBase.has_invW(f)
    mul!(vectmp4, W, linsolve_tmp_vec)
  else
    cache.linsolve(vectmp4, W, linsolve_tmp_vec)
  end

  k4 = reshape(vectmp4, sizeu...)
  @. u = uprev + a51*k1 + a52*k2 + a53*k3 + a54*k4
  f( du,  u, p, t+dt)

  if mass_matrix == I
    @. linsolve_tmp = du + (dtC52*k2 + dtC54*k4 + dtC51*k1 + dtC53*k3)
  else
    @. du1 = dtC52*k2 + dtC54*k4 + dtC51*k1 + dtC53*k3
    mul!(du2,mass_matrix,du1)
    @. linsolve_tmp = du + du2
  end

  if DiffEqBase.has_invW(f)
    mul!(vectmp5, W, linsolve_tmp_vec)
  else
    cache.linsolve(vectmp5, W, linsolve_tmp_vec)
  end

  k5 = reshape(vectmp5, sizeu...)
  u .+= k5
  f( du,  u, p, t + dt)

  if mass_matrix == I
    # @. linsolve_tmp = du + (dtC61*k1 + dtC62*k2 + dtC65*k5 + dtC64*k4 + dtC63*k3)
    @tight_loop_macros for i in uidx
      @inbounds linsolve_tmp[i] = du[i] + (dtC61*k1[i] + dtC62*k2[i] + dtC65*k5[i] + dtC64*k4[i] + dtC63*k3[i])
    end
  else
    # @. linsolve_tmp = dtC61*k1 + dtC62*k2 + dtC65*k5 + dtC64*k4 + dtC63*k3
    @tight_loop_macros for i in uidx
      @inbounds du1[i] = dtC61*k1[i] + dtC62*k2[i] + dtC65*k5[i] + dtC64*k4[i] + dtC63*k3[i]
    end
    mul!(du2,mass_matrix,du1)
    @. linsolve_tmp = du + du2
  end

  if DiffEqBase.has_invW(f)
    mul!(vectmp6, W, linsolve_tmp_vec)
  else
    cache.linsolve(vectmp6, W, linsolve_tmp_vec)
  end

  k6 = reshape(vectmp6, sizeu...)
  u .+= k6

  if integrator.opts.adaptive
    calculate_residuals!(atmp, k6, uprev, u, integrator.opts.abstol,
                         integrator.opts.reltol,integrator.opts.internalnorm)
    integrator.EEst = integrator.opts.internalnorm(atmp)
  end

  if integrator.opts.calck
    @unpack h21,h22,h23,h24,h25,h31,h32,h33,h34,h35 = cache.tab
    @. integrator.k[1] = h21*k1 + h22*k2 + h23*k3 + h24*k4 + h25*k5
    @. integrator.k[2] = h31*k1 + h32*k2 + h33*k3 + h34*k4 + h35*k5
  end
end

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

### Rodas5 Method

@muladd function perform_step!(integrator, cache::Rosenbrock5ConstantCache, repeat_step=false)
  @unpack t,dt,uprev,u,f,p = integrator
  @unpack tf,uf = cache
  @unpack a21,a31,a32,a41,a42,a43,a51,a52,a53,a54,a61,a62,a63,a64,a65,C21,C31,C32,C41,C42,C43,C51,C52,C53,C54,C61,C62,C63,C64,C65,C71,C72,C73,C74,C75,C76,C81,C82,C83,C84,C85,C86,C87,gamma,d1,d2,d3,d4,d5,c2,c3,c4,c5 = cache.tab

  # Precalculations
  dtC21 = C21/dt
  dtC31 = C31/dt
  dtC32 = C32/dt
  dtC41 = C41/dt
  dtC42 = C42/dt
  dtC43 = C43/dt
  dtC51 = C51/dt
  dtC52 = C52/dt
  dtC53 = C53/dt
  dtC54 = C54/dt
  dtC61 = C61/dt
  dtC62 = C62/dt
  dtC63 = C63/dt
  dtC64 = C64/dt
  dtC65 = C65/dt
  dtC71 = C71/dt
  dtC72 = C72/dt
  dtC73 = C73/dt
  dtC74 = C74/dt
  dtC75 = C75/dt
  dtC76 = C76/dt
  dtC81 = C81/dt
  dtC82 = C82/dt
  dtC83 = C83/dt
  dtC84 = C84/dt
  dtC85 = C85/dt
  dtC86 = C86/dt
  dtC87 = C87/dt

  dtd1 = dt*d1
  dtd2 = dt*d2
  dtd3 = dt*d3
  dtd4 = dt*d4
  dtd5 = dt*d5
  dtgamma = dt*gamma

  # Time derivative
  dT = calc_tderivative(integrator, cache)

  W = calc_W!(integrator, cache, dtgamma, repeat_step, true)

  du1 = f(uprev, p, t)

  linsolve_tmp =  du1 + dtd1*dT

  k1 = W\linsolve_tmp
  u = uprev  + a21*k1
  du = f(u, p, t+c2*dt)

  linsolve_tmp =  du + dtd2*dT + dtC21*k1

  k2 = W\linsolve_tmp
  u = uprev  + a31*k1 + a32*k2
  du = f(u, p, t+c3*dt)

  linsolve_tmp =  du + dtd3*dT + (dtC31*k1 + dtC32*k2)

  k3 = W\linsolve_tmp
  u = uprev  + a41*k1 + a42*k2 + a43*k3
  du = f(u, p, t+c4*dt)

  linsolve_tmp =  du + dtd4*dT + (dtC41*k1 + dtC42*k2 + dtC43*k3)

  k4 = W\linsolve_tmp
  u = uprev  + a51*k1 + a52*k2 + a53*k3 + a54*k4
  du = f(u, p, t+c5*dt)

  linsolve_tmp = du + dtd5*dT + (dtC52*k2 + dtC54*k4 + dtC51*k1 + dtC53*k3)

  k5 = W\linsolve_tmp
  u = uprev  + a61*k1 + a62*k2 + a63*k3 + a64*k4 + a65*k5
  du = f(u, p, t+dt)

  linsolve_tmp =  du + (dtC61*k1 + dtC62*k2 + dtC63*k3 + dtC64*k4 + dtC65*k5)

  k6 = W\linsolve_tmp
  u = u + k6
  du = f(u, p, t+dt)

  linsolve_tmp = du + (dtC71*k1 + dtC72*k2 + dtC73*k3 + dtC74*k4 + dtC75*k5 + dtC76*k6)

  k7 = W\linsolve_tmp
  u = u + k7
  du = f(u, p, t+dt)

  linsolve_tmp = du + (dtC81*k1 + dtC82*k2 + dtC83*k3 + dtC84*k4 + dtC85*k5 + dtC86*k6 + dtC87*k7)

  k8 = W\linsolve_tmp
  u = u + k8
  du = f(u, p, t+dt)

  if integrator.opts.adaptive
    atmp = calculate_residuals(k8, uprev, u, integrator.opts.abstol,
                               integrator.opts.reltol,integrator.opts.internalnorm)
    integrator.EEst = integrator.opts.internalnorm(atmp)
  end

  if integrator.opts.calck
    #=
    @unpack h21,h22,h23,h24,h25,h31,h32,h33,h34,h35 = cache.tab
    integrator.k[1] =  h21*k1 + h22*k2 + h23*k3 + h24*k4 + h25*k5
    integrator.k[2] =  h31*k1 + h32*k2 + h33*k3 + h34*k4 + h35*k5
    =#
    integrator.k[1] = du1
    integrator.k[2] = du
  end

  integrator.fsallast = du
  integrator.u = u
end

@muladd function perform_step!(integrator, cache::Rosenbrock5Cache, repeat_step=false)
  @unpack t,dt,uprev,u,f,p = integrator
  @unpack du,du1,du2,vectmp,vectmp2,vectmp3,vectmp4,vectmp5,vectmp6,vectmp7,vectmp8,fsalfirst,fsallast,dT,J,W,uf,tf,linsolve_tmp,linsolve_tmp_vec,jac_config = cache
  @unpack a21,a31,a32,a41,a42,a43,a51,a52,a53,a54,a61,a62,a63,a64,a65,C21,C31,C32,C41,C42,C43,C51,C52,C53,C54,C61,C62,C63,C64,C65,C71,C72,C73,C74,C75,C76,C81,C82,C83,C84,C85,C86,C87,gamma,d1,d2,d3,d4,d5,c2,c3,c4,c5 = cache.tab

  # Assignments
  sizeu  = size(u)
  uidx = eachindex(integrator.uprev)
  mass_matrix = integrator.f.mass_matrix
  atmp = du # does not work with units - additional unitless array required!

  # Precalculations
  dtC21 = C21/dt
  dtC31 = C31/dt
  dtC32 = C32/dt
  dtC41 = C41/dt
  dtC42 = C42/dt
  dtC43 = C43/dt
  dtC51 = C51/dt
  dtC52 = C52/dt
  dtC53 = C53/dt
  dtC54 = C54/dt
  dtC61 = C61/dt
  dtC62 = C62/dt
  dtC63 = C63/dt
  dtC64 = C64/dt
  dtC65 = C65/dt
  dtC71 = C71/dt
  dtC72 = C72/dt
  dtC73 = C73/dt
  dtC74 = C74/dt
  dtC75 = C75/dt
  dtC76 = C76/dt
  dtC81 = C81/dt
  dtC82 = C82/dt
  dtC83 = C83/dt
  dtC84 = C84/dt
  dtC85 = C85/dt
  dtC86 = C86/dt
  dtC87 = C87/dt

  dtd1 = dt*d1
  dtd2 = dt*d2
  dtd3 = dt*d3
  dtd4 = dt*d4
  dtd5 = dt*d5
  dtgamma = dt*gamma

  calc_rosenbrock_differentiation!(integrator, cache, dtd1, dtgamma, repeat_step, true)

  if DiffEqBase.has_invW(f)
    mul!(vectmp, W, linsolve_tmp_vec)
  else
    cache.linsolve(vectmp, W, linsolve_tmp_vec, !repeat_step)
  end

  k1 = reshape(vectmp, sizeu...)
  @. u = uprev + a21*k1
  f( du,  u, p, t+c2*dt)

  if mass_matrix == I
    @. linsolve_tmp = du + dtd2*dT + dtC21*k1
  else
    @. du1 = dtC21*k1
    mul!(du2,mass_matrix,du1)
    @. linsolve_tmp = du + dtd2*dT + du2
  end

  if DiffEqBase.has_invW(f)
    mul!(vectmp2, W, linsolve_tmp_vec)
  else
    cache.linsolve(vectmp2, W, linsolve_tmp_vec)
  end

  k2 = reshape(vectmp2, sizeu...)
  @. u = uprev + a31*k1 + a32*k2
  f( du,  u, p, t+c3*dt)

  if mass_matrix == I
    @. linsolve_tmp = du + dtd3*dT + (dtC31*k1 + dtC32*k2)
  else
    @. du1 = dtC31*k1 + dtC32*k2
    mul!(du2,mass_matrix,du1)
    @. linsolve_tmp = du + dtd3*dT + du2
  end

  if DiffEqBase.has_invW(f)
    mul!(vectmp3, W, linsolve_tmp_vec)
  else
    cache.linsolve(vectmp3, W, linsolve_tmp_vec)
  end

  k3 = reshape(vectmp3, sizeu...)
  @. u = uprev + a41*k1 + a42*k2 + a43*k3
  f( du,  u, p, t+c4*dt)

  if mass_matrix == I
    @. linsolve_tmp = du + dtd4*dT + (dtC41*k1 + dtC42*k2 + dtC43*k3)
  else
    @. du1 = dtC41*k1 + dtC42*k2 + dtC43*k3
    mul!(du2,mass_matrix,du1)
    @. linsolve_tmp = du + dtd4*dT + du2
  end

  if DiffEqBase.has_invW(f)
    mul!(vectmp4, W, linsolve_tmp_vec)
  else
    cache.linsolve(vectmp4, W, linsolve_tmp_vec)
  end

  k4 = reshape(vectmp4, sizeu...)
  @. u = uprev + a51*k1 + a52*k2 + a53*k3 + a54*k4
  f( du,  u, p, t+c5*dt)

  if mass_matrix == I
    #  @. linsolve_tmp = du + dtd5*dT + (dtC52*k2 + dtC54*k4 + dtC51*k1 + dtC53*k3)
      @tight_loop_macros for i in uidx
        @inbounds linsolve_tmp[i] = du[i] + dtd5*dT[i] + (dtC52*k2[i] + dtC54*k4[i] + dtC51*k1[i] + dtC53*k3[i])
      end
  else
    @. du1 = dtC52*k2 + dtC54*k4 + dtC51*k1 + dtC53*k3
    mul!(du2,mass_matrix,du1)
    @. linsolve_tmp = du + dtd5*dT + du2
  end

  if DiffEqBase.has_invW(f)
    mul!(vectmp5, W, linsolve_tmp_vec)
  else
    cache.linsolve(vectmp5, W, linsolve_tmp_vec)
  end

  k5 = reshape(vectmp5, sizeu...)
  # @. u = uprev + a61*k1 + a62*k2 + a63*k3 + a64*k4 + a65*k5
  @tight_loop_macros for i in uidx
    @inbounds u[i] = uprev[i] + a61*k1[i] + a62*k2[i] + a63*k3[i] + a64*k4[i] + a65*k5[i]
  end
  f( du,  u, p, t+dt)

  if mass_matrix == I
    # @. linsolve_tmp = du + (dtC61*k1 + dtC62*k2 + dtC63*k3 + dtC64*k4 + dtC65*k5)
    @tight_loop_macros for i in uidx
      @inbounds linsolve_tmp[i] = du[i] + (dtC61*k1[i] + dtC62*k2[i] + dtC63*k3[i] + dtC64*k4[i] + dtC65*k5[i])
    end
  else
    # @. du1 = dtC61*k1 + dtC62*k2 + dtC63*k3 + dtC64*k4 + dtC65*k5
    @tight_loop_macros for i in uidx
      @inbounds du1[i] = dtC61*k1[i] + dtC62*k2[i] + dtC63*k3[i] + dtC64*k4[i] + dtC65*k5[i]
    end
    mul!(du2,mass_matrix,du1)
    @. linsolve_tmp = du + du2
  end

  if DiffEqBase.has_invW(f)
    mul!(vectmp6, W, linsolve_tmp_vec)
  else
    cache.linsolve(vectmp6, W, linsolve_tmp_vec)
  end

  k6 = reshape(vectmp6, sizeu...)
  u .+= k6
  f( du,  u, p, t+dt)

  if mass_matrix == I
    # @. linsolve_tmp = du + (dtC71*k1 + dtC72*k2 + dtC73*k3 + dtC74*k4 + dtC75*k5 + dtC76*k6)
    @tight_loop_macros for i in uidx
      @inbounds linsolve_tmp[i] = du[i] + (dtC71*k1[i] + dtC72*k2[i] + dtC73*k3[i] + dtC74*k4[i] + dtC75*k5[i] + dtC76*k6[i])
    end
  else
    # @. du1 =dtC72*k2 + dtC73*k3 + dtC74*k4 + dtC75*k5 + dtC76*k6
    @tight_loop_macros for i in uidx
      @inbounds du1[i] = dtC71*k1[i] + dtC72*k2[i] + dtC73*k3[i] + dtC74*k4[i] + dtC75*k5[i] + dtC76*k6[i]
    end
    mul!(du2,mass_matrix,du1)
    @. linsolve_tmp = du + du2
  end

  if DiffEqBase.has_invW(f)
    mul!(vectmp7, W, linsolve_tmp_vec)
  else
    cache.linsolve(vectmp7, W, linsolve_tmp_vec)
  end

  k7 = reshape(vectmp7, sizeu...)
  u .+= k7
  f( du,  u, p, t+dt)

  if mass_matrix == I
    # @. linsolve_tmp = du + (dtC81*k1 + dtC82*k2 + dtC83*k3 + dtC84*k4 + dtC85*k5 + dtC86*k6 + dtC87*k7)
    @tight_loop_macros for i in uidx
      @inbounds linsolve_tmp[i] = du[i] + (dtC81*k1[i] + dtC82*k2[i] + dtC83*k3[i] + dtC84*k4[i] + dtC85*k5[i] + dtC86*k6[i] + dtC87*k7[i])
    end
  else
    # @. du1 = dtC81*k1 + dtC82*k2 + dtC83*k3 + dtC84*k4 + dtC85*k5 + dtC86*k6 + dtC87*k7
    @tight_loop_macros for i in uidx
      @inbounds du1[i] = dtC81*k1[i] + dtC82*k2[i] + dtC83*k3[i] + dtC84*k4[i] + dtC85*k5[i] + dtC86*k6[i] + dtC87*k7[i]
    end
    mul!(du2,mass_matrix,du1)
    @. linsolve_tmp = du + du2
  end

  if DiffEqBase.has_invW(f)
    mul!(vectmp8, W, linsolve_tmp_vec)
  else
    cache.linsolve(vectmp8, W, linsolve_tmp_vec)
  end

  k8 = reshape(vectmp8, sizeu...)
  u .+= k8
  f( fsallast,  u, p, t+dt)

  if integrator.opts.adaptive
    calculate_residuals!(atmp, k8, uprev, u, integrator.opts.abstol,
                         integrator.opts.reltol,integrator.opts.internalnorm)
    integrator.EEst = integrator.opts.internalnorm(atmp)
  end

  if integrator.opts.calck
    #=
    @unpack h21,h22,h23,h24,h25,h31,h32,h33,h34,h35 = cache.tab
    @. integrator.k[1] = h21*k1 + h22*k2 + h23*k3 + h24*k4 + h25*k5
    @. integrator.k[2] = h31*k1 + h32*k2 + h33*k3 + h34*k4 + h35*k5
    =#
  end
end