text:
"""Work out the first ten digits of the sum of the following one-hundred 50-digit numbers."""

numbers = """37107287533902102798797998220837590246510135740250
46376937677490009712648124896970078050417018260538
74324986199524741059474233309513058123726617309629
91942213363574161572522430563301811072406154908250
23067588207539346171171980310421047513778063246676
89261670696623633820136378418383684178734361726757
28112879812849979408065481931592621691275889832738
44274228917432520321923589422876796487670272189318
47451445736001306439091167216856844588711603153276
70386486105843025439939619828917593665686757934951
62176457141856560629502157223196586755079324193331
64906352462741904929101432445813822663347944758178
92575867718337217661963751590579239728245598838407
58203565325359399008402633568948830189458628227828
80181199384826282014278194139940567587151170094390
35398664372827112653829987240784473053190104293586
86515506006295864861532075273371959191420517255829
71693888707715466499115593487603532921714970056938
54370070576826684624621495650076471787294438377604
53282654108756828443191190634694037855217779295145
36123272525000296071075082563815656710885258350721
45876576172410976447339110607218265236877223636045
17423706905851860660448207621209813287860733969412
81142660418086830619328460811191061556940512689692
51934325451728388641918047049293215058642563049483
62467221648435076201727918039944693004732956340691
15732444386908125794514089057706229429197107928209
55037687525678773091862540744969844508330393682126
18336384825330154686196124348767681297534375946515
80386287592878490201521685554828717201219257766954
78182833757993103614740356856449095527097864797581
16726320100436897842553539920931837441497806860984
48403098129077791799088218795327364475675590848030
87086987551392711854517078544161852424320693150332
59959406895756536782107074926966537676326235447210
69793950679652694742597709739166693763042633987085
41052684708299085211399427365734116182760315001271
65378607361501080857009149939512557028198746004375
35829035317434717326932123578154982629742552737307
94953759765105305946966067683156574377167401875275
88902802571733229619176668713819931811048770190271
25267680276078003013678680992525463401061632866526
36270218540497705585629946580636237993140746255962
24074486908231174977792365466257246923322810917141
91430288197103288597806669760892938638285025333403
34413065578016127815921815005561868836468420090470
23053081172816430487623791969842487255036638784583
11487696932154902810424020138335124462181441773470
63783299490636259666498587618221225225512486764533
67720186971698544312419572409913959008952310058822
95548255300263520781532296796249481641953868218774
76085327132285723110424803456124867697064507995236
37774242535411291684276865538926205024910326572967
23701913275725675285653248258265463092207058596522
29798860272258331913126375147341994889534765745501
18495701454879288984856827726077713721403798879715
38298203783031473527721580348144513491373226651381
34829543829199918180278916522431027392251122869539
40957953066405232632538044100059654939159879593635
29746152185502371307642255121183693803580388584903
41698116222072977186158236678424689157993532961922
62467957194401269043877107275048102390895523597457
23189706772547915061505504953922979530901129967519
86188088225875314529584099251203829009407770775672
11306739708304724483816533873502340845647058077308
82959174767140363198008187129011875491310547126581
97623331044818386269515456334926366572897563400500
42846280183517070527831839425882145521227251250327
55121603546981200581762165212827652751691296897789
32238195734329339946437501907836945765883352399886
75506164965184775180738168837861091527357929701337
62177842752192623401942399639168044983993173312731
32924185707147349566916674687634660915035914677504
99518671430235219628894890102423325116913619626622
73267460800591547471830798392868535206946944540724
76841822524674417161514036427982273348055556214818
97142617910342598647204516893989422179826088076852
87783646182799346313767754307809363333018982642090
10848802521674670883215120185883543223812876952786
71329612474782464538636993009049310363619763878039
62184073572399794223406235393808339651327408011116
66627891981488087797941876876144230030984490851411
60661826293682836764744779239180335110989069790714
85786944089552990653640447425576083659976645795096
66024396409905389607120198219976047599490197230297
64913982680032973156037120041377903785566085089252
16730939319872750275468906903707539413042652315011
94809377245048795150954100921645863754710598436791
78639167021187492431995700641917969777599028300699
15368713711936614952811305876380278410754449733078
40789923115535562561142322423255033685442488917353
44889911501440648020369068063960672322193204149535
41503128880339536053299340368006977710650566631954
81234880673210146739058568557934581403627822703280
82616570773948327592232845941706525094512325230608
22918802058777319719839450180888072429661980811197
77158542502016545090413245809786882778948721859617
72107838435069186155435662884062257473692284509516
20849603980134001723930671666823555245252804609722
53503534226472524250874054075591789781264330331690"""

# one can use BigInt for this but lets do this with normal ints and strings
numbers = split(numbers,"\n")

# the default, Int64 can handle 19 digit number and will be sufficient for this
length(string(typemax(Int64)))

# taking only the first 12 digits as the later digits carry-overs can affect

numbers = [parse(Int,numbers[1:12]) for numbers in numbers]

string(sum(numbers))[1:10]


text:
"""
$(SIGNATURES)
Solve a trajectory optimization problem with direct collocation

# Arguments
* model: TrajectoryOptimization.Model for the dynamics
* obj: TrajectoryOptimization.ConstrainedObjective to describe the cost function
* dt: time step. Used to determine the number of knot points. May be modified
    by the solver in order to achieve an integer number of knot points.
* method: Collocation method.
    :midpoint - Zero order interpolation on states and controls.
    :trapezoidal - First order interpolation on states and zero order on control.
    :hermite_simpson_separated - Hermite Simpson collocation with the midpoints
        included as additional decision variables with constraints
    :hermite_simpson - condensed verision of Hermite Simpson. Currently only
        supports grads=:auto or grads=:none (recommended)
* grads: Specifies the gradient information provided to SNOPT.
    :none - returns no gradient information to SNOPT
    :auto - uses ForwardDiff to calculate gradients
    :quadratic - uses functions exploiting quadratic cost functions
"""
function solve_dircol(solver::Solver,X0::Matrix,U0::Matrix;
        nlp::Symbol=:ipopt, method::Symbol=:auto, grads::Symbol=:quadratic, start=:cold, options::Dict{String,T}=Dict{String,Any}()) where T

    if solver.obj isa UnconstrainedObjective
        solver = Solver(solver,obj=ConstrainedObjective(solver.obj))
        # solver.state.constrained = false
    end

    reset_evals(solver)

    obj = solver.obj
    model = solver.model

    X0 = copy(X0)
    U0 = copy(U0)

    if method == :auto
        if solver.integration == :rk3_foh
            method = :hermite_simpson
        elseif solver.integration == :midpoint
            method = :midpoint
        else
            method = :hermite_simpson
        end
    end

    # Constants
    n,m,N = get_sizes(solver)
    m̄, = get_num_controls(solver)
    dt = solver.dt
    N = convert_N(N,method)

    if N != size(X0,2)
        @info "Interpolating initial guess"
        X0,U0 = interp_traj(N,obj.tf,X0,U0)
    end

    if size(U0,1) < m̄
        U0 = [U0; ones(1,size(U0,2))*get_initial_dt(solver)]
    end

    if nlp == :ipopt
        return solve_ipopt(solver,X0,U0,method,options=options)
    elseif nlp == :snopt
        return solve_snopt(solver,X0,U0, method=method, grads=grads, start=start)
    else
        ArgumentError("$nlp is not a known NLP solver. Options are (:ipopt, :snopt)")
    end
    # return sol, stats, prob
end

"""
$(SIGNATURES)
Automatically generate an initial guess by linearly interpolating the state
between initial and final state and settings the controls to zero.
"""
function solve_dircol(solver::Solver;
        method::Symbol=:auto, grads::Symbol=:quadratic, start=:cold)
    # Constants
    N = solver.N
    N = convert_N(N,method)

    X0, U0 = get_initial_state(solver.obj,N)
    solve_dircol(solver, X0, U0, method=method, grads=grads, start=start)
end

function convertInf!(A::VecOrMat{Float64},infbnd=1.1e20)
    infs = isinf.(A)
    A[infs] = sign.(A[infs])*infbnd
    return nothing
end

function convert_N(N::Int,method::Symbol)::Int
    nSeg = N-1
    if method == :hermite_simpson_separated
        N = 2nSeg + 1
    else
        N = nSeg + 1
    end
    return N
end

function get_weights(method::Symbol,N::Int)
    if method == :trapezoid
        weights = ones(N)
        weights[[1,end]] .= 0.5
    elseif method == :hermite_simpson_separated ||
            method == :hermite_simpson
        weights = ones(N)*2/6
        weights[2:2:end] .= 4/6
        weights[[1,end]] .= 1/6
    elseif method == :midpoint
        weights = ones(N)
        weights[end] = 0
    end
    return weights
end

function gen_custom_constraint_fun(solver::Solver,method)
    N, = get_N(solver,method)
    n,m = get_sizes(solver)
    m̄, = get_num_controls(solver)
    NN = (n+m̄)N
    obj = solver.obj
    pI_obj, pE_obj = count_constraints(solver.obj)
    pI_c,   pE_c   = pI_obj[2], pE_obj[2]  # Number of custom stage constraints
    pI_N_c, pE_N_c = pI_obj[4], pE_obj[4]  # Number of custom terminal constraints
    pC = pE_c + pI_c        # Number of custom stage constraints (total)
    pC_N = pE_N_c + pI_N_c      # Number of custom terminal constraints (total)
    P = (N-1)pC + pC_N  # Total number of custom constraints
    PI = (N-1)pI_c + pI_N_c  # Total number of inequality constraints
    PE = (N-1)pE_c + pE_N_c  # Total number of equality constraints
    P = PI+PE

    function c_fun!(C_vec,X,U)
        CE = view(C_vec,1:PE)
        CI = view(C_vec,PE.+(1:PI))
        cE = reshape(view(CE,1:(N-1)pE_c),pE_c,N-1)
        cI = reshape(view(CI,1:(N-1)pI_c),pI_c,N-1)

        # Equality Constraints
        if pE_c > 0
            for k = 1:N-1
                obj.cE(view(cE,1:pE_c,k),X[:,k],U[:,k])
            end
        end
        if pE_N_c > 0
            obj.cE(view(CE,(N-1)pE_c+1:PE),X[:,N])
        end

        # Inequality Constraints
        if pI_c > 0
            for k = 1:N-1
                obj.cI(view(cI,1:pI_c,k),X[:,k],U[:,k])
            end
        end
        if pI_N_c > 0
            obj.cI(view(CI,(N-1)pI_c+1:PI),X[:,N])
        end
        return nothing
    end

    # Jacobian
    jac_cI = generate_general_constraint_jacobian(obj.cI, pI_c, n, m)
    jac_cE = generate_general_constraint_jacobian(obj.cE, pE_c, n, m)

    function jacobian_block!(vals, jac::Function, x, u, p::Int)
        blk = reshape(vals,p,n+m̄)
        Ac = view(blk,1:p,1:n)
        Bc = view(blk,1:p,n.+(1:m))
        jac(Ac,Bc,x,u)
    end

    function jac_c!(vals,X::Matrix{Float64},U::Matrix{Float64})
        # Equality Constraints
        n_blk = (n+m̄)pE_c
        nG_PE = (N-1)n_blk  # number of nonzero entries in custom equality constraint jacobian
        for k = 1:N-1
            off = (k-1)*n_blk
            block = view(vals,off.+(1:n_blk))
            jacobian_block!(block, jac_cE, X[:,k], U[1:m,k], pE_c)
        end
        if pE_N_c > 0
            n_blk = n*pE_N_c
            block = view(vals,nG_PE.+(1:n_blk))
            nG_PE += n_blk
            Ac = reshape(block,pE_N_c,n)
            jac_cE(Ac,X[:,N])
        end

        # Inequality Constraints
        n_blk = (n+m̄)pI_c
        nG_PI = (N-1)n_blk  # number of nonzero entries in custom equality constraint jacobian
        for k = 1:N-1
            off = (k-1)*n_blk + nG_PE
            block = view(vals,off.+(1:n_blk))
            jacobian_block!(block, jac_cI, X[:,k], U[1:m,k], pI_c)
        end
        if pI_N_c > 0
            n_blk = n*pI_N_c
            block = view(vals,(nG_PE+nG_PI).+(1:n_blk))
            Ac = reshape(block,pE_N_c,n)
            jac_cE(Ac,X[:,N])
        end
    end

    J = spzeros(P,NN)
    JE = view(J,1:PE,:)
    JI = view(J,PE+1:P,:)

    function jacobian_sparsity(start::Int=0)
        i = start
        # Equality Constraints
        n_blk = (n+m)pE_c
        num_block = reshape(1:n_blk,pE_c,n+m̄)
        if pE_c > 0
            for k = 1:N-1
                off_1 = (k-1)pE_c
                off_2 = (k-1)*(n+m̄)
                JE[off_1.+(1:pE_c),off_2.+(1:n+m)] = num_block .+ i
                i += n_blk
            end
        end
        if pE_N_c > 0
            n_blk = n*pE_N_c
            num_block = reshape(1:n_blk,pE_N_c,n)
            off = (N-1)*(n+m̄)
            JE[(N-1)pE_c.+1:PE,off.+(1:n)] = num_block .+ i
            i += n_blk
        end

        # Inequality Constraints
        n_blk = (n+m)pI_c
        num_block = reshape(1:n_blk,pI_c,n+m̄)
        if pI_c > 0
            for k = 1:N-1
                off_1 = (k-1)pI_c
                off_2 = (k-1)*(n+m̄)
                JI[off_1.+(1:pI_c),off_2.+(1:n+m)] = num_block .+ i
                i += n_blk
            end
        end
        if pE_N_c > 0
            n_blk = n*pI_N_c
            num_block = reshape(1:n_blk,pI_N_c,n)
            off = (N-1)*(n+m̄)
            JI[(N-1)pI_c.+1:PI,off.+(1:n)] = num_block .+ i
            i += n_blk
        end
        return J
    end

    return c_fun!, jac_c!, jacobian_sparsity
end

function get_bounds(solver::Solver,method::Symbol)
    n,m = get_sizes(solver)
    m̄, = get_num_controls(solver)
    N,N_ = get_N(solver,method)
    obj = solver.obj
    x_L = zeros((n+m̄),N)
    x_U = zeros((n+m̄),N)

    ## STATE CONSTRAINTS ##
    x_L[1:n,:] .= obj.x_min
    x_U[1:n,:] .= obj.x_max
    x_L[n.+(1:m),:] .= obj.u_min
    x_U[n.+(1:m),:] .= obj.u_max

    # Minimum time bounds
    if solver.state.minimum_time
        x_L[n+m̄,:] .= solver.opts.min_dt
        x_U[n+m̄,:] .= solver.opts.max_dt
        p_dt = N-2
    else
        p_dt = 0
    end

    # Initial Condition
    x_L[1:n,1] .= obj.x0
    x_U[1:n,1] .= obj.x0

    # Terminal Constraint
    x_L[1:n,N] .= obj.xf
    x_U[1:n,N] .= obj.xf

    ## CONSTRAINTS ##
    p_colloc = (N-1)n
    pI_obj, pE_obj = count_constraints(solver.obj)
    pI_c,   pE_c   = pI_obj[2], pE_obj[2]  # Number of custom stage constraints
    pI_N_c, pE_N_c = pI_obj[4], pE_obj[4]  # Number of custom terminal constraints
    PE_c = (N-1)pE_c + pE_N_c  # Total number of custom equality constraints
    PI_c = (N-1)pI_c + pI_N_c  # Total number of custom inequality constraints
    PE = PE_c + p_colloc + p_dt  # Total equality constraints
    PI = PI_c                    # Total inequality constraints
    P = PI+PE                    # Total constraints

    # Get bounds
    g_L = zeros(P)
    g_U = zeros(P)
    g_L[1:PE] .= 0
    g_U[1:PE] .= 0
    g_L[PE+1:end] .= -Inf
    g_U[PE+1:end] .= 0

    # Convert Infinite bounds
    convertInf!(x_L)
    convertInf!(x_U)
    convertInf!(g_L)
    convertInf!(g_U)
    return vec(x_L), vec(x_U), g_L, g_U
end

"""
Stack state and controls for all time steps into a single vector of variables
Z = [X1,U1,X2,U2,..,] (all transposed)
"""
function packZ(X,U)
    n, N = size(X)
    m = size(U,1)
    Nx = length(X)
    Nu = length(U)
    Nz = Nx+Nu

    if size(X,2) == size(U,2)
        Z = zeros(n+m,N)
        Z[1:n,:] = X
        Z[1+n:n+m,:] = U
        Z = vec(Z)
    else
        Z = zeros(Nz)
        Z_k = reshape(view(Z,1:Nz-n),n+m,N-1)
        Z_k[1:n,:] = X[:,1:N-1]
        Z_k[1+n:n+m,:] = U[:,1:N-1]
        Z[Nz-n+1:Nz] = X[:,N]
    end
    return Z
end

function unpackZ(Z, sze)
    n,m,N = sze
    Z = reshape(Z,n+m,N)
    X = Z[1:n,:]
    U = Z[n+1:end,:]
    return X,U
end

function get_initial_state(obj::Objective, N::Int)
    n,m = get_sizes(obj)
    X0 = line_trajectory(obj.x0, obj.xf, N)
    U0 = zeros(m,N)
    return X0, U0
end

"""
Evaluate Objective Value
"""
function cost(solver::Solver,res::DircolResults)
    cost(solver,res.X_,res.U_,res.weights)
end

function cost(solver::Solver,X::AbstractMatrix,U::AbstractMatrix,method::Symbol)
    # pull out solver/objective
    N,N_ = get_N(solver,method)
    n,m = get_sizes(solver)
    m̄, = get_num_controls(solver)
    obj = solver.obj
    Q = obj.cost.Q; R = obj.cost.R; xf::Vector{Float64} = obj.xf; Qf::Matrix{Float64} = obj.cost.Qf
    dt = solver.dt

    J = 0.0
    if method == :hermite_simpson || method == :hermite_simpson_separated
        nSeg = (N_-1)÷2
        Xk = view(X,:,1:2:N_)
        Uk = view(U,:,1:2:N_)
        Xm = view(X,:,2:2:N_-1)
        Um = view(U,:,2:2:N_-1)
        solver.state.minimum_time ? dt = view(U,m̄,1:2:N_) : dt = ones(N)*solver.dt

        for k = 1:nSeg
             # Simpson quadrature (integral approximation) for foh stage cost
            J += dt[k]/6*(stage_cost(obj.cost,Xk[:,k],Uk[1:m,k]) + 4stage_cost(obj.cost,Xm[:,k],Um[1:m,k]) + stage_cost(obj.cost,Xk[:,k+1],Uk[1:m,k+1]))
            solver.state.minimum_time ? J += solver.opts.R_minimum_time*dt[k] : nothing
        end
    elseif method == :midpoint
        solver.state.minimum_time ? dt = view(U,m̄,1:2:N_) : dt = ones(N)*solver.dt
        for k = 1:N-1
            J += dt[k]*stage_cost(obj.cost,X[:,k],U[1:m,k])
        end
    elseif method == :trapezoid
        solver.state.minimum_time ? dt = view(U,m̄,1:2:N_) : dt = ones(N)*solver.dt
        for k = 1:N-1
            J += dt[k]*(stage_cost(obj.cost,X[:,k],U[1:m,k]) + stage_cost(obj.cost,X[:,k+1],U[1:m,k+1]))/2
        end
    end

    J += stage_cost(obj.cost,X[:,N_])

    return J
end

function cost(solver::Solver,X::AbstractArray,U::AbstractArray,weights::Vector{Float64})
    n,m = get_sizes(solver)
    N_ = size(X,2)
    m̄, = get_num_controls(solver)
    Qf = solver.obj.cost.Qf; Q = solver.obj.cost.Q;
    xf = solver.obj.xf; R = solver.obj.cost.R;
    solver.state.minimum_time ? dt = U[m̄,:] : dt = ones(N_)*solver.dt

    J = zeros(eltype(X),N_)
    for k = 1:N_
        J[k] = stage_cost(solver.obj.cost,X[:,k],U[1:m,k])*dt[k]
    end

    J = weights'J
    J += 0.5*(X[:,N_] - xf)'*Qf*(X[:,N_] - xf)
end

"""
Gradient of Objective
"""
function cost_gradient!(grad_f, solver::Solver, res::DircolResults, method::Symbol)
    cost_gradient!(solver,res.X_,res.U_,res.fVal_,res.A,res.B,res.weights,grad_f,method)
end

function cost_gradient!(solver::Solver, X, U, fVal, A, B, weights, vals, method::Symbol)
    n,m = get_sizes(solver)
    m̄, = get_num_controls(solver)
    N,N_ = get_N(solver,method)
    dt = solver.dt

    obj = solver.obj
    xf = obj.xf
    Q = obj.cost.Q; R = obj.cost.R; Qf = obj.cost.Qf;
    # X,U = res.X_, res.U_
    grad_f = reshape(vals, n+m̄, N)

    if method == :hermite_simpson
        I_n = Matrix(I,n,n)

        Xk = view(X,:,1:2:N_)
        Uk = view(U,:,1:2:N_)
        Xm = view(X,:,2:2:N_-1)
        Um = view(U,:,2:2:N_-1)
        Ak = view(A,:,:,1:2:N_)
        Bk = view(B,:,:,1:2:N_)
        fk = fVal

        if solver.state.minimum_time
            dt = Uk[m̄,:]
            c_dt = solver.opts.R_minimum_time
        else
            dt = ones(N)*solver.dt
        end

        grad_f[1:n,1] =     (Q*(Xk[:,1]-xf) + 4*(I_n/2 + dt[1]/8*Ak[:,:,1])'Q*(Xm[:,1] - xf))*dt[1]/6
        grad_f[n.+(1:m),1] = (R*Uk[1:m,1] + 4(dt[1]/8*Bk[:,:,1]'Q*(Xm[:,1] - xf) + R*Um[1:m,1]/2))*dt[1]/6
        solver.state.minimum_time ? grad_f[n+m̄,1] = (stage_cost(obj.cost,Xk[:,1],Uk[1:m,1]) + 4stage_cost(obj.cost,Xm[:,1],Um[1:m,1]) + stage_cost(obj.cost,Xk[:,2],Uk[1:m,2]))/6 +
                                                 (fk[:,1] - fk[:,2])'*Q*(Xm[:,1] - xf)*dt[1]/12 + c_dt : nothing
        for k = 2:N-1
            grad_f[1:n,k] = (Q*(Xk[:,k]-xf) + 4(I_n/2 + dt[k]/8*Ak[:,:,k])'Q*(Xm[:,k] - xf))*dt[k]/6 +
                            (Q*(Xk[:,k]-xf) + 4(I_n/2 - dt[k]/8*Ak[:,:,k])'Q*(Xm[:,k-1] - xf))*dt[k-1]/6
            grad_f[n.+(1:m),k] = (R*Uk[1:m,k] + 4(dt[k]/8*Bk[:,:,k]'Q*(Xm[:,k] - xf)   + R*Um[1:m,k]/2))*dt[k]/6 +
                                 (R*Uk[1:m,k] - 4(dt[k]/8*Bk[:,:,k]'Q*(Xm[:,k-1] - xf) - R*Um[1:m,k-1]/2))*dt[k-1]/6
            solver.state.minimum_time ? grad_f[n+m̄,k] = (stage_cost(obj.cost,Xk[:,k],Uk[1:m,k]) + 4stage_cost(obj.cost,Xm[:,k],Um[1:m,k]) + stage_cost(obj.cost,Xk[:,k+1],Uk[1:m,k+1]))/6 +
                                                     (fk[:,k] - fk[:,k+1])'*Q*(Xm[:,k] - xf)*dt[k]/12 + c_dt : nothing
        end
        grad_f[1:n,N] = (Q*(Xk[:,N]-xf) + 4(I_n/2 - dt[N-1]/8*Ak[:,:,N])'Q*(Xm[:,N-1] - xf))*dt[N-1]/6
        grad_f[n.+(1:m),N] = (R*Uk[1:m,N] - 4(dt[N-1]/8*Bk[:,:,N]'Q*(Xm[:,N-1] - xf) - R*Um[1:m,N-1]/2))*dt[N-1]/6
        solver.state.minimum_time ? grad_f[n+m̄,N] = 0 : nothing
        grad_f[1:n,N] += Qf*(Xk[:,N] - xf)
    elseif method == :midpoint
        I_n = Matrix(I,n,n)
        Xm = X

        # Get dt
        solver.state.minimum_time ? dt = U[m̄,:] : dt = ones(N)*solver.dt

        grad_f[1:n,1] = Q*(Xm[:,1] - xf)*dt[1]/2
        grad_f[n.+(1:m),1] = R*U[1:m,1]*dt[1]
        solver.state.minimum_time ? grad_f[n+m̄,1] = stage_cost(obj.cost,Xm[:,1],U[1:m,1]) : nothing
        for k = 2:N-1
            grad_f[1:n,k] = Q*(Xm[:,k] - xf)*dt[k]/2 + Q*(Xm[:,k-1] - xf)*dt[k-1]/2
            grad_f[n.+(1:m),k] = R*U[1:m,k]*dt[k]
            solver.state.minimum_time ? grad_f[n+m̄,k] = stage_cost(obj.cost,Xm[:,k],U[1:m,k]) : nothing
        end
        grad_f[1:n,N] = Q*(Xm[:,N-1] - xf)*dt[N-1]/2
        grad_f[n+1:end,N] = zeros(m̄)
        grad_f[1:n,N] += Qf*(X[:,N] - xf)
    else
        for k = 1:N
            grad_f[1:n,k] = weights[k]*Q*(X[:,k] - xf)*dt
            grad_f[n+1:end,k] = weights[k]*R*U[:,k]*dt
        end
        grad_f[1:n,N] += Qf*(X[:,N] - xf)
    end
    return nothing
end

"""
$(SIGNATURES)
Return placeholders for trajectories
"""
function init_traj_points(solver::Solver,fVal::Matrix,method::Symbol)
    N,N_ = get_N(solver,method)
    n,m = get_sizes(solver)
    m̄, = get_num_controls(solver)
    X_,U_,fVal_ = zeros(n,N_),zeros(m̄,N_),zeros(n,N_)
    if method == :trapezoid || method == :hermite_simpson_separated
        fVal_ = fVal
    end
    return X_,U_,fVal_
end

"""
$(SIGNATURES)
Return all the trajectory points used to evaluate integrals
"""
function get_traj_points!(solver::Solver,res::DircolResults,method::Symbol)
    get_traj_points!(solver,res.X,res.U,res.X_,res.U_,res.fVal,method)
end

function get_traj_points!(solver::Solver,X,U,X_,U_,method::Symbol)
    fVal = zeros(X)
    fVal_ = zeros(X_)
    update_derivatives!(solver,fVal,X,U)
    get_traj_points!(solver,X,U,X_,U_,fVal,fVal_)
end

function get_traj_points!(solver::Solver,X,U,X_,U_,fVal,method::Symbol)
    n,m,N = get_sizes(solver)
    m̄,N_ = size(U_)
    dt = solver.dt
    if method == :hermite_simpson
        X_[:,1:2:end] = X
        U_[:,1:2:end] = U
        # fVal_[:,1:2:end] = fVal

        # Midpoints
        Xm = view(X_,:,2:2:N_-1)
        Um = view(U_,1:m,2:2:N_-1)
        Um .= (U[1:m,1:end-1] + U[1:m,2:end])/2
        # fValm = view(fVal_,:,2:2:N_-1)
        for k = 1:N-1
            solver.state.minimum_time ? dt = U[m̄,k] : nothing
            x1,x2 = X[:,k], X[:,k+1]
            Xm[:,k] = (x1+x2)/2 + dt/8*(fVal[:,k]-fVal[:,k+1])
            # solver.fc(view(fValm,:,k),Xm[:,k],Um[:,k])
        end
        # if solver.state.minimum_time
        #     U_[m̄,2:2:N_-1] = U[m̄,1:N-1]
        #     U_[m̄,3:2:N_-2] = (U[m̄,1:N-2] + U[m̄,2:N-1])/2
        #     U_[m̄,N_] = U[m̄,N-1]
        # end
    elseif method == :midpoint
        Xm = (X[:,1:end-1] + X[:,2:end])/2
        X_[:,1:end-1] = Xm
        X_[:,end] = X[:,end]
        # res.U_ is set in the results constructor
        # for k = 1:N-1
        #     solver.fc(view(fVal_,:,k),res.X_[:,k],U_[:,k])
        # end
    # else
    #     X_ .= X
    #     U_ .= U
    end
    return nothing
end

function get_traj_points(solver,X,U,gfVal,gX_,gU_,method::Symbol,cost_only::Bool=false)
    if !cost_only
        update_derivatives!(solver,X,U,gfVal)
    end
    if method == :trapezoid || method == :hermite_simpson_separated
        X_,U_ = X,U
    else
        if cost_only && method == :hermite_simpson
            update_derivatives!(solver,X,U,gfVal)
        end
        get_traj_points!(solver,X,U,gX_,gU_,gfVal,method)
        X_,U_ = gX_,gU_
        if method == :midpoint
            U_ = U
        end
    end
    return X_, U_
end

function get_traj_points_derivatives!(solver::Solver,res::DircolResults,method::Symbol)
    get_traj_points_derivatives!(solver::Solver,res.X_,res.U_,res.fVal_,res.fVal,method::Symbol)
end

function get_traj_points_derivatives!(solver::Solver,X_,U_,fVal_,fVal,method::Symbol)
    if method == :hermite_simpson
        N_ = size(X_,2)
        fVal_[:,1:2:end] = fVal
        Xm,Um,fm = X_[:,2:2:end-1],U_[:,2:2:end-1],view(fVal_,:,2:2:N_-1)
        update_derivatives!(solver,Xm,Um,fm)
    elseif method == :midpoint
        update_derivatives!(solver,X_,U_,fVal_)
    else
        fVal_ .= fVal
    end
end

function update_derivatives!(solver::Solver,res::DircolResults,method::Symbol)
    # Calculate derivative
    if method != :midpoint
        update_derivatives!(solver,res.X,res.U,res.fVal)
    end
end

function update_derivatives!(solver::Solver,X::AbstractArray,U::AbstractArray,fVal::AbstractArray)
    n,m = get_sizes(solver)
    N = size(X,2)
    for k = 1:N
        dynamics(solver.model,view(fVal,:,k),X[:,k],U[1:m,k])
        # solver.model.f(view(fVal,:,k),X[:,k],U[1:m,k])
    end
end

function update_jacobians!(solver::Solver,res::DircolResults,method::Symbol,cost_only::Bool=false)
    update_jacobians!(solver,res.X_,res.U_,res.A,res.B,method,cost_only) # TODO: pass in DircolVar
end

function update_jacobians!(solver::Solver,X,U,A,B,method::Symbol,cost_only::Bool=false)
    n,m = get_sizes(solver)
    N,N_ = get_N(solver,method)
    Fc = dynamics_jacobian(solver)
    if ~cost_only || method == :hermite_simpson
        if method == :hermite_simpson || method == :midpoint
            inds = cost_only ? (1:2:N_) : (1:N_)  # HS only needs jacobians at knot points for cost gradient
            for k = inds
                A[:,:,k], B[:,:,k] = Fc(X[:,k],U[1:m,k])
            end
        else
            # Z = packZ(X,U)
            # z = reshape(Z,n+m,N)
            for k = 1:N_
                tA,tB = Fc(X[:,k],U[:,k])
                A[:,:,k] = [tA tB]
            end
        end
    end
end

#~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~#
#                                                                              #
#                           CONSTRAINTS                                        #
#                                                                              #
#~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~#

"""
Evaluate constraint values
[ dt*(f(x1)+f(x2))/2 - x2 + x1     ] Collocation Constraints
[ dt*(f(x2)+f(x3))/2 - x3 + x2     ]
[           ...                 ]
[ dt*(f(xN-1)+f(xN))/2 - xN + xN-1 ]
"""
function collocation_constraints!(c_colloc, solver::Solver, res::DircolResults, method::Symbol)
    n,m = get_sizes(solver)
    N,N_ = get_N(solver,method)
    # g = zeros(eltype(res.X),(N-1)*n)
    collocation_constraints!(solver,res.X_,res.U_,res.fVal_,c_colloc,method)
end

function collocation_constraints!(solver::Solver, X, U, fVal, g_colloc, method::Symbol)
    # X,U need to be the "trajectory points", or X_,U_
    N,N_ = get_N(solver,method)
    n,m = get_sizes(solver)
    m̄, = get_num_controls(solver)
    g = reshape(g_colloc,n,N-1)
    dt = solver.dt

    if method == :trapezoid
        # solver.state.minimum_time ? dt = U[m̄,:] : dt = ones(N)*solver.dt
        for k = 1:N-1
            # Collocation Constraints
            g[:,k] = dt*( fVal[:,k+1] + fVal[:,k] )/2 - X[:,k+1] + X[:,k]
        end
    elseif method == :hermite_simpson_separated || method == :hermite_simpson
        iLow = 1:2:N_-1
        iMid = iLow .+ 1
        iUpp = iMid .+ 1
        solver.state.minimum_time ? dt = U[m̄:m̄,:] : dt = ones(1,N_)*solver.dt

        collocation = - X[:,iUpp] + X[:,iLow] + dt[:,iLow].*(fVal[:,iLow] + 4*fVal[:,iMid] + fVal[:,iUpp])/6

        if method == :hermite_simpson
            g .= collocation
        else
            midpoints =  - X[:,iMid] + (X[:,iLow]+X[:,iUpp])/2 + dt[:,iLow].*(fVal[:,iLow] - fVal[:,iUpp])/8
            g[:,iLow] = collocation
            g[:,iMid] = midpoints
        end
        # Xk = view(X,:,1:2:N_)
        # Uk = view(U,:,1:2:N_)
        # Xm = view(X,:,2:2:N_-1)
        # Um = view(U,:,2:2:N_-1)
        # Fk = view(fVal,:,1:2:N_)
        # Fm = view(fVal,:,2:2:N_-1)
        #
        # for k = 1:N-1
        #     x1,x2 = Xk[:,k],Xk[:,k+1]
        #     u1,u2 = Uk[1:m,k],Uk[1:m,k+1]
        #     solver.state.minimum_time ? dt = Uk[m̄,k] : dt = solver.dt
        #     f1 = Fk[:,k]
        #     f2 = Fk[:,k+1]
        #     fm = Fm[:,k]
        #     g[:,k] = dt/6*(f1+4fm+f2) - x2 + x1
        # end

    elseif method == :midpoint
        Xm = X
        # Calculate the knot points from the midpoints (and the terminal point)
        Xk = zero(Xm)
        Xk[:,end] = Xm[:,end]
        solver.state.minimum_time ? dt = U[m̄,:] : dt = ones(N_)*solver.dt
        for k = N-1:-1:1
            Xk[:,k] = 2Xm[:,k] - Xk[:,k+1]
        end
        for k = 1:N-1
            g[:,k] = dt[k]*fVal[:,k] - Xk[:,k+1] + Xk[:,k]
        end
    end
end

function dt_constraints!(solver, g_dt, dt)
    N = length(dt)
    for k = 1:N-2
        g_dt[k] = dt[k] - dt[k+1]
    end
end

"""
Constraint Jacobian
"""
function constraint_jacobian!(c_jacob, solver::Solver, res::DircolResults, method::Symbol)
    N,N_ = get_N(solver,method)
    n,m = get_sizes(solver)
    collocation_constraint_jacobian!(solver,res.X_,res.U_, res.fVal_, res.A,res.B, c_jacob, method)
end

function collocation_constraint_jacobian!(solver::Solver, X, U, fVal, A::Array{Float64,3}, B::Array{Float64,3}, vals, method::Symbol)
    # X and U are X_, U_ trajectory points
    N,N_ = get_N(solver,method)
    n,m = get_sizes(solver)
    m̄, = get_num_controls(solver)
    dt = solver.dt
    Inm = Matrix(I,n,n+m)

    if method == :trapezoid
        Z = packZ(X,U)
        z = reshape(Z,n+m̄,N)
        n_blk = 2(n+m̄)n
        solver.state.minimum_time ? dt = U[m̄,:] : dt = ones(N_)*solver.dt

        function calc_block_trap(k,blk::SubArray{Float64})
            blk = reshape(blk,n,2(n+m̄))
            blk[:,1:n+m] =         dt[k]*A[:,:,k  ]/2+Inm
            blk[:,n.+m̄.+(1:n+m)] = dt[k]*A[:,:,k+1]/2-Inm
            if solver.state.minimum_time
                blk[:,n+m̄] = (fVal[:,k] + fVal[:,k+1])/2
            end
            return nothing
        end

        for k = 1:N-1
            off = (k-1)*n_blk
            block = view(vals,off.+(1:n_blk))
            calc_block_trap(k,block)
        end

    elseif method == :hermite_simpson_separated
        nSeg = Int((N-1)/2)
        Z = packZ(X,U)
        z = reshape(Z,n+m̄,N)

        n_blk = 6(n+m)n
        function calc_block_sep!(k,val_block)
            blk = reshape(val_block,2n,3(n+m))
            fz = A[:,:,k]  # F(z[:,k])
            blk[   (1:n),(1:n+m)] = dt*fz/6 + Inm
            blk[n.+(1:n),(1:n+m)] = dt*fz/8 + Inm/2
            fm = A[:,:,k+1]  # F(z[:,k+1])
            blk[   (1:n),n.+m.+(1:n+m)] = 2*dt*fm/3
            blk[n.+(1:n),n.+m.+(1:n+m)] = -Inm
            fz1 = A[:,:,k+2]  # F(z[:,k+2])
            blk[   (1:n),2(n+m).+(1:n+m)] =  dt*fz1/6 - Inm
            blk[n.+(1:n),2(n+m).+(1:n+m)] = -dt*fz1/8 + Inm/2
            return nothing
        end

        for i = 1:nSeg
            k = 2i-1
            off = (i-1)*n_blk
            block = view(vals,off.+(1:n_blk))
            calc_block_sep!(k,block)
        end
    elseif method == :hermite_simpson
        nSeg = N-1
        n_blk = 2(n+m̄)n

        Xk = view(X,:,1:2:N_)
        Uk = view(U,:,1:2:N_)
        Xm = view(X,:,2:2:N_-1)
        Um = view(U,:,2:2:N_-1)
        Ak = view(A,:,:,1:2:N_)
        Bk = view(B,:,:,1:2:N_)
        AM = view(A,:,:,2:2:N_-1)
        BM = view(B,:,:,2:2:N_-1)
        fk = view(fVal,:,1:2:N_)
        fm = view(fVal,:,2:2:N_-1)

        solver.state.minimum_time ? dt = Uk[m̄,:] : dt = ones(N_)*solver.dt

        function calc_block_hs!(k::Int,vals::SubArray)
            x1,u1 = Xk[:,k],Uk[:,k]
            x2,u2 = Xk[:,k+1],Uk[:,k+1]
            A1,A2 = Ak[:,:,k],Ak[:,:,k+1]
            B1,B2 = Bk[:,:,k],Bk[:,:,k+1]
            xm = Xm[:,k] #(x1+x2)/2 + dt/8*(fVal[:,k]-fVal[:,k+1])
            um = Um[:,k] # (u1+u2)/2
            Am,Bm = AM[:,:,k],BM[:,:,k]
            In = Matrix(I,n,n)
            Im = Matrix(I,m,m)

            vals = reshape(vals,n,2(n+m̄))
            vals[:,1:n] =          dt[k]/6*(A1 + 4Am*( dt[k]/8*A1 + In/2)) + In    # ∇x1
            vals[:,n.+(1:m)] =     dt[k]/6*(B1 + 4Am*( dt[k]/8*B1) + 4Bm*(Im/2))   # ∇u1
            vals[:,n.+m̄.+(1:n)] =  dt[k]/6*(A2 + 4Am*(-dt[k]/8*A2 + In/2)) - In    # ∇x2
            vals[:,2n.+m̄.+(1:m)] = dt[k]/6*(B2 + 4Am*(-dt[k]/8*B2) + 4Bm*(Im/2))   # ∇u2
            if solver.state.minimum_time
                vals[:,n+m̄] = (fk[:,k] + 4fm[:,k] + fk[:,k+1])/6 + dt[k]/12*Am*(fk[:,k] - fk[:,k+1])
            end
            return nothing
        end

        for k = 1:nSeg
            off = (k-1)*n_blk
            block = view(vals,off.+(1:n_blk))
            calc_block_hs!(k,block)
        end

    elseif method == :midpoint
        nSeg = N-1
        n_blk = (2n+m)n
        In = Matrix(I,n,n)

        function calc_jacob_block_midpoint!(k,blk)
            blk = reshape(blk,n,2n+m)
            blk[:,1:n] =        In + dt*A[:,:,k]/2    # ∇x1
            blk[:,n.+(1:m)] =    dt*B[:,:,k]           # ∇u1
            blk[:,n.+m.+(1:n)] = -In + dt*A[:,:,k]/2    # ∇x2
            return nothing
        end

        for k = 1:nSeg
            off = (k-1)*n_blk
            block = view(vals,off.+(1:n_blk))
            calc_jacob_block_midpoint!(k,block)
        end
    end
    return nothing
end

function constraint_jacobian(solver::Solver, res::DircolResults, method::Symbol)
    N,N_ = get_N(solver,method)
    n,m = get_sizes(solver)
    # jacob_g = spzeros((N-1)*n,N*(n+m))
    jacob_g = constraint_jacobian(solver,res.X_,res.U_,res.A,res.B,method)
    return jacob_g
end

function constraint_jacobian(solver::Solver, X, U, A, B, method::Symbol)
    N,N_ = get_N(solver,method)
    n,m = get_sizes(solver)
    jacob_g = spzeros((N-1)*n,N*(n+m))
    Inm = Matrix(I,n,n+m)
    dt = solver.dt

    if method == :trapezoid
        Z = packZ(X,U)
        z = reshape(Z,n+m,N)

        solver.state.minimum_time ? dt = U[m̄:m̄,:] : dt = ones(1,N_)*solver.dt

        # First time step
        fz = A[:,:,1]
        jacob_g[1:n,1:n+m] .= dt[1]*fz/2+Inm

        # Loop over time steps
        for k = 2:N-1
            off_1 = (k-1)*(n)
            off_2 = (k-1)*(n+m)
            # Calculate (n,n+m) Jacobian of both states and controls
            fz = A[:,:,k]  #F(z[:,k])
            jacob_g[off_1.-n.+(1:n),off_2.+(1:n+m)] .= dt[k]*fz/2 - Inm
            jacob_g[off_1  .+ (1:n),off_2.+(1:n+m)] .= dt[k]*fz/2 + Inm
        end

        # Last time step
        fz = A[:,:,N]  # F(z[:,N])
        jacob_g[end-n+1:end,end-n-m+1:end] = dt[N-1]*fz/2-Inm

    elseif method == :hermite_simpson_separated
        nSeg = Int((N-1)/2)
        Z = packZ(X,U)
        z = reshape(Z,n+m,N)

        fz1 = A[:,:,1]  # F(z[:,1])

        function calc_block(k)
            vals = zeros(2n,3(n+m))
            fz = A[:,:,k]  # F(z[:,k])
            vals[   (1:n),(1:n+m)] .= dt*fz/6 + Inm
            vals[n.+(1:n),(1:n+m)] .= dt*fz/8 + Inm/2
            fm = A[:,:,k+1]  # F(z[:,k+1])
            vals[   (1:n),n.+m.+(1:n+m)] .= 2*dt*fm/3
            vals[n.+(1:n),n.+m.+(1:n+m)] .= -Inm
            fz1 .= A[:,:,k+2]  # F(z[:,k+2])
            vals[   (1:n),2(n+m).+(1:n+m)] .=  dt*fz1/6 - Inm
            vals[n.+(1:n),2(n+m).+(1:n+m)] .= -dt*fz1/8 + Inm/2
            return vals
        end

        for i = 1:nSeg
            off_1 = 2(i-1)*(n)
            off_2 = 2(i-1)*(n+m)
            k = 2i-1

            jacob_g[off_1.+(1:2n), off_2.+(1:3(n+m))] = calc_block(k)
        end
    elseif method == :hermite_simpson
        nSeg = N-1

        Xk = view(X,:,1:2:N_)
        Uk = view(U,:,1:2:N_)
        Xm = view(X,:,2:2:N_-1)
        Um = view(U,:,2:2:N_-1)
        Ak = view(A,:,:,1:2:N_)
        Bk = view(B,:,:,1:2:N_)
        AM = view(A,:,:,2:2:N_-1)
        BM = view(B,:,:,2:2:N_-1)

        function calc_jacob_block(k::Int)::Matrix
            x1,u1 = Xk[:,k],Uk[:,k]
            x2,u2 = Xk[:,k+1],Uk[:,k+1]
            A1,A2 = Ak[:,:,k],Ak[:,:,k+1]
            B1,B2 = Bk[:,:,k],Bk[:,:,k+1]
            xm = Xm[:,k] #(x1+x2)/2 + dt/8*(fVal[:,k]-fVal[:,k+1])
            um = Um[:,k] # (u1+u2)/2
            Am,Bm = AM[:,:,k],BM[:,:,k]
            In = Matrix(I,n,n)
            Im = Matrix(I,m,m)

            vals = zeros(n,2(n+m))
            vals[:,1:n] =          dt/6*(A1 + 4Am*( dt/8*A1 + In/2)) + In    # ∇x1
            vals[:,n.+(1:m)] =     dt/6*(B1 + 4Am*( dt/8*B1) + 4Bm*(Im/2))   # ∇u1
            vals[:,n.+m.+(1:n)] =  dt/6*(A2 + 4Am*(-dt/8*A2 + In/2)) - In    # ∇x2
            vals[:,2n.+m.+(1:m)] = dt/6*(B2 + 4Am*(-dt/8*B2) + 4Bm*(Im/2))   # ∇u2
            return vals
        end

        for k = 1:nSeg
            off_1 = (k-1)*(n)
            off_2 = (k-1)*(n+m)
            jacob_g[off_1.+(1:n), off_2.+(1:2(n+m))] = calc_jacob_block(k)
        end

    elseif method == :midpoint
        nSeg = N-1
        In = Matrix(I,n,n)

        function calc_jacob_block_midpoint(k)
            vals = zeros(n,2(n+m))
            vals[:,1:n] =          In + dt*A[:,:,k]/2    # ∇x1
            vals[:,n.+(1:m)] =     dt*B[:,:,k]           # ∇u1
            vals[:,n.+m.+(1:n)] = -In + dt*A[:,:,k]/2    # ∇x2
            vals[:,2n.+m.+(1:m)] = zeros(n,m)            # ∇u2
            return vals
        end

        for k = 1:nSeg
            off_1 = (k-1)*(n)
            off_2 = (k-1)*(n+m)
            jacob_g[off_1.+(1:n), off_2.+(1:2(n+m))] = calc_jacob_block_midpoint(k)
        end
    end

    return jacob_g
end

function time_step_constraint_jacobian!(vals, solver::Solver)
    N = solver.N
    if solver.state.minimum_time
        vals[1:2:2(N-2)] .= 1
        vals[2:2:2(N-2)+1] .= -1
    end
end

function collocation_constraint_jacobian_sparsity(solver::Solver, method::Symbol, start::Int=0)
    N,N_ = get_N(solver,method)
    n,m = get_sizes(solver)
    m̄, = get_num_controls(solver)
    jacob_g = spzeros(Int,(N-1)*n,N*(n+m̄))
    dt = solver.dt
    i = start

    if method == :trapezoid || method == :hermite_simpson

        n_blk = 2(n+m̄)n
        num_block = reshape(1:n_blk,n,2(n+m̄))

        for k = 1:N-1
            off_1 = (k-1)*(n)
            off_2 = (k-1)*(n+m̄)
            jacob_g[off_1.+(1:n), off_2.+(1:2(n+m̄))] = num_block .+ i
            i += n_blk
        end

    elseif method == :hermite_simpson_separated
        nSeg = Int((N-1)/2)
        n_blk = 6(n+m̄)n
        num_block = reshape(1:n_blk,2n,3(n+m̄))

        for k = 1:nSeg
            off_1 = 2(k-1)*(n)
            off_2 = 2(k-1)*(n+m̄)

            jacob_g[off_1.+(1:2n), off_2.+(1:3(n+m̄))] = num_block .+ i
            i += n_blk
        end

    elseif method == :midpoint
        nSeg = N-1
        n_blk = (2n+m̄)n
        num_block = reshape(1:n_blk,n,2n+m̄)

        for k = 1:nSeg
            off_1 = (k-1)*(n)
            off_2 = (k-1)*(n+m̄)
            jacob_g[off_1.+(1:n), off_2.+(1:2n+m̄)] = num_block .+ i
            i += n_blk
        end
    end

    # rows,cols,inds = findnz(jacob_g)
    # v = sortperm(inds)
    # rows = rows[v]
    # cols = cols[v]

    return jacob_g
end

function time_step_constraint_jacobian_sparsity(solver::Solver, start::Int=0)
    n,m,N = get_sizes(solver)
    m̄, = get_num_controls(solver)
    NN = (n+m̄)N
    if solver.state.minimum_time
        jacob_dt = spzeros(Int,N-2,NN)
        dts = n+m̄:n+m̄:NN-2(n+m̄)
        jacob_dt[CartesianIndex.(1:N-2,dts)] = (1:2:2(N-2)) .+ start
        jacob_dt[CartesianIndex.(1:N-2,dts.+n.+m̄)] = (2:2:2(N-2)+1) .+ start
        return jacob_dt
    else
        return sparse([],[],Float64[],0,NN)
    end
end

# JUNK FUNCTIONS

function rollout_midpoint(solver::Solver,U::Matrix)
    N = solver.N
    N = convert_N(N,method)
    n,m = solver.model.n,solver.model.m
    nSeg = N-1
    N_ = 2*nSeg + 1
    X_ = zeros(n,N_)
    U_ = zeros(size(U,1),N_)
    X_[:,1] = solver.obj.x0
    U_[:,1] = U[:,1]

    for k = 1:N_-1
        if isodd(k)
            j = (k+1)÷2
            U_[:,k+2] = U[:,j+1]
            solver.fd(view(X_,:,k+2), X_[:,k], U_[1:m,k], U_[1:m,k+2])
        else
            Ac1, Bc1 = solver.Fc(X_[:,k-1],U_[:,k-1])
            Ac2, Bc2 = solver.Fc(X_[:,k+1],U_[:,k+1])
            M = [(0.5*eye(n) + dt/8*Ac1) (dt/8*Bc1) (0.5*eye(n) - dt/8*Ac2) (-dt/8*Bc2)]

            Xm = M*[X_[:,k-1]; U_[:,k-1]; X_[:,k+1]; U_[:,k+1]]
            Um = (U_[:,k-1] + U_[:,k+1])/2

            X_[:,k] = Xm
            U_[:,k] = Um
        end
    end
    return X_,U_
end

function calc_midpoints(X::Matrix, U::Matrix, solver::Solver)
    N = solver.N
    N = convert_N(N,method)
    n,m = solver.model.n,solver.model.m
    nSeg = N-1
    N_ = 2*nSeg + 1
    X_ = zeros(n,N_)
    U_ = zeros(size(U,1),N_)
    X_[:,1:2:end] = X
    U_[:,1:2:end] = U

    f1 = zeros(n)
    f2 = zeros(n)
    for k = 2:2:N_

        f(f1,X_[:,k-1], U_[:,k-1])
        f(f2,X_[:,k+1], U_[:,k+1])
        x1 = X_[:,k-1]
        x2 = X_[:,k+1]
        Xm = (x1+x2)/2 + dt/8*(f1-f2)

        Um = (U_[:,k-1] + U_[:,k+1])/2

        X_[:,k] = Xm
        U_[:,k] = Um
    end
    return X_,U_
end

function interp(t,T,X,U,F)
    k = findlast(t .> T)
    constraint_decrease_ratio = t-T[k]
    if method == :trapezoid
        u = U[:,k] + constraint_decrease_ratio/dt*(F[:,k+1]-F[:,k])
        x = X[:,k] + F[:,k]*constraint_decrease_ratio + constraint_decrease_ratio^2/(2*dt)*(F[:,k+1]-F[:,k])
    elseif method == :hermite_simpson || method == :hermite_simpson_separated
        x1,x2 = X[:,k], X[:,k+1]
        u1,u2 = U[:,k], U[:,k+1]
        f1,f2 = F[:,k], F[:,k+1]
        xm = (x1+x2)/2 + dt/8*(f1-f2)
        um = (U[:,k] + U[:,k+1])/2

        u = (2(constraint_decrease_ratio-dt/2)(constraint_decrease_ratio-dt)U[:,k] - 4constraint_decrease_ratio*(constraint_decrease_ratio-dt)Um + 2constraint_decrease_ratio*(constraint_decrease_ratio-dt/2)*U[:,k+1])/dt^2
        x = X[:,k] + F[:,k]*constraint_decrease_ratio/dt + 1/2*(-3F[:,])
    end
    return x,u
end

ext: jl


text:
using Test, Pkg
using BenchmarkTools

using KeywordCalls

f(nt::NamedTuple{(:c,:b,:a)}) = nt.a^3 + nt.b^2 + nt.c
@kwcall f(c,b,a)

g(nt::NamedTuple{(:c,:b,:a)}) = f(nt)
@kwcall g(c=0, b, a)

@kwalias f [
    alpha => a
    beta => b
]

@kwalias g [
    alpha => a
    beta => b
]

struct Foo{N,T}
    nt::NamedTuple{N,T}
end

@kwstruct Foo(a,b)

struct Bar{N,T}
    nt::NamedTuple{N,T}
end

@kwstruct Bar(a,b,c=0)

@testset "KeywordCalls.jl" begin
    @testset "Functions" begin
        # @test @inferred f(1, 2, 3) == 32
        @test @inferred f(a=1, b=2, c=3) == 8
        @test @inferred f((a=1, b=2, c=3)) == 8
    end

    @testset "Constructors" begin
        @test @inferred Foo((b=1,a=2)).nt == (a=2,b=1)
    end

    @testset "Functions with defaults" begin
        @test @inferred g(a=1, b=2) == 5
        @test @inferred g((a=1, b=2)) == 5
        @test @inferred g(a=1, b=2, c=3) == 8
        @test @inferred g((a=1, b=2, c=3)) == 8
    end

    @testset "Constructors with defaults" begin
        @test @inferred Bar((b=1,a=2)).nt == (a=2,b=1,c=0)
        @test @inferred Bar((b=1,a=2,c=5)).nt == (a=2,b=1,c=5)
    end

    @testset "Keyword aliases" begin
        @test @inferred f(alpha=1,b=2,c=3) == 8
        @test @inferred g(beta=1, alpha=3) == 28
    end

    if Base.VERSION ≥ v"1.6"
        @testset "No Allocation" begin
            @test 0 == @allocated f(a=1, b=2, c=3)
            @test 0 == @allocated f((a=1, b=2, c=3))
            @test 0 == @allocated Foo((b=1,a=2))
            @test 0 == @allocated g(a=1, b=2)
            @test 0 == @allocated g((a=1, b=2))
            @test 0 == @allocated g(a=1, b=2, c=3)
            @test 0 == @allocated g((a=1, b=2, c=3))
            @test 0 == @allocated f(alpha=1,b=2,c=3)
            @test 0 == @allocated g(beta=1, alpha=3)
        end
    end
end

if isfile("./Precompile2/Manifest.toml")
    rm("./Precompile2/Manifest.toml")
end

Pkg.activate("./Precompile2")
Pkg.develop(PackageSpec(path="./Precompile1"))

using Precompile2

@testset "caching with precompilation" begin
    @test Precompile2.test() == (a=2, b=1)
end


text:
using AbstractTrees

"""
    OneNode

The node of a tree in which every node has 0 or 1 children.
"""
struct OneNode
    nodes::Vector{Int}
    idx::Int
end

AbstractTrees.ChildIndexing(::Type{OneNode}) = IndexedChildren()
AbstractTrees.ParentLinks(::Type{OneNode}) = StoredParents()
AbstractTrees.SiblingLinks(::Type{OneNode}) = StoredSiblings()

AbstractTrees.parent(ot::OneNode) = ot.idx == 1 ? nothing : OneNode(ot.nodes, ot.idx-1)

# no nodes ever have siblings
AbstractTrees.nextsibling(ot::OneNode) = nothing

AbstractTrees.nodevalue(ot::OneNode) = ot.nodes[ot.idx]

# this guarantees that all nodes in the tree have the same type
Base.IteratorEltype(::Type{<:TreeIterator{OneNode}}) = Base.HasEltype()
Base.eltype(::Type{<:TreeIterator{OneNode}}) = OneNode

function AbstractTrees.children(ot::OneNode)
    idx = ot.idx + 1
    idx > length(ot.nodes) ? () : (OneNode(ot.nodes, idx),)
end


text:
# Define `training`, `data_dir`, `eki`

using Statistics
using CairoMakie
using ProgressBars

using ParameterEstimocean.PseudoSteppingSchemes: trained_gp_predict_function, ensemble_array
using ParameterEstimocean.Transformations: ZScore, normalize!, inverse_normalize!
using ParameterEstimocean.Parameters: transform_to_constrained

# Specify a directory to which to save the files generated in this script
dir = joinpath(directory, "emulate_sample_forward_map")
isdir(dir) || mkdir(dir)

"""
    truncate_forward_map_to_length_k_uncorrelated_points(k)

# Arguments
- G: d × M array whose M columns are individual forward map outputs ∈ Rᵈ.
- k: The number of dimensions to reduce the forward map outputs to.
# Returns
- Ĝ: k × M array whose M columns are individual forward map outputs in an
uncorrelated output space Rᵏ computed by PCA analysis using SVD as described
in Appendix A.2. of Cleary et al. "Calibrate Emulate Sample" (2021).
"""
function truncate_forward_map_to_length_k_uncorrelated_points(G, y, k)

    d, M = size(G)
    m = mean(G, dims=2) # d × 1

    # Center the columns of G at zero
    Gᵀ_centered = (G .- m)'

    # SVD
    # Gᵀ = Ĝᵀ Σ Vᵀ
    # (M × d) = (M × d)(d × d)(d × d)
    F = svd(Gᵀ_centered; full=false)
    Ĝᵀ = F.U
    Σ = Diagonal(F.S)
    Vᵀ = F.Vt

    @assert Gᵀ_centered ≈ Ĝᵀ * Σ * Vᵀ
    @show size(Gᵀ_centered), M, d
    @show size(Ĝᵀ), M, d
    @show size(Σ), d, d
    @show size(Vᵀ), d, d

    # Eigenvalue sum
    total_eigenval = sum(Σ .^ 2)

    # Keep only the `k` < d most important dimensions
    # corresponding to the `k` highest singular values.
    # Dimensions become (M × d) = (M × k)(k × k)(k × d)
    Ĝᵀ = Ĝᵀ[:, 1:k]
    Σ = Σ[1:k, 1:k]
    Vᵀ = Vᵀ[1:k, :]

    @info "Preserved $(sum(Σ .^ 2)*100/total_eigenval)% of the original variance in the output data by
           reducing the dimensionality of the output space from $d to $k."

    Ĝ = Ĝᵀ'
    #      Ĝᵀ = (Gᵀ - mᵀ) V Σ⁻¹
    # (M × k) = (M × d)(d × k)(k × k)
    # Therefore
    #       Ĝ = Σ⁻¹ Vᵀ (G - m)
    # (k × M) = (k × k)(k × d)(d × M)
    #
    # Therefore to transform the observations,
    # ŷ = Σ⁻¹ Vᵀ (y - m)
    ŷ = (1 ./ Σ) * Vᵀ * (y - m)
    return Ĝ, ŷ
end

# First, conglomerate all samples generated thus far by EKI.
# This will be the training data for the GP emulator.
# We will filter out all failed particles.
n = 5
# n = eki.iteration

# X = hcat([constrained_ensemble_array(eki, i) for i in 0:n]...) # constrained
X = hcat([ensemble_array(eki, iter) for iter in 0:n]...) # unconstrained
# G = hcat(getproperty.(eki.iteration_summaries, :forward_map_output)...)
@info "Performing emulation based on samples from the first $n iterations of EKI."
G = hcat([summary.forward_map_output for summary in eki.iteration_summaries[0:5]]...)

nan_values = vec(mapslices(any, isnan.(G); dims=1)) # bitvector
not_nan_indices = findall(.!nan_values) # indices of columns (particles) with no `NaN`s
X = X[:, not_nan_indices]
G = G[:, not_nan_indices]

# Transform forward map output samples to uncorrelated space.
# This will allow us to use the uncertainty estimates from each
# GP in the emulator.
k = 20
y = eki.mapped_observations
Ĝ, ŷ = truncate_forward_map_to_length_k_uncorrelated_points(G, y, k)

# We will approximately non-dimensionalize the inputs according to mean and variance
# computed across all generated training samples.
zscore_X = ZScore(mean(X, dims=2), std(X, dims=2))
normalize!(X, zscore_X)

# Ensemble covariance across all generated samples (diagonal should be all ones)
cov_θθ_all_iters = cov(X, X, dims = 2, corrected = false)

###
### Emulate and Sample
###

# The likelihood we wish to sample with MCMC is π(θ|y)=exp(-Φ(θ)), the posterior density on θ given y.
# The MCMC sampler takes in a function `nll` which maps θ to the negative log value Φ(θ).
# In the following example, we use a GP to emulate the forward map output G.

# We will take advantage of the parallelizability of our forward map
# by running parallel chains of MCMC.
n_chains = 128

# Length and burn-in length per chain
chain_length = 1000
burn_in = 100

chain_length_emulate = 10000
burn_in_emulate = 1000

###
### Sample from emulated loss landscape using parallel chains of MCMC
###

# vector of predict functions. Ĝ is k x Nsamples
# predicts = [trained_gp_predict_function(Ĝ[i,:]) for i in size(Ĝ,1)]
predicts = []

@info "Training $k gaussian processes for the emulator."
validation_results=[]
for i in ProgressBar(1:k) # forward map index

    # Values of the forward maps of each sample at index `i`
    yᵢ = Ĝ[i, :]

    # Reserve `validation_fraction` representative samples for the emulator
    # We will sort `yᵢ` and take evenly spaced samples so the samples are representative.
    M = length(yᵢ)
    lq = Int(round(M/4))
    uq = lq*3
    evenly_spaced_samples = Int.(round.(range(lq, uq, length = Int(round(0.03M)))))
    emulator_validation_indices = sort(eachindex(yᵢ), by = i -> yᵢ[i])[evenly_spaced_samples]
    not_emulator_validation_indices = [i for i in 1:M if !(i in emulator_validation_indices)]
    X_validation = X[:, emulator_validation_indices]
    yᵢ_validation = yᵢ[emulator_validation_indices]

    predict = trained_gp_predict_function(X[:, not_emulator_validation_indices], yᵢ[not_emulator_validation_indices]; standardize_X = false, zscore_limit = nothing)
    push!(predicts, predict)

    ŷᵢ_validation, Γgp_validation = predict(X_validation)
    push!(validation_results, (yᵢ_validation, ŷᵢ_validation, diag(Γgp_validation)))
end

n_columns = 5
N_axes = k
n_rows = Int(ceil(N_axes / n_columns))
fig = Figure(resolution = (300n_columns, 350n_rows), fontsize = 8)
ax_coords = [(i, j) for j = 1:n_columns, i = 1:n_rows]
for (forward_map_index, result) in enumerate(validation_results)

    yᵢ_validation, ŷᵢ_validation, Γgp_validation = result
    r = round(Statistics.cor(yᵢ_validation, ŷᵢ_validation); sigdigits=2)
    @info "Pearson R for predictions on reserved subset of training points for $(k)th entry in the transformed forward map output : $r"
    ax = Axis(fig[ax_coords[i]...], xlabel = "True",
                                    xticks = LinearTicks(2),
                                    ylabel = "Predicted",
                                    title = "Index $i. Pearson R: $r")

    scatter!(ax, yᵢ_validation, ŷᵢ_validation)
    lines!(ax, yᵢ_validation, yᵢ_validation; color=(:black, 0.5), linewidth=3)
    errorbars!(yᵢ_validation, ŷᵢ_validation, sqrt.(Γgp_validation), color = :red, linewidth=2)
    save(joinpath(dir, "emulator_validation_performance_linear_linear.png"), fig)
end

# θ is a vector of parameter vectors
function nll_emulator(θ_vector)

    θ_array = hcat(θ_vector...)
    results = [predict(θ_array) for predict in predicts]
    μ_gps = hcat(getindex.(results, 1)...) # length(θ) x k
    Γ_gps = cat(getindex.(results, 2)...; dims=3) # length(θ) x length(θ) x k

    # @show size(μ_gps)
    # @show size(Γ_gps)

    inv_sqrt_Γθ = eki.precomputed_arrays[:inv_sqrt_Γθ]
    μθ = eki.precomputed_arrays[:μθ]
    Γŷ = 0 # assume that Γŷ is negligible compared to Γgp

    Φs = []
    for (i, θ) in enumerate(θ_vector)

        Ggp = μ_gps[i, :] # length-k vector
        Γgp = diagm(Γ_gps[i, i, :])

        θ_unconstrained = copy(θ)
        inverse_normalize!(θ_unconstrained, zscore_X)
        # Φ₁ = (1/2)*|| (Γgp + Γŷ)^(-½) * (ŷ - Ggp) ||²
        Φ₁ = (1/2) * norm(inv(sqrt(Γgp .+ Γŷ)) * (ŷ .- Ggp))^2
        # Φ₂ = (1/2)*|| Γθ^(-½) * (θ - μθ) ||²
        Φ₂ = eki.tikhonov ? (1/2) * norm(inv_sqrt_Γθ * (θ_unconstrained .- μθ))^2 : 0
        Φ₃ = (1/2) * log(det(Γgp .+ Γŷ))

        push!(Φs, Φ₁ + Φ₂ + Φ₃)
    end

    return Φs
end

C = Matrix(Hermitian(cov_θθ_all_iters))
@assert C ≈ cov_θθ_all_iters
dist_θθ_all_iters = MvNormal(zeros(size(X, 1)), C)
perturb() = rand(dist_θθ_all_iters)
proposal(θ) = θ + perturb()
seed_X = [perturb() for _ in 1:n_chains] # Where to initialize θ
# initial_ensemble = eki.iteration_summaries[0].parameters_unconstrained
# seed_X = [initial_ensemble[:,j] for j in 1:size(initial_ensemble,2)]

chain_X_emulated, chain_nll_emulated = markov_chain(nll_emulator, proposal, seed_X, chain_length_emulate; burn_in = burn_in_emulate, n_chains)
# inverse_normalize!.(chain_X_emulated, zscore_X)

samples = hcat(chain_X_emulated...)
inverse_normalize!(samples, zscore_X)
unscaled_chain_X_emulated = collect.(transform_to_constrained(eki.inverse_problem.free_parameters.priors, samples))
# unscaled_chain_X_emulated = [samples[:,j] for j in 1:size(samples, 2)]

###
### Sample from true eki objective using parallel chains of MCMC
###

# θ is a vector of parameter vectors
function nll_true(θ)

    # vector of vectors to 2d array
    θ_mx = hcat(θ...)
    inverse_normalize!(θ_mx, zscore_X)

    G = forward_map(training, θ)

    # # Vector of (Φ₁, Φ₂) pairs, one for each ensemble member at the current iteration
    # objective_values = []
    # error = 0
    # for j in 1:size(θ_mx, 2)
    #     try
    #         error = sum(eki_objective(eki, θ_mx[:, j], G[:, j]; constrained=true))
    #     catch DomainError
    #         error = Inf
    #     end
    #     push!(objective_values, error)
    # end
    objective_values = [sum(eki_objective(eki, θ_mx[:, j], G[:, j]; constrained=false)) for j in 1:size(θ_mx, 2)]

    # @show length(objective_values)
    # @show findall(x -> isfinite(x), objective_values)

    return objective_values
end

chain_X, chain_nll = markov_chain(nll_true, proposal, seed_X, chain_length; burn_in, n_chains)

samples = hcat(chain_X...)
inverse_normalize!(samples, zscore_X)
unscaled_chain_X = collect.(transform_to_constrained(eki.inverse_problem.free_parameters.priors, samples))
# unscaled_chain_X = [samples[:,j] for j in 1:size(samples, 2)]

using FileIO
file = joinpath(dir, "markov_chains.jld2")
save(file, Dict("unscaled_chain_X" => unscaled_chain_X,
                "unscaled_chain_X_emulated" => unscaled_chain_X_emulated))

# G_sobol = forward_map(big_training, params)
# save(file, G)
# G_sobol = load(file)["G"]

begin
    n_columns = 3
    hist_fig, hist_axes = plot_mcmc_densities2(unscaled_chain_X_emulated, parameter_set.names;
                                    n_columns,
                                    directory = dir,
                                    filename = "mcmc_densities_hist.png",
                                    label = "Emulated",
                                    color = (:blue, 0.8),
                                    type = "hist")

    # color = Makie.LinePattern(; direction = [Vec2f(1), Vec2f(1, -1)], width = 2, tilesize = (20, 20),
    #         linecolor = :blue, background_color = (:blue, 0.2))
    # color = Makie.LinePattern(; background_color = (:blue, 0.2))

    density_fig, density_axes = plot_mcmc_densities2(unscaled_chain_X_emulated, parameter_set.names;
                                    n_columns,
                                    directory = dir,
                                    filename = "mcmc_densities_density_textured.png",
                                    label = "Emulated",
                                    show_means = true,
                                    color = (:blue, 0.8),
                                    type = "density")

    plot_mcmc_densities2!(hist_fig, hist_axes, unscaled_chain_X, parameter_set.names;
                                    n_columns,
                                    directory = dir,
                                    filename = "mcmc_densities_hist.png",
                                    label = "True",
                                    color = (:orange, 0.5),
                                    type = "hist")

    plot_mcmc_densities2!(density_fig, density_axes, unscaled_chain_X, parameter_set.names;
                                    n_columns,
                                    directory = dir,
                                    filename = "mcmc_densities_density_textured.png",
                                    label = "True",
                                    show_means = true,
                                    color = (:orange, 0.5),
                                    strokecolor = :orange, strokewidth = 3, strokearound = true,
                                    type = "density")
end

begin
    xticks=(collect(1:Nparam), string.(collect(parameter_set.names)))
    xticklabelrotation = pi/2

    fig = Figure(resolution = (2000, 600), fontsize=28)
    Nparam = length(unscaled_chain_X[1])

    unscaled_chain_X_mx = hcat(unscaled_chain_X...)
    cor_true = Statistics.cor(unscaled_chain_X_mx, dims=2)

    unscaled_chain_X_emulated_mx = hcat(unscaled_chain_X_emulated...)
    cor_emulated = Statistics.cor(unscaled_chain_X_emulated_mx, dims=2)

    lb = minimum([minimum(cor_true), minimum(cor_emulated)])
    ub = maximum([maximum(cor_true), maximum(cor_emulated)])
    lb = -1.0
    ub = 1.0

    ax1 = Axis(fig[1, 1]; xticks, yticks=xticks, title="Emulated", xticklabelrotation)
    hmap1 = heatmap!(ax1, cor_emulated; colormap = :balance, colorrange=(lb, ub))
    # Colorbar(fig[1, 2], hmap1; label="Correlation")

    ax2 = Axis(fig[1, 2]; xticks, yticks=xticks, title="True", xticklabelrotation)
    hmap2 = heatmap!(ax2, cor_true; colormap = :balance, colorrange=(lb, ub))
    Colorbar(fig[1, 3], hmap2; label="Pearson Correlation")

    ax4 = Axis(fig[1, 4]; xticks, yticks=xticks, title="Difference (Emulated - True)", xticklabelrotation)
    hmap4 = heatmap!(ax4, cor_emulated .- cor_true; colormap = :balance, colorrange=(lb, ub))

    colsize!(fig.layout, 3, Relative(1/25))

    save(joinpath(dir, "correlation_heatmaps.png"), fig)
end

# colsize!(fig.layout, 1, Aspect(1, 1.0, 1, 1.0))
# colgap!(fig.layout, 7)
# display(fig)


text:
"""
	histogram(cmd0::String="", arg1=[]; kwargs...)

Reads file and examines the first data column to calculate histogram parameters based on the bin-width provided.

Full option list at [`pshistogram`](http://gmt.soest.hawaii.edu/doc/latest/pshistogram.html)

Parameters
----------

- $(GMT.opt_J)
- **W** : **bin** : **width** : -- Number or Str --

	Sets the bin width used for histogram calculations.
	[`-W`](http://gmt.soest.hawaii.edu/doc/latest/pshistogram.html#w)
- **A** : **horizontal** : -- Bool or [] --

	Plot the histogram horizontally from x = 0 [Default is vertically from y = 0].
	[`-A`](http://gmt.soest.hawaii.edu/doc/latest/pshistogram.html#a)
- $(GMT.opt_B)
- **C** : **color** : -- Str or GMTcpt --

	Give a CPT. The mid x-value for each bar is used to look-up the bar color.
	[`-C`](http://gmt.soest.hawaii.edu/doc/latest/pshistogram.html#c)
- **D** : **annot** : **annotate** : -- Str or [] --

	Annotate each bar with the count it represents.
	[`-D`](http://gmt.soest.hawaii.edu/doc/latest/pshistogram.html#d)
- **F** : **center** : -- Bool or [] --

	Center bin on each value. [Default is left edge].
	[`-F`](http://gmt.soest.hawaii.edu/doc/latest/pshistogram.html#f)
- **G** : **fill** : -- Number or Str --

	Select filling of bars [if no G, L or C set G=100].
	[`-G`](http://gmt.soest.hawaii.edu/doc/latest/pshistogram.html#g)
- **I** : **inquire** : -- Bool or [] --

	Inquire about min/max x and y after binning.
	[`-I`](http://gmt.soest.hawaii.edu/doc/latest/pshistogram.html#i)
- **L** : **labels** : -- Str or [] --

	Draw bar outline using the specified pen thickness [if no G, L or C set L=0.5].
	[`-L`](http://gmt.soest.hawaii.edu/doc/latest/pshistogram.html#l)
- **N** : **normal** : -- Str --

	Draw the equivalent normal distribution; append desired pen [0.5p,black].
	[`-N`](http://gmt.soest.hawaii.edu/doc/latest/pshistogram.html#n)
- $(GMT.opt_P)
- **Q** : **alpha** : -- Number or [] --

	Sets the confidence level used to determine if the mean resultant is significant.
	[`-Q`](http://gmt.soest.hawaii.edu/doc/latest/pshistogram.html#q)
- **R** : **region** : -- Str --

	Specifies the ‘region’ of interest in (r,azimuth) space. r0 is 0, r1 is max length in units.
	[`-R`](http://gmt.soest.hawaii.edu/doc/latest/pshistogram.html#r)
- **S** : **stairs** : -- Str or number --

	Draws a stairs-step diagram which does not include the internal bars of the default histogram.
	[`-S`](http://gmt.soest.hawaii.edu/doc/latest/pshistogram.html#s)
- **Z** : **kind** : -- Number or Str --

	Choose between 6 types of histograms.
	[`-Z`](http://gmt.soest.hawaii.edu/doc/latest/pshistogram.html#z)
- $(GMT.opt_U)
- $(GMT.opt_V)
- $(GMT.opt_X)
- $(GMT.opt_Y)
- $(GMT.opt_bi)
- $(GMT.opt_di)
- $(GMT.opt_e)
- $(GMT.opt_h)
- $(GMT.opt_i)
- $(GMT.opt_p)
- $(GMT.opt_t)
- $(GMT.opt_swap_xy)
"""
function histogram(cmd0::String="", arg1=[]; caller=[], K=false, O=false, first=true, kwargs...)

	arg2 = []		# May be needed if GMTcpt type is sent in via C
	N_args = isempty_(arg1) ? 0 : 1

	length(kwargs) == 0 && return monolitic("pshistogram", cmd0, arg1)	# Speedy mode

	d = KW(kwargs)
	output, opt_T, fname_ext = fname_out(d)		# OUTPUT may have been an extension only

	cmd, opt_B, opt_J, opt_R = parse_BJR(d, "", caller, O, " -JX12c/12c")
	cmd, = parse_JZ(cmd, d)
	cmd  = parse_UVXY(cmd, d)
	cmd, opt_bi = parse_bi(cmd, d)
	cmd, opt_di = parse_di(cmd, d)
	cmd, = parse_e(cmd, d)
	cmd, = parse_h(cmd, d)
	cmd, opt_i = parse_i(cmd, d)
	cmd, = parse_p(cmd, d)
	cmd, = parse_t(cmd, d)
	cmd, = parse_swap_xy(cmd, d)
	cmd = parse_params(cmd, d)

	cmd, K, O, opt_B = set_KO(cmd, opt_B, first, K, O)		# Set the K O dance

	# If file name sent in, read it and compute a tight -R if this was not provided
	if (isempty(opt_R))  opt_R = " "  end		# So it doesn't try to find the -R in next call
	cmd, arg1, opt_R, opt_i = read_data(d, cmd0, cmd, arg1, opt_R, opt_i, opt_bi, opt_di)

	cmd, arg1, arg2, = add_opt_cpt(d, cmd, [:C :color :cmap], 'C', N_args, arg1, arg2)

	cmd = add_opt(cmd, 'A', d, [:A :horizontal])
	cmd = add_opt(cmd, 'D', d, [:D :annot :annotate])
	cmd = add_opt(cmd, 'F', d, [:F :center])
	cmd = add_opt(cmd, 'G', d, [:G :fill])
	cmd = add_opt(cmd, 'I', d, [:I :inquire])
	opt_L = opt_pen(d, 'L', [:L :pen])
	cmd = add_opt(cmd, 'Q', d, [:Q :cumulative])
	cmd = add_opt(cmd, 'S', d, [:S :stairs])
	cmd = add_opt(cmd, 'W', d, [:W :bin :width])
	cmd = add_opt(cmd, 'Z', d, [:Z :kind])
	if (isempty(opt_L) && !occursin("-G", cmd) && !occursin("-C", cmd))		# If no -L, -G or -C set these defaults
		cmd = cmd * " -G150" * " -L0.5p"
	elseif (!isempty(opt_L))
		cmd = cmd * opt_L
	end

	for symb in [:N :normal]
		if (haskey(d, symb))
			if (isa(d[symb], Number))      cmd = @sprintf("%s -N%d", cmd, d[symb])
			elseif (isa(d[symb], String))  cmd = cmd * " -N" * d[symb]
			elseif (isa(d[symb], Tuple))   cmd = cmd * " -N" * parse_arg_and_pen(d[symb])
			end
			break
		end
	end

	cmd = finish_PS(d, cmd, output, K, O)
	return finish_PS_module(d, cmd, "", output, fname_ext, opt_T, K, "pshistogram", arg1, arg2)
end

# ---------------------------------------------------------------------------------------------------
histogram!(cmd0::String="", arg1=[]; caller=[], K=true, O=true, first=false, kw...) =
	histogram(cmd0, arg1; caller=caller, K=K, O=O, first=first, kw...)

histogram(arg1=[]; caller=[], K=false, O=false, first=true, kw...) =
	histogram("", arg1; caller=caller, K=K, O=O, first=first, kw...)

histogram!(arg1=[]; caller=[], K=true, O=true, first=false, kw...) =
	histogram("", arg1; caller=caller, K=K, O=O, first=first, kw...)

pshistogram  = histogram			# Alias
pshistogram! = histogram!			# Alias


text:
using PdfFraud
using GenerativeModels
using Flux
using ValueHistories
using ArgParse
using BSON
using GMExtensions
using CuArrays
using DelimitedFiles, DataFrames, CSV

# this is for running the script from REPL - default values
if length(ARGS) == 0
	push!(ARGS, "test")
end
# parse arguments
s = ArgParseSettings()
@add_arg_table s begin
    "modelpath"
    	default = ""
    	arg_type = String
    	help = "a master dir of where the models are saved"
    "--imodels"
    	default = [0]
    	help = "only compute models given by indices"
    	arg_type = Int
    	nargs = '+'
end
parsed_args = parse_args(ARGS, s)

# extract settings from the parsed arguments
modelpath = abspath(parsed_args["modelpath"])
reprpath = joinpath(dirname(modelpath), "representations")
mkpath(reprpath)
imodels = parsed_args["imodels"]

# get labeled data
data, labels, sha256, page_nums = PdfFraud.load_pdf_data()
data = data |> gpu;

# save the page info
lf = joinpath(reprpath, "index.csv")
index = DataFrame()
index[!,:sha256] = vcat(map(x->repeat([x[1]], x[2]), zip(sha256, page_nums))...)
index[!,:page] = vcat(map(x->1:x, page_nums)...)
CSV.write(lf, index)

# load model(s)
model_files = joinpath.(modelpath, readdir(modelpath))
@info "Found $(length(model_files)) saved models."
if imodels[1] != 0
	model_files = model_files[imodels]
end
@info "Continuing with $(length(model_files)) models."

# now get the representations
for mf in model_files
	model, history, exp_params = PdfFraud.load_model(mf)
	model = model |> gpu
	Z = PdfFraud.encode(model, data, 32, mean) |> cpu;

	# and save them so they can be easily read back
	zf = joinpath(reprpath, replace(basename(mf), ".bson"=>"_encodings.csv"))
	DelimitedFiles.writedlm(zf, Z', ',')
	@info "Succesfuly written $zf"
end


text:
module Types

    using RandomExtensions
    import Dates

    export Entity, EntityData, GAmodel, targetParams, GAParams

    abstract type Entity end
    # -------

    mutable struct EntityData
        entity
        generation::Int
        scores :: Array

        EntityData(entity, generation::Int) = new(entity, generation)
        EntityData(entity, model) = new(entity, model.params.currentGeneration)
    end

    mutable struct GAParams
        populationSize ::  Int
        generations ::  Int
        genomeSize ::  Int
        maxGenomeSize :: Int
        crossoverRate :: Float64
        mutationRate :: Float64
        elitism  :: Bool # Keep previous generation's fittest individual in place of worst in current
        historyPath :: IOStream  # Path to save log of fitness history at each generation. Can be used to plot on Excel chart etc.

        totalFitness :: Float64
        targetFitness :: Float64
        targetFitnessCount ::  Int
        currentGeneration ::  Int

        #thisGeneration :: Array
        #nextGeneration :: Array
        #fitnessTable :: Array{Float64,1}
    end

    # -------

    mutable struct GAmodel
        initial_pop_size::Int
        gen_num::Int

        population::Array
        refSet ::Array
        subSets :: Array
        #pop_data::Array{EntityData}
        #freezer::Array{EntityData}

        rng::AbstractRNG

        ga
        specific_fitness

        scores :: Array
        params :: GAParams

        instructionsSet #:: AbstractDict

        GAmodel() = new(0, 1, Any[], Any[], Any[], MersenneTwister(time_ns()), nothing, nothing, [],
                GAParams(136, 1000 , 150, 150, 0.7, 0.01, true, open("output.txt", "w") ,  0.0 , 0, 0.0 , 0 ),
                Dict() )
        GAmodel(params :: GAParams) = new(0, 1, Any[], Any[], Any[], MersenneTwister(time_ns()),
                nothing, nothing, [], params, Dict())
    end

end  # module Types


text:
abstract type AbstractSignedDistanceField end

(sdf::AbstractSignedDistanceField)(::Point{3, T}) where {T<:Real} = error("Signed Distance Field for $(typeof(sdf)) has not been defined.")
normal(sdf::AbstractSignedDistanceField, ::Point{3, T}) where {T<:Real} = error("Normal of Signed Distance Field for $(typeof(sdf)) has not been defined.")
normalzygote(sdf::AbstractSignedDistanceField, point::Point{3, T}) where {T<:Real} =
    Vec3{T}(normalize(Zygote.gradient(sdf, point)[1]))
normalfinitediff(sdf::AbstractSignedDistanceField, point::Point{3, T}) where {T<:Real} =
    Vec3{T}(normalize(FiniteDifferences.grad(central_fdm(5, 1), sdf, point)[1]))
normalforwarddiff(sdf::AbstractSignedDistanceField, point::Point{3, T}) where {T<:Real} =
    Vec3{T}(normalize(ForwardDiff.gradient(sdf, point)))

struct SphereSignedDistanceField{T<:Real} <: AbstractSignedDistanceField
    radius::T
end

radius(sdf::SphereSignedDistanceField) = sdf.radius
(sdf::SphereSignedDistanceField{T1})(point::Point{3, T2}) where {T1<:Real, T2<:Real} =
    norm(point) - T2(radius(sdf))

# Currently always pointing out
normal(::SphereSignedDistanceField{T1}, point::Point{3, T2}) where {T1<:Real, T2<:Real} =
    Vec3{T2}(normalize(point))

struct MandelBulbSignedDistanceField <: AbstractSignedDistanceField
    numfractaliter::Int64
end

numfractaliter(sdf::MandelBulbSignedDistanceField) = sdf.numfractaliter
function (sdf::MandelBulbSignedDistanceField)(point::Point{3, T}) where {T<:Real}
    px, py, pz = point
    dz = one(T)
    w = copy(point)
    m = dot(w, w)
    for i in 1:numfractaliter(sdf)
        w2 = w .* w
        w4 = w2 .* w2
        x, y, z = w
        x2, y2, z2 = w2
        x4, y4, z4 = w4
        m2 = m .* m
        m4 = m2 .* m2

        dz = 8*sqrt(m4*m2*m)*dz + 1

        k3 = x2 + z2
        k2 = 1 / sqrt(k3*k3*k3*k3*k3*k3*k3)
        k1 = x4 + y4 + z4 - 6y2*z2 - 6x2*y2 + 2z2*x2
        k4 = x2 - y2 + z2

        newwx = px + 64x*y*z*k4*k1*k2 * (x2 - z2) * (x4 - 6x2*z2 + z4)
        newwy = py - 16y2*k3*k4*k4 + k1*k1
        newwz = pz - 8y*k4*k1*k2 * (x4*x4 - 28x4*x2*z2 + 70x4*z4 - 28x2*z2*z4 + z4*z4)

        w = Point{3, T}(newwx, newwy, newwz)
        m = dot(w, w)
        if m > 256
            break
        end
    end

    T(0.25) * log(m) * sqrt(m) / dz
end

normal(sdf::MandelBulbSignedDistanceField, point::Point{3, T}) where {T<:Real} =
    normalforwarddiff(sdf, point)

struct BoundingVolumeSignedDistanceField{T<:Real, BoundingSDF<:AbstractSignedDistanceField,
                                         InteriorSDF<:AbstractSignedDistanceField} <:
                                         AbstractSignedDistanceField
    overshootfactor::T
    boundingSDF::BoundingSDF
    interiorSDF::InteriorSDF
end

overshootfactor(sdf::BoundingVolumeSignedDistanceField) = sdf.overshootfactor
boundingSDF(sdf::BoundingVolumeSignedDistanceField) = sdf.boundingSDF
interiorSDF(sdf::BoundingVolumeSignedDistanceField) = sdf.interiorSDF

function (bsdf::BoundingVolumeSignedDistanceField)(point::Point{3, T}) where {T<:Real}
    bounding_signed_distance = boundingSDF(bsdf)(point)
    if bounding_signed_distance > T(overshootfactor(bsdf))
        bounding_signed_distance
    else
        interiorSDF(bsdf)(point)
    end
end

function normal(bsdf::BoundingVolumeSignedDistanceField, point::Point{3, T}) where {T<:Real}
    bounding_signed_distance = boundingSDF(bsdf)(point)
    if bounding_signed_distance > T(overshootfactor(bsdf))
        normal(boundingSDF(bsdf), point)
    else
        normal(interiorSDF(bsdf), point)
    end
end


text:
#!/usr/bin/env julia

#load path to qjulia home directory
push!(LOAD_PATH, joinpath(@__DIR__, "..", "matrix"))
push!(LOAD_PATH, joinpath(@__DIR__, "..", "core"))

using MatrixBase
using MatrixMarket
using LinearAlgebra
using Random

using QJuliaSolvers
using QJuliaEnums
using MPI

#matrix_path = "/home/astrel/data/nasa2146/nasa2146.mtx"
matrix_path = "/home/astrel/data/nasa2910/nasa2910.mtx"
#matrix_path = "/home/astrel/data/nasa4704/nasa4704.mtx"
#matrix_path = "/home/astrel/data/smt/smt.mtx"

#initialize MPI
MPI.Init()

Random.seed!(2018)

csrM = MatrixBase.CSRMat{Float64}(matrix_path)

MatrixBase.print_CSRMat_info(csrM)

data_type = typeof(csrM.csrVals[1])

b = Vector{data_type}(undef, csrM.Dims[2])
x = Vector{data_type}(undef, csrM.Dims[2])
z = Vector{data_type}(undef, csrM.Dims[2])

x .= 0.0
z .= 0.0
b .= rand(data_type, csrM.Dims[2])

M(b,x) = MatrixBase.csrmv(b, csrM, x)
#or just:
#M=MatrixMarket.mmread(matrix_path)

solv_param = QJuliaSolvers.QJuliaSolverParam_qj()
# Set up parameters
solv_param.inv_type               = QJuliaEnums.QJULIA_LANMR_INVERTER
solv_param.inv_type_precondition  = QJuliaEnums.QJULIA_INVALID_INVERTER
solv_param.tol                    = 1e-8
#
solv_param.maxiter                = 10
solv_param.Nsteps                 = 1

@time QJuliaSolvers.solve(x, b, M, M, solv_param)

#compute residual:
M(z, x)
z .= b - z

println("Solution norm: ", sqrt(dot(x,x)), ", absolute residual: ", sqrt(dot(z,z)))

MPI.Finalize()

max_stars_repo_name: alexstrel/QJulia

max_issues_repo_name: alexstrel/QJulia

max_forks_repo_name: alexstrel/QJulia


text:
### File declaring all the various reaction networks the package is tested on ###
#A large number of reaction networks are added to test as many different cases as possible.
#More test networks can be added favourably.

#Decalares 3 vectors to contain the various test sets (and a fourth with fixed concentrations coupled with the last test set).
reaction_networks_standard = Vector{DiffEqBase.AbstractReactionNetwork}(undef,10)
reaction_networks_hill = Vector{DiffEqBase.AbstractReactionNetwork}(undef,10)
reaction_networks_fixed_conc = Vector{DiffEqBase.AbstractReactionNetwork}(undef,10)
min_reaction_networks_standard = Vector{DiffEqBase.AbstractReactionNetwork}(undef,10)

#Standard reaction networks.
reaction_networks_standard[1] = @reaction_network begin
    (p1,p2,p3), ∅ → (X1,X2,X3)
    (k1,k2), X2 ⟷ X1 + 2X3
    (k3,k4), X1 ⟷ X3
    (d1,d2,d3), (X1,X2,X3) → ∅
end p1 p2 p3 k1 k2 k3 k4 d1 d2 d3

reaction_networks_standard[2] = @reaction_network begin
    mmR(X2,v1,K1), ∅ → X1
    mm(X1,v2,K2), ∅ → X2
    d, X1+X2 → ∅
end v1 K1 v2 K2 d

reaction_networks_standard[3] = @reaction_network begin
    mm(X2,v1,K1), ∅ → X1
    mm(X3,v2,K2), ∅ → X2
    (k1,k2), X1 ⟷ X4
    (k3,k4), X4 + X2 ⟷ X3 +X1
    d, (X1,X2,X3,X4) → ∅
end v1 K1 v2 K2 k1 k2 k3 k4 d

reaction_networks_standard[4] = @reaction_network begin
    mmR(X4,v1,K1), ∅ → X1
    mmR(X1,v2,K2), ∅ → X2
    mmR(X2,v3,K3), ∅ → X3
    mmR(X3,v4,K4), ∅ → X4
    (d1,d2,d3,d4), (X1,X2,X3,X4) → ∅
end v1 K1 v2 K2 v3 K3 v4 K4 d1 d2 d3 d4

reaction_networks_standard[5] = @reaction_network begin
    p, ∅ → X1
    (k1,k2), X1 ⟷ X2
    (k3,k4), X2 ⟷ X3
    (k5,k6), X3 ⟷ X4
    d, X4 → ∅
end p k1 k2 k3 k4 k5 k6 d

reaction_networks_standard[6] = @reaction_network begin
    (p1,p2), ∅ → (X1,X2)
    (k1,k2), 2X1 ⟷ X3
    (k3,k4), X2 + X3 ⟷ 4X4
    (k5,k6), X4 + X1 ⟷ 2X3
    d, (X1,X2,X3,X4) → ∅
end p1 p2 k1 k2 k3 k4 k5 k6 d

reaction_networks_standard[7] = @reaction_network begin
    (p1,p2,p3), ∅ → (X1,X2,X3)
    (k1,k2), X1 + 2X2 ⟷ X4
    (mm(X3,v1,K1),k3), X4 ⟷ X5
    (d1,d2,d3,d4,d5), (X1,X2,X3,X4,X5) → ∅
end p1 p2 p3 k1 k2 k3 v1 K1 d1 d2 d3 d4 d5

reaction_networks_standard[8] = @reaction_network begin
    p, ∅ → 2X1
    k1, X1 → X2
    (k2, k3), X2 → X3
    d, X3 → ∅
end p k1 k2 k3 d

reaction_networks_standard[9] = @reaction_network begin
    (p1,p2,p3), ∅ ⟶ (X1,X2,X3)
    (d1,d2,d3), (X1,X2,X3) ⟶ ∅
    (k1,k2), X1 + X2 ⟷ X3
    (k3,k4), X3 ⟷ X4
    (k5,k6), X4 ⟷ X1 + X2
end p1 p2 p3 d1 d2 d3 k1 k2 k3 k4 k5 k6

reaction_networks_standard[10] = @reaction_network begin
    p, ∅ ⟶ X1
    (k1,k2), X1 → X2
    (k3,k4), X2 → X3
    (k5,k6), X3 → X4
    (k7,k8), X4 → X5
    d, X5  ⟶ ∅
end p k1 k2 k3 k4 k5 k6 k7 k8 d

#Hill type reaction networks (these contaisn a parameter in the exponent and should have integer parameters).
reaction_networks_hill[1] = @reaction_network begin
    hillR(X2,v1,K1,n1),   ∅ → X1
    hillR(X1,v2,K2,n2),   ∅ → X2
    (d1,d2),          (X1,X2) → ∅
end v1 v2 K1 K2 n1 n2 d1 d2

reaction_networks_hill[2] = @reaction_network begin
    hillR(X3,v1,K1,n1),   ∅ → X1
    hillR(X1,v2,K2,n2),   ∅ → X2
    hillR(X2,v3,K3,n3),   ∅ → X3
    (d1,d2,d3),          (X1,X2,X3) → ∅
end v1 v2 v3 K1 K2 K3 n1 n2 n3 d1 d2 d3

reaction_networks_hill[3] = @reaction_network begin
    hillR(X2,v1,K1,n1), ∅ → X1
    hill(X1,v2,K2,n2), ∅ → X2
    d, X1+X2 → ∅
end v1 K1 n1 v2 K2 n2 d

reaction_networks_hill[4] = @reaction_network begin
    hillR(X2,v1,K1,n1)*hillR(X3,v1,K1,n1), ∅ → X1
    hillR(X1,v2,K2,n2)*hillR(X3,v2,K2,n2), ∅ → X2
    hillR(X1,v3,K3,n3)*hillR(X2,v3,K3,n3), ∅ → X3
    (d1,d2,d3), (X1,X2,X3)  ⟶ ∅
end v1 K1 n1 v2 K2 n2 v3 K3 n3 d1 d2 d3

reaction_networks_hill[5] = @reaction_network begin
    hillR(X2,v1,K1,n1)*hill(X4,v1,K1,n1), ∅ → X1
    hill(X5,v2,K2,n2), ∅ → X2
    hill(X3,v3,K3,n3), ∅ → X3
    hillR(X1,v4,K4,n4), ∅ → X4
    hill(X2,v5,K5,n5), ∅ → X5
    (k1,k2), X2 ⟷ X1 + 2X4
    (k3,k4), X4 ⟷ X3
    (k5,k6), 3X5 + X1 ⟷ X2
    (d1,d2,d3,d4,d5), (X1,X2,X3,X4,X5)  ⟶ ∅
end v1 K1 n1 v2 K2 n2 v3 K3 n3 v4 K4 n4 v5 K5 n5 k1 k2 k3 k4 k5 k6 d1 d2 d3 d4 d5

reaction_networks_hill[6] = @reaction_network begin
    v/10+hill(X1,v,K,n), ∅ → X1
    d, X1 → ∅
end v K n d

reaction_networks_hill[7] = @reaction_network begin
    v/10 + hill(X1,v,K,n), ∅ → X1+X2
    (k1,k2), X1 + X2 ↔ X3
    k3, X3 → X1
    d, (X1,X2,X3) → ∅
end v K n k1 k2 k3 d

reaction_networks_hill[8] = @reaction_network begin
    hill(X2,v1,K1,n1), ∅ → X1
    hillR(X1,v2,K2,n2)*hill(X3,v3,K3,n3), ∅ → X2
    hill(X2,v4,K4,n4), ∅ → X3
    (d1,d2,d3), (X1,X2,X3) → ∅
end v1 K1 n1 v2 K2 n2 v3 K3 n3 v4 K4 n4 d1 d2 d3

reaction_networks_hill[9] = @reaction_network begin
    hill(X1,v1,K1,n1)*hillR(X1,v2,K2,n2), ∅ → X1
    d, X1 → ∅
end v1 K1 n1 v2 K2 n2 d

reaction_networks_hill[10] = @reaction_network begin
    hill(X2,v1,K1,n1), ∅ → X1
    hillR(X4,v2,K2,n2), ∅ → X2
    (k1,k2), 2X1 + X2 ⟷ X3
    (k3,k4), 2X2 + X3 ⟷ X4
    (k5,k6), X1 + X2 + X3 + X4 ⟷ X5 + X6
    (d1,d2), (X5,X6) → ∅
end v1 K1 n1 v2 K2 n2 k1 k2 k3 k4 k5 k6 d1 d2

#Reaction networks with fixed concentrations.
#The fixed concentration(s) is declared underneth the reaction network.
reaction_networks_fixed_conc_1 = @reaction_network begin
    (k1,k2), X1 ↔ X2
    (k3,k4), X2 ↔ X3
    (k5,k6), X3 ↔ X1
end k1 k2 k3 k4 k5 k6 FC
@add_constraint reaction_networks_fixed_conc_1 X1+X2+X3=FC
reaction_networks_fixed_conc[1] = reaction_networks_fixed_conc_1

reaction_networks_fixed_conc_2 = @reaction_network begin
    (k1,k2), X1 ↔ 2X1
    (k3,k4), X1 + X2 ↔ X3
    (k5,k6), X3 ↔ X2
end k1 k2 k3 k4 k5 k6 FC
@add_constraint reaction_networks_fixed_conc_2 X2+X3=FC
reaction_networks_fixed_conc[2] = reaction_networks_fixed_conc_2

reaction_networks_fixed_conc_3 = @reaction_network begin
    (k1,k2*X5), X1 ↔ X2
    (k3*X5,k4), X3 ↔ X4
    (p+k5*X2*X3,d), ∅ ↔ X5
end k1 k2 k3 k4 p k5 d FC1 FC2
@add_constraints reaction_networks_fixed_conc_3 begin
    X1+X2=FC1
    X3+X4=FC2
end
reaction_networks_fixed_conc[3] = reaction_networks_fixed_conc_3

reaction_networks_fixed_conc_4 = @reaction_network begin
    (k1,k2), X1 + X2 ↔ X3
    (mm(X3,v,K),d), ∅ ↔ X4
end k1 k2 v K d FC1 FC2
@add_constraints reaction_networks_fixed_conc_4 begin
    X1+X3=FC1
    X2+X3=FC2
end
reaction_networks_fixed_conc[4] = reaction_networks_fixed_conc_4

reaction_networks_fixed_conc_5 = @reaction_network begin
    (k1,k2), X1 ↔ 2X2
    (k3,k4), 2X2 ↔ 3X3
    (k5,k6), 3X3 ↔ 4X4
end k1 k2 k3 k4 k5 k6 FC
@add_constraint reaction_networks_fixed_conc_5 24X1+12X2+8X3+6X4=FC
reaction_networks_fixed_conc[5] = reaction_networks_fixed_conc_5

reaction_networks_fixed_conc_6 = @reaction_network begin
    mmR(X1,v1,K1), X1 → X2
    mmR(X2,v2,K2), X2 → X3
    mmR(X3,v3,K3), X3 → X1
end v1 K1 v2 K2 v3 K3 FC
@add_constraint reaction_networks_fixed_conc_6 X1+X2+X3=FC
reaction_networks_fixed_conc[6] = reaction_networks_fixed_conc_6

reaction_networks_fixed_conc_7 = @reaction_network begin
    (k1,k2), X1 + X2 ↔ X3
    (mm(X3,v,K),d), ∅ ↔ X2
    (k3,k4), X2 ↔ X4
end k1 k2 k3 k4 v K d FC
@add_constraint reaction_networks_fixed_conc_7 X1+X3=FC
reaction_networks_fixed_conc[7] = reaction_networks_fixed_conc_7

reaction_networks_fixed_conc_8 = @reaction_network begin
    (k1,k2), X1 + X2 ↔ X3
    (mm(X3,v1,K1),mm(X4,v2,K2)), X3 ↔ X4
end k1 k2 v1 K1 v2 K2 FC1 FC2
@add_constraints reaction_networks_fixed_conc_8 begin
    X1+X2+X4=FC1
    X2+X3+X4=FC2
end
reaction_networks_fixed_conc[8] = reaction_networks_fixed_conc_8

reaction_networks_fixed_conc_9 = @reaction_network begin
    (k1,k2), X1 + X2 ↔ X3
    (k3,k4), X3 + X4 ↔ X5
    (k5,k6), X5 + X6 ↔ X7
end k1 k2 k3 k4 k5 k6 FC1 FC2 FC3 FC4
@add_constraints reaction_networks_fixed_conc_9 begin
    X1+X3+X5+X7=FC1
    X2+X3+X5+X7=FC2
    X4+X5+X7=FC3
    X6+X7=FC4
end
reaction_networks_fixed_conc[9] = reaction_networks_fixed_conc_9

reaction_networks_fixed_conc_10 = @reaction_network rnType begin
    kDeg,       (w,w2,w2v,v,w2v2,vP,σB,w2σB) ⟶ ∅
    kDeg,       vPp ⟶ phos
    (kBw,kDw),  2w ⟷ w2
    (kB1,kD1),  w2 + v ⟷ w2v
    (kB2,kD2),  w2v + v ⟷ w2v2
    kK1,        w2v ⟶ w2 + vP
    kK2,        w2v2 ⟶ w2v + vP
    (kB3,kD3),  w2 + σB ⟷ w2σB
    (kB4,kD4),  w2σB + v ⟷ w2v + σB
    (kB5,kD5),  vP + phos ⟷ vPp
    kP,         vPp ⟶ v + phos
    v0*((1+F*σB)/(K+σB)),     ∅ ⟶ σB
    λW*v0*((1+F*σB)/(K+σB)),  ∅ ⟶ w
    λV*v0*((1+F*σB)/(K+σB)),  ∅ ⟶ v
end kBw kDw kD kB1 kB2 kB3 kB4 kB5 kD1 kD2 kD3 kD4 kD5 kK1 kK2 kP kDeg v0 F K λW λV pTot;
@add_constraint reaction_networks_fixed_conc_10 vPp+phos=pTot
reaction_networks_fixed_conc[10] = reaction_networks_fixed_conc_10

#Minimal reaction network version of the standard reaction networks.
min_reaction_networks_standard[1] = @min_reaction_network begin
    (p1,p2,p3), ∅ → (X1,X2,X3)
    (k1,k2), X2 ⟷ X1 + 2X3
    (k3,k4), X1 ⟷ X3
    (d1,d2,d3), (X1,X2,X3) → ∅
end p1 p2 p3 k1 k2 k3 k4 d1 d2 d3

min_reaction_networks_standard[2] = @min_reaction_network begin
    mmR(X2,v1,K1), ∅ → X1
    mm(X1,v2,K2), ∅ → X2
    d, X1+X2 → ∅
end v1 K1 v2 K2 d

min_reaction_networks_standard[3] = @min_reaction_network begin
    mm(X2,v1,K1), ∅ → X1
    mm(X3,v2,K2), ∅ → X2
    (k1,k2), X1 ⟷ X4
    (k3,k4), X4 + X2 ⟷ X3 +X1
    d, (X1,X2,X3,X4) → ∅
end v1 K1 v2 K2 k1 k2 k3 k4 d

min_reaction_networks_standard[4] = @min_reaction_network begin
    mmR(X4,v1,K1), ∅ → X1
    mmR(X1,v2,K2), ∅ → X2
    mmR(X2,v3,K3), ∅ → X3
    mmR(X3,v4,K4), ∅ → X4
    (d1,d2,d3,d4), (X1,X2,X3,X4) → ∅
end v1 K1 v2 K2 v3 K3 v4 K4 d1 d2 d3 d4

min_reaction_networks_standard[5] = @min_reaction_network begin
    p, ∅ → X1
    (k1,k2), X1 ⟷ X2
    (k3,k4), X2 ⟷ X3
    (k5,k6), X3 ⟷ X4
    d, X4 → ∅
end p k1 k2 k3 k4 k5 k6 d

min_reaction_networks_standard[6] = @min_reaction_network begin
    (p1,p2), ∅ → (X1,X2)
    (k1,k2), 2X1 ⟷ X3
    (k3,k4), X2 + X3 ⟷ 4X4
    (k5,k6), X4 + X1 ⟷ 2X3
    d, (X1,X2,X3,X4) → ∅
end p1 p2 k1 k2 k3 k4 k5 k6 d

min_reaction_networks_standard[7] = @min_reaction_network begin
    (p1,p2,p3,p4,p5), ∅ → (X1,X2,X3,X4,X5)
    (k1,k2), X1 + 3X2 ⟷ 2X3 + 2X4
    (k3,k4), X2 + X3 ⟷ X5
    (mm(X1,v1,K1),k5), X5 ⟷ X1
    (mm(X2,v2,K2),k6), X1 ⟷ X2
    (d1,d2,d3,d4,d5), (X1,X2,X3,X4,X5) → ∅
end p1 p2 p3 p4 p5 k1 k2 k3 k4 k5 k6 v1 K1 v2 K2 d1 d2 d3 d4 d5

min_reaction_networks_standard[8] = @min_reaction_network begin
    p, ∅ → 2X1
    k1, X1 → X2
    (k2, k3), X2 → X3
    d, X3 → ∅
end p k1 k2 k3 d

min_reaction_networks_standard[9] = @min_reaction_network begin
    (p1,p2,p3), ∅ ⟶ (X1,X2,X3)
    (d1,d2,d3), (X1,X2,X3) ⟶ ∅
    (k1,k2), X1 + X2 ⟷ X3
    (k3,k4), X3 ⟷ X4
    (k5,k6), X4 ⟷ X1 + X2
end p1 p2 p3 d1 d2 d3 k1 k2 k3 k4 k5 k6

min_reaction_networks_standard[10] = @min_reaction_network begin
    p, ∅ ⟶ X1
    (k1,k2), X1 ⟷ X2
    (k3,k4), X2 ⟷ X3
    (k5,k6), X3 ⟷ X4
    (k7,k8), X4 ⟷ X5
    d, X5  ⟶ ∅
end p k1 k2 k3 k4 k5 k6 k7 k8 d


text:
const DimSize = Union{Int, Colon}

function checksize(targetsz::Tuple, sz::Tuple)
    length(targetsz) == length(sz) || return false
    return all(map(_checksizedim, targetsz, sz))
end

_checksizedim(l1::Int, l2::Int) = l1 == l2
_checksizedim(l1::Colon, l2::Int) = true

mockarray(T, sz) = rand(T, map(l -> l isa Colon ? rand(8:16) : l, sz))

@testset "checksize" begin
    @test checksize((10, 1), (10, 1))
    @test !checksize((100, 1), (10, 1))
    @test checksize((:, :, :), (1, 2, 3))
    @test !checksize((:, :, :), (1, 2))
    @test checksize((10, :, 1), (10, 20, 1))
    @test !checksize((10, :, 2), (10, 20, 1))
end

"""
    Bounded(block, size) <: WrapperBlock

A [`WrapperBlock`](#) for annotating spatial data blocks with
size information for their spatial bounds. As an example,
`Image{2}()` doesn't carry any size information since it
supports variable-size images, but sometimes it can be
useful to have the exact size as information where it can
be known.

Encoding using [`ProjectiveTransforms`](#) returns `Bounded`s since it
crops any input to the same size.

## Examples

```julia
block = Image{2}()  # a 2D-image block with out size informatio
wrapper = Bounded(Image{2}(), (128, 128))  # a 2D-image block with fixed size

@test checkblock(block, rand(10, 10))
@test !checkblock(wrapper, rand(10, 10))  # Expects size `(128, 128)`
```

Wrapping a `Bounded` into another `Bounded` with the same dimensionality
will update the bounds:

```julia
block = Image{2}()
Bounded(Bounded(block, (16, 16)), (8, 8)) == Bounded(block, (8, 8))
```

"""
struct Bounded{N, B<:AbstractBlock} <: WrapperBlock
    block::B
    size::NTuple{N, DimSize}
end

function Bounded(bounded::Bounded{M}, size::NTuple{N, DimSize}) where {N, M}
    N == M || error("Cannot rewrap a `Bounded` with different dimensionalities $N and $M")
    Bounded(wrapped(bounded), size)
end

function checkblock(bounded::Bounded{N}, a::AbstractArray{N}) where N
    return checksize(bounded.size, size(a)) && checkblock(parent(bounded), a)
end

@testset "Bounded [block, wrapper]" begin
    @test_nowarn Bounded(Image{2}(), (16, 16))
    bounded = Bounded(Image{2}(), (16, 16))
    @test checkblock(bounded, rand(16, 16))

    # composition
    @test Bounded(bounded, (16, 16)) == bounded
end


text:
const ns = ""

struct ComboLeg
  conId::Int
  ratio::Int
  action::String
  exchange::String
  openClose::LegOpenClose
  shortSaleSlot::Int
  designatedLocation::String
  exemptCode::Int
end
ComboLeg(; conId=     0,
           ratio=     0,
           action=   ns,
           exchange= ns) = ComboLeg(conId, ratio, action, exchange, SAME, 0, ns, -1)

struct DeltaNeutralContract
  conId::Int
  delta::Float64
  price::Float64
end

struct ExecutionFilter
  clientId::Int
  acctCode::String
  time::String                 # "yyyymmdd hh:mm:ss ttz" format
  symbol::String
  secType::String
  exchange::String
  side::String
end
ExecutionFilter(; clientId=  0,
                  acctCode= ns,
                  time=     ns,
                  symbol=   ns,
                  secType=  ns,
                  exchange= ns,
                  side=     ns) = ExecutionFilter(clientId, acctCode, time, symbol, secType, exchange, side)

struct SoftDollarTier
  name::String
  val::String
  displayName::String
end
SoftDollarTier() = SoftDollarTier(ns, ns, ns)


text:
using MRBC; using Random; using NSGAII; using BenchmarkTools; using JLD2;

# global H = Dict{UInt, Dict{UInt, Dict{Int, Dict{NTuple{2,Int}, Float64}}}}()

# @btime nsga_max(10,10,z,init,fCV=CV,fcross=rules_crossover!,fmut=rules_mutation!,pmut=0.3)

M = begin
    Random.seed!(1)
    # lk = Base.Threads.SpinLock()
    # @show H
    @time M = nsga_max(50,100,z,init,fCV=CV,fcross=rules_crossover!,fmut=rules_mutation!,pmut=0.3)
    return M
end

@save "model.jld2" M

# old version: H = Dict{UInt, Dict{UInt, Dict{Int, Dict{NTuple{2,Int}, Float64}}}}()
# 16.428 s (604568605 allocations: 12.88 GiB)

# new version: H = Dict{Tuple{UInt, UInt, UInt, Tuple{UInt, UInt}}, Float64}()
# 15.376 s (535684825 allocations: 11.40 GiB)
# 10.489 s (435886655 allocations: 9.23 GiB)


text:
using Symbolics, Test

@sym x y t

@testset "   UpTuple and DownTuple" begin
    # UpTuple
    ut = UpTuple([x, x^2, -x-y])
    @test ut == ut
    @test string(ut) == "up(x\n   x ^ 2\n   (x + y) * -1)"
    @test (ut(t)).data == [x(t), (x^2)(t), (-x-y)(t)]
    @test string(ut(t)) == string(  UpTuple([x(t), (x^2)(t), (-x-y)(t)])  )
    @test up(ut.data) == ut
    @test ut[2] == x^2
    ut[2] = y
    @test y == ut[2]
    @test ut.data == [x, y, -x-y]
    # DownTuple
    dt = DownTuple([x, x^2, -x-y])
    @test dt == dt
    @test (dt(t)).data == [x(t), (x^2)(t), (-x-y)(t)]
    @test down(dt.data) == dt

    P = up([x^2-1, sin(y)])
    Q = up([x^2-1, sin(y)])

    @test P == Q
    @test all(Q .== P)
end


text:
using MultipleScattering
using Base.Test
using Plots

@testset "Summary" begin

    include("shapetests.jl")
    include("particle_tests.jl")
    include("time_simulation_tests.jl")
    include("moments_tests.jl")

    # test our discretised Fourier transforms
    @testset "Fourier tranforms" begin
      include("fourier_test.jl")
      @test fourier_test()
    end

    @testset "Type stability" begin
        # Define everything as a Float32
        volfrac = 0.01f0
        radius = 1.0f0
        k_arr = collect(LinRange(0.01f0,1.0f0,100))
        simulation = FrequencySimulation(volfrac,radius,k_arr)
        @test typeof(simulation.response[1]) == Complex{Float32}
    end

    @testset "Single scatterer" begin
        include("single_scatter.jl")
        # Test against analytic solution
        @test single_scatter_test()
    end

    @testset "Boundary conditions" begin
        include("boundary_conditions.jl")
        # Test boundary conditions for 4 particles with random properties and positions.
        @test boundary_conditions_test()
    end

    @testset "Plot FrequencySimulation" begin
        # Just run it to see if we have any errors (yes thats a very low bar)
        volfrac = 0.01
        radius = 2.0
        k_arr = collect(LinRange(0.2,1.0,5))
        simulation = FrequencySimulation(volfrac,radius,k_arr)

        plot(simulation)
        plot(simulation,0.2)

        @test true
    end

end