text:
################################################################################
# Particle (flat LC)
################################################################################
dir_model = joinpath(module_dir(), "src/dynamics/particle")
dir_sim   = joinpath(module_dir(), "src/simulation/particle")
dir_sim_env   = joinpath(dir_sim, "flat_lc")
!isdir(dir_sim_env) && mkdir(dir_sim_env)
model = deepcopy(particle)
env = deepcopy(flat_3D_lc)
sim = Simulation(model, env)

path_base = joinpath(dir_model, "dynamics/base.jld2")
path_dyn = joinpath(dir_model, "dynamics/dynamics.jld2")
path_res = joinpath(dir_sim, "flat_lc/residual.jld2")
path_jac = joinpath(dir_sim, "flat_lc/jacobians.jld2")

instantiate_base!(sim.model, path_base)
instantiate_dynamics!(sim.model, path_dyn)

expr_res, rz_sp, rθ_sp = generate_residual_expressions(sim.model, sim.env)
save_expressions(expr_res, path_res, overwrite=true)
isfile(path_jac) && rm(path_jac)
ContactImplicitMPC.JLD2.save(path_jac, "rz_sp", rz_sp, "rθ_sp", rθ_sp)
instantiate_residual!(sim, path_res, path_jac)

################################################################################
# Particle (flat + nonlinear cone)
################################################################################
dir_model = joinpath(module_dir(), "src/dynamics/particle")
dir_sim   = joinpath(module_dir(), "src/simulation/particle")
dir_sim_env   = joinpath(dir_sim, "flat_nc")
!isdir(dir_sim_env) && mkdir(dir_sim_env)
model = deepcopy(particle)
env = deepcopy(flat_3D_nc)
s = Simulation(model, env)

path_base = joinpath(dir_model, "dynamics/base.jld2")
path_dyn = joinpath(dir_model, "dynamics/dynamics.jld2")
path_res = joinpath(dir_sim, "flat_nc/residual.jld2")
path_jac = joinpath(dir_sim, "flat_nc/jacobians.jld2")

instantiate_base!(s.model, path_base)
instantiate_dynamics!(s.model, path_dyn)

expr_res, rz_sp, rθ_sp = generate_residual_expressions(s.model, s.env)
save_expressions(expr_res, path_res, overwrite=true)
isfile(path_jac) && rm(path_jac)
ContactImplicitMPC.JLD2.save(path_jac, "rz_sp", rz_sp, "rθ_sp", rθ_sp)
instantiate_residual!(s, path_res, path_jac)

################################################################################
# Particle (quadratic)
################################################################################
dir_model = joinpath(module_dir(), "src/dynamics/particle")
dir_sim   = joinpath(module_dir(), "src/simulation/particle")
dir_sim_env   = joinpath(dir_sim, "quadratic")
!isdir(dir_sim_env) && mkdir(dir_sim_env)
model = deepcopy(particle)
env = deepcopy(quadratic_bowl_3D_lc)
sim = Simulation(model, env)

path_base = joinpath(dir_model, "dynamics/base.jld2")
path_dyn = joinpath(dir_model, "dynamics/dynamics.jld2")
path_res = joinpath(dir_sim, "quadratic/residual.jld2")
path_jac = joinpath(dir_sim, "quadratic/jacobians.jld2")

instantiate_base!(sim.model, path_base)
instantiate_dynamics!(sim.model, path_dyn)

expr_res, rz_sp, rθ_sp = generate_residual_expressions(sim.model, sim.env)
save_expressions(expr_res, path_res, overwrite=true)
isfile(path_jac) && rm(path_jac)
ContactImplicitMPC.JLD2.save(path_jac, "rz_sp", rz_sp, "rθ_sp", rθ_sp)
instantiate_residual!(sim, path_res, path_jac)

################################################################################
# Particle 2D (flat + LC)
################################################################################
dir_model = joinpath(module_dir(), "src/dynamics/particle_2D")
dir_sim   = joinpath(module_dir(), "src/simulation/particle_2D")
dir_sim_env   = joinpath(dir_sim, "flat_lc")
!isdir(dir_sim_env) && mkdir(dir_sim_env)
model = deepcopy(particle_2D)
env = deepcopy(flat_2D_lc)
sim = Simulation(model, env)

path_base = joinpath(dir_model, "dynamics/base.jld2")
path_dyn = joinpath(dir_model, "dynamics/dynamics.jld2")
path_res = joinpath(dir_sim, "flat_lc/residual.jld2")
path_jac = joinpath(dir_sim, "flat_lc/jacobians.jld2")

instantiate_base!(sim.model, path_base)
instantiate_dynamics!(sim.model, path_dyn)

expr_res, rz_sp, rθ_sp = generate_residual_expressions(sim.model, sim.env)
save_expressions(expr_res, path_res, overwrite=true)
isfile(path_jac) && rm(path_jac)
ContactImplicitMPC.JLD2.save(path_jac, "rz_sp", rz_sp, "rθ_sp", rθ_sp)
instantiate_residual!(sim, path_res, path_jac)

################################################################################
# Particle 2D (flat + nonlinear cone)
################################################################################
dir_model = joinpath(module_dir(), "src/dynamics/particle_2D")
dir_sim   = joinpath(module_dir(), "src/simulation/particle_2D")
dir_sim_env   = joinpath(dir_sim, "flat_nc")
!isdir(dir_sim_env) && mkdir(dir_sim_env)
model = deepcopy(particle_2D)
env = deepcopy(flat_2D_nc)
sim = Simulation(model, env)

path_base = joinpath(dir_model, "dynamics/base.jld2")
path_dyn = joinpath(dir_model, "dynamics/dynamics.jld2")
path_res = joinpath(dir_sim, "flat_nc/residual.jld2")
path_jac = joinpath(dir_sim, "flat_nc/jacobians.jld2")

instantiate_base!(sim.model, path_base)
instantiate_dynamics!(sim.model, path_dyn)

expr_res, rz_sp, rθ_sp = generate_residual_expressions(sim.model, sim.env)
save_expressions(expr_res, path_res, overwrite=true)
isfile(path_jac) && rm(path_jac)
ContactImplicitMPC.JLD2.save(path_jac, "rz_sp", rz_sp, "rθ_sp", rθ_sp)
instantiate_residual!(sim, path_res, path_jac)

################################################################################
# Particle 2D (slope)
################################################################################
dir_model = joinpath(module_dir(), "src/dynamics/particle_2D")
dir_sim   = joinpath(module_dir(), "src/simulation/particle_2D")
dir_sim_env   = joinpath(dir_sim, "slope")
!isdir(dir_sim_env) && mkdir(dir_sim_env)
model = deepcopy(particle_2D)
env = deepcopy(slope1_2D_lc)
sim = Simulation(model, env)

path_base = joinpath(dir_model, "dynamics/base.jld2")
path_dyn = joinpath(dir_model, "dynamics/dynamics.jld2")
path_res = joinpath(dir_sim, "slope/residual.jld2")
path_jac = joinpath(dir_sim, "slope/jacobians.jld2")

instantiate_base!(sim.model, path_base)
instantiate_dynamics!(sim.model, path_dyn)

expr_res, rz_sp, rθ_sp = generate_residual_expressions(sim.model, sim.env)
save_expressions(expr_res, path_res, overwrite=true)
isfile(path_jac) && rm(path_jac)
ContactImplicitMPC.JLD2.save(path_jac, "rz_sp", rz_sp, "rθ_sp", rθ_sp)
instantiate_residual!(sim, path_res, path_jac)

################################################################################
# Hopper (2D)
################################################################################
dir_model = joinpath(module_dir(), "src/dynamics/hopper_2D")
dir_sim   = joinpath(module_dir(), "src/simulation/hopper_2D")
dir_sim_env   = joinpath(dir_sim, "flat")
!isdir(dir_sim_env) && mkdir(dir_sim_env)
model = deepcopy(hopper_2D)
env = deepcopy(flat_2D_lc)
sim = Simulation(model, env)

path_base = joinpath(dir_model, "dynamics/base.jld2")
path_dyn = joinpath(dir_model, "dynamics/dynamics.jld2")
path_res = joinpath(dir_sim, "flat/residual.jld2")
path_jac = joinpath(dir_sim, "flat/jacobians.jld2")

instantiate_base!(sim.model, path_base)
instantiate_dynamics!(sim.model, path_dyn)

expr_res, rz_sp, rθ_sp = generate_residual_expressions(sim.model, sim.env)
save_expressions(expr_res, path_res, overwrite=true)
isfile(path_jac) && rm(path_jac)
ContactImplicitMPC.JLD2.save(path_jac, "rz_sp", rz_sp, "rθ_sp", rθ_sp)
instantiate_residual!(sim, path_res, path_jac)

################################################################################
# Hopper (2D) (sinusoidal)
################################################################################
dir_model = joinpath(module_dir(), "src/dynamics/hopper_2D")
dir_sim   = joinpath(module_dir(), "src/simulation/hopper_2D")
dir_sim_env   = joinpath(dir_sim, "sinusoidal")
!isdir(dir_sim_env) && mkdir(dir_sim_env)
model = deepcopy(hopper_2D)
env = deepcopy(sine2_2D_lc)
sim = Simulation(model, env)

path_base = joinpath(dir_model, "dynamics/base.jld2")
path_dyn = joinpath(dir_model, "dynamics/dynamics.jld2")
path_res = joinpath(dir_sim, "sinusoidal/residual.jld2")
path_jac = joinpath(dir_sim, "sinusoidal/jacobians.jld2")

instantiate_base!(sim.model, path_base)
instantiate_dynamics!(sim.model, path_dyn)

expr_res, rz_sp, rθ_sp = generate_residual_expressions(sim.model, sim.env)
save_expressions(expr_res, path_res, overwrite=true)
isfile(path_jac) && rm(path_jac)
ContactImplicitMPC.JLD2.save(path_jac, "rz_sp", rz_sp, "rθ_sp", rθ_sp)
instantiate_residual!(sim, path_res, path_jac)

################################################################################
# Hopper 2D (Piecewise)
################################################################################
dir_model = joinpath(module_dir(), "src/dynamics/hopper_2D")
dir_sim   = joinpath(module_dir(), "src/simulation/hopper_2D")
dir_sim_env   = joinpath(dir_sim, "piecewise")
!isdir(dir_sim_env) && mkdir(dir_sim_env)
model = deepcopy(hopper_2D)
env = deepcopy(piecewise1_2D_lc)
sim = Simulation(model, env)

path_base = joinpath(dir_model, "dynamics/base.jld2")
path_dyn = joinpath(dir_model, "dynamics/dynamics.jld2")
path_res = joinpath(dir_sim, "piecewise/residual.jld2")
path_jac = joinpath(dir_sim, "piecewise/jacobians.jld2")

instantiate_base!(sim.model, path_base)
expr_dyn = generate_dynamics_expressions(sim.model, derivs=true)
save_expressions(expr_dyn, path_dyn, overwrite=true)
instantiate_dynamics!(sim.model, path_dyn, derivs = true)

expr_res, rz_sp, rθ_sp = generate_residual_expressions(sim.model, sim.env, jacobians = :approx)
save_expressions(expr_res, path_res, overwrite=true)
isfile(path_jac) && rm(path_jac)
ContactImplicitMPC.JLD2.save(path_jac, "rz_sp", rz_sp, "rθ_sp", rθ_sp)
instantiate_residual!(sim, path_res, path_jac, jacobians = :approx)

################################################################################
# Hopper (2D) (stairs)
################################################################################
dir_model = joinpath(module_dir(), "src/dynamics/hopper_2D")
dir_sim   = joinpath(module_dir(), "src/simulation/hopper_2D")
dir_sim_env   = joinpath(dir_sim, "stairs")
!isdir(dir_sim_env) && mkdir(dir_sim_env)
model = deepcopy(hopper_2D)
env = deepcopy(stairs3_2D_lc)
sim = Simulation(model, env)

path_base = joinpath(dir_model, "dynamics/base.jld2")
path_dyn = joinpath(dir_model, "dynamics/dynamics.jld2")
path_res = joinpath(dir_sim, "stairs/residual.jld2")
path_jac = joinpath(dir_sim, "stairs/jacobians.jld2")

instantiate_base!(sim.model, path_base)
instantiate_dynamics!(sim.model, path_dyn)

expr_res, rz_sp, rθ_sp = generate_residual_expressions(sim.model, sim.env)
save_expressions(expr_res, path_res, overwrite=true)
isfile(path_jac) && rm(path_jac)
ContactImplicitMPC.JLD2.save(path_jac, "rz_sp", rz_sp, "rθ_sp", rθ_sp)
instantiate_residual!(sim, path_res, path_jac)

################################################################################
# Hopper (3D)
################################################################################
dir_model = joinpath(module_dir(), "src/dynamics/hopper_3D")
dir_sim   = joinpath(module_dir(), "src/simulation/hopper_3D")
dir_sim_env   = joinpath(dir_sim, "flat")
!isdir(dir_sim_env) && mkdir(dir_sim_env)
model = deepcopy(hopper_3D)
env = deepcopy(flat_3D_lc)
sim = Simulation(model, env)

path_base = joinpath(dir_model, "dynamics/base.jld2")
path_dyn = joinpath(dir_model, "dynamics/dynamics.jld2")
path_res = joinpath(dir_sim, "flat/residual.jld2")
path_jac = joinpath(dir_sim, "flat/jacobians.jld2")

instantiate_base!(sim.model, path_base)
instantiate_dynamics!(sim.model, path_dyn)

expr_res, rz_sp, rθ_sp = generate_residual_expressions(sim.model, sim.env)
save_expressions(expr_res, path_res, overwrite=true)
isfile(path_jac) && rm(path_jac)
ContactImplicitMPC.JLD2.save(path_jac, "rz_sp", rz_sp, "rθ_sp", rθ_sp)
instantiate_residual!(sim, path_res, path_jac)

################################################################################
# Hopper (3D) (sinusoidal)
################################################################################
dir_model = joinpath(module_dir(), "src/dynamics/hopper_3D")
dir_sim   = joinpath(module_dir(), "src/simulation/hopper_3D")
dir_sim_env   = joinpath(dir_sim, "sinusoidal")
!isdir(dir_sim_env) && mkdir(dir_sim_env)
model = deepcopy(hopper_3D)
env = deepcopy(sine2_3D_lc)
sim = Simulation(model, env)

path_base = joinpath(dir_model, "dynamics/base.jld2")
path_dyn = joinpath(dir_model, "dynamics/dynamics.jld2")
path_res = joinpath(dir_sim, "sinusoidal/residual.jld2")
path_jac = joinpath(dir_sim, "sinusoidal/jacobians.jld2")

instantiate_base!(sim.model, path_base)
instantiate_dynamics!(sim.model, path_dyn)

expr_res, rz_sp, rθ_sp = generate_residual_expressions(sim.model, sim.env)
save_expressions(expr_res, path_res, overwrite=true)
isfile(path_jac) && rm(path_jac)
ContactImplicitMPC.JLD2.save(path_jac, "rz_sp", rz_sp, "rθ_sp", rθ_sp)
instantiate_residual!(sim, path_res, path_jac)

################################################################################
# Quadruped
################################################################################
dir_model = joinpath(module_dir(), "src/dynamics/quadruped")
dir_sim   = joinpath(module_dir(), "src/simulation/quadruped")
dir_sim_env   = joinpath(dir_sim, "flat")
!isdir(dir_sim_env) && mkdir(dir_sim_env)
model = deepcopy(quadruped)
env = deepcopy(flat_2D_lc)
sim = Simulation(model, env)

path_base = joinpath(dir_model, "dynamics/base.jld2")
path_dyn = joinpath(dir_model, "dynamics/dynamics.jld2")
path_res = joinpath(dir_sim, "flat/residual.jld2")
path_jac = joinpath(dir_sim, "flat/jacobians.jld2")

instantiate_base!(sim.model, path_base)
instantiate_dynamics!(sim.model, path_dyn)

expr_res, rz_sp, rθ_sp = generate_residual_expressions(sim.model, sim.env)
save_expressions(expr_res, path_res, overwrite=true)
isfile(path_jac) && rm(path_jac)
ContactImplicitMPC.JLD2.save(path_jac, "rz_sp", rz_sp, "rθ_sp", rθ_sp)
instantiate_residual!(sim, path_res, path_jac)

################################################################################
# Quadruped Payload
################################################################################
dir_model = joinpath(module_dir(), "src/dynamics/quadruped")
dir_sim   = joinpath(module_dir(), "src/simulation/quadruped")
dir_sim_env   = joinpath(dir_sim, "payload")
!isdir(dir_sim_env) && mkdir(dir_sim_env)
model = deepcopy(quadruped_payload)
env = deepcopy(flat_2D_lc)
sim = Simulation(model, env)

path_base = joinpath(dir_model, "dynamics_payload/base.jld2")
path_dyn = joinpath(dir_model, "dynamics_payload/dynamics.jld2")
path_res = joinpath(dir_sim, "payload/residual.jld2")
path_jac = joinpath(dir_sim, "payload/jacobians.jld2")

instantiate_base!(sim.model, path_base)
instantiate_dynamics!(sim.model, path_dyn)

expr_res, rz_sp, rθ_sp = generate_residual_expressions(sim.model, sim.env)
save_expressions(expr_res, path_res, overwrite=true)
isfile(path_jac) && rm(path_jac)
ContactImplicitMPC.JLD2.save(path_jac, "rz_sp", rz_sp, "rθ_sp", rθ_sp)
instantiate_residual!(sim, path_res, path_jac)

################################################################################
# Quadruped Sinusoidal
################################################################################
dir_model = joinpath(module_dir(), "src/dynamics/quadruped")
dir_sim   = joinpath(module_dir(), "src/simulation/quadruped")
dir_sim_env   = joinpath(dir_sim, "sinusoidal")
!isdir(dir_sim_env) && mkdir(dir_sim_env)
model = deepcopy(quadruped)
env = deepcopy(sine1_2D_lc)
sim = Simulation(model, env)

path_base = joinpath(dir_model, "dynamics/base.jld2")
path_dyn = joinpath(dir_model, "dynamics/dynamics.jld2")
path_res = joinpath(dir_sim, "sinusoidal/residual.jld2")
path_jac = joinpath(dir_sim, "sinusoidal/jacobians.jld2")

instantiate_base!(sim.model, path_base)
instantiate_dynamics!(sim.model, path_dyn)

expr_res, rz_sp, rθ_sp = generate_residual_expressions(sim.model, sim.env, jacobians = :full)
save_expressions(expr_res, path_res, overwrite=true)
isfile(path_jac) && rm(path_jac)
ContactImplicitMPC.JLD2.save(path_jac, "rz_sp", rz_sp, "rθ_sp", rθ_sp)
instantiate_residual!(sim, path_res, path_jac)

################################################################################
# Quadruped (Piecewise)
################################################################################
dir_model = joinpath(module_dir(), "src/dynamics/quadruped")
dir_sim   = joinpath(module_dir(), "src/simulation/quadruped")
dir_sim_env   = joinpath(dir_sim, "piecewise")
!isdir(dir_sim_env) && mkdir(dir_sim_env)
model = deepcopy(quadruped)
env = deepcopy(piecewise1_2D_lc)
sim = Simulation(model, env)

path_base = joinpath(dir_model, "dynamics/base.jld2")
path_dyn = joinpath(dir_model, "dynamics/dynamics.jld2")
path_res = joinpath(dir_sim, "piecewise/residual.jld2")
path_jac = joinpath(dir_sim, "piecewise/jacobians.jld2")

instantiate_base!(sim.model, path_base)
expr_dyn = generate_dynamics_expressions(sim.model, derivs = true)
save_expressions(expr_dyn, path_dyn, overwrite=true)
instantiate_dynamics!(sim.model, path_dyn, derivs = true)

expr_res, rz_sp, rθ_sp = generate_residual_expressions(sim.model, sim.env, jacobians = :approx)
save_expressions(expr_res, path_res, overwrite=true)
isfile(path_jac) && rm(path_jac)
ContactImplicitMPC.JLD2.save(path_jac, "rz_sp", rz_sp, "rθ_sp", rθ_sp)
instantiate_residual!(sim, path_res, path_jac, jacobians = :approx)

################################################################################
# Flamingo (flat)
################################################################################
dir_model = joinpath(module_dir(), "src/dynamics/flamingo")
dir_sim   = joinpath(module_dir(), "src/simulation/flamingo")
dir_sim_env   = joinpath(dir_sim, "flat")
!isdir(dir_sim_env) && mkdir(dir_sim_env)
model = deepcopy(flamingo)
env = deepcopy(flat_2D_lc)
sim = Simulation(model, env)

path_base = joinpath(dir_model, "dynamics/base.jld2")
path_dyn = joinpath(dir_model, "dynamics/dynamics.jld2")
path_res = joinpath(dir_sim, "flat/residual.jld2")
path_jac = joinpath(dir_sim, "flat/jacobians.jld2")

instantiate_base!(sim.model, path_base)
instantiate_dynamics!(sim.model, path_dyn)

expr_res, rz_sp, rθ_sp = generate_residual_expressions(sim.model, sim.env)
save_expressions(expr_res, path_res, overwrite=true)
isfile(path_jac) && rm(path_jac)
ContactImplicitMPC.JLD2.save(path_jac, "rz_sp", rz_sp, "rθ_sp", rθ_sp)
instantiate_residual!(sim, path_res, path_jac)

################################################################################
# Flamingo (sinusoidal)
################################################################################
dir_model = joinpath(module_dir(), "src/dynamics/flamingo")
dir_sim   = joinpath(module_dir(), "src/simulation/flamingo")
dir_sim_env   = joinpath(dir_sim, "sinusoidal")
!isdir(dir_sim_env) && mkdir(dir_sim_env)
model = deepcopy(flamingo)
env = deepcopy(sine3_2D_lc)
sim = Simulation(model, env)

path_base = joinpath(dir_model, "dynamics/base.jld2")
path_dyn = joinpath(dir_model, "dynamics/dynamics.jld2")
path_res = joinpath(dir_sim, "sinusoidal/residual.jld2")
path_jac = joinpath(dir_sim, "sinusoidal/jacobians.jld2")

instantiate_base!(sim.model, path_base)
instantiate_dynamics!(sim.model, path_dyn)

expr_res, rz_sp, rθ_sp = generate_residual_expressions(sim.model, sim.env)
save_expressions(expr_res, path_res, overwrite=true)
isfile(path_jac) && rm(path_jac)
ContactImplicitMPC.JLD2.save(path_jac, "rz_sp", rz_sp, "rθ_sp", rθ_sp)
instantiate_residual!(sim, path_res, path_jac)

################################################################################
# Flamingo (smooth slope)
################################################################################
dir_model = joinpath(module_dir(), "src/dynamics/flamingo")
dir_sim   = joinpath(module_dir(), "src/simulation/flamingo")
dir_sim_env   = joinpath(dir_sim, "slope")
!isdir(dir_sim_env) && mkdir(dir_sim_env)
model = deepcopy(flamingo)
env = deepcopy(slope_smooth_2D_lc)
sim = Simulation(model, env)

path_base = joinpath(dir_model, "dynamics/base.jld2")
path_dyn = joinpath(dir_model, "dynamics/dynamics.jld2")
path_res = joinpath(dir_sim, "slope/residual.jld2")
path_jac = joinpath(dir_sim, "slope/jacobians.jld2")

instantiate_base!(sim.model, path_base)
instantiate_dynamics!(sim.model, path_dyn)

expr_res, rz_sp, rθ_sp = generate_residual_expressions(sim.model, sim.env)
save_expressions(expr_res, path_res, overwrite=true)
isfile(path_jac) && rm(path_jac)
ContactImplicitMPC.JLD2.save(path_jac, "rz_sp", rz_sp, "rθ_sp", rθ_sp)
instantiate_residual!(sim, path_res, path_jac)

################################################################################
# Flamingo (piecewise)
################################################################################
dir_model = joinpath(module_dir(), "src/dynamics/flamingo")
dir_sim   = joinpath(module_dir(), "src/simulation/flamingo")
dir_sim_env   = joinpath(dir_sim, "piecewise")
!isdir(dir_sim_env) && mkdir(dir_sim_env)
model = deepcopy(flamingo)
env = deepcopy(piecewise1_2D_lc)
sim = Simulation(model, env)

path_base = joinpath(dir_model, "dynamics/base.jld2")
path_dyn = joinpath(dir_model, "dynamics/dynamics.jld2")
path_res = joinpath(dir_sim, "piecewise/residual.jld2")
path_jac = joinpath(dir_sim, "piecewise/jacobians.jld2")

instantiate_base!(sim.model, path_base)
expr_dyn = generate_dynamics_expressions(sim.model, derivs = true)
save_expressions(expr_dyn, path_dyn, overwrite=true)
instantiate_dynamics!(sim.model, path_dyn, derivs=true)

expr_res, rz_sp, rθ_sp = generate_residual_expressions(sim.model, sim.env, jacobians = :approx)
save_expressions(expr_res, path_res, overwrite=true)
isfile(path_jac) && rm(path_jac)
ContactImplicitMPC.JLD2.save(path_jac, "rz_sp", rz_sp, "rθ_sp", rθ_sp)
instantiate_residual!(sim, path_res, path_jac, jacobians = :approx)

################################################################################
# PushBot
################################################################################
dir_model = joinpath(module_dir(), "src/dynamics/pushbot")
dir_sim   = joinpath(module_dir(), "src/simulation/pushbot")
dir_sim_env   = joinpath(dir_sim, "flat")
!isdir(dir_sim_env) && mkdir(dir_sim_env)
model = deepcopy(pushbot)
env = deepcopy(flat_2D_lc)
sim = Simulation(model, env)

path_base = joinpath(dir_model, "dynamics/base.jld2")
path_dyn = joinpath(dir_model, "dynamics/dynamics.jld2")
path_res = joinpath(dir_sim, "flat/residual.jld2")
path_jac = joinpath(dir_sim, "flat/jacobians.jld2")

instantiate_base!(sim.model, path_base)
instantiate_dynamics!(sim.model, path_dyn)

expr_res, rz_sp, rθ_sp = generate_residual_expressions(sim.model, sim.env)
save_expressions(expr_res, path_res, overwrite=true)
isfile(path_jac) && rm(path_jac)
ContactImplicitMPC.JLD2.save(path_jac, "rz_sp", rz_sp, "rθ_sp", rθ_sp)
instantiate_residual!(sim, path_res, path_jac)

################################################################################
# WalledCartpole
################################################################################
dir_model = joinpath(module_dir(), "src/dynamics/walledcartpole")
dir_sim   = joinpath(module_dir(), "src/simulation/walledcartpole")
dir_sim_env   = joinpath(dir_sim, "flat")
!isdir(dir_sim_env) && mkdir(dir_sim_env)
model = deepcopy(walledcartpole)
env = deepcopy(flat_2D_lc)
sim = Simulation(model, env)

path_base = joinpath(dir_model, "dynamics/base.jld2")
path_dyn = joinpath(dir_model, "dynamics/dynamics.jld2")
path_res = joinpath(dir_sim, "flat/residual.jld2")
path_jac = joinpath(dir_sim, "flat/jacobians.jld2")

instantiate_base!(sim.model, path_base)
instantiate_dynamics!(sim.model, path_dyn)

expr_res, rz_sp, rθ_sp = generate_residual_expressions(sim.model, sim.env)
save_expressions(expr_res, path_res, overwrite=true)
isfile(path_jac) && rm(path_jac)
ContactImplicitMPC.JLD2.save(path_jac, "rz_sp", rz_sp, "rθ_sp", rθ_sp)
instantiate_residual!(sim, path_res, path_jac)

################################################################################
# Rigid body
################################################################################
dir_model = joinpath(module_dir(), "src/dynamics/rigidbody")
dir_sim   = joinpath(module_dir(), "src/simulation/rigidbody")
dir_sim_env   = joinpath(dir_sim, "flat")
!isdir(dir_sim_env) && mkdir(dir_sim_env)
model = deepcopy(rigidbody)
env = deepcopy(flat_3D_lc)
sim = Simulation(model, env)

path_base = joinpath(dir_model, "dynamics/base.jld2")
path_dyn = joinpath(dir_model, "dynamics/dynamics.jld2")
path_res = joinpath(dir_sim, "flat/residual.jld2")
path_jac = joinpath(dir_sim, "flat/jacobians.jld2")
instantiate_base!(sim.model, path_base)
instantiate_dynamics!(sim.model, path_dyn)

expr_res, rz_sp, rθ_sp = generate_residual_expressions(sim.model, sim.env, mapping = Gz_func, nv = sim.model.nq - 1)
save_expressions(expr_res, path_res, overwrite=true)
isfile(path_jac) && rm(path_jac)
ContactImplicitMPC.JLD2.save(path_jac, "rz_sp", rz_sp, "rθ_sp", rθ_sp)
instantiate_residual!(sim, path_res, path_jac)

################################################################################
# Centroidal Quadruped
################################################################################
dir_model = joinpath(module_dir(), "src/dynamics/centroidal_quadruped")
dir_sim   = joinpath(module_dir(), "src/simulation/centroidal_quadruped")
dir_sim_env   = joinpath(dir_sim, "flat")
!isdir(dir_sim_env) && mkdir(dir_sim_env)
model = deepcopy(centroidal_quadruped)
env = deepcopy(flat_3D_lc)
sim = Simulation(model, env)

path_base = joinpath(dir_model, "dynamics/base.jld2")
path_dyn = joinpath(dir_model, "dynamics/dynamics.jld2")
path_res = joinpath(dir_sim, "flat/residual.jld2")
path_jac = joinpath(dir_sim, "flat/jacobians.jld2")

instantiate_base!(sim.model, path_base)
instantiate_dynamics!(sim.model, path_dyn)

expr_res, rz_sp, rθ_sp = generate_residual_expressions(sim.model, sim.env)
save_expressions(expr_res, path_res, overwrite=true)
isfile(path_jac) && rm(path_jac)
ContactImplicitMPC.JLD2.save(path_jac, "rz_sp", rz_sp, "rθ_sp", rθ_sp)
instantiate_residual!(sim, path_res, path_jac)

################################################################################
# Centroidal Quadruped undamped
################################################################################
dir_model = joinpath(module_dir(), "src/dynamics/centroidal_quadruped")
dir_sim   = joinpath(module_dir(), "src/simulation/centroidal_quadruped")
dir_sim_env   = joinpath(dir_sim, "flat_undamped")
!isdir(dir_sim_env) && mkdir(dir_sim_env)
model = deepcopy(centroidal_quadruped_undamped)
env = deepcopy(flat_3D_lc)
sim = Simulation(model, env)

path_base = joinpath(dir_model, "dynamics_undamped/base.jld2")
path_dyn = joinpath(dir_model, "dynamics_undamped/dynamics.jld2")
path_res = joinpath(dir_sim, "flat_undamped/residual.jld2")
path_jac = joinpath(dir_sim, "flat_undamped/jacobians.jld2")

instantiate_base!(sim.model, path_base)
instantiate_dynamics!(sim.model, path_dyn)

expr_res, rz_sp, rθ_sp = generate_residual_expressions(sim.model, sim.env)
save_expressions(expr_res, path_res, overwrite=true)
isfile(path_jac) && rm(path_jac)
ContactImplicitMPC.JLD2.save(path_jac, "rz_sp", rz_sp, "rθ_sp", rθ_sp)
instantiate_residual!(sim, path_res, path_jac)

################################################################################
# Point Foot Quadruped
################################################################################
dir_model = joinpath(module_dir(), "src/dynamics/point_foot_quadruped")
dir_sim = joinpath(module_dir(), "src/simulation/point_foot_quadruped")
dir_sim_env = joinpath(dir_sim, "flat")
!isdir(dir_sim_env) && mkdir(dir_sim_env)
model = deepcopy(point_foot_quadruped)
env = deepcopy(flat_3D_lc)
sim = Simulation(model, env)

path_base = joinpath(dir_model, "dynamics/base.jld2")
path_dyn = joinpath(dir_model, "dynamics/dynamics.jld2")
path_res = joinpath(dir_sim, "flat/residual.jld2")
path_jac = joinpath(dir_sim, "flat/jacobians.jld2")

instantiate_base!(sim.model, path_base)
instantiate_dynamics!(sim.model, path_dyn)

expr_res, rz_sp, rθ_sp = generate_residual_expressions(sim.model, sim.env)
save_expressions(expr_res, path_res, overwrite=true)
isfile(path_jac) && rm(path_jac)
ContactImplicitMPC.JLD2.save(path_jac, "rz_sp", rz_sp, "rθ_sp", rθ_sp)
instantiate_residual!(sim, path_res, path_jac)


text:
using Triangulate
using Base.Test

numberofpoints = 5
pointlist = [0., 0., 1., 0., 1., 1., 0.5, 1.5, 0., 1.]
tic()
mesh = Triangulate.triangulate(0.01, numberofpoints, pointlist)
mesh = Triangulate.triangulate(0.001, numberofpoints, pointlist)
mesh = Triangulate.triangulate(0.0001, numberofpoints, pointlist)
t = toq()
println(t)

ext: jl


text:
"""
	gmtread(fname::String, data; kwargs...)

Read GMT object from file. The object is one of "grid" or "grd", "image" or "img",
"data" or "table", "cmap" or "cpt" and "ps" (for postscript), and OGR formats (shp, kml, json).
Use a type specificatin to force a certain reading path (e.g. grd=true to read grids) or take
the chance of letting the data type be guessed via the file extension. Known extensions are:
Grids:      .grd
Images:     .jpg, .png, .tif, .bmp
Datasets:   .dat, .txt, .csv
Datasets:   .shp, .kml, .json
CPT:        .cpt
PostScript: .ps, .eps

Parameters
----------

Specify data type.  Choose among:
- **grd** | **grid** :: [Type => Any]

    Load a grid.
- **img** | **image** :: [Type => Any]

    Load an image.
- **cpt** | **cmap** :: [Type => Any]

    Load a GMT color palette.
- **data** | **dataset** | **table** :: [Type => Any]

    Load a dataset (a table of numbers).
- **ogr** :: [Type => Any]

    Load a dataset via GDAL OGR (a table of numbers). Many things can happen here.
- **ps** :: [Type => Any]

    Load a PostScript file
- **gdal** :: [Type => Any]

    Force reading the file via GDAL. Should only be used to read grids.
- **varname** :: [Type => Str]

    When netCDF files have more than one 2D (or higher) variables use *varname* to pick the wished variable.
    e.g. varname=:slp to read the variable named 'slp'. This option defaults data type to 'grid'
- **layer** | **band** :: [Type => Str, Number, Array]

    When files are multiband or nc files with 3D or 4D arrays, we access them via these keywords.
    layer=4 reads the fourth layer (or band) of the file. But the file can be a grid or an image. If it is a
    grid layer can be a scalar (to read 3D arrays) or an array of two elements (to read a 4D array).
    If file is an image 'layer' can be a 1 or a 1x3 array (to read a RGB image). Not that in this later case
    bands do not need to be contiguous. A band=[0,5,2] composes an RGB out of those bands. See more at
    $(GMTdoc)/GMT_Docs.html#modifiers-for-coards-compliant-netcdf-files)

- $(GMT.opt_R)
- $(GMT.opt_V)
- $(GMT.opt_bi)
- $(GMT.opt_f)

Example: to read a nc called 'lixo.grd'

    G = gmtread("lixo.grd");

to read a jpg image with the bands reversed (this example is currently broken in GMT5. Needs GMT6dev)

    I = gmtread("image.jpg", band=[2,1,0]);
"""
function gmtread(fname::String; kwargs...)

	d = KW(kwargs)
	help_show_options(d)					# Check if user wants ONLY the HELP mode
	cmd, opt_R = parse_R(d, "")
	cmd = parse_common_opts(d, cmd, [:V_params :f :i :h])[1]
	cmd, opt_bi = parse_bi(d, cmd)

	# Process these first so they may take precedence over defaults set below
	opt_T = add_opt(d, "", "Tg", [:grd :grid])
	if (opt_T != "")		# Force read via GDAL
		if ((val = find_in_dict(d, [:gdal])[1]) !== nothing)  fname = fname * "=gd"  end
	else
		opt_T = add_opt(d, "", "Ti", [:img :image])
	end
	if (opt_T == "")  opt_T = add_opt(d, "", "Td", [:data :dataset :table])  end
	if (opt_T == "")  opt_T = add_opt(d, "", "Tc", [:cpt :cmap])  end
	if (opt_T == "")  opt_T = add_opt(d, "", "Tp", [:ps])   end
	if (opt_T == "")  opt_T = add_opt(d, "", "To", [:ogr])  end

	ogr_layer = Int32(0)			# Used only with ogrread. Means by default read only the first layer
	if ((varname = find_in_dict(d, [:varname])[1]) !== nothing) # See if we have a nc varname / layer request
		if (isempty(opt_T))			# Force read via GDAL
			if ((val = find_in_dict(d, [:gdal])[1]) !== nothing)  fname = fname * "=gd"  end
			opt_T = " -Tg"
		end
		fname = fname * "?" * arg2str(varname)
		if ((val = find_in_dict(d, [:layer :band])[1]) !== nothing)
			if (isa(val, Number))     fname *= @sprintf("[%d]", val)
			elseif (isa(val, Array))  fname *= @sprintf("[%d,%d]", val[1], val[2])	# A 4D array
			end
		end
	else									# See if we have a bands request
		if ((val = find_in_dict(d, [:layer :band])[1]) !== nothing)
			if ((lix = guess_T_from_ext(fname)) == " -To")		# See if it's a OGR layer request
				ogr_layer = Int32(val - 1)	# -1 because it's going to be sent to C (zero based)
			else
				fname = fname * "+b"
				if (isa(val, String) || isa(val, Symbol) || isa(val, Number))
					fname = string(fname, val)
				elseif (isa(val, Array) || isa(val, Tuple))
					if (length(val) == 3)
						fname = fname * @sprintf("%d,%d,%d", val[1], val[2], val[3])
					else
						error("Number of bands in the 'band' option can only be 1 or 3")
					end
				end
				(opt_T == "") && (opt_T = " -Ti")
			end
		end
	end

	if (opt_T == "")
		((opt_T = guess_T_from_ext(fname)) == "") && error("Must select one input data type (grid, image, dataset, cmap or ps)")
		if (opt_T == " -Tg" && haskey(d, :ignore_grd))  return nothing  end 	# contourf uses this
	else
		opt_T = opt_T[1:4]      				# Remove whatever was given as argument to type kwarg
	end

	if (opt_T == " -Ti" || opt_T == " -Tg")		# See if we have a mem layout request
		if ((val = find_in_dict(d, [:layout])[1]) !== nothing)
			cmd = (opt_T == " -Ti") ? cmd * " -%" * arg2str(val) : cmd * " -&" * arg2str(val)
		end
	end

	if (opt_T != " -To")			# All others but OGR
		if (opt_T == " -Td" && !isempty(opt_bi))  cmd *= opt_bi  end		# Read from binary file
		cmd *= opt_T
		if (dbg_print_cmd(d, cmd) !== nothing)  return "gmtread " * cmd  end
		O = gmt("read " * fname * cmd)
	else
		if (dbg_print_cmd(d, cmd) !== nothing)  return "ogrread " * cmd  end
		# Because of the certificates shits on Windows. But for some reason the set in gmtlib_check_url_name() is not visible
		(Sys.iswindows())  && run(`cmd /c set GDAL_HTTP_UNSAFESSL=YES`)
		API2 = GMT_Create_Session("GMT", 2, GMT_SESSION_NOEXIT + GMT_SESSION_EXTERNAL + GMT_SESSION_COLMAJOR);
		if (GMTver >= 6.1)
			x = opt_R2num(opt_R)		# See if we have a limits request
			if (GMTver >= 6.2)
				lims = (x === nothing) ? (0.0, 0, 0, 0, 0, 0) : tuple(hcat(x,[0.0 0])...)
				ctrl = OGRREAD_CTRL(Int32(0), ogr_layer, pointer(fname), lims)
				O = ogr2GMTdataset(gmt_ogrread(API2, pointer([ctrl])))
			else
				O = ogr2GMTdataset(gmt_ogrread(API2, fname, (x === nothing) ? C_NULL : x))
			end
		else
			O = ogr2GMTdataset(gmt_ogrread(API2, fname))
		end
	end
end

# ---------------------------------------------------------------------------------
function guess_T_from_ext(fname::String)::String
	# Guess the -T option from a couple of known extensions
	fname, ext = splitext(fname)
	if (length(ext) > 8 || occursin("?", ext))			# A SUBDATASET encoded fname?
		return (occursin("?", ext)) ? " -Tg" : ""
	end
	ext = lowercase(ext[2:end])
	if     (findfirst(isequal(ext), ["grd", "nc", "nc=gd"])  !== nothing)  out = " -Tg";
	elseif (findfirst(isequal(ext), ["dat", "txt", "csv"])   !== nothing)  out = " -Td";
	elseif (findfirst(isequal(ext), ["jpg", "png", "tif", "tiff", "bmp"]) 	!== nothing)  out = " -Ti";
	elseif (findfirst(isequal(ext), ["shp", "kml", "json", "geojson", "gmt", "gpkg"]) !== nothing)  out = " -To";
	elseif (ext == "cpt")  out = " -Tc";
	elseif (ext == "ps" || ext == "eps")  out = " -Tp";
	else
		out = ""
	end
end

"""
	gmtwrite(fname::String, data; kwargs...)

Write a GMT object to file. The object is one of "grid" or "grd", "image" or "img",
"dataset" or "table", "cmap" or "cpt" and "ps" (for postscript).

When saving grids we have a large panoply of formats at our disposal.

Parameters
----------

- **id** ::  [Type => Str]

    Use an ``id`` code when not not saving a grid into a standard COARDS-compliant netCDF grid. This ``id``
    is made up of two characters like ``ef`` to save in ESRI Arc/Info ASCII Grid Interchange format (ASCII float).
    See the full list of ids at $(GMTdoc)grdconvert.html#format-identifier.

    ($(GMTdoc)grdconvert.html#g)
- **scale** | **offset** :: [Type => Number]

    You may optionally ask to scale the data and then offset them with the specified amounts.
    These modifiers are particularly practical when storing the data as integers, by first removing an offset
    and then scaling down the values.
- **nan** | **novalue** | **invalid** | **missing** :: [Type => Number]

    Lets you supply a value that represents an invalid grid entry, i.e., ‘Not-a-Number’.
- **gdal** :: [Type => Bool]

    Force the use of the GDAL library to write the grid (to be used only with grids).
    ($(GMTdoc)GMT_Docs.html#grid-file-format-specifications)
- **driver** :: [Type => Str]

    When saving in other than the netCDF format we must tell the GDAL library what is wished format.
    That is done by specifying the driver name used by GDAL itself (e.g., netCDF, GTiFF, etc...).
- **datatype** :: [Type => Str] 		``Arg = u8|u16|i16|u32|i32|float32``

    When saving with GDAL we can specify the data type from u8|u16|i16|u32|i32|float32 where ‘i’ and ‘u’ denote
    signed and unsigned integers respectively.
- $(GMT.opt_R)
- $(GMT.opt_V)
- $(GMT.opt_bo)
- $(GMT.opt_f)

Example: write the GMTgrid 'G' object into a nc file called 'lixo.grd'

	gmtwrite("lixo.grd", G);
"""
function gmtwrite(fname::String, data; kwargs...)

	(fname == "") && error("First argument cannot be empty. It must contain the file name to write.")

	d = KW(kwargs)
	help_show_options(d)					# Check if user wants ONLY the HELP mode
	cmd, opt_R = parse_R(d, "")
	cmd = parse_V_params(d, cmd)

	if (isa(data, GMTgrid))
		opt_T = " -Tg"
		fname = fname * parse_grd_format(d)		# If we have format requests
		cmd, = parse_f(d, cmd)
	elseif (isa(data, GMTimage))
		opt_T = " -Ti"
		fname *= parse_grd_format(d)			# If we have format requests
	elseif (isa(data, GMTdataset))
		opt_T = " -Td"
		cmd, = parse_bo(d, cmd)					# Write to binary file
	elseif (isa(data, GMTcpt))
		opt_T = " -Tc"
	elseif (isa(data, GMTps))
		opt_T = " -Tp"
	elseif (isa(data, Array{UInt8}))
		fmt = parse_grd_format(d)				# See if we have format requests
		if (fmt == "")							# If no format, write a dataset
			opt_T = " -Td"
			cmd, = parse_bo(d, cmd)				# Write to binary file
		else
			data = mat2img(data)
			fname *= fmt
			opt_T = " -Ti"
		end
	elseif (isa(data, AbstractArray))
		fmt = parse_grd_format(d)				# See if we have format requests
		if (fmt == "")							# If no format, write a dataset
			opt_T = " -Td"
			cmd, = parse_bo(d, cmd)				# Write to binary file
		else
			data = mat2grid(data)
			fname *= fmt
			opt_T = " -Tg"
		end
	else
		error("Input data of unknown data type $(typeof(data))")
	end
	cmd = cmd * opt_T

	if (opt_T == " -Ti" || opt_T == " -Tg")		# See if we have a mem layout request
		if ((val = find_in_dict(d, [:layout])[1]) !== nothing)
			cmd = (opt_T == " -Ti") ? cmd * " -%" * arg2str(val) : cmd * " -&" * arg2str(val)
		end
	end

	if (dbg_print_cmd(d, cmd) !== nothing)  return "gmtwrite " * fname * cmd  end

	gmt("write " * fname * cmd, data)
	return nothing
end

# -----------------------------------------------------------------------------------------------
function parse_grd_format(d::Dict)::String
# Scan options to fill any of  [=<id>][+s<scale>][+o<offset>][+n<nan>][:<driver>[/<dataType>]
# that control the grid/image output format
	out = ""
	for sym in [:id :gdal :driver]
		if (haskey(d, sym))
			if (sym == :gdal || sym == :driver)
				out = "=gd"
			else
				t = arg2str(d[sym])
				(length(t) != 2) && error(@sprintf("Format code MUST have 2 characters and not %s", t))
				out = "=" * t
			end
			break
		end
	end
	if ((val = find_in_dict(d, [:scale])[1]) !== nothing)   out *= "+s" * arg2str(val)  end
	if ((val = find_in_dict(d, [:offset])[1]) !== nothing)  out *= "+o" * arg2str(val)  end
	if ((val = find_in_dict(d, [:nan :novalue :invalid :missing])[1]) !== nothing)
		out *= "+n" * arg2str(val)
	end
	if ((val = find_in_dict(d, [:driver])[1]) !== nothing)
		out *= ":" * arg2str(val)
		((val = find_in_dict(d, [:datatype])[1]) !== nothing) && (out *= "/" * arg2str(val))
	end
	del_from_dict(d, [:id :gdal])
	return out
end


text:
indices = 0:49
rates = [50, 100, 150, 200, 250, 300, 350, 400]
all_rates = 1:784
max_iter = 200

fw_arguments = (
    line_search=FrankWolfe.Nonconvex(),
    max_iteration=max_iter,
    print_iter=max_iter / 10,
    verbose=true,
    #lazy=true,
    #momentum_iterator=FrankWolfe.ConstantMomentumIterator(0.5),
    #momentum_iterator=FrankWolfe.ExpMomentumIterator(),
    #batch_iterator=FrankWolfe.ConstantBatchIterator(80),
    #batch_iterator=FrankWolfe.IncrementBatchIterator(40, 100),
)

max_stars_repo_name: jmaces/fw-rde

max_issues_repo_name: ZIB-IOL/fw-rde

max_forks_repo_name: ZIB-IOL/fw-rde


text:
export sequence_of_three, alternate_items

function sequence_of_three(start::T) where T
    out = Vector{T}()
    push!(out, start, start+1, start+2)
    return out
end

function alternate_items(nrepeats::Integer, item1::T, item2::T) where T
    out = Vector{T}()
    for _ = 1:nrepeats           # _ is often used for a variable you'll not use again
        push!(out, item1, item2)
    end
    return out
end

"""This version supports missing values in it. but it will be slower...and result in non-concrete types. """
function alternate_items(nrepeats::Integer, item1::Union{Missing, T}, item2::Union{Missing, T}) where T
    out = Vector{Union{Missing, T}}() # Union{Missing, T}
    for _ = 1:nrepeats           # _ is often used for a variable you'll not use again
        push!(out, item1, item2)
    end
    return out
end


text:
module TestLightGBM

import Libdl
using LightGBM
using Test

src_dir = abspath(joinpath(@__DIR__, "..", "..", "src"))

# These set of tests use common libraries of each system to test `find_library` without having to modify env variables
function setup_env()
    output = Dict()

    if Sys.islinux()
        output["sample_lib"] = "libcrypt"
    elseif Sys.isunix()
        output["sample_lib"] = "libpython"
    elseif Sys.iswindows()
        output["sample_lib"] = "netmsg"
    end

    output["ref_lib_lightgbm_path"] = joinpath(src_dir, "lib_lightgbm.$(Libdl.dlext)")

    # where to create a fixture library file (custom path) where such library exists in the syspath
    output["custom_fixture_path"] = joinpath(src_dir, "$(output["sample_lib"]).$(Libdl.dlext)")

    # where to create a fixture library file (custom path) where such library does NOT exist in the syspath
    output["lib_not_on_sys_fixture_path"] = joinpath(src_dir, "lib_not_on_sys.$(Libdl.dlext)")

    return output

end

function teardown(settings::Dict)

    rm(settings["custom_fixture_path"], force=true)
    rm(settings["lib_not_on_sys_fixture_path"], force=true)

    return nothing

end

@testset "find_library" begin

    @testset "find_library works with no system lib" begin

        # Arrange
        settings = setup_env()
        cp(settings["ref_lib_lightgbm_path"], settings["lib_not_on_sys_fixture_path"]) # fake file copied from lightgbm

        # Act
        output = LightGBM.find_library("lib_not_on_sys", [src_dir])

        # Assert
        @test output == joinpath(src_dir, "lib_not_on_sys") # custom path detected (without extension)

        teardown(settings)
    end

    @testset "find_library finds system lib first" begin

        # Arrange
        settings = setup_env()
        cp(settings["ref_lib_lightgbm_path"], settings["custom_fixture_path"]) # fake file copied from lightgbm

        # Act
        output = LightGBM.find_library(settings["sample_lib"], [src_dir])

        # Assert
        @test output == settings["sample_lib"] # sys lib detected

        teardown(settings)
    end

    @testset "find_library finds system lib" begin

        # Arrange
        settings = setup_env()

        # Act
        output = LightGBM.find_library(settings["sample_lib"], [src_dir]) # library should only exist in syspath, not custom path

        # Assert
        @test output == settings["sample_lib"] # sys lib detected

        teardown(settings)
    end

    @testset "find_library returns empty and logs error" begin

        # Arrange
        settings = setup_env()

        # Act and assert
        @test_throws LightGBM.LibraryNotFoundError LightGBM.find_library("lib_that_simply_doesnt_exist", [src_dir])

        teardown(settings)

    end
end

end # module


text:
using FillArrays
using Setfield

"""
    GreeDS(alg=PCA(); threshold=0)
    GreeDS(ncomps; threshold=0, options...)

Performs the greedy disk subtraction (GreeDS) algorithm.

This method is an iterative approach to standard ADI reduction which seeks to minimize over-subtraction by constraining the low-rank matrix approximation from `alg`. By default, uses [`PCA`](@ref). If `ncomps` or other PCA options are provided, they will be passed to the constructor.

!!! note
    The GreeDS algorithm requires fully reconstructing a cube at each iteration, which requires knowing the geometry of the input (full-frame, annulus, etc.) and the corresponding parallactic angles. These angles must be passed as a keyword argument `angles`. In the case of reducing data, e.g. `GreeDS()(cube, angles)` the angles will be passed automatically. It is important to clarify, *these angles should correspond to the reference data in the case of RDI*, e.g. `GreeDS()(cube, angles; ref=ref_cube, angles=ref_angles)`

# Algorithms
The following algorithms work natively with GreeDS: [`PCA`](@ref), [`TPCA`](@ref), and [`NMF`](@ref)

# References
1. [Pairet et al. 2018](https://ui.adsabs.harvard.edu/abs/2018arXiv181201333P) "Reference-less algorithm for circumstellar disks imaging"
2. [Pairet et al. 2020](https://ui.adsabs.harvard.edu/abs/2020arXiv200805170P) "MAYONNAISE: a morphological components analysis pipeline for circumstellar disks and exoplanets imaging in the near infrared"
"""
struct GreeDS{AT} <: ADIAlgorithm
    kernel::AT
    threshold
end
GreeDS(alg=PCA(); threshold=0) = GreeDS(alg, threshold)
GreeDS(ncomps::Int; threshold=0, kwargs...) = GreeDS(PCA(ncomps; kwargs...), threshold=threshold)

const PCALIKE = Union{PCA,TPCA,NMF}

function fit(alg::GreeDS{<:PCALIKE}, data::AbstractArray{T,3}; angles, ref::AbstractArray{S,3}=data, kwargs...) where {T,S}
    target = flatten(ref)
    # get the number of components as a range from the underlying alg
    max_ncomps = get_ncomps(alg.kernel.ncomps, target)
    # use the underlyhing algorithm with a lens for further processing
    f = alg.kernel
    f = @set f.ncomps = 1
    design = fit(f, target)
    R = expand(target .- reconstruct(design))
    reduced = collapse!(R, angles)
    @progress name="GreeDS" for n in 1:max_ncomps
        resid = ref .- expand_rotate(reduced, angles, alg.threshold)
        # use lens to update number of components
        f = @set f.ncomps = n
        # use the `resid` cube as the reference frames for the next reduction
        design = fit(f, target; ref=flatten(resid))
        R = expand(target .- reconstruct(design))
        reduced = collapse!(R, angles)
    end
    # RDI not defined in Pairet 18,20; project onto components
    if ref !== data
        A = design.basis
        weights = flatten(data) * A'
        return LinearDesign(A, weights)
    end
    return design
end

function fit(alg::GreeDS{<:PCALIKE}, data::AnnulusView; angles, ref::AnnulusView=data, kwargs...)
    target = data()
    tmpAnn = copy(data)
    # get the number of components as a range from the underlying alg
    max_ncomps = get_ncomps(alg.kernel.ncomps, target)
    # use the underlyhing algorithm with a lens for further processing
    f = alg.kernel
    f = @set f.ncomps = 1
    design = fit(f, target)
    R = inverse(data, target .- reconstruct(design))
    reduced = collapse!(R, angles)
    @progress name="GreeDS" for n in 1:max_ncomps
        tmpAnn .= ref .- expand_rotate(reduced, angles, alg.threshold)
        resid = tmpAnn()
        # use lens to update number of components
        f = @set f.ncomps = n
        # use the `resid` cube as the reference frames for the next reduction
        design = fit(f, target; ref=resid)
        R = inverse(data, target .- reconstruct(design))
        reduced = collapse!(R, angles)
    end
    if ref !== data
        A = design.basis
        weights = data() * A'
        return LinearDesign(A, weights)
    end
    return design
end

"""
    expand_rotate(frame, angles, threshold; kwargs...)

Takes a frame, expands it into a cube, rotates it clockwise by `angles`, and min-clips at `threshold`. Keyword arguments will be passed to `HCIToolbox.derotate!`.
"""
function expand_rotate(frame, angles, threshold; kwargs...)
    N = length(angles)
    _frame = max.(frame, threshold)
    cube = similar(frame, N, size(frame)...)
    Threads.@threads for idx in axes(cube, 1)
        cube[idx, :, :] .= derotate(_frame, -angles[idx]; kwargs...)
    end
    return cube
end


text:
@doc raw"""
    K, γ, info = glover_mcfarlane(G::AbstractStateSpace{Continuous}, γ = 1.1)

Design a controller for `G` that maximizes the stability margin ϵ = 1/γ with normalized coprime factor uncertainty using the method of Glover and McFarlane
```
γ = 1/ϵ = ||[K;I] inv(I-G*K)*inv(M)||∞
G = inv(M + ΔM)*(N + ΔN)
```
γ is given as a relative factor above γmin and must be greater than 1, i.e., if γ = 1.1, the controller will be designed for γ = 1.1*γmin.

We want γmin (which is always ≥ 1) as small as possible, and we usually require that γmin is less than 4, corresponding to 25% allowed coprime uncertainty.

Performance modeling is incorporated in the design by calling `glover_mcfarlane` on the shaped system `W2*G*W1` and then forming the controller as `W1*K*W2`. Using this formulation, traditional loop shaping can be done on `W2*G*W1`.

# Example:
Example 9.3 from the reference below.
```julia
using RobustAndOptimalControl, ControlSystems, Plots, Test
G = tf(200, [10, 1])*tf(1, [0.05, 1])^2     |> ss
Gd = tf(100, [10, 1])                       |> ss
W1 = tf([1, 2], [1, 1e-6])                  |> ss
K, γ, info = glover_mcfarlane(G, 1.1; W1)
@test info.γmin ≈ 2.34 atol=0.005
Gcl = extended_gangoffour(G, K) # Form closed-loop system

bodeplot([G, info.Gs, G*K], lab=["G" "" "G scaled" "" "Loop transfer"]) |> display
bodeplot(Gcl, lab=["S" "KS" "PS" "T"], plotphase=false) |> display # Plot gang of four

plot( step(Gd*feedback(1, info.Gs), 3), lab="Initial controller")
plot!(step(Gd*feedback(1, G*K), 3), lab="Robustified") |> display

nyquistplot([info.Gs, G*K], ylims=(-2,1), xlims=(-2, 1),
    Ms_circles = 1.5,
    lab = ["Initial controller" "Robustified"],
    title = "Loop transfers with and without robustified controller"
    ) |> display
```

Ref: Sec 9.4.1 of Skogestad, "Multivariable Feedback Control: Analysis and Design"

# Extended help

Skogestad gives the following general advice:
1. Scale the plant outputs and inputs. This is very important for most design
    procedures and is sometimes forgotten. In general, scaling improves the
    conditioning of the design problem, it enables meaningful analysis to be made
    of the robustness properties of the feedback system in the frequency domain,
    and for loop-shaping it can simplify the selection of weights. There are a variety
    of methods available including normalization with respect to the magnitude of
    the maximum or average value of the signal in question. If one is to go straight to a design the following variation has
    proved useful in practice:
    (a) The outputs are scaled such that equal magnitudes of cross-coupling into each
        of the outputs is equally undesirable.
    (b) Each input is scaled by a given percentage (say 10%) of its expected range
        of operation. That is, the inputs are scaled to reflect the relative actuator
        capabilities. An example of this type of scaling is given in the aero-engine
        case study of Chapter 12.
2. Order the inputs and outputs so that the plant is as diagonal as possible. The
    relative gain array [`rga`](@ref) can be useful here. The purpose of this pseudo-diagonalization
    is to ease the design of the pre- and post-compensators which, for simplicity, will
    be chosen to be diagonal.

    Next, we discuss the selection of weights to obtain the shaped plant $G_s = W_2 G W_1$
    where $W_1 = W_p W_a W_g$
3. Select the elements of diagonal pre- and post-compensators $W_p$ and $W_2$ so that
    the singular values of $W_2 G W_p$ are desirable. This would normally mean high
    gain at low frequencies, roll-off rates of approximately 20 dB/decade (a slope of
    about 1) at the desired bandwidth(s), with higher rates at high frequencies. Some
    trial and error is involved here. $W_2$ is usually chosen as a constant, reflecting the
    relative importance of the outputs to be controlled and the other measurements
    being fed back to the controller. For example, if there are feedback measurements
    of two outputs to be controlled and a velocity signal, then $W_2$ might be chosen
    to be `diag([1, 1, 0.1])`, where 0.1 is in the velocity signal channel. $W_p$ contains the
    dynamic shaping. Integral action, for low frequency performance; phase-advance
    for reducing the roll-off rates at crossover, and phase-lag to increase the roll-off
    rates at high frequencies should all be placed in $W_p$ if desired. The weights should
    be chosen so that no unstable hidden modes are created in $G_s$.
5. Optional: Introduce an additional gain matrix $W_g$ cascaded with $W_a$ to provide
    control over actuator usage. $W_g$ is diagonal and is adjusted so that actuator rate
    limits are not exceeded for reference demands and typical disturbances on the
    scaled plant outputs. This requires some trial and error.

6. Robustly stabilize the shaped plant $G_s = W_2 G W_1$ , where $W_1 = W_p W_a W_g$,
    using `glover_mcfarlane`. First, the maximum stability
    margin $ϵ_{max} = 1/γ_{min}$ is calculated. If the margin is too small, $ϵmax < 0.25$, then go back and modify the weights. Otherwise, a γ-suboptimal controller is synthesized. There is usually no advantage to be gained by using the optimal controller. When $ϵ_{max}$ > 0.25
    (respectively $γ_{min}$ < 4) the design is usually successful. In this case, at least
    25% coprime factor uncertainty is allowed, and we also find that the shape of the
    open-loop singular values will not have changed much after robust stabilization.
    A small value of ϵmax indicates that the chosen singular value loop-shapes are
    incompatible with robust stability requirements. That the loop-shapes do not
    change much following robust stabilization if γ is small (ϵ large), is justified
    theoretically in McFarlane and Glover (1990).

7. Analyze the design and if all the specifications are not met make further
    modifications to the weights.
8. Implement the controller. The configuration shown in below has been found
    useful when compared with the conventional set up. This is because
    the references do not directly excite the dynamics of $K$, which can result in large amounts of overshoot (classical derivative kick). The constant prefilter ensures a steady-state gain of 1 between r and y, assuming integral action in $W_1$ or $G$ (note, the K returned by this function has opposite sign compared to that of Skogestad, so we use negative feedback here).

Anti-windup can be added to $W_1$ but putting $W_1$ on Hanus form after the synthesis, see [`hanus`](@ref).

```
       ┌─────────┐      ┌────────┐      ┌────────┐
    r  │         │    us│        │  u   │        │  y
   ───►│(K*W2)(0)├──+──►│   W1   ├─────►│   G    ├────┬──►
       │         │  │-  │        │      │        │    │
       └─────────┘  │   └────────┘      └────────┘    │
                    │                                 │
                    │                                 │
                    │   ┌────────┐      ┌────────┐    │
                    │   │        │  ys  │        │    │
                    └───┤   K    │◄─────┤   W2   │◄───┘
                        │        │      │        │
                        └────────┘      └────────┘
```
"""
function glover_mcfarlane(G::AbstractStateSpace{Continuous}, γ = 1.1; W1=1, W2=1)
    γ > 1 || throw(ArgumentError("γ must be greater than 1"))
    Gs = W2*G*W1
    A,B,C,D = ssdata(Gs)

    R = I + D*D'
    S = I + D'D
    Sl = lu(S)
    # arec(A, B, R, Q, S) solves A'X + XA - (XB+S)R^(-1)(B'X+S') + Q = 0

    Ā = A - B*(S\D'C)
    Z,_ = MatrixEquations.arec(Ā', C', R, B*(Sl\B'))
    X,_ = MatrixEquations.arec(Ā, B, S, C'*(R\C))

    γmin = sqrt(1 + ρ(X*Z))

    γ *= γmin

    L = (1-γ^2)*I + X*Z
    F = -(Sl\(D'C + B'X))
    BK = γ^2*(L'\Z)*C'
    AK = A + B*F + BK*(C + D*F)
    CK = B'X
    DK = -D'
    Ks = -ss(AK, BK, CK, DK)
    Gcl = extended_gangoffour(Gs, Ks)
    imargin, ω = hinfnorm2(Gcl)
    K = W2*Ks*W1
    Gcl = extended_gangoffour(G, K)
    K, γ, (; Gcl, margin = inv(imargin), ω, γmin, Ks, Gs)
end

"Spectral radius"
function ρ(X)
    e = eigvals(X)
    abs(e[end])
end

"""
    `Wh` = hanus(W)

Return `Wh` on Hanus form. `Wh` has twice the number of inputs, where the second half of the inputs are "actual inputs", e.g., potentially saturated. This is used to endow `W` with anti-windup protection.
`W` must have an invertable `D` matrix and be minimum phase.

Ref: Sec 9.4.5 of Skogestad, "Multivariable Feedback Control: Analysis and Design"
"""
function hanus(W)
    A,B,C,D = ssdata(W)
    nu = W.nu
    BD = B/D
    A2 = A - BD*C
    B2 = [0*I(nu) BD]
    D2 = [D 0*I(nu)]
    ss(A2, B2, C, D2)
end

"""
    extended_gangoffour(P, C)

Returns a single statespace system that maps
- `w1` reference or measurement noise
- `w2` load disturbance
to
- `z1` control error
- `z2` control input
```
      z1          z2
      ▲  ┌─────┐  ▲      ┌─────┐
      │  │     │  │      │     │
w1──+─┴─►│  C  ├──┴───+─►│  P  ├─┐
    │    │     │      │  │     │ │
    │    └─────┘      │  └─────┘ │
    │                 w2         │
    └────────────────────────────┘
```

The returned system has the transfer-function matrix
```math
\\begin{bmatrix}
I \\\\ C
\\end{bmatrix} (I + PC)^{-1} \\begin{bmatrix}
I & P
\\end{bmatrix}
```

The gang of four can be plotted like so
```julia
Gcl = extended_gangoffour(G, C) # Form closed-loop system
bodeplot(Gcl, lab=["S" "CS" "PS" "T"], plotphase=false) |> display # Plot gang of four
```
Note, the last output of Gcl is the negative of the `CS` and `PS` transfer functions from `gangoffour2`.
See [`glover_mcfarlane`](@ref) for an extended example. See also [`ncfmargin`](@ref).
"""
function extended_gangoffour(P, C)
    ny,nu = size(P)
    S = feedback(ss(I(ny+nu), P.timeevol), [0*I(ny) P; -C 0*I(nu)], pos_feedback=true)
    Gcl = S + cat(0*I(ny), -I(nu), dims=(1,2))
    Gcl
end

"""
    m, ω = ncfmargin(P, K)

Normalized coprime factor margin, defined has the inverse H∞ norm of
```math
\\begin{bmatrix}
I \\\\ K
\\end{bmatrix} (I + PK)^{-1} \\begin{bmatrix}
I & P
\\end{bmatrix}
```
A margin ≥ 0.25-0.3 is a reasonable for robustness.

If controller `K` stabilizes `P` with margin `m`, then `K` will also stabilize `P̃` if `nugap(P, P̃) < m`.

See also [`extended_gangoffour`](@ref), [`diskmargin`](@ref).
"""
function ncfmargin(P, K)
    Gcl = extended_gangoffour(P, K)
    im, w = hinfnorm2(Gcl)
    inv(im), w
end


text:
using DataStructures

"""
HuffmanTree{M<:Integer, N<:Integer}
    A Huffman encoding of words comprising an array of nodes parents, and an
    array of branching direction for each node.  Array location corresponds to
    the hash value of corresponding word.
"""
struct HuffmanTree{M<:Integer, N<:Integer}
    nodeparent::AbstractArray{M}
    branch::AbstractArray{N}
end

"""
    HuffmanTree(pq, vocab_hash)

Take in a PriorityQueue(pq) and a vocabulary hash, and return a Huffman encoding
of the words in pq.
"""
function HuffmanTree(pq::PriorityQueue, vocab_hash::Dict)
    tmp_hash = copy(vocab_hash)

    # initialize return values
    asize = 2*length(tmp_hash)-1
    nodeparent = zeros(Int, asize)
    branch = zeros(Int, asize)

    # create nodes
    while length(pq) > 2
         _newnode!(pq, tmp_hash, nodeparent, branch)
    end
    _lastnode!(pq, tmp_hash, nodeparent, branch)

    return HuffmanTree(nodeparent, branch)
end

function _newnode!(pq::PriorityQueue, tmp_hash::Dict, nodeparent::Array, branch::Array)
    #create node ids
    (nodea, prioritya) = dequeue_pair!(pq)
    (nodeb, priorityb) = dequeue_pair!(pq)
    idxa = tmp_hash[nodea]
    idxb = tmp_hash[nodeb]

    #add a node to the graph
    idxnode = length(tmp_hash)+1
    prioritynode = prioritya + priorityb
    node = "node-$idxnode" # for debugging
    tmp_hash[node] = idxnode

    #connect the child nodes to the new node
    nodeparent[idxa] = idxnode
    nodeparent[idxb] = idxnode

    #set the binary value for the child nodes
    branch[idxa] = 1
    branch[idxb] = 0

    #enqueue the new node
    enqueue!(pq, node, prioritynode)
end

function _lastnode!(pq::PriorityQueue, tmp_hash::Dict, nodeparent::Array, branch::Array)
    #create node ids
    (nodea, prioritya) = dequeue_pair!(pq)
    (nodeb, priorityb) = dequeue_pair!(pq)
    idxa = tmp_hash[nodea]
    idxb = tmp_hash[nodeb]

    #add a node to the graph
    idxnode = length(tmp_hash)+1
    node = "root"
    tmp_hash[node] = idxnode

    #connect the child nodes to the new node
    nodeparent[idxa] = idxnode
    nodeparent[idxb] = idxnode

    #set the binary value for the child nodes
    branch[idxa] = 1
    branch[idxb] = 0
end


text:
module RandomMatrix

using StatsBase: sample, Weights
import Distributions: mean, var, rand
using Distributions
using Random
using LinearAlgebra
import Polynomials.fit as polyfit
using SparseArrays: spzeros
using Plots: heatmap, cgrad

const S = Union{Tuple,AbstractArray,Distribution,DataType}

export
     randDiagonal, randTriangular, randMatrix,
     randHermitian, randSymmetric, GOE, GUE,
     randStochastic,
     randToeplitz, randHankel,
     randUnitary, randOrthogonal , Haar, COE, CUE, randPermutation

include("MatrixModels.jl")

include("StructuredMatrices.jl")

include("AdjacencyMatrices.jl")

include("SamplingMatrix.jl")

include("Transformations.jl")

include("Spacing.jl")

include("Distributions.jl")

include("Math.jl")

include("Plots.jl")
end # module


text:
module FastGradientMethod

using LinearAlgebra
using OSQP
using Printf
using Roots
using SparseArrays

using Base.Iterators

import Base: iterate

export fastgradientmethod

# Include the various iteration helpers
include( "../iterationUtils/stopping.jl" )
include( "../iterationUtils/loop.jl" )
include( "../iterationUtils/apply.jl" )

"""
```julia
    AbstractStep
```

An abstract super type that is used to specify the step length computation in the
Fast Gradient Method. Implementations of the step length computation should define
a type that inherits from this abstract type.
"""
abstract type AbstractStep end

# The actual FGM algorithm
include( "./fgm_algorithm.jl" )
include( "./fgm_stopping.jl" )
include( "./fgm_stepping.jl")
include( "./fgm_analysis.jl" )

end


text:
cconvert(T::Type, x::VulkanStruct) = x
cconvert(T::Type{<:Ptr}, x::AbstractVector{<:VulkanStruct{false}}) = getproperty.(x, :vks)
cconvert(T::Type{<:Ptr}, x::AbstractVector{<:VulkanStruct{true}}) = (x, getproperty.(x, :vks))
cconvert(T::Type{<:Ptr}, x::VulkanStruct{false}) = Ref(x.vks)
cconvert(T::Type{<:Ptr}, x::VulkanStruct{true}) = (x, Ref(x.vks))
cconvert(T::Type{Ptr{_T}}, x::Vector{<:HighLevelStruct}) where {_T} = cconvert(T, convert(Vector{getproperty(@__MODULE__, Symbol(:_, nameof(eltype(x))))}, x))
cconvert(T::Type{Ptr{_T}}, x::HighLevelStruct) where {_T} = cconvert(T, convert(getproperty(@__MODULE__, Symbol(:_, nameof(typeof(x)))), x))
cconvert(T::Type{<:Ptr}, x::Handle) = x

convert(T::Type{<:Ptr}, x::Handle) = x.vks

unsafe_convert(T::Type, x::VulkanStruct) = x.vks
unsafe_convert(T::Type, x::Tuple{<:VulkanStruct{true}, <:Ref}) = unsafe_convert(T, last(x))
unsafe_convert(T::Type, x::Tuple{<:AbstractVector{<:VulkanStruct{true}}, <:Any}) = unsafe_convert(T, last(x))

"""
    `pointer_length(val)`

Return the length `val` considering it as an array.

Differ from `Base.length` in that `pointer_length(C_NULL) == 0` and that a `RefArray`s return the length of their roots.
"""
function pointer_length end

pointer_length(arr::Ptr{Nothing}) = 0
pointer_length(arr::AbstractArray) = length(arr)
pointer_length(arr::RefArray) = length(arr.roots)
pointer_length(arr::Tuple{<:Any,<:Any}) = length(first(arr))

convert_nonnull(T, val) = convert(T, val)
convert_nonnull(T, val::Ptr{Cvoid}) = val == C_NULL ? val : convert(T, val)


text:
# Default test function
# Terminate when all derivatives fall below a threshold or
#   when derivatives are smaller than a fraction of state
function allDerivPass(integrator, abstol, reltol)
    if DiffEqBase.isinplace(integrator.sol.prob)
        testval = first(get_tmp_cache(integrator))
        DiffEqBase.get_du!(testval, integrator)
        if typeof(integrator.sol.prob) <: DiffEqBase.DiscreteProblem
            @. testval =  testval - integrator.u
        end
    else
        testval = get_du(integrator)
        if typeof(integrator.sol.prob) <: DiffEqBase.DiscreteProblem
            testval =  testval - integrator.u
        end
    end

    if typeof(integrator.u) <: Array
        any(abs(d) > abstol && abs(d) > reltol*abs(u) for (d,abstol, reltol, u) =
           zip(testval, Iterators.cycle(abstol), Iterators.cycle(reltol), integrator.u)) && (return false)
    else
        any((abs.(testval) .> abstol) .& (abs.(testval) .> reltol .* abs.(integrator.u))) && (return false)
    end
    return true
end

# Allow user-defined tolerances and test functions but use sensible defaults
# test function must take integrator, time, followed by absolute
#   and relative tolerance and return true/false
function TerminateSteadyState(abstol = 1e-8, reltol = 1e-6, test = allDerivPass)
    condition = (u, t, integrator) -> test(integrator, abstol, reltol)
    affect! = (integrator) -> terminate!(integrator)
    DiscreteCallback(condition, affect!; save_positions = (true, false))
end

export TerminateSteadyState


text:
# Run forward model

# Parameters to adjust:
#######################

update_cache = true
verbose = true
plotyes = true

@show region = [3, 4, 14, 11, 7][end]

include("rgi-setup.jl")
gl_nrs = get_glaciers(region)[1]

# Run them
##########
errs = []
area_vols = zeros(length(gl_nrs), 4) # number, area, volume-area, volume inferred [mk3

start_t = time()
time_fwd = 0.0
for (i,nr) in enumerate(gl_nrs)
    area_vols[i, 1] = nr
    #nr in merge(faulty,strange) && continue
    println("\n")
    try
        gid = BM.RGIGlacier(nr,region)
        @show gid
        pl = BM.LoadPara(gid; update_cache=update_cache)
        print("Time for load:")
        @time gl,pp,pm,pn,pl = BM.load_glacier(gid,pl)
        area_vols[i, 2] = BM.area(gl) / 1e3^2
        area_vols[i, 3] = BM.volume_area_mean_h(gl) * BM.area(gl) /1e3^3

        print("Time for bands:")
        @time gb,pm = BM.make_bands(gl, pp, pm, pn)

        # gl,gb,pp,pm,pn,pl = BM.init_forward(gid, verbose;
        #                                     update_cache=update_cache)
        pm = BM.MPara(pm, iv_nye=true, iv_dist_exp2=0.01)
        print("Time to run forward model:"); gc()
        @time time_fwd += @elapsed fwdsol = BM.fwdm(gb, pp, pm, pn, gb.bdot-gb.dhdt, gb.fsl, gb.temp)

        BM.postproc1d!(fwdsol)
        area_vols[i, 4] = BM.volume(fwdsol)/1e3^3

        if plotyes
            print("Time to plot:")
            @time Plots.savefig(BM.plot_run(fwdsol, reuse=true, plot2d=[:h,:iv,:ivlog][1]),
                                "figs/$(gid.id).png")
        end
    catch e
        e==InterruptException() && throw(e)
        @show e
        push!(errs, (nr, e))
    end
end
dt = round(Int, (time() - start_t)/60) # time in min
time_fwd = round(Int, time_fwd)
dirname = infos[region][1]
writecsv("figs/err-$dirname-t$dt-f$(time_fwd).csv", errs)
writecsv("figs/vol-$dirname-t$dt-f$(time_fwd).csv", area_vols)

if false
    gid = BM.RGIGlacier(nr,region);
    pl = BM.LoadPara(gid; update_cache=update_cache);
    @time gl,pp,pm,pn,pl = BM.load_glacier(gid,pl);
    @time gb,pm = BM.make_bands(gl, pp, pm, pn);
    pm = BM.MPara(pm, iv_nye=true, iv_dist_exp2=0.01);
    @time time_fwd += @elapsed fwdsol = BM.fwdm(gb, pp, pm, pn, gb.bdot-gb.dhdt, gb.fsl, gb.temp);
    BM.postproc1d!(fwdsol);
    BM.plot_run(fwdsol, reuse=true, plot2d=[:h,:iv,:ivlog][1])
end


text:
module Printing
using Printf

# This module is intended to provide fancy macros to display warning

export @inforx, @warnrx;
export @infotx, @warntx;
# ----------------------------------------------------
# --- Fancy prints
# ----------------------------------------------------
# To print fancy message with different colors with Tx and Rx
function customPrint(str,handler;style...)
    msglines = split(chomp(str), '\n')
    printstyled("┌",handler,": ";style...)
    println(msglines[1])
    for i in 2:length(msglines)
        (i == length(msglines)) ? symb="└ " : symb = "|";
        printstyled(symb;style...);
        println(msglines[i]);
    end
end
# define macro for printing Rx info
macro inforx(str)
    quote
        customPrint($(esc(str)),"Rx";bold=true,color=:light_green)
    end
end
# define macro for printing Rx warning
macro warnrx(str)
    quote
        customPrint($(esc(str)),"Rx Warning";bold=true,color=:light_yellow)
    end
end
# define macro for printing Tx info
macro infotx(str)
    quote
        customPrint($(esc(str)),"Tx";bold=true,color=:light_blue)
    end
end
# define macro for printing Tx warning
macro warntx(str)
    quote
        customPrint($(esc(str)),"Tx Warning";bold=true,color=:light_yellow)
    end
end

end


text:
abstract RedundantWaveletClass

immutable Gammatone <: RedundantWaveletClass
    order::Int
end
Gammatone() = Gammatone(4)
isdyadic(::Gammatone) = false

immutable MexicanHat <: RedundantWaveletClass end
isdyadic(::MexicanHat) = true
issteerable(::MexicanHat) = false

uncertainty(::MexicanHat) = 2.0

immutable Morlet <: RedundantWaveletClass end

"""In the special case `n_filters_per_octave=1`, we manually set `ξ=0.39`. That
is more accurate with the Littlewood-Paley energy conservation criterion than
the generic fallback `ξ=0.4`, which is only valid when the wavelet has a
symmetric profile in the Fourier domain. This is no longer the case for
n_filters_per_octave==max_qualityfactor==1 as the Morlet low-frequency corrective
term is no longer negligible."""
default_motherfrequency(class::Morlet, n_filters_per_octave) =
    n_filters_per_octave==1 ? 0.39 : inv(3.0 - exp2(-1.0/n_filters_per_octave))

isdyadic(::Morlet) = false
issteerable(::Morlet) = true

"""
By neglecting the low-frequency corrective term, we write the Morlet wavelet as
a Gaussian of variance σ in the Fourier domain. Its 3 dB bandwidth, defined as
the full width at half maximum (FWHM) of the squared magnitude in the Fourier
domain, is then equal to b = 2σ*sqrt(log(2)).

Therefore, for a given center frequency ω and a quality factor Q, the variance
σ of the Gaussian is equal to σ = b / (2*sqrt(log(2))). In the spatial domain,
this amounts to a Gabor wavelet (a Gaussian modulated by a sine wave, without
any low-frequency corrective term) of variance 1/σ. Its spatial scale (FWTM)
is equal to s = 2*sqrt(log(10))/σ. We conclude that the uncertainty
constant of the Morlet wavelet, defined as the (scale*bandwidth) constant,
is equal to
    h = b*s = sqrt(log(10)/log(2)) = 1.8226...
"""
uncertainty(class::Morlet) = sqrt(log(10.0) / log(2.0))


text:
function get_ball_contact_jacobian(q, body)
    xₜ = q[1]
    yₜ = q[2]
    θ  = q[3]
    D = body.l
    rad = D/2.

    xyc = [xₜ, yₜ-rad]
    ϕ = xyc[2]

    n = [0, 1]
    r = xyc - [xₜ, yₜ]
    rₓnᵤ = r[1]*n[2]; rᵤnₓ = r[2]*n[1]
    rₓnₓ = r[1]*n[1]; rᵤnᵤ = r[2]*n[2]

    J = [[n[2] n[1]];
         [-n[1] n[2]];
         [-rₓnₓ-rᵤnᵤ rₓnᵤ-rᵤnₓ]]

    return J, ϕ
end

function get_block_contact_jacobian(q, body)
    xₜ = q[1]
    yₜ = q[2]
    θ  = q[3]
    L = body.l

    l = sqrt(2* (0.5*L)^2)
    a = [xₜ+l*cos(θ+3π/4), yₜ+l*sin(θ+3π/4)]
    b = [xₜ+l*cos(θ+5π/4), yₜ+l*sin(θ+5π/4)]
    c = [xₜ+l*cos(θ+π/4), yₜ+l*sin(θ+π/4)]
    d = [xₜ+l*cos(θ+7π/4), yₜ+l*sin(θ+7π/4)]

    corners = [a, b, c, d]
    min_index = argmin([a[2], b[2], c[2], d[2]])
    xyc = corners[min_index]
    ϕ = xyc[2]

    n = [0, 1]
    r = xyc - [xₜ, yₜ]
    rₓnᵤ = r[1]*n[2]; rᵤnₓ = r[2]*n[1]
    rₓnₓ = r[1]*n[1]; rᵤnᵤ = r[2]*n[2]

    J = [[n[2] n[1]];
         [-n[1] n[2]];
         [-rₓnₓ-rᵤnᵤ rₓnᵤ-rᵤnₓ]]

    return J, ϕ
end


text:
"""
    binomial_rng(p, n, shape=1; seed=nothing)

Generate a `shape` element array of random variables from a Binomial(`p`, `n`) distribution. Optionally you can set a specific seed.

# Notes

The Binomial(x, n, p) distribution describes the total number of successes in a sequence of n Bernoulli(p) trials.

The pdf is given:

``f(x,n,p) = \\binom{n}{x} p^x (1-p)^{n-x} \\quad x = 0,1,\\dots, n``

# Examples

```julia-repl
julia> binomial_rng(.3, 10)
1×1 Matrix{Int64}:
 3

julia> binomial_rng(.3, 10, (2,2))
2×2×1 Array{Int64, 3}:
[:, :, 1] =
 2  1
 2  2
```

# References

D.P. Kroese, T. Taimre, Z.I. Botev. Handbook of Monte Carlo Methods.
  Wiley Series in Probability and Statistics, John Wiley & Sons, New York, 2011.
"""
function binomial_rng(p::Real, n::Int, shape::Union{Int, Tuple{Vararg{Int}}}=1; seed::Union{Int, Nothing}=nothing)
    check_p(p)
    U = bernoulli_rng(p, (shape..., n), seed=seed)
    X = sum(U, dims=ndims(U))  # want sum over final or `n` dimension
    return X
end


text:
"""
    AcrobotEnv(;kwargs...)

# Keyword arguments
- `T = Float64`
- `link_length_a = T(1.)`
- `link_length_b = T(1.)`
- `link_mass_a = T(1.)`
- `link_mass_b = T(1.)`
- `link_com_pos_a = T(0.5)`
- `link_com_pos_b = T(0.5)`
- `link_moi = T(1.)`
- `max_vel_a = T(4 * π)`
- `max_vel_b = T(9 * π)`
- `g = T(9.8)`
- `dt = T(0.2)`
- `max_steps = 200`
- `book_or_nips = 'book'`
- `avail_torque = [T(-1.), T(0.), T(1.)]`
"""
function AcrobotEnv(;
    T=Float64,
    link_length_a=T(1.0),
    link_length_b=T(1.0),
    link_mass_a=T(1.0),
    link_mass_b=T(1.0),
    link_com_pos_a=T(0.5),
    link_com_pos_b=T(0.5),
    link_moi=T(1.0),
    max_torque_noise=T(0.0),
    max_vel_a=T(4 * π),
    max_vel_b=T(9 * π),
    g=T(9.8),
    dt=T(0.2),
    max_steps=200,
    rng=Random.GLOBAL_RNG,
    book_or_nips="book",
    avail_torque=[T(-1.0), T(0.0), T(1.0)],
)

    params = AcrobotEnvParams{T}(
        link_length_a,
        link_length_b,
        link_mass_a,
        link_mass_b,
        link_com_pos_a,
        link_com_pos_b,
        link_moi,
        max_torque_noise,
        max_vel_a,
        max_vel_b,
        g,
        dt,
        max_steps,
    )

    env = AcrobotEnv(
        params,
        zeros(T, 4),
        0,
        false,
        0,
        rng,
        T(0.0),
        book_or_nips,
        [T(-1.0), T(0.0), T(1.0)],
    )
    reset!(env)
    env
end

acrobot_observation(s) = [cos(s[1]), sin(s[1]), cos(s[2]), sin(s[2]), s[3], s[4]]

RLBase.action_space(env::AcrobotEnv) = Base.OneTo(3)

function RLBase.state_space(env::AcrobotEnv{T}) where {T}
    high = [1.0, 1.0, 1.0, 1.0, env.params.max_vel_a, env.params.max_vel_b]
    Space(ClosedInterval{T}.(-high, high))
end

RLBase.is_terminated(env::AcrobotEnv) = env.done
RLBase.state(env::AcrobotEnv) = acrobot_observation(env.state)
RLBase.reward(env::AcrobotEnv) = env.reward

function RLBase.reset!(env::AcrobotEnv{T}) where {T <: Number}
    env.state[:] = T(0.1) * rand(env.rng, T, 4) .- T(0.05)
    env.t = 0
    env.action = 2
    env.done = false
    env.reward = -1
    nothing
end

# governing equations as per python gym
function (env::AcrobotEnv{T})(a) where {T <: Number}
    env.action = a
    env.t += 1
    torque = env.avail_torque[a]

    # noise to the force action
    noise_range = env.params.max_torque_noise
    if noise_range > 0
        torque += T(2.0 * noise_range) * rand(env.rng, T, 1) .- T(noise_range)
    end

    # augmented state for derivative function
    s_augmented = [env.state..., torque]

    ode = OrdinaryDiffEq.ODEProblem(dsdt, s_augmented, (0.0, env.params.dt), env)
    ns = OrdinaryDiffEq.solve(ode, OrdinaryDiffEq.RK4())
    # only care about final timestep of integration returned by integrator
    ns = ns.u[end]
    ns = ns[1:4]  # omit action

    # wrap the solution
    ns[1] = wrap(ns[1], -π, π)
    ns[2] = wrap(ns[2], -π, π)
    ns[3] = bound(ns[3], -env.params.max_vel_a, env.params.max_vel_a)
    ns[4] = bound(ns[4], -env.params.max_vel_b, env.params.max_vel_b)
    env.state = ns
    # termination criterion
    succeeded = -cos(ns[1]) - cos(ns[2] + ns[1]) > 1.0
    env.done = succeeded || env.t > env.params.max_steps
    env.reward = succeeded ? 0.0 : -1.0
    nothing
end

function dsdt(du, s_augmented, env::AcrobotEnv, t)
    # extract params
    m1 = env.params.link_mass_a
    m2 = env.params.link_mass_b
    l1 = env.params.link_length_a
    lc1 = env.params.link_com_pos_a
    lc2 = env.params.link_com_pos_b
    I1 = env.params.link_moi
    I2 = env.params.link_moi
    g = env.params.g

    # extract action and state
    a = s_augmented[end]
    s = s_augmented[1:end - 1]

    # writing in standard form
    theta1 = s[1]
    theta2 = s[2]
    dtheta1 = s[3]
    dtheta2 = s[4]
    ddtheta1 = 0.0
    ddtheta2 = 0.0

    # governing equations
    d1 = (m1 * lc1^2 + m2 * (l1^2 + lc2^2 + 2 * l1 * lc2 * cos(theta2)) + I1 + I2)
    d2 = m2 * (lc2^2 + l1 * lc2 * cos(theta2)) + I2
    phi2 = m2 * lc2 * g * cos(theta1 + theta2 - pi / 2.0)
    phi1 = (
        -m2 * l1 * lc2 * dtheta2^2 * sin(theta2) -
        2 * m2 * l1 * lc2 * dtheta2 * dtheta1 * sin(theta2) +
        (m1 * lc1 + m2 * l1) * g * cos(theta1 - pi / 2) +
        phi2
    )
    if env.book_or_nips == "nips"
        # the following line is consistent with the description in the
        # paper
        ddtheta2 = ((a + d2 / d1 * phi1 - phi2) / (m2 * lc2^2 + I2 - d2^2 / d1))
    elseif env.book_or_nips == "book"
        # the following line is consistent with the java implementation and the
        # book
        ddtheta2 = (
            (a + d2 / d1 * phi1 - m2 * l1 * lc2 * dtheta1^2 * sin(theta2) - phi2) /
            (m2 * lc2^2 + I2 - d2^2 / d1)
        )
        ddtheta1 = -(d2 * ddtheta2 + phi1) / d1
    end

    # return the values
    du[1] = dtheta1
    du[2] = dtheta2
    du[3] = ddtheta1
    du[4] = ddtheta2
    du[5] = 0.0
end

Random.seed!(env::AcrobotEnv, seed) = Random.seed!(env.rng, seed)

# wrap as per python gym
function wrap(x, m, M)
    """
    Wraps ``x`` so m <= x <= M; but unlike ``bound()`` which
    truncates, ``wrap()`` wraps x around the coordinate system defined by m,M.\n
    For example, m = -180, M = 180 (degrees), x = 360 --> returns 0.
    Args:
        x: a scalar
        m: minimum possible value in range
        M: maximum possible value in range
    Returns:
        x: a scalar, wrapped
    """
    diff = M - m
    while x > M
        x = x - diff
    end
    while x < m
        x = x + diff
    end
return x
end

function bound(x, m, M)
    """Either have m as scalar, so bound(x,m,M) which returns m <= x <= M *OR*
    have m as length 2 vector, bound(x,m, <IGNORED>) returns m[0] <= x <= m[1].
    Args:
        x: scalar
    Returns:
        x: scalar, bound between min (m) and Max (M)
    """

    # bound x between min (m) and Max (M)
    return min(max(x, m), M)
end


text:
using OptimalTransport

using Distances
using Distributions

using Random
using Test

Random.seed!(100)

@testset "wasserstein.jl" begin
    @testset "p2distance" begin
        for metric in (Euclidean(), Euclidean(0.01), TotalVariation())
            @test OptimalTransport.p2distance(metric, Val(1)) === metric
        end

        @test OptimalTransport.p2distance(Euclidean(), Val(2)) == SqEuclidean()
        @test OptimalTransport.p2distance(Euclidean(0.01), Val(2)) == SqEuclidean(0.01)

        p = randexp()
        x = randn(10)
        y = randn(10)
        for metric in (Euclidean(), TotalVariation())
            for _p in (p, Val(p))
                pmetric = OptimalTransport.p2distance(metric, _p)
                @test pmetric(x, y) ≈ metric(x, y)^p
            end
        end
    end

    @testset "prt" begin
        x = randexp()
        for p in (1, 2, 3, randexp())
            @test OptimalTransport.prt(x, p) ≈ x^(1 / p)
            @test OptimalTransport.prt(x, Val(p)) ≈ x^(1 / p)
        end
    end

    @testset "wasserstein" begin
        μ = Normal(randn(), randexp())
        ν = Normal(randn(), randexp())
        for p in (1, 2, 3, randexp()), metric in (Euclidean(), TotalVariation())
            for _p in (p, Val(p))
                # without additional keyword arguments
                w = wasserstein(μ, ν; p=_p, metric=metric)
                @test w ≈ ot_cost((x, y) -> metric(x, y)^p, μ, ν)^(1 / p)

                # with pre-computed plan (random `ν` ensures that plan is used)
                T = ot_plan((x, y) -> metric(x, y)^p, μ, ν)
                w2 = wasserstein(μ, Normal(randn(), rand()); p=_p, metric=metric, plan=T)
                @test w ≈ w2
            end
        end

        # check that `Euclidean` is the default `metric`
        for p in (1, 2, 3, randexp()), _p in (p, Val(p))
            w = wasserstein(μ, ν; p=_p)
            @test w ≈ wasserstein(μ, ν; p=_p, metric=Euclidean())
        end

        # check that `Val(1)` is the default `p`
        for metric in (Euclidean(), TotalVariation())
            w = wasserstein(μ, ν; metric=metric)
            @test w ≈ wasserstein(μ, ν; p=Val(1), metric=metric)
        end
    end

    @testset "squared2wasserstein" begin
        μ = Normal(randn(), randexp())
        ν = Normal(randn(), randexp())
        for metric in (Euclidean(), TotalVariation())
            # without additional keyword arguments
            w = squared2wasserstein(μ, ν; metric=metric)
            @test w ≈ ot_cost((x, y) -> metric(x, y)^2, μ, ν)

            # with pre-computed plan (random `ν` ensures that plan is used)
            T = ot_plan((x, y) -> metric(x, y)^2, μ, ν)
            w2 = squared2wasserstein(μ, Normal(randn(), rand()); metric=metric, plan=T)
            @test w ≈ w2
        end

        # check that `Euclidean` is the default `metric`
        w = squared2wasserstein(μ, ν)
        @test w ≈ squared2wasserstein(μ, ν; metric=Euclidean())
    end
end