text:
using Random
using Parameters
using Debugger

using Statistics
using Printf
using Logging
using LoggingExtras

#using Traceur
#using Profile

Random.seed!(1)

io = open("log.txt", "a")
logio = SimpleLogger(io)
logerr = ConsoleLogger(stderr)
#logger = FileLogger(Dict(Logging.Info => "info.log", Logging.Error => "error.log"))
#logger = IOLogger()
#global_logger(logger)

demux_logger = TeeLogger(
    #MinLevelLogger(FileLogger("info.log"), Logging.Info),
    MinLevelLogger(logio, Logging.Info),
    MinLevelLogger(logerr, Logging.Info),
);

export ActMdp
include("mdp.jl")
include("mcts.jl")
include("search.jl")
include("mpc.jl")

abstract type Scn end

@with_kw mutable struct ScnMulti{T} <: Scn
	nobjs::Int32 = 10
	start = Vector{T}[]
	train_set = Vector{T}[]
	dev_set = Vector{T}[]
	test_set = Vector{T}[]
end

"""
scn init
"""
function init!(scn::ScnMulti;
				nobjs=10)

	scn.nobjs = nobjs
	scn.start = randomStartState(scn)
	scn.train_set = [randomStartState(scn) for i in 1:10000]
	scn.dev_set = [randomStartState(scn) for i in 1:100]
	scn.test_set = [randomStartState(scn) for i in 1:100]
end

"""
scn randomStartState
"""
function randomStartState(scn::ScnMulti)
	state = []
	for n in 1:floor(Int, scn.nobjs/2)
		x = convert(Float64, rand(0:50))
		y = rand(25:190)
		vx = rand(10:25)
		vy = rand(0:5)
		obj = [x, y, vx, vy]
		push!(state, obj)
	end

	for n in 1:floor(Int, scn.nobjs/2)
		x = convert(Float64, rand(150:200))
		y = rand(25:190)
		vx = -rand(10:25)
		vy = -rand(0:5)
		obj = [x, y, vx, vy]
		push!(state, obj)
	end
	return state
end

"""
scn projectState: onto x=100
"""
function projectState(scn::ScnMulti, state)::Vector{Tuple{Float64, Float64}}
	obstacles = Tuple{Float64, Float64}[] # list of (s, t) tuples
	for s in state
		x,y,vx,vy = s
		t_cross = floor( ((100-x)/vx)/0.25 ) * 0.25 # TODO CLEAN THAT
		y_cross = y + t_cross * vy
		#push!(obstacles, (y_cross, t_cross))
		push!(obstacles, (t_cross, y_cross))
	end
	sort!(obstacles)
	# return [obstacles[1]] # just for tests/checks XXX
	return obstacles
end

# julia 1.3.1 used
# run: julia scn.jl
# or : julia scn.jl dp
# or : julia scn.jl mcts

# 1) Create a scenario or scene
scn = ScnMulti{Float64}()
init!(scn)
println(scn.nobjs)

# 2) Get projected state
pstate = projectState(scn, scn.start)
println(pstate)
#exit()

# 3) Setup ActMdp with list of spatio-temporal obstacles
#mdp = ActMdp{Float64}()
mdp = ActMdp()
init!(mdp, obstacles=pstate)
println(mdp.obstacles)
println(mdp.goal)

println(isEnd(mdp, startState(mdp)))
println(actions(mdp, startState(mdp)))
for i in 1:10
	println(pi0(mdp, startState(mdp)))
end

s=startState(mdp)

for i in 1:100
	a = pi0(mdp, s)
	sp, r, done = sampleSuccReward(mdp, s, a)
	println("(s, a, r, sp)=($s, $a, $r, $sp)")
	global s = sp # cf https://discourse.julialang.org/t/error-variable-inside-a-loop/21504
	if done>0#[1]
		println(done)
		break
	end
end

# Default to MCTS
length(ARGS) > 0 ? algo=ARGS[1] : algo="mcts"

#mcts = Mcts{Int64, Float64}()

if algo == "mcts"
	agent = Mcts()
	init!(agent, mdp=mdp, piRollout=pi0)
	print(agent)
	print(agent.mdp)
elseif algo == "mpc"
	mdlParams = MdlParams()
	mpcParams = MpcParams()
	agent = MpcModel(mpcParams, mdlParams)
	println("mpc agent: ", agent)
elseif algo == "dp"
	actSearch = actDP
elseif algo == "ucs"
	actSearch = actUCS
end

# test_algo.py

metric_scores = []
metric_hardbrakes = []
metric_steps_to_goal = []
metric_steps_to_collision = []
metric_speed_at_collision = []
metric_runtime = []
success = 0

for (num_s, scn_start) in enumerate(scn.dev_set)
	#println("num_s = $num_s")
	#if num_s  != 95
	#	continue
	#end
	println("Test $num_s: success=$success/$(num_s-1) scn_start=$scn_start")
	obstacles = projectState(scn, scn_start)
	init!(mdp, obstacles=obstacles)
	#println("obstacles=$(mdp.obstacles)")
	s = mdp.state
	score, hardbrakes, speed_at_collision = 0, 0, nothing
	sequence = []
	runtimes = []
	if algo == "mcts" || algo == "mpc"
		algo == "mcts" && resetTree!(agent)
		for steps in 1:1000
			#runtime = @elapsed a = pi0(mdp, s)
			#@time a = act(mcts, s)
			#@trace a = act(mcts,s)
			#@trace(act(mcts, s), modules=[Main])
			#@profile a = act(mcts,s)
			runtime = @elapsed a = act(agent, s, obstacles)
			push!(runtimes, runtime)
			#println("(s, a)=($s, $a)")
			push!(sequence, a)
			if a <= -4.0
				hardbrakes += 1
			end
			sp, r, done = sampleSuccReward(mdp, s, a)
			println("profiles: $(sp[1]), $(sp[2]), $a")
			score += r
			if done>0#[1]
				if done==2 #[2] == "goal"
					global success += 1
					push!(metric_steps_to_goal, steps)
				elseif done==1 #[2] == "collision"
					push!(metric_steps_to_collision, steps)
					speed_at_collision = sp[2]
					push!(metric_speed_at_collision, speed_at_collision)
				end
				break
			end
			s = sp
		end
		runtime = mean(runtimes)
	elseif algo == "dp" || algo == "ucs"
		mdp.tMax = 12.0
		#@time cost, history = actDP(mdp, s)
		runtime = @elapsed score, sequence, s = actSearch(mdp, s)
		steps = s[3]/mdp.dt + 1
		hardbrakes = length(filter(x -> x<=-4.0, sequence))
		if score < 1
			global success += 1
			push!(metric_steps_to_goal, steps)
		else
			speed_at_collision = s[2]
			push!(metric_steps_to_collision, steps)
			push!(metric_speed_at_collision, speed_at_collision)
		end
	end
	push!(metric_runtime, runtime)
	push!(metric_scores, score)
	push!(metric_hardbrakes, hardbrakes)
	with_logger(demux_logger) do
		@info("Test $num_s: score $score hardbrakes $hardbrakes TTG $(s[3]) runtime $runtime collision_speed $speed_at_collision")
		@info("  actions $sequence")
	end

end

score = mean(metric_scores)
success_rate = success/length(scn.dev_set)
runtime = mean(metric_runtime)
hardbrakes = mean(metric_hardbrakes)
length(metric_steps_to_goal) > 0 ? steps_to_goal = mean(metric_steps_to_goal) : steps_to_goal = nothing
length(metric_steps_to_collision) > 0 ? steps_to_collision = mean(metric_steps_to_collision) : steps_to_collision = nothing
length(metric_speed_at_collision) > 0 ? speed_at_collision = mean(metric_speed_at_collision) : speed_at_collision = nothing

with_logger(demux_logger) do
	@info("METRICS mean values => score $score, success_rate $success_rate, runtime $runtime, hardbrakes $hardbrakes, steps_to_goal $steps_to_goal, steps_to_collision $steps_to_collision, speed_at_collision $speed_at_collision")
end

close(io)


text:
# matmul.jl
module Matmul
export foo!
include("row_major.jl")

function gemm(x, y, z)
    @inbounds @fastmath for m ∈ axes(x, 1), n ∈ axes(y, 2)
        zmn = zero(eltype(z))
        for k ∈ axes(x, 2)
            zmn += x[m, k] * y[k, n]
        end
        z[m, n] = zmn
    end
    return z
end

# z is output, should use . to inplace
# julia array is column-major, numpy aray is row-major
# user should change julia or numpy's layout to keep same behavior
#= EXAMPLE
A[2,3]               B[3,4]               C[2,4]
NUMPY:
[[1, 2, 3]       [[1, 2, 3, 4]         [[38, 44, 50,  56]
 [4, 5, 6]]       [5, 6, 7, 8]          [83, 98, 113,128]]
                  [9,10,11,12]]
JULIA:
inputs read numpy data from memory:
[[1, 3, 5]       [[1, 4, 7,10]
 [2, 4, 6]]       [2, 5, 8,11]
                  [3, 6, 9,12]]
inputs after reshape(reverse(shape)):
[[1, 4]          [[1, 5, 9]
 [2, 5]           [2, 6,10]
 [3, 6]]          [3, 7,11]
                  [4, 8,12]]
inputs after transpose/permutedims:
[[1, 2, 3]       [[1, 2, 3, 4]         [[38, 44, 50,  56]
 [4, 5, 6]]       [5, 6, 7, 8]          [83, 98, 113,128]]
                  [9,10,11,12]]
output after transpose/permutedims:
                                       [[38, 83]
                                        [44, 98]
                                        [50,113]
                                        [56,128]
output after reshape:
                                       [[38, 50, 83, 113]
                                        [44, 56, 98, 128]]
output read numpy data from memory:
                                       [[38, 44, 50,  56]
                                        [83, 98,113, 128]]
=#
function foo!(x, y, z)
    x = change_input_to_row_major(x)
    y = change_input_to_row_major(y)
    z .= gemm(x, y, z)
    z .= change_output_to_row_major(z)
end

end

ext: jl


text:
# Fused Kolmogorov Filter for Variable Screening as described in the paper Mai2014:
#  http://arxiv.org/pdf/1403.7701v2.pdf
#
# Julia implementation by Robert Feldt, robert.feldt@gmail.com
#

# Kolmogorov-Smirnov Statistic, i.e. supremum of differences between empirical cdfs.
function ksstatistic{T<:Real, S<:Real}(x::AbstractVector{T}, y::AbstractVector{S})
  nx, ny = length(x), length(y)
  sortidx = sortperm([x; y])
  pdf_diffs = [ones(nx)/nx; -ones(ny)/ny][sortidx]
  cdf_diffs = cumsum(pdf_diffs)
  deltap = maximum(cdf_diffs)
  deltan = -minimum(cdf_diffs)
  delta = max(deltap, deltan)
  return delta
end

# Rank variables by their fused kolmogorov filter statistic.
#   varrank, fkfstats, kfstats = fused_kolmogorov_filter(X, y)
function fused_kolmogorov_filter{T <: Real, S <: Real}(X::AbstractMatrix{T}, y::AbstractVector{S};
  partitions = 3:6)

  # Convert vector of bools to an int vector
  if typeof(y[1]) <: Bool
    y = map(b -> b ? 2 : 1, y)
  end

  # Calc actual number of partitions to use based on number of unique y values
  num_ys = length(unique(y))
  minpartitions = min(minimum(partitions), num_ys)
  maxpartitions = min(maximum(partitions), num_ys)
  @assert minpartitions > 1
  numpartitions = maxpartitions - minpartitions + 1

  n,p = size(X)

  # Now loop and calc the fkfstatistics per variable (column) in X
  kfstats = zeros(numpartitions, p)
  for G in minpartitions:maxpartitions
    splitvalues = quantile(y, cumsum(ones(G)/G)[1:end-1])
    slice_idxs = slice_indices(y, splitvalues)
    idxsperslice = map(s -> find(i -> i == s, slice_idxs), 1:G)
    gindex = G - minpartitions + 1
    for pi in 1:p
      xs = X[:,pi]
      # For all pairwise combinations of slices we calc the ksstatistic, the max of them
      # is the Kolmogorov filter statistic
      kfs = map(combinations(1:G, 2)) do slices
        lidxs, midxs = idxsperslice[slices[1]], idxsperslice[slices[2]]
        ksstatistic(xs[lidxs], xs[midxs])
      end
      kfstats[gindex, pi] =  maximum(kfs)
    end
  end

  # Fuse the kfstats by summing
  fkfstats = sum(kfstats, 1)[:]

  return sortperm(fkfstats, rev=true), fkfstats, kfstats
end

# For each value in y find the slice it belongs to given the max slice value
# in splitvalues.
function slice_indices{T <: Real, S <: Real}(y::AbstractVector{T}, splitvalues::AbstractVector{S})
  n = length(y)
  nsplits = length(splitvalues)
  idxs = zeros(Int, n)
  for i in 1:n
    j = 1
    while j <= nsplits && y[i] >= splitvalues[j]
      j += 1
    end
    idxs[i] = j
  end
  idxs
end

# Test problem is a simple linear model:
N = 100
P = 1000
X = randn(N, P)
y = 10*X[:,1] + 10*X[:,2] + randn(N)
@time xrank, fkfs, kfs = fused_kolmogorov_filter(X, y; partitions = 3:7);
@show xrank[1:5]


text:
#=
Julia implementation of the van Rossum distance, from
Houghton & Kreuz (2012), [On the efficient calculation of van Rossum distances](https://doi.org/10.3109/0954898X.2012.673048)

Implementation by Geoge Datseris.
=#

export vanRossum, vanRossum_fast

"""
    vanRossum(u, v, τ)
Calculate the van Rossum distance [1] between two spike trains `u, v` with
time parameter `τ` using the exact analytic expression, equation (5)
of [2]. Notice that our normalization is *twice* as much as the original
paper (1.0 instead of 0.5).

`u, v` are expected to be 1D vectors that only list the times of the spikes.

[1] : van Rossum (2001), [A novel spike distance](https://doi.org/10.1162/089976601300014321)

[2] : Houghton & Kreuz (2012), [On the efficient calculation of van Rossum distances](https://doi.org/10.3109/0954898X.2012.673048)
"""
vanRossum(u, v, τ) = sqrt(vR_ds(u, u, τ) + vR_ds(v, v, τ) - 2vR_ds(u, v, τ))
function vR_ds(u, v, τ) # vanRossum double sum
    s = 0.0
    @inbounds @fastmath for i in 1:length(u), j in 1:length(v)
        s += exp(- abs(u[i] - v[j])/τ)
    end
    return s
end

"""
    vanRossum_fast(u, v, τ) -> d
Calculate the van Rossum distance between spike vectors `u, v`, with
time parameter `τ`. This function uses the fast algorithm proposed by
Houghton and Kreuz [1], (equation (9)).

`u, v` are expected to be **sorted** 1D vectors that only list the times of the spikes.

```math
\\begin{aligned}
    [d(\\mathbf{u}, \\mathbf{v} ; \\tau)]^{2} = & \\frac{n_{1}+n_{2}}{2}+\\sum_{i} \\sum_{j | u_{i} ; u_{j}} e^{-\\left(u_{i}-u_{j}\\right) / \\tau}
    + \\sum_{i} \\sum_{j | v_{i}>v_{j}} e^{-\\left(v_{i}-v_{j}\\right) / \\tau} \\\\
    & - \\sum_{i} \\sum_{j | u_{i}>v_{j}} e^{-\\left(u_{i}-v_{j}\\right) / \\tau}
    - \\sum_{i} \\sum_{j | v_{i}>u_{j}} e^{-\\left(v_{i}-u_{j}\\right) / \\tau}
\\end{aligned}
```

[1] : Houghton & Kreuz (2012), [On the efficient calculation of van Rossum distances](https://doi.org/10.3109/0954898X.2012.673048)
"""
function vanRossum_fast(u::AbstractVector{T1}, v::AbstractVector{T2}, τ) where {T1, T2}
    error("This function is not giving correct results. Please check the code.")
    @assert issorted(u) && issorted(v)

    nu, nv = length(u), length(v)
    mu, mv = markage_vector(u, τ), markage_vector(v, τ)
    # First two factors of Eq. (9):
    Au, Av = sum(mu), sum(mv)
    # Last two factors of Eq. (9):
    Buv, Bvu = cross_term(u, v, mv, τ), cross_term(v, u, mu, τ)

    return sqrt( (nu + nv)/2 + Au + Av - Buv - Bvu )
end

"""
    markage_vector(u, τ)
Create the markage vector, Eq. (6) in the Houghton paper.
"""
function markage_vector(u::AbstractVector{T}, τ) where T
    m = zeros(T, length(u))
    @inbounds @fastmath for i in 2:length(u)
        m[i] = (m[i-1] + 1.0) * exp(-(u[i] - u[i-1])/τ)
    end
    return m
end

"""
    cross_term(u, v, mv, τ)
Calculate Eq. (12) of the Houghton paper.
```math
\\sum_{i} \\sum_{j | u_{i}>v_{j}} e^{-\\left(u_{i}-v_{j}\\right) / \\tau}
=
\\sum_{i} e^{-\\left(u_{i}-v_{J(i)}\\right) / \\tau}\\left[1+m_{J(i)}(\\mathbf{v})\\right]
```
"""
function cross_term(u, v, mv, τ)
    s = 0.0
    L = length(v)
    Jprev = 1
    @inbounds @fastmath for i in 1:length(u)
        @inbounds J = findlast(j -> v[j] < u[i], Jprev:L)
        isnothing(J) && continue # this happens at first and last index
        s += exp(-(u[i] - v[J])/τ) * (1.0 + mv[J])
        Jprev = J
    end
    return s
end


text:
function compute_derivatives(obj::ComplexOrdinaryDifferentialEquation, arg0::jdouble, arg1::Vector{JComplex})
    return jcall(obj, "computeDerivatives", Vector{JComplex}, (jdouble, Vector{JComplex}), arg0, arg1)
end

function get_dimension(obj::ComplexOrdinaryDifferentialEquation)
    return jcall(obj, "getDimension", jint, ())
end

function init(obj::ComplexOrdinaryDifferentialEquation, arg0::jdouble, arg1::Vector{JComplex}, arg2::jdouble)
    return jcall(obj, "init", void, (jdouble, Vector{JComplex}, jdouble), arg0, arg1, arg2)
end


text:
module VoxelRayTracers

include("core.jl")
include("plots.jl")

end # module


text:
# Some methods for validated integration of ODEs

const _DEF_MINABSTOL = 1.0e-50

"""
    remainder_taylorstep!(f!, t, x, dx, xI, dxI, δI, δt, params)

Returns a remainder for the integration step for the dependent variables (`x`)
checking that the solution satisfies the criteria for existence and uniqueness.
"""
function remainder_taylorstep!(f!::Function, t::Taylor1{T},
        x::Vector{Taylor1{TaylorN{T}}}, dx::Vector{Taylor1{TaylorN{T}}},
        xI::Vector{Taylor1{Interval{T}}}, dxI::Vector{Taylor1{Interval{T}}},
        δI::IntervalBox{N,T}, δt::Interval{T}, params) where {N,T}

    orderT = get_order(dx[1])
    aux = δt^(orderT+1)
    Δx  = IntervalBox([xI[i][orderT+1] for i in eachindex(xI)]) * aux
    Δdx = IntervalBox([dxI[i][orderT+1] for i in eachindex(xI)]) * aux
    Δ0  = IntervalBox([dx[i][orderT](δI) for i in eachindex(x)]) * aux / (orderT+1)
    Δ = Δ0 + Δdx * δt
    Δxold = Δx

    # Checking existence and uniqueness
    iscontractive(Δ, Δx) && return (true, Δx, t[0])

    # If the check didn't work, compute new remainders. A new Δx is proposed,
    # and the corresponding Δdx is computed
    xxI  = Array{Taylor1{TaylorN{Interval{T}}}}(undef, N)
    dxxI = Array{Taylor1{TaylorN{Interval{T}}}}(undef, N)
    vv = Array{Interval{T}}(undef, N)
    for its = 1:50
        # Remainder of Picard iteration
        Δ = picard_remainder!(f!, t, x, dx, xxI, dxxI, δI, δt, Δx, Δ0, params)

        # Checking existence and uniqueness
        iscontractive(Δ, Δx) && return (true, Δx, t[0])
        # iscontractive(Δ, Δx) && return _contract_iteration!(f!, t, x, dx, xxI, dxxI, δI, δt, Δx, Δdx, Δ0, params)

        # Expand Δx in the directions needed
        Δxold = Δx
        if Δ ⊆ Δx
            @inbounds for ind in 1:N
                # Widen the directions where ⊂ does not hold
                vv[ind] = Δx[ind]
                if Δ[ind] == Δx[ind]
                    vv[ind] = widen.(Δ[ind])
                end
            end
            Δx = IntervalBox(vv)
            continue
        end
        Δx = Δ
    end

    return (false, Δx)
end

"""
    iscontractive(Δ, Δx)

Checks if `Δ .⊂ Δx` is satisfied. If ``Δ ⊆ Δx` is satisfied, it returns
`true` if all cases where `==` holds corresponds to the zero `Interval`.
"""
function iscontractive(Δ::Interval{T}, Δx::Interval{T}) where{T}
    (Δ ⊂ Δx || Δ == Δx == zero_interval(T)) && return true
    return false
end
function iscontractive(Δ::IntervalBox{N,T}, Δx::IntervalBox{N,T}) where{N,T}
    @inbounds for ind in 1:N
        iscontractive(Δ[ind], Δx[ind]) || return false
    end
    return true
end

"""
    picard_remainder!(f!, t, x, dx, xxI, dxxI, δI, δt, Δx, Δ0, params)

Return the remainder of Picard operator
"""
function picard_remainder!(f!::Function, t::Taylor1{T},
    x::Vector{Taylor1{TaylorN{T}}}, dx::Vector{Taylor1{TaylorN{T}}},
    xxI::Vector{Taylor1{TaylorN{Interval{T}}}},
    dxxI::Vector{Taylor1{TaylorN{Interval{T}}}},
    δI::IntervalBox{N,T}, δt::Interval{T},
    Δx::IntervalBox{N,T}, Δ0::IntervalBox{N,T}, params) where {N,T}

    # Extend `x` and `dx` to have interval coefficients
    zI = zero_interval(T)
    @. begin
        xxI = x + Δx
        dxxI = dx + zI
    end

    # Compute `dxxI` from the equations of motion
    f!(dxxI, xxI, params, t)

    # Picard iteration, considering only the bound of `f` and the last coeff of f
    Δdx = IntervalBox( evaluate.( (dxxI - dx)(δt), (δI,) ) )
    Δ = Δ0 + Δdx * δt
    return Δ
end

# # Picard iterations to contract further Δx, once Δ ⊂ Δx holds
# # **Currently not used**
# function _contract_iteration!(f!::Function, t::Taylor1{T},
#         x::Vector{Taylor1{TaylorN{T}}}, dx::Vector{Taylor1{TaylorN{T}}},
#         xxI::Vector{Taylor1{TaylorN{Interval{T}}}}, dxxI::Vector{Taylor1{TaylorN{Interval{T}}}},
#         δI::IntervalBox{N,T}, δt::Interval{T},
#         Δx::IntervalBox{N,T}, Δdx::IntervalBox{N,T}, Δ0::IntervalBox{N,T}, params) where {N,T}
#
#     # Some abbreviations
#     Δ = Δ0 + Δdx * δt
#     Δxold = Δx
#
#     # Picard contractions
#     for its = 1:10
#         # Remainder of Picard iteration
#         Δ = picard_remainder!(f!, t, x, dx, xxI, dxxI, δI, δt, Δx, Δ0, params)
#
#         # If contraction doesn't hold, return old bound
#         iscontractive(Δ, Δx) || return Δxold
#
#         # Contract estimate
#         Δxold = Δx
#         Δx = Δ
#     end
#
#     return Δxold
# end

"""
    absorb_remainder(a::TaylorModelN)

Returns a TaylorModelN, equivalent to `a`, such that the remainder
is mostly absorbed in the constant and linear coefficients. The linear shift assumes
that `a` is normalized to the `IntervalBox(-1..1, Val(N))`.

Ref: Xin Chen, Erika Abraham, and Sriram Sankaranarayanan,
"Taylor Model Flowpipe Construction for Non-linear Hybrid
Systems", in Real Time Systems Symposium (RTSS), pp. 183-192 (2012),
IEEE Press.
"""
function absorb_remainder(a::TaylorModelN{N,T,T}) where {N,T}
    Δ = remainder(a)
    orderQ = get_order(a)
    δ = symmetric_box(N, T)
    aux = diam(Δ)/(2N)
    rem = zero_interval(T)

    # Linear shift
    lin_shift = mid(Δ) + sum((aux*TaylorN(i, order=orderQ) for i in 1:N))
    bpol = a.pol + lin_shift

    # Compute the new remainder
    aI = a(δ)
    bI = bpol(δ)

    if bI ⊆ aI
        rem = Interval(aI.lo-bI.lo, aI.hi-bI.hi)
    elseif aI ⊆ bI
        rem = Interval(bI.lo-aI.lo, bI.hi-aI.hi)
    else
        r_lo = aI.lo-bI.lo
        r_hi = aI.hi-bI.hi
        if r_lo > 0
            rem = Interval(-r_lo, r_hi)
        else
            rem = Interval( r_lo, -r_hi)
        end
    end

    return TaylorModelN(bpol, rem, a.x0, a.dom)
end

# Postverify and define Taylor models to be returned
function scalepostverify_sw!(xTMN::Vector{TaylorModelN{N,T,T}},
        X::Vector{TaylorN{T}}) where {N,T}
    postverify = true
    x0 = xTMN[1].x0
    B = domain(xTMN[1])
    zI = zero_interval(T)#zero(B[1])
    @inbounds for i in eachindex(xTMN)
        pol = polynomial(xTMN[i])
        ppol = fp_rpa(TaylorModelN(pol(X), zI, x0, B ))
        postverify = postverify && (xTMN[i](B) ⊆ ppol(B))
        xTMN[i] = copy(ppol)
    end
    @assert postverify """
        Failed to post-verify shrink-wrapping:
        X = $(linear_polynomial(X))
        xTMN = $(xTMN)
        """
    return postverify
end

"""
    shrink_wrapping!(xTMN::TaylorModelN)

Returns a modified inplace `xTMN`, which has absorbed the remainder
by the modified shrink-wrapping method of Florian Bünger.
The domain of `xTMN` is the normalized interval box `[-1,1]^N`.

Ref: Florian B\"unger, Shrink wrapping for Taylor models revisited,
Numer Algor 78:1001–1017 (2018), https://doi.org/10.1007/s11075-017-0410-1
"""
function shrink_wrapping!(xTMN::Vector{TaylorModelN{N,T,T}}) where {N,T}
    # Original domain of TaylorModelN should be the symmetric normalized box
    B = symmetric_box(N, T)
    @assert all(domain.(xTMN) .== (B,))
    zI = zero_interval(T)
    x0 = zero_box(N, T)
    @assert all(expansion_point.(xTMN) .== (x0,))

    # Vector of independent TaylorN variables
    order = get_order(xTMN[1])
    X = [TaylorN(T, i, order=order) for i in 1:N]

    # Remainder of original TaylorModelN and componentwise mag
    rem = remainder.(xTMN)
    r = mag.(rem)
    qB = r .* B
    one_r = ones(eltype(r), N)

    # Shift to remove constant term
    xTN0 = constant_term.(xTMN)
    xTNcent = polynomial.(xTMN) .- xTN0
    xTNcent_lin = linear_polynomial(xTNcent)

    # Step 4 of Bünger algorithm: Jacobian (at zero) and its inverse
    jac = TaylorSeries.jacobian(xTNcent_lin)
    # If the conditional number is too large (inverse of jac is ill defined),
    # don't change xTMN
    cond(jac) > 1.0e4 && return one_r
    # Inverse of the Jacobian
    invjac = inv(jac)

    # Componentwise bound
    r̃ = mag.(invjac * qB) # qB <-- r .* B
    qB´ = r̃ .* B
    @assert invjac * qB ⊆ qB´

    # Step 6 of Bünger algorithm: compute g
    g = invjac*xTNcent .- X
    # g = invjac*(xTNcent .- xTNcent_lin)
    # ... and its jacobian matrix (full dependence!)
    jacmatrix_g = TaylorSeries.jacobian(g, X)

    # Alternative to Step 7: Check the validity of Eq 16 (or 17) for Lemma 2
    # of Bünger's paper, for s=0, and s very small. If it satisfies it,
    # postverify and return. Otherwise, use Bünger's step 7.
    q = 1.0 .+ r̃
    s = zero(q)
    @. q = 1.0 + r̃ + s
    jaq_q1 = jacmatrix_g * (q .- 1.0)
    eq16 = all(mag.(evaluate.(jaq_q1, (q .* B,))) .≤ s)
    if eq16
        postverify = scalepostverify_sw!(xTMN, q .* X)
        postverify && return q
    end
    s .= eps.(q)
    @. q = 1.0 + r̃ + s
    jaq_q1 = jacmatrix_g * (q .- 1.0)
    eq16 = all(mag.(evaluate.(jaq_q1, (q .* B,))) .≤ s)
    if eq16
        postverify = scalepostverify_sw!(xTMN, q .* X)
        postverify && return q
    end

    # Step 7 of Bünger algorithm: estimate of `q`
    # Some constants/parameters
    q_tol = 1.0e-12
    q = 1.0 .+ r̃
    ff = 65/64
    q_max = ff .* q
    s = zero(q)
    q_old = similar(q)
    q_1 = similar(q)
    jaq_q1 .= jacmatrix_g * r̃
    iter_max = 100
    improve = true
    iter = 0
    while improve && iter < iter_max
        qB .= q .* B
        q_1 .= q .- 1.0
        q_old .= q
        mul!(jaq_q1, jacmatrix_g, q_1)
        eq16 = all(evaluate.(jaq_q1, (qB,)) .≤ s)
        eq16 && break
        @inbounds for i in eachindex(xTMN)
            s[i] = mag( jaq_q1[i](qB) )
            q[i] = 1.0 + r̃[i] + s[i]
            # If q is too large, return xTMN unchanged
            q[i] > q_max[i] && return -one_r
        end
        improve = any( ((q .- q_old)./q) .> q_tol )
        iter += 1
    end
    # (improve || q == one_r) && return one_r
    # Compute final q and rescale X
    @. q = 1.0 + r̃ + ff * s
    @. X = q * X

    # Postverify
    postverify = scalepostverify_sw!(xTMN, X)

    return q
end

"""
    validated-step!
"""
function validated_step!(f!, t::Taylor1{T}, x::Vector{Taylor1{TaylorN{T}}},
        dx::Vector{Taylor1{TaylorN{T}}}, xaux::Vector{Taylor1{TaylorN{T}}},
        tI::Taylor1{T}, xI::Vector{Taylor1{Interval{T}}},
        dxI::Vector{Taylor1{Interval{T}}}, xauxI::Vector{Taylor1{Interval{T}}},
        t0::T, tmax::T, sign_tstep::Int,
        xTMN::Vector{TaylorModelN{N,T,T}}, xv::Vector{IntervalBox{N,T}},
        rem::Vector{Interval{T}}, zbox::IntervalBox{N,T}, symIbox::IntervalBox{N,T},
        nsteps::Int, orderT::Int, abstol::T, params, parse_eqs::Bool,
        adaptive::Bool, minabstol::T, absorb::Bool,
        check_property::Function=(t, x)->true) where {N,T}

    # One step integration (non-validated)
    # TaylorIntegration.__jetcoeffs!(Val(parse_eqs), f!, t, x, dx, xaux, params)
    # δt = TaylorIntegration.stepsize(x, abstol)
    δt = TaylorIntegration.taylorstep!(f!, t, x, dx, xaux, abstol, params, parse_eqs)
    f!(dx, x, params, t)  # Update `dx[:][orderT]`

    # One step integration for the initial box
    # TaylorIntegration.__jetcoeffs!(Val(parse_eqs), f!, tI, xI, dxI, xauxI, params)
    # δtI = TaylorIntegration.stepsize(xI, abstol)
    δtI = TaylorIntegration.taylorstep!(f!, tI, xI, dxI, xauxI, abstol, params, parse_eqs)
    f!(dxI, xI, params, tI)  # Update `dxI[:][orderT+1]`

    # Step size
    δt = min(δt, sign_tstep*(tmax-t0))
    δt = sign_tstep * δt

    # Test if `check_property` is satisfied; if not, half the integration time.
    # If after 25 checks `check_property` is not satisfied, throw an error.
    nsteps += 1
    issatisfied = false
    rem_old = copy(rem)

    local _success, _t0
    reduced_abstol = abstol
    bool_red = true
    while bool_red
        # Validate the solution: remainder consistent with Schauder thm
        δtI = sign_tstep * Interval(0, sign_tstep*δt)
        (_success, Δ) = remainder_taylorstep!(f!, t, x, dx, xI, dxI, symIbox, δtI, params)

        # Shrink stepsize δt if adaptive is true and _success is false
        if !_success
            if adaptive
                bool_red = reduced_abstol > minabstol
                if bool_red
                    reduced_abstol = reduced_abstol/10
                    δt = δt * 0.1^(1/orderT)
                    continue
                else
                    @warn("Minimum absolute tolerance reached: ", t[0], δt, reduced_abstol, Δ)
                end
            else
                @warn("It cannot prove existence and unicity of the solution: ", t[0], δt, Δ)
            end
        end

        # Remainder
        rem .= rem_old .+ Δ

        # Create TaylorModelN to store remainders and evaluation
        @inbounds begin
            for i in eachindex(x)
                xTMN[i] = fp_rpa( TaylorModelN(x[i](δtI), rem[i], zbox, symIbox) )

                # If remainder is still too big, do it again
                j = 0
                while absorb && (j < 10) && (mag(rem[i]) > 1.0e-10)
                    t[0] == 0 && println("absorb_remainder ")
                    j += 1
                    xTMN[i] = absorb_remainder(xTMN[i])
                    rem[i] = remainder(xTMN[i])
                end
            end
            xv[nsteps] = evaluate(xTMN, symIbox) # IntervalBox

            issatisfied = check_property(t0+δt, xv[nsteps])
            if !issatisfied
                # δt = δt/2
                bool_red = reduced_abstol > minabstol
                @info("issatisfied: ", bool_red, δt)
                if bool_red
                    reduced_abstol = reduced_abstol / 10
                    δt = δt * 0.1^(1/orderT)
                    continue
                else
                    @warn("Minimum absolute tolerance reached (issatisfied): ", t[0], δt, reduced_abstol, Δ)
                end
            end
        end # @inbounds
        break
    end

    if !issatisfied && !adaptive
        @warn("""
            Something went wrong:
            """, issatisfied, adaptive, bool_red, reduced_abstol,
            t0, δt, Δ, nsteps, xv[nsteps], check_property(t0+δt, xv[nsteps])
        )
    end

    return (_success, δt, reduced_abstol)
end

"""
    initialize!(X0::IntervalBox{N, T}, orderQ, orderT, x, dx, xTMN, xI, dxI, rem, xTM1v) where {N, T}

Initialize the auxiliary integration variables and normalize the given interval
box to the domain `[-1, 1]^n`.
"""
function initialize!(X0::IntervalBox{N,T}, orderQ, orderT, x, dx, xTMN, xI, dxI, rem, xTM1v) where {N,T}
    @assert N == get_numvars()

    # center of the box and vector of widths
    q0 = mid.(X0)
    δq0 = X0 .- q0

    # normalized domain
    zI = zero_interval(T)
    zB = zero_box(N, T)
    S = symmetric_box(N, T)

    qaux = normalize_taylor.(q0 .+ TaylorN.(1:N, order=orderQ), (δq0,), true)
    @. begin
        x = Taylor1(qaux, orderT)
        dx = x
        xTMN = TaylorModelN(qaux, zI, (zB,), (S,))

        xI = Taylor1(X0, orderT+1)
        dxI = xI
        rem = zI

        xTM1v[:, 1] = TaylorModel1(deepcopy(x), zI, zI, zI)
    end
    return nothing
end
function initialize!(X0::IntervalBox{N,T}, orderQ, orderT, x, dx, xTMN, rem, xTM1v) where {N,T}
    @assert N == get_numvars()
    q0 = mid.(X0)
    δq0 = X0 .- q0
    zI = zero_interval(T)
    zB = zero_box(N, T)
    S = symmetric_box(N, T)

    qaux = normalize_taylor.(q0 .+ TaylorN.(1:N, order=orderQ), (δq0,), true)
    @. begin
        x = Taylor1(qaux, orderT)
        dx = x
        xTMN = TaylorModelN(qaux, zI, (zB,), (S,))
        rem = zI
        xTM1v[:, 1] = TaylorModel1(deepcopy(x), zI, zI, zI)
    end
    return nothing
end

"""
    initialize!(X0::Vector{TaylorModel1{TaylorN{T}, T}}, orderQ, orderT, x, dx, xTMN, xI, dxI, rem, xTM1v) where {T}

Initialize the auxiliary integration variables assuming that the given vector
of taylor models `X0` is normalized to the domain `[-1, 1]^n` in space.
"""
function initialize!(X0::Vector{TaylorModel1{TaylorN{T},T}}, orderQ, orderT, x, dx, xTMN, xI, dxI, rem, xTM1v) where {T}
    # nomalized domain
    N = get_numvars()
    zI = zero_interval(T)
    zB = zero_box(N, T)
    S = symmetric_box(N, T)

    qaux = constant_term.(polynomial.(X0))
    x0t = expansion_point(X0[1])
    domt = domain(X0[1])
    @. begin
        x = Taylor1(qaux, orderT)
        dx = x
        xTMN = TaylorModelN(qaux, zI, (zB,), (S,))

        # we assume that qaux is normalized to S=[-1, 1]^N
        xI = Taylor1(evaluate(qaux, (S,)), orderT+1)
        dxI = xI

        # remainder
        rem = remainder(X0)

        # output vector
        xTM1v[:, 1] = TaylorModel1(deepcopy(x), rem, x0t, domt)
    end
    return nothing
end
function initialize!(X0::Vector{TaylorModel1{TaylorN{T},T}}, orderQ, orderT, x, dx, xTMN, rem, xTM1v) where {T}
    # nomalized domain
    N = get_numvars()
    zI = zero_interval(T)
    zB = zero_box(N, T)
    S = symmetric_box(N, T)

    qaux = constant_term.(polynomial.(X0))
    x0t = expansion_point(X0[1])
    domt = domain(X0[1])
    @. begin
        x = Taylor1(qaux, orderT)
        dx = x
        xTMN = TaylorModelN(qaux, zI, (zB,), (S,))

        # remainder
        rem = remainder(X0)

        # output vector
        xTM1v[:, 1] = TaylorModel1(deepcopy(x), rem, x0t, domt)
    end
    return nothing
end

function validated_integ(f!, X0, t0::T, tmax::T, orderQ::Int, orderT::Int, abstol::T, params=nothing;
                         maxsteps::Int=2000, parse_eqs::Bool=true,
                         adaptive::Bool=true, minabstol::T=T(_DEF_MINABSTOL),
                         absorb::Bool=false,
                         check_property::Function=(t, x)->true) where {T<:Real}

    # Set proper parameters for jet transport
    N = get_numvars()
    dof = N

    # Some variables
    zt = zero(t0)
    zI = zero_interval(T)
    zB = zero_box(N, T)
    S  = symmetric_box(N, T)
    t  = t0 + Taylor1(orderT)
    tI = t0 + Taylor1(orderT+1)

    # Allocation of vectors
    # Output
    tv    = Array{T}(undef, maxsteps+1)
    xv    = Array{IntervalBox{N,T}}(undef, maxsteps+1)
    xTM1v = Array{TaylorModel1{TaylorN{T},T}}(undef, dof, maxsteps+1)
    rem = Array{Interval{T}}(undef, dof)

    # Internals: jet transport integration
    x     = Array{Taylor1{TaylorN{T}}}(undef, dof)
    dx    = Array{Taylor1{TaylorN{T}}}(undef, dof)
    xaux  = Array{Taylor1{TaylorN{T}}}(undef, dof)
    xTMN  = Array{TaylorModelN{N,T,T}}(undef, dof)

    # Internals: Taylor1{Interval{T}} integration
    xI    = Array{Taylor1{Interval{T}}}(undef, dof)
    dxI   = Array{Taylor1{Interval{T}}}(undef, dof)
    xauxI = Array{Taylor1{Interval{T}}}(undef, dof)

    # Set initial conditions
    initialize!(X0, orderQ, orderT, x, dx, xTMN, xI, dxI, rem, xTM1v)
    sign_tstep = copysign(1, tmax-t0)

    # Output vectors
    @inbounds tv[1] = t0
    @inbounds xv[1] = evaluate(xTMN, S)

    # Determine if specialized jetcoeffs! method exists (built by @taylorize)
    parse_eqs = TaylorIntegration._determine_parsing!(parse_eqs, f!, t, x, dx, params)

    local _success # if true, the validation step succeeded
    setformat(:full)
    red_abstol = abstol

    # Integration
    nsteps = 1
    while sign_tstep*t0 < sign_tstep*tmax

        # Validated step of the integration
        (_success, δt, red_abstol) = validated_step!(f!, t, x, dx, xaux, tI, xI, dxI, xauxI,
                                t0, tmax, sign_tstep, xTMN, xv, rem, zB, S,
                                nsteps, orderT, red_abstol, params,
                                parse_eqs, adaptive, minabstol,
                                absorb, check_property)
        δtI = sign_tstep * Interval(zt, sign_tstep*δt)

        # New initial conditions and time, and output vectors
        nsteps += 1
        @inbounds tv[nsteps] = t0
        t0 += δt
        @inbounds t[0] = t0
        @inbounds tI[0] = t0
        @. begin
            xTM1v[:, nsteps] = TaylorModel1(x, rem, zI, δtI)  # deepcopy?
            x = Taylor1(evaluate(x, δt), orderT)
            # dx = Taylor1(zero(constant_term(x)), orderT)
            xI = Taylor1(evaluate(xTMN, (S,)), orderT+1)
            # dxI = xI
        end

        # Try to increase `red_abstol` if `adaptive` is true
        if adaptive
            red_abstol = min(abstol, 10*red_abstol)
        end

        # If the integration step is unsuccessfull, break with a warning; note that the
        # last integration step (which was not successfull) is returned
        if !_success
            @warn("""
            Exiting due to unsuccessfull step
            """, _success, t0)
            break
        end

        if nsteps > maxsteps
            @warn("""
            Maximum number of integration steps reached; exiting.
            """)
            break
        end

    end

    return view(tv,1:nsteps), view(xv,1:nsteps), view(xTM1v, :, 1:nsteps)
end

"""
    picard(dx, x0, box)

Computes the picard (integral) operator for the initial condition `x0`.
`dx` must be the rhs of the differential equation.
"""
function _picard(dx, x0, box)
    ∫f = integrate(dx, 0., box)
    pol = ∫f.pol + x0.pol
    Δk = ∫f.rem
    return pol, Δk
end

_picard_rem(dx, box) = remainder(integrate(dx, box))

"""
    picard_iteration(f!, dx, xTM1K, params, t, x0, box, ::Val{true})

Computes the picard (integral) operator for the set of equations `f!` and the initial condition`x0`.
The Val parameter enables the selection of the desired method. This one returns the remainder of
the resulting Taylor Model with the remainder of the initial condition included.
"""
function picard_iteration(f!, dx, xTM1K, params, t, x0, box, ::Val{true})
    f!(dx, xTM1K, params, t)
    return @. _picard_rem(dx, (box,)) + remainder(x0)
end

"""
    picard_iteration(f!, dx, xTM1K, params, t, x0, box)

Computes the picard (integral) operator for the set of equations `f!` and the initial condition`x0`.
This method returns the remainder of the resulting Taylor Model without the remainder of the initial condition.
"""
function picard_iteration(f!, dx, xTM1K, params, t, x0, box)
    f!(dx, xTM1K, params, t)
    return _picard_rem.(dx, (box,))
end

"""
    _validate_step(xTM1K, f!, dx, x0, params, t, box, dof; ε=1e-10, maxsteps=20, extrasteps=50)

Validate the Taylor Model solution for the current integration time step.
This function implements the epsilon inflation algorithm proposed by Bünger
with some custom adaptations.

Ref: Florian B\"unger, A Taylor model toolbox for solving ODEs implemented in MATLAB/INTLAB,
J. Comput. Appl. Math. 368, 112511, https://doi.org/10.1016/j.cam.2019.112511
"""
function _validate_step!(xTM1K, f!, dx, x0, params, x, t, box, dof, rem, abstol,
                        δt, sign_tstep, E, E′, polv, low_ratiov, hi_ratiov,
                        adaptive::Bool, minabstol;
                        ε=1e-10, δ=1e-6, validatesteps=20, extrasteps=50)
    #
    T = eltype(box[1])
    zI = zero_interval(T)
    domT = sign_tstep * Interval{T}(0, sign_tstep*δt)
    orderT = get_order(t)
    @. begin
        polv = deepcopy.(x)
        xTM1K = TaylorModel1(polv, zI, zI, domT)
        # xTM1K = TaylorModel1(polv, rem, zI, domT)
        E = remainder(xTM1K)
        # E = remainder(x0)
    end
    εi = (1 - ε) .. (1 + ε)
    δi = -δ .. δ

    _success = false
    reduced_abstol = abstol
    # Try to prove existence and uniqueness up to numchecks, including reducing the
    # time step
    bool_red = true
    # for nchecks = 1:numchecks
    while bool_red

        # Try to prove existence and uniqueness up to validatesteps
        nsteps = 0
        E′ .= picard_iteration(f!, dx, xTM1K, params, t, x0, box, Val(true)) # 0-th iteration
        @. xTM1K = TaylorModel1(polv, E′, zI, domT)
        while nsteps < validatesteps
            E′ .= picard_iteration(f!, dx, xTM1K, params, t, x0, box)
            all(iscontractive.(E′, E)) && break

            # Only inflates the required component
            @inbounds for i in eachindex(dx)
                if !iscontractive(E′[i], E[i])
                    E[i] = E′[i] * εi + δi
                end
                xTM1K[i] = TaylorModel1(polv[i], E[i], zI, domT)
            end
            nsteps += 1
        end
        _success = all(iscontractive.(E′, E))
        _success && break

        # Shrink stepsize `δt` if `adaptive` is `true` and `_success` is false,
        # up to some minimum
        if !_success
            if adaptive
                bool_red = reduced_abstol > minabstol
                if bool_red
                    reduced_abstol = reduced_abstol/10
                    δt = δt * 0.1^(1/orderT)
                    domT = sign_tstep * Interval{T}(0, sign_tstep*δt)
                    @. begin
                        xTM1K = TaylorModel1(polv, zI, zI, domT)
                        # xTM1K = TaylorModel1(polv, rem, zI, domT)
                        E = remainder(xTM1K)
                        # E = remainder(x0)
                    end
                else
                    @warn("Minimum absolute tolerance reached: ", t[0], E′, E,
                    _success, all(iscontractive.(E′, E)), reduced_abstol)
                end
            else
                @warn("""It cannot prove existence and unicity of the solution:
                    t0 = $(t[0])
                    δt = $(δt)
                    Δx = $(E)
                """)
                break
            end
        end
    end

    if !all(iscontractive.(E′, E))
        @warn("Maximum number of validate steps reached.", t[0], E′, E,
            _success, all(iscontractive.(E′, E)))
        return (_success, δt, reduced_abstol)
    end

    # @. begin
    #     low_ratiov = inf(E′) / inf(E)
    #     hi_ratiov  = sup(E′) / sup(E)
    # end

    # # Contract further the remainders if the last contraction improves more than 5%
    # for ind = 1:extrasteps
    #     minimum(low_ratiov) > 0.90 && minimum(hi_ratiov) > 0.90 && break
    #     E .= remainder.(xTM1K)
    #     E′ .= picard_iteration(f!, dx, xTM1K, params, t, x0, box)
    #     @. begin
    #         xTM1K = TaylorModel1(polv, E′, zI, dom)
    #         low_ratiov = inf(E′) / inf(E)
    #         hi_ratiov  = sup(E′) / sup(E)
    #     end
    #     # @show(ind, E, E′, all(iscontractive.(E′, E)), low_ratiov)
    # end

    return (_success, δt, reduced_abstol)
end

function validated_integ2(f!, X0, t0::T, tf::T, orderQ::Int, orderT::Int,
                         abstol::T, params=nothing;
                         parse_eqs=true, maxsteps::Int=2000,
                         absorb::Bool=false,
                         adaptive::Bool=true, minabstol=T(_DEF_MINABSTOL),
                         validatesteps::Int=30, ε::T=1e-10, δ::T=1e-6,
                         absorb_steps::Int=3) where {T <: Real}
    N = get_numvars()
    dof = N

    zt = zero(t0)
    zI = zero_interval(T)
    zB = zero_box(N, T)
    S = symmetric_box(N, T)
    t = t0 + Taylor1(orderT)

    tv = Array{T}(undef, maxsteps+1)
    xv = Array{IntervalBox{N,T}}(undef, maxsteps+1)
    xTM1v = Array{TaylorModel1{TaylorN{T},T}}(undef, dof, maxsteps+1)
    x = Array{Taylor1{TaylorN{T}}}(undef, dof)
    dx = Array{Taylor1{TaylorN{T}}}(undef, dof)
    xaux = Array{Taylor1{TaylorN{T}}}(undef, dof)
    xTMN = Array{TaylorModelN{N,T,T}}(undef, dof)
    dxTM1 = Array{TaylorModel1{TaylorN{T},T}}(undef, dof)
    xTM1 = Array{TaylorModel1{TaylorN{T},T}}(undef, dof)
    low_ratiov = Array{T}(undef, dof)
    hi_ratiov = Array{T}(undef, dof)
    rem = Array{Interval{T}}(undef, dof)
    E = Array{Interval{T}}(undef, dof)
    E′ = Array{Interval{T}}(undef, dof)

    # Set initial conditions
    initialize!(X0, orderQ, orderT, x, dx, xTMN, rem, xTM1v)
    @inbounds for i in eachindex(x)
        xTM1[i] = TaylorModel1(deepcopy(x[i]), zI, zI, zI)
    end
    polv = polynomial.(xTM1)
    fill!(E, zI)
    fill!(E′, zI)

    sign_tstep = copysign(1, tf - t0)
    nsteps = 1
    @inbounds tv[1] = t0
    @inbounds xv[1] = evaluate(xTMN, S)

    parse_eqs = TaylorIntegration._determine_parsing!(parse_eqs, f!, t, x, dx, params)
    red_abstol = abstol
    setformat(:full)

    while t0 * sign_tstep < tf * sign_tstep
        δt = TaylorIntegration.taylorstep!(f!, t, x, dx, xaux, abstol, params, parse_eqs)
        f!(dx, x, params, t)

        δt = min(δt, sign_tstep*(tf-t0))
        δt = sign_tstep * δt

        # Reuse previous TaylorModel1 to save some allocations
        (_success, δt, red_abstol) = _validate_step!(xTM1, f!, dxTM1, xTMN, params, x, t,
                                            S, dof, rem, red_abstol, δt, sign_tstep, E, E′,
                                            polv, low_ratiov, hi_ratiov,
                                            adaptive, minabstol,
                                            ε=ε, δ=δ,
                                            validatesteps=validatesteps)
        domt = sign_tstep * Interval(zt, sign_tstep*δt)

        # δtI = (δt .. δt) ∩ domt # assure it is inside the domain in t
        nsteps += 1
        @inbounds tv[nsteps] = t0
        t0 += δt
        @inbounds t[0] = t0

        # Flowpipe
        @. begin
            rem = remainder(xTM1)
            xTMN = fp_rpa(TaylorModelN(copy(evaluate(xTM1, domt)), rem, (zB,), (S,)))
        end
        xv[nsteps] = evaluate(xTMN, S)

        # New initial condition
        @inbounds for i in eachindex(x)
            aux_pol = evaluate(xTM1[i], δt) #δtI
            # rem[i] = remainder(xTM1[i])
            xTMN[i] = fp_rpa(TaylorModelN(deepcopy(aux_pol), rem[i], zB, S))
            # xTMN[i] = fp_rpa(TaylorModelN(deepcopy(aux_pol), 0 .. 0, zB, S))

            # Absorb remainder
            j = 0
            while absorb && (j < absorb_steps) && (mag(rem[i]) > 1.0e-10)
                t[0] == 0 && println("absorb_remainder ")
                j += 1
                xTMN[i] = absorb_remainder(xTMN[i])
                rem[i] = remainder(xTMN[i])
            end

            x[i] = Taylor1(polynomial(xTMN[i]), orderT)
            xTM1v[i, nsteps] = xTM1[i]
        end

        # Try to increase `red_abstol` if `adaptive` is true
        if adaptive
            red_abstol = min(abstol, 10*red_abstol)
        end

        # If the integration step is unsuccessfull, break with a warning; note that the
        # last integration step (which was not successfull) is returned
        if !_success
            @warn("""
            Exiting due to unsuccessfull step
            """, _success, t0)
            break
        end

        if nsteps > maxsteps
            @warn("""
            Maximum number of integration steps reached; exiting.
            """)
            break
        end
    end

    return view(tv, 1:nsteps), view(xv, 1:nsteps), view(xTM1v, :, 1:nsteps)
end


text:
# Julia wrapper for header: bpConverterTypesC.h
# Automatically generated using Clang.jl

# Julia wrapper for header: bpImageConverterInterfaceC.h
# Automatically generated using Clang.jl

export bpImageConverterC_Create, bpImageConverterC_Destroy, bpImageConverterC_GetLastException, bpImageConverterC_NeedCopyBlock,bpImageConverterC_Finish,
    bpImageConverterC_CopyBlockUInt8, bpImageConverterC_CopyBlockUInt16, bpImageConverterC_CopyBlockUInt32, bpImageConverterC_CopyBlockFloat

function bpImageConverterC_Create(
    aDataType,          aImageSize,       aSample,
    aDimensionSequence, aFileBlockSize,   aOutputFile,
    aOptions,           aApplicationName, aApplicationVersion,
    aProgressCallback,  aCallbackUserData)

    ccall((:bpImageConverterC_Create, bpImarisWriter96), bpImageConverterCPtr,
    (bpConverterTypesC_DataType,               bpConverterTypesC_Size5DPtr, bpConverterTypesC_Size5DPtr,
     bpConverterTypesC_DimensionSequence5DPtr, bpConverterTypesC_Size5DPtr, bpConverterTypesC_String,
     bpConverterTypesC_OptionsPtr,             bpConverterTypesC_String,    bpConverterTypesC_String,
     bpConverterTypesC_ProgressCallback,       Ptr{Cvoid}),
     aDataType,                                aImageSize,                  aSample,
     aDimensionSequence,                       aFileBlockSize,              aOutputFile,
     aOptions,                                 aApplicationName,            aApplicationVersion,
     aProgressCallback,                        aCallbackUserData)
end

function bpImageConverterC_Create(
    aDataType, aImageSize::Size5D, aSample::Size5D,
    aDimensionSequence::DimensionSequence5D, aFileBlockSize::Size5D, aOutputFile,
    aOptions::Options, aApplicationName::String, aApplicationVersion::String,
    aProgressCallback, aCallbackUserData
)
    bpImageConverterC_Create(
        aDataType, Ref(aImageSize), Ref(aSample),
        Ref(aDimensionSequence), Ref(aFileBlockSize),
        aOutputFile, Ref(aOptions),
        aApplicationName, aApplicationVersion,
        aProgressCallback, Ref(aCallbackUserData)
    )
end
const create = bpImageConverterC_Create

function bpImageConverterC_Destroy(aImageConverterC)
    ccall((:bpImageConverterC_Destroy, bpImarisWriter96), Cvoid, (bpImageConverterCPtr,), aImageConverterC)
end
const destroy = bpImageConverterC_Destroy

function bpImageConverterC_GetLastException(aImageConverterC)
    ccall((:bpImageConverterC_GetLastException, bpImarisWriter96), Cstring, (bpImageConverterCPtr,), aImageConverterC)
end

function bpImageConverterC_NeedCopyBlock(aImageConverterC, aBlockIndex)
    ccall((:bpImageConverterC_NeedCopyBlock, bpImarisWriter96), Bool, (bpImageConverterCPtr, bpConverterTypesC_Index5DPtr), aImageConverterC, aBlockIndex)
end
const needCopyBlock = bpImageConverterC_NeedCopyBlock

function bpImageConverterC_CopyBlockUInt8(aImageConverterC, aFileDataBlock, aBlockIndex)
    ccall((:bpImageConverterC_CopyBlockUInt8, bpImarisWriter96), Cvoid,
    (bpImageConverterCPtr, Ptr{bpConverterTypesC_UInt8}, bpConverterTypesC_Index5DPtr),
     aImageConverterC,     aFileDataBlock,               aBlockIndex)
end

function bpImageConverterC_CopyBlockUInt16(aImageConverterC, aFileDataBlock, aBlockIndex)
    ccall((:bpImageConverterC_CopyBlockUInt16, bpImarisWriter96), Cvoid,
    (bpImageConverterCPtr, Ptr{bpConverterTypesC_UInt16}, bpConverterTypesC_Index5DPtr),
     aImageConverterC,     aFileDataBlock,                aBlockIndex)
end

function bpImageConverterC_CopyBlockUInt32(aImageConverterC, aFileDataBlock, aBlockIndex)
    ccall((:bpImageConverterC_CopyBlockUInt32, bpImarisWriter96), Cvoid,
    (bpImageConverterCPtr, Ptr{bpConverterTypesC_UInt32}, bpConverterTypesC_Index5DPtr),
         aImageConverterC, aFileDataBlock,                aBlockIndex)
end

function bpImageConverterC_CopyBlockFloat(aImageConverterC, aFileDataBlock, aBlockIndex)
    ccall((:bpImageConverterC_CopyBlockFloat, bpImarisWriter96), Cvoid,
    (bpImageConverterCPtr, Ptr{bpConverterTypesC_Float}, bpConverterTypesC_Index5DPtr),
         aImageConverterC, aFileDataBlock,               aBlockIndex)
end

# High level copyBlock
# aFileDataBlock must be an AbstractArray for which `pointer` is applicable
copyBlock(aImageConverterC, aFileDataBlock::AbstractArray{UInt8}, aBlockIndex::Index5D) =
    GC.@preserve aFileDataBlock bpImageConverterC_CopyBlockUInt8( aImageConverterC, pointer(aFileDataBlock), Ref(aBlockIndex))
copyBlock(aImageConverterC, aFileDataBlock::AbstractArray{UInt16}, aBlockIndex::Index5D) =
    GC.@preserve aFileDataBlock bpImageConverterC_CopyBlockUInt16(aImageConverterC, pointer(aFileDataBlock), Ref(aBlockIndex))
copyBlock(aImageConverterC, aFileDataBlock::AbstractArray{UInt32}, aBlockIndex::Index5D) =
    GC.@preserve aFileDataBlock bpImageConverterC_CopyBlockUInt32(aImageConverterC, pointer(aFileDataBlock), Ref(aBlockIndex))
copyBlock(aImageConverterC, aFileDataBlock::AbstractArray{Float32}, aBlockIndex::Index5D) =
    GC.@preserve aFileDataBlock bpImageConverterC_CopyBlockFloat( aImageConverterC, pointer(aFileDataBlock), Ref(aBlockIndex))

# Low level copyBlock
# Pass in pointers or references for aFileDataBlock and aBlockIndex
copyBlock(aImageConverterC, aFileDataBlock::Ptr{UInt8}, aBlockIndex) =
    bpImageConverterC_CopyBlockUInt8( aImageConverterC, aFileDataBlock, aBlockIndex)
copyBlock(aImageConverterC, aFileDataBlock::Ptr{UInt16}, aBlockIndex) =
    bpImageConverterC_CopyBlockUInt16(aImageConverterC, aFileDataBlock, aBlockIndex)
copyBlock(aImageConverterC, aFileDataBlock::Ptr{UInt32}, aBlockIndex) =
    bpImageConverterC_CopyBlockUInt32(aImageConverterC, aFileDataBlock, aBlockIndex)
copyBlock(aImageConverterC, aFileDataBlock::Ptr{Float32}, aBlockIndex) =
    bpImageConverterC_CopyBlockFloat( aImageConverterC, aFileDataBlock, aBlockIndex)

function bpImageConverterC_Finish(aImageConverterC, aImageExtent, aParameters, aTimeInfoPerTimePoint, aColorInfoPerChannel, aAutoAdjustColorRange)
    ccall((:bpImageConverterC_Finish, bpImarisWriter96), Cvoid,
    (bpImageConverterCPtr,             bpConverterTypesC_ImageExtentPtr,  bpConverterTypesC_ParametersPtr,
     bpConverterTypesC_TimeInfoVector, bpConverterTypesC_ColorInfoVector, Bool),
     aImageConverterC,                 aImageExtent,                      aParameters,
     aTimeInfoPerTimePoint,            aColorInfoPerChannel,              aAutoAdjustColorRange)
end

function bpImageConverterC_Finish(
    aImageConverterC::Ptr{Nothing},
    aImageExtent::ImageExtent,
    aParameters::Vector{ParameterSection},
    aTimeInfoPerTimePoint::Vector{TimeInfo},
    aColorInfoPerChannel::Vector{ColorInfo},
    aAutoAdjustColorRange::Bool)

    ps = bpConverterTypesC_ParameterSection.(aParameters)
    vParametersC = bpConverterTypesC_Parameters(
        pointer( ps ),
        length(  ps )
    )

    #ti = bpConverterTypesC_TimeInfo.(aTimeInfoPerTimePoint)
    ti = aTimeInfoPerTimePoint
    vTimeInfoPerTimePointC = bpConverterTypesC_TimeInfos(
        pointer( ti ),
        length(  ti )
    )

    ci = bpConverterTypesC_ColorInfo.(aColorInfoPerChannel)
    vColorInfoPerChannelC = bpConverterTypesC_ColorInfos(
        pointer( ci ),
        length(  ci )
    )

    GC.@preserve aParameters aTimeInfoPerTimePoint aColorInfoPerChannel ps ci begin
    bpImageConverterC_Finish(
        aImageConverterC,
        Ref(aImageExtent),
        Ref(vParametersC),
        Ref(vTimeInfoPerTimePointC),
        Ref(vColorInfoPerChannelC),
        aAutoAdjustColorRange)
    end
end
const finish = bpImageConverterC_Finish

# Accessory functions, originally from test program

function numBlocks(aSize, aBlockSize)
    return (aSize + aBlockSize - 1) ÷ aBlockSize
end

function checkErrors(aConverter)
    vException = bpImageConverterC_GetLastException(aConverter)
    if(vException != C_NULL)
        error("ImarisWriter Error: " * unsafe_string(vException))
    end
end


text:
# These do most of the work in the package.
# They are all @generated or recusive functions for performance.

const UnionAllTupleOrVector = Union{Vector{UnionAll},Tuple{UnionAll,Vararg}}

"""
Sort dimensions into the order they take in the array.

Missing dimensions are replaced with `nothing`
"""
@inline Base.permutedims(tosort::AbDimTuple, perm::Union{Vector{<:Integer},Tuple{<:Integer,Vararg}}) =
    map(p -> tosort[p], Tuple(perm))
@inline Base.permutedims(tosort::Union{Vector{<:Integer},Tuple{<:Integer,Vararg}}, perm::AbDimTuple) =
    tosort

@inline Base.permutedims(tosort::AbDimTuple, order::UnionAllTupleOrVector) =
    permutedims(tosort, Tuple(map(d -> constructorof(d)(), order)))
@inline Base.permutedims(tosort::UnionAllTupleOrVector, order::AbDimTuple) =
    permutedims(Tuple(map(d -> constructorof(d)(), tosort)), order)
@inline Base.permutedims(tosort::AbDimTuple, order::AbDimVector) =
    permutedims(tosort, Tuple(order))
@inline Base.permutedims(tosort::AbDimVector, order::AbDimTuple) =
    permutedims(Tuple(tosort), order)
@inline Base.permutedims(tosort::AbDimTuple, order::AbDimTuple) =
    Base.permutedims(tosort, order, map(d -> dims(grid(d)), order))
@generated Base.permutedims(tosort::AbDimTuple, order::AbDimTuple, griddims) =
    permutedims_inner(tosort, order, griddims)

permutedims_inner(tosort::Type, order::Type, griddims::Type) = begin
    indexexps = []
    for (i, dim) in enumerate(order.parameters)
        dimindex = findfirst(d -> basetypeof(d) <: basetypeof(dim), tosort.parameters)
        if dimindex == nothing
            # The grid may allow dimensions not in dims
            # that will be transformed to the actual dimensions (ie for rotations).
            if griddims <: Nothing
                push!(indexexps, :(nothing))
            else
                gridindex = findfirst(d -> basetypeof(d) <: basetypeof(griddims.parameters[i]), tosort.parameters)
                if gridindex == nothing
                    push!(indexexps, :(nothing))
                else
                    push!(indexexps, :(tosort[$gridindex]))
                end
            end
        else
            push!(indexexps, :(tosort[$dimindex]))
        end
    end
    Expr(:tuple, indexexps...)
end

"""
Convert a tuple of AbstractDimension to indices, ranges or Colon.
"""
@inline dims2indices(A, lookup, emptyval=Colon()) =
    dims2indices(dims(A), lookup, emptyval)
@inline dims2indices(dims::AbDimTuple, lookup, emptyval=Colon()) =
    dims2indices(dims, (lookup,), emptyval)
@inline dims2indices(dims::AbDimTuple, lookup::Tuple{Vararg{Union{Colon,Integer,AbstractArray}}},
                     emptyval=Colon()) = lookup
@inline dims2indices(dims::AbDimTuple, lookup::Tuple, emptyval=Colon()) =
    dims2indices(map(grid, dims), dims, permutedims(lookup, dims), emptyval)

# Deal with irregular grid types that need multiple dimensions indexed together
@inline dims2indices(grids::Tuple{DependentGrid,Vararg}, dims::Tuple, lookup::Tuple, emptyval) = begin
    (irregdims, irreglookup), (regdims, reglookup) = splitgridtypes(grids, dims, lookup)
    (irreg2indices(map(grid, irregdims), irregdims, irreglookup, emptyval)...,
     dims2indices(map(grid, regdims), regdims, reglookup, emptyval)...)
end

@inline dims2indices(grids::Tuple, dims::Tuple, lookup::Tuple, emptyval) = begin
    (dims2indices(grids[1], dims[1], lookup[1], emptyval),
     dims2indices(tail(grids), tail(dims), tail(lookup), emptyval)...)
end
@inline dims2indices(grids::Tuple{}, dims::Tuple{}, lookup::Tuple{}, emptyval) = ()

@inline dims2indices(grid, dim::AbDim, lookup::Type{<:AbDim}, emptyval) = Colon()
@inline dims2indices(grid, dim::AbDim, lookup::Nothing, emptyval) = emptyval
@inline dims2indices(grid, dim::AbDim, lookup::AbDim, emptyval) = val(lookup)
@inline dims2indices(grid, dim::AbDim, lookup::AbDim{<:Selector}, emptyval) =
    sel2indices(grid, dim, val(lookup))

# Selectors select on grid dimensions
@inline irreg2indices(grids::Tuple, dims::Tuple, lookup::Tuple{AbDim{<:Selector},Vararg}, emptyval) =
    sel2indices(grids, dims, map(val, lookup))
# Other dims select on regular dimensions
@inline irreg2indices(grids::Tuple, dims::Tuple, lookup::Tuple, emptyval) = begin
    (dims2indices(grids[1], dims[1], lookup[1], emptyval),
     dims2indices(tail(grids), tail(dims), tail(lookup), emptyval)...)
end

@inline splitgridtypes(grids::Tuple{DependentGrid,Vararg}, dims, lookup) = begin
    (irregdims, irreglookup), reg = splitgridtypes(tail(grids), tail(dims), tail(lookup))
    irreg = (dims[1], irregdims...), (lookup[1], irreglookup...)
    irreg, reg
end
@inline splitgridtypes(grids::Tuple{IndependentGrid,Vararg}, dims, lookup) = begin
    irreg, (regdims, reglookup) = splitgridtypes(tail(grids), tail(dims), tail(lookup))
    reg = (dims[1], regdims...), (lookup[1], reglookup...)
    irreg, reg
end
@inline splitgridtypes(grids::Tuple{}, dims, lookup) = ((), ()), ((), ())

"""
Slice the dimensions to match the axis values of the new array

All methods returns a tuple conatining two tuples: the new dimensions,
and the reference dimensions. The ref dimensions are no longer used in
the new struct but are useful to give context to plots.

Called at the array level the returned tuple will also include the
previous reference dims attached to the array.
"""
@inline slicedims(A, dims::AbDimTuple) = slicedims(A, dims2indices(A, dims))
@inline slicedims(dims2slice::AbDimTuple, dims::AbDimTuple) =
    slicedims(dims2slice, dims2indices(dims2slice, dims))
@inline slicedims(dims2slice::AbDimTuple, dims::Tuple{}) = ()
# Results are split as (dims, refdims)
@inline slicedims(A, I::Tuple) = slicedims(dims(A), refdims(A), I)
@inline slicedims(dims::Tuple, refdims::Tuple, I::Tuple{}) = dims, refdims
@inline slicedims(dims::Tuple, refdims::Tuple, I::Tuple) = begin
    newdims, newrefdims = slicedims(dims, I)
    newdims, (refdims..., newrefdims...)
end
@inline slicedims(dims::Tuple{}, I::Tuple) = (), ()
@inline slicedims(dims::AbDimTuple, I::Tuple) = begin
    d = slicedims(dims[1], I[1])
    ds = slicedims(tail(dims), tail(I))
    (d[1]..., ds[1]...), (d[2]..., ds[2]...)
end
@inline slicedims(dims::Tuple{}, I::Tuple{}) = (), ()

@inline slicedims(d::AbDim, i::Colon) = (d,), ()
@inline slicedims(d::AbDim, i::Number) =
    (), (rebuild(d, val(d)[i], slicegrid(grid(d), val(d), i)),)
# TODO deal with unordered arrays trashing the index order
@inline slicedims(d::AbDim{<:AbstractArray}, i::AbstractArray) =
    (rebuild(d, val(d)[i]),), ()
@inline slicedims(d::AbDim{<:LinRange}, i::UnitRange) = begin
    range = val(d)
    start, stop, len = range[first(i)], range[last(i)], length(i) ÷ step(i)
    (rebuild(d, LinRange(start, stop, len), slicegrid(grid(d), val(d), i)),), ()
end

"""
Get the number of an AbstractDimension as ordered in the array
"""
@inline dimnum(A, lookup) = dimnum(typeof(dims(A)), lookup)
@inline dimnum(dimtypes::Type, lookup::AbstractArray) = dimnum(dimtypes, (lookup...,))
@inline dimnum(dimtypes::Type, lookup::Number) = lookup
@inline dimnum(dimtypes::Type, lookup::Tuple) =
    (dimnum(dimtypes, lookup[1]), dimnum(dimtypes, tail(lookup))...,)
@inline dimnum(dimtypes::Type, lookup::Tuple{}) = ()
@inline dimnum(dimtypes::Type, lookup::AbDim) = dimnum(dimtypes, typeof(lookup))
@generated dimnum(dimtypes::Type{DTS}, lookup::Type{D}) where {DTS,D} = begin
    index = findfirst(dt -> D <: basetypeof(dt), DTS.parameters)
    if index == nothing
        :(throw(ArgumentError("No $lookup in $dimtypes")))
    else
        :($index)
    end
end

hasdim(A, lookup::Tuple) = map(l -> hasdim(A, l), lookup)
hasdim(A, lookup) = hasdim(typeof(dims(A)), typeof(lookup))
hasdim(A, lookup::Type) = hasdim(typeof(dims(A)), lookup)
@generated hasdim(dimtypes::Type{DTS}, lookup::Type{D}) where {DTS,D} = begin
    index = findfirst(dt -> D <: basetypeof(dt), DTS.parameters)
    if index == nothing
        :(false)
    else
        :(true)
    end
end

"""
    formatdims(A, dims)

Format the passed-in dimension(s).

Mostily this means converting indexes of tuples and UnitRanges to
`LinRange`, which is easier to handle internally. Errors are also thrown if
dims don't match the array dims or size.

If a [`Grid`](@ref) hasn't been specified, a grid type is chosen
based on the type and element type of the index:
- `AbstractRange` become `RegularGrid`
- `AbstractArray` become `AlignedGrid`
- `AbstractArray` of `Symbol` or `String` become `CategoricalGrid`
"""
formatdims(A::AbstractArray{T,N}, dims::Tuple) where {T,N} = begin
    dimlen = length(dims)
    dimlen == N || throw(ArgumentError("dims ($dimlen) don't match array dimensions $(N)"))
    formatdims(size(A), dims)
end
formatdims(size::NTuple{N,Integer}, dims::AbDimTuple) where N = map(formatdims, size, dims)
formatdims(len::Integer, dim::AbDim{<:AbstractArray}) = begin
    checklen(dim, len)
    rebuild(dim, val(dim), identify(grid(dim), val(dim)))
end
formatdims(len::Integer, dim::AbDim{<:AbstractRange}) = begin
    checklen(dim, len)
    range = LinRange(first(dim), last(dim), len)
    rebuild(dim, range, identify(grid(dim), range))
end
formatdims(len::Integer, dim::AbDim{<:NTuple{2}}) = begin
    range = LinRange(first(dim), last(dim), len)
    rebuild(dim, range, identify(grid(dim), range))
end
formatdims(len::Integer, dim::AbDim) = dim

checklen(dim, len) =
    length(dim) == len ||
        throw(ArgumentError("length of $(basetypeof(dim)) ($(length(dim))) does not match size of array dimension ($len)"))

identify(::UnknownGrid, index::AbstractArray) =
    AlignedGrid(; order=order=orderof(index))
identify(::UnknownGrid, index::AbstractArray{<:Union{Symbol,String}}) =
    CategoricalGrid(; order=orderof(index))
identify(::UnknownGrid, index::AbstractRange) =
    RegularGrid(order=orderof(index), span=step(index))
identify(grid::Grid, index) = grid

orderof(index::AbstractArray) = begin
    sorted = issorted(index; rev=isrev(indexorder(index)))
    order = sorted ? Ordered(; index=indexorder(index)) : Unordered()
end

indexorder(index::AbstractArray) =
    first(index) <= last(index) ? Forward() : Reverse()

"""
Replace the specified dimensions with an index of length 1 to match
a new array size where the dimension has been reduced to a length
of 1, but the number of dimensions has not changed.

Used in mean, reduce, etc.

Grid traits are also updated to correspond to the change in cell span, sampling
type and order.
"""
@inline reducedims(A, dimstoreduce) = reducedims(A, (dimstoreduce,))
@inline reducedims(A, dimstoreduce::Tuple) = reducedims(dims(A), dimstoreduce)
@inline reducedims(dims::AbDimTuple, dimstoreduce::Tuple) =
    map(reducedims, dims, permutedims(dimstoreduce, dims))
@inline reducedims(dims::AbDimTuple, dimstoreduce::Tuple{Vararg{Int}}) =
    map(reducedims, dims, permutedims(map(i -> dims[i], dimstoreduce), dims))

# Reduce matching dims, ignore nothing vals - they are the dims not being reduced
@inline reducedims(dim::AbDim, ::Nothing) = dim
@inline reducedims(dim::AbDim, ::AbDim) = reducedims(grid(dim), dim)
# Reduce specialising on grid type
@inline reducedims(grid::UnknownGrid, dim) = rebuild(dim, first(val(dim)), UnknownGrid())
@inline reducedims(grid::CategoricalGrid, dim) =
    rebuild(dim, [:combined], CategoricalGrid(Unordered()))
@inline reducedims(grid::AlignedGrid, dim) = begin
    grid = AlignedGrid(Unordered(), locus(grid), MultiSample())
    rebuild(dim, reducedims(locus(grid), dim), grid)
end
@inline reducedims(grid::BoundedGrid, dim) = begin
    grid = BoundedGrid(Unordered(), locus(grid), MultiSample(), bounds(grid, dim))
    rebuild(dim, reducedims(locus(grid), dim), grid)
end
@inline reducedims(grid::RegularGrid, dim) = begin
    grid = RegularGrid(Unordered(), locus(grid), MultiSample(), span(grid) * length(val(dim)))
    rebuild(dim, reducedims(locus(grid), dim), grid)
end
@inline reducedims(grid::DependentGrid, dim) =
    rebuild(dim, [nothing], UnknownGrid)

# Get the index value at the reduced locus.
# This is the start, center or end point of the whole index.
reducedims(locus::Start, dim) = [first(val(dim))]
reducedims(locus::End, dim) = [last(val(dim))]
reducedims(locus::Center, dim) = begin
    index = val(dim)
    len = length(index)
    if iseven(len)
        [(index[len ÷ 2] + index[len ÷ 2 + 1]) / 2]
    else
        [index[len ÷ 2 + 1]]
    end
end

swapdim(A, dim::AbDim) = rebuild(A; dims=swapdim(dims(A), dim))
swapdim(dims::Tuple, dim::AbDim) =
    if basetypeof(dim) <: basetypeof(dims[1])
        (dim, swapdim(tail(dims), dim)...)
    else
        (dims[1], swapdim(tail(dims), dim)...)
    end
swapdim(dims::Tuple{}, dim::AbDim) = ()

"""
Get the dimension(s) matching the type(s) of the lookup dimension.
"""
@inline dims(A::AbstractArray, lookup) = dims(dims(A), lookup)
@inline dims(ds::AbDimTuple, lookup::Integer) = ds[lookup]
@inline dims(ds::AbDimTuple, lookup::Tuple) =
    (dims(ds, lookup[1]), dims(ds, tail(lookup))...)
@inline dims(ds::AbDimTuple, lookup::Tuple{}) = ()
@inline dims(ds::AbDimTuple, lookup) = dims(ds, basetypeof(lookup))
@generated dims(ds::DT, lookup::Type{L}) where {DT<:AbDimTuple,L} = begin
    index = findfirst(dt -> dt <: L, DT.parameters)
    if index == nothing
        :(throw(ArgumentError("No $lookup in $dims")))
    else
        :(ds[$index])
    end
end


text:
using ITensors
using ITensorTDVP
using LinearAlgebra

function heisenberg(n; h=zeros(n))
  os = OpSum()
  for j in 1:(n - 1)
    os += 0.5, "S+", j, "S-", j + 1
    os += 0.5, "S-", j, "S+", j + 1
    os += "Sz", j, "Sz", j + 1
  end
  for j in 1:n
    if h[j] ≠ 0
      os -= h[j], "Sz", j
    end
  end
  return os
end

n = 10
s = siteinds("S=1/2", n)

using Random
Random.seed!(12)

# MBL when W > 3.5-4
W = 12
# Random fields h ∈ [-W, W]
h = W * (2 * rand(n) .- 1)
H = MPO(heisenberg(n; h), s)

initstate = rand(["↑", "↓"], n)
ψ = MPS(s, initstate)

dmrg_x_kwargs = (
  nsweeps=10, reverse_step=false, normalize=true, maxdim=20, cutoff=1e-10, outputlevel=1
)

ϕ = dmrg_x(H, ψ; dmrg_x_kwargs...)

@show inner(ψ', H, ψ) / inner(ψ, ψ)
@show inner(H, ψ, H, ψ) - inner(ψ', H, ψ)^2
@show inner(ϕ', H, ϕ) / inner(ϕ, ϕ)
@show inner(H, ϕ, H, ϕ) - inner(ϕ', H, ϕ)^2


text:
# data from Dosing Extracted Data 2-12-16
# Walkup pbo
week = [0 4 8 12]
# nwks = length(week)

wpbomn = [19.6 16.0 13.6 12.6]
wpbosd = [3.9 4.1 5.2 6.3]
wnpbo = [76 76 76 76]  # all weeks

# walkup sert {Note: 1 = SSRI, 0 = SNRI (at end of name)
ws1mn = [18.8 14.2 11.2 9.8]
ws1sd = [3.9 4.0 5.0 6.2]
wns1 = [133 133 133 133] # all weeks

# Birmaher pbo
bpbomn = [14.9 11.4 8.6 9.3]
bpbosd = [3.5 3.7 5.1 4.8]
bnpbo = [37 37 37 37]

# Birmaher Flx
bs1mn = [15.6 9.8 7.8 7.1]
bs1sd = [3.5 5.4 5.4 5.9]
bns1 = [37 37 37 37]

# Wagner pbo
wgpbomn = [77.55 65.4 57.58 54.0]
wgpbosd = [19.34 21.44 12.15 25.8]
wgnpbo = [156 150 150 150]

# Wagner Parox
wgs1mn = [77.54 53.46 38.87 32.0]
wgs1sd = [22.34 21.69 21.29 21.1]
wgns1 = [163 159 159 159]

# Rynn pbo - no week 12 here
rypb1mn = [23.3 21.3 21.0]
rypb1sd = [4.0 7.6 7.8]
rynpb1 = [11 11 11]

# Rynn Sert
rys1mn = [20.6 12.0 7.8]
rys1sd = [3.6 6.4 5.7]
ryns1 = [11 11 11]

# Rupp pbo - no week 12 here
rpbomn = [17.0 15.3 15.9]
rpbosd = [3.6 4.4 5.3]  # week 12 miscoded?
rnpbo = [63 56 49]

# Rupp flvx
rs1mn = [18.7 11.1 7.1]
rs1sd = [2.9 6.0 6.1]  # week 12 miscoded?
rns1 = [61 52 50]

# Rynn Vfx study pbo - no week 12 here
# N.B.: no SDs available beyond week 0
rypb0mn = [40.0 30.6 28.53 0.0]
rypb0sd = [5.5 5.5 5.5 0.0]
rynpb0 = [154 154 154 0]

# Rynn Vfx
rys0mn = [39.75 27.68 24.21 0.0]
rys0sd = [5.0 5.0 5.0 0.0]
ryns0 = [159 159 159 0]

# Strawn Dulox study pbo - week 7 and 10 instead of 8 and 12 here
stpbomn = [17.2 12.59 11.44 11.0]
stpbosd = [2.4 2.4 2.4 2.7]
stnpbo = [133 133 133 133]

# Strawn Dulox
sts0mn = [17.3 10.8 9.6 8.0]
sts0sd = [2.2 2.2 2.2 2.5]
stns0 = [135 135 135 135]

# Geller study pbo ### INCLUDING 12 WEEKS
glpbomn = [16.92 14.09 12.95 13.72]
glpbosd = [1.8 2.3 2.7 2.7]
glnpbo = [80 80 58 58]  # no week 12

#Geller Atom
gls0mn = [17.5 12.33 10.82 12.0]
gls0sd = [1.8 2.3 2.7 2.7]
glns0 = [78 78 55 55]

# March study pbo
mpbomn = [66.2 56.6 51.9 0.0]
mpbosd = [10.6 9.0 12.7 0.0]
mnpbo = [148 148 148 0]  # no week 12

# March Vfx
ms0mn = [64.8 53.1 46.7 0.0]
ms0sd = [10.0 8.9 12.4 0.0]
msns0 = [137 137 137 0]

include("bayes_meta_analysis.jl")
include("bayes_odds_pvalue.jl")
using Plots, StatPlots, PlotRecipes
gr()

# Walkup
wmnp, wsep = standardize_scores(wpbomn,wpbosd,wnpbo)
wmnt, wset = standardize_scores(ws1mn,ws1sd,wns1)

# Wagner
wgmnp, wgsep = standardize_scores(wgpbomn,wgpbosd,wgnpbo)
wgmnt, wgset = standardize_scores(wgs1mn,wgs1sd,wgns1)

# Rupp
rmnp, rsep = standardize_scores(rpbomn,rpbosd,rnpbo)
rmnt, rset = standardize_scores(rs1mn,rs1sd,rns1)

# low dose combined: Birhamer Rynn

# Rynn
rymnp, rysep = standardize_scores(rypb1mn,rypb1sd,rynpb1)
rymnt, ryset = standardize_scores(rys1mn,rys1sd,ryns1)

# Birmaher
bmnp, bsep = standardize_scores(bpbomn,bpbosd,bnpbo)
bmnt, bset = standardize_scores(bs1mn,bs1sd,bns1)

#### SNRIs

# Rynn
ry0mnp, ry0sep = standardize_scores(rypb0mn,rypb0sd,rynpb0)
ry0mnt, ry0set = standardize_scores(rys0mn,rys0sd,ryns0)

# Strawn
stmnp, stsep = standardize_scores(stpbomn,stpbosd,stnpbo)
stmnt, stset = standardize_scores(sts0mn,sts0sd,stns0)

# Geller
glmnp, glsep = standardize_scores(glpbomn,glpbosd,glnpbo)
glmnt, glset = standardize_scores(gls0mn,gls0sd,glns0)

# March
mmnp, msep = standardize_scores(mpbomn,mpbosd,mnpbo)
mmnt, mset = standardize_scores(ms0mn,ms0sd,msns0)

### nwks = 3 => use only up to week 8
### nwks = 4 => use up to week 12
nwks = 3
# log trajectory model
### want diff(treat - pcbo) means here:
mn = wmnp  # means for trajectory plot
z = [1 2 3]'   # for up to week 8 = 3 data points
# z = [1 2 3 4]'   # for up to week 12 = 4 data points
lz = log.(z)
Xloglin = hcat(ones(nwks),lz)
b = Xloglin \ mn[1:nwks]
s2 = sum((mn[1:nwks] - Xloglin*b).^2)/2
println("coeffs = ",b)
println("s^2 (eqn. variance) = ",s2)

# compute R^2
tss = sum((mn[1:nwks]' - mean(mn[1:nwks])).^2)
R2 = 1 - s2*2/tss
println("Rsquared = ",R2)
zs = 1.0:0.1:4.2
score = b[1] + b[2].*log.(zs')

plot(1.0:0.1:4.2,score',size=(400,200),label="")
mns = mn[1:nwks]
scatter!(z',mns',label="")

s = sqrt(s2)
tcrit5 = 4.3  # 5% crit. value from Student-t with 2 d.f.
up95 = score .+ s*tcrit5
lo95 = score .- s*tcrit5

plot(1.0:0.1:4.2,score',size=(400,200),label="")
scatter!(z',mns',label="")
plot!(1.0:0.1:4.2,up95',size=(400,200),label="",color=:green)
plot!(1.0:0.1:4.2,lo95',size=(400,200),label="",color=:green)
hline!([1.0], color=:red,label="")

# allow nwks to vary (add a loop)
# change x axis labels to weeks instead of period
function traj_plot(means;overlay = false,nwks=3)
  if (nwks == 3)
    z = [1 2 3]'   # for up to week 8 = 3 data points
  else
    z = [1 2 3 4]'   # for up to week 12 = 4 data points
  end

  mn = means
  lz = log.(z)
  Xloglin = hcat(ones(nwks),lz)
  b = Xloglin \ mn[1:nwks]
  s2 = sum((mn[1:nwks] - Xloglin*b).^2)/(nwks-1.)
  println("coeffs = ",b)
  println("s^2 (eqn. variance) = ",s2)

  # compute R^2
  tss = sum((mn[1:nwks]' - mean(mn[1:nwks])).^2)
  R2 = 1 - s2*2/tss
  println("Rsquared = ",R2)
  zs = 1.0:0.1:4.2
  score = b[1] + b[2].*log.(zs')

  #plot(1.0:0.1:4.2,score',size=(400,200),label="")
  mns = mn[1:nwks]
  #scatter!(z',mns',label="")

  s = sqrt(s2)
  tcrit5 = 4.3  # 5% crit. value from Student-t with 2 d.f.
  up95 = score .+ s*tcrit5
  lo95 = score .- s*tcrit5

  if (overlay == false)
    plot(1.0:0.1:4.2,score',size=(400,200),label="")
  else
    plot!(1.0:0.1:4.2,score',size=(400,200),label="")
  end
  scatter!(z',mns',label="")
  plot!(1.0:0.1:4.2,up95',size=(400,200),label="",color=:green)
  plot!(1.0:0.1:4.2,lo95',size=(400,200),label="",color=:green)
#  hline!([1.0], color=:red,label="")
end

nwks=4
traj_plot(wmnp,nwks=nwks)
traj_plot(wmnt, overlay=true,nwks=nwks)

# dufference between pcbo and treatment

# Walkup
drawst, drawsnt = post_treat_notreat(wmnt, wset.^2, wns1, wmnp, wsep.^2, wnpbo,M=10000)
dtreatpcbo = treat_pcbo_difference(drawst,drawsnt)

nwks = 4

mndf = zeros(nwks)
for i = 1:nwks
  mndf[i] = mean(dtreatpcbo[:,i])
end
sddf = std(dtreatpcbo[:,nwks])

traj_plot(mndf,nwks=nwks)

# Birmaher
drawst, drawsnt = post_treat_notreat(bmnt, bset.^2, bns1, bmnp, bsep.^2, bnpbo,M=10000)
dtreatpcbo = treat_pcbo_difference(drawst,drawsnt)

mndf = zeros(nwks)
for i = 1:nwks
  mndf[i] = mean(dtreatpcbo[:,i])
end
sddf = std(dtreatpcbo[:,nwks])

traj_plot(mndf,nwks=nwks)

# Rupp
drawst, drawsnt = post_treat_notreat(rmnt, rset.^2, rns1, rmnp, rsep.^2, rnpbo,M=10000)
dtreatpcbo = treat_pcbo_difference(drawst,drawsnt)

nwks = 3
mndf = zeros(nwks)
for i = 1:nwks
  mndf[i] = mean(dtreatpcbo[:,i])
end
sddf = std(dtreatpcbo[:,nwks])

traj_plot(mndf)

##############################################################

##  Combine studies, week by week - Walkup + Wagner
#

# Placebo
m2wp = zeros(nwks,1)
s2wp = zeros(nwks,1)
n2wp = zeros(nwks,1)
for i in 1:nwks
  m2wp[i], s2wp[i], n2wp[i] = update_mean(wmnp[i],wgmnp[i],wsep[i]^2,wgsep[i]^2,wnpbo[i],wgnpbo[i])
end

# high dose pcbo combined: Walkup and Wagner:
himp = m2wp
his2p = s2wp
hinp = n2wp

# treatment
m2wt = zeros(nwks,1)
s2wt = zeros(nwks,1)
n2wt = zeros(nwks,1)
for i in 1:nwks
  m2wt[i], s2wt[i], n2wt[i] = update_mean(wmnt[i],wgmnt[i],wset[i]^2,wgset[i]^2,wns1[i],wgns1[i])
end

# high dose combined: Walkup and Wagner:
himt = m2wt
his2t = s2wt
hint = n2wt

# low dose combined: Birhamer Rynn

# Rynn
rymnp, rysep = standardize_scores(rypb1mn,rypb1sd,rynpb1)
rymnt, ryset = standardize_scores(rys1mn,rys1sd,ryns1)

# Birmaher
bmnp, bsep = standardize_scores(bpbomn,bpbosd,bnpbo)
bmnt, bset = standardize_scores(bs1mn,bs1sd,bns1)

lowmp = zeros(nwks,1)
los2p = zeros(nwks,1)
lownp = zeros(nwks,1)
lowmt = zeros(nwks,1)
los2t = zeros(nwks,1)
lownt = zeros(nwks,1)
for i in 1:nwks
  # Rynn-Birhamer pcbo
  lowmp[i], los2p[i], lownp[i] = update_mean(rymnp[i],bmnp[i],rysep[i]^2,bsep[i]^2,rynpb1[i],bnpbo[i])
  #Rynn-Birhamer treatment
  lowmt[i], los2t[i], lownt[i] = update_mean(rymnt[i],bmnt[i],ryset[i]^2,bset[i]^2,ryns1[i],bns1[i])
end

M = 100000 # no. of draws
dfwkh = zeros(M,nwks)  # High dose studies
dfwkl = zeros(M,nwks)  # Low dose studies
for i in 1:nwks
  drawst, drawsnt = post_treat_notreat(himt[i], his2t[i], hint[i], himp[i], his2p[i], hinp[i],M=M)
  dfwkh[:,i] = treat_pcbo_difference(drawst,drawsnt)
  drawst, drawsnt = post_treat_notreat(lowmt[i], los2t[i], lownt[i], lowmp[i], los2p[i], lownp[i],M=M)
  dfwkl[:,i] = treat_pcbo_difference(drawst,drawsnt)
end

#using Plots, StatPlots, PlotRecipes
#gr()

plot(dfwkh[:,1], st=:density, linecolor = "blue" , label="hi wk0",gridcolor="grey",xlims=(-0.8,0.5), fill=(0,:blue),alpha=0.2,title="High & Low dosage")
plot!(dfwkh[:,2], st=:density, linecolor = "blue" , label="hi wk4", fill=(0,:blue),alpha=0.3)
plot!(dfwkh[:,3], st=:density, linecolor = "blue" , label="hi wk8", fill=(0,:blue),alpha=0.4)
#plot!(dfwk[4], st=:density, linecolor = "purple" , label="Δwk12")

plot!(dfwkl[:,1], st=:density, linecolor = "green" , label="lo wk0",gridcolor="grey", fill=(0,:green),alpha=0.2)
plot!(dfwkl[:,2], st=:density, linecolor = "green" , label="lo wk4", fill=(0,:green),alpha=0.3)
plot!(dfwkl[:,3], st=:density, linecolor = "green" , label="lo wk8", fill=(0,:green),alpha=0.4)
#plot!(dfwk[4], st=:density, linecolor = "purple" , label="Δwk12")
savefig("hilodosage.png")

# Birmaher results exhibit a decline in pcbo
# effectiveness from week 4 to week 8
# Birmaher alone:
M = 100000 # no. of draws
dfwkb = zeros(M,nwks)  # High dose studies
#dfwkl = zeros(M,nwks)  # Low dose studies
for i in 1:nwks
  drawst, drawsnt = post_treat_notreat(bmnt[i], bset[i].^2, bns1[i], bmnp[i], bsep[i].^2, bnpbo[i],M=M)
  dfwkb[:,i] = treat_pcbo_difference(drawst,drawsnt)
#  drawst, drawsnt = post_treat_notreat(lowmt[i], los2t[i], lownt[i], lowmp[i], los2p[i], lownp[i],M=M)
#  dfwkl[:,i] = treat_pcbo_difference(drawst,drawsnt)
end

plot(dfwkb[:,1], st=:density, linecolor = "black" , label="hi wk0",gridcolor="grey")
plot!(dfwkb[:,2], st=:density, linecolor = "green" , label="hi wk4")
plot!(dfwkb[:,3], st=:density, linecolor = "red" , label="hi wk8")
#pl


text:
using Documenter
using Pat

makedocs(
    format = Documenter.HTML(
        prettyurls = get(ENV, "CI", nothing) == "true"
    ),
    modules = [Pat],
    sitename = "Pat.jl",
    authors = "Rolfe Power",
    linkcheck = !("skiplinks" in ARGS),
    pages = [
        "Home" => "index.md",
    ]
)

deploydocs(
    repo="github.com/rjpower4/Pat.jl.git",
    devbranch="main",
)


text:
# ------------------------------------------------------------------
# Licensed under the MIT License. See LICENSE in the project root.
# ------------------------------------------------------------------

"""
    laplacematrix(mesh; weights=:cotangent)

The Laplace-Beltrami (a.k.a. Laplacian) matrix of the `mesh`.
Optionally specify the discretization `weights`.

## Weights

* `:uniform`   - `Lᵢⱼ = 1 / |𝒩(i)|, ∀j ∈ 𝒩(i)`
* `:cotangent` - `Lᵢⱼ = cot(αᵢⱼ) + cot(βᵢⱼ), ∀j ∈ 𝒩(i)`

## References

* Botsch et al. 2010. [Polygon Mesh Processing](http://www.pmp-book.org).

* Pinkall, U. & Polthier, K. 1993. [Computing discrete minimal surfaces and their conjugates]
  (https://projecteuclid.org/journals/experimental-mathematics/volume-2/issue-1/Computing-discrete-minimal-surfaces-and-their-conjugates/em/1062620735.full).
"""
function laplacematrix(mesh; weights=:cotangent)
  # convert to half-edge topology
  ℳ = topoconvert(HalfEdgeTopology, mesh)

  # retrieve adjacency relation
  𝒩 = Adjacency{0}(topology(ℳ))

  # initialize matrix
  n = nvertices(ℳ)
  L = spzeros(n, n)

  # fill matrix with weights
  if weights == :uniform
    uniformlaplacian!(L, 𝒩)
  elseif weights == :cotangent
    v = vertices(ℳ)
    @assert eltype(ℳ) <: Triangle "cotangent weights only defined for triangle meshes"
    cotangentlaplacian!(L, 𝒩, v)
  else
    throw(ArgumentError("invalid discretization weights"))
  end

  L
end

function uniformlaplacian!(L, 𝒩)
  n = size(L, 1)
  for i in 1:n
    js = 𝒩(i)
    for j in js
      L[i,j] = 1 / length(js)
    end
    L[i,i] = -1
  end
end

function cotangentlaplacian!(L, 𝒩, v)
  n = size(L, 1)
  for i in 1:n
    js = CircularVector(𝒩(i))
    for k in 1:length(js)
      j₋, j, j₊ = js[k-1], js[k], js[k+1]
      vᵢ, vⱼ =  v[i],  v[j]
      v₋, v₊ = v[j₋], v[j₊]
      αᵢⱼ = ∠(vⱼ, v₋, vᵢ)
      βᵢⱼ = ∠(vᵢ, v₊, vⱼ)
      L[i,j] = cot(αᵢⱼ) + cot(βᵢⱼ)
    end
    L[i,i] = -sum(L[i,js])
  end
end

"""
    measurematrix(mesh)

The measure (or "mass") matrix of the `mesh`, i.e. a diagonal
matrix with entries `Mᵢᵢ = 2Aᵢ` where `Aᵢ` is (one-third of) the
sum of the areas of triangles sharing vertex `i`.

The discrete cotagent Laplace-Beltrami operator can be written
as `Δ = M⁻¹L`. When solving systems of the form `Δu = f`, it
is useful to write `Lu = Mf` and exploit the symmetry of `L`.
"""
function measurematrix(mesh)
  # convert to half-edge topology
  ℳ = topoconvert(HalfEdgeTopology, mesh)

  # retrieve coboundary relation
  ∂ = Coboundary{0,2}(topology(ℳ))

  # initialize matrix
  n = nvertices(ℳ)
  M = 1.0*I(n)

  # pre-compute all measures
  A = measure.(ℳ)

  # fill matrix with measures
  for i in 1:n
    Aᵢ = sum(A[∂(i)]) / 3
    M[i,i] = 2Aᵢ
  end

  M
end


text:
# Author: AJ Tyler
# Date: 25/01/22
module Visualisation # This module converts a configuration and its classifications into a coloursied .vesta file
export vestaFile # This function outputs a colourised .vesta file

"""
This function converts a classified configuration into a colourised .vesta file. It does this by using a reference .vesta file,
'blueprint.vesta', and only changes the lines corresponding to atom coordinates and colours based on detected symmetries.

Inputs:
configDir: (String) The path to (or name of, if in the local directory) the directory containing the .xyz files.
fileName: (String) The unique prefix of the fileName.
configurations: (Array{Float64}) 3D array containing the atom coordinates for each configuration in the file.
classifications: Vector{Dict{Vector{Int64},String}} Vector of dictionaries mapping a list of atoms to their symmetry.
capSymmetries: Vector{Dict{Dict{Int64,Vector{Int64}},String}} Vector of dictionaries mapping list of atoms forming a cap to their symmetry.
rCut: (Float64) The cut-off radii used in the CNA analysis in units of EBL. Default is 4/3.
EBL: (Float64) Equilibrium Bond Length. Default is 3.1227 Angstroms (from MP2 data for Neon2).
L: (Int64) The number of configurations in the file.
N: (Int64) The number of atoms in each configuration.
"""
function vestaFile(configDir,fileName,configurations,classifications,capSymmetries,rCut,EBL,L,N)
	oldFP = open("blueprint.vesta") # Obtain file pointer to reference .vesta file
	lines = readlines(oldFP) # Get vector of lines of the reference file
	for i in 1:L # For all configurations in the file
		open("$configDir/visualisation/$(fileName)_$i.vesta","a") do newFP # Create new .vesta file
			c = 1 # Initialise line counter
			while c <= length(lines) # While have not reached eof
				if (lines[c] == "STRUC") # If have reached structure section of the .vesta file
					println(newFP,"STRUC") # write header to new file
					for j in 1:N # For each atom in the configuration
						# Add the atom's coordinate to the file (aswell as other required values for Vesta)
						println(newFP,"  $j  1  $j  1.0000  $(configurations[i,j,1])  $(configurations[i,j,2])  $(configurations[i,j,3])    1")
						 # Not why this line is neccessary in the Vesta file
						println(newFP,"                    0.000000   0.000000   0.000000  0.00")
					end
					println(newFP,"  0 0 0 0 0 0 0") # Not why this line is neccessary in the Vesta file
					println(newFP,"THERI 1") # Write header to new file
					for j in 1:N # For each atom in the configuration
						println(newFP,"  $j  $j  0.000000") # Not why this line is neccessary in the Vesta file
					end
					c+= 42 # Skip lines to get to next section in lines
				elseif (lines[c] == "SBOND") # If have reached bond section of file
					println(newFP,"SBOND") # Print section header
					println(newFP,"  1     1     1    0.0000    $(round(rCut*EBL;digits=5))  0  1  1  0  1  0.250  2.000 127 127 127") # Write bond information
					println(newFP,"  0 0 0 0")
					println(newFP,"SITET") # Write header to new file
					for j in 1:N # For each atom in the configuration
						atomColour = colourAtom(j,classifications[i],capSymmetries[i]) # Colour the atom according to its identified symmetries
						println(newFP,"  $j  $j  0.8000  $atomColour  76  76  76 204 0") # Add colour to file
					end
					c += 17 # Skip lines to get to next section in lines
				else
					println(newFP,lines[c]) # Print same line from reference file to new file
					c+= 1 # Increment line counter
				end
			end
		end
	end
end

"""
This function determines a colour (RGB values) for an atom given its indentified symmetries.
The colours have the following meanings:
Red -> icosahedral, Orange -> BCC, Green -> FCC, Blue -> HCP, Purple -> OTHER,Black -> No identifiable structure.
The shades of the colour also have meanings. Darker shades mean the atom is part of an identified atom core. Light shades mean the atom is a
'cap' atom. An inbetween shade means the atom is either a capped atom or if core classification failed but the atom still has an identified symmetry.

Inputs:
atomNum: (Int64) The atom number to be colourised based off the order the atoms were listed in the .xyz file.
classifications: Dict{Vector{Int64},String} Dictionary mapping a list of atoms to their symmetry.
capSymmetries: Dict{Dict{Int64,Vector{Int64}},String} Dictionary mapping list of atoms forming a cap to their symmetry.

Outputs:
colour: (String) String representation of the RGB values that make up the desired colour for the input atom.
"""
function colourAtom(atomNum,classification,capSymmetries)
	for key in keys(classification) # For each list of atoms that have a symmetry
		if atomNum in key # If this atom is in that list
			symmetry = get!(classification,key,nothing) # Find what symmetry it has
			if occursin("Core",symmetry) # If symmetry is a core symmetry
				if occursin("ICO",symmetry) # If symmetry is icosahedral
					return "153  0  0" # Return dark red
				elseif occursin("BCC",symmetry)
					return "153  76  0" # Return dark orange
				elseif occursin("FCC",symmetry)
					return "76  153  0" # Return dark green
				elseif occursin("HCP",symmetry)
					return "0  0  153" # Return dark blue
				else # IF OTHER
					return "76  0  153" # Return dark purple
				end
			else # If not a coe symmetry
				if occursin("ICO",symmetry)
					return "255  0  0" # Return red
				elseif occursin("BCC",symmetry)
					return "255  128  0" # Return orange
				elseif occursin("FCC",symmetry)
					return "128  255  0" # Return green
				elseif occursin("HCP",symmetry)
					return "0  0  255" # Return blue
				else # If OTHER
					return "127  0  255" # Return purple
				end
			end
		end
	end

	for dict in keys(capSymmetries) # For each cap symmetry
		symmetry = get!(capSymmetries,dict,nothing) # Get symmetry type
		for key in keys(dict) # For each capping atom
			if atomNum == key # If this atom is the capping atom
				if occursin("ICO",symmetry)
					return "255  102  102" # Return light red
				elseif occursin("BCC",symmetry)
					return "255  178  102" # Return light orange
				elseif occursin("FCC",symmetry)
					return "178  255  102" # Return light green
				elseif occursin("HCP",symmetry)
					return "102  102  255" # Return light blue
				else
					return "178  102  255" # Return light purple
				end
			elseif atomNum in get!(dict,key,nothing) # If atom is a capped atom
				if occursin("ICO",symmetry)
					return "255  0  0" # Return red
				elseif occursin("BCC",symmetry)
					return "255  128  0" # Return orange
				elseif occursin("FCC",symmetry)
					return "128  255  0" # Return green
				elseif occursin("HCP",symmetry)
					return "0  0  255" # Return blue
				else
					return "127  0  255" # Return purple
				end
			end
		end
	end

	return "76  76  76" # IF atom does not have a symmetry, return grey
end

end


text:
function Lattices.edges(lattice::Square; order::Int=1)
    if isodd(order)
        k = (order + 1) ÷ 2
        FusedEdgesIterator(
            EdgesIterator{:vertical, k}(lattice),
            EdgesIterator{:horizontal, k}(lattice)
        )
    else
        k = order ÷ 2
        FusedEdgesIterator(
            EdgesIterator{:upright, k}(lattice),
            EdgesIterator{:upleft, k}(lattice)
        )
    end
end

struct FusedEdgesIterator{T <: Tuple}
    its::T
end

FusedEdgesIterator(its...) = FusedEdgesIterator(its)
Base.size(it::FusedEdgesIterator) = map(length, it.its)

Base.eltype(::FusedEdgesIterator) = EdgeType
Base.length(it::FusedEdgesIterator) = sum(length, it.its)

# Generates the coupling
Base.rand(::Type{T}, it::FusedEdgesIterator) where T = rand(T, size(it))
Base.rand(it::FusedEdgesIterator) = rand(Float64, it)

function Base.iterate(it::FusedEdgesIterator, state=((1, 1, 1), 1))
    it_state, nit = state
    if nit > length(it.its)
        return nothing
    end

    it_result = iterate(it.its[nit], it_state)

    if it_result !== nothing
        result, it_state = it_result
        return result, (it_state, nit)
    end

    if nit + 1 > length(it.its)
        return nothing
    end

    it_result = iterate(it.its[nit+1], (1, 1, 1))
    if it_result !== nothing
        result, it_state = it_result
        return result, (it_state, nit+1)
    end

    nothing
end

function Base.show(io::IO, it::FusedEdgesIterator)
    println(io, "Fused EdgesIterator:")
    for each in it.its
        if each === last(it.its)
            print(io, "  ", each)
        else
            println(io, "  ", each)
        end
    end
end

"""
    EdgesIterator{Region, Order, LT}

iterate through `order`-th edge in `Region` on square lattide `LT`.
"""
struct EdgesIterator{Region, Order, LT}
    EdgesIterator{Region, Order}(ltc::LT) where {Region, Order, LT} = new{Region, Order, LT}()
end

Base.eltype(::EdgesIterator) = EdgeType

Base.length(::EdgesIterator{:vertical, K, FixedSquare{h, w}}) where {K, h, w} = (h - K) * w
Base.length(::EdgesIterator{:horizontal, K, FixedSquare{h, w}}) where {K, h, w} = (w - K) * h
Base.length(::EdgesIterator{:upright, K, FixedSquare{h, w}}) where {K, h, w} = (w - k) * (h - k)
Base.length(::EdgesIterator{:upleft, K, FixedSquare{h, w}}) where {K, h, w} = (w - k) * (h - k)

function Base.iterate(it::EdgesIterator{:vertical, K, FixedSquare{h, w}}, state = (1, 1, 1)) where {K, h, w}
    i, j, count = state
    if count > length(it)
        return nothing
    elseif i + K > h
        return ((1, j+1), (1+K, j+1)), (2, j+1, count + 1)
    else
        ((i, j), (i+K, j)), (i+1, j, count+1)
    end
end

function Base.iterate(it::EdgesIterator{:horizontal, K, FixedSquare{h, w}}, state = (1, 1, 1)) where {K, h, w}
    i, j, count = state
    if count > length(it)
        return nothing
    elseif i > h
        return ((1, j+1), (1, j+K+1)), (2, j+1, count + 1)
    else
        ((i, j), (i, j+K)), (i+1, j, count+1)
    end
end

function Base.iterate(it::EdgesIterator{:upright, K, FixedSquare{h, w}}, state = (1, 1, 1)) where {K, h, w}
    i, j, count = state
    if count > length(it)
        return nothing
    elseif i+K > h
        return ((1, j+1), (1+K, j+K+1)), (2, j+1, count+1)
    else
        ((i, j), (i+K, j+K)), (i+1, j, count+1)
    end
end

function Base.iterate(it::EdgesIterator{:upleft, K, FixedSquare{h, w}}, state = (1, 1, 1)) where {K, h, w}
    i, j, count = state
    if count > length(it)
        return nothing
    elseif i+K > h
        return ((1+K, j+1), (1, j+K+1)), (2, j+1, count+1)
    else
        ((i+K, j), (i, j+K)), (i+1, j, count+1)
    end
end

#####################
Base.length(::EdgesIterator{R, K, PeriodicSquare{h, w}}) where {R, K, h, w} = h * w

function Base.iterate(it::EdgesIterator{:vertical, K, PeriodicSquare{h, w}}, state = (1, 1, 1)) where {K, h, w}
    i, j, count = state
    if count > length(it)
        return nothing
    elseif i > h
        return ((1, j+1), (K % h + 1, j+1)), (2, j+1, count+1)
    else
        ((i, j), ((i+K-1)%h+1, j)), (i+1, j, count+1)
    end
end

function Base.iterate(it::EdgesIterator{:horizontal, K, PeriodicSquare{h, w}}, state = (1, 1, 1)) where {K, h, w}
    i, j, count = state
    if count > length(it)
        return nothing
    elseif i > h
        return ((1, j+1), (1, (j+K)%w+1)), (2, j+1, count+1)
    else
        ((i, j), (i, (j+K-1)%w+1)), (i+1, j, count+1)
    end
end

function Base.iterate(it::EdgesIterator{:upright, K, PeriodicSquare{h, w}}, state = (1, 1, 1)) where {K, h, w}
    i, j, count = state
    if count > length(it)
        return nothing
    elseif i < h
        return ((1, j+1), (K%h+1, (j+K)%w+1)), (2, j+1, count+1)
    else
        ((i, j), ((i+K-1)%h+1, (j+K-1)%w+1)), (i+1, j, count+1)
    end
end

function Base.iterate(it::EdgesIterator{:upleft, K, PeriodicSquare{h, w}}, state = (1, 1, 1)) where {K, h, w}
    i, j, count = state
    if count > length(it)
    elseif i < h
        return ((K%h+1, j+1), (1, (j+K)%w+1)), (2, j+1, count+1)
    else
        (((i+K-1)%h+1, j), (i, (j+K-1)%w+1)), (i+1, j, count+1)
    end
end


text:
using Distributions
using IndependentRandomSequences
using Base.Test

#IID tests
W = Uniform(-1,1)
N = 2
Y = IIDRandomSequence(W,N)

@test insupport(Y,[0,0]) == true
@test insupport(Y,[-1,1]) == true
@test insupport(Y,[-2,0]) == false
@test insupport(Y,[0,0,0]) == false

@test pdf(Y,[0., 0.]) == 1/4
@test pdf(Y,[2., 0.]) == 0.
@test pdf(Y,[0. 0. ; 2. 0.]') == [1/4, 0.]

@test mean(Y) == [0., 0.]
@test var(Y) == [1/3, 1/3]

#INID tests, homogenous types
W,X = Uniform(-1,1),Uniform(0,1)
Y = INIDRandomSequence([W,X])

@test insupport(Y,[0,0]) == true
@test insupport(Y,[-1,1/2]) == true
@test insupport(Y,[-2,0]) == false
@test insupport(Y,[0,0,0]) == false

@test pdf(Y,[0., 1/2]) == 1/2
@test pdf(Y,[2., 1/2]) == 0.
@test pdf(Y,[0. 1/2;  2. 0.]') == [1/2, 0.]

@test mean(Y) == [0., 1/2]
@test var(Y) == [1/3, 1/12]

#INID test, hetergeneous types
W,X = Binomial(3,.5),Bernoulli(.5)
Y = INIDRandomSequence([W,X])

@test insupport(Y,[3,0]) == true
@test insupport(Y,[-2,0]) == false
@test insupport(Y,[0,0,0]) == false

@test cov(Y) == [3/4 0;0 1/4]


text:
using SimpleTraits

@testset "basic functionality test" begin
    x = BooleanSemiringElement(true)
    y = BooleanSemiringElement(false)
    z = BooleanSemiringElement{Bool}(true)

    @test x == z
    @test x != y != z
    @test x+y isa BooleanSemiringElement
    @test x*y isa BooleanSemiringElement
    @test x+y == y+x == x
    @test x+z == z+x == z == x

    @test zero(BooleanSemiringElement) == y
    @test one(BooleanSemiringElement) == x

    @test iszero(zero(BooleanSemiringElement))
    @test isone(one(BooleanSemiringElement))

    @test x⊕y == x+y
    @test x⊙y == x*y
end

@testset "conversions between elements of the same semiring with different types" begin
    x = RealSemiringElement{Int64}(3)
    y = RealSemiringElement{Float64}(11.1)
    z = RealSemiringElement{Int16}(1)

    xc = convert(RealSemiringElement{Float64}, x)
    yc = convert(RealSemiringElement{Float64}, y)
    zc = convert(RealSemiringElement{Float64}, z)

    @test x == xc
    @test y == yc
    @test z == zc

    @test x+x isa RealSemiringElement{Int64}
    @test y+y isa RealSemiringElement{Float64}
    @test z+z isa RealSemiringElement{Int16}
    @test x+y isa RealSemiringElement{Float64}
    @test x+z isa RealSemiringElement{Int64}
    @test y+z isa RealSemiringElement{Float64}

    @test xc isa RealSemiringElement{Float64}
    @test yc isa RealSemiringElement{Float64}
    @test zc isa RealSemiringElement{Float64}
end

@testset "operations between different semirings" begin
    x = MaxPlusSemiringElement(3.0)
    y = MinPlusSemiringElement(5.0)
    z = RealSemiringElement(2)

    # TODO: make these domain errors
    @test_throws ErrorException x+y
    @test_throws ErrorException x+z
    @test_throws ErrorException y+z
end

@testset "semiring type methods" begin
    x = RealSemiringElement(3)
    y = RealSemiringElement(5.0)
    z = MaxPlusSemiringElement(11.2)

    @test issamesemiring(x, y)
    @test !issamesemiring(x, z)
    @test !issamesemiring(y, z)

    @test semiring(x) <: RealSemiringElement
    @test eltype(semiring(x)) == Int64
    @test eltype(semiring(z)) == Float64
end

@testset "vectors and matrices" begin
    A = MaxPlusSemiringElement{Int64}[3 5; 1 2]
    B = MaxPlusSemiringElement{Int64}[10 6; -1 -2]

    @test A+B == MaxPlusSemiringElement{Int64}[10 6; 1 2]
    @test A*B == MaxPlusSemiringElement{Int64}[13 9; 11 7]

    C = MaxPlusSemiringElement{Float64}[1.0 2.0; 3.0 4.0]
    D = MaxPlusSemiringElement[1.0 2.0; 3.0 4.0]
    E = MaxPlusSemiringElement{Float64}[1 2; 3 4]
    @test C == D == E

    x = rand(3,3)
    y = rand(3)
    F = convert(Matrix{RealSemiringElement{Float64}}, x)
    G = convert(Matrix{RealSemiringElement{Float64}}, x+x)
    H = convert(Matrix{RealSemiringElement{Float64}}, x*x)
    I = convert(Vector{RealSemiringElement{Float64}}, y)
    J = convert(Vector{RealSemiringElement{Float64}}, x*y)

    @test F isa Matrix{RealSemiringElement{Float64}}
    @test G isa Matrix{RealSemiringElement{Float64}}
    @test H isa Matrix{RealSemiringElement{Float64}}
    @test I isa Vector{RealSemiringElement{Float64}}
    @test J isa Vector{RealSemiringElement{Float64}}

    @test F+F == G
    @test F*F == H
    @test F*I == J

    @test A+C isa Matrix{MaxPlusSemiringElement{Float64}}
end

@testset "promotion" begin
    x = MinPlusSemiringElement(3)
    y = MinPlusSemiringElement(4.0)
    z = MinPlusSemiringElement(5//3)
    a = MaxPlusSemiringElement(1)

    @test promote_type(typeof(x), typeof(y), typeof(z)) == MinPlusSemiringElement{Float64}
    xp, yp, zp = promote(x, y, z)
    @test xp isa promote_type(typeof(x), typeof(y), typeof(z))
    @test yp isa promote_type(typeof(x), typeof(y), typeof(z))
    @test zp isa promote_type(typeof(x), typeof(y), typeof(z))
    @test_throws DomainError promote(x, y, z, a)
end

@testset "test each implementation" begin
    for S in (RealSemiringElement, MaxPlusSemiringElement, MinPlusSemiringElement)
        for elty in (Int32, Int64, Float32, Float64, Rational)
            x = S(elty(5))
            y = S(elty(6))
            z = S(elty(2))
            @test x+zero(x) == zero(x) + x == x
            @test x*one(x) == one(x)*x == x
            @test zero(x)*(x+y) == (x+y)*zero(x) == zero(x)
            @test (x+y)*z == x*z + y*z
            @test z*(x+y) == z*x + z*y
        end
    end

    x = BooleanSemiringElement(true)
    y = BooleanSemiringElement(false)
    z = BooleanSemiringElement(true)

    @test x+zero(x) == zero(x) + x == x
    @test x*one(x) == one(x)*x == x
    @test zero(x)*(x+y) == (x+y)*zero(x) == zero(x)
    @test (x+y)*z == x*z + y*z
    @test z*(x+y) == z*x + z*y

    # optional methods defined for RealSemiringElements

    x = RealSemiringElement(3.0)
    y = RealSemiringElement(0.5)

    @test x-x == zero(x)
    @test x/x == one(x)
    @test x/y == RealSemiringElement(3.0/0.5)
    @test x*inv(x) == one(x)
    @test_throws DomainError star(x)
    @test star(y) == RealSemiringElement(1/(1-.5))

    @test y+addinv(y) == addinv(y)+y == zero(y)
    @test y*mulinv(y) == mulinv(y)*y == one(y)
end

@testset "traits" begin
    @traitfn commutativetest(x::X) where {X; IsCommutative{X}} = true
    @traitfn commutativetest(x::X) where {X; !IsCommutative{X}} = false

    @traitfn idempotenttest(x::X) where {X; IsIdempotent{X}} = true
    @traitfn idempotenttest(x::X) where {X; !IsIdempotent{X}} = false

    @traitfn startest(x::X) where {X; IsStar{X}} = true
    @traitfn startest(x::X) where {X; !IsStar{X}} = false

    w = BooleanSemiringElement(true)
    x = MaxPlusSemiringElement(3.0)
    y = MinPlusSemiringElement(5.0)
    z = RealSemiringElement(2)

    @test commutativetest(w)
    @test commutativetest(x)
    @test commutativetest(y)
    @test commutativetest(z)

    @test idempotenttest(w)
    @test idempotenttest(x)
    @test idempotenttest(y)
    @test !idempotenttest(z)

    @test startest(w)
    @test startest(x)
    @test startest(y)
    @test startest(z)
end