text:
"""
Bridges `CP.MinimumAmong` to MILP formulations, by the means of big-M
constraints.
"""
struct MinimumAmong2MILPBridge{T} <: MOIBC.AbstractBridge
    vars::Vector{MOI.VariableIndex}
    vars_bin::Vector{MOI.ConstraintIndex{MOI.VariableIndex, MOI.ZeroOne}}
    cons_lt::Vector{MOI.ConstraintIndex{MOI.ScalarAffineFunction{T}, MOI.LessThan{T}}}
    cons_gt::Vector{MOI.ConstraintIndex{MOI.ScalarAffineFunction{T}, MOI.GreaterThan{T}}}
    con_choose_one::MOI.ConstraintIndex{MOI.ScalarAffineFunction{T}, MOI.EqualTo{T}}
end

function MOIBC.bridge_constraint(
    ::Type{MinimumAmong2MILPBridge{T}},
    model,
    f::MOI.VectorOfVariables,
    s::CP.MinimumAmong,
) where {T}
    return MOIBC.bridge_constraint(
        MinimumAmong2MILPBridge{T},
        model,
        MOI.VectorAffineFunction{T}(f),
        s,
    )
end

function MOIBC.bridge_constraint(
    ::Type{MinimumAmong2MILPBridge{T}},
    model,
    f::MOI.VectorAffineFunction{T},
    s::CP.MinimumAmong,
) where {T}
    f_scalars = MOIU.scalarize(f)
    dim = MOI.output_dimension(f)
    n_array = dim - 1

    # For this formulation work, both lower and upper bounds are required on
    # the argument of the absolute value.
    for i in 1:n_array
        @assert CP.has_lower_bound(model, f_scalars[i + 1])
        @assert CP.has_upper_bound(model, f_scalars[i + 1])
    end

    # New variables.
    vars, vars_bin = MOI.add_constrained_variables(model, [MOI.ZeroOne() for _ in 1:n_array])

    # The minimum is at most every other value.
    cons_lt = MOI.ConstraintIndex{MOI.ScalarAffineFunction{T}, MOI.LessThan{T}}[
        MOI.add_constraint(
            model,
            f_scalars[1] - f_scalars[i + 1],
            MOI.LessThan(zero(T))
        )
        for i in 1:n_array
    ]

    # The minimum is greater than only one value (i.e. equal to that one).
    big_m = T[
        abs(
            CP.get_upper_bound(model, f_scalars[i + 1]) -
            CP.get_lower_bound(model, f_scalars[i + 1])
        )
        for i in 1:n_array
    ]
    cons_gt = MOI.ConstraintIndex{MOI.ScalarAffineFunction{T}, MOI.GreaterThan{T}}[
        MOI.add_constraint(
            model,
            f_scalars[1] - f_scalars[i + 1] - big_m[i] * (one(T) - vars[i]),
            MOI.GreaterThan(zero(T))
        )
        for i in 1:n_array
    ]

    # At most one such inequality holds.
    con_choose_one = MOI.add_constraint(
        model,
        sum(one(T) * vars),
        MOI.EqualTo(one(T))
    )

    return MinimumAmong2MILPBridge(vars, vars_bin, cons_lt, cons_gt, con_choose_one)
end

function MOI.supports_constraint(
    ::Type{MinimumAmong2MILPBridge{T}},
    ::Union{Type{MOI.VectorOfVariables}, Type{MOI.VectorAffineFunction{T}}},
    ::Type{CP.MinimumAmong},
) where {T}
    return true
end

function MOIB.added_constrained_variable_types(::Type{MinimumAmong2MILPBridge{T}}) where {T}
    return [(MOI.ZeroOne,)]
end

function MOIB.added_constraint_types(::Type{MinimumAmong2MILPBridge{T}}) where {T}
    return [
        (MOI.ScalarAffineFunction{T}, MOI.LessThan{T}),
        (MOI.ScalarAffineFunction{T}, MOI.GreaterThan{T}),
        (MOI.ScalarAffineFunction{T}, MOI.EqualTo{T}),
    ]
end

function MOI.get(b::MinimumAmong2MILPBridge, ::MOI.NumberOfVariables)
    return length(b.vars)
end

function MOI.get(
    b::MinimumAmong2MILPBridge{T},
    ::MOI.NumberOfConstraints{
        MOI.VariableIndex, MOI.ZeroOne,
    },
) where {T}
    return length(b.vars_bin)
end

function MOI.get(
    ::MinimumAmong2MILPBridge{T},
    ::MOI.NumberOfConstraints{
        MOI.ScalarAffineFunction{T}, MOI.EqualTo{T},
    },
) where {T}
    return 1
end

function MOI.get(
    b::MinimumAmong2MILPBridge{T},
    ::MOI.NumberOfConstraints{
        MOI.ScalarAffineFunction{T}, MOI.LessThan{T},
    },
) where {T}
    return length(b.cons_lt)
end

function MOI.get(
    b::MinimumAmong2MILPBridge{T},
    ::MOI.NumberOfConstraints{
        MOI.ScalarAffineFunction{T}, MOI.GreaterThan{T},
    },
) where {T}
    return length(b.cons_gt)
end

function MOI.get(
    b::MinimumAmong2MILPBridge{T},
    ::MOI.ListOfVariableIndices,
) where {T}
    return copy(b.vars)
end

function MOI.get(
    b::MinimumAmong2MILPBridge{T},
    ::MOI.ListOfConstraintIndices{
        MOI.VariableIndex, MOI.ZeroOne,
    },
) where {T}
    return copy(b.vars_bin)
end

function MOI.get(
    b::MinimumAmong2MILPBridge{T},
    ::MOI.ListOfConstraintIndices{
        MOI.ScalarAffineFunction{T}, MOI.EqualTo{T},
    },
) where {T}
    return [b.con_choose_one]
end

function MOI.get(
    b::MinimumAmong2MILPBridge{T},
    ::MOI.ListOfConstraintIndices{
        MOI.ScalarAffineFunction{T}, MOI.LessThan{T},
    },
) where {T}
    return copy(b.cons_lt)
end

function MOI.get(
    b::MinimumAmong2MILPBridge{T},
    ::MOI.ListOfConstraintIndices{
        MOI.ScalarAffineFunction{T}, MOI.GreaterThan{T},
    },
) where {T}
    return copy(b.cons_gt)
end


text:
using Plots
gr()
n = 3
while n < 24
    t = 0:(pi/n):20
    f(t) = 5*sin.(3*t)
    display(
    plot(f,t,
    seriestype = :scatter,
    title = "Response Sampling",
    xlabel = "time (s)",
    ylabel = "Transferred Output (m,F)"
    ))
    global n *= 2

end

ext: jl


text:
"""
Fast MPC (modified for implicit dynamics (f(xt, ut, xt+1) = 0))
https://web.stanford.edu/~boyd/papers/pdf/fast_mpc.pdf
"""

mutable struct NewtonStructureSolver{N,M,NQ,NN,NM,NQNQ,NQM,T,AA,AB,AC,BA,QA,QB,QV,QAI,QBI,QVI,RA}
	"""
	[S  C'; C] [Δz; Δν] = r

	qa = [q0, q1, ..., qT-1]
	qb = [q1, q2, ..., qT]
	"""

	# dimensions
	n::Int  # state
	m::Int  # control
	nq::Int # configuration
	H::Int  # horizon
	nz::Int # decision variables
	nd::Int # dual variables

	# indices
	# [u1, x2, u2, ..., xT]
	x_idx::Vector{Vector{Int}}
	u_idx::Vector{Vector{Int}}
	qa_idx::Vector{Vector{Int}}
	qb_idx::Vector{Vector{Int}}
	n_idx::Vector{Vector{Int}}

	# trajectories x = [qa; qb]
	u::Vector{SVector{M,T}}
	qa::Vector{SVector{NQ,T}}
	qb::Vector{SVector{NQ,T}}

	ν1::Vector{SVector{NQ,T}}
	ν2::Vector{SVector{NQ,T}}

	# candidate trajectories
	u_cand::Vector{SVector{M,T}}
	qa_cand::Vector{SVector{NQ,T}}
	qb_cand::Vector{SVector{NQ,T}}

	ν1_cand::Vector{SVector{NQ,T}}
	ν2_cand::Vector{SVector{NQ,T}}

	# dynamics Jacobians
	Aa::Vector{AA} # ∂ft/∂qt-1
	Ab::Vector{AB} # ∂ft/∂qt
	Ac::Vector{AC} # ∂ft/∂qt+1
	Ba::Vector{BA} # ∂ft/∂ut

	# objective
	q_ref::Vector{SVector{NQ,T}}
	u_ref::Vector{SVector{M,T}}

	Qa::Vector{QA}
	Qb::Vector{QB}
	Qv::Vector{QV}
	Ra::Vector{RA}

	# objective inverse
	Q̃a::Vector{QAI}
	Q̃b::Vector{QBI}
	Q̃v::Vector{QVI}
	R̃a::Vector{RA}

	# Y = C * inv(S) * C' = [Y11 Y12...;Y21 Y22...;...;...YT-1T-1]
	Yii::Vector{Matrix{T}}
	Yiis::Vector{SMatrix{N,N,T,NN}}
	Yij::Vector{Matrix{T}}
	Yijs::Vector{SMatrix{N,N,T,NN}}

	# Yii = [Yiia Yiib; Yiic Yiid]
	Yiia::Vector{SMatrix{NQ,NQ,T,NQNQ}}
	Yiib::Vector{SMatrix{NQ,NQ,T,NQNQ}}
	Yiic::Vector{SMatrix{NQ,NQ,T,NQNQ}}
	Yiid::Vector{SMatrix{NQ,NQ,T,NQNQ}}

	Yiiav::Vector{SubArray{T,2,Matrix{T},Tuple{Vector{Int},Vector{Int}},false}}
	Yiibv::Vector{SubArray{T,2,Matrix{T},Tuple{Vector{Int},Vector{Int}},false}}
	Yiicv::Vector{SubArray{T,2,Matrix{T},Tuple{Vector{Int},Vector{Int}},false}}
	Yiidv::Vector{SubArray{T,2,Matrix{T},Tuple{Vector{Int},Vector{Int}},false}}

	# Yij = [Yija Yijb; Yijc Yijd]
	Yija::Vector{SMatrix{NQ,NQ,T,NQNQ}}
	Yijb::Vector{SMatrix{NQ,NQ,T,NQNQ}}
	Yijc::Vector{SMatrix{NQ,NQ,T,NQNQ}}
	Yijd::Vector{SMatrix{NQ,NQ,T,NQNQ}}

	Yijav::Vector{SubArray{T,2,Matrix{T},Tuple{Vector{Int},Vector{Int}},false}}
	Yijbv::Vector{SubArray{T,2,Matrix{T},Tuple{Vector{Int},Vector{Int}},false}}
	Yijcv::Vector{SubArray{T,2,Matrix{T},Tuple{Vector{Int},Vector{Int}},false}}
	Yijdv::Vector{SubArray{T,2,Matrix{T},Tuple{Vector{Int},Vector{Int}},false}}

	# L = cholesky(Y)
	Liis::Vector{LowerTriangular{T,SMatrix{N,N,T,NN}}}
	Ljis::Vector{SMatrix{N,N,T,NN}}

	# residual: r = [rlag; rdyn]
	# rlag = [rlagu; rlagqa; rlagqb]
	rlagu::Vector{SVector{M,T}}
	rlagqa::Vector{SVector{NQ,T}}
	rlagqb::Vector{SVector{NQ,T}}

	# rdyn = [rdyn1; rdyn2]
	rdyn1::Vector{SVector{NQ,T}}
	rdyn2::Vector{SVector{NQ,T}}

	# β = -rdyn + C * inv(S) * rlag = [β1..., βT-1]; β1 = [β11; β21], ...
	βn::Vector{SVector{N,T}}
	β1::Vector{SVector{NQ,T}}
	β2::Vector{SVector{NQ,T}}

	# y = L \ β
	y::Vector{SVector{N,T}}

	# Δν = L' \ y
	Δνn::Vector{SVector{N,T}}
	Δν1::Vector{SVector{NQ,T}}
	Δν2::Vector{SVector{NQ,T}}

	# Δz = inv(S) * (rlag - C' * Δν)
	Δu::Vector{SVector{M,T}}
	Δqa::Vector{SVector{NQ,T}}
	Δqb::Vector{SVector{NQ,T}}

	# temporary arrays
	tmp_nn::SMatrix{N,N,T,NN}
	tmp_nn2::SMatrix{N,N,T,NN}
	tmp_nqnq::SMatrix{NQ,NQ,T,NQNQ}
	tmp_nqnq2::SMatrix{NQ,NQ,T,NQNQ}
	tmp_nm::SMatrix{N,M,T,NM}
	tmp_nqm::SMatrix{NQ,M,T,NQM}
	tmp_nq::SVector{NQ,T}

	# idx
	idx_nq::SVector{NQ, Int}
	idx_nq2::SVector{NQ, Int}

	# regularization
	In::Diagonal{T, SVector{N,T}}
	Inq::Diagonal{T, SVector{NQ,T}}
	Im::Diagonal{T, SVector{M,T}}

	# data
	θ::Vector{Vector{T}}

	# dual fill
	dual_fill::SVector{NQ,T}

	# options
	opts::NewtonOptions{T}
end

function newton_structure_solver(nq::Int, m::Int, H::Int; opts = NewtonOptions())
	# dimensions
	n = 2 * nq
	nz = m * (H - 1) + n * (H - 1)
	nd = n * (H - 1)

	# indices
	u_idx = [collect((t - 1) * (m + n) .+ (1:m)) for t = 1:H-1]
	x_idx = [collect((t - 1) * (m + n) + m .+ (1:n)) for t = 1:H-1]
	qa_idx = [collect(x_idx[t][1:nq]) for t = 1:H-1]
	qb_idx = [collect(x_idx[t][nq .+ (1:nq)]) for t = 1:H-1]
	n_idx = [collect((t - 1) * n .+ (1:n)) for t = 1:H-1]

	# trajectories
	u = [SVector{m}(zeros(m)) for t = 1:H-1]
	qa = [SVector{nq}(zeros(nq)) for t = 1:H]
	qb = [SVector{nq}(zeros(nq)) for t = 1:H]

	ν1 = [SVector{nq}(zeros(nq)) for t = 1:H-1]
	ν2 = [SVector{nq}(zeros(nq)) for t = 1:H-1]

	# candidate trajectories
	u_cand = [SVector{m}(zeros(m)) for t = 1:H-1]
	qa_cand = [SVector{nq}(zeros(nq)) for t = 1:H]
	qb_cand = [SVector{nq}(zeros(nq)) for t = 1:H]

	ν1_cand = [SVector{nq}(zeros(nq)) for t = 1:H-1]
	ν2_cand = [SVector{nq}(zeros(nq)) for t = 1:H-1]

	# dynamics Jacobians
	Aa = [SMatrix{nq, nq}(zeros(nq, nq)) for t = 1:H-1]
	Ab = [SMatrix{nq, nq}(zeros(nq, nq)) for t = 1:H-1]
	Ac = [SMatrix{nq, nq}(Diagonal(ones(nq))) for t = 1:H-1]
	Ba = [SMatrix{nq,m}(zeros(nq, m)) for t = 1:H-1]

	# objective
	q_ref = [SVector{nq}(zeros(nq)) for t = 1:H+1]
	u_ref = [SVector{m}(zeros(m)) for t = 1:H-1]

	Qa = [SMatrix{nq,nq}(Diagonal(ones(nq))) for t = 1:H]
	Qb = [SMatrix{nq,nq}(Diagonal(ones(nq))) for t = 1:H]
	Qv = [SMatrix{nq,nq}(Diagonal(0.1 * ones(nq))) for t = 1:H]
	Ra = [SMatrix{m,m}(Diagonal(ones(m))) for t = 1:H-1]

	# objective inverse
	Q̃ = [inv(Array([Qa[t] Qv[t]; Qv[t]' Qb[t]])) for t = 1:H]

	Q̃a = [SMatrix{nq,nq}(Q̃[t][1:nq, 1:nq]) for t = 1:H]
	Q̃b = [SMatrix{nq,nq}(Q̃[t][nq .+ (1:nq), nq .+ (1:nq)]) for t = 1:H]
	Q̃v = [SMatrix{nq,nq}(Q̃[t][1:nq, nq .+ (1:nq)]) for t = 1:H]
	R̃a = [inv(Ra[t]) for t = 1:H-1]

	# Y
	Yii = [zeros(n, n) for t = 1:H-1]
	Yij = [zeros(n, n) for t = 1:H-1]
	Yiis = [SMatrix{n,n}(zeros(n, n)) for t = 1:H-1]
	Yijs = [SMatrix{n,n}(zeros(n, n)) for t = 1:H-1]

	Yiiav = [view(Yii[t], collect(1:nq), collect(1:nq)) for t = 1:H-1]
	Yiibv = [view(Yii[t], collect(1:nq), collect(nq .+ (1:nq))) for t = 1:H-1]
	Yiicv = [view(Yii[t], collect(nq .+ (1:nq)), collect(1:nq)) for t = 1:H-1]
	Yiidv = [view(Yii[t], collect(nq .+ (1:nq)), collect(nq .+ (1:nq))) for t = 1:H-1]

	Yijav = [view(Yij[t], collect(1:nq), collect(1:nq)) for t = 1:H-1]
	Yijbv = [view(Yij[t], collect(1:nq), collect(nq .+ (1:nq))) for t = 1:H-1]
	Yijcv = [view(Yij[t], collect(nq .+ (1:nq)), collect(1:nq)) for t = 1:H-1]
	Yijdv = [view(Yij[t], collect(nq .+ (1:nq)), collect(nq .+ (1:nq))) for t = 1:H-1]

	Yiia = [SMatrix{nq,nq}(zeros(nq,nq)) for t = 1:H-1]
	Yiib = [SMatrix{nq,nq}(zeros(nq,nq)) for t = 1:H-1]
	Yiic = [SMatrix{nq,nq}(zeros(nq,nq)) for t = 1:H-1]
	Yiid = [SMatrix{nq,nq}(zeros(nq,nq)) for t = 1:H-1]

	Yija = [SMatrix{nq,nq}(zeros(nq,nq)) for t = 1:H-1]
	Yijb = [SMatrix{nq,nq}(zeros(nq,nq)) for t = 1:H-1]
	Yijc = [SMatrix{nq,nq}(zeros(nq,nq)) for t = 1:H-1]
	Yijd = [SMatrix{nq,nq}(zeros(nq,nq)) for t = 1:H-1]

	# L
	Liis = [LowerTriangular(SMatrix{n,n}(zeros(n, n))) for t = 1:H-1]
	Ljis = [SMatrix{n,n}(zeros(n, n)) for t = 1:H-1]

	# r
	rlagu = [SVector{m}(zeros(m)) for t = 1:H-1]
	rlagqa = [SVector{nq}(zeros(nq)) for t = 1:H-1]
	rlagqb = [SVector{nq}(zeros(nq)) for t = 1:H-1]

	rdyn1 = [SVector{nq}(zeros(nq)) for t = 1:H-1]
	rdyn2 = [SVector{nq}(zeros(nq)) for t = 1:H-1]

	# β
	βn = [SVector{n}(zeros(n)) for t = 1:H-1]
	β1 = [SVector{nq}(zeros(nq)) for t = 1:H-1]
	β2 = [SVector{nq}(zeros(nq)) for t = 1:H-1]

	# y
	y = [SVector{n}(zeros(n)) for t = 1:H-1]

	# Δν
	Δνn = [SVector{n}(zeros(n)) for t = 1:H-1]
	Δν1 = [SVector{nq}(zeros(nq)) for t = 1:H-1]
	Δν2 = [SVector{nq}(zeros(nq)) for t = 1:H-1]

	# Δz
	Δu = [SVector{m}(zeros(m)) for t = 1:H-1]
	Δqa = [SVector{nq}(zeros(nq)) for t = 1:H-1]
	Δqb = [SVector{nq}(zeros(nq)) for t = 1:H-1]

	# temporary arrays
	tmp_nn = SMatrix{n,n}(zeros(n, n))
	tmp_nn2 = SMatrix{n,n}(zeros(n, n))
	tmp_nqnq = SMatrix{nq,nq}(zeros(nq, nq))
	tmp_nqnq2 = SMatrix{nq,nq}(zeros(nq, nq))
	tmp_nm = SMatrix{n,m}(zeros(n, m))
	tmp_nqm = SMatrix{nq,m}(zeros(nq, m))
	tmp_nq = SVector{nq}(zeros(nq))

	# idx
	idx_nq = SVector{nq}(collect(1:nq))
	idx_nq2 = SVector{nq}(collect(nq .+ (1:nq)))

	# regularization
	In = Diagonal(SVector{n}(opts.β_init * ones(n)))
	Inq = Diagonal(SVector{nq}(opts.β_init * ones(nq)))
	Im = Diagonal(SVector{m}(opts.β_init * ones(m)))

	# data
	θ = [zeros(0) for t = 1:H-1]

	# dual fill
	dual_fill = SVector{nq}(zeros(nq))

	NewtonStructureSolver(
		n,
		m,
		nq,
		H,
		nz,
		nd,
		x_idx,
		u_idx,
		qa_idx,
		qb_idx,
		n_idx,
		u,
		qa,
		qb,
		ν1,
		ν2,
		u_cand,
		qa_cand,
		qb_cand,
		ν1_cand,
		ν2_cand,
		Aa,
		Ab,
		Ac,
		Ba,
		q_ref,
		u_ref,
		Qa,
		Qb,
		Qv,
		Ra,
		Q̃a,
		Q̃b,
		Q̃v,
		R̃a,
		Yii,
		Yiis,
		Yij,
		Yijs,
		Yiia,
		Yiib,
		Yiic,
		Yiid,
		Yiiav,
		Yiibv,
		Yiicv,
		Yiidv,
		Yija,
		Yijb,
		Yijc,
		Yijd,
		Yijav,
		Yijbv,
		Yijcv,
		Yijdv,
		Liis,
		Ljis,
		rlagu,
		rlagqa,
		rlagqb,
		rdyn1,
		rdyn2,
		βn,
		β1,
		β2,
		y,
		Δνn,
		Δν1,
		Δν2,
		Δu,
		Δqa,
		Δqb,
		tmp_nn,
		tmp_nn2,
		tmp_nqnq,
		tmp_nqnq2,
		tmp_nm,
		tmp_nqm,
		tmp_nq,
		idx_nq,
		idx_nq2,
		In,
		Inq,
		Im,
		θ,
		dual_fill,
		opts)
end

function NewtonStructure(sim::Simulation, H::Int, traj::ContactTraj, obj, κ; opts = NewtonOptions())

	s = newton_structure_solver(sim.model.dim.q, sim.model.dim.u, H, opts = opts)
	update_objective!(s, obj)
	initialize_trajectories!(s, traj, traj.q[1], traj.q[2], warm_start_duals = false)

	return s
end

function compute_Y!(Yiia, Yiib, Yiic, Yiid, Yija, Yijb, Yijc, Yijd, Aa, Ab, Ac, Ba, Q̃a, Q̃b, Q̃v, R̃a, tmp_q_nn, tmp_q_nn2, tmp_q_nm, Inq, T)

	for t = 1:T-1
		if t == 1
			Yiia[t] = Q̃a[t+1]

			Yiib[t] = Q̃v[t+1]

			Yiic[t] = Q̃v[t+1]

			Yiid[t] = Q̃b[t+1]
			tmp_q_nm = Ba[t] * R̃a[t]
			tmp_q_nn = tmp_q_nm * transpose(Ba[t])
			Yiid[t] += tmp_q_nn

		else
			Yiia[t] = Q̃a[t+1]

			Yiib[t] = Q̃v[t+1]

			Yiic[t] = Q̃v[t+1]

			Yiid[t] = Q̃b[t+1]
			tmp_q_nm = Ba[t] * R̃a[t]
			tmp_q_nn = tmp_q_nm * transpose(Ba[t])
			Yiid[t] += tmp_q_nn

			Yiia[t] += Q̃b[t]

			Yiib[t] += Q̃v[t] * transpose(Aa[t])
			Yiib[t] += Q̃b[t] * transpose(Ab[t])

			Yiic[t] += Aa[t] * Q̃v[t]
			Yiic[t] += Ab[t] * Q̃b[t]

			tmp_q_nn = Aa[t] * Q̃a[t]
			Yiid[t] += tmp_q_nn * transpose(Aa[t])
			tmp_q_nn = Aa[t] * Q̃v[t]
			Yiid[t] += tmp_q_nn * transpose(Ab[t])
			tmp_q_nn = Ab[t] * Q̃v[t]
			Yiid[t] += tmp_q_nn * transpose(Aa[t])
			tmp_q_nn = Ab[t] * Q̃b[t]
			Yiid[t] += tmp_q_nn * transpose(Ab[t])
		end

		# regularization
		Yiia[t] += Inq
		Yiid[t] += Inq

		t == T-1 && continue
		Yija[t] = -Q̃v[t+1]

		Yijb[t] = -Q̃a[t+1] * transpose(Aa[t+1]) - Q̃v[t+1] * transpose(Ab[t+1])

		Yijc[t] = -Q̃b[t+1]

		Yijd[t] = -Q̃v[t+1] * transpose(Aa[t+1]) - Q̃b[t+1] * transpose(Ab[t+1])

	end
	nothing
end

function update_Y!(Yiis, Yijs, Yii, Yij, Yiia, Yiib, Yiic, Yiid, Yija, Yijb, Yijc, Yijd, Yiiav, Yiibv, Yiicv, Yiidv, Yijav, Yijbv, Yijcv, Yijdv, T)
	for t = 1:T-1
		Yiiav[t] .= Yiia[t]
		Yiibv[t] .= Yiib[t]
		Yiicv[t] .= Yiic[t]
		Yiidv[t] .= Yiid[t]

		Yijav[t] .= Yija[t]
		Yijbv[t] .= Yijb[t]
		Yijcv[t] .= Yijc[t]
		Yijdv[t] .= Yijd[t]

		Yiis[t] = Yii[t]
		Yijs[t] = Yij[t]
	end
	nothing
end

function compute_L!(Lii, Lji, Yii, Yij, tmp_nn, tmp_nn2, T)
	for t = 1:T-1
		if t == 1
			Lii[t] = cholesky(Hermitian(Yii[t])).L

			# Lii[t] = cholesky(Yii[t]).L
		else
			# Lii[t] = cholesky(Hermitian(Yii[t] - Lji[t - 1] * Lji[t - 1]')).L

			tmp_nn = Yii[t]
			tmp_nn2 = transpose(Lji[t-1]) * Lji[t-1]
			tmp_nn -= tmp_nn2
			Lii[t] = cholesky(Hermitian(tmp_nn)).L
		end
		# Lji[t] = (LowerTriangular(Lii[t]) \ Yij[t])'

		Lji[t] = Lii[t] \ Yij[t]
	end
	nothing
end

function compute_β!(βn, β1, β2, rlagu, rlagqa, rlagqb, rdyn1, rdyn2, Aa, Ab, Ac, Ba, Q̃a, Q̃b, Q̃v, R̃, T)
	for t = 1:T-1
		if t == 1
			β1[1] = -1.0 * rdyn1[1] + Q̃a[2] * rlagqa[1]
			β1[1] += Q̃v[2] * rlagqb[1]

			β2[1] = -1.0 * rdyn2[1] - Ba[1] * R̃[1] * rlagu[1] + Ac[1] * Q̃b[2] * rlagqb[1]
			β2[1] += Ac[1] * Q̃v[2] * rlagqa[1]
		else
			β1[t] = -1.0 * rdyn1[t] + Q̃a[t+1] * rlagqa[t] - Q̃b[t] * rlagqb[t-1]
			β1[t] += -1.0 * Q̃v[t] * rlagqa[t-1] + Q̃v[t+1] * rlagqb[t]

			β2[t] = -1.0 * rdyn2[t] - Ba[t] * R̃[t] * rlagu[t] + Ac[t] * Q̃b[t+1] * rlagqb[t] - Aa[t] * Q̃a[t] * rlagqa[t-1] - Ab[t] * Q̃b[t] * rlagqb[t-1]
			β2[t] += -Aa[t] * Q̃v[t] * rlagqb[t-1] - Ab[t] * Q̃v[t] * rlagqa[t-1] + Ac[t] * Q̃v[t+1] * rlagqa[t]
		end
		βn[t] = [β1[t]; β2[t]]
	end
end

function compute_y!(y, Lii, Lji, b, T)
	for t = 1:T-1
		if t == 1
			# y[1] .= Lii[1] \ β[n_idx[1]]

			y[1] = Lii[1] \ b[1]
		else
			# y[t] .= Lii[t] \ (β[n_idx[t]] - transpose(Lji[t - 1]) * y[t - 1])

			y[t] = Lii[t] \ (b[t] - transpose(Lji[t - 1]) * y[t - 1])
		end
	end
	nothing
end

function compute_Δν!(x, x1, x2, Lii, Lji, y, idx1, idx2, T)
	for t = T-1:-1:1
		if t == T-1
			# x[t] = LowerTriangular(Lii[t])' \ y[t]

			x[t] = transpose(Lii[t]) \ y[t]
		else
			# x[t] = LowerTriangular(Lii[t])' \ (y[t] - Lji[t] * x[t+1])

			x[t] = transpose(Lii[t]) \ (y[t] - Lji[t] * x[t + 1])
		end

		x1[t] = x[t][idx1]
		x2[t] = x[t][idx2]
	end
	nothing
end

function compute_Δz!(Δu, Δqa, Δqb, Δν1, Δν2, Aa, Ab, Ac, Ba, Q̃a, Q̃b, Q̃v, R̃, rlagu, rlagqa, rlagqb, T)
	for t = 1:T-1
		Δu[t] = R̃[t] * (rlagu[t] + transpose(Ba[t]) * Δν2[t])

		if t < T-1
			Δqa[t] = Q̃a[t+1] * (rlagqa[t] - Δν1[t] + transpose(Aa[t+1]) * Δν2[t+1])
			Δqa[t] += Q̃v[t+1] * (rlagqb[t] - transpose(Ac[t]) * Δν2[t] + transpose(Ab[t+1]) * Δν2[t+1] + Δν1[t+1])

			Δqb[t] = Q̃b[t+1] * (rlagqb[t] - transpose(Ac[t]) * Δν2[t] + transpose(Ab[t+1]) * Δν2[t+1] + Δν1[t+1])
			Δqb[t] += Q̃v[t+1] * (rlagqa[t] - Δν1[t] + transpose(Aa[t+1]) * Δν2[t+1])
		else
			Δqa[t] = Q̃a[t+1] * (rlagqa[t] - Δν1[t])
			Δqa[t] += Q̃v[t+1] * (rlagqb[t] - transpose(Ac[t]) * Δν2[t])

			Δqb[t] = Q̃b[t+1] * (rlagqb[t] - transpose(Ac[t]) * Δν2[t])
			Δqb[t] += Q̃v[t+1] * (rlagqa[t] - Δν1[t])
		end
	end
end

function factorize!(s::NewtonStructureSolver)
	compute_Y!(s.Yiia, s.Yiib, s.Yiic, s.Yiid, s.Yija, s.Yijb, s.Yijc, s.Yijd,
		s.Aa, s.Ab, s.Ac, s.Ba, s.Q̃a, s.Q̃b, s.Q̃v, s.R̃a,
		s.tmp_nqnq, s.tmp_nqnq2, s.tmp_nqm, s.Inq, s.H)

	update_Y!(s.Yiis, s.Yijs, s.Yii, s.Yij, s.Yiia, s.Yiib, s.Yiic, s.Yiid,
		s.Yija, s.Yijb, s.Yijc, s.Yijd, s.Yiiav, s.Yiibv, s.Yiicv, s.Yiidv,
		s.Yijav, s.Yijbv, s.Yijcv, s.Yijdv, s.H)

	compute_L!(s.Liis, s.Ljis, s.Yiis, s.Yijs, s.tmp_nn, s.tmp_nn2, s.H)
end

function ContactImplicitMPC.solve!(s::NewtonStructureSolver)
	compute_β!(s.βn, s.β1, s.β2, s.rlagu, s.rlagqa, s.rlagqb,
		s.rdyn1, s.rdyn2, s.Aa, s.Ab, s.Ac, s.Ba, s.Q̃a, s.Q̃b, s.Q̃v, s.R̃a, s.H)

	compute_y!(s.y, s.Liis, s.Ljis, s.βn, s.H)

	compute_Δν!(s.Δνn, s.Δν1, s.Δν2, s.Liis, s.Ljis, s.y, s.idx_nq, s.idx_nq2, s.H)

	compute_Δz!(s.Δu, s.Δqa, s.Δqb, s.Δν1,  s.Δν2, s.Aa, s.Ab, s.Ac, s.Ba,
		s.Q̃a, s.Q̃b, s.Q̃v, s.R̃a, s.rlagu, s.rlagqa, s.rlagqb, s.H)
end

# objective
mutable struct QuadraticObjective{Q,V,U} <: Objective
    q::Vector{Q}
	v::Vector{V}
    u::Vector{U}
end

function quadratic_objective(model::ContactModel, H::Int;
    q = [Diagonal(SVector{model.dim.q}(ones(model.dim.q))) for t = 1:H+1],
	v = [Diagonal(SVector{model.dim.q}(0.1 * ones(model.dim.q))) for t = 1:H],
    u = [Diagonal(SVector{model.dim.u}(ones(model.dim.u))) for t = 1:H-1])
    return QuadraticObjective(q, v, u)
end

function update_objective!(s::NewtonStructureSolver, obj::QuadraticObjective)
	n = s.n
	m = s.m
	nq = s.nq
	H = s.H

	# build objective for state representation: x = (qa, qb)
	Q = [[(t > 1 ? 0.5 : 1.0) * obj.q[t] + obj.v[t] -1.0 * obj.v[t];
	      -1.0 * obj.v[t]' (t < H ? 0.5 : 1.0) * obj.q[t] + obj.v[t]] + s.In for t = 1:H]

	Q̃ = [inv(Array(Q[t])) for t = 1:H]

	R = [obj.u[t] + s.Im for t = 1:H-1]
	R̃ = [inv(R[t]) for t = 1:H-1]

	for t = 1:H
		s.Qa[t] = Q[t][1:nq, 1:nq]
		s.Qb[t] = Q[t][nq .+ (1:nq), nq .+ (1:nq)]
		s.Qv[t] = Q[t][1:nq, nq .+ (1:nq)]

		s.Q̃a[t] = Q̃[t][1:nq, 1:nq]
		s.Q̃b[t] = Q̃[t][nq .+ (1:nq), nq .+ (1:nq)]
		s.Q̃v[t] = Q̃[t][1:nq, nq .+ (1:nq)]

		t == H && continue
		s.Ra[t] = R[t]

		s.R̃a[t] = R̃[t]
	end
end

function update_dynamics_jacobian!(s::NewtonStructureSolver, im_traj::ImplicitTraj)
	for t = 1:s.H-1
		s.Aa[t] = im_traj.δq0[t]
		s.Ab[t] = im_traj.δq1[t]
		# s.Ac[t] = Diagonal(SVector{s.nq}(ones(s.nq))) # pre-allocated
		s.Ba[t] = im_traj.δu1[t]
	end
	return nothing
end

function dynamics_constraints!(s::NewtonStructureSolver, u, qa, qb, d)
	# update implicit dynamics
	for t = 1:s.H-1
		# q^{t+1}_{t-1} - q^t_t = 0
		s.rdyn1[t] = qa[t+1] - qb[t]

		# q^{t+1}_t - s(q^t_{t-1}, q^t_{t}, u_t)
		# println(norm(qb[t+1] - d[t], 1))
		s.rdyn2[t] = qb[t+1] - d[t]
	end
	return nothing
end

function lagrangian_gradient!(s::NewtonStructureSolver, u, qa, qb, ν1, ν2)
	# objective terms
	for t = 1:s.H-1
		s.rlagu[t] = s.Ra[t] * (u[t] - s.u_ref[t])

		s.rlagqa[t] = s.Qa[t+1] * (qa[t+1] - s.q_ref[t+1])
		s.rlagqb[t] = s.Qb[t+1] * (qb[t+1] - s.q_ref[t+2])

		s.rlagqa[t] -= s.Qv[t+1] * (qb[t+1] - qa[t+1])
		s.rlagqb[t] += s.Qv[t+1] * (qb[t+1] - qa[t+1])
	end

	# configuration equality terms
	for t = 1:s.H-1
		s.rlagqa[t] += ν1[t]
		t == 1 && continue
		s.rlagqb[t-1] -= ν1[t]
	end

	# dynamics terms
	for t = 1:s.H-1
		s.rlagu[t] -= transpose(s.Ba[t]) * ν2[t]
		s.rlagqb[t] += ν2[t]
		t == 1 && continue
		s.rlagqa[t-1] -= transpose(s.Aa[t]) * ν2[t]
		s.rlagqb[t-1] -= transpose(s.Ab[t]) * ν2[t]
	end
	return nothing
end

function implicit_dynamics!(im_traj::ImplicitTraj, model, env, u, qa, qb, H, θ; κ = traj.κ) where {T, D}

	nq = model.dim.q
	m = model.dim.u

	for t = 1:H-1
		θ[t][1:nq] = copy(qa[t])
		θ[t][nq .+ (1:nq)] = copy(qb[t])
		θ[t][2nq .+ (1:m)] = copy(u[t])

		# initialized solver
		z_initialize!(im_traj.ip[t].z, model, env, copy(qb[t])) #TODO: try alt. schemes
		im_traj.ip[t].θ .= copy(θ[t])

		# solve
		status = interior_point_solve!(im_traj.ip[t])

		# !status && error("implicit dynamics failure (t = $t)")
		!status && (@warn "implicit dynamics failure (t = $t)")
	end
	return nothing
end

function compute_residual!(s::NewtonStructureSolver, u, qa, qb, ν1, ν2, H, im_traj::ImplicitTraj, model, env, κ)

	# update
	implicit_dynamics!(im_traj, model, env, u, qa, qb, H, s.θ, κ = κ)

	# update static Jacobians
	update_dynamics_jacobian!(s, im_traj)

	# update constraints
	dynamics_constraints!(s, u, qa, qb, im_traj.dq2)

	# update gradient of Lagrangian
	lagrangian_gradient!(s, u, qa, qb, ν1, ν2)
end

function step!(s::NewtonStructureSolver, α::T) where T
	for t = 1:s.H-1
		s.u_cand[t] = s.u[t] - α * s.Δu[t]
		s.qa_cand[t+1] = s.qa[t+1] - α * s.Δqa[t]
		s.qb_cand[t+1] = s.qb[t+1] - α * s.Δqb[t]
		s.ν1_cand[t] = s.ν1[t] - α * s.Δν1[t]
		s.ν2_cand[t] = s.ν2[t] - α * s.Δν2[t]
	end
end

function accept_step!(s::NewtonStructureSolver)
	for t = 1:s.H-1
		s.u[t] = s.u_cand[t]
		s.qa[t+1] = s.qa_cand[t+1]
		s.qb[t+1] = s.qb_cand[t+1]
		s.ν1[t] = s.ν1_cand[t]
		s.ν2[t] = s.ν2_cand[t]
	end
end

function residual_norm(s, mode = 1)
	e = 0.0

	for t = 1:s.H-1
		e += norm(s.rlagu[t], mode)
		e += norm(s.rlagqa[t], mode)
		e += norm(s.rlagqb[t], mode)

		e += norm(s.rdyn1[t], mode)
		e += norm(s.rdyn2[t], mode)
	end
	return e
end

function initialize_trajectories!(s::NewtonStructureSolver, ref_traj::ContactTraj, q0, q1;
	warm_start_duals = True)

	tmp_nq = SVector{s.nq}(zeros(s.nq)) # TODO: preallocate

	# initialize trajectories
	for t = 1:s.H
		s.qa[t] = ref_traj.q[t]
		s.qb[t] = ref_traj.q[t+1]
		s.qa_cand[t] = ref_traj.q[t]
		s.qb_cand[t] = ref_traj.q[t+1]

		t >= s.H && continue
		s.u[t] = ref_traj.u[t]
		s.u_cand[t] = ref_traj.u[t]

		if !warm_start_duals
			s.ν1[t] = s.dual_fill
			s.ν2[t] = s.dual_fill

			s.ν1_cand[t] = s.dual_fill
			s.ν2_cand[t] = s.dual_fill
		end

		s.θ[t] = ref_traj.θ[t]
	end

	# initialize reference trajectory
	for t = 1:s.H+1
		s.q_ref[t] = ref_traj.q[t]
		t >= s.H && continue
		s.u_ref[t] = ref_traj.u[t]
	end

	# initial conditions
	s.qa[1] = q0
	s.qb[1] = q1
	s.qa_cand[1] = q0
	s.qb_cand[1] = q1

	return nothing
end

function newton_solve!(s::NewtonStructureSolver, sim::Simulation,
    im_traj::ImplicitTraj, ref_traj::ContactTraj; q0 = ref_traj.q[1], q1 = ref_traj.q[2],
    warm_start::Bool = false,
	κ = 1.0e-4)

	# copy_traj = deepcopy(ref_traj) # TODO: preallocate

	initialize_trajectories!(s, ref_traj, warm_start_duals = warm_start,
		q0, q1)

    # compute residual
	compute_residual!(s, s.u, s.qa, s.qb, s.ν1, s.ν2, s.H, im_traj, sim.model, sim.env, κ)
    r_norm = residual_norm(s, 1)

	# println("initialize!")
	# start timer
	elapsed_time = 0.0

    for l = 1:s.opts.max_iter
		# check timer
		elapsed_time >= s.opts.max_time && break

		# update timer
		elapsed_time += @elapsed begin

		# check convergence
		r_norm / (s.nz + s.nd) < s.opts.r_tol && break

		# factorize system
		factorize!(s)

		# solve system
		ContactImplicitMPC.solve!(s)

		# line search the step direction
		α = 1.0
		iter = 0
		step!(s, α)

		# println("step!")

		# compute candidate residual
		compute_residual!(s, s.u_cand, s.qa_cand, s.qb_cand, s.ν1_cand, s.ν2_cand, s.H, im_traj, sim.model, sim.env, κ)
		residual_norm(s, 1)
		r_cand_norm = residual_norm(s, 1)

		# backtrack
        while r_cand_norm^2.0 >= (1.0 - 0.001 * α) * r_norm^2.0
            α = 0.5 * α

            iter += 1
            if iter > 6
                break
            end

			# step
			step!(s, α)

			# compute candiate residual
			compute_residual!(s, s.u_cand, s.qa_cand, s.qb_cand, s.ν1_cand, s.ν2_cand, s.H, im_traj, sim.model, sim.env, κ)
			r_cand_norm = residual_norm(s, 1)
        end

        # upate
		accept_step!(s)
		r_norm = r_cand_norm

        # # regularization update
        # if iter > 6
        #     core.β = min(core.β * 1.3, 1.0e2)
        # else
        #     core.β = max(1.0e1, core.β / 1.3)
        # end

        s.opts.verbose && println(" l: ", l ,
				"     t: ", scn(elapsed_time, digits = 0),
                "     r: ", scn(r_norm / (s.nz + s.nd), digits = 0),
                # "     Δ: ", scn(norm(core.Δ.r, 1) / length(core.Δ.r), digits = 0),
                "     α: ", -Int(round(log(α))),
                "     κ: ", scn(im_traj.ip[1].κ[1], digits = 0))
	end
    end

    return nothing
end


text:
# Copyright (c) 2017: Miles Lubin and contributors
# Copyright (c) 2017: Google Inc.
#
# Use of this source code is governed by an MIT-style license that can be found
# in the LICENSE.md file or at https://opensource.org/licenses/MIT.

const _NonnegToNonposMap{T} =
    Union{Variable.NonposToNonnegBridge{T},Constraint.NonnegToNonposBridge{T}}

function map_set(::Type{<:_NonnegToNonposMap}, set::MOI.Nonnegatives)
    return MOI.Nonpositives(set.dimension)
end

function inverse_map_set(::Type{<:_NonnegToNonposMap}, set::MOI.Nonpositives)
    return MOI.Nonnegatives(set.dimension)
end

const _FlipSignMap{T} =
    Union{Variable.FlipSignBridge{T},Constraint.FlipSignBridge{T}}

function map_function(::Type{<:_FlipSignMap{T}}, func) where {T}
    return MOIU.operate(-, T, func)
end

# The map is an involution
function inverse_map_function(BT::Type{<:_FlipSignMap}, func)
    return map_function(BT, func)
end

# The map is symmetric
function adjoint_map_function(BT::Type{<:_FlipSignMap}, func)
    return map_function(BT, func)
end

# The map is a symmetric involution
function inverse_adjoint_map_function(BT::Type{<:_FlipSignMap}, func)
    return map_function(BT, func)
end

max_stars_repo_name: JuliaOpt/MathOptInterface

max_issues_repo_name: JuliaOpt/MathOptInterface

max_forks_repo_name: JuliaOpt/MathOptInterface


text:
using Images

"""
```
lines = hough_transform_standard(
    img_edges::AbstractMatrix;
    stepsize=1,
    angles=range(0,stop=pi,length=minimum(size(img))),
    vote_threshold=minimum(size(img)) / stepsize -1,
    max_linecount=typemax(Int))
```

Returns a vector of tuples corresponding to the tuples of (r,t)
where r and t are parameters for normal form of line:
    `x * cos(t) + y * sin(t) = r`

-   `r` = length of perpendicular from (1,1) to the line
-   `t` = angle between perpendicular from (1,1) to the line and x-axis

The lines are generated by applying hough transform on the image.

Parameters:
-    `img_edges`      = Image to be transformed (eltype should be `Bool`)
-    `stepsize`       = Discrete step size for perpendicular length of line
-    `angles`         = List of angles for which the transform is computed
-    `vote_threshold` = Accumulator threshold for line detection
-    `max_linecount`  = Maximum no of lines to return

# Example
```julia
julia> using ImageFeatures

julia> img = fill(false,5,5); img[3,:] .= true; img
5×5 Array{Bool,2}:
 false  false  false  false  false
 false  false  false  false  false
  true   true   true   true   true
 false  false  false  false  false
 false  false  false  false  false

julia> hough_transform_standard(img)
1-element Array{Tuple{Float64,Float64},1}:
 (3.0, 1.5707963267948966)
```
"""
function hough_transform_standard(
    img_edges::AbstractMatrix{Bool};
    stepsize=1,
    angles=range(0,stop=pi,length=minimum(size(img_edges))),
    vote_threshold=minimum(size(img_edges)) / stepsize -1,
    max_linecount=typemax(Int))

    stepsize > 0 || error("Discrete step size must be positive")
    ρ = stepsize
    θ = angles

    #function to compute local maximum lines with values > threshold and return a vector containing them
    function findlocalmaxima!(validLines::AbstractVector{CartesianIndex{2}}, accumulator_matrix::Array{Int,2}, threshold)
        for val in CartesianIndices(size(accumulator_matrix))
            if  accumulator_matrix[val] >  threshold                             &&
                accumulator_matrix[val] >  accumulator_matrix[val[1],val[2] - 1] &&
                accumulator_matrix[val] >= accumulator_matrix[val[1],val[2] + 1] &&
                accumulator_matrix[val] >  accumulator_matrix[val[1] - 1,val[2]] &&
                accumulator_matrix[val] >= accumulator_matrix[val[1] + 1,val[2]]
                push!(validLines,val)
            end
        end
    end

    indsy, indsx = axes(img_edges)
    ρinv = 1 / ρ
    numangle = length(θ)
    numrho = round(Int,(2(length(indsx) + length(indsy)) + 1)*ρinv)

    accumulator_matrix = zeros(Int, numangle + 2, numrho + 2)

    #Pre-Computed sines and cosines in tables
    sinθ, cosθ = sin.(θ).*ρinv, cos.(θ).*ρinv

    #Hough Transform implementation
    constadd = round(Int,(numrho -1)/2)
    for pix in CartesianIndices(size(img_edges))
        if img_edges[pix]
            for i in 1:numangle
                dist = round(Int, pix[1] * sinθ[i] + pix[2] * cosθ[i])
                dist += constadd
                accumulator_matrix[i + 1, dist + 1] += 1
            end
        end
    end

    #Finding local maximum lines
    validLines = Vector{CartesianIndex{2}}(undef, 0)
    findlocalmaxima!(validLines, accumulator_matrix, vote_threshold)

    #Sorting by value in accumulator_matrix
    @noinline sort_by_votes(validLines, accumulator_matrix) = sort!(validLines, lt = (a,b)-> accumulator_matrix[a]>accumulator_matrix[b])
    sort_by_votes(validLines, accumulator_matrix)

    max_linecount = min(max_linecount, length(validLines))

    lines = Vector{Tuple{Float64,Float64}}(undef, 0)

    #Getting lines with Maximum value in accumulator_matrix && size(lines) < max_linecount
    for l in 1:max_linecount
        lrho = ((validLines[l][2]-1) - (numrho-1)*0.5)*ρ
        langle = θ[validLines[l][1]-1]
        push!(lines,(lrho,langle))
    end

    lines
end

"""
```
circle_centers, circle_radius = hough_circle_gradient(img_edges, img_phase, radii; scale=1, min_dist=minimum(radii), vote_threshold)
```
Returns two vectors, corresponding to circle centers and radius.

The circles are generated using a hough transform variant in which a non-zero point only votes for circle
centers perpendicular to the local gradient. In case of concentric circles, only the largest circle is detected.

Parameters:
-   `img_edges`    = edges of the image
-   `img_phase`    = phase of the gradient image
-   `radii`        = circle radius range
-   `scale`        = relative accumulator resolution factor
-   `min_dist`     = minimum distance between detected circle centers
-   `vote_threshold`   = accumulator threshold for circle detection

[`canny`](@ref) and [`phase`](@ref) can be used for obtaining img_edges and img_phase respectively.

# Example

```julia
julia> using Images, ImageFeatures, FileIO, ImageView

julia> img = load(download("http://docs.opencv.org/3.1.0/water_coins.jpg"));

julia> img = Gray.(img);

julia> img_edges = canny(img, (Percentile(99), Percentile(80)));

julia> dx, dy=imgradients(img, KernelFactors.ando5);

julia> img_phase = phase(dx, dy);

julia> centers, radii = hough_circle_gradient(img_edges, img_phase, 20:30);

julia> img_demo = Float64.(img_edges); for c in centers img_demo[c] = 2; end

julia> imshow(img_demo)
```
"""
function hough_circle_gradient(
        img_edges::AbstractArray{Bool,2},
        img_phase::AbstractArray{<:Number,2},
        radii::AbstractRange{<:Integer};
        scale::Number=1,
        min_dist::Number=minimum(radii),
        vote_threshold::Number=minimum(radii)*min(scale, length(radii)))

    rows,cols=size(img_edges)

    non_zeros=CartesianIndex{2}[]
    centers=CartesianIndex{2}[]
    circle_centers=CartesianIndex{2}[]
    circle_radius=Int[]
    accumulator_matrix=zeros(Int, Int(floor(rows/scale))+1, Int(floor(cols/scale))+1)

    function vote!(accumulator_matrix, x, y)
        fx = Int(floor(x))
        fy = Int(floor(y))

        for i in fx:fx+1
            for j in fy:fy+1
                if checkbounds(Bool, accumulator_matrix, i, j)
                    @inbounds accumulator_matrix[i, j] += 1
                end
            end
        end
    end

    for j in axes(img_edges, 2)
        for i in axes(img_edges, 1)
            if img_edges[i,j]
                sinθ = -cos(img_phase[i,j]);
                cosθ = sin(img_phase[i,j]);

                for r in radii
                    x=(i+r*sinθ)/scale
                    y=(j+r*cosθ)/scale
                    vote!(accumulator_matrix, x, y)

                    x=(i-r*sinθ)/scale
                    y=(j-r*cosθ)/scale
                    vote!(accumulator_matrix, x, y)
                end
                push!(non_zeros, CartesianIndex{2}(i,j));
            end
        end
    end

    for i in findlocalmaxima(accumulator_matrix)
        if accumulator_matrix[i]>vote_threshold
            push!(centers, i);
        end
    end

    @noinline sort_by_votes(centers, accumulator_matrix) = sort!(centers, lt=(a, b) -> accumulator_matrix[a]>accumulator_matrix[b])

    sort_by_votes(centers, accumulator_matrix)

    dist(a, b) = sqrt(sum(abs2, (a-b).I))

    f = CartesianIndex(map(r->first(r), axes(accumulator_matrix)))
    l = CartesianIndex(map(r->last(r), axes(accumulator_matrix)))
    radius_accumulator=Vector{Int}(undef, Int(floor(dist(f,l)/scale)+1))

    for center in centers
        center=(center-1*one(center))*scale
        fill!(radius_accumulator, 0)

        too_close=false
        for circle_center in circle_centers
            if dist(center, circle_center)< min_dist
                too_close=true
                break
            end
        end
        if too_close
            continue;
        end

        for point in non_zeros
            r=Int(floor(dist(center, point)/scale))
            if radii.start/scale<=r<=radii.stop/scale
                radius_accumulator[r+1]+=1
            end
        end

        voters, radius = findmax(radius_accumulator)
        radius=(radius-1)*scale;

        if voters>vote_threshold
            push!(circle_centers, center)
            push!(circle_radius, radius)
        end
    end
    return circle_centers, circle_radius
end

@deprecate hough_circle_gradient(
        img_edges, img_phase,
        scale, min_dist,
        vote_threshold, radii) hough_circle_gradient(img_edges, img_phase, radii; scale=scale, min_dist=min_dist, vote_threshold=vote_threshold)

@deprecate hough_transform_standard(image, ρ, θ, threshold, linesMax) hough_transform_standard(
    image, stepsize=ρ, angles=θ, vote_threshold=threshold, max_linecount=linesMax)


text:
save_results = false

TO.diffmethod(::RobotZoo.Cartpole) = RD.ForwardAD()
@testset "Cartpole" begin
# load expected results
res = load(joinpath(@__DIR__,"cartpole.jld2"))

# Set up problem and solver
prob, opts = Problems.Cartpole()
t = TO.get_times(prob)
U = prob.constraints[1].z_max[end] * 2 * sin.(t .* 2)
U = [[u] for u in U]
initial_controls!(prob, U)
rollout!(prob)

solver = ALTROSolver(prob, opts, save_S=true)
n,m,N = size(solver)
ilqr = Altro.get_ilqr(solver)
Altro.initialize!(ilqr)
X = states(ilqr)
U = controls(ilqr)
@test X ≈ res["X"] atol=1e-6
@test U ≈ res["U"] atol=1e-6

# State diff Jacobian
TO.state_diff_jacobian!(ilqr.G, ilqr.model, ilqr.Z)
G = ilqr.G
@test G[1] == zeros(n,n)
@test G[N] == zeros(n,n)
@test G ≈ res["G"] atol=1e-6

# Dynamics Jacobians
TO.dynamics_expansion!(TO.integration(ilqr), ilqr.D, ilqr.model, ilqr.Z)
TO.error_expansion!(ilqr.D, ilqr.model, ilqr.G)
A = [Matrix(D.A_) for D in ilqr.D]
B = [Matrix(D.B_) for D in ilqr.D]
@test A ≈ res["A"] atol=1e-6
@test B ≈ res["B"] atol=1e-6

# Unconstrained Cost Expansion
TO.cost_expansion!(ilqr.quad_obj, ilqr.obj.obj, ilqr.Z, init=true, rezero=true)
TO.error_expansion!(ilqr.E, ilqr.quad_obj, ilqr.model, ilqr.Z, ilqr.G)
hess0 = [[E.Q E.H'; E.H E.R] for E in ilqr.E]
grad0 = [Vector([E.q; E.r]) for E in ilqr.E]
@test hess0 ≈ res["hess0"] atol=1e-6
@test grad0 ≈ res["grad0"] atol=1e-6

# AL Cost Expansion
TO.cost_expansion!(ilqr.quad_obj, ilqr.obj, ilqr.Z, init=true, rezero=true)
TO.error_expansion!(ilqr.E, ilqr.quad_obj, ilqr.model, ilqr.Z, ilqr.G)
hess = [[E.Q E.H'; E.H E.R] for E in ilqr.E]
grad = [Vector([E.q; E.r]) for E in ilqr.E]
@test hess ≈ res["hess"] atol=1e-6
@test grad ≈ res["grad"] atol=1e-6

# Backward Pass
ΔV = Altro.static_backwardpass!(ilqr)
K = Matrix.(ilqr.K)
d = Vector.(ilqr.d)
S = [Matrix(S.Q) for S in ilqr.S]
s = [Vector(S.q) for S in ilqr.S]
Qzz = [[E.Q E.H'; E.H E.R] for E in ilqr.Q]
Qz = [Vector([E.q; E.r]) for E in ilqr.Q]
@test K ≈ res["K"] atol=1e-6
@test d ≈ res["d"] atol=1e-6
@test S ≈ res["S"] atol=1e-6
@test s ≈ res["s"] atol=1e-6
@test Qzz ≈ res["Qzz"] atol=1e-6
@test Qz  ≈ res["Qz"] atol=1e-6

if save_results
    @save joinpath(@__DIR__,"cartpole.jld2") X U G A B hess0 grad0 hess grad K d S s Qzz Qz
end

end


text:
@info "Using $(Threads.nthreads()) threads"

resultpath = get(ARGS, 1, joinpath(@__DIR__, "build", "result.json"))
mkpath(dirname(resultpath))

using BenchmarkTools
include("benchmarks.jl")
_, time, = @timed result = run(SUITE; verbose = true)
BenchmarkTools.save(resultpath, result)

write(joinpath(dirname(resultpath), "time"), string(time))


text:
"""
Runs the atkinson94 algorithm and additionally plots the Stalactite text plot as described in the paper. Works for data with less than 100 points at the moment (since screen width of 80-120 is the standard).

# References
Atkinson, Anthony C. "Fast very robust methods for the detection of multiple outliers."
Journal of the American Statistical Association 89.428 (1994): 1329-1339.
"""
function atkinsonstalactiteplot(setting::RegressionSetting; iters=nothing, crit=3.0)
    X = designMatrix(setting)
    y = responseVector(setting)
    return atkinsonstalactiteplot(X, y, iters=iters, crit=crit)
end

function atkinsonstalactiteplot(X::Array{Float64,2}, y::Array{Float64,1}; iters=nothing, crit=3.0)
        n, p = size(X)
        output = atkinson94(X, y, iters=iters, crit=crit)
        residuals_matrix = output["residuals_matrix"]
        generate_stalactite_plot(residuals_matrix, n, p, crit)
end

function generate_stalactite_plot(residuals_matrix::Array{Float64, 2}, n::Int64, p::Int64, crit)
    print("m  ")
    print_tens_row(n)
    println()
    print("   ")
    print_ones_row(n)
    println()

    for m=p:n
        if m <= 9
            print(" $m ")
        else
            print("$m ")
        end
        for i=1:n
            print(stalactite_char_value(residuals_matrix[m-p+1, i], crit))
        end
        println()
    end
    print("   ")
    print_ones_row(n)
    println()
    print("   ")
    print_tens_row(n)
    println()
end

function print_tens_row(n::Int64)
    for i=1:Int(floor(n/10))
        for i=1:9
            print(" ")
        end
        print(i)
    end
end

function print_ones_row(n::Int64)
    for i=1:n
        print(i%10)
    end
end

function stalactite_char_value(x, crit)
    if x >= crit
        return '*'
    elseif x >= crit - 1
        return '+'
    else
        return ' '
    end
end

max_stars_repo_name: akadal/LinRegOutliers

max_issues_repo_name: akadal/LinRegOutliers

max_forks_repo_name: akadal/LinRegOutliers


text:
##############optim
mutable struct IterOptimState{TΘ⁺<:VolumePartsIter, Tmodel, T} <: AbstractIterState{T}
  Θ⁺::TΘ⁺
  λ⁺::T

  model::Tmodel

  starttime::Float64
  besttime::Float64
  maxelapsedsincebest::Float64
  maxelapsed::Float64

  iter::Int
  bestiter::Int
  maxitersincebest::Int
  maxiter::Int

  solvetype::Symbol
  lastreporttime::Float64
  minreporttime::Float64
  stop::Union{Nothing,Symbol}
end

IterOptimState(Θ::VolumePartsIter, ::Type{T};
  callback = (x)->false, #noop, false implies continuation of the optimization

  Θ⁺::VolumePartsIter = Θ |> deepcopy,
  λ⁺::T = T(Inf),

  model=error("model is required"),

  starttime::Float64=time(),
  besttime::Float64=time(),
  maxelapsedsincebest::Float64=PARAM[:optimmaxelapsedsincebest],
  maxelapsed::Float64=PARAM[:optimmaxelapsed],

  iter::Int=0,
  bestiter::Int=0,
  maxitersincebest::Int=PARAM[:optimmaxitersincebest],
  maxiter::Int=PARAM[:optimmaxiter],

  solvetype::Symbol=PARAM[:itersolvetype],
  lastreporttime::Float64=time(),
  minreporttime::Float64=PARAM[:iterminreporttime],
  stop::Union{Nothing,Symbol} = nothing,
  ) where T = IterOptimState(Θ⁺, λ⁺,
    model,
    starttime, besttime, maxelapsedsincebest,maxelapsed,
    iter, bestiter, maxitersincebest,maxiter,
    solvetype, lastreporttime, minreporttime, stop)

#reports the current status of the solver
function status(opt, Ψ::IterOptimState)
  (time()<Ψ.minreporttime+Ψ.lastreporttime) && return nothing
  Ψ.lastreporttime=time()

  elapsed::Int = round(time() - Ψ.starttime)
  elapsedsincebest::Int = round(time() - Ψ.besttime)

  itersincebest::Int = Ψ.iter - Ψ.bestiter

  println("iter=$(Ψ.iter), optimiter=$(opt.iteration), iter_since_best=$itersincebest," *
    "optimadjloss=$(opt.value), bestloss(unadj)=$(Ψ.λ⁺), " *
    "time=$elapsed, " * "timesincebest = $elapsedsincebest")

  return nothing
end

#reports status and initiates shutdown if the state has a stop flag
function callbackoptim!(opt, Ψ::IterOptimState{<:Any,<:Any,T}) where {T}

  status(opt, Ψ)

  if !(Ψ.stop===nothing)
    @info "STOP: Ψ.stop=$(Ψ.stop)"
    error("Working around optim issue #834 by throwing error")
  end

  return !(Ψ.stop===nothing) #doesn't stop the optimization due to issue #834
end

#main update function for optim
#NOTE: only called for evaluations of f- gradient call doesn't update state
function updateΨ!(x::AbstractVector{T}, loss, Θ::AbstractVolumePartsIter{TM,TV,T},
  Xv::AbstractXYIter{TM, TV, T},
  RegressionType::Val, Ψ::IterOptimState, GrowthType::Val) where {TM, TV, T}

  #first get the loss
  λ = updateA₁!(x, loss,Θ,Xv,RegressionType,GrowthType)

  #update the state and check additional stopping conditions
  #@info (λ, typeof(λ))
  updateΨ!(Θ, Ψ, λ)

  return λ
end

function updateΨ!(Θ::VolumePartsIter, Ψ::IterOptimState{<:Any,<:Any,T}, λ::Float64) where T
  Ψ.iter+=1
  t=time()

  if λ < Ψ.λ⁺ #keep track of the best solution
    copyto!(Ψ.Θ⁺.G, Θ.G)
    Ψ.λ⁺ = λ
    Ψ.besttime=t
    Ψ.bestiter=Ψ.iter
  end

  #use the solver for stopping criteria
  #run through the stopping criteria
  elapsed::Int = round(time() - Ψ.starttime)
  elapsedsincebest::Int = round(time() - Ψ.besttime)
  itersincebest::Int = Ψ.iter - Ψ.bestiter

  #set a stop flag if desired
  if  elapsed > Ψ.maxelapsed
    Ψ.stop=:maxtime
  elseif elapsedsincebest>Ψ.maxelapsedsincebest
    Ψ.stop=:maxtimesincebest
  elseif Ψ.iter ≥ Ψ.maxiter
    Ψ.stop=:maxiter
  elseif itersincebest  ≥ Ψ.maxitersincebest
    Ψ.stop=:maxitersincebest
  end

  return nothing
end

#handles processing of the line searhc parameter

function linesearchsetup(linesearchalg::Symbol = PARAM[:optimlinesearchalg],
    linesearchargs=PARAM[:optimlinesearchargs],
    linesearchinitial::Symbol=PARAM[:optimlinesearchinitial])
  alg=Dict(
    :hagerzhang=>LineSearches.HagerZhang,
    :morethuente=>LineSearches.MoreThuente,
    :backtracking=>LineSearches.BackTracking)

  initial=Dict(
    :initialprevious=>LineSearches.InitialPrevious,
    :initialstatic=>LineSearches.InitialStatic,
    :initialhagerzhang=>LineSearches.InitialHagerZhang,
    :initialquadratic=>LineSearches.InitialQuadratic,
    :initialconstantchange=>LineSearches.InitialConstantChange)

  return (alg=alg[linesearchalg](; linesearchargs...),
    initial=initial[linesearchinitial]())
end

#LBFGS solver
function solveA!(loss, Θ::AbstractVolumePartsIter{TM,TV,T},
    Xv::AbstractXYIter{TM, TV, T},
    SolveType::Val{:optimlbfgs};
    RegressionType::Val = Val(PARAM[:iterregressiontype]),
    GrowthType::Val = Val(PARAM[:itergrowthtype])) where {TM, TV, T}

  dims = (K=size(Θ.A,1), T=size(Θ.A,2))
  ls = linesearchsetup()
  lbfgs = Optim.LBFGS(
    m=PARAM[:lbfgsm],
    linesearch = ls.alg,
    alphaguess = ls.initial,
    )

  Ψ = IterOptimState(Θ,Float64, model=lbfgs)
  lowerbound::T = PARAM[:optimlowerbound]
  upperbound::T = PARAM[:optimupperbound]
  lower::Vector{T} = fill(lowerbound, (dims.T-1) * dims.K)
  upper::Vector{T} = fill(upperbound, (dims.T-1) * dims.K)

  obj(x::Vector{T}) = updateΨ!(x, loss, Θ, Xv, RegressionType, Ψ, GrowthType)
  x::Vector{T} = initializeΘgvector(Θ.g, T, GrowthType) #provides an initial vector for the optimizer
  #println("x: $x")
  ∇obj! = ∇lossforzygote(Θ, Xv, GrowthType)
  runoptimize(options) = Optim.optimize(obj, ∇obj!, lower, upper, x, Fminbox(Ψ.model), options)
  #test
  obj(x)
  ∇obj!(deepcopy(x),x)

  return solveA!(loss, Θ, Xv, RegressionType, Ψ, runoptimize)
end

#conjugate gradient solver
#Note on line searches- HZ much better than backtracking
function solveA!(loss, Θ::AbstractVolumePartsIter{TM,TV,T},
    Xv::AbstractXYIter{TM, TV, T},
    SolveType::Val{:optimcg};
    RegressionType::Val = Val(PARAM[:iterregressiontype]),
    GrowthType::Val = Val(PARAM[:itergrowthtype])) where {TM, TV, T}

  dims = (K=size(Θ.A,1), T=size(Θ.A,2))
  ls = linesearchsetup()
  cg = Optim.ConjugateGradient(
    eta=PARAM[:cgeta],
    linesearch = ls.alg,
    alphaguess = ls.initial,
    )

  Ψ = IterOptimState(Θ,Float64, model=cg)
  lowerbound::T = PARAM[:optimlowerbound]
  upperbound::T = PARAM[:optimupperbound]
  lower::Vector{T} = fill(lowerbound, (dims.T-1) * dims.K)
  upper::Vector{T} = fill(upperbound, (dims.T-1) * dims.K)

  obj(x::Vector{T}) = updateΨ!(x, loss, Θ, Xv, RegressionType, Ψ, GrowthType)
  x::Vector{T} = initializeΘgvector(Θ.g, T, GrowthType) #provides an initial vector for the optimizer
  #println("x: $x")
  ∇obj! = ∇lossforzygote(Θ, Xv, GrowthType)
  runoptimize(options) = Optim.optimize(obj, ∇obj!, lower, upper, x, Fminbox(Ψ.model), options)
  #test
  obj(x)
  ∇obj!(deepcopy(x),x)

  return solveA!(loss, Θ, Xv, RegressionType, Ψ, runoptimize)
end

####2 step solve
function solveA!(loss, Θ::AbstractVolumePartsIter{TM,TV,T},
  Xv::AbstractXYIter{TM, TV, T},
  SolveType::Val{:optimcg2step};
  RegressionType::Val = Val(PARAM[:iterregressiontype]),
  GrowthType::Val = Val(PARAM[:itergrowthtype]),
  conditionaliter=PARAM[:iterconditionaliter]) where {TM, TV, T}

  dims = (K=size(Θ.A,1), T=size(Θ.A,2))
  lsconditional = linesearchsetup()
  cgconditional = Optim.ConjugateGradient(
    eta=PARAM[:cgeta],
    linesearch = lsconditional.alg,
    alphaguess = lsconditional.initial,
    )

  #this is just for the initial heuristic burnin
  Ψconditional = IterOptimState(Θ,Float64, model=cgconditional, maxiter=conditionaliter)
  lowerbound::T = PARAM[:optimlowerbound]
  upperbound::T = PARAM[:optimupperbound]
  lower::Vector{T} = fill(lowerbound, (dims.T-1) * dims.K)
  upper::Vector{T} = fill(upperbound, (dims.T-1) * dims.K)

  objconditional(x::Vector{T}) = updateΨ!(x, loss, Θ, Xv, Val{:none}(), Ψconditional, GrowthType)
  #provides an initial vector for the optimizer
  xconditional::Vector{T} = initializeΘgvector(Θ.g, T, GrowthType)

  #println("x: $x")
  ∇objconditional! = ∇lossforzygote(Θ, Xv, GrowthType, RegressionType=Val{:none}())
  runoptimizeconditional(options) = Optim.optimize(objconditional, ∇objconditional!, lower, upper,
    xconditional, Fminbox(Ψconditional.model), options)
  #test
  objconditional(xconditional)
  ∇objconditional!(deepcopy(xconditional),xconditional)

  λconditional = solveA!(loss, Θ, Xv, Val{:none}(), Ψconditional, runoptimizeconditional)
  @info "Burnin complete with λconditional=$λconditional"

  #******now run the real pass
  ls = linesearchsetup()
  cg = Optim.ConjugateGradient(
    eta=PARAM[:cgeta],
    linesearch = ls.alg,
    alphaguess = ls.initial,
    )

  Ψ = IterOptimState(Θ,Float64, model=cg)
  obj(x::Vector{T}) = updateΨ!(x, loss, Θ, Xv, RegressionType, Ψ, GrowthType)
  x::Vector{T} = initializeΘgvector(Θ.g, T, GrowthType, initializeto1=false)
  ∇obj! = ∇lossforzygote(Θ, Xv, GrowthType)
  runoptimize(options) = Optim.optimize(obj, ∇obj!, lower, upper, x, Fminbox(Ψ.model), options)
  λ = solveA!(loss, Θ, Xv, RegressionType, Ψ, runoptimize)

  @info "Second pass complete with λ=$λ"
  return λ
end

#experimental
function solveA!(loss, Θ::AbstractVolumePartsIter{TM,TV,T},
    Xv::AbstractXYIter{TM, TV, T},
    SolveType::Val{:optimadhoc};
    RegressionType::Val = Val(PARAM[:iterregressiontype]),
    GrowthType::Val = Val(PARAM[:itergrowthtype])) where {TM,TV,T}

  dims = (K=size(Θ.A,1), T=size(Θ.A,2))
  ls = linesearchsetup()
  lbfgs = Optim.LBFGS(
    m=PARAM[:lbfgsm],
    linesearch = ls.alg,
    alphaguess = ls.initial,
    )

  oaccel = Optim.OACCEL(nlprecon=lbfgs)

  Ψ = IterOptimState(Θ,Float64, model=oaccel)
  lowerbound::T = PARAM[:optimlowerbound]
  upperbound::T = PARAM[:optimupperbound]
  lower::Vector{T} = fill(lowerbound, (dims.T-1) * dims.K)
  upper::Vector{T} = fill(upperbound, (dims.T-1) * dims.K)

  obj(x::Vector{T}) = updateΨ!(x, loss, Θ, Xv, RegressionType, Ψ, GrowthType)
  x::Vector{T} = initializeΘgvector(Θ.g, T, GrowthType) #provides an initial vector for the optimizer
  ∇obj! = ∇lossforzygote(Θ, Xv, GrowthType)
  runoptimize(options) = Optim.optimize(obj, ∇obj!, lower, upper, x, Fminbox(Ψ.model), options)

  return solveA!(loss, Θ, Xv, RegressionType, Ψ, runoptimize)
end

function solveA!(loss, Θ::AbstractVolumePartsIter{TM,TV,T},
  Xv::AbstractXYIter{TM, TV, T},
  RegressionType::Val, Ψ::IterOptimState, runoptimize;
  callback=(opt)->callbackoptim!(opt,Ψ),
  GrowthType::Val = Val(PARAM[:itergrowthtype])) where {TM, TV, T}

  dims = (K=size(Θ.A,1), T=size(Θ.A,2))

  #can't put the below into the additoinal state due to the self-reference of the additional state
  optimoptions=Optim.Options(
    f_abstol = PARAM[:optimf_abstol],
    x_abstol = PARAM[:optimx_abstol],
    f_reltol = PARAM[:optimf_reltol],
    x_reltol = PARAM[:optimx_reltol],

    g_abstol = PARAM[:optimg_tol],
    g_reltol = 0.0,
    outer_g_reltol = 0.0,
    outer_g_abstol = PARAM[:optimg_tol], #doesn't seem to work?

    #NOTE: these limits should never trigger- iterations and time limited in callback
    iterations = 10^8,#PARAM[:optimiterations],
    outer_iterations = 10^8,
    time_limit = 10^8,#PARAM[:optimtime_limit],

    store_trace = PARAM[:optimstore_trace],
    extended_trace = PARAM[:optimextended_trace],
    allow_f_increases = PARAM[:optimallow_f_increases],
    allow_outer_f_increases=PARAM[:optimallow_f_increases],
    callback = callback)

  local res
  #res = runoptimize(optimoptions)
  #error("stop")

  local completed::Bool = false
  local res

  #short circuit for debugging purposes
  if PARAM[:runstacktraceoniter]
   res = runoptimize(optimoptions)
   error("No error to trace!:\nres: $res")
  end

  try
    res = runoptimize(optimoptions) #main optimziaiton entry point
    completed=true
  catch err
    errmessage = "$err"
    if length(errmessage) > 10_000 #sometimes we get really long error messages
      errmessage = errmessage[1:10_000]
    end

    if Ψ.stop === nothing
      Ψ.stop = :aborted
      @warn "Aborted optimization with error $errmessage"
    else #this means we intentionally threw the error to break the optimization
      @info "Completed optimization and terminated via workaround (error message $errmessage)"
    end

  finally

    elapsed::Int = round(time() - Ψ.starttime)
    elapsedsincebest::Int = round(time() - Ψ.besttime)
    itersincebest::Int = Ψ.iter - Ψ.bestiter
    #Θ.G .= Ψ.Θ⁺.G

    #make sure everything is up to date
    updateA₁!(Θ.g|>deepcopy, loss, Θ, Xv, RegressionType, Val{:identity}())
    updateA₁!(Ψ.Θ⁺.g|>deepcopy, loss, Ψ.Θ⁺, Xv, RegressionType, Val{:identity}())

    #compare the current candidate against the stored best candidate in Ψ
    #state candidate should be at least as good as curent candidate
    @info "updateAfromGAₜ!(Θ,Xv, 1)"
    updateAfromGAₜ!(Θ,Xv, 1) #update to the final values of A

    λ::T = loss(Θ, Xv, RegressionType)
    (λ > T(Ψ.λ⁺)) || @warn("Current estimated loss λ
      better than \"best\" λ⁺ estimate of loss. If the analysis was aborted part-way,
      this can be ignored, otherwise figure out why this happened")

    if T(Ψ.λ⁺) < λ
      #update the current candidate with the best candidate from Ψ
      updateA₁!(Ψ.Θ⁺.g|>deepcopy, loss, Θ, Xv, RegressionType, Val{:identity}())
      @assert (Ψ.Θ⁺.g ≈ Θ.g) && (Ψ.Θ⁺.G ≈ Θ.G)

      updateAfromGAₜ!(Θ,Xv, 1) #update to the final values of A
      λ = loss(Θ, Xv, RegressionType)

      #integrity checks- makes sure the best and current loss values are consistent
      updateA₁!(Ψ.Θ⁺.g|>deepcopy, loss, Ψ.Θ⁺, Xv, RegressionType, Val{:identity}())
      updateAfromGAₜ!(Ψ.Θ⁺,Xv, 1) #update to the final values of A
      @assert (Ψ.Θ⁺.g ≈ Θ.g) && (Ψ.Θ⁺.G ≈ Θ.G)
      (Ψ.Θ⁺.A ≈ Θ.A) || error("Ψ.Θ⁺.A ≈/ Θ.A
        *******Ψ.Θ⁺.A[:,1:5]: $(Ψ.Θ⁺.A[:,1:5])
        *******Θ.A[:,1:5]: $(Θ.A[:,1:5])")
      λ⁺ = loss(Ψ.Θ⁺, Xv, RegressionType)
      (λ ≈ Ψ.λ⁺) || error("λ ≠ Ψ.λ⁺!!! λ=$λ and Ψ.λ⁺=$(Ψ.λ⁺) and λ⁺=$λ⁺")
    end
    @assert  (λ ≤ T(Ψ.λ⁺)) || (λ ≈ T(Ψ.λ⁺))

    #why did we stop?
    #TODO- clean this up- definately a work in progress
    if Ψ.stop === nothing
      @assert Optim.converged(res)
      @info "STOP: Assuming convergence though converged=$(Optim.converged(res))"
      Ψ.stop=:converged
    end

    if completed

      #Ψ.Θ⁺.g .= conditiongrowth(res.minimizer, GrowthType)
      updateA₁!(res.minimizer, loss, Ψ.Θ⁺, Xv, RegressionType, GrowthType)

      λ⁺ = loss(Ψ.Θ⁺, Xv, RegressionType)
      λ = loss(Θ, Xv, RegressionType)

      if λ⁺ < λ
        updateA₁!(Ψ.Θ⁺.g|>deepcopy, loss, Θ, Xv, RegressionType, Val{:identity}())
      else
        updateA₁!(Θ.g|>deepcopy, loss, Ψ.Θ⁺, Xv, RegressionType, Val{:identity}())
      end

      @assert loss(Θ, Xv, RegressionType) ≈ loss(Ψ.Θ⁺, Xv, RegressionType)
      updateAfromGAₜ!(Θ,Xv, 1)
      #=((Θ.G |> Matrix |> vec ≈ resminimizer .|> T)
        || (Ψ.Θ⁺.G |> Matrix |> vec ≈ resminimizer .|> T)) || error(
        "res minimizer=\n$(resminimizer)\n ... while Θ.G=\n$(Θ.G)"
      )=#

      @info "printing res (Experimental):"
      println(res)
    end

    b = IOBuffer()
    write(b, "***Results info ($(Ψ.solvetype)):\n" *
      "stop_reason: $(Ψ.stop)\n" *
      "iterations: $(Ψ.iter)/$(Ψ.maxiter) " *
        "(optim counted=$(completed ? Optim.iterations(res) : "NA aborted"))\n" *
      "elapsed_at_completion: $(elapsed)/$(Ψ.maxelapsed)\n" *
      "elapsed since best: ~$(elapsedsincebest)\n" *
      "iter since best: ~$(itersincebest)\n" *
      "Best resid: $(Ψ.λ⁺)\n")

    write(b, "\n\n***Other Simulation parameters:\n" *
      "measuretype: $(PARAM[:measuretype])\n" *
      "regressionmethod: $(PARAM[:regressionmethod])\n" *
      "controlsbykt: $(PARAM[:controlsbykt])\n" *
      "controlsbyt: $(PARAM[:controlsbyt])\n" *
      "controlsglobal: $(PARAM[:controlsglobal])\n" *
      "growth type: $GrowthType\n" *
      "momentum weight spec: $(PARAM[:momentumweightspec])\n" *
      "sorted default thresholds: $(PARAM[:sorteddefaultthresholds])\n" *
      "regression method: $(RegressionType)\n" *
      "report interval: $(PARAM[:iterminreporttime])\n" *
      "Types (Matrix, Vector, T): $TM, $TV, $T\n" *
      "bounds: [$(PARAM[:optimlowerbound]), $(PARAM[:optimupperbound])]\n" *
      "linesearch (args): $(PARAM[:optimlinesearchalg]) ($(PARAM[:optimlinesearchargs]))\n" *
      "linesearch initial: $(PARAM[:optimlinesearchinitial])\n," *
      "optimoptions: $(optimoptions)\n" *
      "model config: $(Ψ.model)\n")
    write(b, "\n$(formatparameters())\n")

    iterinfo = String(take!(b))
    #println(iterinfo)
    open("$(PARAM[:outputpath])\\$(iterresultsname(Ψ.λ⁺))_info.txt", "w+") do f
      write(f, iterinfo)
    end
    return Ψ.λ⁺
  end

  @assert false #should never get here

end

  #######BBOptim

max_stars_repo_name: clintonTE/CCA

max_issues_repo_name: clintonTE/CCA

max_forks_repo_name: clintonTE/CCA


text:
"""
    MetropolisHastings{D}

`MetropolisHastings` has one field, `proposal`.
`proposal` is a `Proposal`, `NamedTuple` of `Proposal`, or `Array{Proposal}` in the shape of your data.
For example, if you wanted the sampler to return a `NamedTuple` with shape

```julia
x = (a = 1.0, b=3.8)
```

The proposal would be

```julia
proposal = (a=StaticProposal(Normal(0,1)), b=StaticProposal(Normal(0,1)))
````

Other allowed proposals are

```
p1 = StaticProposal(Normal(0,1))
p2 = StaticProposal([Normal(0,1), InverseGamma(2,3)])
p3 = StaticProposal((a=Normal(0,1), b=InverseGamma(2,3)))
p4 = StaticProposal((x=1.0) -> Normal(x, 1))
```

The sampler is constructed using

```julia
spl = MetropolisHastings(proposal)
```

When using `MetropolisHastings` with the function `sample`, the following keyword
arguments are allowed:

- `init_params` defines the initial parameterization for your model. If
none is given, the initial parameters will be drawn from the sampler's proposals.
- `param_names` is a vector of strings to be assigned to parameters. This is only
used if `chain_type=Chains`.
- `chain_type` is the type of chain you would like returned to you. Supported
types are `chain_type=Chains` if `MCMCChains` is imported, or
`chain_type=StructArray` if `StructArrays` is imported.
"""
struct MetropolisHastings{D} <: MHSampler
    proposal::D
end

StaticMH(d) = MetropolisHastings(StaticProposal(d))
RWMH(d) = MetropolisHastings(RandomWalkProposal(d))

function propose(rng::Random.AbstractRNG, sampler::MHSampler, model::DensityModel)
    return propose(rng, sampler.proposal, model)
end
function propose(
    rng::Random.AbstractRNG,
    sampler::MHSampler,
    model::DensityModel,
    transition_prev::Transition,
)
    return propose(rng, sampler.proposal, model, transition_prev.params)
end

function transition(sampler::MHSampler, model::DensityModel, params)
    logdensity = AdvancedMH.logdensity(model, params)
    return transition(sampler, model, params, logdensity)
end
function transition(sampler::MHSampler, model::DensityModel, params, logdensity::Real)
    return Transition(params, logdensity)
end

# Define the first sampling step.
# Return a 2-tuple consisting of the initial sample and the initial state.
# In this case they are identical.
function AbstractMCMC.step(
    rng::Random.AbstractRNG,
    model::DensityModel,
    sampler::MHSampler;
    init_params=nothing,
    kwargs...
)
    params = init_params === nothing ? propose(rng, sampler, model) : init_params
    transition = AdvancedMH.transition(sampler, model, params)
    return transition, transition
end

# Define the other sampling steps.
# Return a 2-tuple consisting of the next sample and the the next state.
# In this case they are identical, and either a new proposal (if accepted)
# or the previous proposal (if not accepted).
function AbstractMCMC.step(
    rng::Random.AbstractRNG,
    model::DensityModel,
    sampler::MHSampler,
    transition_prev::AbstractTransition;
    kwargs...
)
    # Generate a new proposal.
    candidate = propose(rng, sampler, model, transition_prev)

    # Calculate the log acceptance probability and the log density of the candidate.
    logdensity_candidate = logdensity(model, candidate)
    logα = logdensity_candidate - logdensity(model, transition_prev) +
        logratio_proposal_density(sampler, transition_prev, candidate)

    # Decide whether to return the previous params or the new one.
    transition = if -Random.randexp(rng) < logα
        AdvancedMH.transition(sampler, model, candidate, logdensity_candidate)
    else
        transition_prev
    end

    return transition, transition
end

function logratio_proposal_density(
    sampler::MetropolisHastings, transition_prev::AbstractTransition, candidate
)
    return logratio_proposal_density(sampler.proposal, transition_prev.params, candidate)
end


text:
#=##############################################################################
# DESCRIPTION
    Windcraft vehicle geometry generation.

# AUTHORSHIP
  * Author    : Judd Mehr and Eduardo J. Alvarez
  * Email     : Edo.AlvarezR@gmail.com
  * Created   : Apr 2020
  * License   : MIT
=###############################################################################

"""
    Generates the geometry of M600 windcraft
"""
function generate_geometry_windcraft(;  # INCLUSION OPTIONS:
                                        circlepath      = true,
                                        includewing     = true,
                                        wingalpha       = 4.0,
                                        includetail     = true,
                                        includepylons   = true,
                                        includerotors   = true,
                                        numrotors       = 8,
                                        counterrotate   = true,
                                        includecontrols = true,
                                        inlinerotors    = false,
                                        # REFINEMENT OPTIONS
                                        numbladeelements= 15,
                                        latticepermeter = 1,
                                        # AIRCRAFT OPTIONS
                                        rotor_file      = "propturbo-M600.csv",
                                        data_path       = uns.def_data_path,
                                        xfoil           = true,
                                        n_factor::Int   = 1,
                                        # OUTPUT OPTIONS
                                        run_name        = "windcraft",
                                        verbose         = true,
                                        v_lvl           = 0)

    ############################################################################
    # PARAMETERS: This block just defines parameters needed to create geometry.
    ############################################################################

    #Print What's Happening as we go
    gt.verbalize("Defining parameters: ", v_lvl, verbose)

    # ------------ GEOMETRIC PARAMETERS (meters) -------------------------------
    # ----- Main Wing (wing system)----- #
    #Set Global Origin and Axis
    vehicleorigin = [0.0; 0.0; 0.0]
    vehicleaxis = [1.0 0.0 0.0; 0.0 1.0 0.0; 0.0 0.0 1.0]

    #TODO Need to figure out how to best define lattices.
    # latticepermeter = 1.25

    rootspan         = 12.5 #INPUT span of flap (root section span)
    tipspan          = 13.5 #INPUT span of aileron (tip section span)
    if includewing == true
        if includecontrols == true
            # --- Control Surface Parameters (simple wing) --- #
            # Flaps
            flapchord           = 0.4*1.19 #chord length of flaps
            flapAR              = rootspan/flapchord #INPUT flap aspect ratio
            flapchordlength     = [flapchord; flapchord]
            flapchordpos        = [0.0; 1.0]
            leftflapchordpos    = [-1.0; 0.0]
            flaptwist           = zeros(2) #INPUT flap root section twist
            flapsweep           = [0.0] #INPUT flap span sweep
            flapdihedral        = [0.0] #INPUT flap span dihedral
            flapnumlattice      = round(Int,latticepermeter*rootspan) #TODO figure out how many needed.
            # Ailerons
            aileronchord         = flapchord #chord length of ailerons

            aileronAR            = tipspan/aileronchord #INPUT aileron aspect ratio
            aileronchordlength   = [aileronchord; aileronchord]
            rightaileronchordpos = [0.0; 1.0]
            leftaileronchordpos  = [-1.0; 0.0]
            ailerontwist         = zeros(2) #INPUT aileron root section twist
            ailerondihedral      = [0.0] #INPUT aileron span dihedral
            aileronnumlattice    = round(Int,latticepermeter*tipspan) #TODO figure out how many needed.

            # --- Main Wing Parameters (complex wing) --- #
            mainwingspan        = 26.0 #INPUT main wing total span
            #note that chords of main wing are without contributions from ailerons/flaps
            mainwingrootchord   = 1.666 - flapchord #chord length of root, minus flap
            mainwingtipchord    = 0.8925 - flapchord #chord length of tip, minus flap
            mainwingAR          = mainwingspan/mainwingtipchord #INPUT 'Aspect Ratio'
            mainwingchordlength = [mainwingrootchord; mainwingrootchord; mainwingtipchord]./mainwingtipchord #INPUT section chord lengths along semi-span (from center)
            mainwingchordpos    = [0.0; rootspan; mainwingspan]./mainwingspan #INPUT spanwise locations of chord sections (from center)
            mainwingtwist       = zeros(3) #INPUT chord section twist
            mainwingsweep       = [0.0; 1.5] #INPUT span section sweep
            mainwingdihedral    = [0.0; 2.5] #INPUT span section dihedral
            mainwingnumlattice  = round(Int,latticepermeter*mainwingspan) #TODO figure out how many needed.

            #Moving Control Surfaces to correct positions
            y1                  = tipspan/2*tand(mainwingsweep[2])
            d1                  = mainwingrootchord - y1
            y2                  = d1 - mainwingtipchord
            aileronsweep        = [0.0]
            aileronroll         = -2.58
            aileronyaw          = -(180/pi * atan( y2,tipspan/2 ) ) #INPUT aileron span sweep
            flappos             = [mainwingrootchord; 0.0; 0.0]
            rightaileronpos     = [mainwingrootchord; rootspan/2; 0.0]
            leftaileronpos      = [mainwingrootchord; -rootspan/2; 0.0]
            rightaileronaxis    = rotate("x",aileronroll)*rotate("z",aileronyaw)*vehicleaxis
            leftaileronaxis     = rotate("x",-aileronroll)*rotate("z",-aileronyaw)*vehicleaxis

        else
            # --- Main Wing Parameters [INCLUDING CONTROL SURFACE CHORD LENGTHS (complex wing) --- #
            mainwingspan        = 26.0 #INPUT main wing total span
            #note that chords of main wing are without contributions from ailerons/flaps
            mainwingrootchord   = 1.666 #- flapchord #chord length of root, minus flap
            mainwingtipchord    = 0.8925 #- flapchord #chord length of tip, minus flap
            mainwingAR          = mainwingspan/mainwingtipchord #INPUT 'Aspect Ratio'
            mainwingchordlength = [mainwingrootchord; mainwingrootchord; mainwingtipchord]./mainwingtipchord #INPUT section chord lengths along semi-span (from center)
            mainwingchordpos    = [0.0; rootspan; mainwingspan]./mainwingspan #INPUT spanwise locations of chord sections (from center)
            mainwingtwist       = zeros(3) #INPUT chord section twist
            mainwingsweep       = [0.0; 1.5] #INPUT span section sweep
            mainwingdihedral    = [0.0; 2.5] #INPUT span section dihedral
            mainwingnumlattice  = round(Int,latticepermeter*mainwingspan) #TODO figure out how many needed.
        end

    else
        mainwing            = []
        mainwingspan        = 26.0
        mainwingtipchord    = 0.8925
        mainwingAR          = mainwingspan/mainwingtipchord
    end

    # If we want to add the Empennage, here are the parameters for it
    if includetail == true
        # --- Horizontal Stabilizer (simple wing) --- #
        hstabspan           = 4.9
        hstabrootchord      = 1.0
        hstabtipchord       = 0.4
        hstabAR             = hstabspan/hstabtipchord
        hstabtaperratio     = 0.4
        hstabchordlength    = [hstabrootchord; hstabtipchord]./hstabtipchord
        hstabtwist          = 0.0
        hstabsweep          = 14.0
        hstabdihedral       = 0.0
        hstabpos            = [6.6; 0.0; -0.875]
        hstabrotation       = 3.0*pi/180
        hstabaxis           = [cos(hstabrotation) -sin(hstabrotation) 0; sin(hstabrotation) cos(hstabrotation) 0; 0 0 1]

        # --- Verticatal Stabilizer (complex wing) --- #
        vstabspan           = 4.0
        vstabrootchord      = vstabspan/(3.3*2) #1.3/2
        vstabtipchord       = vstabrootchord*0.8
        vstabmidchord       = vstabrootchord*1.125
        vstabAR             = vstabspan/vstabtipchord
        vstablchordlength   = [vstabrootchord; vstabmidchord; vstabtipchord]./vstabtipchord
        vstabchordpos       = [0.0; 0.25; 1.0]
        vstabtwist          = [0.0; 0.0; 0.0]
        vstabsweep          = [0.0; 0.0]
        vstabdihedral       = [0.0; 0.0]
        vstabnumlattice     = round(Int,latticepermeter*2*vstabspan)
        vstabsymmetry       = false
        vstabxpos           = 7.0
        vstabzpos           = -0.875
        vstabpos            = [vstabxpos; 0.0; vstabzpos]
        vstabaxis           = [1.0 0.0 0.0; 0.0 0.0 1.0; 0.0 1.0 0.0]

        # --- Rudder (complex wing) --- #
        rudderspan          = vstabspan
        rudderrootchord     = vstabrootchord*.3
        ruddertipchord      = rudderrootchord*0.8
        ruddermidchord      = rudderrootchord*1.25
        rudderAR            = rudderspan/ruddertipchord
        rudderlchordlength  = [rudderrootchord; ruddermidchord; ruddertipchord]./ruddertipchord
        rudderchordpos      = [0.0; 0.25; 1.0]
        ruddertwist         = [0.0; 0.0; 0.0]
        ruddersweep         = [0.0; 0.0]
        rudderdihedral      = [0.0; 0.0]
        ruddernumlattice    = round(Int,latticepermeter*2*rudderspan)
        ruddersymmetry      = false
        rudderpos           = [vstabxpos+vstabrootchord; 0.0; vstabzpos] #7.59
    end #if we want the empennage

    #If we want to include the rotors, do it here.
    # --- Rotors --- #
    rotorspacing    = 2.4
    if includerotors == true
        toprotorxpos    = -1.4
        bottomrotorxpos = toprotorxpos #-1.35

        #if we have inline rotors
        if inlinerotors == true
            if numrotors == 1
                n_rotors    = 1
                rotorpos1   = [toprotorxpos; 0.0; 0.0]
                rotorposs   = rotorpos1
                rotorccw    = [1]
            elseif numrotors == 2
                n_rotors = 2
                rotorpos1   = [toprotorxpos; 0.0; rotorspacing/2.0]
                rotorpos2   = [toprotorxpos; 0.0; -rotorspacing/2.0]
                rotorposs   = [rotorpos1 rotorpos2]
                if counterrotate == true
                    rotorccw        = [1,2]
                else
                    rotorccw        = [1,1]
                end
            else
                gt.verbalize("number specified rotors: $numrotors.  4 being defined...", v_lvl, verbose)
                n_rotors        = 4
                rotorpos1  = [toprotorxpos; 0.5*rotorspacing; rotorspacing/2.0]
                rotorpos2  = [toprotorxpos; 0.5*rotorspacing; -rotorspacing/2.0]
                rotorpos3  = [toprotorxpos; -0.5*rotorspacing; rotorspacing/2.0]
                rotorpos4  = [toprotorxpos; -0.5*rotorspacing; -rotorspacing/2.0]
                rotorposs  = [rotorpos1 rotorpos2 rotorpos3 rotorpos4]
                rotorccw        = [1,2,1,2]
            end
        #if we have offset rotors
        else
            n_rotors        = 8
            toprotorxpos    = -1.4
            bottomrotorxpos = -1.35
            rotorspacing    = 2.4
            # Position of rotors on main wing numbering starts on top right (lookingforward from windcraft) and reads counterclockwise
            rotorpos1 = [toprotorxpos; 1.5*rotorspacing; rotorspacing/2.0]
            rotorpos2 = [toprotorxpos; 0.5*rotorspacing; rotorspacing/2.0]
            rotorpos3 = [toprotorxpos; -0.5*rotorspacing; rotorspacing/2.0]
            rotorpos4 = [toprotorxpos; -1.5*rotorspacing; rotorspacing/2.0]
            rotorpos5 = [bottomrotorxpos; -1.5*rotorspacing; -rotorspacing/2.0]
            rotorpos6 = [bottomrotorxpos; -0.5*rotorspacing; -rotorspacing/2.0]
            rotorpos7 = [bottomrotorxpos; 0.5*rotorspacing; -rotorspacing/2.0]
            rotorpos8 = [bottomrotorxpos; 1.5*rotorspacing; -rotorspacing/2.0]
            rotorposs = [rotorpos1 rotorpos2 rotorpos3 rotorpos4 rotorpos5 rotorpos6 rotorpos7 rotorpos8]
            rotorccw  = [1,1,2,2,1,1,2,2]
        end

        rotorpitch       = 0.0
        # numbladeelements = 15 #todo how many?

    end #if we want rotors

    # if we want to add the pylons, do that here.
    if includepylons == true
        # --- Pylons (wing systems) --- #
        #due to odd geometry, need to make pylons 2-wing (complex) systems.
        # Top section
        pylontoprootchord   = 0.7
        pylontoptipchord    = 0.9
        pylontopspan        = rotorspacing
        pylontopAR          = pylontopspan/pylontoptipchord
        pylontopchordlength = [pylontoprootchord; pylontoptipchord]
        pylontopchordpos    = [0.0; 1.0]
        pylontoptwist       = [0.0; 0.0]
        pylontopsweep       = [-6.0]
        pylontopdihedral    = [0.0]
        pylontopnumlattice  = round(Int,latticepermeter*2*pylontopspan)
        pylontopsymmetry    = false
        pylonxshift         = -0.9
        pylonspacing        = 2.4
        pylontoppos1        = [pylonxshift; 1.5*pylonspacing; 0]
        pylontoppos2        = [pylonxshift; 0.5*pylonspacing; 0]
        pylontoppos3        = [pylonxshift; -0.5*pylonspacing; 0]
        pylontoppos4        = [pylonxshift; -1.5*pylonspacing; 0]
        pylontopaxis        = [1.0 0.0 0.0; 0.0 0.0 1.0; 0.0 1.0 0.0]

        # Bottom Section
        pylonbottomspan         = rotorspacing
        pylonbottomrootchord    = 1.7
        pylonbottomtipchord     = 1.0
        pylonbottomAR           = pylonbottomspan/pylonbottomtipchord
        pylonbottomchordlength  = [pylonbottomrootchord; pylonbottomtipchord]
        pylonbottomchordpos     = [0.0; 1.0]
        pylonbottomtwist        = [0.0; 0.0]
        pylonbottomsweep        = [3.0]
        pylonbottomdihedral     = [0.0]
        pylonbottomnumlattice   = round(Int,latticepermeter*2*pylonbottomspan)
        pylonbottomsymmetry     = false
        pylonbottompos1         = [pylonxshift; 1.5*pylonspacing; 0]
        pylonbottompos2         = [pylonxshift; 0.5*pylonspacing; 0]
        pylonbottompos3         = [pylonxshift; -0.5*pylonspacing; 0]
        pylonbottompos4         = [pylonxshift; -1.5*pylonspacing; 0]
        pylonbottomaxis         = [1.0 0.0 0.0; 0.0 0.0 -1.0; 0.0 1.0 0.0]
    end #if we want pylons

    ############################################################################
    # ASSEMBLY: This block actually builds the geometry using the above parameters
    ############################################################################

    gt.verbalize("Generating geometry...", v_lvl, verbose)

    # --- Generate MAIN WING --- #
    gt.verbalize("Generating Main Ming...", v_lvl, verbose)

    if includewing == true
        mainwing = vlm.complexWing(mainwingspan, mainwingAR, mainwingnumlattice, mainwingchordpos, mainwingchordlength, mainwingtwist, mainwingsweep, mainwingdihedral; symmetric=true, chordalign=0.0,_ign1=true) #? what is _ign1=true
        #set wing an angle of attack
        vlm.setcoordsystem(mainwing, vehicleorigin, rotate("y",-wingalpha,unit="deg")*vehicleaxis)
    end

    if includewing ==true && includecontrols == true
        gt.verbalize("Generating Control Surfaces...", v_lvl, verbose)
        # --- Generate Control surfaces --- #
        rightaileron = vlm.complexWing(tipspan, aileronAR, aileronnumlattice, rightaileronchordpos, aileronchordlength, ailerontwist,
        aileronsweep, ailerondihedral; symmetric=false, chordalign=0.0,_ign1=true)
        vlm.setcoordsystem(rightaileron, rightaileronpos,rightaileronaxis)

        rightflap = vlm.complexWing(rootspan, flapAR, flapnumlattice, flapchordpos, flapchordlength, flaptwist, flapsweep, flapdihedral; symmetric=false, chordalign=0.0,_ign1=true)
        vlm.setcoordsystem(rightflap, flappos, vehicleaxis)

        leftflap = vlm.complexWing(rootspan, flapAR, flapnumlattice, leftflapchordpos, flapchordlength, flaptwist, flapsweep, flapdihedral; symmetric=false, chordalign=0.0,_ign1=true)
        vlm.setcoordsystem(leftflap, flappos, vehicleaxis)

        leftaileron = vlm.complexWing(tipspan, aileronAR, aileronnumlattice, leftaileronchordpos, aileronchordlength, ailerontwist,
        aileronsweep, -ailerondihedral; symmetric=false, chordalign=0.0,_ign1=true)
        vlm.setcoordsystem(leftaileron, leftaileronpos, leftaileronaxis)
    end

    # --- Assemble Main Wing (with control surfaces) --- #
    # Assembles moving sections of the wing
    mainwingsystem = vlm.WingSystem()
    if includewing == true
        vlm.addwing(mainwingsystem, "mainwing", mainwing)
    end

    if includewing == true && includecontrols == true
        vlm.addwing(mainwingsystem, "rightaileron", rightaileron)
        vlm.addwing(mainwingsystem, "rightflap", rightflap)
        vlm.addwing(mainwingsystem, "leftflap", leftflap)
        vlm.addwing(mainwingsystem, "leftaileron", leftaileron)
    end
    # --- Start Whole System Assembly --- #
    # Create system assembly
    gt.verbalize("Initializing System Assembly...", v_lvl, verbose)
    system = vlm.WingSystem()
    vlm.addwing(system, "MainWing", mainwingsystem)

    gt.verbalize("Initializing VLM System...", v_lvl, verbose)
    # System to solve through the VLM solver
    vlm_system = vlm.WingSystem()
    if includewing == true
        vlm.addwing(vlm_system, "mainwing", mainwing)
    end

    if includewing == true && includecontrols == true
        vlm.addwing(vlm_system, "rightaileron", rightaileron)
        vlm.addwing(vlm_system, "rightflap", rightflap)
        vlm.addwing(vlm_system, "leftflap", leftflap)
        vlm.addwing(vlm_system, "leftaileron", leftaileron)
    end

    #make dummy rotor in case there aren't any
    rotors = vlm.Rotor[]
    rotor_systems = ()

    #if we want rotors, generate them and add them to the wing system here
    if includerotors == true
        gt.verbalize("Generating Rotors...", v_lvl, verbose)
        # --- Generate ROTORS --- #
        R, B = uns.read_rotor(rotor_file; data_path=data_path)[[1,3]]
        ##more rotor definition stuff:
        n = 200.0 #target RPS
        Vinf = 40.0 #freestream velocity
        vind = sqrt( Vinf^2 + (n*0.84)^2 ) #velocity at 70% blade span
        # Simulation parameters
        J = Vinf/(n*2.0*R)                      # Advance ratio Vinf/(nD)
        rho = 1.225                         # (kg/m^3) air density
        mu = 1.81e-5                        # (kg/ms) air dynamic viscosity
        ReD07 = rho*2.0*R*0.7*vind/mu            # Diameter-based Reynolds at 70% span
        ReD = ReD07/0.7                     # Diameter-based Reynolds

        # Generates base rotors (one on each rotation orientation)
        props = vlm.Rotor[]
        gt.verbalize("Generating first propeller...", v_lvl, verbose)
        @time push!(props, uns.generate_rotor(rotor_file; pitch=rotorpitch,
        n=numbladeelements, CW=true, ReD=ReD,
        verbose=verbose, xfoil=xfoil,
        data_path=data_path,
        # plot_disc=plot_disc,
        verbose=verbose, v_lvl=v_lvl+2))

        gt.verbalize("Generating second propeller...", v_lvl, verbose)
        @time push!(props, vlm.Rotor(!props[1].CW, props[1].r,
                                props[1].chord, props[1].theta,
                                props[1].LE_x, props[1].LE_z,
                                props[1].B, props[1].airfoils))
        @time vlm.initialize(props[2], props[1].m)

        # --- Assemble Rotors --- #
        gt.verbalize("Adding Rotors to Main Wing...", v_lvl, verbose)
        rotors = vlm.Rotor[]
        for i = 1:n_rotors
            copy_prop = props[rotorccw[i]]
            this_prop = deepcopy(copy_prop) # Alternates rotation orientation
            this_O = rotorposs[:,i]
            vlm.setcoordsystem(this_prop, this_O, vehicleaxis; user=true)

            # Rotates props to be tip to tip #?what does this do?
            # vlm.rotate(this_prop, (-1)^(!CW_w) * init_ori_prop)

            # Adds the original polars that don't get copied in deepcopy
            this_prop.airfoils = copy_prop.airfoils
            this_prop._polars = copy_prop._polars
            this_prop._polarroot = copy_prop._polarroot
            this_prop._polartip = copy_prop._polartip

            push!(rotors, this_prop)
        end

        for (i, rotor) in enumerate(rotors)
            vlm.addwing(mainwingsystem, "rotor$i", rotor)
        end

        # --- Define rotor system --- #
        gt.verbalize("Creating Rotor System...", v_lvl, verbose)
        # Rotors grouped by systems of the same RPM
        rotor_systems = (rotors,)
    end #if adding rotors

    # if we want an empennage, generate it and add it here.
    if includetail == true
        # --- Generate Empennage --- #
        gt.verbalize("Generating Empennage...", v_lvl, verbose)

        gt.verbalize("Generating Horizontal Stabilizer...", v_lvl, verbose)
        #Hstab:
        horizontalstabilizer = vlm.simpleWing(hstabspan, hstabAR, hstabtaperratio, hstabtwist, hstabsweep, hstabdihedral)
        vlm.setcoordsystem(horizontalstabilizer,hstabpos,hstabaxis)

        gt.verbalize("Generating Vertical Stabilizer...", v_lvl, verbose)
        #Vstab:
        verticalstabilizer = vlm.complexWing(vstabspan,vstabAR,vstabnumlattice,vstabchordpos,vstablchordlength,vstabtwist,vstabsweep,vstabdihedral,symmetric=false, chordalign=1.0, _ign1=true)
        vlm.setcoordsystem(verticalstabilizer,vstabpos,vstabaxis)
        rudder = vlm.complexWing(rudderspan,rudderAR,ruddernumlattice,rudderchordpos,rudderlchordlength,ruddertwist,ruddersweep,rudderdihedral,symmetric=false, chordalign=0, _ign1=true)
        vlm.setcoordsystem(rudder,rudderpos,vstabaxis)

        # --- Assemble Empennage --- #
        empennagesystem = vlm.WingSystem()
        vlm.addwing(empennagesystem, "hstab", horizontalstabilizer)
        vlm.addwing(empennagesystem, "vstab", verticalstabilizer)
        vlm.addwing(empennagesystem, "rudder", rudder)

        # --- Add Empennage to System Assembly --- #
        gt.verbalize("Adding Empennage to System Assembly...", v_lvl, verbose)
        vlm.addwing(system, "Empennage", empennagesystem)

        # --- Add Empennage to VLM Assembly --- #
        gt.verbalize("Adding Empennage to VLM Assembly...", v_lvl, verbose)
        vlm.addwing(vlm_system, "vstab", verticalstabilizer)
        vlm.addwing(vlm_system, "rudder", rudder)
        vlm.addwing(vlm_system, "hstab", horizontalstabilizer)

    end #if adding empennage

    #if we wanted the pylons, generate and add here
    if includepylons == true
        gt.verbalize("Generating Pylons...", v_lvl, verbose)
        # --- Generate Pylons --- #
        pylon1t = vlm.complexWing(pylontopspan,pylontopAR,pylontopnumlattice,pylontopchordpos,pylontopchordlength,pylontoptwist,pylontopsweep,pylontopdihedral,symmetric=false, chordalign=0.0, _ign1=true)
        vlm.setcoordsystem(pylon1t,pylontoppos1,pylontopaxis)
        pylon1b = vlm.complexWing(pylonbottomspan,pylonbottomAR,pylonbottomnumlattice,pylonbottomchordpos,pylonbottomchordlength,pylonbottomtwist,pylonbottomsweep,pylonbottomdihedral,symmetric=false, chordalign=0.0, _ign1=true)
        vlm.setcoordsystem(pylon1b,pylonbottompos1,pylonbottomaxis)

        pylon2t = vlm.complexWing(pylontopspan,pylontopAR,pylontopnumlattice,pylontopchordpos,pylontopchordlength,pylontoptwist,pylontopsweep,pylontopdihedral,symmetric=false, chordalign=0.0, _ign1=true)
        vlm.setcoordsystem(pylon2t,pylontoppos2,pylontopaxis)
        pylon2b = vlm.complexWing(pylonbottomspan,pylonbottomAR,pylonbottomnumlattice,pylonbottomchordpos,pylonbottomchordlength,pylonbottomtwist,pylonbottomsweep,pylonbottomdihedral,symmetric=false, chordalign=0.0, _ign1=true)
        vlm.setcoordsystem(pylon2b,pylonbottompos2,pylonbottomaxis)

        pylon3t = vlm.complexWing(pylontopspan,pylontopAR,pylontopnumlattice,pylontopchordpos,pylontopchordlength,pylontoptwist,pylontopsweep,pylontopdihedral,symmetric=false, chordalign=0.0, _ign1=true)
        vlm.setcoordsystem(pylon3t,pylontoppos3,pylontopaxis)
        pylon3b = vlm.complexWing(pylonbottomspan,pylonbottomAR,pylonbottomnumlattice,pylonbottomchordpos,pylonbottomchordlength,pylonbottomtwist,pylonbottomsweep,pylonbottomdihedral,symmetric=false, chordalign=0.0, _ign1=true)
        vlm.setcoordsystem(pylon3b,pylonbottompos3,pylonbottomaxis)

        pylon4t = vlm.complexWing(pylontopspan,pylontopAR,pylontopnumlattice,pylontopchordpos,pylontopchordlength,pylontoptwist,pylontopsweep,pylontopdihedral,symmetric=false, chordalign=0.0, _ign1=true)
        vlm.setcoordsystem(pylon4t,pylontoppos4,pylontopaxis)
        pylon4b = vlm.complexWing(pylonbottomspan,pylonbottomAR,pylonbottomnumlattice,pylonbottomchordpos,pylonbottomchordlength,pylonbottomtwist,pylonbottomsweep,pylonbottomdihedral,symmetric=false, chordalign=0.0, _ign1=true)
        vlm.setcoordsystem(pylon4b,pylonbottompos4,pylonbottomaxis)

        # --- Assemble Pylons --- #
        pylon1 = vlm.WingSystem()
        vlm.addwing(pylon1, "pylon1top", pylon1t)
        vlm.addwing(pylon1, "pylon1bottom", pylon1b)

        pylon2 = vlm.WingSystem()
        vlm.addwing(pylon2, "pylon2top", pylon2t)
        vlm.addwing(pylon2, "pylon2bottom", pylon2b)

        pylon3 = vlm.WingSystem()
        vlm.addwing(pylon3, "pylon3top", pylon3t)
        vlm.addwing(pylon3, "pylon3bottom", pylon3b)

        pylon4 = vlm.WingSystem()
        vlm.addwing(pylon4, "pylon4top", pylon4t)
        vlm.addwing(pylon4, "pylon4bottom", pylon4b)

        pylonsystem = vlm.WingSystem()
        vlm.addwing(pylonsystem, "pylon1", pylon1)
        vlm.addwing(pylonsystem, "pylon2", pylon2)
        vlm.addwing(pylonsystem, "pylon3", pylon3)
        vlm.addwing(pylonsystem, "pylon4", pylon4)

        # --- Add Pylons to System Assembly --- #
        gt.verbalize("Adding Pylons to System Assembly...", v_lvl, verbose)
        vlm.addwing(system, "Pylons", pylonsystem)

        # --- Add Pylons to VLM Assembly --- #
        gt.verbalize("Adding Pylons to VLM Assembly...", v_lvl, verbose)
        vlm.addwing(vlm_system, "pylon1", pylon1)
        vlm.addwing(vlm_system, "pylon2", pylon2)
        vlm.addwing(vlm_system, "pylon3", pylon3)
        vlm.addwing(vlm_system, "pylon4", pylon4)

    end #if adding pylons

    # ------------ OUTPUTS------------------------------------------------

    #Rotate to be in correct starting pos.
    O = zeros(3)
    if circlepath == true
        Oaxis = [0 -1 0; 0 0 1; 1 0 0]
    else
        Oaxis = [1 0 0; 0 1 0; 0 0 1]
    end
    vlm.setcoordsystem(system, O, Oaxis)

    # Tilting systems
    gt.verbalize("Creating Tilting System...", v_lvl, verbose)
    if includetail == true
        tilting_wings = (rightaileron,rightflap,leftflap,leftaileron,rudder,horizontalstabilizer)
    elseif includewing == true && includecontrols == true
        tilting_wings = (rightaileron,rightflap,leftflap,leftaileron)
    else
        tilting_wings = ()
    end

    # Convert the Wings into WingSystems
    tilting_systems = []
    for (wi, wing) in enumerate(tilting_wings)
        sys = vlm.WingSystem()
        vlm.addwing(sys, "TiltWing$wi", wing)
        push!(tilting_systems, sys)
    end
    tilting_systems = Tuple(tilting_systems)

    gt.verbalize("Creating Wake System...", v_lvl, verbose)
    # Wake-shedding system (`vlm_system`+`rotors`)
    wake_system = vlm.WingSystem()
    vlm.addwing(wake_system, "SolveVLM", vlm_system)
    if includerotors == true
        for (i, rotor) in enumerate(rotors)
            vlm.addwing(wake_system, "Rotor$i", rotor)
        end
    end

    gt.verbalize("Completed Geometry Generation and Assembly...", v_lvl, verbose)

    # OTHER MONITOR INPUTS
    # get y position of horseshoes for monitor purposes
    if includewing == true
        mainwinghorshoeypos = mainwing._ym
    else
        mainwinghorshoeypos = []
    end

    return (system, vlm_system, rotors, tilting_systems, rotor_systems, wake_system)

end

"""
CONVENIENT ROTATION FUNCTION
"""
function rotate(axis,theta;unit="deg")
    if unit=="deg"; theta*=pi/180; end; #convert to radians

    if axis == "x"
        return [1 0 0; 0 cos(theta) -sin(theta); 0 sin(theta) cos(theta)]
    elseif axis == "y"
        return [cos(theta) 0 sin(theta); 0 1 0; -sin(theta) 0 cos(theta)]
    elseif axis == "z"
        return [cos(theta) -sin(theta) 0; sin(theta) cos(theta) 0; 0 0 1]
    else
        error("That's not an axis...")
    end
end


text:
"""
    DataDiGraph{T,VL,VD,ED,GD} <: AbstractDataGraph{T}

Structure for graphs with metadata based on adjacency list storage.

# Fields
- `ne::Int`: number of edges
- `fadjlist::Vector{Vector{Int}}`: forward adjacency lists
- `badjlist::Vector{Vector{Int}}`: backward adjacency lists
- `labels::Vector{VL}`: list of vertex labels
- `vertices::Dict{VL,T}`: dictionary mapping each vertex label to the associated integer index `v`
- `vertex_data::Vector{VD}`: list of vertex data objects, indexed by integers `v`
- `edge_data::Vector{Vector{ED}}`: list of edge data objects, indexed by `s` first and `d_index` second, where `d_index` is the rank of `d` among the outneighbors of `s`
- `graph_data::GD`: single graph data object
"""
mutable struct DataDiGraph{T<:Integer,VL,VD,ED,GD} <: AbstractDataGraph{T,VL,VD,ED,GD}
    ne::Int
    fadjlist::Vector{Vector{T}}
    badjlist::Vector{Vector{T}}
    labels::Vector{VL}
    vertices::Dict{VL,T}
    vertex_data::Vector{VD}
    edge_data::Vector{Vector{ED}}
    graph_data::GD
end

"""
    DataDiGraph{T}(VL, VD, ED, graph_data)

Constructor taking only label and data types to create an empty [`DataDiGraph`](@ref).
"""
function DataDiGraph{T}(; VL=Symbol, VD=Nothing, ED=Nothing, graph_data=nothing) where {T}
    ne = 0
    fadjlist = Vector{T}[]
    badjlist = Vector{T}[]
    labels = VL[]
    vertices = Dict{VL,T}()
    vertex_data = VD[]
    edge_data = Vector{ED}[]
    return DataDiGraph(
        ne, fadjlist, badjlist, labels, vertices, vertex_data, edge_data, graph_data
    )
end

## Link between vertices and labels

"""
    haskey(g, label)

Check whether a vertex with label `label` exists.
"""
Base.haskey(g::DataDiGraph{T,VL}, label::VL) where {T,VL} = haskey(g.vertices, label)

"""
    get_vertex(g, label)

Retrieve the vertex associated with label `label.
"""
get_vertex(g::DataDiGraph{T,VL}, label::VL) where {T,VL} = g.vertices[label]

"""
    get_label(g, v)

Retrieve the label associated with vertex `v`.
"""
get_label(g::DataDiGraph, v::Integer) = g.labels[v]


text:
# Segment image into red ball (filtered to red channel only) and background (conveted to grayscale)
# returns total number of red pixels, and x and y coordinates at centroid of red pixels
function segment_image(image::AbstractArray, img_width::Int, img_height::Int, drawcross::Bool = true;
        crosscolor = RGB{N0f8}(0, 1, 0))
    # Initialize counters
    c = 0; xs = 0; ys = 0

    # Count, sum, and segment
    for x = 1:img_width
        for y = 1:img_height
            p = image[x, y]

            # Check if red channel contribution is at least twice the green or blue channel for this pixel
            is_red = p.r > .1 && p.g < p.r / 2 && p.b < p.r / 2

            # Segment between ball (render red channel only) and the background (conveted to grayscale)
            if is_red
                c += 1; xs += x; ys += y
                image[x, y] = RGB{N0f8}(p.r, 0, 0)
            else
                image[x, y] = convert(ColorTypes.Gray{N0f8}, p)
            end
        end
    end

    # Return if no red pixels found
    c == 0 && return 0, 0.0, 0.0    # centroid values are invalid

    x = round(Int, xs / c)
    y = round(Int, ys / c)

    # Check if cross would be clipped
    fits_inside = x > 1 && y > 1 && x < img_width && y < img_height

    if drawcross && fits_inside
        # Draw crosshairs
        image[x, y]   = crosscolor
        image[x-1, y] = crosscolor
        image[x+1, y] = crosscolor
        image[x, y-1] = crosscolor
        image[x, y+1] = crosscolor
    end

    # Normalize mean point around center point
    x /= img_width; y /= img_height
    x -= 0.5; y -= 0.5

    return c, x, y # count of red pixels, mean x value, mean y value (translated and scaled)
end

# Acquire image for the inset view
function get_cameraview_image(s::mjSim)
    # Compute the necessary buffer size to hold the camera view image
    img_width = Int(s.camviewport.width)    # Convert from Int32s to Ints with WORD_SIZE bits (64)
    img_height = Int(s.camviewport.height)
    buflen = img_width * img_height * sizeof(TPixel)

    # We may need to allocate a new buffer if user has resized the window
    if length(s.cambuf) != buflen
        s.cambuf = Vector{UInt8}(undef, buflen)
    end

    # Save current camera view
    current_camid = s.cam[].fixedcamid
    current_camtype = s.cam[].type

    # Switch to head mounted camera view
    s.cam[].fixedcamid = s.camid
    s.cam[].type = MJCore.mjCAMERA_FIXED

    # Update camera for the head mounted camera
    mjv_updateCamera(s.m, s.d, s.cam, s.scn)

    # Get the pixels from MuJoCo for the inset view
    mjr_render(s.camviewport, s.scn, s.con)                 # 1 allocation: 32 bytes!
    mjr_readPixels(s.cambuf, C_NULL, s.camviewport, s.con)  # 1 allocation: 32 bytes!

    # Restore current camera view
    s.cam[].fixedcamid = current_camid
    s.cam[].type = current_camtype

    # Update camera for the restored camera
    mjv_updateCamera(s.m, s.d, s.cam, s.scn)

    # Shape the buffer into an image array of RGB pixels
    image = reshape(reinterpret(TPixel, s.cambuf), img_width, img_height)

    # Isolate the red ball
    c, x, y = segment_image(image, img_width, img_height)
    #println("$c, $x, $y")

    # Flip the image horizonally for ImGui and reshape back to UInt8 buffer
    reshape(reinterpret(UInt8, hflip!(image)), (buflen,))
end

function render_camera_view(s::mjSim)
    # Start the Dear ImGui frame for Picture-in-picture (PiP) inset camera view
    ImGui_ImplOpenGL3_NewFrame()
    ImGui_ImplGlfw_NewFrame()
    CImGui.NewFrame()

    # Show image
    CImGui.Begin("Camera View")
    ImGui_ImplOpenGL3_UpdateImageTexture(s.camviewid, s.cambuf, s.camviewport.width, s.camviewport.height, format = GL_RGB)
    CImGui.Image(Ptr{Cvoid}(s.camviewid), (s.camviewport.width, s.camviewport.height))
    CImGui.End()

    # ImGui rendering
    CImGui.Render()
    ImGui_ImplOpenGL3_RenderDrawData(CImGui.GetDrawData())
end


text:
function test_density_matrix()
    effective_spectrum, U_array = sub_partition_weight(L)
    Z = recursion_partition_function(effective_spectrum)
    D = complex(zeros(num_sites, num_sites))
    for i = 1 : num_sites
        for j = 1 : num_sites
            n_i = recursion_OccNum(Z, effective_spectrum)
            D[i, j] = sum(U_array[i, j, :] .* n_i[num_bosons, :])
        end
    end
    return Z[num_bosons], D
end

function test_density_matrix_sampling(num_iterations) # tests for one-body density matrices
    partition_func_array = Vector{Complex{Float64}}()
    density_mat_array = Matrix{Complex{Float64}}[]
    for N = 1 : num_iterations
        partition_func_sample, density_mat_sample = test_density_matrix()
        push!(partition_func_array, partition_func_sample)
        push!(density_mat_array, density_mat_sample)
    end
    Z_mean = mean(partition_func_array)
    Z_error_bar = stdm(partition_func_array, Z_mean) / sqrt(num_iterations)
    D_mean = mean(density_mat_array) / Z_mean
    return Z_mean, Z_error_bar, D_mean
end

function test_matrix_product(num_iterations)
    mat_product_array = Vector{Array{Complex{Float64},1}}()
    for N = 1 : num_iterations
        mat_product_sample = sub_partition_weight(L)
        push!(mat_product_array, mat_product_sample)
    end
    mat_product_mean = mean(mat_product_array)
end

function test_partition_function(num_iterations)
    partition_func_array = Vector{Complex{Float64}}()
    for i = 1 : num_iterations
        effective_spectrum, U_array = sub_partition_weight(L)
        Z = recursion_partition_function(effective_spectrum)
        push!(partition_func_array, Z[num_bosons])
    end
    Z_mean = mean(partition_func_array)
    Z_error_bar = stdm(partition_func_array, Z_mean) / num_iterations
    return Z_mean, Z_error_bar
end

max_stars_repo_name: TongSericus/CanonicalAFQMC

max_issues_repo_name: TongSericus/CanonicalAFQMC

max_forks_repo_name: TongSericus/CanonicalAFQMC