text:
using DiffEqBase, OrdinaryDiffEq

export simulate, simulateinput

################################################################################
# State trajectory
################################################################################
"simulate(data, tau, t)"
function simulate(data::ScSTOdata, tau::Vector{Float64}, t::Vector{Float64})
    Q = data.Q[1:end-1, 1:end-1]
    E = data.E[1:end-1, 1:end-1]
    x0 = data.x0[1:end-1]
    xsim, xswt, cost = simulateCore(data.dynam, tau, x0, Q, E, data.U, t)
    return xsim, xswt, cost, t
end

"simulate(prob, tau, t)"
function simulate(p::ScSTOptiModel, tau::Vector{Float64}, t::Vector{Float64})
    return simulate(p.data, tau, t)
end

"simulate(prob, tau, [length])"
function simulate(p::ScSTOptiModel, tau::Vector{Float64}; length::Int = 1000)
    @assert 1 < length
    t = collect(range(p.data.t0, stop = p.data.tf, length = length))
    return simulate(p, tau, t)
end

################################################################################
# Control trajectory
################################################################################
"simulateinput(data, tau, t)"
function simulateinput(data::ScSTOdata, tau::Vector{Float64}, t::Vector{Float64})
    u = computeinput(tau, data.U, t)
    return u, t
end

"simulateinput(prob, tau, t)"
function simulateinput(p::ScSTOptiModel, tau::Vector{Float64}, t::Vector{Float64})
    return simulateinput(p.data, tau, t)
end

"simulateinput(prob, tau, [length])"
function simulateinput(p::ScSTOptiModel, tau::Vector{Float64}; length::Int = 1000)
    @assert 1 < length
    t = collect(range(p.data.t0, stop = p.data.tf, length = length))
    return simulateinput(p, tau, t)
end

################################################################################
# LOWER LEVEL FUNCTIONS
################################################################################
"""
Core function for simulating evolution of switched nonlinear dynamics
    xsim, xswt, cost = simulateCore(dynam, tau, x0, Q, E, U, t)
"""
function simulateCore(
    dynam::Function,
    tau::Vector{Float64},
    x0::Vector{Float64},
    Q::Matrix{Float64},
    E::Matrix{Float64},
    U::Matrix{Float64},
    t::Vector{Float64},
)
    nx = length(x0)  # Number of States
    N = length(tau)  # Number of switches
    nt = length(t)
    tau = [t[1]; tau; t[end]]  # Extend tau vector to simplify numbering
    xsim = zeros(nx, length(t)) # allocation
    xsim[:, 1] = x0 # initial state
    # Define indeces to determine current switching mode
    tmpInd1 = 1
    tmpInd2 = 1
    xprevSwitch = x0
    # Create Vector of Points at the switching instants
    xswt = zeros(nx, N + 1)
    xswt[:, 1] = x0
    # ODE solver
    odes = OrdinaryDiffEq.Tsit5()
    for i = 1:N+1 # Integrate over all the intervals
        # redefine Dynamic function
        nldyn(x, p, t) = dynam(x, U[:, i])
        while t[tmpInd2] < tau[i+1] && tmpInd2 < nt
            tmpInd2 = tmpInd2 + 1  # Increase time index
        end
        if tmpInd2 > tmpInd1
            # ODE problem
            odep = DiffEqBase.ODEProblem(nldyn, xprevSwitch, (t[tmpInd1], t[tmpInd2]))
            # ODE solution
            if tmpInd2 == tmpInd1 + 1
                xsol = DiffEqBase.solve(odep, odes; save_everystep = false)
                xtmp = [xsol[1] xsol[end]]
            else
                xsol = DiffEqBase.solve(odep, odes; saveat = t[tmpInd1:tmpInd2])
                xtmp = hcat((xsol[i] for i in eachindex(xsol))...)
            end
            xsim[:, tmpInd1:tmpInd2] = xtmp
            # update
            xprevSwitch = xsim[:, tmpInd2]
            tmpInd1 = tmpInd2
            if i < N + 1
                xswt[:, i+1] = xsim[:, tmpInd2]
            end
        end
    end
    # smooth objective
    cost = trapz(t, diag(xsim' * Q * xsim)) + (xsim[:, end]'*E*xsim[:, end])[1]
    return xsim, xswt, cost
end

"""
Compute Actual Inputs from Artificial Ones
    usim = computeinput(tau, U, t)
"""
function computeinput(tau::Vector{Float64}, U::Matrix{Float64}, t::Vector{Float64})
    N = length(tau) + 1
    nu = size(U, 1) # U [nu x (N+1)]
    nt = length(t)
    tauexp = [t[1]; tau; t[end]] # expand [N+2]
    usim = zeros(nu, length(t)) # allocation [nu x nt]
    # Define indeces to span time vector
    tmpInd1 = 1
    tmpInd2 = 1
    for i = 1:N  # Iterate over all intervals
        while t[tmpInd2] < tauexp[i+1] && tmpInd2 < nt
            tmpInd2 += 1
        end
        if tmpInd2 > tmpInd1
            usim[:, tmpInd1:tmpInd2] = repeat(U[:, i], 1, tmpInd2 - tmpInd1 + 1)
            tmpInd1 = tmpInd2
        end
    end
    return usim
end

"""
Trapezoidal integration rule
    trapz(x, y)
"""
function trapz(x::Vector{Float64}, y::Vector{Float64})
    n = length(x)
    @assert length(y) == n
    r = 0.0
    if n == 1
        return r
    else
        for i = 2:n
            r += (x[i] - x[i-1]) * (y[i] + y[i-1])
        end
        r *= 0.5
    end
    return r
end


text:
using MPI
using CLIMA
using CLIMA.Mesh.Topologies
using CLIMA.Mesh.Grids
using CLIMA.DGmethods
using CLIMA.DGmethods.NumericalFluxes
using CLIMA.MPIStateArrays
using CLIMA.ODESolvers
using CLIMA.GenericCallbacks
using CLIMA.VariableTemplates: flattenednames
using CLIMA.ShallowWater
using LinearAlgebra
using StaticArrays
using Logging, Printf, Dates
using CLIMA.VTK
using CLIMA.PlanetParameters: grav
import CLIMA.ShallowWater: shallow_init_state!, shallow_init_aux!, vars_state, vars_aux,
                    shallow_boundary_state!, TurbulenceClosure, LinearDrag,
                    ConstantViscosity, AdvectionTerm, NonLinearAdvection

struct GyreInABox{T} <: SWProblem
  τₒ::T
  fₒ::T # value includes τₒ, g, and ρ
  β::T
  Lˣ::T
  Lʸ::T
  H::T
end

function null_init_state!(::GyreInABox, ::TurbulenceClosure, state,
                          aux, coord, t)
  T = eltype(state.U)
  state.U = @SVector zeros(T, 3)
  state.η = 0
  return nothing
end

η_lsw(x, y, t) =            cos(π*x) * cos(π*y) * cos(√2*π*t)
u_lsw(x, y, t) = 2^(-0.5) * sin(π*x) * cos(π*y) * sin(√2*π*t)
v_lsw(x, y, t) = 2^(-0.5) * cos(π*x) * sin(π*y) * sin(√2*π*t)

function lsw_init_state!(p::GyreInABox, ::TurbulenceClosure, state,
                         aux, coords, t)
  state.U = @SVector [u_lsw(coords[1], coords[2], t),
                      v_lsw(coords[1], coords[2], t),
                      0]

  state.η = η_lsw(coords[1], coords[2], t)

  return nothing
end

v_lkw(x, y, t) = 0
u_lkw(x, y, t) = exp(-0.5 * y^2) * exp(-0.5 * (x - t + 5)^2)
η_lkw(x, y, t) = 1 + u_lkw(x, y, t)

function lkw_init_state!(p::GyreInABox, ::TurbulenceClosure, state,
                         aux, coords, t)
  state.U = @SVector [u_lkw(coords[1], coords[2], t),
                      v_lkw(coords[1], coords[2], t),
                      0]

  state.η = η_lkw(coords[1], coords[2], t)

  return nothing
end

R₋(ϵ)    = (-1 - sqrt(1 + (2 * π * ϵ) ^ 2)) / (2ϵ)
R₊(ϵ)    = (-1 + sqrt(1 + (2 * π * ϵ) ^ 2)) / (2ϵ)
D(ϵ)     = (R₊(ϵ) * (exp(R₋(ϵ)) - 1) + R₋(ϵ) * (1 - exp(R₊(ϵ)))) / (exp(R₊(ϵ)) - exp(R₋(ϵ)))
R₂(x₁,ϵ) = (1 / D(ϵ)) * (((R₊(ϵ) * (exp(R₋(ϵ)) - 1)) * exp(R₊(ϵ) * x₁) + (R₋(ϵ) * (1 - exp(R₊(ϵ)))) * exp(R₋(ϵ) * x₁)) / (exp(R₊(ϵ)) - exp(R₋(ϵ))))
R₁(x₁,ϵ) = (π / D(ϵ)) * (1 .+ ((exp(R₋(ϵ)) - 1) * exp(R₊(ϵ) * x₁) .+ (1 - exp(R₊(ϵ))) * exp(R₋(ϵ) * x₁)) / (exp(R₊(ϵ)) - exp(R₋(ϵ))))

𝒱(x₁,y₁,ϵ)       =  R₂(x₁,ϵ) * sin.(π * y₁)
𝒰(x₁,y₁,ϵ)       = -R₁(x₁,ϵ) * cos.(π * y₁)
ℋ(x₁,y₁,ϵ,βᵖ,fₒ,γ) = (R₂(x₁,ϵ) / (π * fₒ)) * γ * cos(π * y₁) + (R₁(x₁,ϵ) / π) * (sin(π * y₁) * (1.0 + βᵖ * (y₁ - 0.5)) + (βᵖ / π) * cos(π * y₁))

function gyre_init_state!(p::GyreInABox, T::LinearDrag, state,
                             aux, coords, t)
  τₒ = p.τₒ
  fₒ = p.fₒ
  β  = p.β
  Lˣ = p.Lˣ
  Lʸ = p.Lʸ
  H  = p.H

  γ  = T.λ

  βᵖ = β * Lʸ / fₒ
  ϵ  = γ / (Lˣ * β)

  uˢ(ϵ) = (τₒ * D(ϵ)) / (H * γ * π)
  hˢ(ϵ) = (fₒ * Lˣ * uˢ(ϵ)) / grav

  u = uˢ(ϵ) * 𝒰(coords[1]/Lˣ, coords[2]/Lʸ, ϵ)
  v = uˢ(ϵ) * 𝒱(coords[1]/Lˣ, coords[2]/Lʸ, ϵ)
  h = hˢ(ϵ) * ℋ(coords[1]/Lˣ, coords[2]/Lʸ, ϵ, βᵖ, fₒ, γ)

  state.U = @SVector [H * u, H * v, 0]

  state.η = h

  return nothing
end

t1(x, δᵐ)    = cos((√3*x)/(2*δᵐ)) + (√3^-1) * sin((√3*x)/(2*δᵐ))
t2(x, δᵐ)    = 1 - exp((-x)/(2*δᵐ) ) * t1(x, δᵐ)
t3(y, Lʸ)    = π * sin(π*y/Lʸ)
t4(x, Lˣ, C) = C * (1-x/Lˣ)

η_munk(x, y, Lˣ, Lʸ, δᵐ, C) = t4(x, Lˣ, C) * t3(y, Lʸ) * t2(x, δᵐ)

function gyre_init_state!(p::GyreInABox, V::ConstantViscosity, state, aux,
                          coords, t)
  T = eltype(state.U)

  τₒ = p.τₒ
  fₒ = p.fₒ
  β  = p.β
  Lˣ = p.Lˣ
  Lʸ = p.Lʸ
  H  = p.H

  ν  = V.ν

  δᵐ = (ν / β)^(1/3)
  C  = τₒ / (grav*H) * (fₒ/β)

  state.η = η_munk(coords[1], coords[2], Lˣ, Lʸ, δᵐ, C)
  state.U = @SVector zeros(T, 3)

  return nothing
end

ext: jl

max_stars_repo_name: favba/CLIMA

max_issues_repo_name: favba/CLIMA

max_forks_repo_name: favba/CLIMA


text:
using Roots

function polynomialGenerator(a...)
    n = length(a)-1
    poly =  function(x)
                return sum([a[i+1]*x^i for i in 0:n])
            end
    return poly
end

polynomial = polynomialGenerator(1,3,-10)
zeroVals = find_zeros(polynomial,-10,10)
println("Zeros of the function f(x): ", zeroVals)


text:
using Graphics: @mustimplement
using Unitful

export Robot, RobotState, getPosition, dof, state, isReferenced, moveAbs, moveRel, movePark, enable, disable, reset, setup, doReferenceDrive, axisRange, defaultVelocity
export teachNamedPosition, gotoPos, exportNamedPositions, namedPositions, namedPosition, getRobot, getRobots, checkPosition
export ScannerCoords, RobotCoords, getPositionScannerCoords, scannerCoordAxes, scannerCoordOrigin, toRobotCoords, moveRobotOrigin, moveScannerOrigin
export coordinateSystem

@enum RobotState INIT DISABLED READY MOVING ERROR

abstract type Robot <: Device end

struct RobotCoords{T<:Unitful.Length} <: DeviceCoords{T}
  data::Vector{T}
end

function prepareRobotDict(dict::Dict)
  if haskey(dict, "coordinateSystem")
    coordDict = dict_to_splatting(pop!(dict, "coordinateSystem"))

    origin = nothing
    if haskey(coordDict, :origin)
      origin = coordDict[:origin]
    end
    axes = nothing
    if haskey(coordDict, :axes)
      axes = coordDict[:axes]
    end

    coordSystem = ScannerCoordinateSystem(axes, origin)
    dict["coordinateSystem"] = coordSystem
  end
  return dict
end

Base.getindex(c::Union{ScannerCoords,RobotCoords}, i) = getindex(c.data, i)
Base.size(c::Union{ScannerCoords,RobotCoords}) = size(c.data)
Base.setindex!(c::Union{ScannerCoords,RobotCoords}, v, i) = setindex!(c.data,v,i)

# general interface functions to be implemented by devices
@mustimplement dof(rob::Robot)
@mustimplement axisRange(rob::Robot) # must return Vector of Vectors
defaultVelocity(rob::Robot) = nothing # should be implemented for a robot that can handle velocities

# device specific implementations of basic functionality
@mustimplement _moveAbs(rob::Robot, pos::Vector{<:Unitful.Length}, speed::Union{Vector{<:Unitful.Velocity},Nothing})
@mustimplement _moveRel(rob::Robot, dist::Vector{<:Unitful.Length}, speed::Union{Vector{<:Unitful.Velocity},Nothing})
@mustimplement _enable(rob::Robot)
@mustimplement _disable(rob::Robot)
@mustimplement _reset(rob::Robot)
@mustimplement _setup(rob::Robot)
@mustimplement _doReferenceDrive(rob::Robot)
@mustimplement _isReferenced(rob::Robot)
@mustimplement _getPosition(rob::Robot)

function init(rob::Robot)
  @debug "Initializing robot with ID `$(rob.deviceID)`."
  setup(rob)
  rob.present = true
end

neededDependencies(::Robot) = []
optionalDependencies(::Robot) = [AbstractCollisionModule]

# can be overwritten, but does not have to be
state(rob::Robot) = rob.state
setstate!(rob::Robot, state::RobotState) = rob.state = state
coordinateSystem(rob::Robot) = params(rob).coordinateSystem
namedPositions(rob::Robot) = :namedPositions in fieldnames(typeof(params(rob))) ? params(rob).namedPositions : error("The parameter struct of the robot must have a field `namedPositions`.")
namedPosition(rob::Robot, pos::AbstractString) = RobotCoords(params(rob).namedPositions[pos])

# should return a matrix of shape dof(rob)×dof(rob) where the first column is the scanner x-axis in robot coordinates, second column is the scanner y-axis in robot coordinates
scannerCoordAxes(rob::Robot) = coordinateSystem(rob).axes
scannerCoordOrigin(rob::Robot) = coordinateSystem(rob).origin

#overwrite to enable a change in movement order, if overwritten should be a string of length dof(rob), containing the lowercase letters corresponding to the axes, e.g. "yx" or "xzy"
movementOrder(rob::Robot) = "default"

getRobots(scanner::MPIScanner) = getDevices(scanner, Robot)
getRobot(scanner::MPIScanner) = getDevice(scanner, Robot)

include("CollisionModule/CollisionModule.jl")
include("RobotExceptions.jl")
include("DummyRobot.jl")
include("SimulatedRobot.jl")
include("IgusRobot.jl")
include("IselRobot.jl")
include("BrukerRobot.jl")

function gotoPos(rob::Robot, pos_name::AbstractString, args...)
  if haskey(namedPositions(rob), pos_name)
    pos = namedPositions(rob)[pos_name]
    moveAbs(rob, RobotCoords(pos), args...)
  else
    throw(RobotTeachError(rob, pos_name))
  end
end

function teachNamedPosition(rob::Robot, pos_name::AbstractString; override=false)
  pos = getPosition(rob)
  if haskey(namedPositions(rob), pos_name)
    if !override
      throw(RobotTeachError(rob, pos_name))
    else
      namedPositions(rob)[pos_name] = pos
    end
  else
    push!(namedPositions(rob),pos_name=>pos)
  end
end

function exportNamedPositions(rob::Robot)
  println("To save the positions, that have been tought in the current session copy and paste the following section into the config file: ")
  println()
  println("[Devices.$(deviceID(rob)).namedPositions]")
  for (key, value) in namedPositions(rob)
    print("$(key) = [")
    for dim in value
      print("\"$(dim)\", ")
    end
    print("\033[2D") # move cursor back 2 characters (remove last comma and space)
    println("]")
  end
  println()
end

useExplicitCoordinates(rob::Robot) = ((scannerCoordAxes(rob) != LinearAlgebra.I) || (scannerCoordOrigin(rob) != zeros(dof(rob))*u"mm")) # if there is a non unit transform defined, use explicit coordinates

toRobotCoords(rob::Robot, coords::ScannerCoords) = toDeviceCoords(coordinateSystem(rob), coords, RobotCoords)
toRobotCoords(rob::Robot, coords::RobotCoords) = coords
toScannerCoords(rob::Robot, coords::Union{RobotCoords, ScannerCoords}) = toScannerCoords(coordinateSystem(rob), coords)

"Returns true if given position is allowed, false otherwise"
function checkPosition(rob::Robot, pos::ScannerCoords)
  if hasDependency(rob, AbstractCollisionModule)
    cms = dependencies(rob, AbstractCollisionModule)
    return checkCoords(cms, pos)
  end
  # should this stop execution in some cases instead of warning?
  @warn "No valid CollisionModule defined as a dependency for robot $(deviceID(rob)), cant check movement"
  return true
end
function checkPosition(rob::Robot, pos::RobotCoords)
  return checkPosition(rob, toScannerCoords(rob, pos))
end

moveAbs(rob::Robot, pos::Vararg{Unitful.Length,N}) where N = moveAbs(rob, [pos...])
moveAbs(rob::Robot, pos::AbstractVector{<:Unitful.Length}) = moveAbs(rob, pos, defaultVelocity(rob))
moveAbs(rob::Robot, pos::AbstractVector{<:Unitful.Length}, speed::Unitful.Velocity) = moveAbs(rob, pos, speed * ones(dof(rob)))
function moveAbs(rob::Robot, pos::AbstractVector{<:Unitful.Length}, speed::Union{Vector{<:Unitful.Velocity},Nothing})
  if useExplicitCoordinates(rob)
    throw(RobotExplicitCoordinatesError(rob))
  else
    moveAbs(rob, RobotCoords(pos), speed)
  end
end

moveAbs(rob::Robot, pos::ScannerCoords, speed::Union{Vector{<:Unitful.Velocity},Nothing}) = moveAbs(rob, toRobotCoords(rob, pos), speed)

function moveAbs(rob::Robot, pos::RobotCoords, speed::Union{Vector{<:Unitful.Velocity},Nothing})
  length(pos) == dof(rob) || throw(RobotDOFError(rob, length(pos)))
  state(rob) == READY || throw(RobotStateError(rob, READY))
  isReferenced(rob) || throw(RobotReferenceError(rob)) # TODO: maybe this does not have to be limited
  checkAxisRange(rob, pos) || throw(RobotAxisRangeError(rob, pos))

  if !checkPosition(rob, pos)
    throw(RobotSafetyError(rob, pos))
  end

  setstate!(rob, MOVING)
  try
    @debug "Started absolute robot movement to [$(join([string(x) for x in pos], ", "))] with speed $(isnothing(speed) ? speed : "["*join([string(x) for x in speed], ", ")*"]")."
    if movementOrder(rob) == "default"
      _moveAbs(rob, pos.data, speed)
    else
      tmp_pos = getPosition(rob).data
      for i in 1:dof(rob)
        axis = movementOrder(rob)[i] - 'w' # 'x'->1, 'y'->2, 'z'->3
        tmp_pos[axis] = pos.data[axis]
        _moveAbs(rob, tmp_pos, speed)
      end
    end
    setstate!(rob, READY)
  catch exc
    setstate!(rob, ERROR)
    throw(RobotDeviceError(rob, exc))
  end
end

function moveScannerOrigin(rob::Robot, speed::Union{Vector{<:Unitful.Velocity}, Unitful.Velocity, Nothing} = nothing)
  moveAbs(rob, ScannerCoords(zeros(dof(rob)) * u"mm"), speed)
end

function moveRobotOrigin(rob::Robot, speed::Union{Vector{<:Unitful.Velocity}, Unitful.Velocity, Nothing} = nothing)
  moveAbs(rob, RobotCoords(zeros(dof(rob)) * u"mm"), speed)
end

moveRel(rob::Robot, dist::Vararg{Unitful.Length,N}) where N = moveRel(rob, [dist...])
moveRel(rob::Robot, dist::AbstractVector{<:Unitful.Length}) = moveRel(rob, dist, defaultVelocity(rob))
moveRel(rob::Robot, dist::AbstractVector{<:Unitful.Length}, speed::Unitful.Velocity) = moveRel(rob, dist, speed * ones(dof(rob)))

function moveRel(rob::Robot, dist::AbstractVector{<:Unitful.Length}, speed::Union{Vector{<:Unitful.Velocity},Nothing})
  if useExplicitCoordinates(rob)
    throw(RobotExplicitCoordinatesError(rob))
  else
    moveRel(rob, RobotCoords(dist), speed)
  end
end

moveRel(rob::Robot, dist::ScannerCoords, speed::Union{Vector{<:Unitful.Velocity},Nothing}) = moveRel(rob, RobotCoords(inv(scannerCoordAxes(rob)) * dist.data), speed)

function moveRel(rob::Robot, dist::RobotCoords, speed::Union{Vector{<:Unitful.Velocity},Nothing})
  length(dist) == dof(rob) || throw(RobotDOFError(rob, length(dist)))
  state(rob) == READY || throw(RobotStateError(rob, READY))

  if isReferenced(rob)
    pos = RobotCoords(getPosition(rob) + dist)
    checkAxisRange(rob, pos) || throw(RobotAxisRangeError(rob, pos))

    if !checkPosition(rob, pos)
      throw(RobotSafetyError(rob, pos))
    end

  else
    checkAxisRange(rob, abs.(dist)) || throw(RobotAxisRangeError(rob, dist)) #if the absolute distance in any axis is larger than the range, throw an error, however not throwing an error does not mean the movement is safe!
    @warn "Performing relative movement in unreferenced state, cannot validate coordinates! Please proceed carefully and perform only movements which are safe!"
  end

  setstate!(rob, MOVING)

  try
    if movementOrder(rob) == "default"
      _moveRel(rob, dist.data, speed)
    else
      for i in 1:dof(rob)
        tmp_dist = zeros(dof(rob)) * u"mm"
        axis = movementOrder(rob)[i] - 'w' # 'x'->1, 'y'->2, 'z'->3
        tmp_dist[axis] = dist.data[axis]
        _moveRel(rob, tmp_dist, speed)
      end
    end
    setstate!(rob, READY)
  catch exc
    setstate!(rob, ERROR)
    throw(RobotDeviceError(rob, exc))
  end
end

movePark(rob::Robot) = haskey(namedPositions(rob), "park") ? gotoPos(rob, "park") : moveAbs(rob, RobotCoords(zeros(dof(rob))*u"mm"))

function enable(rob::Robot)
  if state(rob) == READY
    return READY
  elseif state(rob) == DISABLED
    try
      _enable(rob)
      setstate!(rob, READY)
    catch exc
      setstate!(rob, ERROR)
      throw(RobotDeviceError(rob, exc))
    end
  else
    throw(RobotStateError(rob, DISABLED))
  end
end

function disable(rob::Robot)
  if state(rob) == DISABLED
    return DISABLED
  elseif state(rob) == READY
    try
      _disable(rob)
      setstate!(rob, DISABLED)
    catch exc
      setstate!(rob, ERROR)
      throw(RobotDeviceError(rob, exc))
    end
  else
    throw(RobotStateError(rob, READY))
  end
end

function Base.reset(rob::Robot)
  try
    _reset(rob)
    setstate!(rob, INIT)
  catch exc
    setstate!(rob, ERROR)
    throw(RobotDeviceError(rob, exc))
  end
end

function setup(rob::Robot)
  state(rob) == INIT || throw(RobotStateError(rob, INIT))
  try
    _setup(rob)
  catch exc
    setstate!(rob, ERROR)
    throw(RobotDeviceError(rob, exc))
  end
  setstate!(rob, DISABLED)
end

function doReferenceDrive(rob::Robot)
  state(rob) == READY || throw(RobotStateError(rob, READY))
  try
    setstate!(rob, MOVING)
    _doReferenceDrive(rob)
    setstate!(rob, READY)
  catch exc
    setstate!(rob, ERROR)
    throw(RobotDeviceError(rob, exc))
  end
end

function getPosition(rob::Robot)
  try
    RobotCoords(_getPosition(rob))
  catch exc
    setstate!(rob, ERROR) # maybe it is not necessary to make this an error
    throw(RobotDeviceError(rob, exc))
  end
end

getPositionScannerCoords(rob::Robot) = toScannerCoords(rob, getPosition(rob))

function isReferenced(rob::Robot)
  try
    _isReferenced(rob)::Bool
  catch exc
    setstate!(rob, ERROR) # maybe it is not necessary to make this an error
    throw(RobotDeviceError(rob, exc))
  end
end

function checkAxisRange(rob::Robot, coords::AbstractVector{<:Unitful.Length})
  axes = axisRange(rob)
  inRange = true
  for i in 1:length(coords)
    inRange &= (axes[i][1] <= coords[i] <= axes[i][2])
  end
  return inRange
end

function RobotState(s::Symbol)
  reverse_dict = Dict(value => key for (key, value) in Base.Enums.namemap(RobotState))
  if s in keys(reverse_dict)
    return RobotState(reverse_dict[s])
  else
    throw(ArgumentError(string("invalid value for Enum RobotState: $s")))
  end
end

Base.convert(t::Type{RobotState}, x::Union{Symbol,Int}) = t(x)
Base.:(==)(x::RobotState, y::Union{Symbol,Int}) = try x == RobotState(y) catch _ return false end


text:
import MetaGraphs
import LightGraphs

using PicoQuant
using Statistics
using GraphPlot
using Colors

export plot
export plot_contraction_tree, create_contraction_tree

"""
    function create_graph(tng::TensorNetworkCircuit)

Create a light graph representation of the tensor network circuit
"""
function create_graph(tng::TensorNetworkCircuit)
    # Find starting nodes and edges
    # Criteria
    # 1. Edges with nothing as source for case with no input nodes
    input_edges = [k for (k, v) in pairs(tng.edges) if v.src === nothing]
    # 2. Nodes with only outgoing edges where input nodes or contracted nodes
    source_nodes = [x for x in keys(tng.nodes) if length(inedges(tng, x)) == 0]

    # setup data structures to track vertex indices as graph is constructed
    vertex_map = Dict{Symbol, Int64}() # for tracking the index of nodes
    index_source_vertex = Dict{Symbol, Int64}()

    # Create graph and add initial nodes
    g = MetaGraphs.MetaGraph()
    layer = 0.
    # For each open input edge, we add a vertex
    for edge in input_edges
        qubit = tng.edges[edge].qubit
        MetaGraphs.add_vertex!(g,
                               Dict(:label => "input_$(qubit)",
                               :locx => layer,
                               :locy => qubit))
        index_source_vertex[edge] = LightGraphs.nv(g)
    end

    # now for node we can connect to
    for node in source_nodes
        qubits = [tng.edges[x].qubit for x in outedges(tng, node)]
        locy = mean(qubits)
        MetaGraphs.add_vertex!(g,
                               Dict(:label => String(node),
                               :locx => layer,
                               :locy => locy))
        vertex_map[node] = MetaGraphs.nv(g)
        for edge in outedges(tng, node)
            Base.push!(input_edges, edge)
            index_source_vertex[edge] = MetaGraphs.nv(g)
        end
        virtualbonds = [x for x in tng.nodes[node].indices if tng.edges[x].virtual]
        for bond in virtualbonds
            get_vertex = x -> haskey(vertex_map, x) ? vertex_map[x] : nothing
            bs = get_vertex(tng.edges[bond].src)
            bd = get_vertex(tng.edges[bond].dst)
            if bs !== nothing && bd !== nothing
                MetaGraphs.add_edge!(g, bs, bd)
                MetaGraphs.set_prop!(g, bs, bd, :label, String(bond))
            end
        end
    end

    # incremement layer index and iteratively construct the rest of graph
    layer += 1.
    # next find potentially next nodes and then iterate through
    # find nodes that are reached by current indices
    connecting_nodes = [tng.edges[x].dst for x in input_edges
                        if tng.edges[x].dst != nothing]

    while length(connecting_nodes) > 0
        for node in connecting_nodes
            incoming = inedges(tng, node)
            if length(setdiff(incoming, input_edges)) == 0
                locy = mean([tng.edges[x].qubit for x in incoming])
                MetaGraphs.add_vertex!(g, Dict(:label => String(node),
                                      :locx => layer, :locy => locy))
                vertex_idx = MetaGraphs.nv(g)
                vertex_map[node] = vertex_idx
                for edge in incoming
                    MetaGraphs.add_edge!(g, index_source_vertex[edge], vertex_idx)
                    MetaGraphs.set_prop!(g, index_source_vertex[edge], vertex_idx,
                                         :label, String(edge))
                end
                input_edges = setdiff(input_edges, incoming)
                outgoing = outedges(tng, node)
                for edge in outgoing
                    index_source_vertex[edge] = vertex_idx
                end
                input_edges = union(input_edges, outedges(tng, node))
                # add virtual bonds
                virtual = [x for x in tng.nodes[node].indices
                           if tng.edges[x].virtual]
                for bond in virtual
                    get_vertex = x -> haskey(vertex_map, x) ? vertex_map[x] : nothing
                    bs = get_vertex(tng.edges[bond].src)
                    bd = get_vertex(tng.edges[bond].dst)
                    if bs != nothing && bd != nothing
                        MetaGraphs.add_edge!(g, bs, bd)
                        MetaGraphs.set_prop!(g, bs, bd, :label, String(bond))
                    end
                end
                layer += 1.
            end
        end
        connecting_nodes = [tng.edges[x].dst for x in input_edges if tng.edges[x].dst != nothing]
    end
    g
end

"""
    function plot(tng::TensorNetworkCircuit; showlabels::Bool=false)

Function for creating a graph of a tensor network graph
"""
function plot(tng::TensorNetworkCircuit; showlabels::Bool=false)
    g = create_graph(tng)
    locx = convert(Vector{Real}, [MetaGraphs.get_prop(g, v, :locx)
                                  for v in MetaGraphs.vertices(g)])
    locy = convert(Vector{Real}, [MetaGraphs.get_prop(g, v, :locy)
                                  for v in MetaGraphs.vertices(g)])
    if showlabels
        gplot(g,
              locx,
              locy,
              edgelabel=[MetaGraphs.get_prop(g, e, :label)
                         for e in MetaGraphs.edges(g)],
              nodelabel=[MetaGraphs.get_prop(g, v, :label)
                         for v in MetaGraphs.vertices(g)])
    else
        gplot(g,
              locx,
              locy)
    end
end

# *************************************************************************** #
#                   Functions to visualise contraction plans
# *************************************************************************** #

"""
    function create_contraction_tree(network::TensorNetworkCircuit,
                                     plan::Array{Array{Symbol, 1}, 1})

This function converts a contraction plan (an array of node symbol pairs) to
a contraction tree (a MetaGraph where nodes represent tensors created during
the contraction).
"""
function create_contraction_tree(network::TensorNetworkCircuit,
                                 plan::Array{Array{Symbol, 1}, 1})

    # Convert the contraction plan to an array of integer pairs.
    edges = [[parse(Int, string(pair[1])[6:end]),
              parse(Int, string(pair[2])[6:end])]
             for pair in plan]

    # Create a metagraph structure to represent the contraction tree.
    graph = LightGraphs.Graph(length(network.nodes))
    graph = MetaGraph(graph)

    # Each node of the tree should have the uncontracted indices of the tensor
    # it represents, and the cost of the contraction that created that tensor,
    # as properties. Leaves of the tree (the tensors in the uncontracted graph)
    # should be initialised with zero cost.
    for (vertex, node) in enumerate(values(network.nodes))
        props = Dict(:cost=>0, :indices=>node.indices)
        set_props!(graph, vertex, props)
    end

    # For each edge in the contraction plan, add a new node to the tree,
    # representing the tensor created by contracting that edge, connect it to
    # the tree and give it the appropriate properties.
    for edge in edges
        LightGraphs.add_vertex!(graph)
        LightGraphs.add_edge!(graph, edge[1], LightGraphs.nv(graph))
        LightGraphs.add_edge!(graph, edge[2], LightGraphs.nv(graph))

        A_indices = get_prop(graph, edge[1], :indices)
        B_indices = get_prop(graph, edge[2], :indices)
        common_indices = intersect(A_indices, B_indices)
        remaining_indices_A = setdiff(A_indices, common_indices)
        remaining_indices_B = setdiff(B_indices, common_indices)
        remaining_indices = union(remaining_indices_A, remaining_indices_B)

        props = Dict(:cost=>length(common_indices), :indices=>remaining_indices)
        set_props!(graph, LightGraphs.nv(graph), props)
    end

    graph
end

"""
    function plot_contraction_tree(contraction_tree::MetaGraph{Int64,Float64})

Plotting function to visualise a contraction tree produced by the function
'create_contraction_tree'.
"""
function plot_contraction_tree(contraction_tree::MetaGraph{Int64,Float64})
    # TODO node sizes are too small for large networks
    node_costs = [get_prop(contraction_tree, v, :cost)
                  for v in LightGraphs.vertices(contraction_tree)]

    nodesize = [log(c+1)+1 for c in node_costs]

    num_nodes = sum(nodesize.==1)

    colors = colormap("blues", maximum(node_costs)+1)
    nodefillc = [c+1 for c in node_costs]
    nodefillc = colors[nodefillc]

    layout=(args...)->spring_layout(args...; C=0.5)

    gplot(contraction_tree, layout=layout,
          nodesize=nodesize, nodefillc=nodefillc)
end


text:
#!/usr/bin/env julia

include("MPC.jl")

# Black magic to make Julia and Python happy
using Distributed

using Pkg
Distributed.@everywhere using Pkg

Distributed.@everywhere begin
  ENV["PYTHON"] = "/usr/bin/python3"
  Pkg.build("PyCall")
end

using PyCall
Distributed.@everywhere using PyCall

#Import Statements for RobotOS
using RobotOS

@rosimport trajectory_msgs.msg: JointTrajectory, JointTrajectoryPoint
@rosimport arthur_planning.msg: arthur_traj
@rosimport sensor_msgs.msg: JointState
rostypegen()
using .trajectory_msgs.msg
using .arthur_planning.msg
using .sensor_msgs.msg

# function get_trajectory(params)
#     qref = typeof(zeros(7))[]
#     push!(qref,  [4.9268856741946365e-05, 0.26016423071338934, 3.140069344584098, -2.27008100955155, -2.40769461674617e-05, 0.9598460188468598, 1.569995694393751])
#     push!(qref,  [-0.003768053224579125, 0.26079787208394173, 3.1395327288847428, -2.269096150780195, -0.004224076946167462, 0.9616197667116058, 1.5721591274301712])
#     push!(qref,  [-0.01522001946854234, 0.2626987961955988, 3.137922881786677, -2.2661415744661304, -0.016824076946167464, 0.9669410103058437, 1.578649426539432])
#     push!(qref,  [-0.03430662987514771, 0.26586700304836064, 3.135239803289901, -2.2612172806093556, -0.03782407694616747, 0.9758097496295736, 1.5894665917215332])
#     push!(qref,  [-0.0610278844443952, 0.2703024926422273, 3.131483493394414, -2.254323269209871, -0.06722407694616747, 0.9882259846827953, 1.6046106229764747])
#     push!(qref,  [-0.0953296828066516, 0.2759962847990445, 3.126661557197997, -2.2454734980167035, -0.10496455313664366, 1.0041645773154986, 1.6240508594052145])
#     push!(qref,  [-0.13259401741002397, 0.2821818315115795, 3.121423165847148, -2.23585940048681, -0.14596455313664364, 1.0214797350427807, 1.6451700866655072])
#     push!(qref,  [-0.1698583520133963, 0.2883673782241145, 3.116184774496299, -2.226245302956917, -0.18696455313664362, 1.0387948927700625, 1.6662893139258002])
#     push!(qref,  [-0.20712268661676866, 0.2945529249366496, 3.1109463831454507, -2.2166312054270234, -0.2279645531366436, 1.0561100504973449, 1.6874085411860928])
#     push!(qref,  [-0.24438702122014103, 0.3007384716491846, 3.1057079917946018, -2.20701710789713, -0.2689645531366436, 1.0734252082246267, 1.7085277684463855])
#     push!(qref,  [-0.2816513558235134, 0.3069240183617196, 3.100469600443753, -2.1974030103672364, -0.3099645531366436, 1.090740365951909, 1.7296469957066785])
#     push!(qref,  [-0.3189156904268857, 0.31310956507425464, 3.095231209092904, -2.1877889128373433, -0.3509645531366436, 1.108055523679191, 1.7507662229669712])
#     push!(qref,  [-0.3561800250302581, 0.3192951117867897, 3.089992817742055, -2.17817481530745, -0.39196455313664363, 1.1253706814064732, 1.7718854502272638])
#     push!(qref,  [-0.3934443596336305, 0.3254806584993247, 3.0847544263912066, -2.1685607177775563, -0.43296455313664367, 1.1426858391337553, 1.7930046774875568])
#     push!(qref,  [-0.43070869423700286, 0.33166620521185974, 3.0795160350403576, -2.158946620247663, -0.47396455313664365, 1.1600009968610374, 1.8141239047478495])
#     push!(qref,  [-0.4679730288403753, 0.3378517519243948, 3.0742776436895087, -2.1493325227177693, -0.5149645531366437, 1.1773161545883195, 1.8352431320081424])
#     push!(qref,  [-0.5052373634437477, 0.34403729863692983, 3.06903925233866, -2.1397184251878762, -0.5559645531366437, 1.1946313123156016, 1.856362359268435])
#     push!(qref,  [-0.54250169804712, 0.3502228453494649, 3.063800860987811, -2.1301043276579827, -0.5969645531366438, 1.2119464700428837, 1.8774815865287278])
#     push!(qref,  [-0.5797660326504925, 0.3564083920619999, 3.0585624696369624, -2.1204902301280892, -0.6379645531366438, 1.2292616277701658, 1.8986008137890207])
#     push!(qref,  [-0.6170303672538648, 0.36259393877453494, 3.0533240782861135, -2.1108761325981957, -0.6789645531366438, 1.2465767854974479, 1.9197200410493134])
#     push!(qref,  [-0.6542947018572371, 0.36877948548706996, 3.0480856869352646, -2.1012620350683022, -0.7199645531366438, 1.26389194322473, 1.940839268309606])
#     push!(qref,  [-0.6915590364606096, 0.37496503219960503, 3.0428472955844157, -2.091647937538409, -0.7609645531366438, 1.281207100952012, 1.961958495569899])
#     push!(qref,  [-0.728823371063982, 0.38115057891214005, 3.0376089042335668, -2.0820338400085157, -0.8019645531366438, 1.2985222586792942, 1.983077722830192])
#     push!(qref,  [-0.7660877056673544, 0.3873361256246751, 3.0323705128827183, -2.072419742478622, -0.8429645531366439, 1.3158374164065763, 2.0041969500904844])
#     push!(qref,  [-0.8033520402707267, 0.39352167233721014, 3.0271321215318694, -2.0628056449487286, -0.8839645531366439, 1.3331525741338583, 2.0253161773507773])
#     push!(qref,  [-0.8406163748740991, 0.39970721904974515, 3.0218937301810205, -2.053191547418835, -0.9249645531366439, 1.3504677318611407, 2.04643540461107])
#     push!(qref,  [-0.8778807094774715, 0.4058927657622802, 3.0166553388301716, -2.043577449888942, -0.965964553136644, 1.3677828895884225, 2.067554631871363])
#     push!(qref,  [-0.915145044080844, 0.41207831247481524, 3.0114169474793226, -2.0339633523590486, -1.006964553136644, 1.3850980473157049, 2.0886738591316556])
#     push!(qref,  [-0.9524093786842164, 0.4182638591873503, 3.006178556128474, -2.024349254829155, -1.0479645531366442, 1.402413205042987, 2.1097930863919485])
#     push!(qref,  [-0.9896737132875887, 0.42444940589988533, 3.0009401647776253, -2.0147351572992616, -1.088964553136644, 1.419728362770269, 2.1309123136522414])
#     push!(qref,  [-1.026938047890961, 0.43063495261242035, 2.9957017734267763, -2.005121059769368, -1.1299645531366442, 1.4370435204975511, 2.1520315409125343])
#     push!(qref,  [-1.0642023824943334, 0.43682049932495537, 2.9904633820759274, -1.9955069622394748, -1.1709645531366442, 1.4543586782248332, 2.173150768172827])
#     push!(qref,  [-1.101466717097706, 0.44300604603749044, 2.9852249907250785, -1.9858928647095815, -1.2119645531366443, 1.4716738359521153, 2.1942699954331197])
#     push!(qref,  [-1.1387310517010785, 0.4491915927500255, 2.97998659937423, -1.976278767179688, -1.2529645531366442, 1.4889889936793974, 2.215389222693412])
#     push!(qref,  [-1.1759953863044506, 0.45537713946256053, 2.974748208023381, -1.9666646696497945, -1.2939645531366444, 1.5063041514066795, 2.236508449953705])
#     push!(qref,  [-1.213259720907823, 0.46156268617509555, 2.969509816672532, -1.9570505721199012, -1.3349645531366443, 1.5236193091339616, 2.257627677213998])
#     push!(qref,  [-1.2478021278200226, 0.46729641720217435, 2.9646540572035307, -1.9481387245716801, -1.3729697583408722, 1.5396697023673502, 2.277204276379416])
#     push!(qref,  [-1.2748022884966306, 0.4717782027187738, 2.9608585403999896, -1.9411727561509207, -1.402676624155692, 1.5522155332738579, 2.292506375168055])
#     push!(qref,  [-1.2941678050105965, 0.47499270549426853, 2.9581362549951593, -1.9361765052728712, -1.423983489970512, 1.5612138684508734, 2.3034816078838536])
#     push!(qref,  [-1.3058986773619201, 0.4769399255286585, 2.9564872009890393, -1.9331499719375316, -1.4368903557853316, 1.566664707898397, 2.310129974526812])
#     push!(qref,  [-1.3099949055506017, 0.47761986282194374, 2.95591137838163, -1.932093156144902, -1.4413972216001514, 1.5685680516164289, 2.3124514750969296])
#     push!(qref,  [-1.3100000000000003, 0.47762070845504206, 2.955910662235214, -1.9320918417907373, -1.4414028267565981, 1.568570418791257, 2.312454362330413])

#     q̇ref = typeof(zeros(7))[]
#     push!(q̇ref,  [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
#     push!(q̇ref,  [-0.07634644162642143, 0.012672827411047383, -0.010732313987104905, 0.019697175427098694, -0.084, 0.03547495729491942, 0.04326866072840471])
#     push!(q̇ref,  [-0.15269288325284286, 0.025345654822094766, -0.02146462797420981, 0.03939435085419739, -0.168, 0.07094991458983883, 0.08653732145680942])
#     push!(q̇ref,  [-0.22903932487926432, 0.03801848223314215, -0.032196941961314715, 0.05909152628129609, -0.252, 0.10642487188475827, 0.12980598218521414])
#     push!(q̇ref,  [-0.3053857665056857, 0.05069130964418953, -0.04292925594841962, 0.07878870170839478, -0.336, 0.14189982917967767, 0.17307464291361885])
#     push!(q̇ref,  [-0.3726433460337236, 0.061855467125350325, -0.05238391350848822, 0.09614097529893409, -0.41, 0.17315157727282096, 0.21119227260292772])
#     push!(q̇ref,  [-0.3726433460337236, 0.061855467125350325, -0.05238391350848822, 0.09614097529893409, -0.41, 0.17315157727282096, 0.21119227260292772])
#     push!(q̇ref,  [-0.3726433460337236, 0.061855467125350325, -0.05238391350848822, 0.09614097529893409, -0.41, 0.17315157727282096, 0.21119227260292772])
#     push!(q̇ref,  [-0.3726433460337236, 0.061855467125350325, -0.05238391350848822, 0.09614097529893409, -0.41, 0.17315157727282096, 0.21119227260292772])
#     push!(q̇ref,  [-0.3726433460337236, 0.061855467125350325, -0.05238391350848822, 0.09614097529893409, -0.41, 0.17315157727282096, 0.21119227260292772])
#     push!(q̇ref,  [-0.3726433460337236, 0.061855467125350325, -0.05238391350848822, 0.09614097529893409, -0.41, 0.17315157727282096, 0.21119227260292772])
#     push!(q̇ref,  [-0.3726433460337236, 0.061855467125350325, -0.05238391350848822, 0.09614097529893409, -0.41, 0.17315157727282096, 0.21119227260292772])
#     push!(q̇ref,  [-0.3726433460337236, 0.061855467125350325, -0.05238391350848822, 0.09614097529893409, -0.41, 0.17315157727282096, 0.21119227260292772])
#     push!(q̇ref,  [-0.3726433460337236, 0.061855467125350325, -0.05238391350848822, 0.09614097529893409, -0.41, 0.17315157727282096, 0.21119227260292772])
#     push!(q̇ref,  [-0.3726433460337236, 0.061855467125350325, -0.05238391350848822, 0.09614097529893409, -0.41, 0.17315157727282096, 0.21119227260292772])
#     push!(q̇ref,  [-0.3726433460337236, 0.061855467125350325, -0.05238391350848822, 0.09614097529893409, -0.41, 0.17315157727282096, 0.21119227260292772])
#     push!(q̇ref,  [-0.3726433460337236, 0.061855467125350325, -0.05238391350848822, 0.09614097529893409, -0.41, 0.17315157727282096, 0.21119227260292772])
#     push!(q̇ref,  [-0.3726433460337236, 0.061855467125350325, -0.05238391350848822, 0.09614097529893409, -0.41, 0.17315157727282096, 0.21119227260292772])
#     push!(q̇ref,  [-0.3726433460337236, 0.061855467125350325, -0.05238391350848822, 0.09614097529893409, -0.41, 0.17315157727282096, 0.21119227260292772])
#     push!(q̇ref,  [-0.3726433460337236, 0.061855467125350325, -0.05238391350848822, 0.09614097529893409, -0.41, 0.17315157727282096, 0.21119227260292772])
#     push!(q̇ref,  [-0.3726433460337236, 0.061855467125350325, -0.05238391350848822, 0.09614097529893409, -0.41, 0.17315157727282096, 0.21119227260292772])
#     push!(q̇ref,  [-0.3726433460337236, 0.061855467125350325, -0.05238391350848822, 0.09614097529893409, -0.41, 0.17315157727282096, 0.21119227260292772])
#     push!(q̇ref,  [-0.3726433460337236, 0.061855467125350325, -0.05238391350848822, 0.09614097529893409, -0.41, 0.17315157727282096, 0.21119227260292772])
#     push!(q̇ref,  [-0.3726433460337236, 0.061855467125350325, -0.05238391350848822, 0.09614097529893409, -0.41, 0.17315157727282096, 0.21119227260292772])
#     push!(q̇ref,  [-0.3726433460337236, 0.061855467125350325, -0.05238391350848822, 0.09614097529893409, -0.41, 0.17315157727282096, 0.21119227260292772])
#     push!(q̇ref,  [-0.3726433460337236, 0.061855467125350325, -0.05238391350848822, 0.09614097529893409, -0.41, 0.17315157727282096, 0.21119227260292772])
#     push!(q̇ref,  [-0.3726433460337236, 0.061855467125350325, -0.05238391350848822, 0.09614097529893409, -0.41, 0.17315157727282096, 0.21119227260292772])
#     push!(q̇ref,  [-0.3726433460337236, 0.061855467125350325, -0.05238391350848822, 0.09614097529893409, -0.41, 0.17315157727282096, 0.21119227260292772])
#     push!(q̇ref,  [-0.3726433460337236, 0.061855467125350325, -0.05238391350848822, 0.09614097529893409, -0.41, 0.17315157727282096, 0.21119227260292772])
#     push!(q̇ref,  [-0.3726433460337236, 0.061855467125350325, -0.05238391350848822, 0.09614097529893409, -0.41, 0.17315157727282096, 0.21119227260292772])
#     push!(q̇ref,  [-0.3726433460337236, 0.061855467125350325, -0.05238391350848822, 0.09614097529893409, -0.41, 0.17315157727282096, 0.21119227260292772])
#     push!(q̇ref,  [-0.3726433460337236, 0.061855467125350325, -0.05238391350848822, 0.09614097529893409, -0.41, 0.17315157727282096, 0.21119227260292772])
#     push!(q̇ref,  [-0.3726433460337236, 0.061855467125350325, -0.05238391350848822, 0.09614097529893409, -0.41, 0.17315157727282096, 0.21119227260292772])
#     push!(q̇ref,  [-0.3726433460337236, 0.061855467125350325, -0.05238391350848822, 0.09614097529893409, -0.41, 0.17315157727282096, 0.21119227260292772])
#     push!(q̇ref,  [-0.3726433460337236, 0.061855467125350325, -0.05238391350848822, 0.09614097529893409, -0.41, 0.17315157727282096, 0.21119227260292772])
#     push!(q̇ref,  [-0.3726433460337236, 0.061855467125350325, -0.05238391350848822, 0.09614097529893409, -0.41, 0.17315157727282096, 0.21119227260292772])
#     push!(q̇ref,  [-0.3081748275792907, 0.05115426887151827, -0.04332132502896183, 0.07950827192114293, -0.3390686581481977, 0.14319578771253505, 0.17465531825059288])
#     push!(q̇ref,  [-0.2318283859528692, 0.038481441460470875, -0.03258901104185691, 0.05981109649404422, -0.2550686581481976, 0.1077208304176156, 0.13138665752218814])
#     push!(q̇ref,  [-0.1554819443264477, 0.02580861404942348, -0.021856697054752, 0.04011392106694551, -0.17106865814819755, 0.07224587312269615, 0.08811799679378339])
#     push!(q̇ref,  [-0.07913550270002623, 0.013135786638376092, -0.011124383067647084, 0.0204167456398468, -0.08706865814819748, 0.036770915827776696, 0.04484933606537864])
#     push!(q̇ref,  [-0.002789061073605048, 0.00046295922732875194, -0.00039206908054222056, 0.0007195702127481829, -0.0030686581481977893, 0.0012959585328574108, 0.0015806753369740911])
#     push!(q̇ref,  [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0])

#     Xref = typeof(zeros(14))[]
#     for k = 1:length(qref)
#         push!(Xref, [qref[k]; q̇ref[k]])
#     end
#     for k = 1:params.H
#         push!(Xref, [qref[end]; q̇ref[end]])
#     end
#     return Xref
# end

# function callback(msg::JointTrajectoryPoint, x0, solve_mpc, params)
#     x0_new = zeros(params.n)
#     x0_new[1:7] .= msg.positions
#     x0_new[8:14] .= msg.velocities
#     if norm(x0_new - x0) >= 1e-6
#         x0 .= x0_new
#         solve_mpc .= true
#     end
# end

function traj_callback(msg::arthur_traj, Xref, x0)
    Xref[1] = typeof(zeros(14))[]
    for k=1:length(msg.traj.points)
        q = msg.traj.points[k].positions
        q̇ = msg.traj.points[k].velocities
        push!(Xref[1], [q; q̇])
    end
    # if norm(x0 - zeros(14)) == 0
    #     copy!(x0, Xref[1][1])
    # end
    # RobotOS.loginfo("In Callback")
    # println(Xref[1][1])
end

function callback_x0(msg::JointState, x0, solve_mpc)
    x0_new = zeros(14)
    x0_new[1:7] .= msg.position
    x0_new[8:14] .= msg.velocity
    if norm(x0_new - x0) >= 1e-6
        x0 .= x0_new
        solve_mpc .= true
    end
end

function loop(sub_obj1, sub_obj2, pub_obj, x0, Xref, params, solve_mpc)
    loop_rate = Rate(1.0/params.dt)
    prob_mpc = ArthurHorizonProblem(Xref[1], x0, params.H, start=1)
    altro_mpc = ALTROSolver(prob_mpc, params.opts)
    solve!(altro_mpc)
    while ! is_shutdown()
        RobotOS._run_callbacks(sub_obj1)
        RobotOS._run_callbacks(sub_obj2)
        JointTrajectoryOutput = JointTrajectory()
        if norm(x0 - Xref[1][end]) > 0.05 && solve_mpc[1]
            # t0 = RobotOS.to_sec(RobotOS.now())
            println(x0)
            # mpc_update(altro_mpc, prob_mpc, Z_track, x0, t0)
            k_mpc = argmin(norm.([(Xref[1][k][1:7] - x0[1:7]) for k=1:length(Xref[1])]))
            # k_mpc = 1
            println(k_mpc)
            prob_mpc = ArthurHorizonProblem(Xref[1], x0, params.H, start=k_mpc)
            altro_mpc = ALTROSolver(prob_mpc, params.opts)
            solve!(altro_mpc)
            X = states(altro_mpc)
            U = controls(altro_mpc)
            points = Array{JointTrajectoryPoint}(undef, length(U))
            for k = 1:length(U)
                point = JointTrajectoryPoint()
                point.positions = X[k][1:7]
                point.velocities = X[k][8:14]
                point.effort = U[k]
                points[k] = point
            end
            JointTrajectoryOutput.header.stamp = RobotOS.now()
            JointTrajectoryOutput.points = points
            # publish(pub_obj, JointTrajectoryOutput)
            # solve_mpc .= false
        end
        publish(pub_obj, JointTrajectoryOutput)
        rossleep(loop_rate)
    end
end

# function loop(sub_obj1, sub_obj2, params, x0, Xref)
#     loop_rate = Rate(1.0/params.dt)
#     while ! is_shutdown()
#         RobotOS._run_callbacks(sub_obj1)
#         RobotOS._run_callbacks(sub_obj2)
#         println(x0)
#         println(Xref[1][1])
#         rossleep(loop_rate)
#     end
# end

function main()
    init_node("MPC_Node")
    params = MPC_Params()
    Xref = [typeof(zeros(14))[]]
    # Xref = get_trajectory(params)
    # x0 = copy(Xref[1])
    solve_mpc = [true]
    x0 = zeros(14)
    pub = Publisher{JointTrajectory}("joint_torques",queue_size=1)

    sub_x0 = Subscriber{JointState}("/my_gen3/joint_states", callback_x0, (x0,solve_mpc), queue_size=1)
    sub_traj = Subscriber{arthur_traj}("/my_gen3/arthur_traj",traj_callback,(Xref,x0),queue_size=1)
    # sub = Subscriber{JointTrajectoryPoint}("state",callback,(x0, solve_mpc, params),queue_size=1)

    while length(Xref[1]) == 0
        RobotOS._run_callbacks(sub_traj)
    end
    while norm(x0 - zeros(14)) == 0
        RobotOS._run_callbacks(sub_x0)
    end

    # prob = ArthurProblem(Xref[1], params=params)
    # altro = ALTROSolver(prob, params.opts)
    # solve!(altro)
    # Z_track = TrajectoryOptimization.get_trajectory(altro)

    # t0 = RobotOS.to_sec(RobotOS.now())
    # TrajectoryOptimization.set_initial_time!(prob, t0)
    # TrajectoryOptimization.set_initial_time!(prob_mpc, t0)

    loop(sub_traj, sub_x0, pub, x0, Xref, params, solve_mpc)
    # loop(sub_traj, sub_x0, params, x0, Xref)
end

if !isinteractive()
    main()
end


text:
macro runtwice(ex)
    quote
        local t0 = time_ns()
        local val0 = $(esc(ex))
        local t1 = time_ns()
        local val1 = $(esc(ex))
        local t2 = time_ns()
        min(t1-t0,t2-t1)/1e9
    end
end

"""
    lrtest(Ns::Int, N::Array{Int,1}, L::Array{Int,1}, dataType::Array{DataType,1}, distType)

Monte-Carlo evaluation of lattice reduction techniques. NxN matrices of
dataType are decomposed with each LR technique, with execution time,
memory, orthogonality defect, and Hermite factor shown.  Accepted
datatypes include Integer types such as Int8; FloatingPoint types such
as Float32; and Complex versions of these types. Accepted distTypes are
"randn" (Gaussian distributed with variance L), "rand" (uniformly
distributed over -L:L), "gen_qary" (tough lattice representative of
those used in lattice cryptography).

# Examples
  # As function of Integer width
  lrtest(50,2.^[4;],[100;],[Int32,Int64,Int128,Float64,BigInt,BigFloat],"rand")
  # As function of N
  lrtest(50,2 .^[0:6;],[100;],[Float64],"rand")
"""
function lrtest(Ns::Int,N::Array{Int,1},L::Array{Int,1},
                dataType::Array{DataType,1},distType)

lrAlgs = [hkz,lll,seysen, sizereduction,brun,]

@printf("      Ns      N      L   dataType")
for ax = 1:min(length(lrAlgs),6)
    @printf(" %13s",lrAlgs[ax])
end
@printf("\n")

xtickStrs = [];
out = []
if length(N)>1
    for s = 1:length(N)
        push!(out,lrsim(Ns,N[s],L[1],dataType[1], distType, lrAlgs));
    end
    xscale=:log10;
    yscale=:log10;
    xval = N;
    xlab = "Matrix Size";
    tstr = @sprintf("Ns=%d,Type=%s,dist=%s",
                    Ns,string(dataType[1]),distType);
elseif length(L)>1
    for s = 1:length(L)
        push!(out,lrsim(Ns,N[1],L[s],dataType[1], distType, lrAlgs));
    end
    xscale = :identity;
    yscale = :log10;
    xval = L;
    xlab = "L";
    tstr = @sprintf("Ns=%d,N=%d,Type=%s,dist=%s",
                    Ns,N[1],string(dataType[1]),distType);
elseif length(dataType)>1
    for s = 1:length(dataType)
        push!(out,lrsim(Ns,N[1],L[1],dataType[s], distType, lrAlgs));
    end
    xscale = :identity;
    yscale = :log10;
    xval = 1:length(dataType)
    xtickStrs = map(string,dataType)
    xlab = "dataType";
    tstr = @sprintf("Ns=%d,N=%d,L=%d,dist=%s",Ns,N[1],L[1],distType);
    for n=xval
        if dataType[n]== DoubleFloat{Float64}
            xtickStrs[n] = "Double64"
        end
    end
end

pColor = ["r-","b.-","k-","g-","c-","m-",
          "r--","b.-.","k--","g.-.","c--","m*-.",
          "rs--","gp-.","bv-","kh--","c+-.","m.-",];
pIdx   = 1;

Nout = size(out,1);

times = zeros(Nout);
orthf = zeros(Nout);
plot(legend=:topleft,linewidth=5,size=(700,525),guidefontsize=14,
     legendfontsize=14, xtickfontsize=10,ytickfontsize=14,titlefontsize=14)
#pltO = plot(legend=:topleft);

for a=1:length(out[1][2])
    for k=1:Nout
        times[k] = out[k][2][a];
        orthf[k] = out[k][3][a];
    end
    plot!(xval,times,xscale=xscale,yscale=yscale,label=out[1][1][a],linewidth=3);
#    plot!(xval,times,xscale=xscale,yscale=yscale,label=out[1][1][a]);
    pIdx = pIdx==length(pColor) ? 1 : pIdx + 1;
end

plot!(xlabel=xlab,ylabel="execution time (sec)")

if ~isempty(xtickStrs)
    plot!(xticks=(xval,xtickStrs))
end

end

function lrsim(Ns,N,L,dataType,distType,lrAlgs)
# sortDict = Dict("Insertion" => d->sort(d,alg=InsertionSort),

distDict = Dict("gen_qary"=> d -> gen_qary!(d,Int(N/2),rand(1:2^L-1)),
                "rand"=> d -> rand!(d,-L:L),
                "randmax"=> d -> rand(dataType,N,N),
                "randn"=> d -> L*randn!(d::AbstractArray{dataType,2}),
                 );
randf! = distDict[distType];

Nalgs = length(lrAlgs);
times = ones(Nalgs,Ns)*Inf;
hermitef = ones(Nalgs,Ns)*Inf;
orthf = ones(Nalgs,Ns)*Inf;
#algNames = cell(Nalgs)
algNames = Array{Any,1}(undef,Nalgs)
data = Array{dataType,2}(undef,N,N);

for ix = 1:Ns
    cnum = Inf;
    while cnum > 1e5  # With Ints need to check for singular matrices
        randf!(data);
        if !(dataType<:BigInt || dataType<:BigFloat)
            cnum = cond(data)
        else
            cnum = 0.0
        end
    end

    #
    # Lattice-reduction algorithms
    #
    for ax = 1:length(lrAlgs)
        # if lrAlgs[ax]==l2
        #     times[ax,ix] = @runtwice B = lrAlgs[ax](data)
        # else
            times[ax,ix] = @runtwice (B,T) = lrAlgs[ax](data)
#        end
        detB = abs(det(B))
        # Hermite factor
        hermitef[ax,ix] = norm(B[:,1])/detB^(1/N)
        # Orthogonality defect
        orthf[ax,ix] = prodBi(B,N)/detB

        # if abs(abs(det(T))-1.0)>1e-6
        #     println("For $(lrAlgs[ax]), det(T) is $(abs(det(T)))")
        #     times[ax,ix] = Inf
        # elseif abs(sum(B-data*T))>1e-6
        #     println("For $(lrAlgs[ax]), sum(B-H*T) "*
        #             "is $(abs(sum(B-data*T))), ")
        #     times[ax,ix] = Inf
        # end
    end
end

#algNames[Nalgs] = "randf";
for ax = 1:length(lrAlgs)
    algNames[ax] = string(lrAlgs[ax]);
end
@printf("%8d %6d %6d %10s", Ns,  N,  L, string(dataType)[1:min(end,10)])

#outtimes = mean(times,2);
outtimes = zeros(Nalgs,1);
outHF = zeros(Nalgs,1);
outOF = zeros(Nalgs,1);
mean(x) = sum(x)/length(x)
for ax = 1:Nalgs
#    stimes = sort(vec(times[ax,:]));
#    outtimes[ax] = mean(stimes[1:floor(Ns*2/3)]);
#    outtimes[ax] = minimum(times[ax,:]);
    outtimes[ax] = mean(times[ax,:]);
    outHF[ax] = mean(hermitef[ax,:]);
    outOF[ax] = mean(orthf[ax,:]);
end

for ax = 1:min(Nalgs,7)
    @printf("    %10.4f",outtimes[ax]*1000)
end
@printf("\n")

return algNames, outtimes, outOF, outHF
end

#######################################################################
function prodBi(B,N)
    prod = 1.0
    for n=1:N
        pn = 0.0
        for m=1:N
            pn+=conj(B[m,n])*B[m,n]
        end
        # println("pn=$(pn), typeof(pn)=$(typeof(pn))")
        prod*=sqrt(pn)
    end
    return prod
end


text:
module NeuroImage

export Dataset
export loadnifti,save,data,affine,size
export cluster
export ijk_to_xyz,xyz_to_ijk

using PyCall

mutable struct Dataset
    fname::String
    pyobj::PyObject
    info::PyObject
    subbricks::Vector{String}
end

Dataset(fname::String,pyobj::PyObject) = Dataset(fname,pyobj,PyObject(nothing),String[])

Dataset(fname::String,a::AbstractArray{T,N},aff::Array{K,2}) where {T<:Real,N,K<:Real} = Dataset(fname,nib.Nifti1Image(a,aff))
Dataset(fname::String,a::AbstractArray{T,N},template::Dataset) where {T<:Real,N} = Dataset(fname,nib.Nifti1Image(a,affine(template)))

@pyimport nibabel as nib
@pyimport neural as nl

function loadnifti(fname::String)
    img = nib.load(fname)
    info = nl.dset_info(fname)
    subbricks = [x["label"] for x in info[:subbricks]]
    return Dataset(fname,img,info,subbricks)
end

function Dataset{T<:Real}(d::Dataset,a::AbstractArray{T,3})
    img = nib.Nifti1Image(a,d.pyobj[:affine],d.pyobj[:header])
    Dataset(d.fname,img)
end

function Dataset{T<:Real}(a::AbstractArray{T,3},fname::String="noname.nii.gz")
    img = nib.Nifti1Image(a,nothing)
    Dataset(fname,img)
end

function save(d::Dataset,fname::String)
    nib.save(d.pyobj,fname)
end

save(d::Dataset) = save(d,d.fname)

function data(d::Dataset)
    d.pyobj[:get_data]()
end

function affine(d::Dataset)
    d.pyobj[:affine]
end

import Base.size
function size(d::Dataset)
    d.pyobj[:header][:get_data_shape]()
end

function ijk_to_xyz{T<:Real}(d::Dataset,ijk::AbstractArray{T,1})
    M = affine(d)[1:3,1:3]
    abc = affine(d)[1:3, 4]
    squeeze(M*reshape([Float64(x) for x in ijk],(3,1)) + abc,2)
end

function xyz_to_ijk{T<:Real}(d::Dataset,xyz::AbstractArray{T,1})
    M = affine(d)[1:3,1:3]
    abc = affine(d)[1:3, 4]
    [Int(round(x)) for x in M \ (xyz .- abc)]
end

using Images

function Base.show(io::IO, mime::MIME"image/png", d::Dataset)
    sd = size(d)
    s = Int(round(sd[1]/3))
    dd = 0
    if length(size(d))==4
        dd = Array{Float32,2}(NeuroImage.data(d)[s,end:-1:1,end:-1:1,1])'
    else
        dd = Array{Float32,2}(NeuroImage.data(d)[s,end:-1:1,end:-1:1,1])'
    end
    dd ./= maximum(dd)
    show(io,mime,colorview(Gray,dd))
end

# AFNI dependent functions

function cluster{T<:Real,K<:Real,L<:Real}(clust_map::Array{T,3},r::K,clust_size::L)
    tmpname = "tmp.nii.gz"
    clustname = "tmp_clust.nii.gz"
    for n in [tmpname,clustname]
        isfile(n) && rm(n)
    end
    temp_dset = Dataset(clust_map,tmpname)
    NeuroImage.save(temp_dset)
    run(pipeline(`3dmerge -1clust $r $clust_size -prefix $clustname $tmpname`,stdout=DevNull,stderr=DevNull))
    newd = data(loadnifti(clustname))
    for n in [tmpname,clustname]
        isfile(n) && rm(n)
    end
    return newd
end

function cluster{K<:Real,L<:Real}(clust_map::BitArray{3},r::K,clust_size::L)
    c = cluster(Array{Int16,3}(clust_map),r,clust_size)
    return c.!=0
end

end


text:
@testset "gather" begin
    @testset "3-argumented" begin
        X = [1 2 3; 4 5 6]
        ind1 = [1 2 2; 1 1 1]
        ind2 = [1 2; 3 1]
        for T = [Int8, Int16, Int32, Int64, Int128,
                 Float16, Float32, Float64]
            @test gather(T.(X), ind1, 1) == T.([1 5 6; 1 2 3])
            @test gather(T.(X), ind2, 2) == T.([1 2; 6 4])
        end
    end

    @testset "2-argumented" begin
        input = [3 3 4 4 5;
                 5 5 6 6 7]
        index = [1 2 3 4;
                 4 2 1 3;
                 3 5 5 3]
        output = cat([3 4 4; 5 6 6], [3 3 5; 5 5 7],
                     [4 3 5; 6 5 7], [4 4 4; 6 6 6], dims=3)
        @test gather(input, index) == output
    end
end


text:
macro T_str(s)
  s
end
standardrules = Grammar(T"""
int   => r([0-9]+)r {(r,v,f,l,c) -> parse(Int64,v)}
float => r([-+]?[0-9]*\.?[0-9]+([eE][-+]?[0-9]+)?)r {(r,v,f,l,c) -> parse(Float64,v)}
space => r([ \t]*)r
"""*
"""
eol   => '\r\n' | '\r' | '\n'
""")


text:
# Use baremodule to shave off a few KB from the serialized `.ji` file
baremodule gmmlib_jll
using Base
using Base: UUID
import JLLWrappers

JLLWrappers.@generate_main_file_header("gmmlib")
JLLWrappers.@generate_main_file("gmmlib", UUID("09858cae-167c-5acb-9302-fddc6874d481"))
end  # module gmmlib_jll


text:
#-------------IFS.jl-----------------------------------------------------------#
#
# Purpose: quick test of iterated function system fractals
#
#------------------------------------------------------------------------------#

using Images

mutable struct Pixel
    color::Vector{Float64}
    val::Float64
end

# Functions for fractal flames
function linear(point::Array{Float64, 1}, c::Float64, color::Array{Float64, 1})
    ret_point = zeros(6)
    ret_point[1] = point[1] * c
    ret_point[2] = point[2] * c
    ret_point[3] = color[1]
    ret_point[4] = color[2]
    ret_point[5] = color[3]
    ret_point[6] = color[4]

    return ret_point
end

function heart(point::Array{Float64, 1}, color::Array{Float64, 1})
    ret_point = zeros(6)
    r = sqrt(point[1]^2 + point[2]^2)
    theta = atan(point[2],point[1])
    ret_point[1] = r*(sin(r*theta))
    ret_point[2] = -r*(cos(r*theta))
    ret_point[3] = color[1]
    ret_point[4] = color[2]
    ret_point[5] = color[3]
    ret_point[6] = color[4]

    return ret_point
end

function sinusoidal(point::Array{Float64, 1}, color::Array{Float64, 1})
    ret_point = zeros(6)
    ret_point[1] = sin(point[1])
    ret_point[2] = cos(point[2])
    ret_point[3] = color[1]
    ret_point[4] = color[2]
    ret_point[5] = color[3]
    ret_point[6] = color[4]

    return ret_point
end

function swirl(point::Array{Float64, 1}, color::Array{Float64, 1})
    ret_point = zeros(6)
    r = sqrt(point[1]^2 + point[2]^2)
    ret_point[1] = point[1]*sin(r^2) - point[2]*cos(r^2)
    ret_point[2] = point[1]*cos(r^2) + point[2]*sin(r^2)
    ret_point[3] = color[1]
    ret_point[4] = color[2]
    ret_point[5] = color[3]
    ret_point[6] = color[4]

    return ret_point
end

function sierpensky_flame(point::Array{Float64, 1}, color::Array{Float64, 1},
                          chosen_function::Int64)
    if (chosen_function == 1)
        point[1:2] *= 0.5
        point[3] = color[1]
        point[4] = color[2]
        point[5] = color[3]
        point[6] = color[4]
    elseif(chosen_function == 2)
        point[1:2] = [(point[1] - 1)/2, (point[2])/2]
        point[3] = color[1]
        point[4] = color[2]
        point[5] = color[3]
        point[6] = color[4]
    else
        point[1:2] = [(point[1])/2, (point[2]-1)/2]
        point[3] = color[1]
        point[4] = color[2]
        point[5] = color[3]
        point[6] = color[4]
    end

    return point
end

function barnsley_flame(point::Array{Float64, 1}, color::Array{Float64, 1},
                        chosen_function::Int64)
    if (chosen_function == 1)
        point[1:2] = [0 0; 0 0.16]*point[1:2]
        point[3] = color[1]
        point[4] = color[2]
        point[5] = color[3]
        point[6] = color[4]
    elseif(chosen_function == 2)
        point[1:2] = [0.85 0.04; -0.04 0.85]*point[1:2] + [0, 1.60]
        point[3] = color[1]
        point[4] = color[2]
        point[5] = color[3]
        point[6] = color[4]
    elseif(chosen_function == 3)
        point[1:2] = [0.2 -0.26; 0.23 0.22]*point[1:2] + [0, 1.60]
        point[3] = color[1]
        point[4] = color[2]
        point[5] = color[3]
        point[6] = color[4]
    else
        point[1:2] = [-0.15 0.28; 0.26 0.24]*point[1:2] + [0, 0.44]
        point[3] = color[1]
        point[4] = color[2]
        point[5] = color[3]
        point[6] = color[4]
    end

    return point

end

function write_image(p::Array{Pixel,2})
    a = Array{RGB{N0f8},2}(undef,size(p,1), size(p,2))
    for i = 1:length(pixels)
        a[i] = RGB{N0f8}(pixels[i].color[1],
                         pixels[i].color[2],
                         pixels[i].color[3])
    end
    save("fractal.png", a)
end

function define_filter(sum::Float64, filter_size::Int64, fudge::Int64)

    filter = zeros(filter_size, filter_size)
    for i = 1:filter_size
        x = -2.0 + 4.0 * i / filter_size
        for j = 1:filter_size
            y = -2.0 + 4.0 * j / filter_size
            sigma2 = Float64(fudge)/(sum+1.0)
            filter[i,j] = (1.0/sqrt(2*pi*sigma2))*exp((-x^2 -y^2)/(2*sigma2))
        end
    end
    #println(filter)

    return filter
end

# Here, we perform a convolution with a variable filter (of constant size, but
# with varying gaussian widths) and trim the edges later.
function density_estimation(p::Array{Pixel, 2}, filter_size::Int64,
                            fudge::Int64)
    p_temp = zeros(size(p)[1]+filter_size, size(p)[2]+filter_size)
    for i = 1:size(p_temp)[1]
        for j = 1:size(p_temp)[2]
            start_pts = [i-div(filter_size,2); j-div(filter_size,2)]
            end_pts = [i+div(filter_size,2)-1; j+div(filter_size,2)-1]
            if (start_pts[1] < 1)
                start_pts[1] = 1
            end
            if (start_pts[2] < 1)
                start_pts[2] = 1
            end
            if (end_pts[1] > size(p)[1])
                end_pts[1] = size(p)[1]
            end
            if (end_pts[2] > size(p)[2])
                end_pts[2] = size(p)[2]
            end

            sum = 0.0
            for l = start_pts[1]:end_pts[1]
                for m = start_pts[2]:end_pts[2]
                    sum += p[l,m].val
                end
            end

            filter = define_filter(sum, filter_size, fudge)
            element = 0
            i2 = 1
            for l = start_pts[1]:end_pts[1]
                j2 = 1
                for m = start_pts[2]:end_pts[2]
                    element += p[l,m].val * filter[i2,j2]
                    j2 += 1
                end
                i2 += 1
            end
            p_temp[i,j] = element
        end
    end

    for i = 1:size(p)[1]
        for j = 1:size(p)[2]
            p[i,j].val = p_temp[i+div(filter_size,2), j+div(filter_size,2)]
        end
    end

    return p
end

function barnsley_fern(n::Int64)
    point = [0.0, 0.0]
    f = open("barnsley.dat", "w")
    for i = 1:n
        rnd = rand()
        if (rnd <= 0.01)
            point = [0 0; 0 0.16]*point
        elseif(rnd > 0.01 && rnd <= 0.86)
            point = [0.85 0.04; -0.04 0.85]*point + [0, 1.60]
        elseif(rnd > 0.86 && rnd <= 0.93)
            point = [0.2 -0.26; 0.23 0.22]*point + [0, 1.60]
        else
            point = [-0.15 0.28; 0.26 0.24]*point + [0, 0.44]
        end

        println(f, point[1], '\t', point[2])
    end
    close(f)
end

function sierpensky(n::Int64)
    element_x = rand()*2 - 1
    element_y = rand()*2 - 1
    point = [element_x element_y]

    f = open("sierpensky.dat", "w")

    for i = 1:n
        if (i > 20)
            println(f, point[1], '\t', point[2])
        end
        rnd = rand()

        if (rnd <= 0.33)
            point = 0.5*point
        elseif(rnd > 0.33 && rnd <= 0.66)
            point[1] = (point[1] - 1)/2
            point[2] = (point[2])/2
        else
            point[1] = (point[1])/2
            point[2] = (point[2]-1)/2
        end
    end

    close(f)
end

function flame(n::Int64, resx::Int64, resy::Int64,
               scalex::Float64, scaley::Float64, gamma::Float64)
    element_x = rand()*scalex*0.5 - 1
    element_y = rand()*scaley*0.5 - 1
    point = [element_x, element_y, 0.0, 0.0, 0.0]

    pixels = [Pixel([0.0, 0.0, 0.0], 0.0) for i = 1:resx, j = 1:resy]

    pixel_file = open("pixels.dat","w")

    color = zeros(3)
    for i = 1:n
        rnd = rand()
        r = sqrt(point[1]^2 + point[2]^2)
        if (abs(r) < 0.0001)
            r = 0.0001
        end
#=
        if (rnd <= 0.01)
            point = barnsley_flame(point, [1.0, 1.0, 1.0, 1.0], 1)
        elseif(rnd > 0.01 && rnd <= 0.86)
            point = barnsley_flame(point, [1.0, 0, 0, 1.0], 2)
        elseif(rnd > 0.86 && rnd <= 0.93)
            point = barnsley_flame(point, [0, 1.0, 0, 1.0], 3)
        else
            point = barnsley_flame(point, [0, 0, 1.0, 1.0], 4)
        end
=#
        if (rnd <= 0.33)
            #point = sierpensky_flame(point, [0.5, 0.5, 0, 1], 1)
            #point = linear(point, 0.5, [1.0, 0.0, 0.0, 1])
            point = sinusoidal(point, [0.0, 0.0, 1.0, 1])
        elseif (rnd > 0.33 && rnd <= 0.66)
            #point = sierpensky_flame(point, [0, 0.5, 0.5, 1], 2)
            point = linear(point, 0.5, [1.0, 0.0, 0.0, 1])
        else
            #point = sierpensky_flame(point, [0.5, 0, 0.5, 1], 3)
            #point = linear(point, 0.5, [1.0, 0.0, 0.0, 1])
            point = swirl(point, [1.0, 1.0, 1.0, 1])
        end

        # Final function attempts
        final_point = linear(point, .2, [0.0, 1.0, 0.0, 1])
        #final_point = sinusoidal(final_point, [0.0, 0.0, 1.0])

        # blending the colors
        if (i > 20)
            xval = Int(floor((final_point[1]+scalex*0.5) / (scalex / resx)))
            yval = Int(floor((final_point[2]+scaley*0.5) / (scaley / resy)))
            if (xval < resx && yval < resy && xval > 0 && yval > 0)
                pixels[yval,xval].val += 1
                pixels[yval,xval].color = (color+point[3:5]*point[6])*0.5
                color = pixels[yval,xval].color
                pixels[yval,xval].color = (color+final_point[3:5]*point[6])*0.5
            end
        end

    end

    # find maximum in a dumb way
    max = 0;
    for pixel in pixels
        if (pixel.val > 0)
            pixel.val = log10(pixel.val)
        end
        if pixel.val > max
            max = pixel.val
        end
    end

    for i = 1:resy-1
        index_string = ""
        for j = 1:resx-1
            index_string *= string(log10(pixels[i,j].val), '\t')
        end
        println(pixel_file, index_string)
    end
    close(pixel_file)

    # Setting log plot and changing brightness
    for pixel in pixels
        pixel.val /= max
        pixel.color = (pixel.color * pixel.val).^(1/gamma)
        for c in pixel.color
            if c > 1
                c = 1
            end
        end
    end

    #pixels = density_estimation(pixels, 50, 1)

    return pixels

end

#pixels = flame(100000000, 1000, 1000, 4., 4., 1.0)
pixels = flame(1000000, 1000, 1000, 0.75, .75, 2.0)
write_image(pixels)
#sierpensky(1000000)
#barnsley_fern(100000)


text:
using SparseInverseProblems
import SparseInverseProblems: getStartingPoint, parameterBounds, psi, dpsi

struct SimpleExample <: BoxConstrainedDifferentiableModel
  evaluation_points :: Vector{Float64}
  grid_points :: Vector{Float64}
  grid :: Matrix{Float64}
  SimpleExample(p,grid) = new(p,grid,psf(grid',p))
end

psf(theta, points) = @. exp(-(points .- theta).^2/2.0)
deriv_psf(theta, points) = @. exp(-(points .- theta).^2/2.0).*(points .- theta)

psi(s :: SimpleExample, parameters :: Vector{Float64}) = psf(parameters,s.evaluation_points)

dpsi(s :: SimpleExample, parameters :: Vector{Float64}) = reshape(deriv_psf(parameters,s.evaluation_points),length(s.evaluation_points),1)

getStartingPoint(model :: SimpleExample, v :: Vector{Float64}) = [model.grid_points[findmin(model.grid'*v)[2]]]

parameterBounds(model :: SimpleExample) = ([-Inf], [Inf])


text:
# some simple subtypes of graphons

export erdosrenyi

# erdos renyi graphs
function erdosrenyi(p::Number)
	if p>=0 && p<=1
	    return SBM([1], fill(p,1,1))
	else
		error("connection probability p must be between 0 and 1")
	end
end


text:
using TensorOperations: @tensor

function check_value_and_type(x, y, z)
    @test x ≈ y ≈ z
    @test typeof(x) == typeof(y)
end

@testset "Einstein summation" begin
    @testset "no dummy indices" begin
        x = rand(Tensorial.tensortype(Space(4, Symmetry(2, 2))))

        # check type instability
        f(x) = @einsum (1,2,3) -> x[3,2,1]
        @inferred(f(x))

        # permutedims
        check_value_and_type((@einsum (1,2,3) -> x[1,2,3]), permutedims(x, Val((1,2,3))), (@tensor t[1,2,3] := Array(x)[1,2,3]))
        check_value_and_type((@einsum (1,3,2) -> x[1,2,3]), permutedims(x, Val((1,3,2))), (@tensor t[1,3,2] := Array(x)[1,2,3]))
        check_value_and_type((@einsum (2,1,3) -> x[1,2,3]), permutedims(x, Val((2,1,3))), (@tensor t[2,1,3] := Array(x)[1,2,3]))
        check_value_and_type((@einsum (2,3,1) -> x[1,2,3]), permutedims(x, Val((2,3,1))), (@tensor t[2,3,1] := Array(x)[1,2,3]))
        check_value_and_type((@einsum (3,1,2) -> x[1,2,3]), permutedims(x, Val((3,1,2))), (@tensor t[3,1,2] := Array(x)[1,2,3]))
        check_value_and_type((@einsum (3,2,1) -> x[1,2,3]), permutedims(x, Val((3,2,1))), (@tensor t[3,2,1] := Array(x)[1,2,3]))

        v1 = rand(Vec{3})
        v2 = rand(Vec{3})
        check_value_and_type((@einsum (i,j) -> v1[i] * v2[j]), v1 ⊗ v2, (@tensor t[i,j] := Array(v1)[i] * Array(v2)[j]))
        check_value_and_type((@einsum (j,i) -> v1[i] * v2[j]), v2 ⊗ v1, (@tensor t[j,i] := Array(v1)[i] * Array(v2)[j]))
        check_value_and_type((@einsum v1[i] * v2[j]), v1 ⊗ v2, (@tensor t[i,j] := Array(v1)[i] * Array(v2)[j]))

        S1 = rand(SymmetricSecondOrderTensor{3})
        S2 = rand(SecondOrderTensor{3})
        check_value_and_type((@einsum (i,j,k,l) -> S1[i,j] * S2[k,l]), S1 ⊗ S2, (@tensor t[i,j,k,l] := Array(S1)[i,j] * Array(S2)[k,l]))
        check_value_and_type((@einsum (i,k,j,l) -> S1[i,j] * S2[k,l]), permutedims(S1 ⊗ S2, Val((1,3,2,4))), (@tensor t[i,k,j,l] := Array(S1)[i,j] * Array(S2)[k,l]))
        check_value_and_type((@einsum S1[i,j] * S2[k,l]), S1 ⊗ S2, (@tensor t[i,j,k,l] := Array(S1)[i,j] * Array(S2)[k,l]))
    end
    @testset "no free indices" begin
        v1 = rand(Vec{3})
        v2 = rand(Vec{3})
        check_value_and_type((@einsum () -> v1[i] * v2[i]), v1 ⋅ v2, only(@tensor t[] := Array(v1)[i] * Array(v2)[i]))
        check_value_and_type((@einsum v1[i] * v2[i]), v1 ⋅ v2, only(@tensor t[] := Array(v1)[i] * Array(v2)[i]))

        S1 = rand(SymmetricSecondOrderTensor{3})
        S2 = rand(SecondOrderTensor{3})
        check_value_and_type((@einsum () -> S1[i,j] * S2[i,j]), S1 ⊡ S2, only(@tensor t[] := Array(S1)[i,j] * Array(S2)[i,j]))
        check_value_and_type((@einsum () -> S1[i,j] * S2[j,i]), S1 ⊡ S2', only(@tensor t[] := Array(S1)[i,j] * Array(S2)[j,i]))
        check_value_and_type((@einsum S1[i,j] * S2[i,j]), S1 ⊡ S2, only(@tensor t[] := Array(S1)[i,j] * Array(S2)[i,j]))
        check_value_and_type((@einsum S1[i,j] * S2[j,i]), S1 ⊡ S2', only(@tensor t[] := Array(S1)[i,j] * Array(S2)[j,i]))
        check_value_and_type((@einsum S1[i,i]), tr(S1), only(@tensor t[] := Array(S1)[i,i]))
        check_value_and_type((@einsum S1[i,i]/3), mean(S1), only(@tensor t[] := Array(S1)[i,i]/3))
        check_value_and_type((@einsum S1[i,i]/S2[j,j]), tr(S1)/tr(S2), tr(Array(S1))/tr(Array(S2))) # not allowed in TensorOperations
    end
    @testset "mixed" begin
        S1 = rand(SymmetricSecondOrderTensor{3})
        S2 = rand(SecondOrderTensor{3})
        check_value_and_type((@einsum (i,j) -> S1[i,k] * S2[k,j]), S1 ⋅ S2, (@tensor t[i,j] := Array(S1)[i,k] * Array(S2)[k,j]))
        check_value_and_type((@einsum (i,j) -> S1[i,k] * S2[j,k]), S1 ⋅ S2', (@tensor t[i,j] := Array(S1)[i,k] * Array(S2)[j,k]))
        check_value_and_type((@einsum S1[i,k] * S2[k,j]), S1 ⋅ S2, (@tensor t[i,j] := Array(S1)[i,k] * Array(S2)[k,j]))
        check_value_and_type((@einsum (i,j) -> S1[i,k] * S2[j,k] + S1[j,k] * S2[i,k]), S1 ⋅ S2' + S2 ⋅ S1', (@tensor t[i,j] := Array(S1)[i,k] * Array(S2)[j,k] + Array(S1)[j,k] * Array(S2)[i,k]))
        check_value_and_type((@einsum (i,j) -> S1[i,k] * (S2[j,k] + S1[k,j] * S2[i,i])), S1 ⋅ (S2' + S1 * tr(S2)), (@tensor t[i,j] := Array(S1)[i,k] * (Array(S2)[j,k] + Array(S1)[k,j] * Array(S2)[i,i])))

        S3 = rand(Tensor{Tuple{@Symmetry{3,3,3}}})
        v1 = rand(Vec{3})
        check_value_and_type((@einsum (i,j) -> S3[j,k,i] * v1[k]), permutedims(S3, Val((3,1,2))) ⋅ v1, (@tensor t[i,j] := Array(S3)[j,k,i] * Array(v1)[k]))
        check_value_and_type((@einsum (j) -> v1[i] * S3[j,k,i] * v1[k]), (S3 ⋅ v1) ⋅ v1, (@tensor t[j] := Array(v1)[i] * Array(S3)[j,k,i] * Array(v1)[k]))
        check_value_and_type((@einsum v1[i] * S3[j,k,i] * v1[k]), (S3 ⋅ v1) ⋅ v1, (@tensor t[j] := Array(v1)[i] * Array(S3)[j,k,i] * Array(v1)[k]))
    end
    @testset "errors" begin
        S1 = rand(SymmetricSecondOrderTensor{3})
        v = rand(Vec{2})
        @test_throws Exception (@einsum (i) -> S1[i,j] * v[j])
        # @test_throws Exception (@einsum (i,j) -> S1[i,j] + S1[i,j])
        # @test_throws Exception (@einsum (i,j) -> S1[i,j] - S1[i,j])
        # @test_throws Exception (@einsum (i,j) -> S1[i,j] * S1[i,j])
        # @test_throws Exception (@einsum (k) -> S1[i,j] * S1[i,j])
        # @test_throws Exception (@einsum (j) -> S1[i,i] * S1[i,j])
        # @test_throws Exception (@einsum S1[i,i] * S1[i,j])
    end
end


text:
# Create nodes and arcs files.

using JEMSS
using Shapefile
using GeoInterface

dir = @__DIR__

jemssDir = JEMSS.jemssDir # shorthand
include(joinpath(jemssDir, "tools/network/convert_osm_network.jl"))

## parameters

osmFilename = joinpath(jemssDir, "data/cities/auckland/data/roads/osm 2017/auckland_main_roads_tertiary+.osm")

levels = Set(1:5) # osm classes 1:8 : motorway, trunk, primary, secondary, tertiary, unclassified/residential, service, pedestrian/living street

# arc speed (km/hr) for each travel mode and each arc class
v2 = [87, 45, 45, 45, 45] # travel mode 2, classes 1:5
v2 *= 0.73 # reduce speeds
v = [
	round.(Int, v2 / 0.7), # travel mode 1
	round.(Int, v2) # travel mode 2
]
classSpeeds = [Dict([j => Float(v[i][j]) for j = 1:length(v[i])]) for i = 1:length(v)]

# tag which nodes can be used for off-road access
v = [false, false, true, true, true, true, true, true] # v[i] is true if node at either end of arc with class = i can be used for off-road access, false otherwise
classOffRoadAccess = Dict([i => v[i] for i = 1:length(v)])

# only keep nodes and arcs that provide a shortest path between a hospital/station and arcs of given classes
spTagClass = [true, true, true, true, true, false, false, false]

# keep nodes and arcs that provide shortest paths from/to nodes near important locations
# like hospitals and stations, even if spTagClass would have them thrown away
folder = joinpath(jemssDir, "data/cities/auckland/urban/1")
hospitalsFilename = joinpath(folder, "hospitals/1/hospitals_1.csv")
stationsFilename = joinpath(folder, "stations/1/stations_1.csv")
mapFilename = joinpath(folder, "maps/1/map_1.csv")

# shapefile of region border
borderFilename = joinpath(jemssDir, "data/cities/auckland/data/border/urban/1/auckland_urban_border.shp")

# divide arcs that are long into multiple arcs, according to maxArcTravelTime value
maxArcTravelTime = 10/60/60/24 # convert seconds to days

## processing

(nodes, arcs) = readOsmNetworkFile(osmFilename; levels = levels);
convertOsmNetwork!(nodes, arcs; levels=levels, classSpeeds=classSpeeds, classOffRoadAccess=classOffRoadAccess)

print("Dividing arcs")
graphDivideArcs!(nodes, arcs; maxArcTravelTime = maxArcTravelTime)
println(" ... done.")

# get nodes that are nearest to hospitals and stations
begin # get od nodes
hospitals = readHospitalsFile(hospitalsFilename)
stations = readStationsFile(stationsFilename)
mp = readMapFile(mapFilename)
odNodes = Int[]
for items in [hospitals, stations]
	for item in items
		i, dist = findNearestNode(mp, nodes, item.location) # if this is slow, instead use version of findNearestNode that uses grid
		push!(odNodes, i)
	end
end
odNodes = odNodes |> unique |> sort
end # get od nodes

nodeChosen = fill(false, length(nodes));
for arc in arcs
	if spTagClass[arc.fields["osm_class"]]
		nodeChosen[arc.fromNodeIndex] = true
		nodeChosen[arc.toNodeIndex] = true
	end
end

# remove nodes outside region from chosen nodes
shp = open(borderFilename) do data
	read(data, Shapefile.Handle)
end
@assert(length(shp.shapes) == 1)
coords = GeoInterface.coordinates(shp.shapes[1]) # idk what to call this var
rings = unique(vcat(coords...)) # there can be some repeated rings (not sure why), these need to be removed
# check if a point is in given rings (should be in an odd number of rings, to be on land)
function pointInRings(pt::Vector{Float}, rings::Vector{Vector{Vector{Float}}})::Bool
	count = 0
	for ring in rings
		count += Shapefile.inring(pt, ring)
	end
	return isodd(count)
end
println("Checking which nodes (of $(length(nodes))) are inside border")
for i = 1:length(nodes)
	mod(i,1000) == 0 ? print("\r node: $i") : nothing
	if nodeChosen[i]
		location = nodes[i].location
		nodeChosen[i] = pointInRings([location.x, location.y], rings)
	end
end
println("\tdone")
@warn("Code to check that nodes are inside the border is not guaranteed to be correct; please check that output is as expected.")

for odNode in odNodes nodeChosen[odNode] = true end
chosenNodes = findall(nodeChosen);

graphTagSpElts!(nodes, arcs, chosenNodes = chosenNodes, originNodes = odNodes, destNodes = odNodes, dijkstraSpDistmxAllowDict = true)

# only keep elements used in shortest paths
# and arcs with required osm class
nodeFilter(node::Node) = node.fields["in_a_sp"]
arcFilter(arc::Arc) = spTagClass[arc.fields["osm_class"]] || arc.fields["in_a_sp"]
graphRemoveElts!(nodes, arcs, nodeFilter = nodeFilter, arcFilter = arcFilter)

# remove arcs not connected to valid nodes,
# needed as we only kept nodes inside the given region (see border shapefile)
graphRemoveDisconnectedArcs!(nodes, arcs)

# change osm_key field of node to string, to preserve value when writing to file
for node in nodes node.fields["osm_key"] = string(node.fields["osm_key"]) end

# remove some fields before saving, to reduce file size and reduce time when reading file
travelTimes = getArcsTravelTimes(arcs) # need to extract travel times before deleting them from arcs
for field in keys(nodes[1].fields) nodesDeleteField!(nodes, field) end
for field in keys(arcs[1].fields) arcsDeleteField!(arcs, field) end

# save nodes and arcs to file
nodesOutputFilename = joinpath(dir, "nodes.csv")
arcsOutputFilename = joinpath(dir, "arcs.csv")
writeNodesFile(nodesOutputFilename, nodes)
writeArcsFile(arcsOutputFilename, arcs, travelTimes, "directed")


text:
################################################################################
#
#   module LatPhysBase
#
#   --> ABSTRACT TYPE DEFINITION
#           - AbstractSite{L,D}
#           - AbstractBond{L,N}
#           - AbstractUnitcell{S,B}
#           - AbstractLattice{S,B,U}
#
#   --> INTERFACING ABSTRACT TYPES
#           - AbstractSite{L,D}
#               - positions
#               - label
#           - AbstractBond{L,N}
#               - connecting identifiers
#               - label
#               - wrap
#           - AbstractUnitcell{S,B}
#               - Bravais lattice vectors
#               - sites
#               - bonds
#           - AbstractLattice{S,B,U}
#               - Unitcell
#               - Bravais lattice vectors
#               - sites
#               - bonds
#
#   --> NAIVE STRUCT DEFINITIONS
#           - Site{L,D}
#           - Bond{L,N}
#           - Unitcell{S,B}
#           - Lattice{S,B,U}
#
#   --> IMPLEMENTING JULIA.BASE FUNCTIONS
#           - show
#           - print
#
################################################################################

# module start
module LatPhysBase

# uses LinearAlgebra
using LinearAlgebra
# using HDF5 for saving
using HDF5

# import functions from Base to be overwritten
import Base.show
import Base.similar
import Base.+

# ABSTRACT TYPES INCLUDING INTERFACE DEFINITION

# Sites
include("abstract_site.jl")
# Bonds
include("abstract_bond.jl")
# Unitcells
include("abstract_unitcell.jl")
# Lattices
include("abstract_lattice.jl")

# CONCRETE TYPES INCLUDING INTERFACE IMPLEMENTATION

# Sites
include("concrete_site.jl")
# Bonds
include("concrete_bond.jl")
# Unitcells
include("concrete_unitcell.jl")
# Lattices
include("concrete_lattice.jl")

# DEFAULT LABELS
include("default_labels.jl")

# CUSTOM NICE PRINTING
include("show.jl")

# module end
end


text:
@testset "LU solver" begin
    n = 20
    m = 10
    A = rand(n, n)
    X = rand(n, m)
    B = rand(n, m)
    x = rand(n)
    b = rand(n)

    sol = ContactImplicitMPC.lu_solver(A)
    ContactImplicitMPC.linear_solve!(sol, X, A, B)
    @test norm(A * X - B, Inf) < 1e-10

    sol = ContactImplicitMPC.lu_solver(A)
    ContactImplicitMPC.linear_solve!(sol, x, A, b)
    @test norm(A * x - b, Inf) < 1e-10
end


text:
function overlaps(t1::Vector, t2::Vector)
    i = j = 1
    idx1 = Int[]
    idx2 = Int[]
    while i <= length(t1) && j <= length(t2)
        if t1[i] > t2[j]
            j += 1
        elseif t1[i] < t2[j]
            i += 1
        else
            push!(idx1, i)
            push!(idx2, j)
            i += 1
            j += 1
        end
    end
    (idx1, idx2)
end

function sorted_unique_merge(a::Vector, b::Vector)

		i, na, j, nb = 1, length(a), 1, length(b)
		c = similar(a, 0)

		while (i <= na) && (j <= nb)
				if a[i] < b[j]
						push!(c, a[i])
						i += 1
				elseif a[i] > b[j]
						push!(c, b[j])
						j += 1
				else
						push!(c, a[i])
						i += 1
						j += 1
				end #if
		end

		append!(c, a[i:end])
		append!(c, b[j:end])

    return c

end #sorted_unique_merge

function setcolnames!(ta::TimeArray, colnames::Vector)
    length(colnames) == length(ta.colnames) ? ta.colnames[:] = colnames :
    length(colnames) > 0 && error("colnames supplied is not correct size")
    return ta
end #setcolnames!


text:
using Distributed

ret=pmap(x->x*2, 1:200000)

println(ret)

max_stars_repo_name: terasakisatoshi/juliaExer

max_issues_repo_name: terasakisatoshi/juliaExer

max_forks_repo_name: terasakisatoshi/juliaExer


text:
##############################################################################
##
## Iteration: eachrow, eachcol
##
##############################################################################

# TODO: Reconsider/redesign eachrow -- ~100% overhead

# Iteration by rows
immutable DTRowIterator{T <: AbstractDataTable}
    dt::T
end
eachrow(dt::AbstractDataTable) = DTRowIterator(dt)

Base.start(itr::DTRowIterator) = 1
Base.done(itr::DTRowIterator, i::Int) = i > size(itr.dt, 1)
Base.next(itr::DTRowIterator, i::Int) = (DataTableRow(itr.dt, i), i + 1)
Base.size(itr::DTRowIterator) = (size(itr.dt, 1), )
Base.length(itr::DTRowIterator) = size(itr.dt, 1)
Base.getindex(itr::DTRowIterator, i::Any) = DataTableRow(itr.dt, i)
Base.map(f::Function, dtri::DTRowIterator) = [f(row) for row in dtri]

# Iteration by columns
immutable DTColumnIterator{T <: AbstractDataTable}
    dt::T
end
eachcol(dt::AbstractDataTable) = DTColumnIterator(dt)

Base.start(itr::DTColumnIterator) = 1
Base.done(itr::DTColumnIterator, j::Int) = j > size(itr.dt, 2)
Base.next(itr::DTColumnIterator, j::Int) = ((_names(itr.dt)[j], itr.dt[j]), j + 1)
Base.size(itr::DTColumnIterator) = (size(itr.dt, 2), )
Base.length(itr::DTColumnIterator) = size(itr.dt, 2)
Base.getindex(itr::DTColumnIterator, j::Any) = itr.dt[:, j]
function Base.map(f::Function, dtci::DTColumnIterator)
    # note: `f` must return a consistent length
    res = DataTable()
    for (n, v) in eachcol(dtci.dt)
        res[n] = f(v)
    end
    res
end


text:
module Steve

using JuLIP

# Functions to calculate each of the matrices in the BBS approach.

function CalcQ(c,t,ω)
   c12, c44 = c[1], c[2]

    A1=c[1]-c[2]+2*c[3];
    B1=c[1]+c[2];

    θ=acos(t[3]);
    ϕ=atan2(t[2],t[1]);

    com=cos(ω);
    som=sin(ω);
    cth=cos(θ);
    sth=sin(θ);
    cph=cos(ϕ);
    sph=sin(ϕ);

    n1=-com*cph*cth+som*sph;
    n2=-cph*som-com*cth*sph;
    n3=com*sth;

    nn = [A1*n1^2+c44 B1*n1*n2    B1*n1*n3;
          B1*n1*n2    A1*n2^2+c44 B1*n2*n3;
          B1*n1*n3    B1*n2*n3    A1*n3^2+c44];

    return -inv(nn);   # TODO: switch to pinv
end

function CalcS(c,t,ω)
   c44 = c[2]
   c12 = c[1]

    A1=c[1]-c[2]+2*c[3];
    B1=c[1]+c[2];

    θ=acos(t[3]);
    ϕ=atan2(t[2],t[1]);

    com=cos(ω);
    som=sin(ω);
    cth=cos(θ);
    sth=sin(θ);
    cph=cos(ϕ);
    sph=sin(ϕ);

    n1=-com*cph*cth+som*sph;
    n2=-cph*som-com*cth*sph;
    n3=com*sth;

    m1=cph*cth*som+com*sph;
    m2=-com*cph+cth*som*sph;
    m3=-som*sth;

    nn = [A1*n1^2+c44 B1*n1*n2    B1*n1*n3;
          B1*n1*n2    A1*n2^2+c44 B1*n2*n3;
          B1*n1*n3    B1*n2*n3    A1*n3^2+c44];

    nm = [A1*m1*n1        c12*m2*n1+c44*m1*n2 c12*m3*n1+c44*m1*n3;
          c12*m1*n2+c44*m2*n1 A1*m2*n2            c12*m3*n2+c44*m2*n3;
          c12*m1*n3+c44*m3*n1 c12*m2*n3+c44*m3*n2 A1*m3*n3];

    return -nn\nm;;
end

function CalcB(c,t,ω)
   c12, c44 = c[1], c[2]

A1=c[1]-c[2]+2*c[3];
B1=c[1]+c[2];

θ=acos(t[3]);
ϕ=atan2(t[2],t[1]);

com=cos(ω);
som=sin(ω);
cth=cos(θ);
sth=sin(θ);
cph=cos(ϕ);
sph=sin(ϕ);

n1=-com*cph*cth+som*sph;
n2=-cph*som-com*cth*sph;
n3=com*sth;

m1=cph*cth*som+com*sph;
m2=-com*cph+cth*som*sph;
m3=-som*sth;

# construct (mm) matrix
mm=[A1*m1*m1+c44        c12*m1*m2+c44*m2*m1 c12*m1*m3+c44*m3*m1;
    c12*m1*m2+c44*m2*m1 A1*m2*m2+c44        c12*m2*m3+c44*m3*m2;
    c12*m1*m3+c44*m3*m1 c12*m2*m3+c44*m3*m2 A1*m3*m3+c44];

# construct (nn) matrix
nn = [A1*n1^2+c44 B1*n1*n2    B1*n1*n3;
      B1*n1*n2    A1*n2^2+c44 B1*n2*n3;
      B1*n1*n3    B1*n2*n3    A1*n3^2+c44];

# construct (mn) matrix
mn = [A1*m1*n1            c12*m1*n2+c44*m2*n1 c12*m1*n3+c44*m3*n1;
      c12*m2*n1+c44*m1*n2 A1*m2*n2            c12*m2*n3+c44*m3*n2;
      c12*m3*n1+c44*m1*n3 c12*m3*n2+c44*m2*n3 A1*m3*n3];

nm = mn';

return mm-mn*(nn\nm);
end

"""
`CalcDisp(c,t,b,S::Array{Float64,2},B::Array{Float64,2},xdis,xfld)`

returns 3 component displacement vector u, input arguments are:
c = vector of elastic constants:
t = unit vector of dislocation line direction
b = Burgers' vector
S = matrix S
B = matrix B
xdis = dislocation position (point it passes through, typically (0 0 0))
xfld = field point, in plane perp to dislocation. For t = [111]/sqrt(3),
this will be A(1 1 -2) + B(1,-1,0)
"""
function CalcDisp(c,t,b,S::Array{Float64,2},B::Array{Float64,2}, xdis,
                  xfld::AbstractVector{Float64})

    x = xfld - xdis;

    ω = atan2(x[2], x[1]); # angle from some reference datum
    # NB this is where the non-uniqueness of the displacement field comes in

    Qintegrand = om -> CalcQ(c,t,om)
    Sintegrand = om -> CalcS(c,t,om)
    I1, E1 = quadgk(Qintegrand,0,ω)
    I2, E2 = quadgk(Sintegrand,0,ω)

    # these are the terms in the expression for the displacement in Balluffi about p250 I think!

    dist_2_line = x - dot(x, t) * t
    term1 = -log(norm(dist_2_line))*S;
    term2 = 4π*I1*B;
    term3 = -I2*S;

    return (1/(2π))*(term1+term2+term3)*b;
end

function CalcDisp( Cvoigt, t, b, Xfld::AbstractVector{JVecF};
                   xdis = JVecF(0,0,0) )
   t = t / norm(t)
   c = [Cvoigt[1,2], Cvoigt[4,4], (Cvoigt[1,1] - Cvoigt[1,2]) / 2]
   S = CalcS(c,t,2π) / (2*pi)
   B = CalcB(c,t,2π) / (8*pi*pi)
   return [ JVecF(CalcDisp(c, t, b, S, B, xdis, x)) for x in Xfld ]
end

end


text:
using MultivariateMeasures
using Test

@testset "MultivariateMeasures.jl" begin

    @testset "LKJCholesky" begin
        D = LKJCholesky{(:k,:η)}
        par = transform(asparams(D, (k=4,)), randn(1))
        d = D(merge((k=4,),par))
        # @test params(d) == par

        η  = par.η
        logη = log(η)

        y = rand(d)
        η = par.η
        ℓ = logdensity(LKJCholesky(4,η), y)
        @test ℓ ≈ logdensity(LKJCholesky(k=4,logη=logη), y)
    end
end


text:
using NLsolve: nlsolve

export solve!
export LSRK144, LSRK54
export RLSRK144, RLSRK54

function solve!(q, timeend, solver;
                after_step::Function = (x...) -> nothing,
                after_stage::Function = (x...) -> nothing)
  finalstep = false
  step = 0
  while true
    step += 1
    time = solver.time
    if time + solver.dt >= timeend
      solver.dt = timeend - time
      finalstep = true
    end
    dostep!(q, solver, after_stage)
    after_step(step, time, q)
    finalstep && break
  end
end

mutable struct LSRK{FT, AT, NS, RHS}
  dt::FT
  time::FT
  rhs!::RHS
  dq::AT
  rka::NTuple{NS, FT}
  rkb::NTuple{NS, FT}
  rkc::NTuple{NS, FT}

  function LSRK(rhs!, rka, rkb, rkc, q, dt, t0)
      FT = eltype(eltype(q))
      dq = similar(q)
      fill!(dq, zero(eltype(q)))
      AT = typeof(q)
      RHS = typeof(rhs!)
      new{FT, AT, length(rka), RHS}(FT(dt), FT(t0), rhs!, dq, rka, rkb, rkc)
  end
end

function dostep!(q, lsrk::LSRK, after_stage)
  @unpack rhs!, dq, rka, rkb, rkc, dt, time = lsrk
  for stage = 1:length(rka)
    stagetime = time + rkc[stage] * dt
    dq .*= rka[stage]
    rhs!(dq, q, stagetime)
    @. q += rkb[stage] * dt * dq
    after_stage(stagetime, q)
  end
  lsrk.time += dt
end

function LSRK54(rhs!, q, dt; t0 = 0)
  rka, rkb, rkc = coefficients_lsrk54()
  LSRK(rhs!, rka, rkb, rkc, q, dt, t0)
end

function LSRK144(rhs!, q, dt; t0 = 0)
  rka, rkb, rkc = coefficients_lsrk144()
  LSRK(rhs!, rka, rkb, rkc, q, dt, t0)
end

mutable struct RLSRK{FT, AT, NS, RHS}
  dt::FT
  time::FT
  γ::FT
  rhs!::RHS
  dq::AT
  q0::AT
  k::AT
  rka::NTuple{NS, FT}
  rkb::NTuple{NS, FT}
  rkb_full::NTuple{NS, FT}
  rkc::NTuple{NS, FT}

  function RLSRK(rhs!, rka, rkb, rkc, q, dt, t0)
      FT = eltype(eltype(q))
      dq = similar(q)
      fill!(dq, zero(eltype(q)))
      q0 = similar(q)
      k = similar(q)
      # construct standard RK b coefficients
      rkb_full = zeros(FT, length(rkb))
      rkb_full[end] = rkb[end]
      for i in length(rkb)-1:-1:1
        rkb_full[i] = rka[i+1] * rkb_full[i+1] + rkb[i]
      end
      γ = FT(1)

      AT = typeof(q)
      RHS = typeof(rhs!)
      new{FT, AT, length(rka), RHS}(FT(dt), FT(t0), γ, rhs!, dq, q0, k,
                                    rka, rkb, Tuple(rkb_full), rkc)
  end
end

function dostep!(q, rlsrk::RLSRK{FT}, after_stage) where {FT}
  @unpack rhs!, dq, q0, k, dt, time = rlsrk
  @unpack rka, rkb, rkb_full, rkc = rlsrk

  q0 .= q
  η0 = entropyintegral(rhs!, q0)
  dη = -zero(FT)

  for stage = 1:length(rka)
    dq .*= rka[stage]
    stagetime = time + rkc[stage] * dt
    rhs!(k, q, stagetime; increment = false)
    dq .+= k
    dη += rkb_full[stage] * dt * entropyproduct(rhs!, q, k)

    @. q += rkb[stage] * dt * dq
    after_stage(stagetime, q)
  end

  @. k = q - q0
  function r(F, γ)
    @. q = q0 + γ[1] * k
    F[1] = entropyintegral(rhs!, q) - η0 - γ[1] * dη
  end
  function dr(J, γ)
    @. q = q0 + γ[1] * k
    J[1] = entropyproduct(rhs!, q, k) - dη
  end
  sol = nlsolve(r, dr, [rlsrk.γ]; ftol=10eps(FT))
  rlsrk.γ = sol.zero[1]
  q .= q0 .+ rlsrk.γ * k

  rlsrk.time += dt
end

function RLSRK54(rhs!, q, dt; t0 = 0)
  rka, rkb, rkc = coefficients_lsrk54()
  RLSRK(rhs!, rka, rkb, rkc, q, dt, t0)
end

function RLSRK144(rhs!, q, dt; t0 = 0)
  rka, rkb, rkc = coefficients_lsrk144()
  RLSRK(rhs!, rka, rkb, rkc, q, dt, t0)
end

function coefficients_lsrk54()
  rka = (
    (0),
    (-567301805773 // 1357537059087),
    (-2404267990393 // 2016746695238),
    (-3550918686646 // 2091501179385),
    (-1275806237668 // 842570457699),
  )

  rkb = (
    (1432997174477 // 9575080441755),
    (5161836677717 // 13612068292357),
    (1720146321549 // 2090206949498),
    (3134564353537 // 4481467310338),
    (2277821191437 // 14882151754819),
  )

  rkc = (
    (0),
    (1432997174477 // 9575080441755),
    (2526269341429 // 6820363962896),
    (2006345519317 // 3224310063776),
    (2802321613138 // 2924317926251),
  )

  rka, rkb, rkc
end

function coefficients_lsrk144()
  rka = (
     0,
    -0.7188012108672410,
    -0.7785331173421570,
    -0.0053282796654044,
    -0.8552979934029281,
    -3.9564138245774565,
    -1.5780575380587385,
    -2.0837094552574054,
    -0.7483334182761610,
    -0.7032861106563359,
     0.0013917096117681,
    -0.0932075369637460,
    -0.9514200470875948,
    -7.1151571693922548,
  )

  rkb = (
    0.0367762454319673,
    0.3136296607553959,
    0.1531848691869027,
    0.0030097086818182,
    0.3326293790646110,
    0.2440251405350864,
    0.3718879239592277,
    0.6204126221582444,
    0.1524043173028741,
    0.0760894927419266,
    0.0077604214040978,
    0.0024647284755382,
    0.0780348340049386,
    5.5059777270269628,
  )

  rkc = (
    0,
    0.0367762454319673,
    0.1249685262725025,
    0.2446177702277698,
    0.2476149531070420,
    0.2969311120382472,
    0.3978149645802642,
    0.5270854589440328,
    0.6981269994175695,
    0.8190890835352128,
    0.8527059887098624,
    0.8604711817462826,
    0.8627060376969976,
    0.8734213127600976,
  )

  rka, rkb, rkc
end


text:
function test_withoutjld2()
  namewithjld2 = "blahblah.jld2"
  namewithoutjld2 = "blahblah"

  return FourierFlows.withoutjld2(namewithjld2) == namewithoutjld2 && FourierFlows.withoutjld2(namewithoutjld2) == namewithoutjld2
end

function test_uniquepath()
  path  = "foo.jld2"
  path1 = "foo_1.jld2"
  path2 = "foo_2.jld2"
  for filename in (path, path1, path2)
    if isfile(filename); rm(filename); end
  end
  touch(path)
  test1 = FourierFlows.uniquepath(path) == path1
  touch(path1)
  test2 = FourierFlows.uniquepath(path) == path2
  return test1 && test2
end

function test_outputconstructor(dev::Device=CPU())
  prob = Problem(nx=32, Lx=2π, kappa=1e-2, dt=1e-7, stepper="ForwardEuler", dev=dev)
  filename = joinpath(".", "testoutput.jld2")
  get_sol(prob) = prob.sol
  get_c(prob) = prob.vars.c

  out1 = Output(prob, filename, (:sol, get_sol))
  out2 = Output(prob, filename, (:sol, get_sol), (:c, get_c))

  return  typeof(out1)<:Output && typeof(out2)<:Output
end

function test_getindex(dev::Device=CPU())
  prob = Problem(nx=32, Lx=2π, kappa=1e-2, dt=1e-7, stepper="ForwardEuler", dev=dev)
  filename = joinpath(".", "testoutput.jld2")

  ctest = devzeros(dev, Float64, (prob.grid.nx, ))
  ctest[3] = π
  prob.vars.c .= ctest

  get_c(prob) = prob.vars.c
  out = Output(prob, filename, (:c, get_c))

  return isapprox(ctest, getindex(out, :c), rtol=rtol_output)
end

function test_saveproblem_saveoutput(dev::Device=CPU())
  nx = 32
  prob = Problem(nx=nx, Lx=2π, kappa=1e-2*ones(nx), dt=1e-7, stepper="ForwardEuler", dev=dev)
  filename = joinpath(".", "testoutput.jld2")
  if isfile(filename); rm(filename); end

  ctest = devzeros(dev, Float64, (prob.grid.nx, ))
  ctest[3] = π
  prob.vars.c .= ctest

  get_c(prob) = collect(prob.vars.c)

  out = Output(prob, filename, (:c, get_c))

  saveproblem(out)

  saveoutput(out)

  file = jldopen(filename)

  return isfile(filename) && isapprox(file["snapshots"]["c"]["0"], collect(ctest), rtol=rtol_output) && isapprox(file["grid"]["Lx"], prob.grid.Lx, rtol=rtol_output) && isapprox(file["eqn"]["L"], prob.eqn.L, rtol=rtol_output)
end

function test_saveproblemTwoDGrid(dev::Device=CPU())
       nx = 32
       Lx = 2π
    kappa = 1e-2
       dt = 1e-7
  stepper = "ForwardEuler"

     grid = TwoDGrid(dev, nx, Lx)
   params = FourierFlows.Diffusion.Params(dev, kappa)
     vars = FourierFlows.Diffusion.Vars(dev, grid)

      # manually construct an Equation for a 2D grid
      L = zeros(dev, Float64, (grid.nkr, grid.nl))
      @. L = -kappa * grid.kr^2
      eqn = FourierFlows.Equation(L, FourierFlows.Diffusion.calcN!, grid)

     prob = FourierFlows.Problem(eqn, stepper, dt, grid, vars, params, dev)

  filename = joinpath(".", "testoutput.jld2")
  if isfile(filename); GC.gc(); rm(filename); end

  get_c(prob) = prob.vars.c
  out = Output(prob, filename, (:c, get_c))

  saveproblem(out)

  file = jldopen(filename)

  return isfile(filename) && isapprox(file["grid"]["Ly"], prob.grid.Ly, rtol=rtol_output) && isapprox(file["eqn"]["L"], collect(prob.eqn.L), rtol=rtol_output)
end

function test_savediagnostic(dev::Device=CPU())
  filename = joinpath(".", "testoutput.jld2")
  if isfile(filename); GC.gc(); rm(filename); end

  prob = Problem(nx=6, Lx=2π, dev=dev)
  getone(prob) = 1
  nsteps=100
  freq=1
  ndata=ceil(Int, (nsteps+1)/freq)
  d = Diagnostic(getone, prob, nsteps=nsteps, freq=freq, ndata=ndata)
  stepforward!(prob, d, nsteps)
  expectedsteps = cat([0], freq:freq:nsteps, dims=1)

  get_c(prob) = prob.vars.c
  out = Output(prob, filename, (:c, get_c))
  saveproblem(out)
  savediagnostic(d, "mydiagnostic", filename)

  file = jldopen(filename)

  return isfile(filename) && file["diags"]["mydiagnostic"]["steps"]==expectedsteps
end


text:
# testing multicolumn read

import XLSX
using DataFrames

multicolumns = XLSX.multicolumn_range("B,D:F")

out = XLSX.readtable(
    "/Users/delnatan/Repository/XLSX.jl/data/multicolumn.xlsx",
    "Sheet1",
    multicolumns;
    header=true,
    infer_eltypes=true
    )