text:
import Distributions
const Dists = Distributions
using Distributions: mean
import MeasureTheory
const MT = MeasureTheory
using HypercubeTransform
using Test

@testset "Scalar" begin
    a = Dists.Normal()
    b = Dists.Uniform()
    c = Dists.Gamma()

    ca = ascube(a)
    cb = ascube(b)
    cc = ascube(c)

    @test transform(ca, 0.5) == 0.0
    @test transform(cb, 0.5) == 0.5
    @test transform(cc, 0.5) == Dists.quantile(c, 0.5)

    @inferred Float64 transform(ca, 0.5)
end

@testset "ArrayHC" begin
    d = Dists.Product([Dists.Normal(), Dists.Uniform(), Dists.Normal()])
    m = MT.For(d.v) do p
            return p
    end
    cd = ascube(d)
    cm = ascube(m)
    @test transform(cd, [0.5,0.5,0.5]) == transform(cm, [0.5,0.5,0.5])
    cc = ascube.(d.v)
    @test transform.(cc, [0.5,0.5,0.5]) == transform(cd, [0.5,0.5,0.5])
    @inferred Vector{Float64} transform(cm, [0.5, 0.5, 0.5])
end

@testset "Dirichlet" begin
    d = Dists.Dirichlet([1.0,1.0,1.0])
    a = Dists.Normal()
    dc = ascube(d)
    @inferred transform(dc, [0.5, 0.5, 0.5])
    p = rand(3, 10_000_00)
    x = transform.(Ref(dc), eachcol(p))
    @test isapprox(mean(x), mean(d), atol=1e-3)

    dad = ascube((a, d))
    @inferred transform(dad, [0.5,0.5,0.5,0.5])
    pad = rand(4, 10_000_00)
    xad = transform.(Ref(dad), eachcol(pad))
    mN = mean(first.(xad))
    mD = mean(last.(xad))
    @test isapprox(mN, mean(a), atol=1e-3)
    @test isapprox(mD, mean(d), atol=1e-3)

end


text:
# This file is auto-generated by AWSMetadata.jl
using AWS
using AWS.AWSServices: signer
using AWS.Compat
using AWS.UUIDs

"""
    add_profile_permission(action, principal, profile_name, statement_id)
    add_profile_permission(action, principal, profile_name, statement_id, params::Dict{String,<:Any})

Adds cross-account permissions to a signing profile.

# Arguments
- `action`: The AWS Signer action permitted as part of cross-account permissions.
- `principal`: The AWS principal receiving cross-account permissions. This may be an IAM
  role or another AWS account ID.
- `profile_name`: The human-readable name of the signing profile.
- `statement_id`: A unique identifier for the cross-account permission statement.

# Optional Parameters
Optional parameters can be passed as a `params::Dict{String,<:Any}`. Valid keys are:
- `"profileVersion"`: The version of the signing profile.
- `"revisionId"`: A unique identifier for the current profile revision.
"""
add_profile_permission(action, principal, profileName, statementId; aws_config::AbstractAWSConfig=global_aws_config()) = signer("POST", "/signing-profiles/$(profileName)/permissions", Dict{String, Any}("action"=>action, "principal"=>principal, "statementId"=>statementId); aws_config=aws_config)
add_profile_permission(action, principal, profileName, statementId, params::AbstractDict{String, <:Any}; aws_config::AbstractAWSConfig=global_aws_config()) = signer("POST", "/signing-profiles/$(profileName)/permissions", Dict{String, Any}(mergewith(_merge, Dict{String, Any}("action"=>action, "principal"=>principal, "statementId"=>statementId), params)); aws_config=aws_config)

"""
    cancel_signing_profile(profile_name)
    cancel_signing_profile(profile_name, params::Dict{String,<:Any})

Changes the state of an ACTIVE signing profile to CANCELED. A canceled profile is still
viewable with the ListSigningProfiles operation, but it cannot perform new signing jobs,
and is deleted two years after cancelation.

# Arguments
- `profile_name`: The name of the signing profile to be canceled.

"""
cancel_signing_profile(profileName; aws_config::AbstractAWSConfig=global_aws_config()) = signer("DELETE", "/signing-profiles/$(profileName)"; aws_config=aws_config)
cancel_signing_profile(profileName, params::AbstractDict{String, <:Any}; aws_config::AbstractAWSConfig=global_aws_config()) = signer("DELETE", "/signing-profiles/$(profileName)", params; aws_config=aws_config)

"""
    describe_signing_job(job_id)
    describe_signing_job(job_id, params::Dict{String,<:Any})

Returns information about a specific code signing job. You specify the job by using the
jobId value that is returned by the StartSigningJob operation.

# Arguments
- `job_id`: The ID of the signing job on input.

"""
describe_signing_job(jobId; aws_config::AbstractAWSConfig=global_aws_config()) = signer("GET", "/signing-jobs/$(jobId)"; aws_config=aws_config)
describe_signing_job(jobId, params::AbstractDict{String, <:Any}; aws_config::AbstractAWSConfig=global_aws_config()) = signer("GET", "/signing-jobs/$(jobId)", params; aws_config=aws_config)

"""
    get_signing_platform(platform_id)
    get_signing_platform(platform_id, params::Dict{String,<:Any})

Returns information on a specific signing platform.

# Arguments
- `platform_id`: The ID of the target signing platform.

"""
get_signing_platform(platformId; aws_config::AbstractAWSConfig=global_aws_config()) = signer("GET", "/signing-platforms/$(platformId)"; aws_config=aws_config)
get_signing_platform(platformId, params::AbstractDict{String, <:Any}; aws_config::AbstractAWSConfig=global_aws_config()) = signer("GET", "/signing-platforms/$(platformId)", params; aws_config=aws_config)

"""
    get_signing_profile(profile_name)
    get_signing_profile(profile_name, params::Dict{String,<:Any})

Returns information on a specific signing profile.

# Arguments
- `profile_name`: The name of the target signing profile.

# Optional Parameters
Optional parameters can be passed as a `params::Dict{String,<:Any}`. Valid keys are:
- `"profileOwner"`: The AWS account ID of the profile owner.
"""
get_signing_profile(profileName; aws_config::AbstractAWSConfig=global_aws_config()) = signer("GET", "/signing-profiles/$(profileName)"; aws_config=aws_config)
get_signing_profile(profileName, params::AbstractDict{String, <:Any}; aws_config::AbstractAWSConfig=global_aws_config()) = signer("GET", "/signing-profiles/$(profileName)", params; aws_config=aws_config)

"""
    list_profile_permissions(profile_name)
    list_profile_permissions(profile_name, params::Dict{String,<:Any})

Lists the cross-account permissions associated with a signing profile.

# Arguments
- `profile_name`: Name of the signing profile containing the cross-account permissions.

# Optional Parameters
Optional parameters can be passed as a `params::Dict{String,<:Any}`. Valid keys are:
- `"nextToken"`: String for specifying the next set of paginated results.
"""
list_profile_permissions(profileName; aws_config::AbstractAWSConfig=global_aws_config()) = signer("GET", "/signing-profiles/$(profileName)/permissions"; aws_config=aws_config)
list_profile_permissions(profileName, params::AbstractDict{String, <:Any}; aws_config::AbstractAWSConfig=global_aws_config()) = signer("GET", "/signing-profiles/$(profileName)/permissions", params; aws_config=aws_config)

"""
    list_signing_jobs()
    list_signing_jobs(params::Dict{String,<:Any})

Lists all your signing jobs. You can use the maxResults parameter to limit the number of
signing jobs that are returned in the response. If additional jobs remain to be listed,
code signing returns a nextToken value. Use this value in subsequent calls to
ListSigningJobs to fetch the remaining values. You can continue calling ListSigningJobs
with your maxResults parameter and with new values that code signing returns in the
nextToken parameter until all of your signing jobs have been returned.

# Optional Parameters
Optional parameters can be passed as a `params::Dict{String,<:Any}`. Valid keys are:
- `"isRevoked"`: Filters results to return only signing jobs with revoked signatures.
- `"jobInvoker"`: Filters results to return only signing jobs initiated by a specified IAM
  entity.
- `"maxResults"`: Specifies the maximum number of items to return in the response. Use this
  parameter when paginating results. If additional items exist beyond the number you specify,
  the nextToken element is set in the response. Use the nextToken value in a subsequent
  request to retrieve additional items.
- `"nextToken"`: String for specifying the next set of paginated results to return. After
  you receive a response with truncated results, use this parameter in a subsequent request.
  Set it to the value of nextToken from the response that you just received.
- `"platformId"`: The ID of microcontroller platform that you specified for the
  distribution of your code image.
- `"requestedBy"`: The IAM principal that requested the signing job.
- `"signatureExpiresAfter"`: Filters results to return only signing jobs with signatures
  expiring after a specified timestamp.
- `"signatureExpiresBefore"`: Filters results to return only signing jobs with signatures
  expiring before a specified timestamp.
- `"status"`: A status value with which to filter your results.
"""
list_signing_jobs(; aws_config::AbstractAWSConfig=global_aws_config()) = signer("GET", "/signing-jobs"; aws_config=aws_config)
list_signing_jobs(params::AbstractDict{String, Any}; aws_config::AbstractAWSConfig=global_aws_config()) = signer("GET", "/signing-jobs", params; aws_config=aws_config)

"""
    list_signing_platforms()
    list_signing_platforms(params::Dict{String,<:Any})

Lists all signing platforms available in code signing that match the request parameters. If
additional jobs remain to be listed, code signing returns a nextToken value. Use this value
in subsequent calls to ListSigningJobs to fetch the remaining values. You can continue
calling ListSigningJobs with your maxResults parameter and with new values that code
signing returns in the nextToken parameter until all of your signing jobs have been
returned.

# Optional Parameters
Optional parameters can be passed as a `params::Dict{String,<:Any}`. Valid keys are:
- `"category"`: The category type of a signing platform.
- `"maxResults"`: The maximum number of results to be returned by this operation.
- `"nextToken"`: Value for specifying the next set of paginated results to return. After
  you receive a response with truncated results, use this parameter in a subsequent request.
  Set it to the value of nextToken from the response that you just received.
- `"partner"`: Any partner entities connected to a signing platform.
- `"target"`: The validation template that is used by the target signing platform.
"""
list_signing_platforms(; aws_config::AbstractAWSConfig=global_aws_config()) = signer("GET", "/signing-platforms"; aws_config=aws_config)
list_signing_platforms(params::AbstractDict{String, Any}; aws_config::AbstractAWSConfig=global_aws_config()) = signer("GET", "/signing-platforms", params; aws_config=aws_config)

"""
    list_signing_profiles()
    list_signing_profiles(params::Dict{String,<:Any})

Lists all available signing profiles in your AWS account. Returns only profiles with an
ACTIVE status unless the includeCanceled request field is set to true. If additional jobs
remain to be listed, code signing returns a nextToken value. Use this value in subsequent
calls to ListSigningJobs to fetch the remaining values. You can continue calling
ListSigningJobs with your maxResults parameter and with new values that code signing
returns in the nextToken parameter until all of your signing jobs have been returned.

# Optional Parameters
Optional parameters can be passed as a `params::Dict{String,<:Any}`. Valid keys are:
- `"includeCanceled"`: Designates whether to include profiles with the status of CANCELED.
- `"maxResults"`: The maximum number of profiles to be returned.
- `"nextToken"`: Value for specifying the next set of paginated results to return. After
  you receive a response with truncated results, use this parameter in a subsequent request.
  Set it to the value of nextToken from the response that you just received.
- `"platformId"`: Filters results to return only signing jobs initiated for a specified
  signing platform.
- `"statuses"`: Filters results to return only signing jobs with statuses in the specified
  list.
"""
list_signing_profiles(; aws_config::AbstractAWSConfig=global_aws_config()) = signer("GET", "/signing-profiles"; aws_config=aws_config)
list_signing_profiles(params::AbstractDict{String, Any}; aws_config::AbstractAWSConfig=global_aws_config()) = signer("GET", "/signing-profiles", params; aws_config=aws_config)

"""
    list_tags_for_resource(resource_arn)
    list_tags_for_resource(resource_arn, params::Dict{String,<:Any})

Returns a list of the tags associated with a signing profile resource.

# Arguments
- `resource_arn`: The Amazon Resource Name (ARN) for the signing profile.

"""
list_tags_for_resource(resourceArn; aws_config::AbstractAWSConfig=global_aws_config()) = signer("GET", "/tags/$(resourceArn)"; aws_config=aws_config)
list_tags_for_resource(resourceArn, params::AbstractDict{String, <:Any}; aws_config::AbstractAWSConfig=global_aws_config()) = signer("GET", "/tags/$(resourceArn)", params; aws_config=aws_config)

"""
    put_signing_profile(platform_id, profile_name)
    put_signing_profile(platform_id, profile_name, params::Dict{String,<:Any})

Creates a signing profile. A signing profile is a code signing template that can be used to
carry out a pre-defined signing job. For more information, see
http://docs.aws.amazon.com/signer/latest/developerguide/gs-profile.html

# Arguments
- `platform_id`: The ID of the signing platform to be created.
- `profile_name`: The name of the signing profile to be created.

# Optional Parameters
Optional parameters can be passed as a `params::Dict{String,<:Any}`. Valid keys are:
- `"overrides"`: A subfield of platform. This specifies any different configuration options
  that you want to apply to the chosen platform (such as a different hash-algorithm or
  signing-algorithm).
- `"signatureValidityPeriod"`: The default validity period override for any signature
  generated using this signing profile. If unspecified, the default is 135 months.
- `"signingMaterial"`: The AWS Certificate Manager certificate that will be used to sign
  code with the new signing profile.
- `"signingParameters"`: Map of key-value pairs for signing. These can include any
  information that you want to use during signing.
- `"tags"`: Tags to be associated with the signing profile that is being created.
"""
put_signing_profile(platformId, profileName; aws_config::AbstractAWSConfig=global_aws_config()) = signer("PUT", "/signing-profiles/$(profileName)", Dict{String, Any}("platformId"=>platformId); aws_config=aws_config)
put_signing_profile(platformId, profileName, params::AbstractDict{String, <:Any}; aws_config::AbstractAWSConfig=global_aws_config()) = signer("PUT", "/signing-profiles/$(profileName)", Dict{String, Any}(mergewith(_merge, Dict{String, Any}("platformId"=>platformId), params)); aws_config=aws_config)

"""
    remove_profile_permission(profile_name, revision_id, statement_id)
    remove_profile_permission(profile_name, revision_id, statement_id, params::Dict{String,<:Any})

Removes cross-account permissions from a signing profile.

# Arguments
- `profile_name`: A human-readable name for the signing profile with permissions to be
  removed.
- `revision_id`: An identifier for the current revision of the signing profile permissions.
- `statement_id`: A unique identifier for the cross-account permissions statement.

"""
remove_profile_permission(profileName, revisionId, statementId; aws_config::AbstractAWSConfig=global_aws_config()) = signer("DELETE", "/signing-profiles/$(profileName)/permissions/$(statementId)", Dict{String, Any}("revisionId"=>revisionId); aws_config=aws_config)
remove_profile_permission(profileName, revisionId, statementId, params::AbstractDict{String, <:Any}; aws_config::AbstractAWSConfig=global_aws_config()) = signer("DELETE", "/signing-profiles/$(profileName)/permissions/$(statementId)", Dict{String, Any}(mergewith(_merge, Dict{String, Any}("revisionId"=>revisionId), params)); aws_config=aws_config)

"""
    revoke_signature(job_id, reason)
    revoke_signature(job_id, reason, params::Dict{String,<:Any})

Changes the state of a signing job to REVOKED. This indicates that the signature is no
longer valid.

# Arguments
- `job_id`: ID of the signing job to be revoked.
- `reason`: The reason for revoking the signing job.

# Optional Parameters
Optional parameters can be passed as a `params::Dict{String,<:Any}`. Valid keys are:
- `"jobOwner"`: AWS account ID of the job owner.
"""
revoke_signature(jobId, reason; aws_config::AbstractAWSConfig=global_aws_config()) = signer("PUT", "/signing-jobs/$(jobId)/revoke", Dict{String, Any}("reason"=>reason); aws_config=aws_config)
revoke_signature(jobId, reason, params::AbstractDict{String, <:Any}; aws_config::AbstractAWSConfig=global_aws_config()) = signer("PUT", "/signing-jobs/$(jobId)/revoke", Dict{String, Any}(mergewith(_merge, Dict{String, Any}("reason"=>reason), params)); aws_config=aws_config)

"""
    revoke_signing_profile(effective_time, profile_name, profile_version, reason)
    revoke_signing_profile(effective_time, profile_name, profile_version, reason, params::Dict{String,<:Any})

Changes the state of a signing profile to REVOKED. This indicates that signatures generated
using the signing profile after an effective start date are no longer valid.

# Arguments
- `effective_time`: A timestamp for when revocation of a Signing Profile should become
  effective. Signatures generated using the signing profile after this timestamp are not
  trusted.
- `profile_name`: The name of the signing profile to be revoked.
- `profile_version`: The version of the signing profile to be revoked.
- `reason`: The reason for revoking a signing profile.

"""
revoke_signing_profile(effectiveTime, profileName, profileVersion, reason; aws_config::AbstractAWSConfig=global_aws_config()) = signer("PUT", "/signing-profiles/$(profileName)/revoke", Dict{String, Any}("effectiveTime"=>effectiveTime, "profileVersion"=>profileVersion, "reason"=>reason); aws_config=aws_config)
revoke_signing_profile(effectiveTime, profileName, profileVersion, reason, params::AbstractDict{String, <:Any}; aws_config::AbstractAWSConfig=global_aws_config()) = signer("PUT", "/signing-profiles/$(profileName)/revoke", Dict{String, Any}(mergewith(_merge, Dict{String, Any}("effectiveTime"=>effectiveTime, "profileVersion"=>profileVersion, "reason"=>reason), params)); aws_config=aws_config)

"""
    start_signing_job(client_request_token, destination, profile_name, source)
    start_signing_job(client_request_token, destination, profile_name, source, params::Dict{String,<:Any})

Initiates a signing job to be performed on the code provided. Signing jobs are viewable by
the ListSigningJobs operation for two years after they are performed. Note the following
requirements:     You must create an Amazon S3 source bucket. For more information, see
Create a Bucket in the Amazon S3 Getting Started Guide.    Your S3 source bucket must be
version enabled.   You must create an S3 destination bucket. Code signing uses your S3
destination bucket to write your signed code.   You specify the name of the source and
destination buckets when calling the StartSigningJob operation.   You must also specify a
request token that identifies your request to code signing.   You can call the
DescribeSigningJob and the ListSigningJobs actions after you call StartSigningJob. For a
Java example that shows how to use this action, see
http://docs.aws.amazon.com/acm/latest/userguide/

# Arguments
- `client_request_token`: String that identifies the signing request. All calls after the
  first that use this token return the same response as the first call.
- `destination`: The S3 bucket in which to save your signed object. The destination
  contains the name of your bucket and an optional prefix.
- `profile_name`: The name of the signing profile.
- `source`: The S3 bucket that contains the object to sign or a BLOB that contains your raw
  code.

# Optional Parameters
Optional parameters can be passed as a `params::Dict{String,<:Any}`. Valid keys are:
- `"profileOwner"`: The AWS account ID of the signing profile owner.
"""
start_signing_job(clientRequestToken, destination, profileName, source; aws_config::AbstractAWSConfig=global_aws_config()) = signer("POST", "/signing-jobs", Dict{String, Any}("clientRequestToken"=>clientRequestToken, "destination"=>destination, "profileName"=>profileName, "source"=>source); aws_config=aws_config)
start_signing_job(clientRequestToken, destination, profileName, source, params::AbstractDict{String, <:Any}; aws_config::AbstractAWSConfig=global_aws_config()) = signer("POST", "/signing-jobs", Dict{String, Any}(mergewith(_merge, Dict{String, Any}("clientRequestToken"=>clientRequestToken, "destination"=>destination, "profileName"=>profileName, "source"=>source), params)); aws_config=aws_config)

"""
    tag_resource(resource_arn, tags)
    tag_resource(resource_arn, tags, params::Dict{String,<:Any})

Adds one or more tags to a signing profile. Tags are labels that you can use to identify
and organize your AWS resources. Each tag consists of a key and an optional value. To
specify the signing profile, use its Amazon Resource Name (ARN). To specify the tag, use a
key-value pair.

# Arguments
- `resource_arn`: The Amazon Resource Name (ARN) for the signing profile.
- `tags`: One or more tags to be associated with the signing profile.

"""
tag_resource(resourceArn, tags; aws_config::AbstractAWSConfig=global_aws_config()) = signer("POST", "/tags/$(resourceArn)", Dict{String, Any}("tags"=>tags); aws_config=aws_config)
tag_resource(resourceArn, tags, params::AbstractDict{String, <:Any}; aws_config::AbstractAWSConfig=global_aws_config()) = signer("POST", "/tags/$(resourceArn)", Dict{String, Any}(mergewith(_merge, Dict{String, Any}("tags"=>tags), params)); aws_config=aws_config)

"""
    untag_resource(resource_arn, tag_keys)
    untag_resource(resource_arn, tag_keys, params::Dict{String,<:Any})

Removes one or more tags from a signing profile. To remove the tags, specify a list of tag
keys.

# Arguments
- `resource_arn`: The Amazon Resource Name (ARN) for the signing profile.
- `tag_keys`: A list of tag keys to be removed from the signing profile.

"""
untag_resource(resourceArn, tagKeys; aws_config::AbstractAWSConfig=global_aws_config()) = signer("DELETE", "/tags/$(resourceArn)", Dict{String, Any}("tagKeys"=>tagKeys); aws_config=aws_config)
untag_resource(resourceArn, tagKeys, params::AbstractDict{String, <:Any}; aws_config::AbstractAWSConfig=global_aws_config()) = signer("DELETE", "/tags/$(resourceArn)", Dict{String, Any}(mergewith(_merge, Dict{String, Any}("tagKeys"=>tagKeys), params)); aws_config=aws_config)

ext: jl


text:
@testset "integrators" begin

    for integrator in keys(AgentBasedModels.addIntegrator_!)
        for platform in testplatforms

            #Declare
            @test_nowarn begin
                m = @agent(
                    3,

                    Equation =
                    begin
                        d(x) = x*dt
                        d(y) = 0*dt
                        d(z) = 1*dt
                    end
                )
                m = compile(m, integrator=integrator, platform=platform, debug=false)
            end

            #ODE
            m = @agent(
                3,

                Equation =
                begin
                    d(x) = -x*dt
                    d(z) = 1*dt
                end
            )
            m = compile(m, integrator=integrator, platform=platform, debug=false)
            #println(m.program)
            com = Community(m,N = 10)
            com.x .= 1.
            com.y .= 2.
            com.z .= 0.
            comt = m.evolve(com,dt=0.01,tMax=5)

            @test all(abs.(comt.x[1:end,1] .- exp.(-comt.t)) .< 0.02)
            @test all(comt.y[1:end,1] .≈ 2 )
            @test all(abs.(comt.z[1:end,1] .- comt.t) .< 0.005)

            #SDE
            m = @agent(
                3,

                Equation =
                begin
                    d(x) = -0*dt + dW
                    d(y) = -0*dt + dW
                end
            )
            m = compile(m, integrator=integrator, platform=platform, debug=false)
            #println(m.program)
            com = Community(m,N = 5000)
            com.x .= 0.
            com.y .= 0.
            comt = m.evolve(com,dt=0.01,tMax=5)

            @test begin
                v = [var(comt.x[i,:]) for i in 1:size(comt.x)[1]]
                all(abs.(v .- comt.t) .< 0.8)
            end
            @test comt.x[:,1] != comt.y[:,1]

        end
    end

end


text:
# include("simulate.jl")

using Ipopt
using GenericLinearAlgebra

## Ipopt directly
function trim_kron(j::Vector{Joint},GravityInInertial::Vector{Float64};
  ix::Vector{Integer}=zeros(Integer,20*(nB-1)+1),
  iy::Vector{Integer}=zeros(Integer,7*(nB-1)))

  nB = length(j) + 1; # number of bodies in the joint tree
  # ix: 0 indicates that the corresponding variable is free during optimization

  # iy: 0 indicates that the corresponding second derivative term needs to be zero
  # (specify indices for iy starting from second body)
  # iy = [1;4;5] means that the ẍ. β̈1 and β̈2 are free for j[1].RB2

  # Find which variables are free for optimization
  freeVarInd = findall(x->x==0,ix); # indices of free variables

  # find which acceleration-level terms are desired to be 0/constrained
  freeAccInd = findall(x->x==1,iy) # indices of free accelerations

  # Initial guesses
  # Initial guess is provided initial location
  x0 = []; # Initial guess
  for k = 1:length(j)
    append!(x0,j[k].RB2.x)
  end
  extFListCopy = externalFTotal(0.0,j);
  u0 = genU(extFListCopy); # Initial guess for control input

  n_xu = length(freeVarInd)-1; # Number of optimization variables
  lb = [-Inf*ones(n_xu);0.0]; ub = [Inf*ones(n_xu);Inf] # Bounds on optimization variables
  # lb[4] = 0.0;
  # lb[14+4] = 0.0;

  m = 1; # Number of nonlinear constraints (xdot = f(x,u)) for second order variables
  lb_g = [-Inf]; ub_g = [0.0];

  # extract states and control from optimization variables
  function getXU(z::AbstractArray{T}) where T<:Real
    z0 = [x0;u0[:,2:end][:];0.0];
    z0[freeVarInd] .= 0;

    Mz = zeros(length(z0), length(freeVarInd))
    for (i,ind) in enumerate(freeVarInd)
      Mz[ind,i] = 1;
    end

    zFull = z0 + Mz*z;

    X = zFull[1:14*(nB-1)];
    U = zFull[14*(nB-1)+1:end-1]
    return X,U
  end

  ## Constraint functions
  function qddFn(xuVar)
    x = xuVar[1:14*nB]; u = reshape(xuVar[14*nB+1:end],(6,nB));
    xdot = fxdot(x,u,j,g);
    y = Vector(undef,7*nB); # qdd
    for i=1:nB
      y[7*(i-1)+1:7*(i)] = xdot[7*(2*i-1)+1:14*i]
    end
    deleteat!(y,1:7); # inertial frame has no acceleration
    deleteat!(y,freeAccInd); # free acceleration variables
    return y
  end

  function quatNormConstr(xuVar)
    x = xuVar[1:14*nB]; u = reshape(xuVar[14*nB+1:end],(6,nB));
    q_constr = Vector(undef, nB)
    for i=1:nB
        q_constr[i] = norm(x[14*(i-1)+4:14*(i-1)+7])^2 - 1;
    end
    return q_constr
  end

  function jointLocConstr(xuVar)
    x = xuVar[1:14*nB]; u = reshape(xuVar[14*nB+1:end],(6,nB));
    jLoc_constr = Matrix(undef,3,length(j))
    for i=1:length(j)
        b1_id = j[i].RB1.bodyID; b2_id = j[i].RB2.bodyID;
        x1 = x[(b1_id-1)*14+1:b1_id*14]
        x2 = x[(b2_id-1)*14+1:b2_id*14]
        j_b1 = x1[1:3] + transpose(quat2dcm(x1[4:7]))*j[i].pos1;
        j_b2 = x2[1:3] + transpose(quat2dcm(x2[4:7]))*j[i].pos2;
        jLoc_constr[:,i] = j_b1 - j_b2;
    end
    return jLoc_constr
  end

  function _nonlinear_constraints(z::AbstractArray{T}) where T<:Real
    xVar, uVar = getXU(z)
    x = [Vector{T}(j[1].RB1.x); xVar]
    u = [zeros(T,6) reshape(uVar,(6, nB-1))]
    xuVar = [x;u[:]]; # Vector of x,u

    # Dynamics constraint
    y = qddFn(xuVar);
    # Quaternion norm constraint
    q_constr = quatNormConstr(xuVar);
    # JointLocation constraint  (translation constraint)
    jLoc_constr = jointLocConstr(xuVar);

    constr = [y;q_constr;jLoc_constr[:]];

    J = sum(constr.*constr) - z[end];
    return J
  end

  ## Helper functions for derivatives
  function kronSum(A,B)
    # kronecker sum of 2 arrays
    out = kron(A, Matrix(I,(size(B)))) + kron(Matrix(I,(size(A))),B)
    return out
  end

  function createCommMat(M)
    # commutation matrix
    r,m = size(M);
    K = zeros(m*r,m*r);
    for i=1:r
        for j=1:m
            ei = 1.0I(r)[:,i] # ith-canonical unit vector of dimension r
            ej = 1.0I(m)[:,j] # jth-canonical unit vector of dimension m
            K += kron(ei*permutedims(ej), ej*permutedims(ei))
        end
    end
    return K
  end

  function fdJ_kronAB(A,B, dA, dB)
      n,q = size(A); p,r = size(B);
      Iq = 1.0I(q); Ip = 1.0I(p); In = 1.0I(n); Ir = 1.0I(r);
      Inq = Matrix{Float64}(I,n,q); Ipr = Matrix{Float64}(I,p,r)
      Krn = createCommMat(rand(r,n));
      Ay1 = (kron(Krn,Ip)); Ay2 = (kron(In,vec(B)));
      Ay = kron(Iq, Ay1*Ay2);
      Bx = kron((kron(Iq,Krn)*kron(vec(A),Ir)), Ip);
      out = Ay*dA + Bx*dB
      return out
  end

  function grad_hess_nonlinear_constraints(z, flag)
    ## First derivatives
    xVar, uVar = getXU(z)
    xv = [j[1].RB1.x; xVar]
    uv = [zeros(6) reshape(uVar,(6, nB-1))];
    xu_var = [xv;uv[:]];

    function Mdiff1(x)
      # First derivatives of mass matrix and powers (1/2, -1/2, -1)
      M = assembleM(x,j)
      Ms = real(sqrt(M))
      M_inv = inv(M)
      Ms_inv = real(sqrt(M_inv))

      dM = ForwardDiff.jacobian(z -> assembleM(z,j),x)
      dM_inv = -(kron(permutedims(M_inv),M_inv))*dM;
      dMs = inv(kronSum(permutedims(Ms), Ms))*dM
      dMs_inv = -kron(permutedims(Ms_inv),Ms_inv)*(dMs);
      return M, Ms, Ms_inv, dM, dMs, dMs_inv, M_inv, dM_inv
    end
    M, Ms, Ms_inv, dM, dMs, dMs_inv, M_inv, dM_inv = Mdiff1(xv);

    function Adiff1(x)
      A, _ = Ab_VecOfMat(x,j);
      dA = ForwardDiff.jacobian(z->Ab_VecOfMat(z,j)[1],x);
      return A, dA
    end
    A, dA = Adiff1(xv);

    function bdiff1(x)
      _, b = Ab_VecOfMat(x,j);
      db = ForwardDiff.jacobian(z->Ab_VecOfMat(z,j)[2],x);
      return b, db
    end
    b, db = bdiff1(xv);

    function Gpdiff1(x)
      function t1diff1(x)
        # t1 = A'*(AM)^{-1}*A';
        sz_A1 = size(A,1)
        t1 = permutedims(A)*inv(A*inv(M)*permutedims(A))

        dA_tr = createCommMat(A)*dA;

        Y = A*M_inv*permutedims(A); invY = inv(Y);
        dY1 = kron(A*M_inv,1.0I(sz_A1))*dA;
        dY2 = kron(A,A)*dM_inv;
        dY3 = kron(1.0I(sz_A1), A*M_inv)*dA_tr;
        dY = dY1 + dY2 + dY3;
        dY_inv = -kron(permutedims(invY),invY)*dY;

        dt1_1 = kron(permutedims(invY),1.0I(size(A,2)))*dA_tr;
        dt1_2 = kron(1.0I(sz_A1),permutedims(A))*dY_inv;
        dt1 = dt1_1 + dt1_2;
        return t1, dt1
      end
      G = A*Ms_inv;
      Gp = permutedims(G)*((G*permutedims(G))\I(size(G,1)))

      t1, dt1 = t1diff1(x)
      dGp1 = kron(permutedims(t1),1.0I(size(M,1)))*dMs_inv

      sz_t2 =size(t1,2)
      t2 = kron(1.0I(sz_t2), Ms_inv)
      dGp2 = t2*dt1;

      dGp = dGp1 + dGp2
      return Gp, dGp
    end
    Gp, dGp = Gpdiff1(xv);

    function Fudiff1(x,u)
      Fu = assembleF(x,u,j,g);
      dFu = ForwardDiff.jacobian(y->assembleF(y,u,j,g),x);
      dFu_u = ForwardDiff.jacobian(y->assembleF(x,y,j,g),u);
      return Fu, dFu, dFu_u
    end
    Fu, dFu, dFu_u = Fudiff1(xv,uv);

    function hFn(x,u)
      A,b = Ab_VecOfMat(x,j);
      Fu = assembleF(x,u,j,g);
      M = assembleM(x,j);
      h = b - A*inv(M)*Fu;
      return h
    end

    function hdiff1(x,u)
      # has derivatives wrt both x,u
      Fu, dFu, dFu_u = Fudiff1(x,u);
      A, dA = Adiff1(x);
      b, db = bdiff1(x);
      M, Ms, Ms_inv, dM, dMs, dMs_inv, M_inv, dM_inv = Mdiff1(x);

      h = b - A*inv(M)*Fu;

      # dh
      term1 = kron(permutedims(M_inv*Fu), 1.0I(size(A,1)))*dA;
      term2 = kron(permutedims(Fu), A)*dM_inv;
      term3 = A*M_inv*dFu;
      dh_x = db - (term1 + term2 + term3);

      # dh_u
      dh_u = -A*M_inv*dFu_u;

      return h, dh_x, dh_u
    end
    h, dh_x, dh_u = hdiff1(xv,uv);

    function Fcdiff1(x,u)
      Fc = Ms*Gp*h

      dFc1 = kron(permutedims(Gp*h),I(size(M,1)))*dMs
      dFc2 = kron(permutedims(h),Ms)*dGp
      dFc3 = Ms*Gp*dh_x;

      dFc = dFc1 + dFc2 + dFc3;

      dFc_u = Ms*Gp*dh_u;
      return Fc, dFc, dFc_u
    end
    Fc, dFc, dFc_u = Fcdiff1(xv,uv);

    function accdiff1(x,u)
      # derivative of qdd w.r.t x
      dacc1 = kron(transpose(Fc+Fu),1.0I(size(M_inv,1)))*dM_inv
      dacc2 = M_inv*(dFc + dFu)
      dqdd_x = dacc1 + dacc2

      return dqdd_x
    end
    dqdd_x = accdiff1(xv,uv);
    ## Constraint derivative functions for gradient

    function dqddFn(xuVar)
      dqdd_u = M_inv*(dFc_u + dFu_u);
      out = [dqdd_x dqdd_u];
      return out
    end
    qdd = qddFn(xu_var);
    dqdd = dqddFn(xu_var);

    function dynConstr_jac(xuVar)
      out = (2*permutedims(qdd)*dqdd)[:];
      return out
    end

    function quatNormConstr_diff(xuVar)
      f(y) = sum(quatNormConstr(y).*quatNormConstr(y));
      df(x) = ForwardDiff.gradient(y->f(y), x);
      grad_f = df(xuVar);
      d2f = ForwardDiff.jacobian(y->df(y), xuVar);
      return grad_f, d2f
    end

    function jointLocConstr_diff(xuVar)
      f(y) = sum((jointLocConstr(y)[:]).*(jointLocConstr(y)[:]));
      df(x) = ForwardDiff.gradient(y->f(y), x);
      grad_f = df(xuVar);
      d2f = ForwardDiff.jacobian(y->df(y), xuVar);
      return grad_f, d2f
    end

    function grad_nonlinear_constraints()
      dqdd_constr = dynConstr_jac(xu_var); # Dyn
      dquatNorm_constr,_ = quatNormConstr_diff(xu_var); # Quat
      djointLoc_constr, _ = jointLocConstr_diff(xu_var); # JointLoc

      dconstr_z = dqdd_constr + dquatNorm_constr + djointLoc_constr;

      # take care of free optimization variables
      freeVarInd_grad = 14 .+ freeVarInd[1:end-1];
      grad_constr_freeInd = dconstr_z[freeVarInd_grad];
      out = [grad_constr_freeInd;-1.0];
      return out
    end

    if flag == 1
      ## First derivatives required anyway
      out = grad_nonlinear_constraints();
    else
      ##  Derivatives for hessian
      function Mdiff2(x)
        M, Ms, Ms_inv, dM, dMs, dMs_inv, M_inv, dM_inv = Mdiff1(x);

        # dM_xx
        dM_fn(y) = ForwardDiff.jacobian(z->assembleM(z,j),y);
        dM_xx = ForwardDiff.jacobian(z->dM_fn(z),x);

        # dMs_xx
        function z1Fn(x)
          z1= kronSum(permutedims(Ms), Ms);
          z1_inv = inv(z1);
          dMs_t = createCommMat(Ms)*dMs;
          dz1_1 = fdJ_kronAB(permutedims(Ms),1.0I(size(Ms,1)),dMs_t,zeros(size(dMs_t)));
          dz1_2 = fdJ_kronAB(1.0I(size(Ms,2)),Ms,zeros(size(dMs)),dMs);
          dz1 = dz1_1 + dz1_2;
          dz1_inv = -(kron(permutedims(z1_inv),z1_inv))*dz1;
          return z1_inv, dz1_inv
        end
        z1_inv, dz1_inv = z1Fn(x);
        out1 = kron(permutedims(dM), 1.0I(size(z1_inv,1)))*dz1_inv;
        out2 = kron(1.0I(size(dM,2)), z1_inv)*dM_xx
        dMs_xx = out1 + out2;

        # dMsInv_xx
        R = kron(permutedims(Ms_inv), Ms_inv);
        dMs_inv_t = createCommMat(Ms_inv)*dMs_inv;
        dR = fdJ_kronAB(permutedims(Ms_inv), Ms_inv, dMs_inv_t, dMs_inv);

        out1 = kron(permutedims(dMs),1.0I(size(R,1)))*dR;
        out2 = kron(1.0I(size(dMs,2)),R)*dMs_xx;
        dMsInv_xx = -(out1 + out2);

        # dMinv_xx
        Q = kron(permutedims(M_inv), M_inv);
        dM_inv_t = createCommMat(M_inv)*dM_inv;
        dQ = fdJ_kronAB(permutedims(M_inv), M_inv, dM_inv_t, dM_inv);
        out1 = kron(permutedims(dM), 1.0I(size(Q,1)))*dQ;
        out2 = kron(1.0I(size(dM,2)), Q)*dM_xx;
        dMinv_xx = -(out1 + out2);

        return dM_xx, dMs_xx, dMsInv_xx, dMinv_xx
      end
      dM_xx, dMs_xx, dMsInv_xx, dMinv_xx = Mdiff2(xv);

      function Adiff2(x)
        dAFn(y) = Adiff1(y)[2];
        cfg1 = ForwardDiff.JacobianConfig(dAFn, x, ForwardDiff.Chunk{1}());
        dA_xx = ForwardDiff.jacobian(dAFn,x, cfg1);
        return dA_xx;
      end
      dA_xx = Adiff2(xv);
      A, dA = Adiff1(xv);

      function bdiff2(x)
        dbFn(y) = bdiff1(y)[2];
        cfg1 = ForwardDiff.JacobianConfig(dbFn, x, ForwardDiff.Chunk{1}());
        db_xx = ForwardDiff.jacobian(dbFn,x, cfg1);
        return db_xx;
      end
      db_xx = bdiff2(xv);
      b, db = bdiff1(xv);

      function Gdiff2(x,u)
        G = A*Ms_inv;
        #dG
        dG1 = kron(permutedims(Ms_inv),1.0I(size(A,1)))*dA;
        dG2 = kron(1.0I(size(Ms_inv,2)),A)*dMs_inv;
        dG = dG1 + dG2;

        # dG_xx
        Q1 = kron(permutedims(Ms_inv), 1.0I(size(A,1)));
        dMs_inv_t = createCommMat(Ms_inv)*dMs_inv;
        dQ1 = fdJ_kronAB(permutedims(Ms_inv), 1.0I(size(A,1)),dMs_inv_t,zeros(size(A,1)^2,length(x)));

        Q2 = kron(1.0I(size(Ms_inv,2)), A);
        dQ2 = fdJ_kronAB(1.0I(size(Ms_inv,2)), A, zeros(size(dMs_inv)), dA);

        out1 = kron(permutedims(dA),1.0I(size(Q1,1)))*dQ1;
        out2 = kron(1.0I(size(dA,2)), Q1)*dA_xx;
        out3 = kron(permutedims(dMs_inv), 1.0I(size(Q2,1)))*dQ2;
        out4 = kron(1.0I(size(dMs_inv,2)), Q2)*dMsInv_xx;

        dG_xx = out1 + out2 + out3 + out4;
        return G, dG, dG_xx
      end
      G, dG, dG_xx = Gdiff2(xv,uv);

      function Hdiff2(x,u)
        # H = G*G^T
        dG_t = createCommMat(G)*dG;

        H = G*permutedims(G);

        dH = kron(G,1.0I(size(G,1)))*dG + kron(1.0I(size(G,1)),G)*dG_t;

        P1 = kron(G,1.0I(size(G,1)));
        dP1 = fdJ_kronAB(G, 1.0I(size(G,1)), dG, zeros(size(G,1)^2,length(x)));

        P2 = kron(1.0I(size(G,1)), G);
        dP2 = fdJ_kronAB(1.0I(size(G,1)), G, zeros(size(G,1)^2,length(x)), dG);

        dG2_t = kron(1.0I(size(dG,2)), createCommMat(G))*dG_xx;

        out1 = kron(permutedims(dG), 1.0I(size(P1,1)))*dP1;
        out2 = kron(1.0I(size(dG,2)), P1)*dG_xx;
        out3 = kron(permutedims(dG_t), 1.0I(size(P2,1)))*dP2;
        out4 = kron(1.0I(size(dG_t,2)), P2)*dG2_t;

        dH_xx = out1 + out2 + out3 + out4;
        return H, dH, dH_xx
      end
      H, dH, dH_xx = Hdiff2(xv,uv);

      function Gpdiff2(x,u)
        dG_t = createCommMat(G)*dG;
        dG2_t = kron(1.0I(size(dG,2)), createCommMat(G))*dG_xx;

        H_inv = inv(H);
        dH_inv = -kron(permutedims(H_inv), H_inv)*dH;
        dH_inv_t = createCommMat(H_inv)*dH_inv;

        W = kron(permutedims(H_inv), H_inv);
        dW = fdJ_kronAB(permutedims(H_inv), H_inv, dH_inv_t, dH_inv);
        h1 = kron(permutedims(dH),1.0I(size(W,1)))*dW;
        h2 = kron(1.0I(size(dH,2)),W)*dH_xx;
        dH_inv2 = -(h1 + h2);

        V1 = kron(permutedims(H_inv), 1.0I(size(G,2)));
        dV1 = fdJ_kronAB(permutedims(H_inv), 1.0I(size(G,2)), dH_inv_t, zeros(size(G,2)^2, length(x)));

        V2 = kron(1.0I(size(H_inv,2)), permutedims(G));
        dV2 = fdJ_kronAB(1.0I(size(H_inv,2)),permutedims(G), zeros(size(dH_inv_t)), dG_t);

        out1 = kron(permutedims(dG_t), 1.0I(size(V1,1)))*dV1;
        out2 = kron(1.0I(size(dG_t,2)), V1)*dG2_t;
        out3 = kron(permutedims(dH_inv), 1.0I(size(V2,1)))*dV2;
        out4 = kron(1.0I(size(dH_inv,2)), V2)*dH_inv2;

        dGp_xx = out1 + out2 + out3 + out4;

        return dGp_xx;
      end
      dGp_xx = Gpdiff2(xv,uv);

      function Fudiff2(x,u)
        dFu_xx = ForwardDiff.jacobian(y->Fudiff1(y,u)[2],x);
        dFu_ux = ForwardDiff.jacobian(y->Fudiff1(x,y)[2],u);
        dFu_xu = ForwardDiff.jacobian(y->Fudiff1(y,u)[3],x);
        dFu_uu = ForwardDiff.jacobian(y->Fudiff1(x,u)[3],u);
        return dFu_xx, dFu_ux, dFu_xu, dFu_uu
      end
      dFu_xx, dFu_ux, dFu_xu, dFu_uu = Fudiff2(xv,uv);

      function hdiff2(x,u)
        # dh_xx
        # terms 1,2
        T1 = kron(permutedims(M_inv*Fu), 1.0I(size(A,1)))
        dMinvFu = kron(permutedims(Fu), 1.0I(size(M_inv,1)))*dM_inv + M_inv*dFu;
        dMinvFu_t = dMinvFu;
        dT1 = fdJ_kronAB(permutedims(M_inv*Fu), 1.0I(size(A,1)), dMinvFu_t, zeros(size(A,1)^2, length(x)));
        term1 = kron(permutedims(dA), 1.0I(size(T1,1)))*dT1
        term2 = kron(1.0I(size(dA,2)), T1)*dA_xx;

        # terms 3,4
        T2 = kron(permutedims(Fu), A);
        dT2 = fdJ_kronAB(permutedims(Fu), A, dFu, dA);
        term3 = kron(permutedims(dM_inv), 1.0I(size(T2,1)))*dT2;
        term4 = kron(1.0I(size(dM_inv,2)), T2)*dMinv_xx;

        # terms 5,6
        T3 = A*M_inv;
        dT3 = kron(permutedims(M_inv), 1.0I(size(A,1)))*dA + kron(1.0I(size(M_inv,2)), A)*dM_inv;
        term5 = kron(permutedims(dFu), 1.0I(size(A,1)))*dT3;
        term6 = kron(1.0I(length(x)), T3)*dFu_xx;

        dh_xx = db_xx - (term1 + term2 + term3 + term4 + term5 + term6);

        # dhFn(y) = ForwardDiff.jacobian(z->hFn(z,u),y);
        # cfg1 = ForwardDiff.JacobianConfig(dhFn, x, ForwardDiff.Chunk{1}());
        # dh_xx = ForwardDiff.jacobian(dhFn,x,cfg1);

        # dh_ux
        E1 = kron(permutedims(M_inv*Fu), 1.0I(size(A,1)));
        dE1_u = fdJ_kronAB(permutedims(M_inv*Fu), 1.0I(size(A,1)), M_inv*dFu_u, zeros(size(A,1)^2, length(u)));

        E2 = kron(permutedims(Fu), A);
        dE2_u = fdJ_kronAB(permutedims(Fu), A, dFu_u, zeros(length(A), length(u)));

        term1 = kron(permutedims(dA),1.0I(size(E1,1)))*dE1_u;
        term2 = kron(permutedims(dM_inv), 1.0I(size(E2,1)))*dE2_u;
        term3 = kron(1.0I(size(dFu,2)), A*M_inv)*dFu_ux;

        dh_ux = -(term1 + term2 + term3);

        # dh_xu
        out1 = kron(permutedims(M_inv*dFu_u), 1.0I(size(A,1)))*dA;
        out2 = kron(permutedims(dFu_u), A)*dM_inv;
        out3 = kron(1.0I(length(u)), A*M_inv)*dFu_xu;
        dh_xu = -(out1 + out2 + out3);

        # dh_uu
        dh_uu = -kron(1.0I(length(u)), A*M_inv)*dFu_uu;

        return dh_xx, dh_ux, dh_xu, dh_uu
      end
      dh_xx, dh_ux, dh_xu, dh_uu = hdiff2(xv,uv);
      h, dh_x, dh_u = hdiff1(xv,uv);

      function Fcdiff2(x,u)
        # dFc_xx
        function jacT1(x)
          D1 = kron(permutedims(Gp*h), 1.0I(size(Ms,1)));
          dGph = kron(permutedims(h), 1.0I(size(Gp,1)))*dGp + Gp*dh_x;
          dGph_t = dGph;
          dD1 = fdJ_kronAB(permutedims(Gp*h), 1.0I(size(Ms,1)), dGph_t, zeros(size(dMs)));

          dMs_t = createCommMat(Ms)*dMs;

          dT1_1 = kron(permutedims(dMs), 1.0I(size(D1,1)))*dD1;
          dT1_2 = kron(1.0I(size(dMs,2)), D1)*dMs_xx;

          dT1 = dT1_1 + dT1_2;
          return dT1
        end

        function jacT2(x)
          D2 = kron(permutedims(h), Ms);
          dD2 = fdJ_kronAB(permutedims(h), Ms, dh_x, dMs);
          # dGp_xx = Gpdiff2(x,u);

          dT2_1 = kron(permutedims(dGp),1.0I(size(D2,1)))*dD2;
          dT2_2 = kron(1.0I(size(dGp,2)), D2)*dGp_xx;
          dT2 = dT2_1 + dT2_2;
          return dT2
        end
        function jacT3(x)
          dT3_1 = kron(permutedims(Gp*dh_x), 1.0I(size(Ms,1)))*dMs;
          dT3_2 = kron(permutedims(dh_x), Ms)*dGp;
          dT3_3 = kron(1.0I(size(dh_x,2)), Ms*Gp)*dh_xx;
          dT3 = dT3_1 + dT3_2 + dT3_3;

          return dT3
        end
        dFc_xx = jacT1(x) + jacT2(x) + jacT3(x);

        # dFc_ux
        # # first term
        D1 = kron(permutedims(Gp*h), 1.0I(size(Ms,1)));
        dGph_u = Gp*dh_u;
        dGph_u_t = dGph_u;
        dD1_u = fdJ_kronAB(permutedims(Gp*h), 1.0I(size(Ms,1)), dGph_u_t, zeros(size(dMs,1),length(u)));
        out1 = kron(permutedims(dMs), 1.0I(size(D1,1)))*dD1_u;
        # # second term
        D2 = kron(permutedims(h), Ms)*dGp;
        dD2_u = fdJ_kronAB(permutedims(h), Ms, dh_u, zeros(size(dMs,1), length(u)));
        out2 = kron(permutedims(dGp), 1.0I(size(D2,1)))*dD2_u;
        # # third term
        out3 = kron(1.0I(size(dh_x,2)), Ms*Gp)*dh_ux;
        dFc_ux = out1 + out2 + out3;

        # dFc_xu
        out1 = kron(permutedims(Gp*dh_u), 1.0I(size(Ms,1)))*dMs;
        out2 = kron(permutedims(dh_u), Ms)*dGp;
        out3 = kron(1.0I(length(u)), Ms*Gp)*dh_xu;
        dFc_xu = out1 + out2 + out3;

        # dFc_uu
        dFc_uu = kron(1.0I(size(dh_u,2)), Ms*Gp)*dh_uu;

        return dFc_xx, dFc_ux, dFc_xu, dFc_uu
      end
      dFc_xx, dFc_ux, dFc_xu, dFc_uu = Fcdiff2(xv,uv);

      function hessqdd(x,u)
        T1 = kron(permutedims(Fc + Fu), 1.0I(size(M_inv,1)));
        dT1 = fdJ_kronAB(permutedims(Fc + Fu), 1.0I(size(M_inv,1)), (dFc + dFu), zeros(size(dM_inv)));
        T2 = dFc + dFu;
        dT2 = dFc_xx + dFu_xx;

        out1 = kron(permutedims(dM_inv), 1.0I(size(T1,1)))*dT1;
        out2 = kron(1.0I(size(dM_inv,2)), T1)*dMinv_xx;
        out3 = kron(permutedims(T2), 1.0I(size(M_inv,1)))*dM_inv;
        out4 = kron(1.0I(size(T2,2)), M_inv)*dT2;

        dqdd_xx = out1 + out2 + out3 + out4;
        return dqdd_xx
      end
      dqdd_xx = hessqdd(xv,uv);

      function cross_qdd_ux(x,u)
        T1 = kron(permutedims(Fc + Fu), 1.0I(size(M_inv,1)));
        dT1_u = fdJ_kronAB(permutedims(Fc + Fu), 1.0I(size(M_inv,1)), dFc_u + dFu_u, zeros(size(M_inv,1)^2, length(u)));

        T2 = dFc + dFu;
        dT2_u = dFc_ux + dFu_ux;

        out1 = kron(permutedims(dM_inv), 1.0I(size(T1,1)))*dT1_u;
        out2 = kron(1.0I(length(x)), M_inv)*dT2_u;
        dqdd_ux = out1+out2;
        return dqdd_ux
      end
      dqdd_ux = cross_qdd_ux(xv,uv);

      function cross_qdd_xu(x,u)
        out1 = kron(permutedims(dFc_u + dFu_u), 1.0I(size(M_inv,1)))*dM_inv;
        out2 = kron(1.0I(length(u)), M_inv)*(dFc_xu + dFu_xu);
        dqdd_xu = out1 + out2;
        return dqdd_xu
      end
      dqdd_xu = cross_qdd_xu(xv,uv);

      function hessqdd_u(x,u)
        dqdd_uu = kron(1.0I(length(u)), M_inv)*(dFc_uu + dFu_uu);
        return dqdd_uu
      end
      dqdd_uu = hessqdd_u(xv,uv);

      function hessDyn(xuVar)
        x = xuVar[1:14*nB]; u = reshape(xuVar[14*nB+1:end], (6,nB));

        y = qdd;
        dy = dqdd;

        d2y_xx = dqdd_xx;#  hessqdd(x,u);
        d2y_xu = dqdd_xu;# cross_qdd_xu(x,u);
        d2y_ux = dqdd_ux;# cross_qdd_ux(x,u);
        d2y_uu = dqdd_uu;# hessqdd_u(x,u);

        d2y = [d2y_xx d2y_ux; d2y_xu d2y_uu];

        out1 = kron(1.0I(length(xuVar)), permutedims(y))*d2y;
        out2 = permutedims(dy)*dy;
        out = 2*(out1 + out2); # hessian of dynamics cost = sum(y.*y)
        return out;
      end
      dqdd_xuVar = hessDyn(xu_var);

      function hess_nonlinear_constraints()
        zLen = length(z);

        hess_dyn = hessDyn(xu_var); # hessian of dynamics cost
        _, hess_qNorm = quatNormConstr_diff(xu_var); # hessian of quatNormConstr cost
        _, hess_jLoc = jointLocConstr_diff(xu_var); # hessian of jointLocConstr cost
        println()
        # println("typeof(hess_dyn) = ", typeof(hess_dyn))
        # println("typeof(hess_qNorm) = ", typeof(hess_qNorm))
        # println("typeof(hess_jLoc) = ", typeof(hess_jLoc))

        hess_constr = hess_dyn +  hess_qNorm + hess_jLoc;

        # taking only free variables into consideration
        freeVarInd_hess = 14 .+ freeVarInd[1:end-1];
        hess_constr_free = hess_constr[freeVarInd_hess, freeVarInd_hess];

        out = zeros(typeof(hess_constr_free[1]),(zLen, zLen));
        out[1:zLen-1, 1:zLen - 1] = hess_constr_free;
        return out
      end
      out = hess_nonlinear_constraints();
    end
    return out
  end

  ## IpOpt functions
  function eval_f(z)
    J = z[end]
    return J
  end

  function eval_g(z, g)
    g[:] .= _nonlinear_constraints(z)
  end

  function eval_grad_f(z, grad_f)
    v = zeros(length(z)); v[end] = 1.0;
    grad_f[:] = v;
  end

  function eval_jac_g(z, mode, rows, cols, values)
    nvar = length(z);
    ng = 1;

    if mode == :Structure
      index = 1;
      for i=1:ng
          for j=1:nvar
              rows[index] = i;
              cols[index] = j;
              index += 1;
          end
      end
    else
      flag = 1;
      values[:] = grad_hess_nonlinear_constraints(z,flag);
    end
  end

  function eval_h(z, mode, rows, cols, obj_factor, lambda, values) # Hessian of cost.
    nvar = length(z);
    if mode == :Structure
      # Symmetric matrix, fill the lower left triangle only
      idx = 1
      for row = 1:nvar
        for col = 1:row
          rows[idx] = row
          cols[idx] = col
          idx += 1
        end
      end
    else
      # H = obj_factor*zeros(nvar, nvar)
      # for i=1:length(lambda)
      flag = 2;
      H = lambda[1]*grad_hess_nonlinear_constraints(z,flag);
      # end

      # Copy them into return variable -- only lower triangle.
      idx = 1;
      for row = 1:nvar
        for col = 1:row
          values[idx] = H[row,col];
          idx += 1;
        end
      end
    end
  end

  # Initial guess from freeVarInd
  n = length(freeVarInd);
  guessFull = [x0;u0[:,2:end][:];0.0];
  inGuess = guessFull[freeVarInd];

  nele_jac = Integer(m*n);
  nele_hess = Integer(n*(n+1)/2);
  prob = createProblem(n, lb, ub, m, lb_g, ub_g, nele_jac, nele_hess, eval_f, eval_g, eval_grad_f, eval_jac_g, eval_h);
  prob.x = inGuess; # Initial guess.
  status = solveProblem(prob);

  trim_x = prob.x[1:length(x0)];
  gam = prob.x[end];
  trim_u = prob.x[length(x0)+1:end-1];
  return trim_x, trim_u, gam
  # return trim_x, gam
end
## testing the trim function
# x0Orig, u0Orig = getXU_0(j);
# nB = length(j) + 1;
# ix= zeros(Integer,20*(nB-1)+1);
# # WITH U=0
# ix[14*(nB-1)+1:20*(nB-1)] .= 1;
# trim_U = u0Orig;
# iy = zeros(Integer,7*(nB-1));
#
# out = trimDiff2(j,g,ix=ix, iy=iy);
# trim_x, trim_u, gam = out;
# println("trim_x = ", trim_x)
# trim_X = [j[1].RB1.x;trim_x]
# # trim_U = [zeros(6) reshape(trim_u,(6,nB-1))];
# # println("trim_U = ", trim_U[:,2])
# # trim_U = zeros(6,nB);
# println("qconstr = ", norm(trim_x[4:7])-1)
# println("ẍ =", norm(fxdot(trim_X,trim_U, j, g)))

max_stars_repo_name: isrlab/Multibody-Dynamics

max_issues_repo_name: isrlab/Multibody-Dynamics

max_forks_repo_name: isrlab/Multibody-Dynamics


text:
using PAM
using Test

@testset "PAM.jl" begin
    X = [3.2 0.8 0.5 1.2 3.9 10.1 14.0 13.0 14.7 14.2;
         1.1 3.3 0.4 3.9 3.9 10.6 14.4 13.7 12.6 10.8]
    D = [0.0 3.25576 2.78927 3.44093 2.88617 11.7414 17.1327 15.9625 16.2635 14.6659;
        3.25576 0.0 2.91548 0.72111 3.15753 11.8229 17.2467 16.0312 16.7242 15.3561;
        2.78927 2.91548 0.0 3.56931 4.87955 14.0071 19.4487 18.2521 18.7211 17.2003;
        3.44093 0.72111 3.56931 0.0 2.7 11.14 16.5557 15.3388 16.0605 14.7177;
        2.88617 3.15753 4.87955 2.7 0.0 9.12853 14.5691 13.3735 13.8683 12.3976;
        11.7414 11.8229 14.0071 11.14 9.12853 0.0 5.44518 4.245 5.01597 4.10488;
        17.1327 17.2467 19.4487 16.5557 14.5691 5.44518 0.0 1.22066 1.93132 3.60555;
        15.9625 16.0312 18.2521 15.3388 13.3735 4.245 1.22066 0.0 2.02485 3.13847;
        16.2635 16.7242 18.7211 16.0605 13.8683 5.01597 1.93132 2.02485 0.0 1.86815;
        14.6659 15.3561 17.2003 14.7177 12.3976 4.10488 3.60555 3.13847 1.86815 0.0]
    k = 2
    results = pam(D,k)
    @test results.medoids == [8,2]
    @test results.assignments == [2, 2, 2, 2, 2, 1, 1, 1, 1, 1]
end


text:
"""
Returns all assets (in no particular order).
"""
function assets()
    r = get("$URL/assets")
    s = String(r.body)
    res = read(s)
    return DataFrame(res.result)
end

"""
Returns a single asset. Lists all markets which have this asset as a base or quote.
"""
function assets(symbol::String)
    r = get("$URL/assets/$symbol")
    s = String(r.body)
    return read(s)
end

"""
Returns all pairs (in no particular order).
"""
function pairs()
    r = get("$URL/pairs")
    s = String(r.body)
    return read(s)
end

"""
Returns a single pair. Lists all markets for this pair.
"""
function pairs(symbol::String)
    r = get("$URL/pairs/$symbol")
    s = String(r.body)
    return read(s)
end

"""
Returns a list of all supported exchanges.
"""
function exchanges()
    r = get("$URL/exchanges")
    s = String(r.body)
    res = read(s)
    return DataFrame(res.result)
end

"""
Returns a single exchange with associated routes.
"""
function exchanges(exchange::String)
    r = get("$URL/exchanges/$exchange")
    s = String(r.body)
    return read(s)
end

"""
Returns a list of all supported markets.
"""
function markets()
    r = get("$URL/markets")
    s = String(r.body)
    res = read(s)
    return DataFrame(res.result)
end

"""
Return supported markets for only a specific exchange.
"""
function markets(exchange::String)
    r = get("$URL/markets/$exchange")
    s = String(r.body)
    return read(s)
end

"""
Returns a single market with associated routes.
"""
function markets(exchange::String, pair::String)
    r = get("$URL/markets/$exchange/$pair")
    s = String(r.body)
    return read(s)
end

"""
Returns the current price for all supported markets.
"""
function prices()
    r = get("$URL/markets/prices")
    s = String(r.body)
    return read(s)
end

"""
Returns a market's last price.
"""
function prices(exchange::String, pair::String)
    r = get("$URL/markets/$exchange/$pair/price")
    s = String(r.body)
    return read(s)
end

"""
Returns the market summary for all supported markets.
"""
function summary()
    r = get("$URL/markets/summaries")
    s = String(r.body)
    return read(s)
end

"""
Returns a market's last price as well as other stats based on a 24-hour sliding window.
"""
function summary(exchange::String, pair::String)
    r = get("$URL/markets/$exchange/$pair/summary")
    s = String(r.body)
    return read(s)
end

"""
Returns a market's most recent trades, incrementing chronologically.
"""
function trades(exchange::String, pair::String)
    r = get("$URL/markets/$exchange/$pair/trades")
    s = String(r.body)
    return read(s)
end

"""
Returns a market's order book.
"""
function orderbook(exchange::String, pair::String)
    r = get("$URL/markets/$exchange/$pair/orderbook")
    s = String(r.body)
    return read(s)
end

function orderbook(exchange::String, pair::String, limit::Int64, depth::Float64, span::Float64)
    r = get("$URL/markets/$exchange/$pair/orderbook?limit=$limit&depth=$depth&span=$span")
    s = String(r.body)
    return read(s)
end

"""
Returns a market's OHLC candlestick data.
Returns data as lists of lists of numbers for each time period integer.
"""
function ohlc(exchange::String, pair::String)
    r = get("$URL/markets/$exchange/$pair/ohlc")
    s = String(r.body)
    return read(s)
end


text:
export ToeplitzOperator, HankelOperator

mutable struct ToeplitzOperator{T<:Number} <: Operator{T}
    negative::Vector{T}
    nonnegative::Vector{T}
end

ToeplitzOperator(V::Vector{T},W::Vector{Q}) where {T<:Number,Q<:Number} =
    ToeplitzOperator{promote_type(T,Q)}(V,W)
ToeplitzOperator(V::AbstractVector,W::AbstractVector) =
    ToeplitzOperator(collect(V),collect(W))

convert(::Type{Operator{TT}},T::ToeplitzOperator) where {TT} =
    ToeplitzOperator(convert(Vector{TT},T.negative),convert(Vector{TT},T.nonnegative))

for op in (:(Base.real), :(Base.imag))
    @eval $op(T::ToeplitzOperator) = ToeplitzOperator($op(T.negative), $op(T.nonnegative))
end

function SymToeplitzOperator(V::Vector)
    W=V[2:end]
    V=copy(V)
    V[1]*=2
    ToeplitzOperator(W,V)
end

for OP in (:domainspace,:rangespace)
    @eval $OP(T::ToeplitzOperator) = ℓ⁰
end

getindex(T::ToeplitzOperator,k::Integer,j::Integer) =
    toeplitz_getindex(T.negative,T.nonnegative,k,j)

function toeplitz_getindex(negative::AbstractVector{T},nonnegative::AbstractVector{T},k::Integer,j::Integer) where T
    if 0<k-j≤length(negative)
        negative[k-j]
    elseif 0≤j-k≤length(nonnegative)-1
        nonnegative[j-k+1]
    else
        zero(T)
    end
end

function toeplitz_getindex(cfs::AbstractVector{T},k::Integer,j::Integer) where T
    if k==j && !isempty(cfs)
        2cfs[1]
    elseif 0<k-j≤length(cfs)-1
        cfs[k-j+1]
    elseif 0<j-k≤length(cfs)-1
        cfs[j-k+1]
    else
        zero(T)
    end
end

function BandedMatrix(S::SubOperator{T,ToeplitzOperator{T},Tuple{UnitRange{Int},UnitRange{Int}}}) where T
    ret = BandedMatrix(Zeros, S)

    kr,jr=parentindices(S)

    neg=parent(S).negative
    pos=parent(S).nonnegative

    toeplitz_axpy!(1.0,neg,pos,kr,jr,ret)
end

bandwidths(T::ToeplitzOperator)=(length(T.negative),length(T.nonnegative)-1)

# slice of a ToeplitzOPerator is a ToeplitzOperator

function Base.getindex(T::ToeplitzOperator,kr::InfRanges,jr::InfRanges)
    sh=first(jr)-first(kr)
    st=step(jr)
    @assert st==step(kr)
    if sh ≥0
        ToeplitzOperator([reverse!(T.nonnegative[1:sh]);T.negative],T.nonnegative[sh+1:st:end])
    else
        ToeplitzOperator(T.negative[-sh+1:st:end],[reverse!(T.negative[1:-sh]);T.nonnegative])
    end
end

## Hankel Operator

mutable struct HankelOperator{T<:Number} <: Operator{T}
    coefficients::Vector{T}
end

for OP in (:domainspace,:rangespace)
    @eval $OP(T::HankelOperator) = ℓ⁰
end

HankelOperator(V::AbstractVector)=HankelOperator(collect(V))

HankelOperator(f::Fun)=HankelOperator(f.coefficients)

@eval convert(::Type{Operator{TT}},T::HankelOperator) where {TT}=HankelOperator(convert(Vector{TT},T.coefficients))

function hankel_getindex(v::AbstractVector,k::Integer,j::Integer)
   if k+j-1 ≤ length(v)
        v[k+j-1]
    else
        zero(eltype(v))
    end
end

getindex(T::HankelOperator,k::Integer,j::Integer) =
    hankel_getindex(T.coefficients,k,j)

function BandedMatrix(S::SubOperator{T,HankelOperator{T},Tuple{UnitRange{Int},UnitRange{Int}}}) where T
    ret=BandedMatrix(Zeros, S)

    kr,jr=parentindices(S)
    cfs=parent(S).coefficients

    hankel_axpy!(1.0,cfs,kr,jr,ret)
end

bandwidths(T::HankelOperator) = (max(0,length(T.coefficients)-1),max(0,length(T.coefficients)-1))

## algebra

function Base.maximum(T::ToeplitzOperator)
    if isempty(T.negative)
        maximum(T.nonnegative)
    elseif isempty(T.nonnegative)
        maximum(T.negative)
    else
        max(maximum(T.negative),maximum(T.nonnegative))
    end
end

-(T::ToeplitzOperator)=ToeplitzOperator(-T.negative,-T.nonnegative)
*(c::Number,T::ToeplitzOperator)=ToeplitzOperator(c*T.negative,c*T.nonnegative)

-(H::HankelOperator)=HankelOperator(-H.coefficients)
*(c::Number,H::HankelOperator)=HankelOperator(c*H.coefficients)

## inv

function Base.inv(T::ToeplitzOperator)
    @assert length(T.nonnegative)==1
    ai=\(T,[1.0];maxlength=100000)
    ToeplitzOperator(ai[2:end],ai[1:1])
end

####
# Toeplitz/Hankel
####

# αn,α0,αp give the constant for the negative, diagonal and positive
# entries.  The usual case is 1,2,1
function toeplitz_axpy!(αn,α0,αp,neg,pos,kr,jr,ret)
    dat=ret.data
    m=size(dat,2)

    dg=diagindrow(ret,kr,jr)

    # diagonal
    if dg ≥ 1
        α0p=α0*pos[1]
        @simd for j=1:m
            @inbounds dat[dg,j]+=α0p
        end
    end

    # positive entries
    for k=2:min(length(pos),dg)
        αpp=αp*pos[k]
        @simd for j=1:m
            @inbounds dat[dg-k+1,j]+=αpp
        end
    end

    # negative entries
    for k=1:min(length(neg),size(dat,1)-dg)
        αnn=αn*neg[k]
        @simd for j=1:m
            @inbounds dat[dg+k,j]+=αnn
        end
    end

    ret
end

# this routine is for when we want a symmetric toeplitz
function sym_toeplitz_axpy!(αn,α0,αp,cfs,kr,jr,ret)
    dg=diagindrow(ret,kr,jr)

    dat=ret.data
    m=size(dat,2)
    # diagonal
    if dg ≥ 1
        α0p=α0*cfs[1]
        @simd for j=1:m
            @inbounds dat[dg,j]+=α0p
        end
    end

    # positive entries
    for k=2:min(length(cfs),dg)
        αpp=αp*cfs[k]
        @simd for j=1:m
            @inbounds dat[dg-k+1,j]+=αpp
        end
    end

    # negative entries
    for k=2:min(length(cfs),size(dat,1)-dg+1)
        αnn=αn*cfs[k]
        @simd for j=1:m
            @inbounds dat[dg+k-1,j]+=αnn
        end
    end

    ret
end

toeplitz_axpy!(α,neg,pos,kr,jr,ret) =
    toeplitz_axpy!(α,α,α,neg,pos,kr,jr,ret)

sym_toeplitz_axpy!(α0,αp,cfs,kr,jr,ret) =
    sym_toeplitz_axpy!(αp,α0,αp,cfs,kr,jr,ret)

function hankel_axpy!(α,cfs,kr,jr,ret)
    dat=ret.data

    st=stride(dat,2)-2
    mink=first(kr)+first(jr)-1

    N=length(dat)

    # dg gives the row corresponding to the diagonal of the original operator
    dg=diagindrow(ret,kr,jr)

    # we need the entry where the first entry is written to
    # this is going to be a shift of the diagonal of the true operator
    dg1=dg+first(kr)-first(jr)

    for k=mink:min(length(cfs),size(dat,1)+mink-dg1)
        dk=k-mink+dg1
        nk=k-mink
        αc=α*cfs[k]
        @simd for j=dk:st:min(dk+nk*st,N)
            @inbounds dat[j]+=αc
        end
    end

    ret
end


text:
@testset "Wilson" begin
    g = SimpleGraph(4)
    add_edge!(g, 1, 2)
    add_edge!(g, 1, 3)
    add_edge!(g, 1, 4)
    add_edge!(g, 2, 3)
    add_edge!(g, 3, 4)

    N = 2*10^4
    diag_edge = Edge(1, 3)
    tol = 0.01

    # checking spanning tree count
    minor = Matrix(view(laplacian_matrix(g), 2:4, 2:4))
    @test Int(det(minor)) == 8

    # Testing Wilson's algorithm on an unweighted graphs
    diag_count = 0
    for ii = 1:N
        st = @inferred(wilson_rst(g, seed=5124154 + 312*ii))
        diag_count += Int(diag_edge in st)
    end
    @test abs(0.5 - diag_count/N) < tol

    rng = MersenneTwister(123411223)
    diag_count = 0
    for ii = 1:N
        st = @inferred(wilson_rst(g, rng=rng))
        diag_count += Int(diag_edge in st)
    end
    @test abs(0.5 - diag_count/N) < tol

    # Testing Wilson's algorithm on a weighted graph
    distmx = ones(Float64, 4, 4)
    distmx[1,3] = 0.5
    distmx[3,1] = 0.5

    diag_count = 0
    N = 10^5
    for ii = 1:N
        st = @inferred(wilson_rst(g, distmx, seed=1509812+192*ii))
        diag_count += Int(Edge(1,3) in st)
    end
    @test abs(1/3 - diag_count/N) < tol
end


text:
const MAX_POOL_FWD_FLOAT = Libdl.dlsym(Native.library, :max_pooling_fwd_float)
const MAX_POOL_FWD_DOUBLE = Libdl.dlsym(Native.library, :max_pooling_fwd_double)
const MAX_POOL_BWD_FLOAT = Libdl.dlsym(Native.library, :max_pooling_bwd_float)
const MAX_POOL_BWD_DOUBLE = Libdl.dlsym(Native.library, :max_pooling_bwd_double)

const MEAN_POOL_FWD_FLOAT = Libdl.dlsym(Native.library, :mean_pooling_fwd_float)
const MEAN_POOL_FWD_DOUBLE = Libdl.dlsym(Native.library, :mean_pooling_fwd_double)
const MEAN_POOL_BWD_FLOAT = Libdl.dlsym(Native.library, :mean_pooling_bwd_float)
const MEAN_POOL_BWD_DOUBLE = Libdl.dlsym(Native.library, :mean_pooling_bwd_double)

function max_pooling_forward(input::Array{Float32}, output::Array{Float32}, mask::Array{Csize_t}, layer)
  width, height, channels, num = size(input)
  pooled_width = size(output, 1)
  pooled_height = size(output, 2)
  ccall(MAX_POOL_FWD_FLOAT, Void,
      (Ptr{Float32}, Ptr{Float32}, Ptr{Csize_t},
      Cint, Cint, Cint, Cint,
      Cint, Cint,
      Cint, Cint, Cint, Cint, Cint, Cint),
      input, output, mask,
      width, height, channels, num,
      pooled_width, pooled_height,
      layer.kernel[1], layer.kernel[2],
      layer.pad[1], layer.pad[2], layer.stride[1], layer.stride[2])
end
function max_pooling_forward(input::Array{Float64}, output::Array{Float64}, mask::Array{Csize_t}, layer)
  width, height, channels, num = size(input)
  pooled_width = size(output, 1)
  pooled_height = size(output, 2)
  ccall(MAX_POOL_FWD_DOUBLE, Void,
      (Ptr{Float64}, Ptr{Float64}, Ptr{Csize_t},
      Cint, Cint, Cint, Cint,
      Cint, Cint,
      Cint, Cint, Cint, Cint, Cint, Cint),
      input, output, mask,
      width, height, channels, num,
      pooled_width, pooled_height,
      layer.kernel[1], layer.kernel[2],
      layer.pad[1], layer.pad[2], layer.stride[1], layer.stride[2])
end

function max_pooling_backward(input::Array{Float32}, output::Array{Float32}, mask::Array{Csize_t}, layer)
  width, height, channels, num = size(input)
  pooled_width = size(output, 1)
  pooled_height = size(output, 2)
  ccall(MAX_POOL_BWD_FLOAT, Void,
      (Ptr{Float32}, Ptr{Float32}, Ptr{Csize_t},
      Cint, Cint, Cint, Cint,
      Cint, Cint,
      Cint, Cint, Cint, Cint, Cint, Cint),
      input, output, mask,
      width, height, channels, num,
      pooled_width, pooled_height,
      layer.kernel[1], layer.kernel[2],
      layer.pad[1], layer.pad[2], layer.stride[1], layer.stride[2])
end
function max_pooling_backward(input::Array{Float64}, output::Array{Float64}, mask::Array{Csize_t}, layer)
  width, height, channels, num = size(input)
  pooled_width = size(output, 1)
  pooled_height = size(output, 2)
  ccall(MAX_POOL_BWD_DOUBLE, Void,
      (Ptr{Float64}, Ptr{Float64}, Ptr{Csize_t},
      Cint, Cint, Cint, Cint,
      Cint, Cint,
      Cint, Cint, Cint, Cint, Cint, Cint),
      input, output, mask,
      width, height, channels, num,
      pooled_width, pooled_height,
      layer.kernel[1], layer.kernel[2],
      layer.pad[1], layer.pad[2], layer.stride[1], layer.stride[2])
end

function mean_pooling_forward(input::Array{Float32}, output::Array{Float32}, layer)
  width, height, channels, num = size(input)
  pooled_width = size(output, 1)
  pooled_height = size(output, 2)

  ccall(MEAN_POOL_FWD_FLOAT, Void,
      (Ptr{Float32}, Ptr{Float32},
      Cint, Cint, Cint, Cint,
      Cint, Cint,
      Cint, Cint, Cint, Cint, Cint, Cint),
      input, output,
      width, height, channels, num,
      pooled_width, pooled_height,
      layer.kernel[1], layer.kernel[2],
      layer.pad[1], layer.pad[2], layer.stride[1], layer.stride[2])
end
function mean_pooling_forward(input::Array{Float64}, output::Array{Float64}, layer)
  width, height, channels, num = size(input)
  pooled_width = size(output, 1)
  pooled_height = size(output, 2)

  ccall(MEAN_POOL_FWD_DOUBLE, Void,
      (Ptr{Float64}, Ptr{Float64},
      Cint, Cint, Cint, Cint,
      Cint, Cint,
      Cint, Cint, Cint, Cint, Cint, Cint),
      input, output,
      width, height, channels, num,
      pooled_width, pooled_height,
      layer.kernel[1], layer.kernel[2],
      layer.pad[1], layer.pad[2], layer.stride[1], layer.stride[2])
end

function mean_pooling_backward(input::Array{Float32}, output::Array{Float32}, layer)
  width, height, channels, num = size(input)
  pooled_width = size(output, 1)
  pooled_height = size(output, 2)

  ccall(MEAN_POOL_BWD_FLOAT, Void,
      (Ptr{Float32}, Ptr{Float32},
      Cint, Cint, Cint, Cint,
      Cint, Cint,
      Cint, Cint, Cint, Cint, Cint, Cint),
      input, output,
      width, height, channels, num,
      pooled_width, pooled_height,
      layer.kernel[1], layer.kernel[2],
      layer.pad[1], layer.pad[2], layer.stride[1], layer.stride[2])
end
function mean_pooling_backward(input::Array{Float64}, output::Array{Float64}, layer)
  width, height, channels, num = size(input)
  pooled_width = size(output, 1)
  pooled_height = size(output, 2)

  ccall(MEAN_POOL_BWD_DOUBLE, Void,
      (Ptr{Float64}, Ptr{Float64},
      Cint, Cint, Cint, Cint,
      Cint, Cint,
      Cint, Cint, Cint, Cint, Cint, Cint),
      input, output,
      width, height, channels, num,
      pooled_width, pooled_height,
      layer.kernel[1], layer.kernel[2],
      layer.pad[1], layer.pad[2], layer.stride[1], layer.stride[2])
end


text:
using Distributed
if nprocs() == 1
    addprocs(3)
end
@everywhere using ParallelDataTransfer

@everywhere module MyModule
    using Distributed
    # export bar
    mat = 1
    function set_mat(arg1)
        global mat
        # println("mat: ", mat)
        println("set mat $arg1")
        mat = arg1
        println("mat: $mat")
    end
    function foo(arg1)
        println("foo: $arg1")
        println("  mat: $mat")
    end
    function foo_call_ebar(arg1)
        ebar(arg1)
    end

    # how to call this function?
    function ebar(arg1)
        println("bar: $arg1")
    end

    include("devel/everywhere__submodule.jl")
    # @everywhere include("everywhere__submodule.jl")  # LoadError
    function foo_call_sbar(arg1)
        sbar(arg1)
    end

    function run(data)
        inner_parameter = 2
        # z = randn(10, 10);
        # sendto(workers(), MyModule.set_mat(mat))
        @spawn MyModule.set_mat(4)
        function suma(data_row)
            println("suma on $(myid())")
            println("  suma mat: $mat")

            return sum(data_row)
        end
        output = pmap(suma, data)
        return output
    end
end

MyModule.foo("1")
@spawnat 2 MyModule.foo("2")

MyModule.foo_call_sbar("2")
# MyModule.foo_call_ebar("2")  # ebar not defined
# MyModule.e_bar("2") # ebar not defined
data = [rand(Int8, 10), rand(Int8, 10), rand(Int8, 10)]

@everywhere MyModule.set_mat(3)
output = MyModule.run(data)
output
output


text:
startstate(grammar) ::State #
startsymbols(grammar) ::Vector{Category} #
completions(grammar, state) ::Vector{Tuple{Category, CategoryRule, LogProb}}
completions(grammar, terminal) ::Vector{Tuple{Category, TerminalRule, LogProb}}
prob(grammar, category, rule) ::LogProb

isfinal(state) ::Bool
is_possible_transition(grammar, state, cat) ::Bool # No need
transition(grammar, state, cat) ::State # Idem

category_type(grammar) ::Type
terminal_type(grammar) ::Type
category_rule_type(grammar) ::Type
terminal_rule_type(grammar) ::Type
score_type(grammar) ::Type
state_type(grammar) ::Type


text:
# This file is a part of RunStatistics.jl, licensed under the MIT License (MIT).

"""
    Partition(n::Int, k::Int, h::Int, c::Vector{Int}, y::Vector{Int})

Express the integer partition of `n` into `k` parts in the multiplicity representation with
`n = \\sum_{i=2}^(h + 1) c_i * y_i`.

(see https://en.wikipedia.org/wiki/Partition_(number_theory))

`h` is the number of distinct parts, `y` an array containing the distinct parts and `c` an
array containing their multiplicities.

NOTE: due to the computation of subsequent partitions with the algorithm used in `next_partition!()`
the arrays `y` and `c` only hold relevant
values for the indices [2, h + 1]

When reading a partition: ignore the first element of `c` and `y` and do not read beyond
`c[h + 1]`, `y[h + 1]`!
"""
mutable struct Partition
    n::Int
    k::Int
    h::Int
    c::Vector{Int}
    y::Vector{Int}
end

"""
    init_partition(n::Int, k::Int)

Initiate the first partition of an integer `n` into `k` parts; arguments must satisfy `0 < k <= n`.
Returns an object of type `Partition`.

The elements in `y[1]` and `c[1]`, of the arrays `y` and `c` containing the distinct parts and their
multiplicities, are buffer values needed for the computation of the next partition
in `next_partition!()`.

When reading a partition: ignore the first element of `c` and `y` and do not read beyond `c[h + 1]`,
`y[h + 1]`: `n = \\sum_{i=2}^(h + 1) c_i * y_i`.
"""
function init_partition(n::Integer, k::Integer)

    @argcheck 0 < k <= n

    c = zeros(k + 1)
    y = zeros(k + 1)
    h = 0

    y[1] = -1
    c[1] = 1

    maxPart = n - k + 1

    if (k == 1 || k == n)
        y[2] = maxPart
        c[2] = n / maxPart
        h = 1
        done = true

    else
        y[2] = 1
        c[2] = k - 1
        y[3] = maxPart
        c[3] = 1
        h = 2
        done = false
    end

    return Partition(n, k, h, c, y)
end

"""
    next_partition!(p::Partition)

Compute the next partition of `p`, using a modified version of Algorithm Z from *A. Zoghbi: Algorithms
for generating integer partitions, Ottawa (1993)*,
https://www.ruor.uottawa.ca/handle/10393/6506.

The partition `p` is updated in place, saving memory.
Returns a `boolean` corresponding to whether the final partition has been reached.
"""
function next_partition!(p::Partition)

    if p.y[p.h+1] - p.y[2] <= 1
        return true
    else
        c = p.c
        y = p.y
        h = p.h

        i = h
        k = c[h+1]
        r = c[h+1] * y[h+1]

        while (y[h+1] - y[i]) < 2
            k += c[i]
            r += c[i] * y[i]
            i -= 1
        end

        if c[i] == 1
            if i != 1
                r += c[i] * y[i]
                y[i] += 1
            else
                i = 2
                y[i] = 1
            end
        else
            c[i] -= 1
            r += y[i]
            i += 1
            y[i] = y[i-1] + 1
        end

        c[i] = k
        r -= c[i] * y[i]
        h = i

        if (r == y[i])
            c[i] += 1
            h = i - 1

        else
            y[h+1] = r
            c[h+1] = 1
        end

        p.c = c
        p.y = y
        p.h = h
        return false
    end
end


text:
@testset "TestIterator" begin
    @testset "Constructors" begin
        data = randn(3)
        iter = TestIterator(data)
        @test iter isa TestIterator{
            typeof(data),
            typeof(Base.IteratorSize(data)),
            typeof(Base.IteratorEltype(data)),
        }
        @test iter.data === data

        data = randn(2, 3, 4)
        iter = TestIterator(data)
        @test iter isa TestIterator{
            typeof(data),
            typeof(Base.IteratorSize(data)),
            typeof(Base.IteratorEltype(data)),
        }
        @test iter.data === data

        data = randn(2, 3, 4)
        iter = TestIterator(data, Base.SizeUnknown(), Base.EltypeUnknown())
        @test iter isa TestIterator{typeof(data),Base.SizeUnknown,Base.EltypeUnknown}
    end

    @testset "iterate" begin
        data = randn(3)
        iter = TestIterator(data)

        @test iterate(iter) === iterate(data)
        _, state = iterate(data)
        @test iterate(iter, state) === iterate(data, state)
    end

    @testset "optional interface methods" begin
        data = randn(2, 3, 4)
        iter = TestIterator(data)
        @test eltype(iter) === eltype(data)
        @test length(iter) === length(data)
        @test size(iter) === size(data)

        iter = TestIterator(data, Base.HasLength(), Base.HasEltype())
        @test length(iter) === length(data)
        @test_throws MethodError size(iter)
        @test eltype(iter) === eltype(iter)

        iter = TestIterator(data, Base.SizeUnknown(), Base.EltypeUnknown())
        @test_throws MethodError length(iter)
        @test eltype(iter) === Any
    end

    @testset "==" begin
        data = randn(2, 3, 4)
        iter1 = TestIterator(data, Base.HasLength(), Base.HasEltype())
        iter2 = TestIterator(data, Base.HasLength(), Base.EltypeUnknown())
        @test iter2 != iter1

        iter3 = TestIterator(copy(data), Base.HasLength(), Base.HasEltype())
        @test iter3 == iter1
    end

    @testset "isequal" begin
        data = randn(2, 3, 4)
        iter1 = TestIterator(data, Base.HasLength(), Base.HasEltype())
        iter2 = TestIterator(data, Base.HasLength(), Base.EltypeUnknown())
        @test !isequal(iter2, iter1)

        iter3 = TestIterator(copy(data), Base.HasLength(), Base.HasEltype())
        @test isequal(iter3, iter1)
    end

    @testset "hash" begin
        data = randn(2, 3, 4)
        iter1 = TestIterator(data, Base.HasLength(), Base.HasEltype())
        iter2 = TestIterator(data, Base.HasLength(), Base.EltypeUnknown())
        @test hash(iter2) != hash(iter1)

        iter3 = TestIterator(copy(data), Base.HasLength(), Base.HasEltype())
        @test hash(iter3) == hash(iter1)
    end

    @testset "to_vec" begin
        data = randn(2, 3, 4)
        iter = TestIterator(data, Base.SizeUnknown(), Base.EltypeUnknown())
        v, back = ChainRulesTestUtils.to_vec(iter)
        @test v isa AbstractVector{eltype(data)}
        @test collect(v) == collect(vec(data))
        iter2 = back(v)
        @test iter2 == iter
    end

    @testset "rand_tangent" begin
        data = randn(2, 3, 4)
        iter = TestIterator(data, Base.SizeUnknown(), Base.EltypeUnknown())
        ∂iter = rand_tangent(iter)
        @test ∂iter isa typeof(iter)
        @test size(∂iter.data) == size(iter.data)
        @test eltype(∂iter.data) === eltype(iter.data)
    end
end


text:
###########################
# Function Cell Interface #
###########################
"""
	fa([;kwargs])

Constructs an untrained cell that when piped data inside, trains a
Factor Analysis transform. We assume that `d` is the dimesionality
of the input dataset, `n` the number of samples

# Keyword arguments (same as in `MultivariateStats`)
* `method` is the FA method used `:em` and `:cm` (default `:cm`)
* `maxoutdim` is the number of output dimensions, default `d-1`
* `mean` is the mean vector, can be `nothing`, `0` or precomputed mean vector (default `nothing`)
* `tol` is the convergence tolerance (default `1.0e-6`)
* `tot` is the maximum number of iterations (default `1000`)
* `η` is the variabce low bound (default `1.0e-6`)

Read the `MultivariateStats.jl` documentation for more information.
"""
fa(;kwargs...) = FunctionCell(fa, (), ModelProperties(), kwtitle("Factor Analysis", kwargs); kwargs...)

############################
# DataCell/Array Interface #
############################
"""
	fa(x, [;kwargs])

Trains a function cell that when piped data into will compute the principal components of
the observations based on the projection matrix extracted from `x`.
"""
# Training
fa(x::T where T<:CellData; kwargs...) = fa(getx!(x); kwargs...)
fa(x::T where T<:AbstractVector; kwargs...) = fa(mat(x, LearnBase.ObsDim.Constant{2}()); kwargs...)
fa(x::T where T<:AbstractMatrix; kwargs...)  = begin

	fadata = fit(MultivariateStats.FactorAnalysis, getobs(x); kwargs...)

	# Build model properties
	modelprops = ModelProperties(length(fadata.mean),size(fadata.W,2))

	# Returned trained cell
	FunctionCell(fa, Model(fadata, modelprops), kwtitle("Factor Analysis",kwargs))
end

# Execution
fa(x::T where T<:CellData, model::Model{<:MultivariateStats.FactorAnalysis}) =
	datacell(fa(getx!(x), model), gety(x))
fa(x::T where T<:AbstractVector, model::Model{<:MultivariateStats.FactorAnalysis}) =
	fa(mat(x, LearnBase.ObsDim.Constant{2}()), model)
fa(x::T where T<:AbstractMatrix, model::Model{<:MultivariateStats.FactorAnalysis}) =
	MultivariateStats.transform(model.data, getobs(x))

"""
	far(M::Cell), where `M=fa(x)`

Trains a cell based on a previously trained Factor Analysis function cell. When piped data into,
the returned output will be the reconstruction of the observations in the original
space.

Read the `MultivariateStats.jl` documentation for more information.

# Examples
```
julia> using j4pr;

julia> F = randn(5,1000) |> fa(maxoutdim=2)
Factor Analysis: maxoutdim=2, 5->2, trained

julia> FR = far(F)
Factor Analysis: reconstruct, 2->5, trained

julia> ([1.0 2 3 4 5]' |> P) |> PR
5×1 Array{Float64,2}:
 0.45788
 2.09689
 3.04864
 3.9806
 4.998
```
"""
# Training for the reconstruction tranform (fixed cell)
far(x::T where T<:CellFunT{<:Model{<:MultivariateStats.FactorAnalysis}}) = begin

	# Build model properties
	modelprops = ModelProperties(getx!(x).properties.odim, getx!(x).properties.idim)

	FunctionCell(far, Model(getx(x).data, modelprops), "Factor Analysis: reconstruct")
end

# Execution
far(x::T where T<:CellData, model::Model{<:MultivariateStats.FactorAnalysis}) =
	datacell(far(getx!(x), model), gety(x))
far(x::T where T<:AbstractVector, model::Model{<:MultivariateStats.FactorAnalysis}) =
	far(mat(x, LearnBase.ObsDim.Constant{2}()), model)
far(x::T where T<:AbstractMatrix, model::Model{<:MultivariateStats.FactorAnalysis})::Matrix{Float64} =
	MultivariateStats.reconstruct(model.data, getobs(x))


text:
# This file contains funtions and help functions for the ABC-MCMC algorithm and the
# DA-ABC-MCMC algorithm.

# Load packages:

using Distributions
using DataFrames
using StatsBase
using PyPlot
using KernelDensity

include(pwd()*"/adaptive updating algorithms/adaptiveupdate.jl")

# Types:

"Type for prior distribution"
type PriorDistribution
  dist::String
  prior_parameters::Array{Float64}
end

"Type for the data"
type Data
  y::Array{Float64}
end

"Pararameters for the model"
type ModelParameters
  theta_true::Array{Float64} # [log(r) log(phi) log(sigma)]
  theta_known::Array{Float64} # NaN
  theta_0::Array{Float64} # [log(r_0) log(phi_0) log(sigma_0)]
end

"type for the algorithm parameters for the ABC-MCMC algorithm"
type AlgorithmParametersABCMCMC
  R::Int64
  burn_in::Int64
  alg::String
  print_interval::Int64
  nbr_summary_stats::Int64
  eps::Vector
  start::String
  w::Vector
end

"Type for the problem (including algorithm parameters) for the PMCMC algorithm"
type ProblemABCMCMC
  data::Data
  alg_param::AlgorithmParametersABCMCMC
  model_param::ModelParameters
  adaptive_update::AdaptationAlgorithm
  prior_dist::PriorDistribution
  parameter_transformation::String
  abc_kernel::String
  abcmcmctype::String
end

"Type for the results"
type Result
  Theta_est::Array{Float64}
  accept_vec::Array{Float64}
  prior_vec::Array{Float64}
end

# Algorithms:

doc"""
    abcmcmc(problem::ProblemABCMCMC, sample_from_prior, evaluate_prior, generate_data, calc_summary)

The ABC-MCMC algorithm.
"""
function abcmcmc(problem::ProblemABCMCMC, sample_from_prior, evaluate_prior, generate_data, calc_summary)

  # data
  y = problem.data.y
  y_star = copy(y)
  length_data = length(y) # length of data set

  # algorithm parameters
  R = problem.alg_param.R # number of iterations
  burn_in = problem.alg_param.burn_in # burn in
  print_interval = problem.alg_param.print_interval # print accaptance rate and covarince function ever print_interval:th iteration
  start = problem.alg_param.start

  # model parameters
  theta_true = problem.model_param.theta_true # [log(r) log(phi) log(sigma)]
  theta_known = problem.model_param.theta_known # NaN
  theta_0 = problem.model_param.theta_0 # [log(r_0) log(phi_0) log(sigma_0)]

  # abc kernel
  abc_kernel = problem.abc_kernel

  # transformation of parameter space
  parameter_transformation = problem.parameter_transformation

  # ABC MCMC type
  abcmcmctype  = problem.abcmcmctype

  # ABC parameters
  nbr_summary_stats = problem.alg_param.nbr_summary_stats
  s = zeros(nbr_summary_stats)
  s_star = zeros(nbr_summary_stats)
  eps = problem.alg_param.eps
  w =  problem.alg_param.w
  s_matrix = zeros(length(s_star), R)

  # pre-allocate matricies and vectors
  Theta = zeros(length(theta_0),R)
  loglik = zeros(R)
  accept_vec = zeros(R)
  prior_vec = zeros(R)
  theta_star = zeros(length(theta_0))
  a_log = 0.

  # training data
  X = zeros(length(theta_0),R)
  Y = zeros(R)

  # parameters for adaptive update
  adaptive_update_params = set_adaptive_alg_params(problem.adaptive_update, length(theta_0),Theta[:,1], R)

  # parameters for prior dist
  dist_type = problem.prior_dist.dist
  prior_parameters = problem.prior_dist.prior_parameters

  # print information at start of algorithm
  @printf "Starting ABC-MCMC estimating %d parameters\n" length(theta_true)
  @printf "Adaptation algorithm: %s\n" typeof(problem.adaptive_update)
  @printf "Prior distribution: %s\n" problem.prior_dist.dist

  # first iteration
  @printf "Iteration: %d\n" 1
  @printf "Covariance:\n"
  print_covariance(problem.adaptive_update,adaptive_update_params, 1)

  s = calc_summary(y,y)

  # initial iteration
  if start == "random"
    while true
      theta_star = sample_from_prior(problem.prior_dist)
      y_star = generate_data(length_data, theta_star,theta_known)
      s_star = calc_summary(y_star,y)
      if ρ(s,s_star,w) < eps[1]
        break
      end
    end
  else
    theta_star = theta_0
  end

  dist_vec = zeros(R)
  ind_vec = zeros(R)
  Theta[:,1] = theta_star
  accept_vec[1] = 1

  @printf "Start value:\n"
  show(theta_star)

  X[:,1] = theta_star
  Y[1] = 1

  s_matrix[:,1] = calc_summary(y_star,y)

  for r = 2:R

    # set print_on to false, only print each print_interval:th iteration
    print_on = false

    # print acceptance rate for the last print_interval iterations
    if mod(r-1,print_interval) == 0
      print_on = true # print ESS and Nbr resample each print_interval:th iteration
      # print progress
      @printf "Percentage done: %.2f %% \n" 100*(r-1)/R
      # print accaptace rate
      @printf "Acceptance rate on iteration %d to %d is %.4f %% \n" r-print_interval r-1  (sum(accept_vec[r-print_interval:r-1])/( r-1 - (r-print_interval) ))*100
      # print covaraince function
      @printf "Covariance:\n"
      print_covariance(problem.adaptive_update,adaptive_update_params, r)
      # print threshold
      @printf "Threshold: %.4f \n" eps[r]
    end

    # Gaussian random walk
    (theta_star, ) = gaussian_random_walk(problem.adaptive_update, adaptive_update_params, Theta[:,r-1], r)

    # store theta_star
    X[:,r] = theta_star

    # compute accaptance probability

    # Generate data
    y_star = generate_data(length_data, theta_star, theta_known)

    # Compute summary stats
    s_star = calc_summary(y_star,y)

    abc_likelihood_star = abckernel(s,s_star,w,eps[r], abc_kernel)
    abc_likelihood_old = abckernel(s,s_matrix[:,r-1],w,eps[r], abc_kernel)

    jacobian_log_star = jacobian(theta_star, parameter_transformation)
    jacobian_log_old = jacobian(Theta[:,r-1], parameter_transformation)

    prior_log_star = evaluate_prior(theta_star,prior_parameters)
    prior_log_old = evaluate_prior(Theta[:,r-1],prior_parameters)

    if abcmcmctype == "general"
      a_log = log(abc_likelihood_star) + prior_log_star +  jacobian_log_star - (log(abc_likelihood_old) +  prior_log_old + jacobian_log_old)
    else abcmcmctype == "original" && abc_kernel == "uniform"
      a_log = log(abc_likelihood_star) + prior_log_star +  jacobian_log_star - (prior_log_old + jacobian_log_old)
    end

    #a_log = log(abc_likelihood_star) + prior_log_star  - (log(abc_likelihood_old) +  prior_log_old)

    u_log = log(rand())
    accept =  u_log < a_log
    Y[r] = abc_likelihood_star # store val for likelihood approx. from the ABC kernel

    # update chain
    if accept # the proposal is accapted
      Theta[:,r] = theta_star # update chain with new proposals
      accept_vec[r] = 1
      s_matrix[:,r] = s_star
    else
      s_matrix[:,r] = s_matrix[:,r-1]
      dist_vec[r] = dist_vec[r-1]
      ind_vec[r] = ind_vec[r-1]
      Theta[:,r] = Theta[:,r-1] # keep old values
    end

    # adaptation of covaraince matrix for the proposal distribution
    adaptation(problem.adaptive_update, adaptive_update_params, Theta, r,a_log)

  end

  @printf "Ending ABC-MCMC with adaptive RW estimating %d parameters\n" length(theta_true)
  @printf "Adaptation algorithm: %s\n" typeof(problem.adaptive_update)
  @printf "Prior distribution: %s\n" problem.prior_dist.dist

  cov_prop_kernel = get_covariance(problem.adaptive_update,adaptive_update_params, R)

  return return_results(Theta,accept_vec,prior_vec, problem,adaptive_update_params),X,Y, cov_prop_kernel, s_matrix

end

# Help functions:

doc"""
    log_unifpdf(x::Float64, a::Float64,b::Float64)

Computes log(unifpdf(x,a,b)).
"""
function log_unifpdf(x::Float64, a::Float64, b::Float64)

  if  x >= a && x<= b
    return -log(b-a);
  else
    return log(0);
  end

end

doc"""
    jacobian(theta::Vector, parameter_transformation::String)

Returnes log-Jacobian for transformation of parameter space.
"""
function jacobian(theta::Vector, parameter_transformation::String)

  if parameter_transformation == "none"
    return 0.
  elseif parameter_transformation == "log"
    return sum(theta)
  end

end

function abckernel(s::Vector, s_star::Vector, w::Vector, eps::Float64, abc_kernel::String)

  if abc_kernel == "uniform"
    return indacator(s , s_star, w, eps)
  elseif abc_kernel == "Gauss"
    error("The Gaussian kernel is not implemented")
  end

end

doc"""
    indacator(s::Vector, s_star::Vector, w::Vector, eps::Float64)

Indacator function for the ABC-MH updating step.
"""
function indacator(s::Vector, s_star::Vector, w::Vector, eps::Float64)

  if ρ(s,s_star,w) < eps
    return 1.
  else
    return 0.
  end

end

doc"""
    ρ(s::Vector, s_star::Vector, w::Vector)

Distance function.
"""
function ρ(s::Vector, s_star::Vector, w::Vector)

  Δs =  (s_star-s)
  dist = Δs'*inv(diagm(w.^2))*Δs
  return sqrt(dist[1])

end

doc"""
    ρ(s::Vector, s_star::Vector)

Distance function (w constant).
"""
function ρ(s::Vector, s_star::Vector)

  ρ(s, s_star, ones(length(s)))

end

doc"""
   predict(beta::Vector, theta::Vector)

Prediction for the probit regression model at theta.
"""
function predict(beta::Vector, theta::Vector)

  p_hat =  exp(theta'*beta)./(1+exp(theta'*beta))
  return p_hat[1]

end

doc"""
    return_results(Theta,loglik,accept_vec,prior_vec, problem,adaptive_update_params)

Constructs the return type of the resutls from the MCWM and PMCMC algorithm.
"""
function return_results(Theta,accept_vec,prior_vec)

    return Result(Theta, accept_vec, prior_vec)

end

doc"""
    return_results(Theta,loglik,accept_vec,prior_vec, problem,adaptive_update_params)

Constructs the return type of the resutls from the MCWH and PMCMC algorithm.
"""
function return_results(Theta,accept_vec,prior_vec, problem,adaptive_update_params)

  if typeof(problem.adaptive_update) == AMUpdate_gen
    return (Result(Theta, loglik, accept_vec, prior_vec), adaptive_update_params[6])
  else
    return Result(Theta, accept_vec, prior_vec)
  end

end

doc"""
    calc_mad(s::Matrix)
"""
function calc_mad(s::Matrix)

  nrow = size(s,1)
  w = zeros(nrow)

  for i = 1:nrow
    w[i] = mad(s[i,:])
  end

  return w

end

doc"""
    stratresample(p , N)

Stratified resampling.

Sample N times with repetitions from a distribution on [1:length(p)] with probabilities p. See [link](http://www.cornebise.com/julien/publis/isba2012-slides.pdf).
"""
function stratresample(p , N)

  p = p/sum(p)  # normalize, just in case...

  cdf_approx = cumsum(p)
  cdf_approx[end] = 1
  #I = zeros(N,1)
  indx = zeros(Int64, N)
  U = rand(N,1)
  U = U/N + (0:(N - 1))/N
  index = 1
  for k = 1:N
    while (U[k] > cdf_approx[index])
      index = index + 1
    end
    indx[k] = index
  end

  return indx

end

max_stars_repo_path: ABC algorithms/abc.jl

max_stars_repo_name: SamuelWiqvist/abcmcmcrickermodel

max_issues_repo_path: ABC algorithms/abc.jl

max_issues_repo_name: SamuelWiqvist/abcmcmcrickermodel

max_forks_repo_path: ABC algorithms/abc.jl

max_forks_repo_name: SamuelWiqvist/abcmcmcrickermodel


text:
# This file is a part of Julia. License is MIT: https://julialang.org/license

module TestPinv

using Test, LinearAlgebra, Random

Random.seed!(12345)

function hilb(T::Type, n::Integer)
    a = Matrix{T}(undef, n, n)
    for i=1:n
        for j=1:n
            a[j,i]=one(T)/(i+j-one(T))
        end
    end
    return a
end
hilb(n::Integer) = hilb(Float64,n)

function hilb(T::Type, m::Integer, n::Integer)
    a = Matrix{T}(undef, m, n)
    for i=1:n
        for j=1:m
            a[j,i]=one(T)/(i+j-one(T))
        end
    end
    return a
end
hilb(m::Integer, n::Integer) = hilb(Float64,m,n)

function onediag(T::Type, m::Integer, n::Integer)
    a=zeros(T,m,n)
    for i=1:min(n,m)
        a[i,i]=one(T)/(float(i)^5)
    end
    a[1,1] = 0
    a[min(m,n),min(m,n)] = 0
    return a
end
onediag(m::Integer, n::Integer) = onediag(Float64, m::Integer, n::Integer)

function onediag_sparse(T::Type, n::Integer)
    a=zeros(T,n)
    for i=1:n
        a[i]=one(T)/(float(i)^5)
    end
    a[1] = 0
    a[n] = 0
    return Diagonal(a)
end
onediag_sparse(n::Integer) = onediag_sparse(Float64, n::Integer)

function tridiag(T::Type, m::Integer, n::Integer)
    a=zeros(T,m,n)
    for i=1:min(n,m)
        a[i,i]=one(T)/(float(i)^5)
    end
    for i=1:min(n,m)-1
        a[i+1,i]=2*one(T)/(float(i)^5)
        a[1,i+1]=2*one(T)/(float(i)^5)
    end
    return a
end
tridiag(m::Integer, n::Integer) = tridiag(Float64, m::Integer, n::Integer)

function test_pinv(a,tol1,tol2)
    m,n = size(a)

    apinv = @inferred pinv(a)
    @test size(apinv) == (n,m)
    @test norm(a*apinv*a-a)/norm(a) ≈ 0 atol=tol1
    @test norm(apinv*a*apinv-apinv)/norm(apinv) ≈ 0 atol=tol1
    b = a*randn(n)
    x = apinv*b
    @test norm(a*x-b)/norm(b) ≈ 0 atol=tol1

    apinv = @inferred pinv(a,sqrt(eps(real(one(eltype(a))))))
    @test size(apinv) == (n,m)
    @test norm(a*apinv*a-a)/norm(a) ≈ 0 atol=tol2
    @test norm(apinv*a*apinv-apinv)/norm(apinv) ≈ 0 atol=tol2
    b = a*randn(n)
    x = apinv*b
    @test norm(a*x-b)/norm(b) ≈ 0 atol=tol2
end

@testset for eltya in (Float32, Float64, ComplexF32, ComplexF64)
    @testset for (m, n) in [(1000, 100), (100, 100), (100, 1000)]
        default_tol = (real(one(eltya))) * max(m,n) * 10
        tol1 = 1e-2
        tol2 = 1e-5
        if real(eltya) == Float32
            tol1 = 1e0
            tol2 = 1e-2
        end
        @testset "dense/ill-conditioned matrix" begin
            a = hilb(eltya, m, n)
            test_pinv(a, tol1, tol2)
        end
        @testset "dense/diagonal matrix" begin
            a = onediag(eltya, m, n)
            test_pinv(a, default_tol, default_tol)
        end
        @testset "dense/tri-diagonal matrix" begin
            a = tridiag(eltya, m, n)
            test_pinv(a, default_tol, tol2)
        end
        @testset "Diagonal matrix" begin
            a = onediag_sparse(eltya, m)
            test_pinv(a, default_tol, default_tol)
        end
        @testset "Vector" begin
            a = rand(eltya, m)
            apinv = @inferred pinv(a)
            @test pinv(hcat(a)) ≈ apinv
            @test isa(apinv, eltya <: Complex ? Adjoint{eltya} : Transpose{eltya})
        end
        @testset "Adjoint/Transpose vector" begin
            a = rand(eltya, m)'
            apinv = @inferred pinv(a)
            @test pinv(vcat(a)) ≈ apinv
            @test apinv isa Vector{eltya}
        end
    end

    @testset "zero valued numbers/vectors/matrices" begin
        a = pinv(zero(eltya))
        @test a ≈ 0.0

        a = pinv([zero(eltya); zero(eltya)])
        @test a[1] ≈ 0.0
        @test a[2] ≈ 0.0

        a = pinv([zero(eltya); zero(eltya)]')
        @test a[1] ≈ 0.0
        @test a[2] ≈ 0.0

        a = pinv(Diagonal([zero(eltya); zero(eltya)]))
        @test a.diag[1] ≈ 0.0
        @test a.diag[2] ≈ 0.0
    end

    @testset "hermitian matrices" begin
        Q = ones(2,2)
        C = pinv(Hermitian(Q))/0.25
        @test C ≈ ones(2,2)
    end

    @testset "non-square diagonal matrices" begin
        A = eltya[1 0 ; 0 1 ; 0 0]
        B = pinv(A)
        @test A*B*A ≈ A
        @test B*A*B ≈ B

        A = eltya[1 0 0 ; 0 1 0]
        B = pinv(A)
        @test A*B*A ≈ A
        @test B*A*B ≈ B
    end

    if eltya <: LinearAlgebra.BlasReal
        @testset "sub-normal numbers/vectors/matrices" begin
            a = pinv(floatmin(eltya)/100)
            @test a ≈ 0.0
            # Complex subnormal
            a = pinv(floatmin(eltya)/100*(1+1im))
            @test a ≈ 0.0

            a = pinv([floatmin(eltya); floatmin(eltya)]/100)
            @test a[1] ≈ 0.0
            @test a[2] ≈ 0.0
            # Complex subnormal
            a = pinv([floatmin(eltya); floatmin(eltya)]/100*(1+1im))
            @test a[1] ≈ 0.0
            @test a[2] ≈ 0.0
            a = pinv(Diagonal([floatmin(eltya); floatmin(eltya)]/100))
            @test a.diag[1] ≈ 0.0
            @test a.diag[2] ≈ 0.0
            # Complex subnormal
            a = pinv(Diagonal([floatmin(eltya); floatmin(eltya)]/100*(1+1im)))
            @test a.diag[1] ≈ 0.0
            @test a.diag[2] ≈ 0.0
        end
    end
end

end # module TestPinv

max_stars_repo_name: SmiteWindows/julia

max_issues_repo_name: SmiteWindows/julia

max_forks_repo_name: SmiteWindows/julia


text:
## Different question types
using JuliaBlackBoard

# used to create .txt file with same basename and same directory as script
OPEN = JuliaBlackBoard.OPEN(@__FILE__)

OPEN() do io

    q = mt"""
Simple **markdown** and $x^2$
"""

    qt = MC
    q = "Not no"
    answer = ("Yes" => true, "No" => false)
    question(io, qt, q, answer)

    qt = MA
    q = "Not no"
    answer =  ("Yes" => true, "No" => false, "Maybe"=>true)
    question(io, qt, q, answer)

    qt = TF
    q = mt"""Not false"""
    answer = true
    question(io, qt, q, answer)

    qt = ESS
    q = "Essay"
    answer = nothing
    question(io, qt, q, answer)

    qt = ORD
    q = "Sort in order"
    answer = ("one", "two", "three")
    question(io, qt, q, answer)

    qt = MAT
    q = "Match spelling with number"
    answer=("one"=>"1", "two"=>"2", "three"=>"3")
    question(io, qt, q, answer)

    qt = FIB
    q = mt"""Positive integer less than $2^2$ (spell)"""
    answer=("one", "two", "three")
    question(io, qt, q,  answer)

    qt = FIB_PLUS
    q = mt"""[var1] and [var2] years ago, the founding fathers..."""
    answer=("var1"=>("four", "Four", "4"), "var2"=>("seven", "y"))
    question(io, qt, q, answer)

    qt = FIL
    q = "file upload"
    answer=nothing
    question(io, qt, q, answer)

    qt = SR
    q = "short response"
    answer="short prompt"
    question(io, qt, q, answer)

    qt = OP
    q = "Rate this"
    answer=nothing
    question(io, qt, q, answer)

    qt = NUM
    q = mt"""The number $1$"""
    answer = 1
    question(io, qt, q, answer)

    qt = NUM
    q = mt"""A number in $[0.9, 1.1]$"""
    answer = (1, 0.1)
    question(io, qt, q, answer...)

    qt = JUMBLED_SENTENCE
    qq = "The [var1] brown [var2] jumped over the lazy [var3]"
    answer = ("var1"=>"quick", "var2"=>"fox", "var3"=>"dog", nothing=>("cat", "slow", "fast"))
    question(io, qt, qq, answer)
end


text:
#
# Insert Sort
#

function insertSort(A)

  for pos in 2:length(A)
    insert(A, pos, A[pos])
  end

end

function insert(A, pos, value)

  i = pos - 1
  while i >= 1 && A[i] > value
    A[i+1] = A[i]
    i = i - 1
  end
  A[i+1] = value

end

function main()

  # list to be sorted
  rng = MersenneTwister(1234);
  A = shuffle(rng, Vector(1:100))
  Aorigin = copy(A)

  # sorting
  @time insertSort(A)

  print("The original list: ")
  println(Aorigin)

  print("The sorted list: ")
  println(A)

end

if contains(@__FILE__, PROGRAM_FILE)
    main()
end

max_stars_repo_name: szmlb/algorithmsInJulia

max_issues_repo_name: szmlb/algorithmsInJulia

max_forks_repo_name: szmlb/algorithmsInJulia