text:
#some demos for the NIPS2018 Challenge: https://www.crowdai.org/challenges/nips-2018-ai-for-prosthetics-challenge
#PyCall needs to point to Anaconda3!
#trick: type "which python" in console, then reset Python_ENV
# ENV["PYTHON"] = "result of which-Python by anaconda 3"
#Pkg.build("PyCall")

#before statup of julia, type: "activate opensim-rl"

using PyCall
#@pyimport osim.env as osim
u_range = zeros(19) .+ 0.5
s_init  = zeros(160)

#u_range = zeros(18) .+ 0.5
#s_init  = zeros(160)

@everywhere function reset_Skeleton(env)
    return Float64.(env[:reset]())
end

#Interact with environment
@everywhere function environment_call_Skeleton(s_t1::Array{Float64,1},a_t::Array{Float64,1}, params::Params, env)

    #Ineract with environment:
    this_act = Float32.(a_t)

    observation, reward, done, info = env[:step](this_act)
    reward += s_t1[1]^2

    if done == true
        #punish for falling over
        reward  -= 100
        terminal = 1
    else
        terminal = 0
    end

    s_t2 = Float64.(observation)
    r_t  = Float64.(reward) * (-1)

    return s_t2 , r_t, terminal
end

@everywhere function run_episode_skeleton!(params::Params,θa::Any, θv::Any, traj::Trajectory, rms::RunMeanStd,env)
    #@pyimport osim.env as osim
    #env = osim.ProstheticsEnv(visualize=true)

    dt        = params.dt;
    t_horizon = params.t_horizon;
    s_init    = reset_Skeleton(env) #+ 0.1*sum(params.σ)*randn(length(params.s_init))
    l_traj    = params.l_traj;
    σ         = params.σ;
    u_range   = params.u_range;
    γ         = params.γ_disc;

    s_t1 = copy(s_init)
    s_t2 = similar(s_t1)
    totalr = 0
    l_out = length(u_range)

    lim_s_t = 200

    for i_t = 1:l_traj
        #Clip to high values
        s_norm = s_t1

        #Evaluate the Network Output
        out_t  = mlp_nlayer_policy_norm(θa,s_norm,params) .+ 0.5
        val_t  = mlp_nlayer_value_norm(θv,s_norm,params)[1]

        #Sample:
        rand_vec = randn(l_out)

        #Get Action with stochastic policy:
        loc_a_t = sample_action(out_t, params.σ, rand_vec)
        p_a_t   = get_π_prob(out_t,σ,loc_a_t)

        #Interact with the Environment:
        s_t2, loc_r_t, terminal = environment_call_Skeleton(s_t1,loc_a_t,params,env)

        #Save data in trajectory:
        traj.s_t[i_t,:]    .= s_norm
        traj.s_t2[i_t,:]   .= s_t2
        traj.a_t[i_t,:]    .= loc_a_t;
        traj.p_a_t[i_t,:]   = p_a_t;
        traj.r_t[i_t]       = loc_r_t*0.01;
        traj.val_t[i_t]     = val_t;
        traj.out_t[i_t,:]   = out_t;
        traj.len            = i_t
        totalr             += loc_r_t

        #Override the state:
        s_t1 .= s_t2;

        #Check if the terminal condition is fullfilled and break
        if terminal == 1
            traj.s_t[i_t,:] .= s_t1
            break
        end

    end

    return totalr / traj.len
end

@everywhere function test_env()
    try
        x = sum(env[:reset]())
        println(x)
        return x
    catch
        return 0
    end
end

"""
Function enabling parallel execution of the episodes by setting up the channels
"""
function parallel_episodes_skeleton(params::Params,θa::Any,θv::Any,all_traj::Traj_Data,rms_state::RunMeanStd)
    n_traj = params.n_traj
    active_workers = workers()
    n_workers = length(active_workers)

    # Create Channels storing results and starting valus
    s_init_channel = RemoteChannel(()->Channel{Array{Float64,1}}(length(workers())+1)) # will contain s_init
    result_channel = RemoteChannel(()->Channel{Tuple}(length(workers())+1)) # will contain traj and gradients

    begin
        for p in workers()
            remote_do(distributed_episodes_skeleton, p, params, θa, θv, rms_state, s_init_channel, result_channel, p)
        end
    end

    # Start optimizations
    for i_traj = 1:n_traj
        s_init_local = params.s_init
        @async put!(s_init_channel, s_init_local)
    end

    prog = Progress(n_traj,1)
    res  = 0
    prec = 0

    ĝ_a = copy(θa)*0
    ĝ_v = copy(θv)*0

    for i_traj = 1:n_traj
        #Take from Channel:
        result = take!(result_channel)
        #Oder the Tuple:
        all_traj.traj[i_traj] = result[1]
        res  += result[4]
        prec += result[5]
        #update progress bar
        ProgressMeter.update!(prog,i_traj)
    end

    # Tell workers that its done
    for j = 1:length(workers())
        NaN_mat = zeros(length(params.s_init))
        fill!(NaN_mat,NaN)
        @async put!(s_init_channel, NaN_mat)
    end

    #Close Channels and clean rubbish
    sleep(0.1)
    close(s_init_channel)
    close(result_channel)

    #Return the final Results
    return  res, prec
end

# Function running on the worker
@everywhere function distributed_episodes_skeleton(params::Params, θa::Any, θv::Any, rms_state::RunMeanStd, s_init_channel::RemoteChannel, result_channel::RemoteChannel, p)

    #env = osim.ProstheticsEnv(visualize=true)

    # Preallocate trajectory
    traj = init_traj(params,params.l_traj)
    s_init_local = zeros(length(params.s_init))

    # Start loop where work is done
    while true
        # break loop if NaN state received
        try
            #println("take envrionment")
            s_init_local = take!(s_init_channel)
            if (sum(isnan, s_init_local) > 0)
                #println("Done, garbage collection!")
                GC.gc()
                break
            end
        catch
            println("Some problem occured, worker $(myid()) exiting trajectory optimization function.")
            break
        end

        #DO THE WORK:
        #-----------------------------------------------------------------------
        res = run_episode_skeleton!(params,θa,θv,traj,rms_state,env)

        #Calculate the Advantage using GAE
        gae!(params,traj)

        #Discount the rewards for supervised Training of Value function
        traj.R_t = vec(discount(traj.r_t, params.γ_disc))

        #Calculate the Policy Gradient using GAE
        ĝ_a_loc = grad_policy(θa,params,traj)

        #Calculate the Value Function gradient using TD(1)
        ĝ_v_loc = 0#grad_value_bs(θv,params,traj)

        #Collect Precision
        prec = loss_value(θv,params,traj)
        #-----------------------------------------------------------------------

        put!(result_channel, (traj, ĝ_a_loc, ĝ_v_loc, res, prec))
    end

    return nothing
end

@everywhere function run_episode_skeleton_demo!(params::Params,θa::Any, θv::Any, traj::Trajectory, rms::RunMeanStd,env)
    #@pyimport osim.env as osim
    #env = osim.ProstheticsEnv(visualize=true)

    dt        = params.dt;
    t_horizon = params.t_horizon;
    s_init    = reset_Skeleton(env) #+ 0.1*sum(params.σ)*randn(length(params.s_init))
    l_traj    = params.l_traj;
    σ         = params.σ;
    u_range   = params.u_range;
    γ         = params.γ_disc;

    s_t1 = copy(s_init)
    s_t2 = similar(s_t1)
    totalr = 0
    l_out = length(u_range)

    lim_s_t = 200

    for i_t = 1:l_traj
        #Take the unnormalizes state
        s_norm = s_t1

        #Evaluate the Network Output
        out_t  = mlp_nlayer_policy_norm(θa,s_norm,params) .+ 0.5
        val_t  = mlp_nlayer_value_norm(θv,s_norm,params)[1]

        rand_vec = zeros(l_out)

        #Get Action with stochastic policy:
        loc_a_t = sample_action(out_t, params.σ, rand_vec)
        p_a_t   = get_π_prob(out_t,σ,loc_a_t)

        #Interact with the Environment:
        s_t2, loc_r_t, terminal = environment_call_Skeleton(s_t1,loc_a_t,params,env)

        #Save data in trajectory:
        traj.s_t[i_t,:]    .= s_norm
        traj.s_t2[i_t,:]   .= s_t2
        traj.a_t[i_t,:]    .= loc_a_t;
        traj.p_a_t[i_t,:]   = p_a_t;
        traj.r_t[i_t]       = loc_r_t*0.01;
        traj.val_t[i_t]     = val_t;
        traj.out_t[i_t,:]   = out_t;
        traj.len            = i_t
        totalr             += loc_r_t

        #Override the state:
        s_t1 .= s_t2;

        #Check if the terminal condition is fullfilled and break
        if terminal == 1
            traj.s_t[i_t,:] .= s_t1
            break
        end

    end

    return totalr / traj.len
end

max_stars_repo_name: NikonPic/REINFORCEMENT-JULIA

max_issues_repo_name: NikonPic/REINFORCEMENT-JULIA

max_forks_repo_name: NikonPic/REINFORCEMENT-JULIA


text:
@with_kw struct TimePoint
    timestamp::Float64                  = 0.0
    coordinates::SVector{2,Float64}     = [0.0, 0.0]
end

"""
A Bernoulli model for grasp success, given the number of attempts made.
"""
struct DiscreteGPM
    bernoulli_prob::Float64
end
cumprob(dgpm::DiscreteGPM, steps::Int64) = 1.0 - (1 - dgpm.bernoulli_prob)^steps

"""
Immutable arm parameters.
"""
@with_kw struct Arm
    base_pos::SVector{2,Float64}
    bin_pos::SVector{2,Float64}
    reach::SVector{2,Float64}
    orig_pos::SVector{2,Float64}
    grasp_model::DiscreteGPM
    downtime::Float64
end

"""
Mutable arm properties.
"""
@with_kw mutable struct ArmProperties
    endeff_pos::SVector{2,Float64}                  = [0.0, 0.0]
    obj_grabbed::String                             = ""
    workspace_objs::Set{String}                     = Set{String}()
    interaction_events::Vector{InteractionEvent}    = Vector{InteractionEvent}(undef, 0)
    tree::SearchTree                                = SearchTree()
end

"""
Sets up the reflected generator for feasible object sequences.
"""
@enum GENSTAGE PREDROP=1 POSTDROP=2 ATBIN=3
@with_kw mutable struct MirrorGenerator
    arm_set::Vector{Arm}                    = Vector{Arm}(undef, 0)
    gen_box_id::Int64                       = 0
    box_pos::Dict{Int64,Float64}            = Dict{Int64,Float64}()
    arm_stages::Vector{GENSTAGE}            = Vector{GENSTAGE}(undef, 0)
end

"""
The environment object for the domain.
"""
@with_kw mutable struct ConveyorBelt <: MRTAEnvironment
    current_time::Float64                       = 0.0
    num_active_boxes::Int64                     = 0
    num_total_boxes::Int64                      = 0
    curr_speed::Float64                         = 0.6
    box_routes::Dict{String,Vector{TimePoint}}  = Dict{String,Vector{TimePoint}}()
    belt_start::Float64                         = 0.0
    belt_end::Float64                           = 1.0
    belt_width::Float64                         = 0.1
    new_box_prob::Float64                       = 0.75
    lost_boxes::Int64                           = 0
    deposited_boxes::Int64                      = 0
    mirror_gen::MirrorGenerator                 = MirrorGenerator()
end
const ArmAllocationServer = GenericAllocation{Arm,ArmProperties}

ext: jl


text:
using AbstractTrees
using VT100
using TerminalUI
using Colors
using JuliaParser: LineNumbers
import JuliaParser: Lexer, Tokens
import JuliaParser.Tokens: ⤄

function do_transform(node,code,create_template,create_portal,from_line)
    max = Tokens.normalize(reduce(⤄,PostOrderDFS(node)))
    file = SourceFile(code)
    startline = compute_line(file, max.offset)
    stopline = compute_line(file, max.offset+max.length)
    nlines = stopline-startline+1
    lines = create_template(file[startline:stopline])
    # First by display line, then by source line, to be reconfigured later
    displaylines = Any[]
    ranges = collect(IndEnumerate(StatelessBFS(node)))
    callback = create_portal(ranges)
    for (i,(ind,range)) in enumerate(ranges)
        if length(displaylines) < length(ind) + 1
            push!(displaylines,deepcopy(lines))
        end
        active_displayline = displaylines[length(ind) + 1]
        range = range.loc
        range = (range.offset):(range.offset + range.length - 1)
        LineBreaking(convert(UInt64,file.offsets[startline]),file,active_displayline)[range] =
          callback()
    end
    # Assemble the cell display
    celllines = Any[]
    for i = startline:stopline
        content = file[i]
        push!(celllines,from_line(content))
        for j = length(displaylines):-1:1
            push!(celllines,displaylines[j][1+i-compute_line(file,max.offset+1)])
        end
    end
    celllines
end

function create_textrep(node, code)
    create_template(lines) = [[Char(' ') for _ in 1:length(line)] for line in lines]
    create_portal(ranges) = ()->'x'
    from_line(content) = [Char(c) for c in content]
    celllines = do_transform(node, code, create_template, create_portal, from_line)
    join(map(x->rstrip(join(x,"")),celllines),'\n')
end

function create_widget(node, code)
    create_template(lines) =
        [Cell[Cell(' ') for i = 1:length(line)] for line in lines]
    function create_portal(ranges)
        n = length(ranges)
        colors = distinguishable_colors(n+1,RGB(0,0,0))[2:end]
        ()->Cell(TerminalUI.dummycell(:white,pop!(colors)), content = ' ')
    end
    from_line(content) = [Cell(TerminalUI.dummycell(:default,:default),
      content = Char(c)) for c in content]
    celllines = do_transform(node, code, create_template, create_portal, from_line)
    TerminalUI.CellDisplay(celllines)
end


text:
module ThreadPools

export @tthreads, @bthreads, @qthreads, @qbthreads
export @logthreads, @logbthreads, @logqthreads, @logqbthreads
export twith, poolresults, isactive, @tspawnat
export dumplog, readlog, showactivity, showstats
export tmap, bmap, qmap, qbmap
export logtmap, logbmap, logqmap, logqbmap
export tforeach, bforeach, qforeach, qbforeach
export logtforeach, logbforeach, logqforeach, logqbforeach
export spawnbg, checked_fetch

include("interface.jl")
include("macros.jl")
include("logs.jl")
include("staticpool.jl")
include("qpool.jl")
include("logstaticpool.jl")
include("logqpool.jl")
include("simplefuncs.jl")
include("spawn_background.jl")

export @pthreads, pwith

@deprecate tforeach(pool::AbstractThreadPool, fn, itrs...) tforeach(fn, pool, itrs...)
@deprecate tmap(pool::AbstractThreadPool, fn, itrs...) tmap(fn, pool, itrs...)
@deprecate pwith(fn, pool) twith(fn, pool)

macro pthreads(thrdid, expr)
    @warn "`@pthreads` is deprecated, use `tthreads` instead."
end

end # module


text:
# # Diurnal cycle

## load packages
using PlantHydraulics
using PlotPlants
FT = Float32;
#------------------------------------------------------------------------------

## prescribing time series (two days)
Δt    = FT(10);
T_day = FT(24 * 60 * 60);
Ts    = collect(FT, 0:Δt:(2*T_day));
Hs    = Ts ./ 3600;
Rs    = (Hs .- 6) ./ 24 .* FT(2π);
Es    = max.(FT(5e-4), sin.(Rs) ./ 100);
#------------------------------------------------------------------------------

# ## Leaf water content
# ### Example of grass
grass = create_grass(FT(-3),FT(6),FT[0,-1,-2,-3],FT[0,1,2,3,4,5,6]);
LWCs  = similar(Es);
for i in eachindex(Es)
    ## update evaporation rate
    for leaf in grass.leaves
        leaf.q_out = Es[i];
    end

    ## update water contents
    update_PVF!(grass, Δt);

    ## calculate total leaf water content
    lwc = 0;
    for leaf in grass.leaves
        lwc += leaf.area * leaf.v_storage;
    end
    LWCs[i] = lwc;
end
#------------------------------------------------------------------------------

_fig,_axes = create_canvas("Grass canopy water content"; figsize=(5.5,3.5));
_ax1 = _axes[1];
_bx1 = _ax1.twinx();
_ax1.plot(Hs, Es, "-", color="salmon");
_bx1.plot(Hs, LWCs, "-", color="royalblue");
_ax1.set_xlabel("Hour (h)");
_ax1.set_ylabel("Leaf transpiration rate (mol m⁻² s⁻¹)", color="salmon");
_ax1.set_xticks(collect(0:4:48));
_ax1.set_xlim(0,48);
_bx1.set_ylabel("Grass canopy water content (mol)", color="royalblue");
_fig.set_tight_layout(true);
_fig
#------------------------------------------------------------------------------

# ### Example of tree
tree = create_tree(FT(-2.1), FT(5.4), FT(8), FT[0,-1,-2,-3],
                           collect(FT,0:1:20));
LWCs = similar(Es);
for i in eachindex(Es)
    ## update evaporation rate
    for leaf in tree.leaves
        leaf.q_out = Es[i];
    end

    ## update water contents
    update_PVF!(tree, Δt);

    ## calculate total leaf water content
    lwc = 0;
    for leaf in tree.leaves
        lwc += leaf.area * leaf.v_storage;
    end
    LWCs[i] = lwc;
end
#------------------------------------------------------------------------------

_fig,_axes = create_canvas("Tree canopy water content"; figsize=(5.5,3.5));
_ax1 = _axes[1];
_bx1 = _ax1.twinx();
_ax1.plot(Hs, Es, "-", color="salmon");
_bx1.plot(Hs, LWCs, "-", color="royalblue");
_ax1.set_xlabel("Hour (h)");
_ax1.set_ylabel("Leaf transpiration rate (mol m⁻² s⁻¹)", color="salmon");
_ax1.set_xticks(collect(0:4:48));
_ax1.set_xlim(0,48);
_bx1.set_ylabel("Tree canopy water content (mol)", color="royalblue");
_fig.set_tight_layout(true);
_fig
#------------------------------------------------------------------------------

# ## Drier soil
# ### Example of grass
grass = create_grass(FT(-3),FT(6),FT[0,-1,-2,-3],FT[0,1,2,3,4,5,6]);
for root in grass.roots
    root.p_ups = -1.0;
end
LWCs  = similar(Es);
for i in eachindex(Es)
    ## update evaporation rate
    for leaf in grass.leaves
        leaf.q_out = Es[i];
    end

    ## update water contents
    update_PVF!(grass, Δt);

    ## calculate total leaf water content
    lwc = 0;
    for leaf in grass.leaves
        lwc += leaf.area * leaf.v_storage;
    end
    LWCs[i] = lwc;
end
#------------------------------------------------------------------------------

_fig,_axes = create_canvas("Drier soil"; figsize=(5.5,3.5));
_ax1 = _axes[1];
_bx1 = _ax1.twinx();
_ax1.plot(Hs, Es, "-", color="salmon");
_bx1.plot(Hs, LWCs, "-", color="royalblue");
_ax1.set_xlabel("Hour (h)");
_ax1.set_ylabel("Leaf transpiration rate (mol m⁻² s⁻¹)", color="salmon");
_ax1.set_xticks(collect(0:4:48));
_ax1.set_xlim(0,48);
_bx1.set_ylabel("Grass canopy water content (mol)", color="royalblue");
_fig.set_tight_layout(true);
_fig
#------------------------------------------------------------------------------


text:
labels = [15; 24]
lnum = length(labels)

# See outer parameters
delta = 1.0   # all-or-nothing cut
seednum = 200
ntimes = 5
epsis = [1.0]
grownum = 10000

for e = 1:length(epsis)

    # Output from HyperLocal
    hl_pr = zeros(lnum,ntimes)
    hl_re = zeros(lnum,ntimes)
    hl_f1 = zeros(lnum,ntimes)
    hl_time = zeros(lnum,ntimes)
    hl_size = zeros(lnum,ntimes)
    newS = zeros(lnum,ntimes)
    hl_cond = zeros(lnum,ntimes)

    # Output from first baseline 1
    b1_pr = zeros(lnum,ntimes)
    b1_re = zeros(lnum,ntimes)
    b1_f1 = zeros(lnum,ntimes)
    b1_cond = zeros(lnum,ntimes)

    # Output from baseline 2
    b2_pr = zeros(lnum,ntimes)
    b2_re = zeros(lnum,ntimes)
    b2_f1 = zeros(lnum,ntimes)
    b2_cond = zeros(lnum,ntimes)

    # Keep track of R
    r_pr = zeros(lnum,ntimes)
    r_re = zeros(lnum,ntimes)
    r_f1 = zeros(lnum,ntimes)
    r_cond = zeros(lnum,ntimes)

    # For each epsilon we store a different matrix of outputs
    epsilon = epsis[e]
    outputmat = "Output/Large_$seednum"*"_$grownum"*"_$epsilon.mat"
    println(outputmat)

    # For a fixed epsilon, seednum, and grownum, run experiments
    # on each cluster multiple times
    for lab = 1:length(labels)
        label = labels[lab]
        T = findall(x->x ==label,NodeLabels)
        nT = length(T)

        for index = 1:ntimes

            # Generate a new seed set
            p = randperm(nT)
            Rstart = T[p[1:seednum]]
            OneHop = get_immediate_neighbors(H,Ht,Rstart)
            Rmore = BestNeighbors(H,d,Rstart,OneHop,grownum)
            R = union(Rmore,Rstart)
            Rs = findall(x->in(x,Rstart),R)     # Force seed nodes to be in output set
            prr, rer, f1r = PRF(T,R)
            r_pr[index] = prr
            r_re[index] = rer
            r_f1[index] = f1r
            condR, volR, cutR = tl_cond(H,R,d,1.0,volA,order)
            r_cond[index] = condR

            # Run HyperLocal
            s = time()
            S, lcond = HyperLocal(H,Ht,order,d,R,epsilon,delta,Rs,true)
            hl_time[lab,index] = time()-s
            condS, volS, cutS = tl_cond(H,S,d,1.0,volA,order)
            pr, re, f1 = PRF(T,S)
            hl_pr[lab,index] = pr
            hl_re[lab,index] = re
            hl_f1[lab,index] = f1
            hl_size[lab,index] = length(S)
            hl_cond[lab,index] = condS
            nS =  length(setdiff(S,R))
            newS[lab,index] = nS

            # First baseline
            kS = nT-length(Rstart)
            B1 = BestNeighbors(H,d,Rstart,OneHop,kS)
            pr1, re1, f11 = PRF(T,B1)
            b1_pr[lab,index] = pr1
            b1_re[lab,index] = re1
            b1_f1[lab,index] = f11
            cond, vol, cut = tl_cond(H,B1,d,1.0,volA,order)
            b1_cond[index] = cond

            # Baseline 2
            B2 = TopNeighbors(H,Rstart,OneHop,kS)
            pr2, re2, f12 = PRF(T,B2)
            b2_pr[lab,index] = pr2
            b2_re[lab,index] = re2
            b2_f1[lab,index] = f12
            cond, vol, cut = tl_cond(H,B2,d,1.0,volA,order)
            b2_cond[index] = cond

            println("$label ($nT): $f11 \t $f12 \t $f1 \t $nS")
        end

        matwrite(outputmat, Dict("hl_size"=>hl_size, "newS"=>newS, "hl_time"=>hl_time,
        "hl_pr"=>hl_pr, "hl_re"=>hl_re, "hl_f1"=>hl_f1, "hl_cond"=>hl_cond,
        "b1_pr"=>b1_pr, "b1_re"=>b1_re, "b1_f1"=>b1_f1,"b1_cond"=>b1_cond,"b2_cond"=>b2_cond,
        "r_pr"=>r_pr, "r_re"=>r_re, "r_f1"=>r_f1, "r_cond"=>r_cond,
        "b2_pr"=>b2_pr, "b2_re"=>b2_re, "b2_f1"=>b2_f1))

    end
end

max_stars_repo_name: nveldt/HypergraphFlowClustering

max_issues_repo_name: nveldt/HypergraphFlowClustering

max_forks_repo_name: nveldt/HypergraphFlowClustering


text:
# -*- coding: utf-8 -*-
# ---
# jupyter:
#   jupytext:
#     formats: ipynb,jl:hydrogen
#     text_representation:
#       extension: .jl
#       format_name: hydrogen
#       format_version: '1.3'
#       jupytext_version: 1.10.3
#   kernelspec:
#     display_name: Julia 1.6.3
#     language: julia
#     name: julia-1.6
# ---

# %%
using SymPy
Poly = sympy.Poly
@vars a b p q x α β

# %%
λ = symbols("λ[0:9]")

# %%
f = x^3 + a*x + b
Poly(f, x)

# %%
g = x^3 + p*x + q
Poly(g, x)

# %%
H = sum(λ[begin+k]*x^k for k in 0:8) + x^9
Poly(H, x)

# %%
h = H(x => α + β).expand()
for k in 6:-1:0
    h = h(α^(k+3) => α^(k+3) - α^k * f(x=>α), β^(k+3) => β^(k+3) - β^k * g(x=>β)).expand()
end
P = Poly(h, α, β)

# %%
C = P.coeffs()

# %%
length(C)

# %%
sol = @time solve(C, λ)

# %% [markdown]
# Assume that
#
# * the minimal polynomial of $\alpha$ is equal to $x^3 + ax + b$,
# * the minimal polynomial of $\beta$ is equal to $x^3 + px + q$, and
# * the minimal polynomial of $\alpha + \beta$ is of degree $9$.
#
# Then the minimal polynomial of $\alpha + \beta$ is equal to
#
# $x^{9} + \left(3 a + 3 p\right) x^{7} + \left(3 b + 3 q\right) x^{6} + \left(3 a^{2} + 3 a p + 3 p^{2}\right) x^{5} + \left(6 a b - 3 a q - 3 b p + 6 p q\right) x^{4} \\+ \left(a^{3} + a^{2} p + a p^{2} + 3 b^{2} - 21 b q + p^{3} + 3 q^{2}\right) x^{3} + \left(3 a^{2} b + 3 a^{2} q - 6 a b p - 6 a p q + 3 b p^{2} + 3 p^{2} q\right) x^{2} \\+ \left(a^{3} p - 2 a^{2} p^{2} + 3 a b^{2} - 3 a b q + a p^{3} - 6 a q^{2} - 6 b^{2} p - 3 b p q + 3 p q^{2}\right) x \\+ a^{3} q + a^{2} b p - 2 a^{2} p q - 2 a b p^{2} + a p^{2} q + b^{3} + 3 b^{2} q + b p^{3} + 3 b q^{2} + q^{3}$.

# %%
h = sum(sol[λ[begin+k]]*x^k for k in 0:8) + x^9
Poly(h, x)

# %% [markdown]
# Assume that
#
# * the minimal polynomial of $\alpha$ is equal to $x^3 + ax + b$,
# * the minimal polynomial of $\beta$ is equal to $x^3 - p $, and
# * the minimal polynomial of $\alpha + \beta$ is of degree $9$.
#
# Then the minimal polynomial of $\alpha + \beta$ is equal to
#
# $x^{9} + 3 a x^{7} + \left(3 b - 3 p\right) x^{6} + 3 a^{2} x^{5} + \left(6 a b + 3 a p\right) x^{4} + \left(a^{3} + 3 b^{2} + 21 b p + 3 p^{2}\right) x^{3} \\+ \left(3 a^{2} b - 3 a^{2} p\right) x^{2} + \left(3 a b^{2} + 3 a b p - 6 a p^{2}\right) x -  a^{3} p + b^{3} - 3 b^{2} p + 3 b p^{2} - p^{3}$.

# %%
h1 = h(p=>0)(q=>-p)
Poly(h1, x)

# %% [markdown]
# Assume that
#
# * the minimal polynomial of $\alpha$ is equal to $x^3 - a$,
# * the minimal polynomial of $\beta$ is equal to $x^3 - b$, and
# * the minimal polynomial of $\alpha + \beta$ is of degree $9$.
#
# Then the minimal polynomial of $\alpha + \beta$ is equal to
#
# $x^{9} + \left(- 3 a - 3 b\right) x^{6} + \left(3 a^{2} - 21 a b + 3 b^{2}\right) x^{3} -  a^{3} - 3 a^{2} b - 3 a b^{2} - b^{3}$.

# %%
h2 = h(a=>0, p=>0)(b=>-a)(q=>-b)
Poly(h2, x)

# %% [markdown]
# The minimal polynomial of $\sqrt[3]{2} + \sqrt[3]{3}$ is equal to $x^{9} - 15 x^{6} - 87 x^{3} - 125$.

# %%
h3 = h2(a=>2, b=>3)
Poly(h3, x)

# %%

max_stars_repo_path: 0023/minimal polynomials of sums of two algebraic elements of degree 3.jl

max_stars_repo_name: genkuroki/public

max_issues_repo_name: genkuroki/public

max_forks_repo_name: genkuroki/public


text:
"""
Calculate normal Energy on an Array

    Enorm(x::Array{Float64,1})
"""
function Enorm(x::Array{Float64,1})
    n = sqrt(sum(diagm(x'*x)));
    n = n[1];
    return n
end

"""
    Enorm(x::Array{Complex64,1})
"""
function Enorm(x::Array{Complex64,1})
    n = sqrt(real(x'*x));
    return n[1]
end

"""
Smooth functional data using box filter in place

    smooth_data!(f::Array{Float64,2}, sparam=10)
"""
function smooth_data!(f::Array{Float64,2}, sparam=10)
    M, N = size(f);
    for r in 1:sparam
        for i in 1:N
            f[2:(M-1), i] = (f[1:(M-2),i]+2*f[2:(M-1),i] + f[3:M,i])/4;
        end
    end
end

"""
    smooth_data!(f::Array{Float64,1}, sparam=10)
    :param sparam: Number of times to run filter (default = 10)
"""
function smooth_data!(f::Array{Float64,1}, sparam=10)
    M = length(f);
    for r in 1:sparam
        f[2:(M-1)] = (f[1:(M-2)]+2*f[2:(M-1)] + f[3:M])/4;
    end
end

"""
Smooth functional data using box filter

    smooth_data(f::Array{Float64,1}, sparam=10)
    :param sparam: Number of times to run filter (default = 10)
"""
function smooth_data(f::Array{Float64,1}, sparam=10)
    M = length(f);
    g = zeros(Float64, M);
    g = copy(f);
    for r in 1:sparam
        g[2:(M-1)] = (g[1:(M-2)]+2*g[2:(M-1)] + g[3:M])/4;
    end

    return g
end

"""
Calculate gradient of function using B-splines

    gradient_spline(timet::Vector, f, smooth=false)
    :param: timet: Vector describing time samples
    :param: f: Vector or Array (M,N) describing functions of M samples
    :param: smooth: smooth data (default = false)
"""
function gradient_spline(timet::Vector, f, smooth=false)
    M = size(f,1);
    spar = 0.0;

    if ndims(f) > 1
        N = size(f,2);
        f0 = zeros(M, N);
        g = zeros(f0);
        g2 = zeros(f0);
        for k in 1:N
            if smooth
                spar = length(timet) * (0.25 * abs(maximum(f[:,k])))^2;
            end
            spl = Spline1D(timet, f[:,k]; s=spar);
            f0[:, k] = evaluate(spl, timet);
            g[:, k] = derivative(spl, timet; nu=1);
            g2[:, k] = derivative(spl, timet; nu=2);
        end
    else
        if smooth
            spar = length(timet) * (0.25 * abs(maximum(f)))^2;
        end
        spl = Spline1D(timet, f; s=spar);
        f0 = evaluate(spl, timet);
        g = derivative(spl, timet; nu=1);
        g2 = derivative(spl, timet; nu=2);
    end

    return f0, g, g2
end

"""
Convert function to square-root slope function (srsf)

    f_to_srsf(f::Array, timet=0, smooth=false)
    :param f: array of shape (M,N) describing N functions of M samples
    :param timet: vector describing time samples (default = 0) will generate
                  linearly spaced time vector of length M
    :param smooth: smooth data (default = false)
"""
function f_to_srsf(f::Array, timet=0, smooth=false)
    if (timet == 0)
        timet = collect(linspace(0,1,length(f)));
    end
    f0, g, g2 = gradient_spline(timet, f, smooth);
    q = g ./ sqrt(abs(g)+eps(Float64));
    return q
end

"""
Convert square-root slope function (srsf) to f

    srsf_to_f(q::Array, time, f0=0.0)
    :param q: array of shape (M,N) describing N srsf of M samples
    :param time: vector describing time samples of length M
    :param f0: initial value of f
"""
function srsf_to_f(q::Array, time, f0=0.0)
    M = size(q,1);
    if ndims(q) > 1
        N = size(q,2);
        f = zeros(M,N);
        for i = 1:N
            qnorm = abs(q[:,i]);
            integrand = q[:,i].*qnorm;
            f[:,i] = f0[i] + cumtrapz(time, integrand);
        end
    else
        integrand = q.*abs(q);
        f = f0+cumtrapz(time, integrand);
    end

    return f
end

"""
Calculate elastic distance between two functions

    elastic_distance(f1::Vector, f2::Vector, timet::Vector,
                     method::AbstractString="SIMUL")
    :param f1: vector of function 1 samples
    :param f2: vector of function 2 samples
    :param timet: vector of time samples
    :param method: optimization method to find warping, default is
                   Dynamic Programming ("DP"). Other options are
                   Coordinate Descent ("DP2"), Riemannian BFGS
                   ("RBFGS"), Simultaneous Alignment ("SIMUL")

    :return da: amplitude distance
    :return dp: phase distance
"""
function elastic_distance(f1::Vector, f2::Vector, timet::Vector,
                          method::AbstractString="SIMUL")
    q1 = f_to_srsf(f1, timet);
    q2 = f_to_srsf(f2, timet);

    gam = optimum_reparam(q1, timet, q2, 0.0, method=method);

    q2a = warp_q_gamma(timet, q2, gam);

    da = sqrt(sum(trapz(timet, (q1-q2a).^2)));

    M = length(gam);
    psi = sqrt(diff(gam)*(M-1));
    mu = ones(M-1);
    dp = real(acos(sum(mu.*psi)/(M-1)));

    return da, dp
end

function optimum_reparam(q1::Array{Float64,1}, timet::Array{Float64,1},
                         q2::Array{Float64,1}, lam::Float64=0.0;
                         method::AbstractString="DP", w=0.01, f1o::Float64=0.0,
                         f2o::Float64=0.0)
    q1 = q1./norm(q1);
    q2 = q2./norm(q2);
    c1 = srsf_to_f(q1,timet,f1o);
    c2 = srsf_to_f(q2,timet,f2o);
    M = length(q2);
    rotated = false;
    isclosed = false;
    skipm = 0;
    auto = 0;
    n1 = 1;
    if (method == "DP")
        G = zeros(M);
        T = zeros(M);
        sizei = Cdouble[0];
        ccall((:DynamicProgrammingQ2, libfdasrsf), Void,
            (Ptr{Float64}, Ptr{Float64}, Ptr{Float64}, Ptr{Float64}, Int32,
            Int32, Int32, Ptr{Float64},Ptr{Float64}, Int32, Int32, Ptr{Float64},
            Ptr{Float64}, Ptr{Float64}, Float64), q1, timet, q2, timet,
            n1, M, M, timet, timet, M, M, G, T, sizei, lam)
        G = G[1:round(Integer,sizei[1])];
        T = T[1:round(Integer,sizei[1])];
        yi = interpolate((T,), G, Gridded(Linear()))
        gam = yi[timet];
    elseif (method == "SIMUL")
        s1,s2,g1,g2,ext1,ext2,mpath = simul_align(c1,c2);
        u = linspace(0,1,length(g1));
        tmin = minimum(timet);
        tmax = maximum(timet);
        timet2 = copy(timet);
        timet2 = (timet2-tmin)/(tmax-tmin);
        gam = simul_gam(collect(u),g1,g2,timet2,s1,s2,timet2);
    elseif (method == "DP2")
        opt = zeros(M+n1*n1+1);
        swap = false;
        fopts = zeros(5);
        comtime = zeros(5);
        @cpp ccall((:optimum_reparam, libgropt), Void,
                   (Ptr{Float64}, Ptr{Float64}, Int32, Int32, Float64, Bool,
                    Bool, Bool, Int32, Int32, Ptr{Float64}, Bool, Ptr{Float64},
                    Ptr{Float64}), c1, c2, M, n1, 0.0, true, rotated, isclosed,
                    skipm, auto, opt, swap, fopts, comtime)

        gam = opt[1:end-2];

        if swap
            gam = invertGamma(gam);
        end

    else
        opt = zeros(M+n1*n1+1);
        swap = false;
        fopts = zeros(5);
        comtime = zeros(5);
        @cpp ccall((:optimum_reparam, libgropt), Void,
                   (Ptr{Float64}, Ptr{Float64}, Int32, Int32, Float64, Bool,
                    Bool, Bool, Int32, Int32, Ptr{Float64}, Bool, Ptr{Float64},
                    Ptr{Float64}), c1, c2, M, n1, w, false, rotated, isclosed,
                    skipm, auto, opt, swap, fopts, comtime)

        if fopts[1] == 1000
            @cpp ccall((:optimum_reparam, libgropt), Void,
                       (Ptr{Float64}, Ptr{Float64}, Int32, Int32, Float64, Bool,
                        Bool, Bool, Int32, Int32, Ptr{Float64}, Bool,
                        Ptr{Float64}, Ptr{Float64}), c1, c2, M, n1, 0.0, true,
                        rotated, isclosed, skipm, auto, opt, swap, fopts,
                        comtime)
        end

        gam = opt[1:end-2];

        if swap
            gam = invertGamma(gam);
        end
    end

    gam = (gam-gam[1]) ./ (gam[end] - gam[1]);

    return gam
end

"""
Calculate optimum parameterization (warping of q2 to q1)

    optimum_reparam(q1, timet, q2, lam=0.0, method="SIMUL", w=0.01, f1o=0.0,
                    f2o=0.0)
    :param q1: array (M,N) or vector (M) describing srsf set 1
    :param timet: vector describing time samples of length M
    :param q2: array (M,N) or vector (M) describing srsf of set 2
    :param lam: control amount of warping (default=0.0)
    :param method: optimization method to find warping, default is
                   Dynamic Programming ("DP"). Other options are
                   Coordinate Descent ("DP2"), Riemannian BFGS
                   ("RBFGS"), Simultaneous Alignment ("SIMUL")
    :param w: Controls RBFGS (default = 0.01)
    :param f1o: initial value of f1, vector or scalar depending on q1, defaults
                to zero
    :param f2o: initial value of f2, vector or scalar depending on q1, defaults
                to zero

    optimum_reparam(q1, time1, q2, time2, lam=0.0, method="DP", w=0.01, f1o=0.0,
                    f2o=0.0)
    same as above, but different timing for q1 and q2

    optimum_reparam(beta1, beta2, lam, method="DP", w=0.01, rotated=true,
                    isclosed=false)
    :param beta1: array (n,T) describing curve 1
    :param beta2: array (n,T) describing curve 2
    :param lam: control amount of warping (default=0.0)
    :param method: optimization method to find warping, default is
                   Dynamic Programming ("DP"). Other options are
                   Coordinate Descent ("DP2"), Riemanain BFGS
                   ("RBFGS")
    :param w: Controls RBFGS (default = 0.01)
    :param rotated: calculate rotation (default = true)
    :param isclosed: closed curve (default = false)

    :return gam: warping function
    :return R: rotation matrix
    :return tau: seed value
"""
function optimum_reparam(q1::Array{Float64,1}, time1::Array{Float64,1},
                         q2::Array{Float64,1}, time2::Array{Float64,1},
                         lam::Float64=0.0; method::AbstractString="DP", w = 0.01,
                         f1o::Float64=0.0, f2o::Float64=0.0)
    q1 = q1./norm(q1);
    q2 = q2./norm(q2);
    c1 = srsf_to_f(q1,time1,f1o);
    c2 = srsf_to_f(q2,time2,f2o);
    M1 = length(q1);
    M2 = length(q2);
    n1 = 1;
    rotated = false;
    isclosed = false;
    skipm = 0;
    auto = 0;
    if (M1 != M2)
        method = "DP";
    end
    if (method == "DP")
        G = zeros(M1);
        T = zeros(M1);
        sizei = Cdouble[0];
        ccall((:DynamicProgrammingQ2, libfdasrsf), Void,
            (Ptr{Float64}, Ptr{Float64}, Ptr{Float64}, Ptr{Float64}, Int32,
            Int32, Int32, Ptr{Float64},Ptr{Float64}, Int32, Int32, Ptr{Float64},
            Ptr{Float64}, Ptr{Float64}, Float64), q1, time1, q2, time2,
            n1, M1, M2, time1, time2, M1, M2, G, T, sizei, lam)
        G = G[1:round(Integer,sizei[1])];
        T = T[1:round(Integer,sizei[1])];
        yi = interpolate((T,), G, Gridded(Linear()))
        gam = yi[time1];
    elseif (method == "SIMUL")
        s1,s2,g1,g2,ext1,ext2,mpath = simul_align(c1,c2);
        u = linspace(0,1,length(g1));
        tmin = minimum(time1);
        tmax = maximum(time1);
        timet1 = copy(time1);
        timet1 = (timet1-tmin)/(tmax-tmin);
        gam = simul_gam(collect(u),g1,g2,timet1,s1,s2,timet1);
    elseif (method == "DP2")
        opt = zeros(M1+n1*n1+1);
        swap = false;
        fopts = zeros(5);
        comtime = zeros(5);
        @cpp ccall((:optimum_reparam, libgropt), Void,
                   (Ptr{Float64}, Ptr{Float64}, Int32, Int32, Float64, Bool,
                    Bool, Bool, Int32, Int32, Ptr{Float64}, Bool,  Ptr{Float64},
                    Ptr{Float64}), c1, c2, M1, n1, 0.0, true, rotated, isclosed,
                    skipm, auto, opt, swap, fopts, comtime)

        gam = opt[1:end-2];

        if swap
            gam = invertGamma(gam);
        end

    else
        opt = zeros(M1+n1*n1+1);
        swap = false;
        fopts = zeros(5);
        comtime = zeros(5);
        @cpp ccall((:optimum_reparam, libgropt), Void,
                   (Ptr{Float64}, Ptr{Float64}, Int32, Int32, Float64, Bool,
                    Bool, Bool, Int32, Int32, Ptr{Float64}, Bool, Ptr{Float64},
                    Ptr{Float64}), c1, c2, M1, n1, w, false, rotated, isclosed,
                    skipm, auto, opt, swap, fopts, comtime)

        if fopts[1] == 1000
            @cpp ccall((:optimum_reparam, libgropt), Void,
                       (Ptr{Float64}, Ptr{Float64}, Int32, Int32, Float64, Bool,
                        Bool, Bool, Int32, Int32, Ptr{Float64}, Bool,
                        Ptr{Float64}, Ptr{Float64}), c1, c2, M1, n1, 0.0, true,
                        rotated, isclosed, skipm, auto, opt, swap, fopts,
                        comtime)
        end

        gam = opt[1:end-2];

        if swap
            gam = invertGamma(gam);
        end
    end

    gam = (gam-gam[1]) ./ (gam[end] - gam[1]);

    return gam
end

function optimum_reparam(q1::Array{Float64,1}, timet::Array{Float64,1},
                         q2::Array{Float64,2}, lam::Float64=0.0;
                         method::AbstractString="DP", w=0.01, f1o::Float64=0.0,
                         f2o::Array{Float64,1}=zeros(length(q2)))
    q1 = q1./norm(q1);
    c1 = srsf_to_f(q1,timet,f1o);
    M, N = size(q2);
    n1 = 1;
    gam = zeros(M, N);
    rotated = false;
    isclosed = false;
    skipm = 0;
    auto = 0;
    for ii in 1:N
        qi = q2[:, ii];
        qi = qi./norm(qi);
        ci = srsf_to_f(qi,timet,f2o[ii]);
        if (method == "DP")
            G = zeros(M);
            T = zeros(M);
            sizei = Cdouble[0];
            ccall((:DynamicProgrammingQ2, libfdasrsf), Void,
              (Ptr{Float64}, Ptr{Float64}, Ptr{Float64}, Ptr{Float64}, Int32,
              Int32, Int32, Ptr{Float64},Ptr{Float64}, Int32, Int32,
              Ptr{Float64}, Ptr{Float64}, Ptr{Float64}, Float64), q1, timet,
              qi, timet, n1, M, M, timet, timet, M, M, G, T, sizei, lam)
            G = G[1:round(Integer,sizei[1])];
            T = T[1:round(Integer,sizei[1])];
            yi = interpolate((T,), G, Gridded(Linear()))
            gam0 = yi[timet];
        elseif (method == "SIMUL")
            s1,s2,g1,g2,ext1,ext2,mpath = simul_align(c1,ci);
            u = linspace(0,1,length(g1));
            tmin = minimum(timet);
            tmax = maximum(timet);
            timet2 = copy(timet);
            timet2 = (timet2-tmin)/(tmax-tmin);
            gam0 = simul_gam(collect(u),g1,g2,timet2,s1,s2,timet2);
        elseif (method == "DP2")
            opt = zeros(M+n1*n1+1);
            swap = false;
            fopts = zeros(5);
            comtime = zeros(5);
            @cpp ccall((:optimum_reparam, libgropt), Void,
                    (Ptr{Float64}, Ptr{Float64}, Int32, Int32, Float64, Bool,
                     Bool, Bool, Int32, Int32, Ptr{Float64}, Bool, Ptr{Float64},
                     Ptr{Float64}), c1, ci, M, n1, 0.0, true, rotated, isclosed,
                     skipm, auto, opt, swap, fopts, comtime)

            gam0 = opt[1:end-2];

            if swap
                gam0 = invertGamma(gam0);
            end

        else
            opt = zeros(M+n1*n1+1);
            swap = false;
            fopts = zeros(5);
            comtime = zeros(5);
            @cpp ccall((:optimum_reparam, libgropt), Void,
                    (Ptr{Float64}, Ptr{Float64}, Int32, Int32, Float64, Bool,
                     Bool, Bool, Int32, Int32, Ptr{Float64}, Bool,
                     Ptr{Float64}, Ptr{Float64}), c1, ci, M, n1, w, false,
                     rotated, isclosed, skipm, auto, opt, swap, fopts, comtime)

            if fopts[1] == 1000
                @cpp ccall((:optimum_reparam, libgropt), Void,
                        (Ptr{Float64}, Ptr{Float64}, Int32, Int32, Float64, Bool,
                         Bool, Bool, Int32, Int32, Ptr{Float64}, Bool,
                         Ptr{Float64}, Ptr{Float64}), c1, ci, M, n1, 0.0, true,
                         rotated, isclosed, skipm, auto, opt, swap, fopts,
                         comtime)
            end

            gam0 = opt[1:end-2];

            if swap
                gam0 = invertGamma(gam0);
            end
        end

        gam[:, ii] = (gam0-gam0[1]) ./ (gam0[end] - gam0[1]);
    end

    return gam
end

function optimum_reparam(q1::Array{Float64,2}, timet::Array{Float64,1},
                         q2::Array{Float64,2}, lam::Float64=0.0;
                         method::AbstractString="DP", w=0.01,
                         f1o::Array{Float64,1}=zeros(length(q1)),
                         f2o::Array{Float64,1}=zeros(length(q2)))
    M, N = size(q1);
    n1 = 1;
    gam = zeros(M, N);
    rotated = false;
    isclosed = false;
    skipm = 0;
    auto = 0;
    for ii in 1:N
        q1i = q1[:, ii] ./ norm(q1[:, ii]);
        q2i = q2[:, ii] ./ norm(q2[:, ii]);
        c1i = srsf_to_f(q1i, timet, f1o[ii]);
        c2i = srsf_to_f(q2i, timet, f2o[ii]);
        if (method == "DP")
            G = zeros(M);
            T = zeros(M);
            sizei = Cdouble[0];
            ccall((:DynamicProgrammingQ2, libfdasrsf), Void,
              (Ptr{Float64}, Ptr{Float64}, Ptr{Float64}, Ptr{Float64}, Int32,
              Int32, Int32, Ptr{Float64},Ptr{Float64}, Int32, Int32,
              Ptr{Float64}, Ptr{Float64}, Ptr{Float64}, Float64), q1i, timet,
              q2i, timet, n1, M, M, timet, timet, M, M, G, T, sizei, lam)
            G = G[1:round(Integer,sizei[1])];
            T = T[1:round(Integer,sizei[1])];
            yi = interpolate((T,), G, Gridded(Linear()))
            gam0 = yi[timet];
            sizei = Cdouble[0];
        elseif (method == "SIMUL")
            s1,s2,g1,g2,ext1,ext2,mpath = simul_align(c1i,c2i);
            u = linspace(0,1,length(g1));
            tmin = minimum(timet);
            tmax = maximum(timet);
            timet2 = copy(timet);
            timet2 = (timet2-tmin)/(tmax-tmin);
            gam0 = simul_gam(collect(u),g1,g2,timet2,s1,s2,timet2);
        elseif (method == "DP2")
            opt = zeros(M+n1*n1+1);
            swap = false;
            fopts = zeros(5);
            comtime = zeros(5);
            @cpp ccall((:optimum_reparam, libgropt), Void,
                    (Ptr{Float64}, Ptr{Float64}, Int32, Int32, Float64, Bool,
                     Bool, Bool, Int32, Int32, Ptr{Float64}, Bool, Ptr{Float64},
                     Ptr{Float64}), c1i, c2i, M, n1, 0.0, true, rotated,
                     isclosed, skipm, auto, opt, swap, fopts, comtime)

            gam0 = opt[1:end-2];

            if swap
                gam0 = invertGamma(gam0);
            end

        else
            opt = zeros(M+n1*n1+1);
            swap = false;
            fopts = zeros(5);
            comtime = zeros(5);
            @cpp ccall((:optimum_reparam, libgropt), Void,
                    (Ptr{Float64}, Ptr{Float64}, Int32, Int32, Float64, Bool,
                     Bool, Bool, Int32, Int32, Ptr{Float64}, Bool, Ptr{Float64},
                     Ptr{Float64}), c1i, c2i, M, n1, w, false, rotated,
                     isclosed, skipm, auto, opt, swap, fopts, comtime)

            if fopts[1] == 1000
                @cpp ccall((:optimum_reparam, libgropt), Void,
                        (Ptr{Float64}, Ptr{Float64}, Int32, Int32, Float64, Bool,
                         Bool, Bool, Int32, Int32, Ptr{Float64}, Bool,
                         Ptr{Float64}, Ptr{Float64}), c11, c2i, M, n1, 0.0,
                         true, rotated, isclosed, skipm, auto, opt, swap, fopts,
                         comtime)
            end

            gam0 = opt[1:end-2];

            if swap
                gam0 = invertGamma(gam0);
            end
        end

        gam[:, ii] = (gam0-gam0[1]) ./ (gam0[end] - gam0[1]);
    end

    return gam
end

"""
Warp srsf by gamma

    warp_q_gamma(time::Vector, q::Vector, gam::Vector)
    :param time: describes time samples
    :param q: describes srsf
    :param gam: describes warping function
"""
function warp_q_gamma(time::Vector, q::Vector, gam::Vector)
    M = length(gam);
    gam_dev = gradient(gam, 1/(M-1));
    tmp = interpolate((time,), q, Gridded(Linear()))
    xout = (time[end] - time[1]) .* gam + time[1];
    q_temp = tmp[xout] .* sqrt(gam_dev);

    return q_temp
end

"""
Warp function by gamma

    warp_f_gamma(time::Vector, f::Vector, gam::Vector)
    :param time: describes time samples
    :param f: describes function
    :param gam: describes warping function
"""
function warp_f_gamma(time::Vector, f::Vector, gam::Vector)
    M = length(gam);
    tmp = interpolate((time,), f, Gridded(Linear()))
    xout = (time[end] - time[1]) .* gam + time[1];
    f_temp = tmp[xout];

    return f_temp
end

"""
Generate random warping functions

    rgam(N, sigma, num)
    :param N: number of time points
    :param sigma: standard deviation across samples
    :param num: number of random warpings
"""
function rgam(N, sigma, num)
    gam = zeros(N, num);

    TT = N-1;
    timet = linspace(0,1,TT);
    mu = sqrt(ones(N-1)*TT/(N-1));
    omega = (2*pi)/TT;
    for k in 1:num
        alpha_i = randn(1)*sigma;
        v = alpha_i .* ones(TT);
        cnt = 1;
        for l in 2:10
            alpha_i = randn(1)*sigma;
            if (mod(l,2) != 0)
                v = v+alpha_i.*sqrt(2).*cos(cnt*omega*timet);
                cnt += 1;
            elseif (mod(l,2) == 0)
                v = v+alpha_i.*sqrt(2).*sin(cnt*omega*timet);
            end
        end

        v = v - (mu * transpose(v)) * mu/TT;
        vn = norm(v)/sqrt(TT);
        psi = cos(vn)*mu + sin(vn) * v/vn;
        gam[2:end, k] = cumsum(psi.*psi)./TT;
        gam[:, k] = (gam[:,k] - gam[1,k]) ./ (gam[end,k] - gam[1,k]);
    end

    return gam
end

"""
Generates random warping functions based on gam

    random_gamma(gam, num)
    :param gam: array (M,N) describing warping functions
    :param num: number of functions to generate
"""
function random_gamma(gam, num)
    mu, gam_mu, psi, vec1 = sqrt_mean(gam);
    K = Base.covm(vec1, mean(vec1,2), 2);

    U, s, V = svd(K);
    n = 5;
    TT = size(vec1, 1) + 1;
    vm = mean(vec1, 2);
    rgam = zeros(TT, num);
    for k in 1:num
        a = randn(n)
        v = zeros(size(vm,1));
        for i in 1:n
            v = v + a[i] * sqrt(s[i]) * U[:, i];
        end

        vn = norm(v) / sqrt(TT);
        psi = cos(vn) .* mu + sin(vn) .* v./vn;
        tmp = zeros(TT);
        tmp[2:TT] = cumsum(psi.*psi)/TT;
        rgam[:, k] = (tmp - tmp[1]) ./ (tmp[end] - tmp[1]);
    end
    return rgam
end

"""
Invert warping function

    invert_gamma(gam)
    :param gam: vector describing warping function
"""
function invert_gamma(gam::Vector)
    N = length(gam);
    x = collect(1:N)/N;
    gamI = approx(gam,x,x);
    gamI = (gamI - gamI[1]) ./ (gamI[end] - gamI[1]);
    return gamI
end

"""
Bayesian qtocurve function

    qtocurve(q, timet=0)
"""
function qtocurve(q, timet=0)
    m = length(q);
    if (timet == 0)
        timet = linspace(0, 1, m+1);
    end
    curve = zeros(m+1);
    for i = 2:(m+1)
        curve[i] = q[i-1]*abs(q[i-1])*(timet[i]-timet[i-1])+curve[i-1];
    end

    return curve
end

"""
Calculate sqrt mean inverse of warping function

    sqrt_mean_inverse(gam)
    :param gam: array (M,N) describing warping functions
"""
function sqrt_mean_inverse(gam::Array)
    eps1 = eps(Float64);
    T1, n = size(gam);
    dt = 1.0/(T1-1);
    psi = Array(Float64,T1-1,n);
    for k = 1:n
        psi[:,k] = sqrt(diff(gam[:,k]) / dt + eps1);
    end

    # Find Direction
    mnpsi = mean(psi, 2);
    d1 = repmat(mnpsi, 1, n);
    d = (psi - d1).^2;
    dqq = sqrt(sum(d,1));
    min_ind = indmin(dqq);
    mu = psi[:, min_ind];
    maxiter = 20;
    tt = 1;
    lvm = zeros(maxiter);
    vec = zeros(T1-1, n);
    for itr in 1:maxiter
        for k in 1:n
            dot1 = trapz(collect(linspace(0,1,T1-1)), mu.*psi[:,k]);
            if dot1 > 1
                dot1 = 1;
            elseif dot1 < (-1)
                dot1 = -1;
            end
            leng = acos(dot1);
            if leng > 0.0001
                vec[:, k] = (leng/sin(leng)) * (psi[:,k] - cos(leng) * mu);
            else
                vec[:, k] = zeros(T1-1);
            end
        end
        vm = mean(vec,2);
        vm1 = vm.*vm;
        lvm[itr] = sqrt(sum(vm1) * dt);
        if lvm[itr] == 0
            break
        end
        mu = cos(tt * lvm[itr]) * mu + (sin(tt * lvm[itr])/lvm[itr]) * vm;
        if (lvm[itr] < 1e-6) | (itr >= maxiter)
            break
        end
    end
    tmp = mu .* mu;
    gam_mu = zeros(T1)
    gam_mu[2:end] = cumsum(tmp)./T1;
    gam_mu = (gam_mu - minimum(gam_mu)) ./ (maximum(gam_mu) - minimum(gam_mu));
    gamI = invert_gamma(gam_mu);
    return gamI
end

"""
Calculate zero crossing of optimal warping function

    zero_crossing(Y,q,bt,timet,y_max,y_min,gmax,gmin)
"""
function zero_crossing(Y, q, bt, timet, y_max, y_min, gmax, gmin)
    max_itr = 100;
    a = zeros(max_itr);
    a[1] = 1;
    f = zeros(max_itr);
    f[1] = y_max - Y;
    f[2] = y_min - Y;
    mrp = f[1];
    mrn = f[2];
    mrp_ind = 1;  # most recent positive index
    mrn_ind = 2;  # most recent negative index
    out_ii = 1;

    for ii in 3:max_itr
        x1 = a[mrp_ind];
        x2 = a[mrn_ind];
        y1 = mrp;
        y2 = mrn;
        a[ii] = (x1*y2 - x2*y1) / (y2-y1);

        gam_m = a[ii] *gmax + (1-a[ii]) * gmin;
        qtmp = warp_q_gamma(timet, q, gam_m);
        f[ii] = trapz(timet, qtmp.*bt) - Y;

        if abs(f[ii]) < 1e-5
            break
            out_ii = ii;
        elseif f[ii] > 0
            mrp = f[ii];
            mrp_ind = ii;
        else
            mrn = f[ii];
            mrn_ind = ii;
        end
        out_ii = ii;
    end

    gamma = a[out_ii] * gmax + (1-a[out_ii]) * gmin;

    return gamma

end

"""
Calculate sqrt mean of warping functions

    sqrt_mean(gam)
    :param gam: array (M,N) describing warping functions
"""
function sqrt_mean(gam::Array)
    eps1 = eps(Float64);
    TT, n = size(gam);
    psi = Array(Float64,TT-1,n);
    for k = 1:n
        psi[:,k] = sqrt(diff(gam[:,k]) * TT + eps1);
    end

    # Find Direction
    mnpsi = mean(psi, 2);
    w = mean(psi, 2);
    mu = w./sqrt(sum(w.^2/(TT-1)));
    maxiter = 500;
    tt = 1;
    lvm = zeros(maxiter);
    vec1 = zeros(TT-1, n);
    for itr in 1:maxiter
        for k in 1:n
            dot1 = sum(mu.*psi[:,k]./(TT-1));
            if dot1 > 1
                dot1 = 1;
            elseif dot1 < (-1)
                dot1 = -1;
            end
            leng = dot1
            if leng > 0.0001
                vec1[:, k] = (leng/sin(leng)) * (psi[:,k] - cos(leng) * mu);
            else
                vec1[:, k] = zeros(TT-1);
            end
        end
        vm = mean(vec1,2);
        vm1 = vm.*vm;
        lvm[itr] = Enorm(vm[:])/sqrt(TT);
        if lvm[itr] == 0
            break
        end
        mu = cos(tt * lvm[itr]) * mu + (sin(tt * lvm[itr])/lvm[itr]) * vm;
        if (lvm[itr] < 1e-6) | (itr >= maxiter)
            break
        end
    end
    tmp = mu .* mu;
    gam_mu = zeros(TT)
    gam_mu[2:end] = cumsum(tmp)./TT;
    gam_mu = (gam_mu - minimum(gam_mu)) ./ (maximum(gam_mu) - minimum(gam_mu));
    return mu, gam_mu, psi, vec1
end

"""
Find Karcher inverse of warping functions

    findkarcherinv(warps, times, roundi=false)
"""
function findkarcherinv(warps, times, roundi=false)
    m, n = size(warps);
    psi_m = zeros(m-1, n);
    for j = 1:n
        psi_m[:, j] = sqrt(diff(warps[:, j])./times);
    end
    w = mean(psi_m, 2);
    mupsi = w./sqrt(sum(w.^2/(m-1)));
    v_m = zeros(m-1, n);
    mupsi_update = zeros(m);
    check = 1.;
    while (check > 0.01)
        for i = 1:n
            theta = acos(sum(mupsi.*psi_m[:, i]./(m-1)));
            v_m[:, i] = theta/sin(theta) * (psi_m[:, i]-cos(theta)*mupsi);
        end
        vbar = vec(mean(v_m, 2));
        check = Enorm(vbar)/sqrt(m-1);
        if (check > 0)
            mupsi_update = cos(0.01*check)*mupsi+sin(0.01*check)*vbar/check;
        else
            mupsi_update = cos(0.01*check)*mupsi;
        end
    end
    karcher_s = 1 + [0; cumsum(mupsi_update.^2)*times]
    f_i = interpolate((vec(karcher_s),), float(collect(1:times:(m-1)*times+1)),
                      Gridded(Linear()))
    if (roundi)
        invidy = [round(f_i[1:((m-1)*times)]); (m-1)*times+1];
    else
        invidy = [f_i[1:((m-1)*times)]; (m-1)*times+1];
    end
    revscalevec = sqrt(diff(invidy));

    return invidy, revscalevec
end

"""
Simultaneous alignment between two functions

    simul_align(f1::Vector, f2::Vector)
"""
function simul_align(f1::Vector,f2::Vector)
    # parameterize by arc-length
    s1 = arclength(f1);
    s2 = arclength(f2);

    len1 = maximum(s1);
    len2 = maximum(s2);

    f1 /= len1;
    s1 /= len1;
    f2 /= len2;
    s2 /= len2;

    # get srvf (should be +/-1)
    q1 = diff(f1)./diff(s1);
    q1[diff(s1).==0] = 0;
    q2 = diff(f2)./diff(s2);
    q2[diff(s2).==0] = 0;

    # get extreme points
    ext1, d1 = extrema_1s(s1,q1);
    ext2, d2 = extrema_1s(s2,q2);

    D, P, mpath = match_ext(s1,ext1,d1,s2,ext2,d2);

    te1 = s1[ext1];
    te2 = s2[ext2];

    g1, g2 = simul_reparam(te1,te2,mpath);

    return s1,s2,g1,g2,ext1,ext2,mpath
end

"""
Calculate arc-length parametrization of function

    arclength(f::Vector)
"""
function arclength(f::Vector)
    t1 = zeros(length(f));
    t1[2:end] = abs(diff(f));
    t1 = cumsum(t1);

    return t1
end

"""
Find location of change of sign of srsf that is arclength parameterized

    extrema_1s(t::Vector, q::Vector)
"""
function extrema_1s(t::Vector, q::Vector)
    q = round(q);

    if q[1] !=0
        d = -q[1];
    else
        d = q[q.!=0];
        d = d[1];
    end

    ext = find(diff(q))+1;

    ext2 = zeros(Integer, length(ext)+2);
    ext2[1] = 1;
    ext2[2:end-1] = round(ext);
    ext2[end] = length(t);

    return ext2, d
end

"""
Find matching between two extremas

    match_ext(t1, ext1, d1, t2, ext2, d2)
"""
function match_ext(t1,ext1::Array{Integer,1},d1,t2,ext2::Array{Integer,1},d2)
    te1 = t1[ext1];
    te2 = t2[ext2];

    # We'll pad each sequence to start on a 'peak' and end on a 'valley'
    pad1 = zeros(Integer, 2);
    pad2 = zeros(Integer, 2);
    if d1==-1
        te1a = zeros(length(te1)+1);
        te1a[2:end] = te1
        te1a[1] = te1a[2];
        te1 = copy(te1a);
        pad1[1] = 1;
    end

    if mod(length(te1),2)==1
        te1a = zeros(length(te1)+1);
        te1a[1:end-1] = te1;
        te1a[end] = te1[end];
        te1 = copy(te1a);
        pad1[2] = 1;
    end

    if d2 == -1
        te2a = zeros(length(te2)+1);
        te2a[2:end] = te2;
        te2a[1] = te2a[2];
        te2 = copy(te2a);
        pad2[1] = 1;
    end

    if mod(length(te2),2) == 1
        te2a = zeros(length(te2)+1);
        te2a[1:end-1] = te2;
        te2a[end] = te2[end];
        te2 = copy(te2a);
        pad2[2] = 1;
    end

    n1 = length(te1);
    n2 = length(te2);

    # initialize weight and path matrices
    D = zeros(n1,n2);
    P = zeros(n1,n2,2);

    for i = 1:n1
        for j = 1:n2
            if mod(i+j,2)==0
                for ib = (i-1):-2:(1+mod(i,2))
                    for jb = (j-1):-2:(1+mod(j,2))
                        icurr = ib+1:2:i;
                        jcurr = jb+1:2:j;
                        W = sqrt(sum(te1[icurr]-te1[icurr-1])) *
                            sqrt(sum(te2[jcurr]-te2[jcurr-1]));
                        Dcurr = D[ib,jb] + W;
                        if Dcurr > D[i,j]
                            D[i,j] = Dcurr;
                            P[i,j,:] = [ib,jb];
                        end
                    end
                end
            end
        end
    end

    D = D[1+pad1[1]:end-pad1[2],1+pad2[1]:end-pad2[2]];
    P = P[1+pad1[1]:end-pad1[2],1+pad2[1]:end-pad2[2],:];
    P[:,:,1] -= pad1[1];
    P[:,:,2] -= pad2[1];

    # Retrieve Best Path
    if pad1[2] == pad2[2]
        mpath = collect(size(D));
    elseif D[end-1,end] > D[end,end-1]
        mpath = collect(size(D)) - [1,0];
    else
        mpath = collect(size(D)) - [0,1];
    end
    mpath = round(Integer, mpath);
    P = round(Integer, P);
    prev_vert = reshape(P[mpath[1],mpath[2],:],1,2);

    mpath = mpath';

    while any(prev_vert.>0)
        mpath = [prev_vert;mpath];
        prev_vert = reshape(P[mpath[1,1],mpath[1,2],:],1,2);
    end

    return D, P, mpath
end

"""
Find simultaneous re-parametrization

    simul_reparam(te1, te2, mpath)
"""
function simul_reparam(te1, te2, mpath)
    g1 = [0.];
    g2 = [0.];

    if mpath[1,2] == 2
        g1 = [g1;0.];
        g2 = [g2; te2[2]];
    elseif mpath[1,1] == 2
        g1 = [g1;te1[2]];
        g2 = [g2; 0.];
    end

    m = size(mpath,1);
    for i = 1:m-1
        gg1, gg2 = simul_reparam_segment(vec(mpath[i,:]),vec(mpath[i+1,:]),
                                         te1,te2);

        g1 = [g1;gg1];
        g2 = [g2;gg2];
    end

    n1 = length(te1);
    n2 = length(te2);
    if (mpath[end,1] == n1-1) || (mpath[end,2] == n2-1)
        g1 = [g1; 1.];
        g2 = [g2; 1.];
    end

    return g1, g2
end

function simul_reparam_segment(src, tgt, te1, te2)
    i1 = src[1]+1:2:tgt[1];
    i2 = src[2]+1:2:tgt[2];

    v1 = sum(te1[i1]-te1[i1-1]);
    v2 = sum(te2[i2]-te2[i2-1]);
    R = v2/v1;

    a1 = src[1];
    a2 = src[2];
    t1 = te1[a1];
    t2 = te2[a2];
    u1 = 0.;
    u2 = 0.;

    gg1 = Array(Float64,0);
    gg2 = Array(Float64,0);

    while (a1<tgt[1]) && (a2<tgt[2])
        if a1==tgt[1]-1 && a2==tgt[2]-1
            a1 = tgt[1];
            a2 = tgt[2];
            gg1 = [gg1; te1[a1]];
            gg2 = [gg2; te2[a2]];
        else
            p1 = (u1+te1[a1+1]-t1)/v1;
            p2 = (u2+te2[a2+1]-t2)/v2;
            if p1<p2
                lam = t2 + R*(te1[a1+1]-t1);
                gg1 = [gg1; te1[a1+1]; te1[a1+2]];
                gg2 = [gg2; lam; lam];
                u1 = u1 + te1[a1+1] - t1;
                u2 = u2 + lam - t2;
                t1 = te1[a1+2];
                t2 = copy(lam);
                a1 += 2;
            else
                lam = t1 + (1./R)*(te2[a2+1]-t2);
                gg1 = [gg1; lam; lam];
                gg2 = [gg2; te2[a2+1]; te2[a2+2]];
                u1 = u1 + lam - t1;
                u2 = u2 + te2[a2+1] - t2;
                t1 = copy(lam);
                t2 = te2[a2+2];
                a2 += 2;
            end
        end
    end

    return gg1, gg2
end

"""
Calculate warping from q2 to q2 from simultaneous warping

    simul_gam(u, g1,g2,t,s1,s2,tt)
"""
function simul_gam(u::Array{Float64,1},g1,g2,t::Array{Float64,1},s1,s2,
                   tt::Array{Float64,1})
    gs1 = interp1_flat(u,g1,tt);
    gs2 = interp1_flat(u,g2,tt);

    gt1 = interp1_flat(s1,t,gs1);
    gt2 = interp1_flat(s2,t,gs2);

    gam = interp1_flat(gt1,gt2,tt);

    return gam
end

function old_dp(q1::Vector, q2::Vector,lam)
    N = length(q1);
    M = 5*N;
    spl = Spline1D(collect(1.:N)/N, q2);
    q2L = spl(collect(1.:M)/M);

    Nbrs = [1 1; 1 2; 2 1; 2 3; 3 2; 1 3; 3 1; 1 4; 3 4; 4 3; 4 1; 1 5; 2 5; 3 5; 4 5; 5 4; 5 3; 5 2; 5 1];

    E = zeros(N,N);
    E[1,:] = Inf;
    E[:,1] = Inf;
    E[1,1] = 0.0;
    Path = zeros(N,N,2);
    for i = 2:N
        for j = 2:N
            CandE = 100000*ones(size(Nbrs,1));
            for Num = 1:size(Nbrs,1)
                k = i - Nbrs[Num,1];
                l = j - Nbrs[Num,2];
                if (k>0 && l>0)
                    CandE[Num] = E[k,l] + cost_fn(q1,q2,q2L,k,l,i,j,N,lam);
                end
                E[i,j], idx = findmin(CandE);
                Path[i,j,1] = i - Nbrs[idx,1];
                Path[i,j,2] = j - Nbrs[idx,2];
            end
        end
    end

    x = zeros(N);
    y = zeros(N);
    x[1] = N;
    y[1] = N;
    cnt = 1;
    while (x[cnt]>1)
        yi = round(Integer,y[cnt]);
        xi = round(Integer,x[cnt]);
        y[cnt+1] = Path[yi,xi,1];
        x[cnt+1] = Path[yi,xi,2];
        cnt += 1;
    end
    x = x[1:cnt];
    y = y[1:cnt];
    idx = sortperm(x);
    x = x[idx];
    y = y[idx];
    yy = zeros(N);
    for i = 1:N
        F = abs(i-x);
        idx = indmin(F);
        if x[idx] == i
            yy[i] = y[idx];
        else
            if x[idx] > i
                a = x[idx] - i;
                b = i - x[idx-1];
                yy[i] = (a*y[idx-1] + b*y[idx])/(a+b);
            else
                a = i - x[idx];
                b = x[idx+1] - i;
                yy[i] = (a*y[idx+1] + b*y[idx])/(a+b);
            end
        end
    end
    gam = yy/N;

    return gam
end

function cost_fn(q1,q2,q2L,k,l,i,j,N,lam)
    M = length(q2L);
    x = collect(k:1:i);
    m = (j-l)/(i-k);
    y = (x-k)*m + l;
    idx = round(Integer, y*M/N);
    vec = sqrt(m)*q2L[idx];
    E = norm(q1[x]-vec)^2/N;

    return E
end


text:
"""
    RegularInTime{
        T, Tts<:AbstactVector{<:Real}, Tvs<:AbstractVector{<:AbstractVector{T}},
    } <: AbstractVector{T}

Represents data that has multiple observations at each of a given collection of time slices.
"""
struct RegularInTime{
    T, Tts<:AbstractVector{<:Real}, Tvs<:AbstractVector{<:AbstractVector{T}},
} <: AbstractVector{T}
    ts::Tts
    vs::Tvs
end

Base.size(x::RegularInTime) = (sum(length, x.vs), )

function Base.collect(x::RegularInTime)
    time_inputs = vcat([fill(t, length(x)) for (t, x) in zip(x.ts, x.vs)]...)
    space_inputs = vcat(x.vs...)
    return [(x, t) for (x, t) in zip(space_inputs, time_inputs)]
end

Base.show(io::IO, x::RegularInTime) = Base.show(io::IO, collect(x))

"""
    match_to(y::AbstractVector, x::RegularInTime)

Convert `y` into a vector length `length(x.ts)`, each element containing the number of
elements in the corresponding element of `x.vs`.
"""
function match_to(y::AbstractVector{T}, x::RegularInTime) where {T}
    y_vec = Vector{Vector{T}}(undef, length(x.ts))
    pos = 1
    for t in eachindex(x.ts)
        Nt = length(x.vs[t])
        y_vec[t] = y[pos:pos + Nt - 1]
        pos += Nt
    end
    return y_vec
end

get_time(x::RegularInTime) = x.ts


text:
using ADSeismic
using ADCME
using PyPlot
using PyCall
using DelimitedFiles
using MAT
using Statistics
np = pyimport("numpy")
matplotlib.use("Agg")
use_gpu()

################### Generate synthetic data #####################
output_dir = "data/acoustic"
if !ispath(output_dir)
  mkpath(output_dir)
end
figure_dir = "figure/acoustic/rupture-time/"
if !ispath(figure_dir)
  mkpath(figure_dir)
end
result_dir = "result/acoustic/rupture-time/"
if !ispath(result_dir)
  mkpath(result_dir)
end
# if isfile(joinpath(result_dir, "loss.txt"))
#   rm(joinpath(result_dir, "loss.txt"))
# end

################### Generate gaussian source #####################
# reset_default_graph()

ap_sim = load_acoustic_model("models/layer-model-rupture.mat"; IT_DISPLAY=0)
rcv_sim = load_acoustic_receiver("models/layer-model-rupture.mat")

ix = matread("models/layer-model-rupture.mat")["source"][1]["ix"][:]
iy = matread("models/layer-model-rupture.mat")["source"][1]["iy"][:]
s =  matread("models/layer-model-rupture.mat")["source"][1]["shift"][:] .* ap_sim.param.DELTAT
amp = matread("models/layer-model-rupture.mat")["source"][1]["amp"][:]
a = 1/pi

rs = []
for i = 1:length(s)
  gauss = Gauss(ap_sim.param, a, s[i], amp[i])
  push!(rs, gauss)
end
rs = hcat(rs...)
src = [AcousticSource(ix, iy, rs)]

sess = Session(); init(sess)

srcv = run(sess, src[1].srcv)
vmax = maximum(srcv)
vmin = -vmax
t = collect(0:ap_sim.param.NSTEP)* ap_sim.param.DELTAT
n = collect(1:length(src[1].srci)).-0.5
n, t = np.meshgrid(n, t)
close("all")
pcolormesh(n, t, srcv, vmax=vmax, vmin=vmin, cmap="seismic")
xlabel("Location index")
ylabel("Time (s)")
savefig(joinpath(figure_dir, "ground_truth.png"))
writedlm(joinpath(result_dir, "ground_truth.txt"), srcv)

################### Generate synthetic data #####################
# reset_default_graph()

ap_sim = load_acoustic_model("models/layer-model-rupture.mat"; IT_DISPLAY=0)
# src = load_acoustic_source("models/layer-model-rupture.mat")
rcv_sim = load_acoustic_receiver("models/layer-model-rupture.mat")
Rs_ = compute_forward_GPU(ap_sim, src, rcv_sim)

sess = Session(); init(sess)
Rs = run(sess, Rs_)

for i = 1:length(src)
    writedlm(joinpath(output_dir, "rupture-time-r$i.txt"), Rs[i])
end

u = run(sess, ap_sim(src[1]).u)
visualize_file(u, ap_sim.param, dirs=figure_dir)

################### Inversion using Automatic Differentiation #####################
reset_default_graph()

ap_sim = load_acoustic_model("models/layer-model-rupture.mat"; IT_DISPLAY=0)
rcv_sim = load_acoustic_receiver("models/layer-model-rupture.mat")

ix = matread("models/layer-model-rupture.mat")["rupture"]["ix"][:]
iy = matread("models/layer-model-rupture.mat")["rupture"]["iy"][:]
nsrc = length(ix)

# create variables
s_ = Variable(-1.5*ones(nsrc))
s = ap_sim.param.NSTEP*ap_sim.param.DELTAT*sigmoid(s_)
amp_ = Variable(0.1*ones(nsrc))
amp = 1e7*sigmoid(amp_)
if ! @isdefined a
  a = 1/pi
end

rs = []
for i = 1:length(s)
  gauss = Gauss(ap_sim.param, a, s[i], amp[i])
  push!(rs, gauss)
end
rs = hcat(rs...)
src_ = [AcousticSource(ix, iy, rs)]

Rs = Array{Array{Float64,2}}(undef, length(src_))
for i = 1:length(src_)
    Rs[i] = readdlm(joinpath(output_dir, "rupture-time-r$i.txt"))
end

sess = Session(); init(sess)

vars = [s_, amp_]
losses, gs = compute_loss_and_grads_GPU(ap_sim, src_, rcv_sim, Rs, vars)
g = Array{PyObject}(undef, length(gs[1]))
for i = 1:length(gs[1])
  g[i] = sum([pp[i] for pp in gs])
end

loss = sum(losses)
sess = Session(); init(sess)

function sigmoid_(x)
  return 1 / (1 + exp(-x))
end

function callback(v, iter, loss)
    if iter%5==0
      # @show sigmoid_.(v[2])*700.0
      # @show sigmoid_.(v[3])*1e7
      close("all")
      t = collect(0:ap_sim.param.NSTEP)* ap_sim.param.DELTAT
      n = collect(1:length(ix)).-0.5
      n, t = np.meshgrid(n, t)
      pcolormesh(n, t, v[1], vmax=vmax, vmin=vmin, cmap="seismic")
      xlabel("Location index")
      ylabel("Time (s)")
      # if iter>=0
      #   title("Iteration = $iter")
      # end
      savefig(joinpath(figure_dir, "inv_$(lpad(iter,4,"0")).png"))
      writedlm(joinpath(result_dir, "inv_$(lpad(iter,5,"0")).txt"), v[1])
      open(joinpath(result_dir, "loss.txt"), "a") do io
        writedlm(io, loss)
      end
    else
      return
    end
end

vars_ = [src_[1].srcv, vars...]
# callback(run(sess, vars_), 0, run(sess, loss))
LBFGS!(sess, loss, vars=vars_, callback=callback)


text:
# ## Non-zero inflated, continuous. e.g. LogNormal
# function expert_tn_pos(d::e, tl::Real, yl::Real, yu::Real, tu::Real) where {e<:NonZIContinuousExpert}

#     expert_tn = (tl == tu) ? logpdf.(d, tl) : logcdf.(d, tu) + log1mexp.(logcdf.(d, tl) - logcdf.(d, tu))
#     expert_tn = (tu == 0.) ? -Inf : expert_tn

#     return expert_tn
# end

# ## Zero inflated, continuous. e.g. ZILogNormal
# function expert_tn_pos(d::e, tl::Real, yl::Real, yu::Real, tu::Real) where {e<:ZIContinuousExpert}

#     expert_tn = (tl == tu) ? logpdf.(d, tl) : logcdf.(d, tu) + log1mexp.(logcdf.(d, tl) - logcdf.(d, tu))
#     expert_tn = (tu == 0.) ? -Inf : expert_tn

#     return expert_tn
# end

# ## Non-zero inflated, discrete. e.g. Poisson
# function expert_tn_pos(d::e, tl::Real, yl::Real, yu::Real, tu::Real) where {e<:NonZIDiscreteExpert}

#     expert_tn = (tl == tu) ? logpdf.(d, tl) : logcdf.(d, tu) + log1mexp.(logcdf.(d, ceil.(tl) .- 1) - logcdf.(d, tu))

#     return expert_tn
# end

# ## Zero inflated, discrete. e.g. ZIPoisson
# function expert_tn_pos(d::e, tl::Real, yl::Real, yu::Real, tu::Real) where {e<:ZIDiscreteExpert}

#     expert_tn = (tl == tu) ? logpdf.(d, tl) : logcdf.(d, tu) + log1mexp.(logcdf.(d, ceil.(tl) .- 1) - logcdf.(d, tu))

#     return expert_tn
# end

max_stars_repo_name: sparktseung/JuliaLRMoE

max_issues_repo_name: sparktseung/JuliaLRMoE

max_forks_repo_name: sparktseung/JuliaLRMoE


text:
## formatting conveniences

"markdown can leave wrapping p's"
function strip_p(txt)
    if occursin(r"^
", txt)
        txt = replace(replace(txt, r"^
" => ""), r"
$" => "")
    end
    txt
end

function md(x)
    out = sprint(io ->  show(io, "text/html", Markdown.parse(string(x))))
    strip_p(out)
end

markdown_to_latex(x) = sprint(io -> show(io, "text/latex", Markdown.parse(x)))

"""
Hide output and input, but execute cell.

Examples
```
2 + 2
Invisible()
```
"""
mutable struct Invisible
end

"""
Show output as HTML

Examples
```
HTMLoutput("<em>em</em>")
```

"""
mutable struct HTMLoutput
    x
end
Base.show(io::IO, ::MIME"text/plain", x::HTMLoutput) = print(io, """
$(x.x)
""")
Base.show(io::IO, ::MIME"text/html", x::HTMLoutput) = print(io, x.x)
Base.show(io::IO, ::MIME"text/latex", x::HTMLoutput) = println(io, "...unable to display raw html...")

"""
Show as input, but do not execute.
Examples:
```
Verbatim("This will print, but not be executed")
```
"""
mutable struct Verbatim
    x
end
Base.show(io::IO, ::MIME"text/plain", x::Verbatim) = print(io, """
$(x.x)
""")
Base.show(io::IO, ::MIME"text/html", x::Verbatim) = print(io, x.x)
Base.show(io::IO, ::MIME"text/latex", x::Verbatim) = print(io, "\verb@$(markdown_to_latex(x.x))@")

"""
Hide input, but show output

Examples
```
x = 2 + 2
Outputonly(x)
```
"""
mutable struct Outputonly
    x
end

## Bootstrap things
abstract type Bootstrap end
Base.show(io::IO, ::MIME"text/html", x::Bootstrap) = print(io, """$(x.x)""")
Base.show(io::IO, ::MIME"text/latex", x::Bootstrap) = print(io, """XXX BOOTSTRAP $(markdown_to_latex(x.x))""")

mutable struct Alert <: Bootstrap
    x
    d::Dict
end

### An alert
function alert(txt; kwargs...)
    d = Dict()
    for (k,v) in kwargs
        d[k] = v
    end
    Alert(txt, d)
end

warning(txt; kwargs...) = alert(txt; class="warning", kwargs...)
note(txt; kwargs...) = alert(txt; class="info", kwargs...)

function Base.show(io::IO, ::MIME"text/html", x::Alert)
    cls = haskey(x.d,:class) ? x.d[:class] : "success"
    txt = sprint(io -> show(io, "text/html", Markdown.parse(x.x)))
    tpl = """

$txt

"""

    print(io, tpl)
end

function Base.show(io::IO, ::MIME"text/latex", x::Alert)
    println(io, """
\\begin{mdframed}
$(markdown_to_latex(x.x))
\\end{mdframed}
""")
end

mutable struct Example <: Bootstrap
    x
    d::Dict
end

## use nm="name" to pass along name
function example(txt; kwargs...)
 d = Dict()
    for (k,v) in kwargs
        d[k] = v
    end
    Example(txt, d)
end

function Base.show(io::IO, ::MIME"text/html", x::Example)
    nm = haskey(x.d,:nm) ? " $(x.d[:nm])" : ""
    txt = sprint(io -> show(io, "text/html", Markdown.parse(x.x)))
    tpl = """

example:$nm$txt

"""

    print(io, tpl)
end

function Base.show(io::IO, ::MIME"text/latex", x::Example)
    println(io, """Example $(markdown_to_latex(x.x))""")
end

mutable struct Popup <: Bootstrap
    x
    title
    icon
    label
end

"""

Create a button to toggle the display of more detail.

Can modify text, title, icon and label (for the button)

The text, title, and label can use Markdown.

LaTeX markup does not work, as MathJax rendering is not supported in the popup.

"""
popup(x; title=" ", icon="share-alt", label=" ") = Popup(x, title, icon, label)

popup_html_tpl=mt"""
<button type="button" class="btn btn-sm" aria-label="Left Align"\
data-toggle="popover"\
title='{{{title}}}'\
data-html=true\
data-content='{{{body}}}'\
>\
{{#button_label}} {{{button_label}}}{{/button_label}}\
</button>\
"""

function Base.show(io::IO, ::MIME"text/html", x::Popup)
    d = Dict()
    d["title"] = sprint(io -> show(io, "text/html", Markdown.parse(x.title)))
    d["icon"] = x.icon
    label = sprint(io -> show(io, "text/html", Markdown.parse(x.label)))
    d["button_label"] = strip_p(label)
    d["body"] = sprint(io -> show(io, "text/html", Markdown.parse(x.x)))
    println(d)
    Mustache.render(io, popup_html_tpl, d)
end

function Base.show(io::IO, ::MIME"text/latex", x::Popup)
    println(io, """
\\begin{quotation}
$(markdown_to_latex(x.x))
\\end{quotation}
""")
end

"""

Way to convert rectangular gird of values into a table

"""
mutable struct Table <: Bootstrap
    x
end
table(x) = Table(x)

table_html_tpl=mt"""

{{{:nms}}}
{{{:body}}}

"""

function Base.show(io::IO, ::MIME"text/html", x::Table)
    vals = Base.invokelatest(names, x.x)
    d = Dict()
    d[:nms] = "
$(join(map(string, vals), "	"))
\n"
    bdy = ""
    m,n = Base.invokelatest(size, x.x)
    for i in 1:m
        bdy = bdy * "
"
        for j in 1:n
            val = Base.invokelatest(getindex, x.x, i, j)
            if ismissing(val)
                val = "."
            end
            bdy = bdy * "$(md(val))	"
        end
        bdy = bdy * "
\n"
    end
    d[:body] = bdy
    Mustache.render(io, table_html_tpl, d)
end

function df_to_table(df, label="label", caption="caption")
    nc = size(df, 2)
    perc = string(round(1/nc, digits=2))
    fmt = "l" * repeat("p{$perc\\textwidth}", nc-1)
    header = join(string.(names(df)), " & ")
    row = join(["{{{:$x}}}" for x in map(string, names(df))], " & ")

tpl="""
\\begin{table}[!ht]
  \\centering
  \\begin{tabular}{$fmt}
  $header\\\\
  \\midrule\\\\
{{#:DF}}    $row\\\\
{{/:DF}}
  \\bottomrule
  \\end{tabular}
  \\label{tab:$label}

\\end{table}
"""

    Mustache.render(tpl, DF=df)
end

function Base.show(io::IO, ::MIME"text/latex", x::Table)
    d = markdown_to_latex.(x.x)
    println(io, df_to_table(d))
end

mutable struct NamedTable <: Bootstrap
data
rownames
colnames
end

function Base.show(io::IO, ::MIME"text/html", x::NamedTable)
    vals = x.data
    cnames = x.colnames
    rnames = x.rownames
    d = Dict()
    d[:nms] = "
	$(join(map(string, cnames), "	"))
\n"
    m,n = Base.invokelatest(size, x.data)
    buf = IOBuffer()
    for i in 1:m
        print(buf, "
", rnames[i],"	")
        for j in 1:n
            val = Base.invokelatest(getindex, x.data, i, j)
            print(buf, "", md(val), "	")
        end
        println(buf, "
")
    end
    d[:body] = String(take!(buf))
    Mustache.render(io, table_html_tpl, d)
end

function Base.show(io::IO, ::MIME"text/latex", x::NamedTable)
    d = markdown_to_latex.(x.x)
    println(io, df_to_table(d))
end


text:
# Data structure and methods for myosin motors

"Mutable data structure for a myosin motor"
mutable struct Myosin_Motor
    index::Int # Integer motor index
    f1::Actin_Filament # 1st actin filament
    f2::Actin_Filament # 2nd actin filament
    t1::Vector{Int} # 1st filament translation
    t2::Vector{Int} # 2nd filament translation
    to_remove::Bool # Binary variable: true if motor is to be removed (filament turnover)
    "Inner constructor to generate new myosin motors and update State"
    function Myosin_Motor(s::State, l::Cross_Link, ind::Int, Lxx, Lxy, Lyx, Lyy)
        push!(s.mp, get_motor_relative_pos_filament(s, l, Lxx, Lxy, Lyx, Lyy)); # Write relative positions to State
        return new(ind, l.f1, l.f2, l.t1, l.t2, false) # Generate motor
    end
end

"Convert relative position along segments to relative position along filaments"
function get_motor_relative_pos_filament(s::State, l::Cross_Link, Lxx, Lxy, Lyx, Lyy)
    # Compute dimensional segment lengths
    Ls1 = get_segment_lengths(l.f1, s, Lxx, Lxy, Lyx, Lyy); # Filament 1
    Ls2 = get_segment_lengths(l.f2, s, Lxx, Lxy, Lyx, Lyy); # Filament 2
    # Compute relative positions
    mp = Vector{}(undef, 2); # Pre-allocate vector of relative positions
    mp[1] = (sum(Ls1[1:l.seg1_index]) - (1-l.relative_pos1)*Ls1[l.seg1_index])/sum(Ls1);
    mp[2] = (sum(Ls2[1:l.seg2_index]) - (1-l.relative_pos2)*Ls2[l.seg2_index])/sum(Ls2);
    return mp
end

"Convert relative position along filaments to relative position along segments"
function get_motor_relative_pos_segment(m::Myosin_Motor, s::State{T}, Lxx, Lxy, Lyx, Lyy) where {T}
    # Must be dimensional for non-square domains
    # 1. Compute filament lengths
    L1 = zero(T); L2 = zero(T); # Pre-allocate filament lengths
    for seg in m.f1.segments
        L1 += get_segment_length(m.f1, s, seg, Lxx, Lxy, Lyx, Lyy)
    end
    for seg in m.f1.segments
        L2 += get_segment_length(m.f2, s, seg, Lxx, Lxy, Lyx, Lyy)
    end
    # 2. Determine dimensional motor positions
    m1 = s.mp[m.index][1]*L1; m2 = s.mp[m.index][2]*L2;
    # 3. Determine index of relevant segments
    L1_cumulative = zero(T); L2_cumulative = zero(T); # Pre-allocate cumulative lengths
    seg1::Int = length(m.f1.segments); seg2::Int = length(m.f2.segments); # Pre-allocate
    for i = 1:length(m.f1.segments)
        L1_cumulative += get_segment_length(m.f1, s, m.f1.segments[i], Lxx, Lxy, Lyx, Lyy);
        if m1 <= L1_cumulative
            seg1 = i; break
        end
    end
    for i = 1:length(m.f2.segments)
        L2_cumulative += get_segment_length(m.f2, s, m.f2.segments[i], Lxx, Lxy, Lyx, Lyy);
        if m2 <= L2_cumulative
            seg2 = i; break
        end
    end
    # 4. Determine relative position along segment
    L1_minus = L1_cumulative - get_segment_length(m.f1, s, m.f1.segments[seg1], Lxx, Lxy, Lyx, Lyy);
    L2_minus = L2_cumulative - get_segment_length(m.f2, s, m.f2.segments[seg2], Lxx, Lxy, Lyx, Lyy);
    rel_mp1 = (m1 - L1_minus)/(L1_cumulative - L1_minus);
    rel_mp2 = (m2 - L2_minus)/(L2_cumulative - L2_minus);
    return seg1, seg2, rel_mp1, rel_mp2
end

"Obtain dimensional, translated motor position"
function get_motor_pos(m::Myosin_Motor, s::State{T}, Lxx, Lxy, Lyx, Lyy) where {T}
    # Find relative position along segment
    seg1, seg2, relative_pos1, relative_pos2 = get_motor_relative_pos_segment(m, s, Lxx, Lxy, Lyx, Lyy)
    # Obtain segment nodes position
    m1x, m1y, p1x, p1y = get_segment_nodes(m.f1, m.f1.segments[seg1], s);
    m2x, m2y, p2x, p2y = get_segment_nodes(m.f2, m.f2.segments[seg2], s);
    # Compute motor positions
    x1 = m1x + relative_pos1*(p1x - m1x);
    y1 = m1y + relative_pos1*(p1y - m1y);
    x2 = m2x + relative_pos2*(p2x - m2x);
    y2 = m2y + relative_pos2*(p2y - m2y);
    return x1, y1, x2, y2
end


text:
using SearchLight

SearchLight.Configuration.load()
eval(Meta.parse("using SearchLight$(SearchLight.config.db_config_settings["adapter"])"))
SearchLight.connect()


text:
#crash.jl
#a separate file for all the crashing stuff

function cross2d(x::Array{Float64,1},y::Array{Float64,1})
	@assert length(x) == length(y)
	if length(x) == 3
		return cross(x,y)
	elseif length(x) != 2
		error("Incorrect length for cross product")
	end

	return x[1]*y[2]-x[2]*y[1]
end

function line_segment_intersect(p::Array{Float64,1},pr::Array{Float64,1},q::Array{Float64,1},qs::Array{Float64,1})
	if (length(p) != length(q)) || (length(p) != length(pr)) || (length(q) != length(qs))
		error("Error: inconsistent dimensionality")
	end

	s = qs - q
	r = pr - p

	rxs = cross2d(r,s)
	q_pxr = cross2d(q-p,r)
	if rxs == 0
		if q_pxr == 0
			#collinear
			t0 = dot(q-p,r)/dot(r,r)
			t1 = t0 + dot(s,r)/dot(r,r)
			if dot(r,s) > 0
				if max(0.,min(t1,1.)-max(0.,t0)) > 0
					return true
				end
				return false
			else
				if max(0.,min(t0,1.)-max(0.,t1)) > 0
					return true
				end
				return false
			end
		else
			return false
		end
	end

	t = cross2d(q-p,s)/cross2d(r,s)
	u = cross2d(q-p,r)/cross2d(r,s)

	if (max(0.,min(1.,t)) == t) && (max(0.,min(1.,u)) == u)
		return true
	end

	return false
end

function poly_intersect(X::Array{Array{Float64,2},1},Y::Array{Array{Float64,2},1})
	#each row corresponds to x, y, z....
	#need i,i+1 to be each line segment
	#does pairwise comparison of line segments in O(n2) time
	##TODO: still need to check the cases of being completely inside of one another

	for x in X
		for y in Y
			if size(x) != size(y)
				error("Error: ill formed input1")
			end
			if size(x)[2] > 2
				error("Error: ill formed input2")
			end
			# This is correct: input has format: [x x'; y y']
			#  so each slice is [x;y] and [x';y'] respectively
			if line_segment_intersect(x[:,1],x[:,2],y[:,1],y[:,2]) # XXX this doesnt seem right
				return true
			end
		end
	end

	#check if one is inside the other
	max1 = max.(X[1][:,1],X[1][:,2])
	min1 = min.(X[1][:,1],X[1][:,2])
	max2 = max.(Y[1][:,1],Y[1][:,2])
	min2 = min.(Y[1][:,1],Y[1][:,2])
	for i = 2:length(X)
		max1 = max.(max1,max.(X[i][:,1],X[i][:,2]))
		min1 = min.(min1,min.(X[i][:,1],X[i][:,2]))
	end
	for i = 2:length(Y)
		max2 = max.(max2,max.(Y[i][:,1],Y[i][:,2]))
		min2 = min.(min2,min.(Y[i][:,1],Y[i][:,2]))
	end

	#Case: X is contained in Y
	if !(false in (max1 .< max2)) && !(false in (min1 .> min2))
		return true
	end
	#Case: Y is contained in X
	if !(false in (max2 .< max1)) && !(false in (min2 .> min1))
		return true
	end

	return false
end

# to see, run visualize(mdp, s, a, sp, two_frame_crash=true, debug=true)
function is_crash(mdp::Union{MLMDP{MLState,MLAction},MLPOMDP{MLState,MLAction}}, s::MLState, sp::MLState, debug::Bool=false; warning::Bool=false)
	pp = mdp.dmodel.phys_param
	dt = pp.dt
	nb_col = 2*pp.nb_lanes-1
	agent_pos = s.cars[1].x#pp.lane_length/2.
	agent_y = s.cars[1].y*pp.w_lane
	agent_vel = s.cars[1].vel

	#treat agent_pos, agent_y as (0,0)
	w_car = pp.w_car
	l_car = pp.l_car
	#TODO: make it so that X takes in to account the fact that the agent car can change lanes
	w_car_ = w_car

	agent_y_ = sp.cars[1].y*pp.w_lane
	if agent_y_ <= agent_y
		w_car_ += agent_y - agent_y_
		agent_y = agent_y_
	else
		w_car_ += agent_y_ - agent_y
	end
	X = Array{Float64,2}[[agent_pos agent_pos; agent_y agent_y+w_car_],[agent_pos+l_car agent_pos+l_car; agent_y agent_y+w_car_],
						[agent_pos agent_pos+l_car; agent_y agent_y],[agent_pos agent_pos+l_car; agent_y+w_car_ agent_y+w_car_]]

	#center of the ego car
	x = [agent_pos; agent_y]
	x2 = [sp.cars[1].x; agent_y_]
	#corner-corner distance between two sedan
	d = norm([2*l_car;2*w_car])
	if debug
		subplot(212)
		for _x in X
			plot(vec(_x[1,:]),vec(_x[2,:]),color="k")
		end
	end

	#another way to ignore ego car
	id = Dict{Int,Int}(car.id=>i+1 for (i,car) in enumerate(s.cars[2:end]))
	idp = Dict{Int,Int}(car.id=>i+1 for (i,car) in enumerate(sp.cars[2:end]))
	ids = intersect(keys(id), keys(idp))
	# NOTE: VVV
	#if its not in both, then its entering or leaving and thus at extremum of track
	#and most likely not eligible for crash detection

	cars = Tuple{CarState,CarState}[(s.cars[id[car_id]],
                                            sp.cars[idp[car_id]],)
										    for car_id in ids] #1 or 2

	for (env_car,env_car_) in cars

		if env_car.id == 1 #presumably ego car XXX probably not needed
			continue
		end
		if env_car.x < 0. # XXX probably unneeded
			continue
		end
		p = env_car.x
		y = env_car.y*pp.w_lane
		p_ = env_car_.x
		yp = env_car_.y * pp.w_lane

		dp = p_ - p
		dy = yp - y

		y1 = [p;y]
		y2 = [p_;yp]
		if norm(x-y1) > d && norm(x-y2) > d && norm(x2-y1) > d && norm(x2-y2) > d
			continue
		end
		##TODO: make consistent with new formulation
		Y1 = Array{Float64,2}[[p p; y y+w_car],[p+l_car p+l_car; y y+w_car],[p p+l_car; y y],[p p+l_car; y+w_car y+w_car]]
		Y2 = Array{Float64,2}[xy + [dp dp; dy dy] for xy in Y1]
		Y3 = Array{Float64,2}[[p p_; y yp],[p+l_car p_+l_car; y yp],[p p_; y+w_car yp+w_car],[p+l_car p_+l_car; y+w_car yp+w_car]]

		Y = Array{Float64,2}[Y1;Y2;Y3]
		if debug
			for Yi in Y
				plot(vec(Yi[1,:]),vec(Yi[2,:]),color="b")
			end
		end
		if poly_intersect(X,Y)
            if warning
                crash_warning(mdp, s, sp)
            end
			return true
		end
	end

	return false
end

function crash_warning(mdp, s, sp)
    warn("""
    Crash!

    mdp = $mdp

    s = $s

    sp = $sp
    """)
end


text:
_range_to_nt(r::AbstractRange) = (
    first = first(r),
    last = last(r),
    step = step(r)
)
_nt_to_range(nt::NamedTuple) =
    range(nt.first, nt.last, step = nt.step)

_range_to_nt(r::UnitRange) = (
    first = first(r),
    last = last(r)
)
_nt_to_range(nt::NamedTuple{(:first, :last)}) =
    UnitRange(nt.first, nt.last)

_range_to_nt(r::LinRange) = (
    first = first(r),
    last = last(r),
    length = length(r)
)
_nt_to_range(nt::NamedTuple{(:first, :last, :length)}) =
    LinRange(nt.first, nt.last, nt.length)

_edge_to_nt(edge::AbstractRange) = _range_to_nt(edge)
_edge_to_nt(edge::AbstractVector) = collect(edge)
_edge_to_nt(edge::Vector{<:Real}) = edge

_nt_to_edge(nt::NamedTuple) = _nt_to_range(nt)
_nt_to_edge(nt::AbstractVector) = nt

function _nt_to_histogram(nt::NamedTuple)
    return StatsBase.Histogram(
        tuple(map(b ->_nt_to_edge(b.binedges), nt.binning)...),
        nt.weights,
        nt.binning[1].closedleft ? :left : :right,
        nt.isdensity
    )
end

function _histogram_to_nt(h::StatsBase.Histogram)
    axs_idxs = eachindex(h.edges)
    axs_sym = Symbol.(["axis_$(i)" for i in axs_idxs])
    axs = [(
        binedges = _edge_to_nt(h.edges[i]),
        closedleft = h.closed == :left
    ) for i in axs_idxs]
    return (
        binning = NamedTuple{tuple(axs_sym...)}(axs),
        weights = h.weights,
        isdensity = h.isdensity
    )
end

function LegendDataTypes.writedata(
    output::HDF5.H5DataStore, name::AbstractString,
    x::Histogram,
    fulldatatype::DataType = typeof(x)
) where {T}
    @assert fulldatatype == typeof(x)
    writedata(output, name, _histogram_to_nt(x))
end

function LegendDataTypes.readdata(
    input::HDF5.H5DataStore, name::AbstractString,
    AT::Type{<:Histogram}
)
    nt = readdata(input, name, NamedTuple{(:binning,:weights,:isdensity)})
    _nt_to_histogram(nt)
end