text:
function _precompile_()
    ccall(:jl_generating_output, Cint, ()) == 1 || return nothing
    Base.precompile(Tuple{typeof(gen_ifunc),Module,Expr})   # time: 1.9265794
    Base.precompile(Tuple{typeof(render_arg),Expr})   # time: 0.045112614
    Base.precompile(Tuple{typeof(dual_ex),Module,Expr})   # time: 0.04482814
    Base.precompile(Tuple{typeof(memkernel),Expr})   # time: 0.042466346
    Base.precompile(Tuple{typeof(functionfoot),Vector{Any}})   # time: 0.028968
    Base.precompile(Tuple{PlusEq{typeof(cos)},Float64,Float64})   # time: 0.024845816
    Base.precompile(Tuple{typeof(assign_ex),Expr,Expr,Bool})   # time: 0.02307638
    Base.precompile(Tuple{typeof(pushvar!),Vector{Symbol},Expr})   # time: 0.008682777
    Base.precompile(Tuple{typeof(precom_opm),Symbol,Symbol,Expr})   # time: 0.024082141
    Base.precompile(Tuple{typeof(precom_opm),Symbol,Expr,Expr})   # time: 0.014433064
    Base.precompile(Tuple{PlusEq{typeof(sin)},Float64,Float64})   # time: 0.004275546
    Base.precompile(Tuple{typeof(get_argname),Expr})   # time: 0.00546523
    Base.precompile(Tuple{typeof(forstatement),Symbol,Expr,Vector{Any},CompileInfo,Nothing})   # time: 0.003776189
    Base.precompile(Tuple{typeof(julia_usevar!),SymbolTable,Expr})   # time: 0.049144164
    Base.precompile(Tuple{typeof(julia_usevar!),SymbolTable,Symbol})   # time: 0.003254819
    Base.precompile(Tuple{typeof(precom_ex),Module,LineNumberNode,PreInfo})   # time: 0.001450645
    Base.precompile(Tuple{typeof(precom_body),Module,SubArray{Any, 1, Vector{Any}, Tuple{UnitRange{Int64}}, true},PreInfo})   # time: 0.001423381
    Base.precompile(Tuple{typeof(precom_body),Module,Vector{Any},PreInfo})   # time: 0.001126582
    Base.precompile(Tuple{typeof(variable_analysis_ex),Expr,SymbolTable})   # time: 0.31368348
    Base.precompile(Tuple{typeof(precom_ex),Module,Expr,PreInfo})   # time: 0.20325074
    Base.precompile(Tuple{typeof(compile_ex),Module,Expr,CompileInfo})   # time: 0.19640462
    Base.precompile(Tuple{Core.kwftype(typeof(almost_same)),NamedTuple{(:atol,), Tuple{Float64}},typeof(almost_same),Vector{Int64},Vector{Int64}})   # time: 0.080980584
    Base.precompile(Tuple{typeof(unzipped_broadcast),PlusEq{typeof(*)},Tuple{Float64, Float64},Tuple{Float64, Float64},Tuple{Float64, Float64}})   # time: 0.07962415
    Base.precompile(Tuple{typeof(unzipped_broadcast),PlusEq{typeof(identity)},Vector{Float64},Vector{Float64}})   # time: 0.060837016
    Base.precompile(Tuple{typeof(almost_same),Vector{Float64},Vector{Float64}})   # time: 0.043818954
    Base.precompile(Tuple{typeof(functionfoot),Vector{Expr}})   # time: 0.03988443
    Base.precompile(Tuple{typeof(assign_ex),Expr,Symbol,Bool})   # time: 0.03273888
    Base.precompile(Tuple{typeof(assign_ex),Expr,Float64,Bool})   # time: 0.031806484
    Base.precompile(Tuple{typeof(Base.show_function),IOContext{IOBuffer},PlusEq{typeof(abs2)},Bool})   # time: 0.025549678
    Base.precompile(Tuple{typeof(precom_opm),Symbol,Symbol,Symbol})   # time: 0.023482375
    Base.precompile(Tuple{PlusEq{typeof(^)},ComplexF64,ComplexF64,Vararg{ComplexF64, N} where N})   # time: 0.018600011
    Base.precompile(Tuple{typeof(functionfoot),Vector{Symbol}})   # time: 0.018092046
    Base.precompile(Tuple{PlusEq{typeof(tan)},Float64,Float64})   # time: 0.015275044
    Base.precompile(Tuple{typeof(precom_if),Module,Expr,PreInfo})   # time: 0.010871046
    Base.precompile(Tuple{MinusEq{typeof(^)},Union{Float64, ComplexF64},Union{Float64, ComplexF64},Union{Float64, ComplexF64}})   # time: 0.010170748
    Base.precompile(Tuple{PlusEq{typeof(*)},Float64,Float64,Vararg{Float64, N} where N})   # time: 0.007921303
    Base.precompile(Tuple{PlusEq{typeof(+)},Float64,Float64,Vararg{Float64, N} where N})   # time: 0.00782437
    Base.precompile(Tuple{typeof(_isconst),Expr})   # time: 0.007764913
    Base.precompile(Tuple{typeof(swapvars!),SymbolTable,Expr,Expr})   # time: 0.007498742
    Base.precompile(Tuple{typeof(Base.show_function),IOContext{IOBuffer},PlusEq{typeof(angle)},Bool})   # time: 0.007020253
    Base.precompile(Tuple{typeof(precom_opm),Symbol,Expr,Int64})   # time: 0.006372358
    Base.precompile(Tuple{typeof(precom_opm),Symbol,Symbol,Int64})   # time: 0.006353995
    Base.precompile(Tuple{typeof(Base.show_function),IOContext{IOBuffer},PlusEq{typeof(abs)},Bool})   # time: 0.005321797
    Base.precompile(Tuple{typeof(unzipped_broadcast),PlusEq{typeof(*)},Vector{Float64},Vector{Float64},Vector{Float64}})   # time: 0.005210431
    Base.precompile(Tuple{typeof(isreversible),Function,Type{Tuple{Number, Any, Any}}})   # time: 0.003735743
    Base.precompile(Tuple{typeof(removedot),Symbol})   # time: 0.003666485
    Base.precompile(Tuple{PlusEq{typeof(*)},Float64,Int64,Vararg{Any, N} where N})   # time: 0.003475684
    Base.precompile(Tuple{MinusEq{typeof(*)},Float64,Int64,Vararg{Any, N} where N})   # time: 0.003429493
    Base.precompile(Tuple{PlusEq{typeof(*)},Float64,Float64,Vararg{Any, N} where N})   # time: 0.002768054
    Base.precompile(Tuple{MinusEq{typeof(*)},Float64,Float64,Vararg{Any, N} where N})   # time: 0.002728645
    Base.precompile(Tuple{typeof(assign_vars),SubArray{Any, 1, Vector{Any}, Tuple{UnitRange{Int64}}, true},Expr,Bool})   # time: 0.00237789
    Base.precompile(Tuple{typeof(unzipped_broadcast),MinusEq{typeof(*)},Vector{Float64},Vector{Float64},Vector{Float64}})   # time: 0.002377491
    Base.precompile(Tuple{typeof(almost_same),Tuple{Float64, Float64, Vector{Float64}, Vector{Float64}},Tuple{Float64, Float64, Vector{Float64}, Vector{Float64}}})   # time: 0.002303422
    Base.precompile(Tuple{typeof(unzipped_broadcast),MinusEq{typeof(identity)},Vector{Float64},Vector{Float64}})   # time: 0.00225225
    Base.precompile(Tuple{typeof(dual_swap),Expr,Expr})   # time: 0.002167955
    Base.precompile(Tuple{PlusEq{typeof(sin)},ComplexF64,ComplexF64})   # time: 0.002135965
    Base.precompile(Tuple{typeof(whilestatement),Expr,Expr,Vector{Any},CompileInfo})   # time: 0.001927632
    Base.precompile(Tuple{typeof(variable_analysis_ex),LineNumberNode,SymbolTable})   # time: 0.001729012
    Base.precompile(Tuple{typeof(compile_ex),Module,LineNumberNode,CompileInfo})   # time: 0.001608265
    Base.precompile(Tuple{typeof(assign_ex),Int64,Expr,Bool})   # time: 0.001299157
    Base.precompile(Tuple{typeof(_pop_value),Expr})   # time: 0.001298544
    Base.precompile(Tuple{typeof(assign_ex),Symbol,Symbol,Bool})   # time: 0.001287453
    Base.precompile(Tuple{typeof(assign_ex),Float64,Expr,Bool})   # time: 0.001275525
    Base.precompile(Tuple{MinusEq{typeof(/)},Float64,Float64,Vararg{Any, N} where N})   # time: 0.001080575
    Base.precompile(Tuple{typeof(_push_value),Expr,Expr,Bool})   # time: 0.00106306
    Base.precompile(Tuple{MinusEq{typeof(*)},Float64,Float64,Vararg{Float64, N} where N})   # time: 0.001049093
    Base.precompile(Tuple{PlusEq{typeof(/)},Float64,Float64,Vararg{Any, N} where N})   # time: 0.00102371
end


text:
is_valid(sides) = sort(sides)[1]+sort(sides)[2] > sort(sides)[3]
n_unique(sides) = length(Set(sides))
is_equilateral(sides) = n_unique(sides) == 1 && is_valid(sides)
is_isosceles(sides) = n_unique(sides) ≤ 2 && is_valid(sides)
is_scalene(sides) = n_unique(sides) == 3 && is_valid(sides)

ext: jl

max_stars_repo_name: Rigante-pl/Exercism-Julia

max_issues_repo_name: Rigante-pl/Exercism-Julia

max_forks_repo_name: Rigante-pl/Exercism-Julia


text:
"""
```
estimate(m::AbstractModel, df::DataFrame; verbose::Symbol=:low, proposal_covariance=Matrix())
```

Estimate the DSGE parameter posterior distribution.

### Arguments
- `m`: model object
- `df`: well-formed data as DataFrame

### Optional Arguments:
- `verbose`: The desired frequency of function progress messages printed to standard out.
   - `:none`: No status updates will be reported.
   - `:low`: Status updates will be provided in csminwel and at each block in
     Metropolis-Hastings.
   - `:high`: Status updates provided at each iteration in Metropolis-Hastings.
- `proposal_covariance`: Used to test the metropolis_hastings algorithm with a precomputed
  covariance matrix for the proposal distribution. When the Hessian is singular,
  eigenvectors corresponding to zero eigenvectors are not well defined, so eigenvalue
  decomposition can cause problems. Passing a precomputed matrix allows us to ensure that
  the rest of the routine has not broken.
"""
function estimate(m::AbstractModel, df::DataFrame;
                  verbose::Symbol=:low,
                  proposal_covariance::Matrix=Matrix())
    data = df_to_matrix(m, df)
    estimate(m, data; verbose=verbose, proposal_covariance=proposal_covariance)
end
function estimate(m::AbstractModel;
                  verbose::Symbol=:low,
                  proposal_covariance::Matrix=Matrix())
    # Load data
    df = load_data(m; verbose=verbose)
    estimate(m, df; verbose=verbose, proposal_covariance=proposal_covariance)
end
function estimate(m::AbstractModel, data::Matrix{Float64};
                  verbose::Symbol=:low,
                  proposal_covariance::Matrix=Matrix())

    ########################################################################################
    ### Step 1: Initialize
    ########################################################################################

    post = posterior(m, data)[:post]

    ########################################################################################
    ### Step 2: Find posterior mode (if reoptimizing, run csminwel)
    ########################################################################################

    # Specify starting mode

    vint = get_setting(m, :data_vintage)
    if reoptimize(m)
        println("Reoptimizing...")

        # Inputs to optimization algorithm
        n_iterations       = 100
        ftol               = 1e-10
        converged          = false

        # If the algorithm stops only because we have exceeded the maximum number of
        # iterations, continue improving guess of modal parameters
        total_iterations = 0
        optimization_time = 0
        while !converged
            tic()
            out, H = optimize!(m, data;
                ftol=ftol, iterations=n_iterations, show_trace=true, verbose=verbose)
            converged = !out.iteration_converged

            total_iterations += out.iterations
            if VERBOSITY[verbose] >= VERBOSITY[:low]
                @printf "Total iterations completed: %d\n" total_iterations
                @printf "Optimization time elapsed: %5.2f\n" optimization_time += toq()
            end

            # Write params to file after every `n_iterations` iterations
            params = map(θ->θ.value, m.parameters)
            h5open(rawpath(m, "estimate", "paramsmode.h5"),"w") do file
                file["params"] = params
            end
        end
    end

    params = map(θ->θ.value, m.parameters)

    ########################################################################################
    ### Step 3: Compute proposal distribution
    ###
    ### In Metropolis-Hastings, we draw sample parameter vectors from
    ### the proposal distribution, which is a degenerate multivariate
    ### normal centered at the mode. Its variance is the inverse of
    ### the hessian. We find the inverse via eigenvalue decomposition.
    ########################################################################################

    # Calculate the Hessian at the posterior mode
    hessian = if calculate_hessian(m)
        if VERBOSITY[verbose] >= VERBOSITY[:low]
            println("Recalculating Hessian...")
        end

        hessian, _ = hessian!(m, params, data; verbose=verbose)

        h5open(rawpath(m, "estimate","hessian.h5"),"w") do file
            file["hessian"] = hessian
        end

        hessian

    # Read in a pre-calculated Hessian
    else
        fn = hessian_path(m)
        if VERBOSITY[verbose] >= VERBOSITY[:low]
            println("Using pre-calculated Hessian from $fn")
        end

        hessian = h5open(fn,"r") do file
            read(file, "hessian")
        end

        hessian
    end

    # Compute inverse hessian and create proposal distribution, or
    # just create it with the given cov matrix if we have it
    propdist = if isempty(proposal_covariance)
        # Make sure the mode and hessian have the same number of parameters
        n = length(params)
        @assert (n, n) == size(hessian)

        # Compute the inverse of the Hessian via eigenvalue decomposition
        S_diag, U = eig(hessian)
        big_eig_vals = find(x -> x > 1e-6, S_diag)
        rank = length(big_eig_vals)

        S_inv = zeros(n, n)
        for i = (n-rank+1):n
            S_inv[i, i] = 1/S_diag[i]
        end

        hessian_inv = U*sqrt(S_inv) #this is the inverse of the hessian
        DSGE.DegenerateMvNormal(params, hessian_inv)
    else
        DSGE.DegenerateMvNormal(params, proposal_covariance)
    end

    if DSGE.rank(propdist) != n_parameters_free(m)
        println("problem –    shutting down dimensions")
    end

    ########################################################################################
    ### Step 4: Sample from posterior using Metropolis-Hastings algorithm
    ########################################################################################

    # Set the jump size for sampling
    cc0 = 0.01
    cc = 0.09

    metropolis_hastings(propdist, m, data, cc0, cc; verbose=verbose);

    ########################################################################################
    ### Step 5: Calculate and save parameter covariance matrix
    ########################################################################################

    compute_parameter_covariance(m);

    return nothing
end

"""
```
metropolis_hastings{T<:AbstractFloat}(propdist::Distribution, m::AbstractModel,
    data::Matrix{T}, cc0::T, cc::T; verbose::Symbol = :low)
```
Implements the Metropolis-Hastings MCMC algorithm for sampling from the posterior
distribution of the parameters.

### Arguments
- `propdist` The proposal distribution that Metropolis-Hastings begins sampling from.
- `m`: The model object
- `data`: Data matrix for observables
- `cc0`: Jump size for initializing Metropolis-Hastings.
- `cc`: Jump size for the rest of Metropolis-Hastings.

### Optional Arguments
- `verbose`: The desired frequency of function progress messages printed to standard out.

   - `:none`: No status updates will be reported.

   - `:low`: Status updates provided at each block.

   - `:high`: Status updates provided at each draw.
"""
function metropolis_hastings{T<:AbstractFloat}(propdist::Distribution,
                                               m::AbstractModel,
                                               data::Matrix{T},
                                               cc0::T,
                                               cc::T;
                                               verbose::Symbol=:low)

    # If testing, set the random seeds at fixed numbers
    if m.testing
        srand(m.rng, 654)
    end

    # Set number of draws, how many we will save, and how many we will burn
    # (initialized here for scoping; will re-initialize in the while loop)
    n_blocks = 0
    n_sim    = 0
    mhthin   = 0
    n_burn   = 0
    n_params = n_parameters(m)

    # Initialize algorithm by drawing para_old from a normal distribution centered on the
    # posterior mode until the parameters are within bounds or the posterior value is
    # sufficiently large.
    para_old = rand(propdist, m; cc=cc0)
    post_old = -Inf
    like_old = -Inf

    TTT_old = []
    RRR_old = []
    CCC_old = []

    zend_old = []
    ZZ_old = []
    DD_old = []
    QQ_old = []

    initialized = false
    while !initialized

        n_blocks = n_mh_blocks(m)
        n_sim    = n_mh_simulations(m)
        n_burn   = n_mh_burn(m)
        mhthin   = mh_thin(m)

        post_out = posterior!(m, para_old, data; mh=true)
        post_old, like_old, out = post_out[:post], post_out[:like], post_out[:mats]

        if post_old > -Inf
            propdist.μ = para_old

            TTT_old = out[:TTT]
            RRR_old = out[:RRR]
            CCC_old = out[:CCC]
            zend_old  = out[:zend]

            initialized = true
        end

    end

    # Report number of blocks that will be used

    if VERBOSITY[verbose] >= VERBOSITY[:low]
        println("Blocks: $n_blocks")
        println("Draws per block: $n_sim")
    end

    # For n_sim*mhthin iterations within each block, generate a new parameter draw.
    # Decide to accept or reject, and save every (mhthin)th draw that is accepted.

    all_rejections = 0

    # Initialize matrices for parameter draws and transition matrices
    mhparams     = zeros(n_sim, n_parameters(m))
    mhlikelihood = zeros(n_sim)
    mhposterior  = zeros(n_sim)
    mhTTT        = zeros(n_sim, n_states_augmented(m), n_states_augmented(m))
    mhRRR        = zeros(n_sim, n_states_augmented(m), n_shocks_exogenous(m))
    mhCCC        = zeros(n_sim, n_states_augmented(m))
    mhzend       = zeros(n_sim, n_states_augmented(m))

    # Open HDF5 file for saving output
    simfile = h5open(rawpath(m,"estimate","mhsave.h5"),"w")
    n_saved_obs = n_sim * (n_blocks - n_burn)
    parasim = d_create(simfile, "mhparams", datatype(Float32),
                       dataspace(n_saved_obs,n_params),
                       "chunk", (n_sim,n_params))
    postsim = d_create(simfile, "mhposterior", datatype(Float32),
                       dataspace(n_saved_obs,1),
                       "chunk", (n_sim,1))
    TTTsim  = d_create(simfile, "mhTTT", datatype(Float32),
                       dataspace(n_saved_obs,n_states_augmented(m),n_states_augmented(m)),
                       "chunk", (n_sim,n_states_augmented(m),n_states_augmented(m)))
    RRRsim  = d_create(simfile, "mhRRR", datatype(Float32),
                       dataspace(n_saved_obs,n_states_augmented(m),n_shocks_exogenous(m)),
                       "chunk", (n_sim,n_states_augmented(m),n_shocks_exogenous(m)))
    zsim    = d_create(simfile, "mhzend", datatype(Float32),
                       dataspace(n_saved_obs,n_states_augmented(m)),
                       "chunk", (n_sim,n_states_augmented(m)))

    # likesim = d_create(simfile, "mhlikelihood", datatype(Float32),
    #                  dataspace(n_saved_obs,1),
    #                  "chunk", (n_sim,1))
    # CCCsim  = d_create(simfile, "mhCCC", datatype(Float32),
    #                  dataspace(n_saved_obs,n_states_augmented(m)),
    #                  "chunk",(n_sim,n_states_augmented(m)))

    # keep track of how long metropolis_hastings has been sampling
    total_sampling_time = 0.

    for block = 1:n_blocks

        tic()
        block_rejections = 0

        for j = 1:(n_sim*mhthin)

            # Draw para_new from the proposal distribution
            para_new = rand(propdist, m; cc=cc)

            # Solves the model, check that parameters are within bounds, gensys returns a
            # meaningful system, and evaluate the posterior.
            post_out = posterior!(m, para_new, data; mh=true)
            post_new, like_new, out = post_out[:post], post_out[:like], post_out[:mats]

            if VERBOSITY[verbose] >= VERBOSITY[:high]
                println("Block $block, Iteration $j: posterior = $post_new")
            end

            # Choose to accept or reject the new parameter by calculating the
            # ratio (r) of the new posterior value relative to the old one
            # We compare min(1, r) to a number drawn randomly from a
            # uniform (0, 1) distribution. This allows us to always accept
            # the new draw if its posterior value is greater than the previous draw's,
            # but it gives some probability to accepting a draw with a smaller posterior value,
            # so that we may explore tails and other local modes.

            r = exp(post_new - post_old)

            x = rand(m.rng)

            if x < min(1.0, r)
                # Accept proposed jump
                para_old = para_new
                post_old = post_new
                like_old = like_new
                propdist.μ = para_new

                TTT_old = out[:TTT]
                RRR_old = out[:RRR]
                CCC_old = out[:CCC]

                zend_old = out[:zend]
                ZZ_old = out[:ZZ]
                DD_old = out[:DD]
                QQ_old = out[:QQ]

                if VERBOSITY[verbose] >= VERBOSITY[:high]
                    println("Block $block, Iteration $j: accept proposed jump")
                end

            else
                # Reject proposed jump
                block_rejections += 1

                if VERBOSITY[verbose] >= VERBOSITY[:high]
                    println("Block $block, Iteration $j: reject proposed jump")
                end

            end

            # Save every (mhthin)th draw

            if j % mhthin == 0
                draw_index = convert(Int,j/mhthin)

                mhlikelihood[draw_index] = like_old
                mhposterior[draw_index]  = post_old
                mhparams[draw_index, :]  = para_old'
                mhTTT[draw_index, :, :]  = TTT_old
                mhRRR[draw_index, :, :]  = RRR_old
                mhCCC[draw_index, :, :]  = CCC_old'
                mhzend[draw_index, :]    = zend_old'
            end
        end # of block

        all_rejections += block_rejections
        block_rejection_rate = block_rejections/(n_sim*mhthin)

        ## Once every iblock times, write parameters to a file

        # Calculate starting and ending indices for this block (corresponds to a new chunk in memory)
        block_start = n_sim*(block-n_burn-1)+1
        block_end   = block_start+n_sim-1

        # Write data to file if we're past n_burn blocks
        if block > n_burn
            parasim[block_start:block_end, :]   = map(Float32, mhparams)
            postsim[block_start:block_end, :]   = map(Float32, mhposterior)
            TTTsim[block_start:block_end,:,:]   = map(Float32, mhTTT)
            RRRsim[block_start:block_end,:,:]   = map(Float32, mhRRR)
            zsim[block_start:block_end,:]       = map(Float32, mhzend)
            # likesim[block_start:block_end, :] = map(Float32, mhlikelihood)
            # CCCsim[block_start:block_end, :]  = map(Float32, mhCCC)
        end

        # Calculate time to complete this block, average block
        # time, and expected time to completion
        if VERBOSITY[verbose] >= VERBOSITY[:low]
            block_time = toq()
            total_sampling_time += block_time
            total_sampling_time_minutes = total_sampling_time/60
            expected_time_remaining_sec     = (total_sampling_time/block)*(n_blocks - block)
            expected_time_remaining_minutes = expected_time_remaining_sec/60

            println("Completed $block of $n_blocks blocks.")
            println("Total time to compute $block blocks: $total_sampling_time_minutes minutes")
            println("Expected time remaining for Metropolis-Hastings: $expected_time_remaining_minutes minutes")
            println("Block $block rejection rate: $block_rejection_rate \n")
        end

    end # of loop over blocks

    close(simfile)

    rejection_rate = all_rejections/(n_blocks*n_sim*mhthin)
    if VERBOSITY[verbose] >= VERBOSITY[:low]
        println("Overall rejection rate: $rejection_rate")
    end
end

"""
```
compute_parameter_covariance(m::AbstractModel)
```

Calculates the parameter covariance matrix from saved parameter draws, and writes it to the
parameter_covariance.h5 file in the `workpath(m, "estimate")` directory.

### Arguments
* `m::AbstractModel`: the model object
"""
function compute_parameter_covariance(m::AbstractModel)

    # Read in saved parameter draws
    param_draws_path = rawpath(m,"estimate","mhsave.h5")
    if !isfile(param_draws_path)
        @printf STDERR "Saved parameter draws not found.\n"
        return
    end
    param_draws = h5open(param_draws_path, "r") do f
        read(f, "mhparams")
    end

    # Calculate covariance matrix
    param_covariance = cov(param_draws)

    # Write to file
    h5open(workpath(m, "estimate","parameter_covariance.h5"),"w") do f
        f["mhcov"] = param_covariance
    end

    return param_covariance
end


text:
### A Pluto.jl notebook ###
# v0.12.10

using Markdown
using InteractiveUtils

# ╔═╡ f100ffde-2863-11eb-3532-1f0d9dd21ff1
begin
	import Pkg
	Pkg.add("Images")
	using Images
end

# ╔═╡ f0adfde0-2863-11eb-0c9c-299a0c9c8d30
begin
	Pkg.add(["FileIO","ImageMagick","ImageIO"])
	using FileIO,ImageMagick,ImageIO
end

# ╔═╡ f078e420-2863-11eb-388d-c353185fe9a5

begin
	Pkg.add(["TestImages"])
	using TestImages
end

# ╔═╡ 96de8290-2ac9-11eb-26c9-7b604f9321b4
begin
	ENV["PYTHON"]=""
	Pkg.add("PyPlot")
	using PyPlot
end

# ╔═╡ dba26b1e-2863-11eb-029f-29cc35edd5b5
md"""https://juliaimages.org/stable/install/

go over below too

https://augmentorjl.readthedocs.io/en/latest/introduction/images.html
"""

# ╔═╡ f133a8a0-2863-11eb-069c-e18b17dad15c
md"""## Installation"""

# ╔═╡ f0e49e40-2863-11eb-3907-a5a06d4c345f
md"""
## Loading your first image
If this is your first time working with images in julia, it's likely that you'll need to install some image IO backends to load the images. The current available backends for image files are:

-ImageMagick.jl covers most image formats and has extra functionality. This can be your first choice if you don't have a preference.
-QuartzImageIO.jl exposes macOS's native image IO functionality to Julia. In some cases it's faster than ImageMagick, but it might not cover all your needs.
-ImageIO.jl is a new image IO backend (requires julia >=v"1.3") that provides an optimized performance for PNG files. Check benchmark here
-OMETIFF.jl supports OME-TIFF files. If you don't know what it is, then it is likely that you don't need this package."""

# ╔═╡ f0c70420-2863-11eb-048e-3be8b3e0ce36
md"""
## ________________________________________________________________________________
Lets add 3 packages, two from above Imagemagic and IMageIO  and FILEIO for loading and saving an image"""

# ╔═╡ f0940d40-2863-11eb-01f2-d56817b1701e
Gorilla1 = load("D:\\MyGit\\Colors\\Images\\gorilla.PNG")

# ╔═╡ 56efc290-286a-11eb-2e43-bb5b9b390701
md"""below package can be used to have sample images - use name such as - mandrill, blobs, mri-stack, lighthouse"""

# ╔═╡ a8529180-286a-11eb-20c4-7f5f64ed554a
md"""mosaicview - can be used to bring many images together in one frame """

# ╔═╡ f05dbb00-2863-11eb-0cfe-f777be6a1567
begin

	img1 = testimage("mandrill") # 512*512 RGB image
	img2 = testimage("blobs") # 254*256 Gray image
	mosaicview(img1, img2; nrow=1)
end

# ╔═╡ 0947e350-286b-11eb-325a-8d2e67ff66d9
md"""same thing but using column"""

# ╔═╡ f04291e0-2863-11eb-174f-3b05ddd98536
begin

	img3 = testimage("mandrill") # 512*512 RGB image
	img4 = testimage("lighthouse") # 254*256 Gray image
	mosaicview(img3, img4; ncolumn=1)
end

# ╔═╡ f029b2b2-2863-11eb-0e3d-49e44cfdbbe7
begin
	img5 = testimage("mri-stack") # 226×186×27 Gray image
	mosaicview(img5; fillvalue=0.5, npad=2, ncol=7, rowmajor=true)
end

# ╔═╡ f00f9b02-2863-11eb-1d74-afd1a6737dca
typeof(img5)

# ╔═╡ eff33960-2863-11eb-249a-77c16b73fdc3
md"""Elements of image are called pixels; in JuliaImages we provide an abstraction on this concept. For example, we have Gray for grayscale image, RGB for RGB image, Lab for Lab image, and etc.

-Creating a pixel is initializing a struct of that type:"""

# ╔═╡ efd96fd0-2863-11eb-251c-37939a648cd5
GB = Gray(0.0) # black

# ╔═╡ efbf3110-2863-11eb-1c5f-356164fbfef9
GW = Gray(1.0) # white

# ╔═╡ efa678f0-2863-11eb-1173-67f66e15c5c9
RR = RGB(1.0, 0.0, 0.0) # red

# ╔═╡ ef8c6140-2863-11eb-020c-8b55259803e7
RG = RGB(0.0, 1.0, 0.0) # green

# ╔═╡ ef7333f0-2863-11eb-16bc-25d6fc50c384
RB = RGB(0.0, 0.0, 1.0) # blue

# ╔═╡ ef482c50-2863-11eb-1526-99ce6c5a360f
md"""## Color conversions are construction/view"""

# ╔═╡ 8039a8d0-2871-11eb-37cd-1d48db835856
change the Gorilla image - img1 into grayscale

# ╔═╡ 60e49260-2871-11eb-2bd5-870f0080d3cf
GrayGori = Gray.(img1)

# ╔═╡ 60c9de70-2871-11eb-14ef-2144139c568b
YAY = RGB24.(img1)

# ╔═╡ 718a8ba0-2872-11eb-029d-115fb5593f4f
typeof(img1)

# ╔═╡ c6cf0060-2ac9-11eb-0603-dd171d023a6e
md"""## Matplotlib plotting from Python
This module provides a Julia interface to the Matplotlib plotting library from Python, and specifically to the matplotlib.pyplot module.

PyPlot uses the Julia PyCall package to call Matplotlib directly from Julia with little or no overhead (arrays are passed without making a copy)."""

# ╔═╡ 96c1f9e0-2ac9-11eb-2f3c-113d0494607c
Pkg.add("PyPlot")

# ╔═╡ 967b7b02-2ac9-11eb-0ba2-3796f545c9e1

# ╔═╡ 5a370382-2ac9-11eb-283a-619dc0e644be
function plotHist(hst, show=false)

    PyPlot.plot(hst)
    PyPlot.xlabel("Intensities")
    PyPlot.xlim([0, 256])
    PyPlot.yticks([])
    if (show)
        PyPlot.show()
    end
    PyPlot.savefig("img1")
end

# ╔═╡ c953f430-2871-11eb-2b2a-b38914ba0441
md"""the above was a success , maybe it works on PNG image with ease, let me retry this on my own PNG image"""

# ╔═╡ 60942870-2871-11eb-3afe-bb30d05c71ba
img_PNG = "D:\\MyGit\\Colors\\Images\\4by4png.PNG"

# ╔═╡ 60797482-2871-11eb-1788-2d086011ea08
imgPNG = load(img_PNG)

# ╔═╡ 605e7270-2871-11eb-2eb6-1f29fbb7673a
typeof(imgPNG)

# ╔═╡ da23bc90-2872-11eb-2ec7-7be9de168f09
channelview(imgPNG)

# ╔═╡ d8e44550-29ff-11eb-2694-4d399d9fed8e
size(imgPNG)

# ╔═╡ 605300c0-2871-11eb-2777-89cfcfd09ae6
YAY1 = RGB24.(imgPNG)

# ╔═╡ 4dffa8ce-2874-11eb-316d-27352df40430
 YAY3 = parse.(UInt32, hex.(imgPNG, :RRGGBBAA), base=16)

# ╔═╡ f4e07740-2ac4-11eb-1027-513fa1cbb82a
 YAY3_A = parse.(UInt32, hex.(imgPNG, :RRGGBB), base=16)

# ╔═╡ 719372e0-29ff-11eb-3bdb-1b055026907d
YAY4 = reshape(YAY3, (1,16))

# ╔═╡ 717f75b0-29ff-11eb-35ae-538dfc5fa50f
run(`echo hello!`)

# ╔═╡ 716536f0-29ff-11eb-1134-2907e44207b4

# ╔═╡ 4de73ed0-2874-11eb-19db-2b929f47e1ff

# ╔═╡ 08ea43e0-2874-11eb-0c58-4fd45c736a58
YAY2 = c24.(imgPNG)

# ╔═╡ 602b0660-2871-11eb-1ffe-f3d473c96054
typeof(YAY1)

# ╔═╡ 60131190-2871-11eb-33e0-3d031413bc87
channelview(YAY1)

# ╔═╡ 5ff77342-2871-11eb-3efd-4725fe2aa77b

# ╔═╡ 03ebbb30-2874-11eb-1aca-cb82b241fd90

# ╔═╡ 03d3030e-2874-11eb-1765-c505cb92efb1

# ╔═╡ 03b7b2e0-2874-11eb-0bc4-1bccae2c986c

# ╔═╡ 5fe17a40-2871-11eb-117b-950068345b4e
md"""## Understanding Colors Bases System

- Base 10 is our Numbers 0-9.
- Base 16 is [0 1 2 3 4 5 6 7 8 9 a b c d e f ] (A-F equals 10-15)

"""

# ╔═╡ 6644ed30-29fd-11eb-39d9-eb3d3f96751f
md"""So 255 in Hexa would be explained as in a 15 number system as - 15*16 = 240 and 15*1 = 15 and 240+15 = 255, and fifteen is F, so 255 is FF """

# ╔═╡ 662641a0-29fd-11eb-15bb-19fe09bccd88
md"""So white is 255,255,255 so in hex it would be #FFFFFF"""

# ╔═╡ 660d6270-29fd-11eb-1867-e3ef33c0b59c
md"""in hex if we have pairs like #999999 or #FFFFFF, then we can just use one per pair as #999 or #FFF"""

# ╔═╡ 512e7da0-2a00-11eb-26f6-b373f0612fa9
md"""## Understanding the last AA in  #rrggbbaa color notation
The value of alpha in the new hex notation would range from 00 to FF where 00 is the minimum value i.e 0% and FF is the max value which would be 100% opacity.

If you're aware of hexadecimal calculations, you'll easily be able to calculate the value of alpha based on the percentage of opacity that you need.

Calculating the hex code for 50% Alpha

Since the decimal values for opacity range from 0 to 255, in a traditional rgb notation; the value for 50% opacity would be 127, and hexadecimal value for this would be 7F

Here are some hex values that I get to see on a daily basis:

0% - 00

10% - 1A,

25% - 40,

50% - 7F,

75% - BF,

100% - FF"""

# ╔═╡ 65f23950-29fd-11eb-2dbc-ddef7cb4e594
md"""Explains changing bases, base 2 to 10, 16 etc

https://en.wikibooks.org/wiki/Introducing_Julia/Strings_and_characters"""

# ╔═╡ 5fc69f40-2871-11eb-03cb-e9aee3fcadee
red1 = RGB(1, 0, 0)

# ╔═╡ 5fae5c50-2871-11eb-0636-95066c1ad99b
typeof(red1)

# ╔═╡ 5f95f250-2871-11eb-2a23-0d38013f9ff2
channelview(red1), [2,3,1]

# ╔═╡ 5f7b1750-2871-11eb-34f5-091996aaffd1

# ╔═╡ Cell order:
# ╟─dba26b1e-2863-11eb-029f-29cc35edd5b5
# ╟─f133a8a0-2863-11eb-069c-e18b17dad15c
# ╠═f100ffde-2863-11eb-3532-1f0d9dd21ff1
# ╟─f0e49e40-2863-11eb-3907-a5a06d4c345f
# ╟─f0c70420-2863-11eb-048e-3be8b3e0ce36
# ╠═f0adfde0-2863-11eb-0c9c-299a0c9c8d30
# ╠═f0940d40-2863-11eb-01f2-d56817b1701e
# ╟─56efc290-286a-11eb-2e43-bb5b9b390701
# ╠═f078e420-2863-11eb-388d-c353185fe9a5
# ╠═a8529180-286a-11eb-20c4-7f5f64ed554a
# ╠═f05dbb00-2863-11eb-0cfe-f777be6a1567
# ╟─0947e350-286b-11eb-325a-8d2e67ff66d9
# ╠═f04291e0-2863-11eb-174f-3b05ddd98536
# ╠═f029b2b2-2863-11eb-0e3d-49e44cfdbbe7
# ╠═f00f9b02-2863-11eb-1d74-afd1a6737dca
# ╟─eff33960-2863-11eb-249a-77c16b73fdc3
# ╠═efd96fd0-2863-11eb-251c-37939a648cd5
# ╠═efbf3110-2863-11eb-1c5f-356164fbfef9
# ╠═efa678f0-2863-11eb-1173-67f66e15c5c9
# ╠═ef8c6140-2863-11eb-020c-8b55259803e7
# ╠═ef7333f0-2863-11eb-16bc-25d6fc50c384
# ╟─ef482c50-2863-11eb-1526-99ce6c5a360f
# ╠═8039a8d0-2871-11eb-37cd-1d48db835856
# ╠═60e49260-2871-11eb-2bd5-870f0080d3cf
# ╠═60c9de70-2871-11eb-14ef-2144139c568b
# ╠═718a8ba0-2872-11eb-029d-115fb5593f4f
# ╠═c6cf0060-2ac9-11eb-0603-dd171d023a6e
# ╠═96de8290-2ac9-11eb-26c9-7b604f9321b4
# ╠═96c1f9e0-2ac9-11eb-2f3c-113d0494607c
# ╠═967b7b02-2ac9-11eb-0ba2-3796f545c9e1
# ╠═5a370382-2ac9-11eb-283a-619dc0e644be
# ╟─c953f430-2871-11eb-2b2a-b38914ba0441
# ╠═60942870-2871-11eb-3afe-bb30d05c71ba
# ╠═60797482-2871-11eb-1788-2d086011ea08
# ╠═605e7270-2871-11eb-2eb6-1f29fbb7673a
# ╠═da23bc90-2872-11eb-2ec7-7be9de168f09
# ╠═d8e44550-29ff-11eb-2694-4d399d9fed8e
# ╠═605300c0-2871-11eb-2777-89cfcfd09ae6
# ╠═4dffa8ce-2874-11eb-316d-27352df40430
# ╠═f4e07740-2ac4-11eb-1027-513fa1cbb82a
# ╠═719372e0-29ff-11eb-3bdb-1b055026907d
# ╠═717f75b0-29ff-11eb-35ae-538dfc5fa50f
# ╠═716536f0-29ff-11eb-1134-2907e44207b4
# ╠═4de73ed0-2874-11eb-19db-2b929f47e1ff
# ╠═08ea43e0-2874-11eb-0c58-4fd45c736a58
# ╠═602b0660-2871-11eb-1ffe-f3d473c96054
# ╠═60131190-2871-11eb-33e0-3d031413bc87
# ╠═5ff77342-2871-11eb-3efd-4725fe2aa77b
# ╠═03ebbb30-2874-11eb-1aca-cb82b241fd90
# ╠═03d3030e-2874-11eb-1765-c505cb92efb1
# ╠═03b7b2e0-2874-11eb-0bc4-1bccae2c986c
# ╟─5fe17a40-2871-11eb-117b-950068345b4e
# ╟─6644ed30-29fd-11eb-39d9-eb3d3f96751f
# ╟─662641a0-29fd-11eb-15bb-19fe09bccd88
# ╟─660d6270-29fd-11eb-1867-e3ef33c0b59c
# ╟─512e7da0-2a00-11eb-26f6-b373f0612fa9
# ╟─65f23950-29fd-11eb-2dbc-ddef7cb4e594
# ╠═5fc69f40-2871-11eb-03cb-e9aee3fcadee
# ╠═5fae5c50-2871-11eb-0636-95066c1ad99b
# ╠═5f95f250-2871-11eb-2a23-0d38013f9ff2
# ╠═5f7b1750-2871-11eb-34f5-091996aaffd1


text:
# -*- coding: utf-8 -*-
include("gerda_data_processing/init.jl")
include("gerda_data_processing/fct.jl")
include("fct-sim.jl")

plots_base_path = "pulses/sim/raw_0vbb/plots/"
base_path_raw   = "pulses/sim/raw_0vbb/"
base_path       = "pulses/sim/0vbb/"
folders = glob(base_path*"*-*")
# cal = JSON.parsefile("data_pulses/FEP_pulses/calibration/cal.json");
fit_params = JSON.parsefile("pulses/fit_params.json")
Ecal = JSON.parsefile("pulses/cal_dict.json");

channels = []
# Find all detectors in data/raw/
for folder in folders
    ch = parse(Int64, split(split(folder, "/")[end], "-")[1])
    ch in channels ? "Channel is already in the list" : push!(channels, ch)
end

for ch in channels
    ch_str = lpad(ch, 2, "0");
#     IJulia.clear_output(true)
    Base.run(`clear`)
    @info("Start calculating A and E for Ch" * string(ch) * " | " * channel_to_name[ch])
    parameters = Table(A=[], E=[], energy=[])

    files = glob(base_path * ch_str * "-" * channel_to_name[ch] * "/*.h5");

    pro = Progress(length(files), dt=0.5,
            barglyphs=BarGlyphs('|','█', ['▁' ,'▂' ,'▃' ,'▄' ,'▅' ,'▆', '▇'],' ','|',),
            barlen=10)

    for file in files
        if stat(file).size > 1000
            data = HDF5.h5open(file, "r") do h5f
                LegendHDF5IO.readdata(h5f, "data")
            end
#             pulses = diff.(data.waveform.value);
#             A = maximum.(movingaverage.(pulses, 5, 3));
#             E = maximum.(movingaverage.(pulses, 201, 6));
            A = []
            E = []
            for wf in data.waveform
                push!(A, maximum(diff(multi_mwa(float.(wf.value),5,3))))
                push!(E, maximum(diff(multi_mwa(float.(wf.value),201,6))))
            end
            energy = []
            for e in sum.(data.energy)
                push!(energy, e.val)
            end
            append!(parameters, Table(A=A, E=E, energy=energy))
        end
        next!(pro)
    end

#     for e in parameters.energy
#         push!(energy, e.val)
#     end
    @info("Store file")
    filename = base_path * ch_str * "-" * channel_to_name[ch] * "-AE_Euncal.h5"
    HDF5.h5open(filename, "w") do h5f
        LegendHDF5IO.writedata(h5f, "data", Table(A    = Array{Float64,1}(parameters.A),
                                                E_uncal= Array{Float64,1}(parameters.E),
                                                energy = Array{Float64,1}(parameters.energy)))
    end
end


text:
@testset "Element" begin
    @testset "RemProj" begin
        r = Ray([1//1, -5//1, 0//1])
        l = Line([-15//1, 75//1, -26//1])
        r = Polyhedra.remproj(r, l)
        @test r isa Ray
        @test coord(r) == [1//1, -5//1, -15//1]
    end
    @testset "Simplify" begin
        r = @inferred simplify(Ray([0, 0]))
        @test r isa Ray{2, Int}
        @test coord(r) == [0, 0]
        p = @inferred simplify([6, 4])
        @test p isa Vector{Int}
        @test p == [6, 4]
        l = @inferred simplify(Line([6, 4]))
        @test l isa Line{2, Int}
        @test coord(l) == [3, 2]
        hp = @inferred simplify(HyperPlane([6, 4], 2))
        @test hp isa HyperPlane{2, Int}
        @test hp.a == [3, 2]
        @test hp.β == 1
        hs = @inferred simplify(HalfSpace([0, 0], 0))
        @test hs isa HalfSpace{2, Int}
        @test hs.a == [0, 0]
        @test hs.β == 0
        hpr = @inferred simplify(HyperPlane([0//1, 0//1], 0//1))
        @test hpr isa HyperPlane{2, Rational{Int}}
        @test hpr.a == [0, 0]
        @test hpr.β == 0
    end
    @testset "Lift" begin
        @test lift(HalfSpace([1, 2], 3)) == HalfSpace([-3, 1, 2], 0)
        @test lift(HalfSpace((@SVector [1, 2]), 3)) == HalfSpace((@SVector [-3, 1, 2]), 0)
        @test lift(HyperPlane([1, 2], 3)) == HyperPlane([-3, 1, 2], 0)
        @test lift(HyperPlane((@SVector [1, 2]), 3)) == HyperPlane((@SVector [-3, 1, 2]), 0)
        #@test lift(SymPoint([2, 3])) == SymPoint([1, 2, 3])
        #@test lift(SymPoint(@SVector [2, 3])) == SymPoint(@SVector [1, 2, 3])
        @test lift([2, 3]) == [1, 2, 3]
        @test lift(@SVector [2, 3]) == @SVector [1, 2, 3]
        @test lift(Line([2, 3])) == Line([0, 2, 3])
        @test lift(Line(@SVector [2, 3])) == Line(@SVector [0, 2, 3])
        @test lift(Ray([2, 3])) == Ray([0, 2, 3])
        @test lift(Ray(@SVector [2, 3])) == Ray(@SVector [0, 2, 3])
    end
    @testset "Promote" begin
        @test promote_type(HyperPlane{2, Int, Vector{Int}}, HyperPlane{2, Float64, Vector{Float64}}) == HyperPlane{2, Float64, Vector{Float64}}
        @test promote_type(HyperPlane{2, Float64, SVector{2, Float64}}, HyperPlane{2, Int, SVector{2, Int}}) == HyperPlane{2, Float64, SVector{2, Float64}}
        @test_throws ErrorException promote_type(HyperPlane{2, Int, Vector{Int}}, HyperPlane{2, Int, SVector{2, Int}})
        @test_throws ErrorException promote_type(HyperPlane{2, Int, Vector{Int}}, HyperPlane{2, Float64, SVector{2, Float64}})
    end
end


text:
using ConstrainedDynamics
using ConstrainedDynamicsVis
using ConstrainedControl
using LinearAlgebra
using Rotations

# Parameters
joint_axis = [1.0;0.0;0.0]

length1 = 1.0

pll = [0.0;0.0;length1/2] # joint connection point
pul = [0.0;0.0;length1/4]
pp = [0.0;0.0;length1/4*sqrt(2)]

# Links
origin = Origin{Float64}()
lowerlegl = Box(0.1, 0.1, length1, length1)
lowerlegr = deepcopy(lowerlegl)
upperlegl = Box(0.1, 0.1, length1/2, length1/2)
upperlegr = deepcopy(upperlegl)
platform = Box(0.1, 0.1, length1/2*sqrt(2), length1/2*sqrt(2))

# Constraints
floorlr = EqualityConstraint(Revolute(origin, lowerlegl, joint_axis; p2=-pll),Revolute(origin, lowerlegr, joint_axis; p2=-pll),FixedOrientation(origin,platform;qoffset = UnitQuaternion(RotX(pi/2))))
kneel = EqualityConstraint(Revolute(lowerlegl, upperlegl, joint_axis; p1=pll, p2=-pul))
kneer = EqualityConstraint(Revolute(lowerlegr, upperlegr, joint_axis; p1=pll, p2=-pul))
platl = EqualityConstraint(Revolute(platform, upperlegl, joint_axis; p2=pul, p1=pp))
platr = EqualityConstraint(Revolute(platform, upperlegr, joint_axis; p2=pul, p1=-pp))

links = [lowerlegl;lowerlegr;upperlegl;upperlegr;platform]
constraints = [platl;platr;floorlr;kneel;kneer]

mech = Mechanism(origin, links, constraints, g=-9.81, Δt = 0.01)
setPosition!(origin,lowerlegl,p2 = -pll,Δq = UnitQuaternion(RotX(pi/4)))
setPosition!(origin,lowerlegr,p2 = -pll,Δq = UnitQuaternion(RotX(-pi/4)))
setPosition!(lowerlegl,upperlegl,p1 = pll, p2 = -pul,Δq = UnitQuaternion(RotX(-pi/2)))
setPosition!(lowerlegr,upperlegr,p1 = pll, p2 = -pul,Δq = UnitQuaternion(RotX(pi/2)))
setPosition!(upperlegl,platform,p1 = pul, p2 = pp,Δq = UnitQuaternion(RotX(3pi/4)))

xd = [links[i].state.xc for i=1:5]
qd=[links[i].state.qc for i=1:5]

Q = [diagm(ones(12))*0.0 for i=1:5]
Q[5][2,2]=10.0
Q[5][3,3]=10.0
Q[5][5,5]=1.0
Q[5][6,6]=1.0
R = [ones(1,1)*0.1 for i=1:2]

lqr = LQR(mech, getid.(links), getid.(constraints[1:2]), Q, R, Inf, xd=xd, qd=qd, Fτd = [[[6.7879484]];[[-6.7879484]]])

### Calculation of initial conditions
s1 = 101
s2 = 101
successful = [false for i=1:s1*s2]
coords = [[0.0;0.0] for i=1:s1*s2]
counter = 1

# Find valid initial conditions
for i = 1:s1
    ppz = -1.5*length1 + 3*length1*(i-1)/(s1-1)
    ppz < 0 && continue
    for j = 1:s2
        global counter

        ppy = -1.5*length1 + 3*length1*(j-1)/(s2-1)
        coords[counter] = [ppy;ppz]

        pp0 = [0;ppy;ppz]
        # display(pp)
        ppr = pp0 + [0;pp[3];0]
        ppl = pp0 - [0;pp[3];0]

        if norm(ppr)<=1.5*length1 && norm(ppl)<=1.5*length1
            if norm(ppr)>=0.5*length1 && norm(ppl)>=0.5*length1
                successful[counter] = true
            end
        end
        counter+=1
    end
end

posres = findall(x->x==true, successful)
A = [getindex.(coords[posres],1) getindex.(coords[posres],2)]

# Find valid initial angles
nc = size(A)[1]
anglesl = [zeros(2) for i=1:nc]
anglesr = [zeros(2) for i=1:nc]

# a: lowerleg
# b: upperleg
# c: platform
for i = 1:nc
    pp0 = A[i,:]
    ppr = pp0 + [pp[3];0]
    ppl = pp0 - [pp[3];0]

    a = length1
    b = length1/2

    cl = norm(ppl)
    αl = acos((b^2+cl^2-a^2)/(2*b*cl))
    βl = acos((a^2+cl^2-b^2)/(2*a*cl))
    γl = acos((a^2+b^2-cl^2)/(2*a*b))
    δl = abs(atan(ppl[1]/ppl[2]))

    cr = norm(ppr)
    αr = acos((b^2+cr^2-a^2)/(2*b*cr))
    βr = acos((a^2+cr^2-b^2)/(2*a*cr))
    γr = acos((a^2+b^2-cr^2)/(2*a*b))
    δr = abs(atan(ppr[1]/ppr[2]))

    if ppl[1]<=0 && ppl[2]>=0
        anglesl[i] = [δl+βl;-pi+γl]
    elseif ppl[1]>=0 && ppl[2]>=0
        anglesl[i] = [-δl+βl;-pi+γl]
    elseif ppl[1]>=0 && ppl[2]<=0
        anglesl[i] = [-pi+δl-βl;pi-γl]
    elseif ppl[1]<=0 && ppl[2]<=0
        anglesl[i] = [-pi-δl-βl;pi-γl]
    end

    if ppr[1]<=0 && ppr[2]>=0
        anglesr[i] = [δr-βr;pi-γr]
    elseif ppr[1]>=0 && ppr[2]>=0
        anglesr[i] = [-δr-βr;pi-γr]
    elseif ppr[1]>=0 && ppr[2]<=0
        anglesr[i] = [-pi+δr+βr;-pi+γr]
    elseif ppr[1]<=0 && ppr[2]<=0
        anglesr[i] = [-pi-δr+βr;-pi+γr]
    end
end
### End

i = 97
setPosition!(origin,lowerlegl,p2 = -pll,Δq = UnitQuaternion(RotX(anglesl[i][1])))
setPosition!(origin,lowerlegr,p2 = -pll,Δq = UnitQuaternion(RotX(anglesr[i][1])))
setPosition!(lowerlegl,upperlegl,p1 = pll, p2 = -pul,Δq = UnitQuaternion(RotX(anglesl[i][2])))
setPosition!(lowerlegr,upperlegr,p1 = pll, p2 = -pul,Δq = UnitQuaternion(RotX(anglesr[i][2])))
setPosition!(origin,platform,p1 = [0;A[i,:]],Δq = UnitQuaternion(RotX(pi/2)))

storage = simulate!(mech,10,lqr,record = true)
visualize(mech,storage)


text:
using Test
using TensorOperations
using Random
using LinearAlgebra

Random.seed!(1234567)

TensorOperations.enable_blas()
TensorOperations.enable_cache()
include("methods.jl")
include("tensor.jl")
TensorOperations.disable_cache()
include("methods.jl")
include("tensor.jl")
TensorOperations.disable_blas()
include("methods.jl")
include("tensor.jl")

include("tensoropt.jl")
include("auxiliary.jl")


text:
using LabView0mqJl

try
   # include(get_script_path(raw"C:\_LabView_projects\ZMQ\LabView0mqJl.jl\src\Julia\this-and-that\test_user_broken-file.jl"))
   include(get_script_path(raw"C:\_LabView_projects\ZMQ\LabView0mqJl.jl\src\Julia\this-and-that\test_user_f.jl"))
   LabView0mqJl.setglobals(isOK=true)
   # println("here OK")
catch excep
   # @show excep
   # println("there not OK")
   LabView0mqJl.setglobals(isOK=false, excpn=(;excep, stack_trace=backtrace()))
   # global scriptOK = false
   # global scriptexcep = (;excep, stack_trace=backtrace())
   # println(scriptexcep.excep)
end

# @show LabView0mqJl.scriptOK, scriptexists #, scriptexcep
server_0mq4lv(fns)


text:
"""
Canonical polyadic N-mode tensor decomposition (CANDECOMP/PARAFAC).
"""
immutable CANDECOMP{T<:Number, N} <: TensorDecomposition{T, N}
  factors::NTuple{N, Matrix{T}} # rank X i-th tensor mode factor matrices
  lambdas::Vector{T}
  props::Dict{Symbol, Any}      # extra properties

  CANDECOMP{T, N}(factors::NTuple{N, Matrix{T}}, lambdas::Vector{T}) where {T<:Number, N} =
    new(factors, lambdas, Dict{Symbol, Any}())

  (::Type{CANDECOMP}){T, N}(factors::NTuple{N, Matrix{T}}, lambdas::Vector{T}) =
    CANDECOMP{T, N}(factors, lambdas)
end

"""
Returns CANDECOMP rank.
"""
Base.rank(decomp::CANDECOMP) = length(decomp.lambdas)

"""
Re-composes the tensor from CANDECOMP decomposition.
"""
@generated function compose!{T,N}(dest::Array{T,N}, factors::NTuple{N, Matrix{T}}, lambdas::Vector{T})
  quote
    @nloops $N i dest begin
        elm = zero(T)
        # FIXME traversal of factors is not very efficient, would be better to have them transposed
        for j in 1:length(lambdas)
            elm += lambdas[j] * (*(@ntuple($N, k -> factors[k][i_k, j])...))
        end
        @nref($N, dest, i) = elm
    end
    dest
  end
end

@generated function compose{T,N}(factors::NTuple{N, Matrix{T}}, lambdas::Vector{T})
  quote
    compose!(Array{T}(@ntuple $N i -> size(factors[i], 1)), factors, lambdas)
  end
end

compose(decomp::CANDECOMP) = compose(decomp.factors, decomp.lambdas)

compose!{T,N}(dest::Array{T,N}, decomp::CANDECOMP{T,N}) = compose!(dest, decomp.factors, decomp.lambdas)

"""
Calculates canonical polyadic tensor decomposition (CANDECOMP/PARAFAC).

Returns:
  `CANDECOMP` object
"""
function candecomp{T,N}(tnsr::StridedArray{T,N},
                   r::Integer,
                   initial_guess::NTuple{N, Matrix{T}};
                   method::Symbol=:ALS,
                   tol::Float64=1e-5,
                   maxiter::Integer=100,
                   compute_error::Bool=false,
                   verbose::Bool=true)

    # _check_tensor(tnsr, r) # decompositions with some of the sizes equal to the original tensor should be allowed
    verbose && info("initializing factor matrices...")
    all([(size(tnsr, i), r) == size(initial_guess[i]) for i in 1:N]) || throw(ArgumentError("dimension of initial guess does not match input tensor."))
    factors = collect(Matrix{T}, initial_guess)
    verbose && info("applying candecomp $method method...")
    res = _candecomp(Val{method}, tnsr, r, factors, tol, maxiter, verbose)
    if compute_error
      _set_rel_residue(res, tnsr)
    end
    return res
end

"Stub for non-implemented CANDECOMP algorithms."
_candecomp{T,N,S<:Symbol}(
  method::Val{S},
  tnsr::StridedArray{T,N},
  r::Integer,
  factors::Vector{Matrix{Float64}},
  tol::Float64,
  maxiter::Integer,
  verbose::Bool) = throw(ArgumentError("Unknown CANDECOMP method: $method"))

"""
Computes CANDECOMP by ALS (Alternating Least Squares) method.
"""
function _candecomp{T,N}(
    method::Type{Val{:ALS}},
    tnsr::StridedArray{T,N},
    r::Integer,
    factors::Vector{Matrix{Float64}},
    tol::Float64,
    maxiter::Integer,
    verbose::Bool)

    gram = [F'F for F in factors]
    tnsr_norm = vecnorm(tnsr)
    tnsr_flat = _col_unfold(tnsr, N)
    tnsr_size = size(tnsr)
    niters = 0
    converged = false
    resid = tnsr_norm
    lbds = Matrix{Float64}(1, r)
    V = Matrix{Float64}(div(length(tnsr), minimum(tnsr_size)), r)
    while !converged && niters < maxiter
        VB = 0
        @inbounds for i in 1:N
            idx = [N:-1:i + 1; i - 1:-1:1]
            VB = prod(k -> tnsr_size[k], idx)
            V[1:VB, :] = reduce(khatrirao, factors[idx])
            factors[i] = _row_unfold(tnsr, i) * V[1:VB, :] / reduce(((x, y) -> broadcast(*, x, y)), gram[idx])
            sum!(lbds, abs.(factors[i]))
            factors[i] ./= lbds
            At_mul_B!(gram[i], factors[i], factors[i])
        end
        resid_old = resid
        resid = vecnorm(V[1:VB, :] * broadcast(*, factors[N], lbds)' - tnsr_flat)
        converged = abs.(resid - resid_old) < tol * resid_old
        niters += 1
    end

    verbose && _iter_status(converged, niters, maxiter)

    return CANDECOMP((factors...), squeeze(lbds, 1))
end

"""
Computes CANDECOMP by SGSD (Simultaneous Generalized Schur Decomposition) method.
"""
function _candecomp{T,N}(
    method::Type{Val{:SGSD}},
    tnsr::StridedArray{T,N},
    r::Int,
    factors::Vector{Matrix{Float64}},
    tol::Float64,
    maxiter::Integer,
    verbose::Bool)

    ndims(tnsr) == 3 || throw(ArgumentError("This algorithm only applies to 3-mode tensors."))
    (n1, n2, n3) = size(tnsr)
    IB = [min(n1 - 1, r), (n2 == r) ? 2 : 1]
    Q = qr(factors[1], thin=false)[1]
    Z = qr(flipdim(factors[2], 2), thin=false)[1]
    q = Array{Float64}(n1, n1)
    z = Array{Float64}(n2, n2)

    R = zeros(size(tnsr))
    @tensor R[4,5,3] = tnsr[1,2,3] * Q[1,4] * Z[2,5]

    res = vecnorm(tnsr)
    converged = false
    niters = 0
    @inbounds while !converged && niters < maxiter
        q = eye(n1)
        z = eye(n2)

        for i in 1:IB[1]
            q[:, i:n1] *= svd(q[:, i:n1]' * view(R, :, n2 - r + i, :))[1]
        end

        @tensor R[4,2,3] = R[1,2,3] * q[1,4]

        for i in r:-1:IB[2]
            z[:, 1:n2 - r + i] *= flipdim(svd(view(R, i, :, :)' * z[:, 1:n2 - r + i])[3], 2)
        end
        Q *= q
        Z *= z

        @tensor R[4,5,3] = tnsr[1,2,3] * Q[1,4] * Z[2,5]

        res_old = res
        res = vecnorm(tril(squeeze(sum(R .^ 2, 3), 3), n2 - r - 1))
        converged = abs.(res - res_old) < tol
        niters += 1
    end

    verbose && _iter_status(converged, niters, maxiter)

    R = R[1:r, n2-r+1:n2, :]
    M = cat(3, eye(r, r), eye(r, r))
    @inbounds for i in (r - 1):-1:1, j = (i + 1):r
        if i + 1 < j
            d = (i + 1):(j - 1)
            println(d)

            M[i, j, :] = hcat(r[j, j, :][:], r[i, i, :][:]) \ (r[i, j, :][:] - mapslices(R3 -> sum(M[i, d, 1] * broadcast(*, diag(R3)[d], M[d, j, 2])), R, [1, 2])[:])
        end
    end

    factors[1] = Q[:, 1:r] * M[:, :, 1]
    factors[2] = Z[:, n2 - r + 1:n2] * M[:, :, 2]'
    factors[3] = _row_unfold(tnsr, 3) * khatrirao(factors[2], factors[1]) / broadcast(*, factors[2]'factors[2], factors[1]'factors[1])

    lbds = ones(1, r)
    for i in 1:3
        lbd = mapslices(vecnorm, factors[i], 1)
        factors[i] ./= lbd
        lbds = broadcast(*, lbds, lbd)
    end

    return CANDECOMP((factors...), squeeze(lbds, 1))
end

"""
Codes of implemented CANDECOMP methods.
"""
const CANDECOMP_methods = Symbol[t.parameters[1] for t in filter(t -> isa(t, DataType),
                                                                 [Base.unwrap_unionall(m.sig).parameters[2].parameters[1] for m in methods(_candecomp)])] |> Set{Symbol}


text:
include(joinpath(pwd(), "models/hopper_2wall.jl"))
include(joinpath(pwd(), "src/constraints/contact.jl"))
include(joinpath(pwd(), "src/constraints/loop.jl"))
include(joinpath(pwd(), "src/constraints/free_time.jl"))

# Free-time model
model_ft = free_time_model(model)

function fd(model::Hopper, x⁺, x, u, w, h, t)
	q3 = view(x⁺, model.nq .+ (1:model.nq))
	q2⁺ = view(x⁺, 1:model.nq)
	q2⁻ = view(x, model.nq .+ (1:model.nq))
	q1 = view(x, 1:model.nq)
	u_ctrl = view(u, model.idx_u)
	λ = view(u, model.idx_λ)
	b = view(u, model.idx_b)
	h = u[end]

	[q2⁺ - q2⁻;
	((1.0 / h) * (M_func(model, q1) * (SVector{4}(q2⁺) - SVector{4}(q1))
	- M_func(model, q2⁺) * (SVector{4}(q3) - SVector{4}(q2⁺)))
	+ transpose(B_func(model, q3)) * SVector{2}(u_ctrl)
	+ transpose(N_func(model, q3)) * SVector{1}(λ)
	+ transpose(P_func(model, q3)) * SVector{2}(b)
	- h * G_func(model, q2⁺))]
end

function maximum_dissipation(model::Hopper, x⁺, u, h)
	q3 = x⁺[model.nq .+ (1:model.nq)]
	q2 = x⁺[1:model.nq]
	ψ = u[model.idx_ψ]
	ψ_stack = ψ[1] * ones(model.nb)
	η = u[model.idx_η]
	h = u[end]
	return P_func(model, q3) * (q3 - q2) / h + ψ_stack - η
end

# Horizon
T = 21

# Time step
tf = 1.0
h = tf / (T - 1)

# Bounds
_uu = Inf * ones(model_ft.m)
_uu[end] = 5.0 * h
_ul = zeros(model_ft.m)
_ul[model_ft.idx_u] .= -Inf
_ul[end] = 0.5 * h
ul, uu = control_bounds(model_ft, T, _ul, _uu)

# Initial and final states
q1 = [0.0, 0.25 , 0.0, 0.25]
q_right = [-0.25, 0.5 + 0.25, pi / 2.0, 0.25]
q_top = [-0.5, 0.5 + 0.5, pi, 0.25]
q_left = [-0.75, 0.5 + 0.25, 3.0 * pi / 2.0, 0.25]
qT = [-1.0, 0.5,  2.0 * pi, 0.5]

xl, xu = state_bounds(model_ft, T,
		[model_ft.qL; model_ft.qL],
		[model_ft.qU; model_ft.qU],
        x1 = [q1; Inf * ones(model_ft.nq)],
		xT = [Inf * ones(model_ft.nq); qT])

q_ref = [linear_interpolation(q1, q_right, 6)...,
         linear_interpolation(q_right, q_top, 6)[2:end]...,
         linear_interpolation(q_top, q_left, 6)[2:end]...,
         linear_interpolation(q_left, qT, 6)[2:end]...]

x_ref = configuration_to_state(q_ref)

# Objective
Qq = 1.0 * Diagonal([1.0, 1.0, 1.0, 1.0])
Q = cat(0.5 * Qq, 0.5 * Qq, dims = (1, 2))
# QT = cat(0.5 * Qq, 100.0 * Diagonal(ones(model_ft.nq)), dims = (1, 2))
R = Diagonal([1.0e-1, 1.0e-3, zeros(model_ft.m - model_ft.nu)...])

obj_tracking = quadratic_time_tracking_objective(
    [Q for t = 1:T],
    [R for t = 1:T-1],
    [x_ref[t] for t = 1:T],
    [zeros(model_ft.m) for t = 1:T],
    1.0)

obj_contact_penalty = PenaltyObjective(1.0e5, model_ft.m - 1)

obj = MultiObjective([obj_tracking, obj_contact_penalty])

# Constraints
con_free_time = free_time_constraints(T)
con_contact = contact_constraints(model_ft, T)
con_loop = loop_constraints(model_ft, 1, T)
con = multiple_constraints([con_free_time, con_contact])

# Problem
prob = trajectory_optimization_problem(model_ft,
               obj,
               T,
               xl = xl,
               xu = xu,
               ul = ul,
               uu = uu,
               con = con
               )

# Trajectory initialization
x0 = deepcopy(x_ref) # linear interpolation on state
u0 = [[1.0e-5 * rand(model_ft.m-1); h] for t = 1:T-1] # random controls

# Pack trajectories into vector
z0 = pack(x0, u0, prob)

#NOTE: may need to run examples multiple times to get good trajectories
# Solve nominal problem
include_snopt()

@time z̄ , info = solve(prob, copy(z0),
	nlp = :SNOPT7,
	tol = 1.0e-3, c_tol = 1.0e-3, mapl = 5)

x̄, ū = unpack(z̄, prob)

@show ū[4][end]
@show check_slack(z̄, prob)

using Plots
plot(hcat(ū...)[end,:], linetype=:steppost)

include(joinpath(pwd(), "models/visualize.jl"))
vis = Visualizer()
render(vis)
visualize!(vis, model_ft, state_to_configuration(x̄), Δt = ū[1][end])


text:
# RLWE specific tests
# Copyright (C): Jaka Smrekar (vinctux) <vinctux@outlook.com>, 2016

using Krypto
using Base.Test

println("RLWE TESTS STARTED...")

# Test values - 1024 octets (random, 0x00, 0xFF, flip)
T = [[csrand(0o1:0o255) for i in 1:256],
     [0o0 for i in 1:256],
     [0o377 for i in 1:256],
     [i % 2 == 0 ? 0o0 : 0o1 for i in 1:256]]

Q = LatticeModuli(3, 12, 1)   # 12289 is default; 40961 not supported (LUTs needed)
N = 1024    # default, also [256, 512, 1024]
B = 12      # default, also [8, 16]
println("RLWE config: [$(Int16(Q))-$(N)-$(B)]")
print("Generating polynomial 'A' ...")
@time A = genrandpoly(N, Q)
print("Generating RLWE keypair ($(N)-bit) ...")
@time PUB, PRIV = RLWEKeyGen(N)

for i in 1:length(T)
    print("Running encryption test #$(i) ...")
    @time E = RLWEEncrypt(PUB, T[i])
    println("Encrypted: $(E[1])\n$(E[2])\n\n")
    @time D = RLWEDecrypt(PRIV, E)
    println("Decrypted: $(D)\n\n\n===================================\n\n\n")
    @test D == typeof(D)(T[i])
end

println("ALL RLWE TESTS PASSED.")


text:
contains_inf(array) = mapreduce(x->(isinf(x)), &, array)

# A data structure representing the problem data and constraints
type MDSProblem
    num_points::Int
    dist::Array{Float64, 2}
    sphere_radius::Float64
    # constraint and control loci are locations of fish probes,
    # constraint_ratios are ratios of locus distances to the "base" distance
    control_locus_1::Int
    control_locus_2::Int
    c_loci_1::Array{Int, 1}
    c_loci_2::Array{Int, 1}
    constraint_ratios::Array{Float64,1}
    # original distance between control_locus_1 and control_locus_2
    c1_c2_distance::Float64

    # Constructors
    # New problem with just a distance matrix
    MDSProblem(dist::Array{Float64, 2}) =
        new(size(dist)[1], dist, 1.0, 0, 0,
            Array(Int,0), Array(Int,0), Array(Float64,0), 0.0)
    # New problem with just a radius constraint
    MDSProblem(dist::Array{Float64, 2}, r::Float64) =
        new(size(dist)[1], dist, r, 0, 0,
            Array(Int,0), Array(Int,0), Array(Float64,0), 0.0)
    # New problem with all constraints
    MDSProblem(dist::Array{Float64, 2}, r::Float64,
               cl1::Int,
               cl2::Int,
               c_loci_1::Array{Int,1},
               c_loci_2::Array{Int,1},
               c_ratios::Array{Float64,1},
               d1::Float64) =
        new(size(dist)[1], dist, r, cl1, cl2, c_loci_1, c_loci_2, c_ratios, d1)
end

function new_mds(dist::Array{Float64,2}, r::Float64, constraints_file::AbstractString;
            resolution::Int=200000, infer_scale::Bool=false)
    # returns a MDSProblem with the constraints in constraints_file
    c = load_constraints(constraints_file, resolution=resolution)
    return MDSProblem(dist, r, c[1], c[2], c[3], c[4], c[5], c[6])
end

function infer_scale(mds::MDSProblem)
    # Infers the scaling factor for a constrained problem,
    # assuming that the original scaling factor was 1 for the distances.
    d1 = mds.c1_c2_distance
    return d1/mds.dist[mds.control_locus_1, mds.control_locus_2]
end

function load_data(filename::AbstractString; exponent=1/3, scale=1)
    # loads data (in the format of a contact map),
    # convert to NxN array of "wish distances"
    contact_data = readcsv(filename, Float64)
    dist = scale./(contact_data).^exponent
    return dist
end

function load_constraints(filename::AbstractString; resolution::Int=200000)
    # returns a tuple containing all the constraint information -
    # control_locus_1, control_locus_2, c_loci_1, c_loci_2,
    # constraint_ratios
    #
    # file format:
    # num_constraints
    # locus1    locus2  distance
    # locus1    locus3  distance
    # etc.
    # locus1, locus2, etc. are all bin numbers
    f = open(filename, "r")
    lines = readlines(f)
    n = parse(Int, strip(lines[1])) - 1
    l1 = split(strip(lines[2]))
    c1 = parse(Int, l1[1])
    c2 = parse(Int, l1[2])
    d1 = float(l1[3])
    cl1 = Array(Int, n)
    cl2 = Array(Int, n)
    cr = Array(Float64, n)
    for i in 1:n
        line = lines[2+i]
        l = split(strip(line))
        cl1[i] = int(l[1])
        cl2[i] = int(l[2])
        cr[i] = float(l[3])/d1
    end
    return (c1, c2, cl1, cl2, cr, d1)
end

function remove_zeros(dist::Array{Float64, 2})
    # Removes all-zero and all-inf rows/columns from a contact map
    dist_index = 1
    while dist_index <= size(dist)[1]
        if mapreduce(x->(isinf(x) || x==0), &, dist[dist_index, 1:end-1])
            dist = [dist[1:dist_index-1, :], dist[dist_index+1:end, :]]
            dist = [dist[:, 1:dist_index-1] dist[:,dist_index+1:end]]
        else
            dist_index += 1
        end
    end
    return dist
end

function get_removed_indices(prob::MDSProblem)
    # Returns a list containing all the indices that are all infs
    dist_index = 1
    dist = prob.dist
    all_removed_indices = Array(Int, 0)
    while dist_index <= size(dist, 1)
        if mapreduce(x->(isinf(x) || x==0), &, dist[dist_index, 1:end-1])
            append!(all_removed_indices, [dist_index])
        end
        dist_index += 1
    end
    return all_removed_indices
end

function print_constraints(prob::MDSProblem)
    # Prints the problem constraints
    println("Constraints")
    println("Locus 1: ", prob.control_locus_1)
    println("Locus 2: ", prob.control_locus_2)
    println("Other loci: ", [(x1, x2) for (x1, x2) in
            zip(prob.c_loci_1, prob.c_loci_2)])
    println("Length ratios: ", prob.constraint_ratios)
end

function remove_infs(prob::MDSProblem)
    # Removes all rows from the distance matrix that have all infinities.
    # Also shifts the constraints down.
    dist_index = 1
    dist = prob.dist
    while dist_index <= size(dist)[1]
        if mapreduce(x->(isinf(x) || x==0), &, dist[dist_index, 1:end-1])
            dist = [dist[1:dist_index-1, :]; dist[dist_index+1:end, :]]
            dist = [dist[:, 1:dist_index-1] dist[:,dist_index+1:end]]
            # update fish constraints
            if prob.control_locus_1 != 0
                if prob.control_locus_1 >= dist_index
                    prob.control_locus_1 -=1
                end
                if prob.control_locus_2 >= dist_index
                    prob.control_locus_2 -=1
                end
                for i in range(1, length(prob.c_loci_1))
                    if prob.c_loci_1[i] >= dist_index
                        prob.c_loci_1[i] -= 1
                    end
                    if prob.c_loci_2[i] >= dist_index
                        prob.c_loci_2[i] -= 1
                    end
                end
            end
        else
            dist_index += 1
        end
    end
    prob.dist = dist
    prob.num_points = size(dist)[1]
    return prob
end

function make_ipopt_problem(mds::MDSProblem; radius_constraint=false,
        fish_constraint=false,
        initial_coords=false)
    # Creates an Ipopt problem for the given problem instance

    # 3 vars for each point- x, y, z
    num_vars = 3*mds.num_points
    num_constraints = 0
    n, n2 = size(mds.dist)
    dist = mds.dist
    rad = mds.sphere_radius
    #nuc_rad = mds.nucleolus_radius
    # TODO: add options for initial coordinates
    initial_x = [rand()*2*rad - rad for i in 1:num_vars]
    # m is the number of constraints
    m = 0
    if radius_constraint
        m = mds.num_points
    end
    #println(m)
    # constraint lower bound
    g_L = zeros(m)
    # constraint upper bound
    g_U = fill(rad^2, m)
    num_jac_elements=m*3
    # the index of g at which fish constraints start
    fish_constraint_index = 0
    # dict of (locus_1, locus_2) => num_constraint
    fish_constraint_dict = Dict{Tuple{Int32, Int32}, Int32}()

    # for FISH constraints, do something
    if fish_constraint
        fish_constraint_index = m+1
        m = m + length(mds.constraint_ratios)
        new_g_L = zeros(length(mds.constraint_ratios))
        new_g_U = zeros(length(mds.constraint_ratios))
        for (i, (i1, i2, r)) in enumerate(zip(mds.c_loci_1, mds.c_loci_2, mds.constraint_ratios))
            # reasoning: if i1, i2 differ from c1, c2, then there are 4 points
            # or 12 variables.
            if haskey(fish_constraint_dict, (i1, i2))
                continue
            end
            n_jac = 12
            if i1 == mds.control_locus_1
                n_jac -= 3
            end
            if i2 == mds.control_locus_1
                n_jac -= 3
            end
            if i1 == mds.control_locus_2
                n_jac -= 3
            end
            if i2 == mds.control_locus_2
                n_jac -= 3
            end
            fish_constraint_dict[i1,i2] = n_jac
            num_jac_elements += n_jac
            # WARNING: arbitrary constant alert
            new_g_L[i] = -0.00001
            new_g_U[i] = 0.00001
        end
        append!(g_L, new_g_L)
        append!(g_U, new_g_U)
    end

    interactions = Array(Tuple{Int, Int}, 0)
    for i in 1:n-1
        for j in i+1:n
            if dist[i,j] != Inf
                push!(interactions, (i, j))
            end
        end
    end
    println("Number of interactions: ", length(interactions))
    println("Number of variables: ", num_vars)
    println("Number of constraints: ", m)
    println("Number of nonzero Jacobian elements: ", num_jac_elements)

    ###### Callback Functions ###########################################

    function eval_f(x::Vector{Float64})
        val = 0
        for (i, j) in interactions
            i_c = 3*(i-1) + 1
            j_c = 3*(j-1) + 1
            s = (x[i_c] - x[j_c])^2 +
                (x[i_c+1] - x[j_c+1])^2 +
                (x[i_c+2] - x[j_c+2])^2
            # w is some scaling factor
            w = 1/(dist[i,j])^2
            val += w*(sqrt(s) - dist[i,j])^2
        end
        #println("x max: ", maximum(x))
        return val
    end

    function eval_g(x::Vector{Float64}, g::Vector{Float64})
        # Evaluating constraint functions
        g[:] = zeros(size(g))
        if radius_constraint
            for i in 1:mds.num_points
                i_c = 3*(i-1) + 1
                g[i] = x[i_c]^2 + x[i_c+1]^2 + x[i_c+2]^2
            end
        end
        if fish_constraint
            c1 = 3*(mds.control_locus_1-1)+1
            c2 = 3*(mds.control_locus_2-1)+1
            control_distance = (x[c1]-x[c2])^2 +
                       (x[c1+1]-x[c2+1])^2 +
                       (x[c1+2]-x[c2+2])^2
            #println("control_distance: ", control_distance)
            for i in fish_constraint_index:m
                c_i = i-fish_constraint_index + 1
                i1 = mds.c_loci_1[c_i]
                i2 = mds.c_loci_2[c_i]
                i1_c = 3*(i1-1)+1
                i2_c = 3*(i2-1)+1
                g[i] = (x[i1_c]-x[i2_c])^2 +
                       (x[i1_c+1]-x[i2_c+1])^2 +
                       (x[i1_c+2]-x[i2_c+2])^2
                g[i] = g[i] - mds.constraint_ratios[c_i]^2*control_distance
                #println(g[i])
            end
            #println(g[fish_constraint_index:m])
        end
    end

    function eval_grad_f(x::Vector{Float64}, grad_f::Vector{Float64})
        # evaluates gradient of the objective function
        grad_f[:] = zeros(size(grad_f))
        for (i, j) in interactions
            i_c = 3*(i-1) + 1
            j_c = 3*(j-1) + 1
            s = (x[i_c] - x[j_c])^2 + (x[i_c+1] - x[j_c+1])^2 + (x[i_c+2] - x[j_c+2])^2
            s2 = sqrt(s)
            f = s2-dist[i,j]
            common_term = 1-dist[i,j]/s2
            w = 1/dist[i,j]^2
            common_term *= w
            grad_f[i_c] += common_term*2*(x[i_c] - x[j_c])
            grad_f[i_c+1] += common_term*2*(x[i_c+1] - x[j_c+1])
            grad_f[i_c+2] += common_term*2*(x[i_c+2] - x[j_c+2])
            grad_f[j_c] -= common_term*2*(x[i_c] - x[j_c])
            grad_f[j_c+1] -= common_term*2*(x[i_c+1] - x[j_c+1])
            grad_f[j_c+2] -= common_term*2*(x[i_c+2] - x[j_c+2])
        end
        #println(grad_f)
        #println("grad max: ", maximum(grad_f))
        #println("grad min: ", minimum(grad_f))
        #mi = indmax(grad_f)
        #println("index of grad max: ", mi)
        #println("x max: ", maximum(x))
        #println("x min: ", minimum(x))
    end

    function eval_jac_g(
        x::Vector{Float64},         # Current solution
        mode,                       # Either :Structure or :Values
        rows::Vector{Int32},        # Sparsity structure - row indices
        cols::Vector{Int32},        # Sparsity structure - column indices
        values::Vector{Float64})
        #println("eval jac g")
        if mode == :Structure
            #println("eval jac g structure")
            if fish_constraint
                i_new = fish_constraint_index
                i_c = 3*(i_new-1)+1
                for (i, (i1, i2, r)) in enumerate(zip(mds.c_loci_1, mds.c_loci_2, mds.constraint_ratios))
                    i_c_start = i_c
                    #println("i_c start: ", i_c)
                    n_jac = fish_constraint_dict[i1, i2]
                    cols[i_c] = 3*(i1-1) + 1
                    cols[i_c+1] = 3*(i1-1) + 2
                    cols[i_c+2] = 3*(i1-1) + 3
                    i_c += 3
                    cols[i_c] = 3*(i2-1) + 1
                    cols[i_c+1] = 3*(i2-1) + 2
                    cols[i_c+2] = 3*(i2-1) + 3
                    i_c += 3
                    if i1 != mds.control_locus_1 && i2 != mds.control_locus_1
                        m1 = mds.control_locus_1
                        cols[i_c] = 3*(m1-1) + 1
                        cols[i_c+1] = 3*(m1-1) + 2
                        cols[i_c+2] = 3*(m1-1) + 3
                        i_c += 3
                    end
                    if i1 != mds.control_locus_2 && i2 != mds.control_locus_2
                        m2 = mds.control_locus_2
                        cols[i_c] = 3*(m2-1) + 1
                        cols[i_c+1] = 3*(m2-1) + 2
                        cols[i_c+2] = 3*(m2-1) + 3
                        i_c += 3
                    end
                    #println("i_c: ", i_c)
                    for j in 0:n_jac-1
                        rows[i_c_start+j] = fish_constraint_index+i-1
                    end
                end
            end
            if radius_constraint
                for i in 1:mds.num_points
                    i_c = 3*(i-1) + 1
                    rows[i_c] = i
                    rows[i_c+1] = i
                    rows[i_c+2] = i
                    cols[i_c] = i_c
                    cols[i_c+1] = i_c+1
                    cols[i_c+2] = i_c+2
                end
            end
        else
            if fish_constraint
                i_new = fish_constraint_index
                i_c = 3*(i_new-1)+1
                m1 = mds.control_locus_1
                m1_c = 3*(m1-1)+1
                m2 = mds.control_locus_2
                m2_c = 3*(m2-1)+1
                for (i, (i1, i2, r)) in enumerate(zip(mds.c_loci_1, mds.c_loci_2, mds.constraint_ratios))
                    # 1. gradients for i1
                    i1_c = 3*(i1-1) + 1
                    i2_c = 3*(i2-1) + 1
                    values[i_c] = 2*(x[i1_c]-x[i2_c])
                    values[i_c+1] = 2*(x[i1_c+1]-x[i2_c+1])
                    values[i_c+2] = 2*(x[i1_c+2]-x[i2_c+2])
                    if i1 == mds.control_locus_1
                        values[i_c] -= r^2*2*(x[i1_c]-x[m2_c])
                        values[i_c+1] -= r^2*2*(x[i1_c+1]-x[m2_c+1])
                        values[i_c+2] -= r^2*2*(x[i1_c+2]-x[m2_c+2])
                    elseif i1 == mds.control_locus_2
                        values[i_c] += r^2*2*(x[m1_c] - x[i1_c])
                        values[i_c+1] += r^2*2*(x[m1_c+1] - x[i1_c+1])
                        values[i_c+2] += r^2*2*(x[m1_c+2] - x[i1_c+2])
                    end
                    # 2. gradients for i2
                    i_c += 3
                    values[i_c] = -2*(x[i1_c]-x[i2_c])
                    values[i_c+1] = -2*(x[i1_c+1]-x[i2_c+1])
                    values[i_c+2] = -2*(x[i1_c+2]-x[i2_c+2])
                    if i2 == mds.control_locus_1
                        values[i_c] -= r^2*2*(x[i2_c]-x[m2_c])
                        values[i_c+1] -= r^2*2*(x[i2_c+1]-x[m2_c+1])
                        values[i_c+2] -= r^2*2*(x[i2_c+2]-x[m2_c+2])
                    elseif i2 == mds.control_locus_2
                        values[i_c] += r^2*2*(x[m1_c] - x[i2_c])
                        values[i_c+1] += r^2*2*(x[m1_c+1] - x[i2_c+1])
                        values[i_c+2] += r^2*2*(x[m1_c+2] - x[i2_c+2])
                    end
                    i_c += 3
                    if i1 != mds.control_locus_1 && i2 != mds.control_locus_1
                        # do mds.control_locus_1
                        values[i_c] = -1*r^2*2*(x[m1_c]-x[m2_c])
                        values[i_c+1] = -1*r^2*2*(x[m1_c+1]-x[m2_c+1])
                        values[i_c+2] = -1*r^2*2*(x[m1_c+2]-x[m2_c+2])
                        i_c += 3
                    end
                    if i1 != mds.control_locus_2 && i2 != mds.control_locus_2
                        # do mds.control_locus_2
                        values[i_c] = r^2*2*(x[m1_c] - x[m2_c])
                        values[i_c+1] = r^2*2*(x[m1_c+1] - x[m2_c+1])
                        values[i_c+2] = r^2*2*(x[m1_c+2] - x[m2_c+2])
                        i_c += 3
                    end
                end
            end
            if radius_constraint
                for i in 1:mds.num_points
                    i_c = 3*(i-1) + 1
                    values[i_c] = 2*x[i_c]
                    values[i_c+1] = 2*x[i_c+1]
                    values[i_c+2] = 2*x[i_c+2]
                end
            end
        end
        #println("finished eval jac g")
    end

    function eval_h(
        x::Vector{Float64},         # Current solution
        mode,                       # Either :Structure or :Values
        rows::Vector{Int32},        # Sparsity structure - row indices
        cols::Vector{Int32},        # Sparsity structure - column indices
        obj_factor::Float64,        # Lagrangian multiplier for objective
        lambda::Vector{Float64},    # Multipliers for each constraint
        values::Vector{Float64})    # The values of the Hessian

        if mode == :Structure
            # rows[...] = ...
            # ...
            # cols[...] = ...
        else
            # values[...] = ...
        end
    end

    function intermediate(
        alg_mod::Int,
        iter_count::Int,
        obj_value::Float64,
        inf_pr::Float64, inf_du::Float64,
        mu::Float64, d_norm::Float64,
        regularization_size::Float64,
        alpha_du::Float64, alpha_pr::Float64,
        ls_trials::Int)
        # ...
        return true  # Keep going
    end

    problem = createProblem(
        num_vars::Int,                     # Number of variables
        fill(-Inf, num_vars)::Vector{Float64},       # Variable lower bounds
        fill(Inf, num_vars)::Vector{Float64},       # Variable upper bounds
        m::Int,                     # Number of constraints
        g_L,       # Constraint lower bounds
        g_U,       # Constraint upper bounds
        num_jac_elements,              # Number of non-zeros in Jacobian
        0,             # Number of non-zeros in Hessian
        eval_f,                     # Callback: objective function
        eval_g,                     # Callback: constraint evaluation
        eval_grad_f,                # Callback: objective function gradient
        eval_jac_g,                 # Callback: Jacobian evaluation
        nothing)           # Callback: Hessian evaluation
    addOption(problem, "hessian_approximation", "limited-memory")
    addOption(problem, "tol", 1e-1)
    #addOption(problem, "acceptable_tol", 1e-1)
    addOption(problem, "max_iter", 500)
    addOption(problem, "constr_viol_tol", 1e10)
    addOption(problem, "mu_init", 0.0001)
    addOption(problem, "acceptable_iter", 0)
    problem.x = initial_x
    return problem

end


text:
module TestGaussianProcesses
@time include("utils.jl")
@time include("gp.jl")
@time include("means.jl")
@time include("kernels.jl")
@time include("optim.jl")
@time include("mcmc.jl")
@time include("gpa.jl")
@time include("elastic.jl")
@time include("memory.jl")
@time include("test_crossvalidation.jl")
@time include("test_sparse.jl")
@time include("heteroscedastic.jl")
#@time include("test_vi.jl")
end


text:
#!/usr/bin/env julia

# Draw lines with various dash styles.

using Compose
using Color

function draw_lines(caps)
    if length(caps) == 0
        context()
    else
        compose(compose(context(), line([(5mm, 5mm), (15mm, 5mm)]), strokelinecap(caps[1])),
                compose(context(0, 5mm), draw_lines(caps[2:end])))
    end
end

caps = [LineCapButt(), LineCapSquare(), LineCapRound()]
c = draw_lines(caps)

imgs = [SVG("linecaps.svg", 2cm, 3cm),
        PDF("linecaps.pdf", 2cm, 3cm)]
for img = imgs
    draw(img, compose(c, stroke(color("black")), linewidth(2mm)))
end


text:
using CombOpt
using Test

@test isempty(detect_ambiguities(Base, Core, CombOpt))

tests = [
    "read_graph",
    "dijkstra",
    "bellman_ford",
    "forward_star",
]

if length(ARGS) > 0
    tests = ARGS
end

@testset "CombOpt" begin

    for t in tests
        fp = joinpath(dirname(@__FILE__), "test_$t.jl")
        println("$fp ...")
        include(fp)
    end

end


text:
function StatsModels.ModelFrame(f::StatsModels.Formula, d; kwargs...)
    TableTraits.isiterabletable(d) || error()
    StatsModels.ModelFrame(f, DataFrames.DataFrame(d); kwargs...)
end

function StatsModels.StatsBase.fit(::Type{T}, f::StatsModels.Formula, d, args...; contrasts::Dict = Dict(), kwargs...) where
                       T<:Union{ StatsModels.StatsBase.StatisticalModel,  StatsModels.StatsBase.RegressionModel}
    TableTraits.isiterabletable(d) || error()
    StatsModels.StatsBase.fit(T, f, DataFrames.DataFrame(d), args...; contrasts = contrasts, kwargs...)
end


text:
# cuSPARSE helper functions

## matrix descriptor

mutable struct CuMatrixDescriptor
    handle::cusparseMatDescr_t

    function CuMatrixDescriptor()
        descr_ref = Ref{cusparseMatDescr_t}()
        cusparseCreateMatDescr(descr_ref)
        obj = new(descr_ref[])
        finalizer(cusparseDestroyMatDescr, obj)
        obj
    end
end

Base.unsafe_convert(::Type{cusparseMatDescr_t}, desc::CuMatrixDescriptor) = desc.handle

function CuMatrixDescriptor(MatrixType, FillMode, DiagType, IndexBase)
    desc = CuMatrixDescriptor()
    if MatrixType != CUSPARSE_MATRIX_TYPE_GENERAL
        cusparseSetMatType(desc, MatrixType)
    end
    cusparseSetMatFillMode(desc, FillMode)
    cusparseSetMatDiagType(desc, DiagType)
    if IndexBase != CUSPARSE_INDEX_BASE_ZERO
        cusparseSetMatIndexBase(desc, IndexBase)
    end
    return desc
end


text:
@testset "distributionsapi" begin
    v = Char['a', 'a', 'b', 'c']
    d = SampleDistribution(v)
    @test ncategories(d) == 3
    for i ∈ 1:3
        @test rand(d) ∈ v
    end
    @test pdf(d, 'a') ≈ 0.5
    @test pdf(d, 'b') ≈ 0.25
    @test pdf(d, 'c') ≈ 0.25
    @test logpdf(d, 'a') ≈ log(0.5)
    @test insupport(d, 'a')
    @test !insupport(d, 'f')
    @test_throws MethodError insupport(d, 1)
    @test cdf(d, 'a') ≈ 1.0
    @test cdf(d, 'b') ≈ 0.5
    @test minimum(d) == 'a'
    @test maximum(d) == 'c'
    @test mode(d) == 'a'
    @test modes(d) == Char['a']
    @test expectation(d, x -> 5.0) ≈ 5.0
    @test_throws MethodError mgf(d, 1.0)
    @test_throws MethodError cf(d, 1.0)
    v2 = [1, 1, 2, 3]
    d2 = SampleDistribution(v2)
    @test cdf(d2, 1) ≈ 1.0
    @test cdf(d2, 2) ≈ 0.5
    @test mean(d2) ≈ 1.75
    @test 8.22 ≤ mgf(d2, 1.0) ≤ 8.23
    @test -0.082 ≤ real(cf(d2, 1.0)) ≤ -0.081
    @test expectation(d2, x -> 5.0) ≈ 5.0
    @test expectation(d2, identity) ≈ 1.75
    @test std(d2) ≈ std(v2, corrected=false)
end


text:
module CausticsEngineering

using DocStringExtensions

using Images, Plots
gr()

include("utilities.jl")
include("create_mesh.jl")

export main, engineer_caustics

end

max_stars_repo_name: Mehul-Bagaria/causticsEngineering

max_issues_repo_name: Mehul-Bagaria/causticsEngineering

max_forks_repo_name: Mehul-Bagaria/causticsEngineering


text:
module test_h3_lib_inspection

using Test
using H3.Lib

Lib.h3GetResolution
Lib.h3GetBaseCell
Lib.stringToH3
Lib.h3ToString
Lib.h3IsValid
Lib.h3IsResClassIII
Lib.h3IsPentagon

@test Lib.h3GetResolution(0x085283473fffffff) == 5

@test Lib.h3GetBaseCell(0x085283473fffffff) == 20

@test Lib.stringToH3("85283473fffffff") == 0x085283473fffffff

@test Lib.h3IsValid(0x85283473fffffff) == 1
@test Lib.h3IsValid(0x5004295803a8800) == 0

@test Lib.h3IsResClassIII(0x085283473fffffff) == 1

@test Lib.h3IsPentagon(0x085283473fffffff) == 0

end # module test_h3_lib_inspection


text:
"Normalize logical storage path"
function normalize_path(p::AbstractString)
    # \ to / since normpath on linux won't handle it
    p = replace(p, '\\'=>'/')
    p = normpath(p)
    # \ to / again since normpath on windows creates \
    p = replace(p, '\\'=>'/')
    p == "/" ? p : rstrip(p, '/')
end

# Stores files in a regular file system
struct DirectoryStore <: AbstractStore
    folder::String
    function DirectoryStore(p)
      mkpath(normalize_path(p))
      new(normalize_path(p))
    end
end

function Base.getindex(d::DirectoryStore, i::String)
  fname=joinpath(d.folder,i)
  if isfile(fname)
    read(fname)
  else
    nothing
  end
end

function Base.setindex!(d::DirectoryStore,v,i::String)
  fname=d.folder * "/" * i
  folder = dirname(fname)
  isdir(folder) || mkpath(folder)
  write(fname,v)
  v
end

storagesize(d::DirectoryStore,p) = sum(filter(i->i ∉ (".zattrs",".zarray"),readdir(d.folder * "/" * p))) do f
  fname = d.folder * "/" * p * "/" * f
  filesize(fname)
end

function subdirs(s::DirectoryStore,p)
  pbase = joinpath(s.folder,p)
  if !isdir(pbase)
    return String[]
  else
    return filter(i -> isdir(joinpath(s.folder,p, i)), readdir(pbase))
  end
end
function subkeys(s::DirectoryStore,p)
  pbase = joinpath(s.folder,p)
  if !isdir(pbase)
    return String[]
  else
    return filter(i -> isfile(joinpath(s.folder,p, i)), readdir(pbase))
  end
end
Base.delete!(s::DirectoryStore, k::String) = isfile(joinpath(s.folder, k)) && rm(joinpath(s.folder, k))


text:
import StaticArrays: StaticArray, tuple_prod

"""
    StructArrays.staticschema(::Type{<:StaticArray{S, T}}) where {S, T}

The `staticschema` of a `StaticArray` element type is the `staticschema` of the underlying `Tuple`.
```julia
julia> StructArrays.staticschema(SVector{2, Float64})
Tuple{Float64, Float64}
```
"""
@generated function StructArrays.staticschema(::Type{<:StaticArray{S, T}}) where {S, T}
    return quote
        Base.@_inline_meta
        return NTuple{$(tuple_prod(S)), T}
    end
end
StructArrays.createinstance(::Type{T}, args...) where {T<:StaticArray} = T(args)
StructArrays.component(s::StaticArray, i) = getindex(s, i)


text:
# Define global constants
const V_DES = 1; const SIGMA_IDM = 2; const T_HEADWAY = 3; const S_MIN=4;
const POLITENESS = 5;const ADV_TH = 6;const SENSOR_SIGMA = 7;
const COOPERATION = 8;

# function: particle to c-IDM model
"""
function cidm_from_particle(particle)

- Use particle to assign paramters of CooperativeIDM

# Example
```julia
# See hallucinate_a_step
```
"""
function cidm_from_particle(f::FilteringEnvironment,particle)
    return CooperativeIDM(
                env=f.mergeenv,
                c=particle[COOPERATION],
                idm = IntelligentDriverModel(
                            v_des = particle[V_DES],
                            σ=particle[SIGMA_IDM],T=particle[T_HEADWAY],
                            s_min=particle[S_MIN]
                )
            )
end

# function: hallucinate_a_step
"""
    function hallucinate_a_step(scene_input,particle;car_id=-1)
- Hallucinate one step starting from `scene_input` using parameters given by `particle`

# Examples
```julia
scene = f.traj[1]
particle = [29.,NaN,1.5,5.,0.35,0.1,NaN,1.0]
hallucinate_a_step(f,scene,particle,car_id=6)
```
"""
function hallucinate_a_step(f::FilteringEnvironment,scene_input,particle;car_id=-1)
    if car_id==-1 @show "hallucinate_a_step says: Please give a valid car_id" end

    scene = deepcopy(scene_input)
    models = Dict{Int64,DriverModel}()

    for veh in scene
        if veh.id == car_id
            models[veh.id] = cidm_from_particle(f,particle)
        else
            models[veh.id] = IntelligentDriverModel(v_des=15.)
        end
    end

    nticks = 1
    scene_list = simulate(scene,f.roadway,models,nticks,f.timestep)

    new_scene = scene_list[2] # simulate stores start scene in 1st elem
    # Thus, we need elem 2 as the hallucinated scene
    halluc_state = new_scene.entities[findfirst(car_id,scene)].state
    halluc_pos = halluc_state.posF.s
    halluc_lane = get_lane_id(scene,car_id)

    return halluc_pos,halluc_lane
end

# function: weight and resample
"""
    function weight_and_resample(start_scene,true_nextpos,true_nextlane,p_mat;car_id=-1)
- Hallucination from `start_scene`
- Compare against ground truth at `true_nextpos`, `true_nextlane`
- Assign weights to particles
- Perform resampling and return a new matrix of particles and associated weight vector

# Examples
```julia
# Test for one step on initial particle matrix
limits = [10. 40.;0.1 10.;0.5 5.;1. 10.;0. 1.;-1. 1.;0. 20.;0. 1.]
init_pmat = initial_pmat(limits=limits,num_particles=10,seed=4)
id = 6
scene = f.traj[1]
true_next_scene = deepcopy(f.traj[2])
true_nextpos = get_frenet_s(true_next_scene;car_id=id)
true_nextlane = get_lane_id(true_next_scene,id)
weight_and_resample(f,scene,true_nextpos,true_nextlane,init_pmat,car_id=id)
```
"""
function weight_and_resample(f::FilteringEnvironment,start_scene,true_nextpos,true_nextlane,p_mat;
    car_id=-1,verbosity=false)
    if car_id==-1 @show "compute_particle_likelihood says: Please give valid car_id" end
    num_p = size(p_mat,2)
    lkhd_vec = Array{Float64}(undef,num_p)
    for i in 1:num_p
        if verbosity print("w_and_resample says: particle number = $i \n") end
        particle = p_mat[:,i]

        std_dev_acc = p_mat[SIGMA_IDM]
        if std_dev_acc <= 0 std_dev_acc = 0.1 end
        std_dev_pos = f.timestep*f.timestep*std_dev_acc
        hpos,hlane = hallucinate_a_step(f,start_scene,particle,car_id=car_id)

        start_lane = get_lane_id(start_scene,car_id)
        lane_has_changed = false

        if start_lane != true_nextlane
            lane_has_changed = true
        end

        p_lanechange = get_lane_change_prob(f,start_scene,particle,car_id=car_id)

        prob_lane = 0.5 # Initialize to random
        if lane_has_changed
            prob_lane = p_lanechange
        else
            prob_lane = 1-p_lanechange
        end

        prob_pos = pdf(Normal(hpos,std_dev_pos),true_nextpos)
        if verbosity
            print("weight and resample says: true_nextpos = $(true_nextpos) and hpos=$(hpos) and hlane=$(hlane)\n")
            print("weight and resample says: prob_pos = $(prob_pos) and prob_lane=$(prob_lane)\n")
        end
        lkhd_vec[i] = prob_lane*prob_pos
    end
    p_weight_vec = StatsBase.weights(lkhd_vec./sum(lkhd_vec)) # Convert to weights form to use julia sampling

    idx = sample(1:num_p,p_weight_vec,num_p)
    if verbosity print("weight and resample says: ids are $(idx)\n") end
    new_p_mat = p_mat[:,idx] #Careful that idx is (size,1) and not (size,2)

    return new_p_mat, p_weight_vec
end

# function: multistep_update
"""
function multistep_update(f::FilteringEnvironment;car_id,start_frame,last_frame,num_p=500,seed=1)

- Run filtering over a trajectory by starting from a true scene
- Repeatedly calls `weight_and_resample` on a demonstration trajectory

# Caveats
- Hard coded limits on initial particle distribution generation

# Returns
- `final_p_mat`: Matrix with particles in separate columns. Mean of final particle set
- `iterwise_p_mat`: A list with the associated particle matrix at each iteration

# Examples
```julia
final_p_mat,iterwise_p_mat = multistep_update(f,car_id=6,start_frame=1,last_frame=99)
```
"""
function multistep_update(f::FilteringEnvironment;car_id,start_frame,last_frame,num_p=500,seed=1)
    start_scene = f.traj[start_frame]
    v = start_scene[findfirst(car_id,start_scene)].state.v
    limits = [v-2 v+2;0.1 5.;0.5 5.;1. 10.;0. 1.;-1. 1.;0. 1.;0. 1.]
    p_mat = initial_pmat(limits=limits,num_particles=num_p,seed=seed)
    iterwise_p_set = [] # Stores particle set at every iteration
    push!(iterwise_p_set,p_mat)

    for framenum in start_frame:last_frame
        #print("frame number = $(framenum)\n")
        scene = deepcopy(f.traj[framenum])
        true_next_scene = deepcopy(f.traj[framenum+1])
        true_nextpos = get_frenet_s(true_next_scene;car_id=car_id)
        true_nextlane = get_lane_id(true_next_scene,car_id)
        # print("multistep update says: true_nextpos = $(true_nextpos), true_nextlane=$(true_nextlane)\n")
        p_mat_new, weight_vec = weight_and_resample(f,scene,true_nextpos,
        true_nextlane,p_mat,car_id=car_id)

            # Addition of dithering noise
        #params = [:v_des,:σ]
        #p_mat = addnoise(p_mat_new, weight_vec)

        p_mat = p_mat_new # No noise addition for now

            # Storage into list that contains particle matrix at every step
        push!(iterwise_p_set,p_mat)
    end

        # Find the mean particle after filtering is over
    mean_particle = mean(p_mat,dims=2)

    return mean_particle,iterwise_p_set
end

"""
    function obtain_driver_models(f::FilteringEnvironment,veh_id_list,num_particles,start_frame,last_frame)
Driver models for each vehicle in veh_id_list

# Arguments
- `veh_id_list`: List with vehicle ids
- `num_p`: Number of particles
- `ts`: Frame to start filtering from
- `te`: Frame to end hallucination at

# Returns
- `models` Dict with veh id as key and IDM driver model as value
- `final_particles` Dict with veh id as key and avg particle over final particle set as value
- `mean_dist_mat`: Every elem is the mean dist of particle set at that iter (row) for that car (column)

# Example
```julia
veh_id_list = [6]
f = FilteringEnvironment()
new_models,final_particles,mean_dist_mat = obtain_driver_models(f,veh_id_list=veh_id_list,num_p=50,ts=1,te=5)
avg_over_cars = mean(mean_dist_mat,dims=2)
```
"""
function obtain_driver_models(f::FilteringEnvironment;veh_id_list=[],num_p=50,ts=1,te=100)

    models = Dict{Int64,DriverModel}() # key is vehicle id, value is driver model
    final_particles = Dict{Int64,Array{Float64}}() # key is vehicle id, value is final particles

        # Loop over all the cars and get their corresponding parameters by particle filter
    num_cars = length(veh_id_list)
    num_iters = te-ts+2 # NEED TO CONFIRM THIS

        # num_iters x num_cars. Every elem is the mean dist of particle set at that iter for that car
    mean_dist_mat = fill(0.,num_iters,num_cars)

    for (ii,veh_id) in enumerate(veh_id_list)
        print("obtain_driver_models. vehicle id = $(veh_id) \n")

        mean_particle, iterwise_p_set = multistep_update(f,num_p=num_p,
                car_id = veh_id,start_frame = ts,last_frame = te)
        #print("mean_particle", mean_particle, "\n")

        final_particles[veh_id] = mean_particle
            # note: T=0.2 and s_min=1.
        models[veh_id] = cidm_from_particle(f,mean_particle)

        #print("After filtering mean_particle = $(mean_particle)\n")
        # num_iters = length(iterwise_p_set) # SHOULD MATCH OUTSIDE LOOP VARIABLE
        mean_dist_over_iters = fill(0.,num_iters,1)
        for (jj,p_mat) in enumerate(iterwise_p_set)
            mean_dist_over_iters[jj,1] = avg_dist_particles(p_mat,mean_particle)
        end

        mean_dist_mat[:,ii] = mean_dist_over_iters
    end

    return models, final_particles, mean_dist_mat
end

max_stars_repo_name: sisl/AutomotiveInteraction

max_issues_repo_name: sisl/AutomotiveInteraction

max_forks_repo_name: sisl/AutomotiveInteraction