function mul_revDR(a::Interval,b::Interval,c::Interval) # DONE
    if (in(0,c) && ~in(0,b))
        c = c ∩ (a / b)
    elseif (~in(0,c) && in(0,b))
        b = b ∩ (a / c)
    elseif (~in(0,c) && ~in(0,b))
        b = b ∩ (a / c)
        c = c ∩ (a / b)
    end
    return a, b, c
end
mul_revDR(a,b,c) = div_revDR(promote(a,b,c)...)

function exp_revDR(y::Interval, x::Interval) # DONE
    y_new = y ∩ (0..∞)
    x_new = x ∩ log(y_new)

    return y_new, x_new
end
exp_revDR(a,b) = exp_revDR(promote(a,b)...)

function acos_rev(y::Interval, x::Interval) # DONE
    y_new = y ∩ (0.0..2*pi)
    x_new = x ∩ cos(y_new)

    return y_new, x_new
end
acos_rev(a,b) = acos_rev(promote(a,b)...)

function atan_rev(y::Interval, x::Interval) # DONE
    y_new = y ∩ (-pi/2.0..pi/2.0)
    x_new = x ∩ tan(y_new)

    return y_new, x_new
end
atan_rev(a,b) = atan_rev(promote(a,b)...)

function sinh_rev(y::Interval,x::Interval) # DONE
    x_new = x ∩ asinh(y_new)

    return y, x_new
end
sinh_rev(a,b) = sinh_rev(promote(a,b)...)

function cosh_rev(y::Interval,x::Interval) # DONE
    y_new = y ∩ (1.0..∞)
    x_new = x ∩ acosh(y_new)

    return y_new, x_new
end
cosh_rev(a,b) = cosh_rev(promote(a,b)...)

function tanh_rev(y::Interval,x::Interval) # DONE
    y_new = y ∩ ((-1.0)..(1.0))
    x_new = x ∩ atanh(y_new)

    return y_new, x_new
end
tanh_rev(a,b) = tanh_rev(promote(a,b)...)

function asinh_rev(y::Interval,x::Interval) # DONE
    x_new = x ∩ sinh(y_new)

    return y, x_new
end
asinh_rev(a,b) = asinh_rev(promote(a,b)...)

function acosh_rev(y::Interval,x::Interval) # DONE
    y_new = y ∩ (0.0..∞)
    x_new = x ∩ cosh(y_new)

    return y_new, x_new
end
acosh_rev(a,b) = acosh_rev(promote(a,b)...)

function atanh_rev(y::Interval,x::Interval) # DONE
    x_new = x ∩ tanh(y_new)

    return y, x_new
end
atanh_rev(a,b) = atanh_rev(promote(a,b)...)

function step_rev(y::Interval,x::Interval) # DONE
    y_new = y ∩ (0..1)

    return y_new, x
end
step_rev(a,b) = step_rev(promote(a,b)...)

function sign_rev(y::Interval,x::Interval) # DONE
    y_new = y ∩ ((-1.0)..(1.0))

    return y_new, x
end
sign_rev(a,b) = sign_rev(promote(a,b)...)

function exp2_rev(y::Interval,x::Interval) # DONE
    y_new = y ∩ (0..∞)
    x_new = x ∩ log10(y_new)

    return y_new, x_new
end
exp2_rev(a,b) = exp2_rev(promote(a,b)...)

function exp10_rev(y::Interval,x::Interval) # DONE
    y_new = y ∩ (0..∞)
    x_new = x ∩ log10(y_new)

    return y_new, x_new
end
exp10_rev(a,b) = exp10_rev(promote(a,b)...)

function log2_rev(y::Interval,x::Interval) # DONE
    y_new = y ∩ (0..∞)
    x_new = x ∩ exp2(y_new)

    return y_new, x_new
end
log2_rev(a,b) = log2_rev(promote(a,b)...)

function log10_rev(y::Interval,x::Interval) # DONE
    y_new = y ∩ (0..∞)
    x_new = x ∩ exp10(y_new)

    return y_new, x_new
end
log10_rev(a,b) = log10_rev(promote(a,b)...)

function one_rev(y::Interval,x::Interval) # DONE
    y_new = y ∩ ((1.0)..(1.0))

    return y_new, x
end
one_rev(a,b) = one_rev(promote(a,b)...)

function zero_rev(y::Interval,x::Interval) # DONE
    y_new = y ∩ ((0.0)..(0.0))

    return y_new, x
end
zero_rev(a,b) = zero_rev(promote(a,b)...)

function min_rev(z::Interval,x::Interval,y::Interval)
    y_new = y ∩ (0..∞)
    x_new = x ∩ log(y_new)

    return y_new, x_new
end
function max_rev(z::Interval,x::Interval,y::Interval)
    y_new = y ∩ (0..∞)
    x_new = x ∩ log(y_new)

    return y_new, x_new
end


using Statistics
using Interpolations
"""
    contrast(image; step=2)

Measure the contrast of an image, in the sense of high contrast imaging.
That is, divide the image into annuli moving outwards from the centre
(index 0,0 if offset image) and calculate the standard deviation in
each.

Returns a vector of annulus locations in pixels and a vector of standard
deviations.

*NOTE* This is the 1σ contrast. Multiply by five to get the usual confidence
value.
"""
function contrast(image; step=2)
    I = origin(image)
    if I[1] == I[2] == 1
        image = centered(image)
    end
    dx = UnitRange(axes(image,1))
    dy = UnitRange(axes(image,2))
    dr = sqrt.(
        dx.^2 .+ (dy').^2
    )

    c_img = collect(image)

    bins = 0:step:maximum(dr)
    # bins = 30:step:100
    contrast = zeros(size(bins))
    mask = falses(size(image))
    mask2 = isfinite.(c_img)
    for i in eachindex(bins)
        bin = bins[i]
        mask .= (bin.-step/2) .< dr .< (bin.+step/2)
        mask .&= mask2
        c = std(view(c_img, mask))
        contrast[i] = c
    end

    return (;separation=bins, contrast)
end

function radialbins(image; step=2)
    I = origin(image)
    if I[1] == I[2] == 1
        image = centered(image)
    end
    dx = UnitRange(axes(image,1))
    dy = UnitRange(axes(image,2))
    dr = sqrt.(
        dx.^2 .+ (dy').^2
    )

    c_img = collect(image)

    bins = 0:step:maximum(dr)
    # bins = 30:step:100
    contrast = zeros(size(bins))
    mask = falses(size(image))
    mask2 = isfinite.(c_img)
    radial_bins = map(eachindex(bins)) do i
        bin = bins[i]
        mask .= (bin.-step/2) .< dr .< (bin.+step/2)
        mask .&= mask2
        return view(c_img, mask)
    end

    return (;separation=bins, data=radial_bins)
end

"""
    out,prep = contrast!_prep(image, bins)
    contrast!(out,prep)

A fast non-allocating routine for calculating
contrast. Calculates the 1σ contrast of image
moving outwards from the centre accoring to `bins`.

After calling the `prep` function, you can alter
`image` with new data and calling `contrast!`
will still be very fast and non-allocating.
"""
function contrast!_prep(image,bins,spatialmask=trues(size(image)))
    cont = zeros(size(bins))
    # masks = falses(length(bins),size(image,1),size(image,2))
    # masks = BitMatrix[]
    # We want matrices of bool, not bitmatrix for non-allocating masking view performance
    masks = [
        fill(false, size(image))
        for _ in bins
    ]
    dx = axes(image,1) .- mean(axes(image,1))
    dy = axes(image,2) .- mean(axes(image,2))
    dr = sqrt.(
        dx.^2 .+ (dy').^2
    )
    s = step(bins)/2
    for i in eachindex(bins)
        bin = bins[i]
        masks[i] .= (bin.-s) .< dr .< (bin.+s)
        masks[i] .&= spatialmask
        # push!(masks,(bin.-s) .< dr .< (bin.+s) )
    end
    views = [view(image, mask) for mask in masks]
    return cont, views
end
function contrast!(out,views)
    for (i,v) in pairs(views)
        out[i] = std(v)
    end
end

export contrast
export contrast!_prep
export contrast!

"""
    contrast_interp(image; step=2)

Returns a linear interpolation on top of the results from `contrast`.
Extrapolated results return Inf.
"""
function contrast_interp(image; step=2)
    cont = contrast(image; step)
    return LinearInterpolation(cont.separation, cont.contrast, extrapolation_bc=Inf)
end

using Distributions

"""
    contrast_dist_interp(::Type{Distribution}, image; step=2)

Returns a linear interpolation on top of the results from `contrast`.
Extrapolated results return Inf.
"""
function contrast_dist_interp(Dist::Type{<:UnivariateDistribution}, image; step=2)
    sep, bins = radialbins(image; step)
    has_px = map(!isempty, bins)
    sep = sep[has_px]
    bins = bins[has_px]
    dists = map(bins) do bin
        fit(Dist, bin)
    end
    params = Distributions.params.(dists)
    param_arrs = [zeros(eltype(first(dists)), length(params)) for _ in 1:length(first(params))]
    for j in eachindex(param_arrs)
        for (i,param) in enumerate(params)
            param_arrs[j][i] = params[i][j]
        end
    end
    param_interps = Tuple(map(param_arrs) do parr
        LinearInterpolation(sep, parr, extrapolation_bc=Inf)
    end)
    return function (sep_px)
        params = map(f->f(sep_px), param_interps)
        dist = Dist(params...)
    end
end

export contrast_interp
export contrast_dist_interp

function profile(image; step=2)
    I = origin(image)
    if I[1] == I[2] == 1
        image = centered(image)
    end
    dx = UnitRange(axes(image,1))
    dy = UnitRange(axes(image,2))
    dr = sqrt.(
        dx.^2 .+ (dy').^2
    )

    c_img = collect(image)

    bins = 0:step:maximum(dr)
    # bins = 30:step:100
    profile = zeros(size(bins))
    mask = falses(size(image))
    mask2 = isfinite.(c_img)
    for i in eachindex(bins)
        bin = bins[i]
        mask .= (bin.-step/2) .< dr .< (bin.+step/2)
        mask .&= mask2
        c = mean(view(c_img, mask))
        profile[i] = c
    end

    return (;separation=bins, profile)
end

function snrmap(image, contrast=nothing)
    I = origin(image)
    if I[1] == I[2] == 1
        image = centered(image)
    end
    dx = UnitRange(axes(image,1))
    dy = UnitRange(axes(image,2))
    dr = sqrt.(
        dx.^2 .+ (dy').^2
    )

    if isnothing(contrast)
        contrast = contrast_interp(image)
    end
    snr = profile2image(contrast, image)

    out = similar(image)
    out .= image ./ snr
    return out
end
export snrmap


for N in [Float64, Float32, Rational{Int}]
    c1 = N[1, 2]
    c2 = N[3, 4, 5]
    Z1 = Zonotope(c1, N[1 0; 0 1])
    Z2 = Zonotope(c2, N[1 0 0; 0 1 0; 0 0 1])
    H1 = Hyperrectangle(c1, N[1, 1])
    H2 = Hyperrectangle(c2, N[1, 1, 1])
    B1 = BallInf(c1, N(1))
    B2 = BallInf(c2, N(1))

    Z1Z2 = cartesian_product(Z1, Z2)
    Z1H2 = cartesian_product(Z1, H2)
    H1Z2 = cartesian_product(H1, Z2)
    H1H2 = cartesian_product(H1, H2)
    H1B2 = cartesian_product(H1, B2)
    B1B2 = cartesian_product(B1, B2)
    for X in [Z1Z2, Z1H2, H1Z2]
        @test X isa Zonotope{N}
    end
    for X in [H1H2, H1B2, B1B2]
        @test X isa Hyperrectangle{N}
    end
    if test_suite_polyhedra || N <: AbstractFloat
        for X in [Z1Z2, Z1H2, H1Z2, H1H2, H1B2]
            @test isequivalent(X, B1B2)
        end
    end

    # Universe is handled correctly (#2735)
    P = HalfSpace(N[1, 2], N(3))
    U = Universe{N}(2)
    @test isequivalent(cartesian_product(P, U), HalfSpace(N[1, 2, 0, 0], N(3)))
    @test isequivalent(cartesian_product(U, P), HalfSpace(N[0, 0, 1, 2], N(3)))
    @test cartesian_product(U, U) == Universe{N}(4)
end


# imaginary sqrt
function imqr(x)
    try
        sqrt(x) + 0im
    catch e
        sqrt(-x)im
    end
end
# imaginary sqrt call
println(imqr(1))
println(imqr(-1))


using LinearAlgebra

@doc """
  `block_qx(W::ITensor,ul::reg_form)::Tuple{ITensor,ITensor,Index}`

Perform a block respecting QX decomposition of the operator valued matrix `W`.
The appropriate decomposition, QR, RQ, QL, LQ is selected based on the `reg_form` `ul`
and the `orth` keyword argument.
The new internal `Index` between Q and R/L is modified so that the tags are "Link,qx" instead
"Link,qr" etc. returned by the qr/rq/ql/lq routines.  Q and R/L are also gauge fixed so that
the corner element of R/L is 1.0 and Q⁺Q=d𝕀 where d is the dimensionality of the local
Hilbert space.

# Arguments
- `W` Operator valued matrix for decomposition.
- `ul` upper/lower regular form of `W`. We can auto detect here, but is more efficient if this is done by the higher level calling routines.

# Keywords
- `orth::orth_type = right` : choose `left` or `right` orthogonal (canonical) form
- `epsrr::Float64 = 1e-14` : cutoff for rank revealing QX which removes zero pivot rows and columns.
   All rows with max(abs(R[r,:]))<epsrr are considered zero and removed.  epsrr==0.0 indicates no rank reduction.

# Returns a Tuple containing
- `Q` with orthonormal columns or rows depending on orth=left/right, dimensions: (χ+1)x(χq+1)
- `R` or `L` depending on `ul` with dimensions: (χq+2)x(χ\'+2)
- `iq` the new internal link index between `Q` and `R`/`L` with dimensions χq+2 and tags="Link,qx"

# Example
```julia
julia>using ITensors
julia>using ITensorMPOCompression
julia>N=5; #5 sites
julia>NNN=2; #Include 2nd nearest neighbour interactions
julia>sites = siteinds("S=1/2",N);
#
#  Make a Hamiltonian directly, i.e. not using autoMPO
#
julia>H=make_transIsing_MPO(sites,NNN);
#
#  Use pprint to see the structure for site #2. I = unit operator and S = any
#  other operator
#
julia>pprint(H[2]) #H[1] is a row vector, so let's see what H[2] looks like
I 0 0 0 0
S 0 0 0 0
S 0 0 0 0
0 0 I 0 0
0 S 0 S I
#
#  Now do a block respecting QX decomposition. QL decomposition is chosen because
#  H[2] is in lower regular form and the default ortho direction is left.
#
julia>Q,L,iq=block_qx(H[2]); #Block respecting QL
#
#  The first column of Q is unchanged because it is outside the V-block.
#  Also one column was removed because rank revealing QX is the default algorithm.
#
julia>pprint(Q)
I 0 0 0
S 0 0 0
S 0 0 0
0 I 0 0
0 0 S I
#
#  Similarly L is missing one row due to the rank revealing QX algorithm
#
julia>pprint(L,iq) #we need to tell pprint which index is the row index.
I 0 0 0 0
0 0 I 0 0
0 S 0 S 0
0 0 0 0 I
```
"""
function block_qx(W::ITensor,ul::reg_form=lower;kwargs...)::Tuple{ITensor,ITensor,Index}
  d,n,r,c=parse_links(W)
  return block_qx(W,n,r,c,ul;kwargs...)
end

function block_qx(W_::ITensor,n::Int64,r::Index,c::Index,ul::reg_form=lower;kwargs...)::Tuple{ITensor,ITensor,Index}
  #
  # Copy so that we don't mess up the original MPO
  #
  W=copy(W_)
  #
  # settle the left/right && upper/lower question
  #
  lr::orth_type=get(kwargs, :orth, left)
  ms=matrix_state(ul,lr)
  #
  #  decide some strings and variables based on lr.
  #
  (tln,cr)= lr==left ? ("l=$n",c) : ("l=$(n-1)",r)

  ilw=filterinds(inds(W),tags=tln)[1] #get the link to the next site
  offset=V_offsets(ms)
  V=getV(W,offset) #extract the V block
  il=filterinds(inds(V),tags=tln)[1] #link to next site
  iothers=noncommoninds(inds(V),il) #group all other indices for QX factorization

  if ul==lower
    if lr==left
      Q,RL=ql(V,iothers;positive=true,kwargs...) #block respecting QL decomposition
    else #right
      RL,Q=lq(V,iothers;positive=true,kwargs...) #block respecting LQ decomposition
    end
  else #upper
    if lr==left
      Q,RL=ITensorMPOCompression.qr(V,iothers;positive=true,kwargs...) #block respecting QR decomposition
    else #right
      RL,Q=rq(V,iothers;positive=true,kwargs...) #block respecting RQ decomposition
    end
  end
  set_scale!(RL,Q,offset) #rescale so the L(n,n)==1.0
  ITensors.@debug_check begin
    @assert norm(V-RL*Q)<1e-12 #make sure decomp worked
  end
  qx=String(tags(commonind(RL,Q))[2]) #should be "ql","lq","qr" os "rq"
  replacetags!(Q ,qx,"qx") #releive client code from the burden dealing ql,lq,qr,rq tags
  replacetags!(RL,qx,"qx")
  W=setV(W,Q,ms) #Q is the new V, stuff Q into W. THis can resize W
  RLplus,iqx=growRL(RL,ilw,offset) #Now make a full size version of RL
  ilw=filterinds(W,tags=tln)[1]
  replaceind!(W,ilw,iqx)
  @assert hastags(W,"qx")
  return W,RLplus,iqx
end

function ql!(A::StridedMatrix{<:LAPACK.BlasFloat}, ::NoPivot; blocksize=36)
  tau=similar(A, Base.min(size(A)...))
  x=LAPACK.geqlf!(A, tau)
  #save L from the lower portion of A, before orgql! mangles it!
  nr,nc=size(A)
  mn=Base.min(nr,nc)
  L=similar(A,(mn,nc))
  for r in 1:mn
    for c in 1:r+nc-mn
      L[r,c]=A[r+nr-mn,c]
    end
    for c in r+1+nc-mn:nc
      L[r,c]=0.0
    end
  end
  # Now we need shift the orth vectors from the right side of Q over the left side, before
  if (mn<nc)
      for r in 1:nr
        for c in 1:mn
            A[r,c]=A[r,c+nc-mn];
        end
      end
  end
# A may now have extra columns from mn+1:nc, but they get chopped of in the outer functions
  LAPACK.orgql!(A,tau)
  return  A,L
end

function lq!(A::StridedMatrix{<:LAPACK.BlasFloat}, ::NoPivot; blocksize=36)
  tau=similar(A, Base.min(size(A)...))
  x=LAPACK.gelqf!(A, tau)
  #save L from the lower portion of A, before orgql! mangles it!
  nr,nc=size(A)
  mn=Base.min(nr,nc)
  L=similar(A,(nr,mn))
  for c in 1:mn
    for r in 1:c-1
      L[r,c]=0.0
    end
    for r in c:nr
      L[r,c]=A[r,c]
    end
  end

  LAPACK.orglq!(A,tau)
  # now A is Q but it may nave extra rows that need to be chopped off
  Q=A[1:mn,:]
  return  L,Q
end

function rq!(A::StridedMatrix{<:LAPACK.BlasFloat}, ::NoPivot; blocksize=36)
  tau=similar(A, Base.min(size(A)...))
  x=LAPACK.gerqf!(A, tau)
  #save R from the lower portion of A, before orgql! mangles it!
  nr,nc=size(A)
  mn=Base.min(nr,nc)
  R=similar(A,(nr,mn))
  for c in 1:mn
    for r in 1:c+nr-mn
      R[r,c]=A[r,c+nc-mn]
    end
    for r in c+1+nr-mn:nr
      R[r,c]=0.0
    end
  end
  #
  # If nr>nc we need shift the orth vectors from the bottom of Q up to top before
  # unpacking the reflectors.
  #
  if mn<nr
    for c in 1:nc
      for r in 1:mn
        A[r,c]=A[r+nr-mn,c]
      end
    end
    A=A[1:mn,:] #whack the extra rows in A or orgrq! will complain
  end
  LAPACK.orgrq!(A,tau)
  return  R,A
end

ql!(A::AbstractMatrix) = ql!(A, NoPivot())
lq!(A::AbstractMatrix) = lq!(A, NoPivot())
rq!(A::AbstractMatrix) = rq!(A, NoPivot())

function ql(A::AbstractMatrix{T}, arg...; kwargs...) where T
    Base.require_one_based_indexing(A)
    AA = similar(A, LinearAlgebra._qreltype(T), size(A))
    copyto!(AA, A)
    return ql!(AA, arg...; kwargs...)
end

function lq(A::AbstractMatrix{T}, arg...; kwargs...) where T
  Base.require_one_based_indexing(A)
  AA = similar(A, LinearAlgebra._qreltype(T), size(A))
  copyto!(AA, A)
  return lq!(AA, arg...; kwargs...)
end

function rq(A::AbstractMatrix{T}, arg...; kwargs...) where T
  Base.require_one_based_indexing(A)
  AA = similar(A, LinearAlgebra._qreltype(T), size(A))
  copyto!(AA, A)
  return rq!(AA, arg...; kwargs...)
end

function ql_positive(M::AbstractMatrix)
  sparseQ, L = ql(M)
  Q = convert(Matrix, sparseQ)
  nr,nc = size(Q)
  dc=nc>nr ? nc-nr : 0 #diag is shifted over by dc if nc>nr
  for c in 1:nc
    if c<=nr && real(L[c, c+dc]) < 0.0
      L[c, 1:c+dc] *= -1
      Q[:,c] *= -1
    end
  end
  return (Q, L)
end

function lq_positive(M::AbstractMatrix)
  L, sparseQ = lq(M)
  Q = convert(Matrix, sparseQ)
  nr,nc = size(Q)
  for r in 1:nr
    if r<=nc && real(L[r, r]) < 0.0
      L[r:end, r] *= -1
      Q[r,:] *= -1
    end
  end
  return (L, Q)
end

function rq_positive(M::AbstractMatrix)
  R, sparseQ = rq(M)
  Q = convert(Matrix, sparseQ)
  nr,nc = size(R)
  dr=nr>nc ? nr-nc : 0 #diag is shifted down by dr if nr>nc
  for r in 1:nr
    if r<=nc && real(R[r+dr, r]) < 0.0
      R[1:r+dr, r] *= -1
      Q[r,:] *= -1
    end
  end
  return (R, Q)
end

function ql(T::NDTensors.DenseTensor{ElT,2,IndsT}; kwargs...) where {ElT,IndsT}
    positive = get(kwargs, :positive, false)
    # TODO: just call qr on T directly (make sure
    # that is fast)
    if positive
      QM, LM = ql_positive(matrix(T))
    else
      QM, LM = ql(matrix(T))
    end
    # Make the new indices to go onto Q and L
    q, l = inds(T)
    q = dim(q) < dim(l) ? sim(q) : sim(l)
    Qinds = IndsT((ind(T, 1), q))
    Linds = IndsT((q, ind(T, 2)))
    Q = NDTensors.tensor(NDTensors.Dense(vec(Matrix(QM))), Qinds)
    L = NDTensors.tensor(NDTensors.Dense(vec(LM)), Linds)
    return Q, L
  end

  function lq(T::NDTensors.DenseTensor{ElT,2,IndsT}; kwargs...) where {ElT,IndsT}
    positive = get(kwargs, :positive, false)
    # TODO: just call qr on T directly (make sure
    # that is fast)
    if positive
      LM, QM = lq_positive(matrix(T))
    else
      LM, QM = lq(matrix(T))
    end

    # Make the new indices to go onto Q and L
    l, q = inds(T)
    #@show inds(T)
    q = dim(q) < dim(l) ? sim(q) : sim(l)
    Qinds = IndsT((q,ind(T, 2)))
    Linds = IndsT((ind(T, 1),q))
    #@show Linds Qinds
    Q = NDTensors.tensor(NDTensors.Dense(vec(Matrix(QM))), Qinds)
    L = NDTensors.tensor(NDTensors.Dense(vec(LM)), Linds)
    return L, Q
  end

  function rq(T::NDTensors.DenseTensor{ElT,2,IndsT}; kwargs...) where {ElT,IndsT}
    positive = get(kwargs, :positive, false)
    # TODO: just call qr on T directly (make sure
    # that is fast)
    if positive
      RM, QM = rq_positive(matrix(T))
    else
      RM, QM = rq(matrix(T))
    end

    # Make the new indices to go onto Q and L
    #@show inds(T)
    l, q = inds(T)
    q = dim(q) < dim(l) ? sim(q) : sim(l)
    Qinds = IndsT((q,ind(T, 2)))
    Linds = IndsT((ind(T, 1),q))
    #@show Linds Qinds
    Q = NDTensors.tensor(NDTensors.Dense(vec(Matrix(QM))), Qinds)
    R = NDTensors.tensor(NDTensors.Dense(vec(RM)), Linds)
    return R, Q
  end

# ql decomposition of an order-n dense tensor according to
# positions Lpos and Rpos
function ql(
  T::NDTensors.DenseTensor{<:Number,N,IndsT},
  Lpos::NTuple{NL,Int},
  Rpos::NTuple{NR,Int};
  kwargs...
) where {N,IndsT,NL,NR}
  M = NDTensors.permute_reshape(T, Lpos, Rpos)
  QM, LM = ql(M; kwargs...)
  l = ind(LM, 1)
  # TODO: simplify this by permuting inds(T) by (Lpos,Rpos)
  # then grab Linds,Rinds
  Linds = NDTensors.similartype(IndsT, Val{NL})(ntuple(i -> inds(T)[Lpos[i]], Val(NL)))
  Qinds = NDTensors.push(Linds, l)
  Q = reshape(QM, Qinds)
  Rinds = NDTensors.similartype(IndsT, Val{NR})(ntuple(i -> inds(T)[Rpos[i]], Val(NR)))
  Rinds = NDTensors.pushfirst(Rinds, l)
  L = NDTensors.reshape(LM, Rinds)
  return Q, L
end
# ql decomposition of an order-n block sparse tensor according to
# positions Lpos and Rpos
function ql(
  T::NDTensors.BlockSparseTensor{Elt,N,IndsT},
  Lpos::NTuple{NL,Int},
  Rpos::NTuple{NR,Int};
  kwargs...
) where {Elt,N,IndsT,NL,NR}

  nb=nnzblocks(T)
  Qs = Vector{DenseTensor{Elt,NL+1,NTuple{NL+1, Int64},<:NDTensors.Dense}}(undef, nb)
  Ls = Vector{DenseTensor{Elt,NR+1,NTuple{NR+1, Int64},<:NDTensors.Dense}}(undef, nb)

  #@show Lpos Rpos inds(T) dim(T,1) dim(T,2)
  #
  @show "in ql block sparse" Lpos Rpos
  for (n, b) in enumerate(eachnzblock(T))
    blockT = blockview(T, b) #this is a DenseTensor so we call the dense ql for each block.
    Qs[n],Ls[n] = ql(blockT,Lpos,Rpos;kwargs...) #All reshaping is taken care of in this call.
    @show blockT Qs[n] Ls[n]
  end

  #
  #  We need to figure out what is the index between Q and L this code is stolen from
  #  ITensors/NDTensors/src/blocksparse/linearalgebra.jl line ~107 in
  #  function LinearAlgebra.svd(T::BlockSparseMatrix{ElT}; kwargs...)
  #  I don;t know what it is supposed to be doing
  #
  nb1_lt_nb2 = (
    nblocks(T)[1] < nblocks(T)[2] ||
    (nblocks(T)[1] == nblocks(T)[2] && dim(T, 1) < dim(T, 2))
  )

  if nb1_lt_nb2
    qind = sim(ind(T, 1))
  else
    qind = sim(ind(T, 2))
  end
   # qind may have too many blocks ... really why?
  if nblocks(qind) > nb
    resize!(qind, nb)
  end

  if dir(qind) != dir(inds(T)[1])
    qind = dag(qind)
  end
  indsQ = setindex(inds(T), qind, 2)
  @show indsQ
  indsL = dag(qind),inds(T)[2]
  @show indsL typeof(indsL)
  nzblocksQ = Vector{Block{4}}(undef, nb)
  nzblocksL = Vector{Block{2}}(undef, nb)
  for n in 1:nb
    blockT=nzblocks(T)[n]
    @show blockT
    blockQ = (blockT[Lpos[1]],UInt(1), blockT[Lpos[2]],blockT[Lpos[3]] )
    @show blockQ
    nzblocksQ[n]=blockQ
    blockL = (blockT[Rpos[1]], UInt(1))
    @show blockL
    nzblocksL[n]=blockL
  end
  Q = BlockSparseTensor(Elt, undef, nzblocksQ, indsQ)
  L = BlockSparseTensor(Elt, undef, nzblocksL, indsL)
  for n in 1:nb
    blockview(Q, nzblocksQ[n]) .= Qs[n]
    blockview(L, nzblocksL[n]) .= Ls[n]
  end
  @show Q
  @show L
  return Q,L
end

# lq decomposition of an order-n tensor according to
# positions Lpos and Rpos
function lq(
  T::NDTensors.DenseTensor{<:Number,N,IndsT},
  Lpos::NTuple{NL,Int},
  Rpos::NTuple{NR,Int};
  kwargs...
) where {N,IndsT,NL,NR}
  M = NDTensors.permute_reshape(T, Lpos, Rpos)
  LM, QM = lq(M; kwargs...)
  l = ind(LM, 2)
  # TODO: simplify this by permuting inds(T) by (Lpos,Rpos)
  # then grab Linds,Rinds
  Rinds = NDTensors.similartype(IndsT, Val{NR})(ntuple(i -> inds(T)[Rpos[i]], Val(NR)))
  Qinds = NDTensors.pushfirst(Rinds, l)
  Q = reshape(QM, Qinds)
  Linds = NDTensors.similartype(IndsT, Val{NL})(ntuple(i -> inds(T)[Lpos[i]], Val(NL)))
  Linds = NDTensors.push(Linds, l)
  L = NDTensors.reshape(LM, Linds)
  return L, Q
end

# lq decomposition of an order-n tensor according to
# positions Lpos and Rpos
function rq(
  T::NDTensors.DenseTensor{<:Number,N,IndsT},
  Lpos::NTuple{NL,Int},
  Rpos::NTuple{NR,Int};
  kwargs...
) where {N,IndsT,NL,NR}
  M = NDTensors.permute_reshape(T, Lpos, Rpos)
  RM, QM = rq(M; kwargs...)
  l = ind(RM, 2)
  # TODO: simplify this by permuting inds(T) by (Lpos,Rpos)
  # then grab Linds,Rinds
  Rinds = NDTensors.similartype(IndsT, Val{NR})(ntuple(i -> inds(T)[Rpos[i]], Val(NR)))
  Qinds = NDTensors.pushfirst(Rinds, l)
  Q = reshape(QM, Qinds)
  Linds = NDTensors.similartype(IndsT, Val{NL})(ntuple(i -> inds(T)[Lpos[i]], Val(NL)))
  Linds = NDTensors.push(Linds, l)
  R = NDTensors.reshape(RM, Linds)
  return R, Q
end

function ql(A::ITensor, Linds...; kwargs...)
    tags::TagSet = get(kwargs, :tags, "Link,ql")
    Lis = commoninds(A, ITensors.indices(Linds))
    Ris = uniqueinds(A, Lis)
    Lpos, Rpos = NDTensors.getperms(inds(A), Lis, Ris)
    QT, LT = ql(ITensors.tensor(A), Lpos, Rpos; kwargs...)
    Q, L = itensor(QT), itensor(LT)
    q::Index = ITensors.commonind(Q, L)
    settags!(Q, tags, q)
    settags!(L, tags, q)
    q = settags(q, tags)
    #
    #  Do row removal for rank revealing LQ
    #
    epsrr::Float64 = get(kwargs, :epsrr , 1e-14)
    if epsrr>0.0 L,Q=trim(L,Q,epsrr) end
    return Q, L, q
end

function lq(A::ITensor, Rinds...; kwargs...)
  tags::TagSet = get(kwargs, :tags, "Link,lq")
  Ris = commoninds(A, ITensors.indices(Rinds))
  Lis = uniqueinds(A, Ris)
  Lpos, Rpos = NDTensors.getperms(inds(A), Lis, Ris)
  LT, QT = lq(ITensors.tensor(A), Lpos, Rpos; kwargs...)
  Q, L = itensor(QT), itensor(LT)
  q::Index = ITensors.commonind(Q, L)
  settags!(Q, tags, q)
  settags!(L, tags, q)
  q = settags(q, tags)
  #
  #  Do row removal for rank revealing LQ
  #
  epsrr::Float64 = get(kwargs, :epsrr , 1e-14)
  if epsrr>0.0 L,Q=trim(L,Q,epsrr) end
  return L, Q, q
end

function rq(A::ITensor, Rinds...; kwargs...)
  tags::TagSet = get(kwargs, :tags, "Link,rq")
  Ris = commoninds(A, ITensors.indices(Rinds))
  Lis = uniqueinds(A, Ris)
  Lpos, Rpos = NDTensors.getperms(inds(A), Lis, Ris)
  RT, QT = rq(ITensors.tensor(A), Lpos, Rpos; kwargs...)
  Q, R = itensor(QT), itensor(RT)
  q::Index = ITensors.commonind(Q, R)
  settags!(Q, tags, q)
  settags!(R, tags, q)
  q = settags(q, tags)
  #
  #  Do row removal for rank revealing RQ
  #
  epsrr::Float64 = get(kwargs, :epsrr , 1e-14)
  if epsrr>0.0 R,Q=trim(R,Q,epsrr) end
  return R, Q, q
end

function qr(A::ITensor, Rinds...; kwargs...)
  Q,R,q=ITensors.qr(A,Rinds...;kwargs...)
  #
  #  Do row removal for rank revealing RQ
  #
  epsrr::Float64 = get(kwargs, :epsrr , 1e-14)
  if epsrr>0.0 R,Q=trim(R,Q,epsrr) end
  return Q, R, q
end

function trim(R::ITensor,Q::ITensor,eps::Float64)
  iq=commonind(R,Q)
  zeros=find_zero_rows(R,iq,eps)
  if sum(zeros)==0
    return R,Q
  end
  #@printf "Rank Reveal removing %4i rows with epsrr=%.1e\n" sum(zeros) eps
  nq=dim(iq)-sum(zeros)
  iRo=noncommoninds(R,iq)
  iQo=noncommoninds(Q,iq)
  iqn=Index(nq,tags(iq))
  T=eltype(R)
  Rn=ITensor(T(0.0),iqn,iRo)
  Qn=ITensor(T(0.0),iQo,iqn)
  ivqn=1
  for ivq in eachindval(iq)
    if zeros[ivq.second]==false
      for ivRo in eachindval(iRo...)
        Rn[iqn=>ivqn,ivRo]=R[ivq,ivRo]
      end #for ivRo
      for ivQo in eachindval(iQo...)
        Qn[iqn=>ivqn,ivQo...]=Q[ivq,ivQo...]
      end #for ivQo
      ivqn+=1
    end #if zero
  end #for ivq
  return Rn,Qn
end

function find_zero_rows(R::ITensor,iq::Index,eps::Float64)::Array{Bool}
  zeros=falses(dim(iq))
  others=noncommoninds(R,iq)
  for iqv in eachindval(iq)
    s=0.0
    for io in eachindval(others...)
      s+=abs(R[iqv,io])
    end
    zeros[iqv.second]= (s<=eps)
  end
  return zeros
end
#=
is=Index(2,"Site,n=1")
ir=Index(4,"Link,l=0")
ic=Index(4,"Link,l=1")
iq=Index(4,"Link,qx")
#AQ=[i*1.0 for i in 1:dim(ic)*dim(iq)*dim(is)*dim(is)]
#Q=ITensor(eltype(AQ),AQ,ic,iq,is,is')
AQ=[i*1.0 for i in 1:dim(ic)*dim(iq)]
Q=ITensor(eltype(AQ),AQ,ic,iq)

A=[1:4;;5:8;;9:12;;13:16]
A[2,:].=0.0
R=ITensor(eltype(A),A,iq,ir)
z=find_zero_rows(R,iq,0.0)
Rn,Qn=trim(R,Q,1e-14)
@show Rn
@show Qn
=#


using DataFrames

mutable struct problem
    data::Array{Float64}
    Sigma::Array{Float64}
end

################ Pitprops
thepitprops=[[1,0.954,0.364,0.342,-0.129,0.313,0.496,0.424,0.592,0.545,0.084,-0.019,0.134];
       [0.954,1,0.297,0.284,-0.118,0.291,0.503,0.419,0.648,0.569,0.076,-0.036,0.144];
       [0.364,0.297,1,0.882,-0.148,0.153,-0.029,-0.054,0.125,-0.081,0.162,0.22,0.126];
       [0.342,0.284,0.882,1,0.22,0.381,0.174,-0.059,0.137,-0.014,0.097,0.169,0.015];
       [-0.129,-0.118,-0.148,0.22,1,0.364,0.296,0.004,-0.039,0.037,-0.091,-0.145,-0.208];
       [0.313,0.291,0.153,0.381,0.364,1,0.813,0.09,0.211,0.274,-0.036,0.024,-0.329];
       [0.496,0.503,-0.029,0.174,0.296,0.813,1,0.372,0.465,0.679,-0.113,-0.232,-0.424];
       [0.424,0.419,-0.054,-0.059,0.004,0.09,0.372,1,0.482,0.557,0.061,-0.357,-0.202];
       [0.592,0.648,0.125,0.137,-0.039,0.211,0.465,0.482,1,0.526,0.085,-0.127,-0.076];
       [0.545,0.569,-0.081,-0.014,0.037,0.274,0.679,0.557,0.526,1,-0.319,-0.368,-0.291];
       [0.084,0.076,0.162,0.097,-0.091,-0.036,-0.113,0.061,0.085,-0.319,1,0.029,0.007];
       [-0.019,-0.036,0.22,0.169,-0.145,0.024,-0.232,-0.357,-0.127,-0.368,0.029,1,0.184];
       [0.134,0.144,0.126,0.015,-0.208,-0.329,-0.424,-0.202,-0.076,-0.291,0.007,0.184,1];]
thepitprops=reshape(thepitprops, (13,13));
B=sqrt(thepitprops);
pitprops=problem(B, thepitprops)


# minimal definition of a custom kernel
struct MyKernel <: SimpleKernel end

KernelFunctions.kappa(::MyKernel, d2::Real) = exp(-d2)
KernelFunctions.metric(::MyKernel) = SqEuclidean()

@testset "custom" begin
    @test kappa(MyKernel(), 3) == kappa(SqExponentialKernel(), 3)
    @test kernelmatrix(MyKernel(), [1 2; 3 4], [5 6; 7 8]) == kernelmatrix(SqExponentialKernel(), [1 2; 3 4], [5 6; 7 8])
    @test kernelmatrix(MyKernel(), [1 2; 3 4]) == kernelmatrix(SqExponentialKernel(), [1 2; 3 4])
end


#TODO: Fix offset in visualization for periodic bounds
#TODO: Fix visualization for different bounds -- add simple bound

using FFTW
using Plots
using Plots.PlotMeasures
using LinearAlgebra

import Plots: default
default(show=false, reuse=true)

ENV["GKSwstype"]="nul"

abstract type Bounds end

abstract type Periodic <: Bounds end

function signal_plot(i, signal1, signal2, out; filebase = "out",
                     out_name = "convolution", scale = 1.0,
                     bounds_type = :periodic)

    println(i)
    n = length(out)
    temp = -ones(n)

    signal1 = vcat(signal1, zeros(n - length(signal1)))
    signal2 = reverse(signal2)

    if bounds_type == :periodic
        if i < length(signal2)
            shift_signal = real([signal2[end-i+1:end]; zeros(n-length(signal2));
                                signal2[1:end-i]])
        else
            shift_signal = real([zeros(i-length(signal2)); signal2; zeros(n-i)])
        end
    else
        if i < length(signal2)
            shift_signal = real([signal2[end-i+1:end]; zeros(n-i)])
        else
            shift_signal = real([zeros(i-length(signal2)); signal2; zeros(n-i)])
        end
    end

    conv_sum = real(signal1).*shift_signal

#=
    n1 = length(signal1)
    n2 = length(signal2)
    if n2 < n1
        signal2 = [reverse(signal2);zeros(n1-n2)]
    elseif n1 < n2
        signal1 = [signal1;zeros(n2-n1)]
        signal2 = reverse(signal2)
    else
        signal2 = reverse(signal2)
    end
    if bounds_type == :periodic
        shift_signal = real([signal2[n1-i:n1-1];signal2[1:n1-i]])
    elseif bounds_type == :simple
        shift_signal = vcat(zeros(i), signal2[1:end-i])
    else
        println("bound type not found")
    end
=#

    conv_sum = real(signal1).*shift_signal

    plt = plot(real(signal1); label = "signal (f(x))",
               ylabel = "Signal and Filter values", dpi=200,
               ylims=(0,1.35), size=(600,400), right_margin=12mm,
               xlabel = "x", legend=:topright)
    plt = plot!(plt, shift_signal; label = "filter (g(x-t))")
    plt = plot!(plt, zeros(n); label = "area under f(t)g(x-t)",
                ribbon=(zeros(n),conv_sum))
    plt = plot!(plt, temp; label = out_name, linecolor=:purple,
                linestyle=:dash)

    plt = twinx()
    plt = plot!(plt, real(out[1:i]); label = "",
                linewidth=2, linestyle=:dash, ylabel = "Convolution output",
                grid=:off, ylims = (0,scale*1.35), linecolor=:purple)
    plt = plot!(plt, ([i,i],[0,real(out[i])/scale]); label = "",
                linewidth=2, linecolor=:purple)
#=
    plt = plot!(plt, vcat(zeros(i-1), real(out[i])/scale); label = "",
                linewidth=2, linecolor=:purple)
=#
    savefig(filebase*lpad(string(i),5,string(0))*".png")
end

function conv_fft(signal1::Array{Float64,1},
                  signal2::Array{Float64,1})
    return ifft(fft(signal1).*fft(signal2))
end

function conv_periodic(signal1::Array, signal2::Array; norm_factor = 1,
                  plot = false, frame_output = 1)

    n = length(signal1)

    out = Array{Float64,1}(undef,n)
    sum = 0

    for i = 1:n
        for j = 1:n
            if j < length(signal1) && mod1(i-j, n)+1 < length(signal2)
                sum += signal1[j] * signal2[mod1(i-j, n)+1]
            end
        end
        out[i] = sum

        if plot && i % frame_output == 0
            signal_plot(i, signal1, signal2, out; scale = norm_factor,
                        bounds_type = :periodic)
        end

        sum = 0
    end
    return out
end

function conv_lin(signal1::Array, signal2::Array;
                  norm_factor = 1, plot = false, frame_output = 1)

    n = length(signal1) + length(signal2)

    out = Array{Float64,1}(undef,n)
    sum = 0

    for i = 1:n
        for j = 1:i
            if j < length(signal1) && i-j+1 < length(signal2)
                sum += signal1[j] * signal2[i-j+1]
            end
        end
        out[i] = sum

        if plot && i % frame_output == 0
            signal_plot(i, signal1, signal2, out; scale = norm_factor,
                        bounds_type = :none)
        end

        sum = 0
    end

#=
    n = length(signal1)
    out = zeros(n)
    rsum = 0

    for i = 1:n-1
        if i+length(signal2) <= length(signal1)
            rsum = sum(signal1[i:i+length(signal2)-1].*reverse(signal2))
        elseif bounds_type == :periodic
            offset = i+length(signal2)-length(signal1)
            println("offset is ", offset)
            rsum = sum(vcat(signal1[i:end], signal1[1:offset-1])
                       .*reverse(signal2))
        elseif bounds_type == :simple
            diff = i+length(signal2)-length(signal1)
            rsum = sum(signal1[i:i+length(signal2)-1-diff].*
                       reverse(signal2)[1:end-diff])
        else
            println("bound type not found")
        end
        out[i] = rsum
        if plot && i % frame_output == 0
            signal_plot(i, signal1, signal2, out; scale = norm_factor,
                        bounds_type = bounds_type)
        end
        rsum = 0
    end
=#

    return out
end

function conv_check(signal1::Array, signal2::Array)
    n = length(signal1) + length(signal2)
    out = Array{Complex{Float64},1}(undef,n)
    sum = 0

    for i = 1:n-1
        for j = 1:i-1
            if i-j < length(signal2) && j < length(signal1)
                sum += signal1[j] * signal2[i-j]
            end
        end
        out[i] = sum
        sum = 0
    end

    return out
end

function main()
    # Gaussian signals
    #x = [exp(-((i-500)/1000)^2/.01) for i = 1:1000]
    y = [exp(-((i-50)/100)^2/.01) for i = 1:100]
    #y = vcat(y[div(length(y),2)+1:end], y[1:div(length(y),2)])

    # Square wave
    x = zeros(1000)
    x[400:600] .= 1.0 + 0im

    # Random
    x = rand(100)

    # Saw tooth
    #y = [(float(i))/200 for i = 200:-1:0]

    # Normalize
    #normalize!(x)
    #normalize!(y)
    out_conv = conv_periodic(x,y, norm_factor=sum(x.*x); plot=true,
                             frame_output = 1)
    #out_conv = conv_lin(x,y, norm_factor=sum(x.*x); plot=true,
    #                   frame_output = 1)

    return out_conv
end


using UnitfulCosmo
using Test

@testset "planck" begin
    # from Mukhanov. temperature, length, mass, time. no test for current yet
    @test isapprox(planck(2.725u"K"), 1.93e-32, rtol=1e-2)
    @test isapprox(unplanck(u"K", 1.93e-32), 2.725u"K", rtol=1e-2)
    @test isapprox(planck(310u"cm^-3"), 1.31e-96, rtol=1e-2)
    @test isapprox(unplanck(u"cm^-3", 1.31e-96), 310u"cm^-3", rtol=1e-2)
    @test isapprox(planck(1u"s"), 1.86e43, rtol=1e-2)
    @test isapprox(unplanck(u"s", 1.86e43), 1u"s", rtol=1e-2)
    @test isapprox(planck(4.45e5u"g*cm^-3"), 8.63e-89, rtol=1e-2)
    @test isapprox(unplanck(u"g*cm^-3", 8.63e-89), 4.45e5u"g*cm^-3", rtol=1e-2)
    @test_throws Unitful.DimensionError unplanck(u"Mpc", 1u"Mpc^-1")
end

@testset "mpc" begin
    @test mpc(1u"Mpc") ≈ 1u"Mpc"
    @test unmpc(u"Mpc", 1u"Mpc") ≈ 1u"Mpc"
    @test mpc(1u"yr") ≈ 3.064e-7u"Mpc" rtol=1e-2
    @test unmpc(u"yr", 3.064e-7u"Mpc") ≈ 1u"yr" rtol=1e-2
    @test mpc(10u"kg") ≈ 8.772e65u"Mpc^-1" rtol=1e-2
    @test unmpc(u"kg", 8.772e65u"Mpc^-1") ≈ 10u"kg" rtol=1e-2
    @test mpc(273u"K") ≈ 3.679e27u"Mpc^-1" rtol=1e-2
    @test unmpc(u"K", 3.679e27u"Mpc^-1") ≈ 273u"K" rtol=1e-2
    @test mpc(10u"A") ≈ 1.945391641e33u"Mpc^-1" rtol=1e-2
    @test unmpc(u"A", 1.945391641e33u"Mpc^-1") ≈ 10u"A" rtol=1e-2
    @test mpc(10u"kg"; base=u"kpc") ≈ 8.772e62u"kpc^-1" rtol=1e-2

    @test_throws Unitful.DimensionError unmpc(u"Mpc", 1u"Mpc^-1")
    @test_throws ArgumentError("mpc(q; base)` where `base` has dimensions `𝐌` has not yet been implemented. Please use a base with dimensions of `length` and then use `unmpc` to convert to your desired units."
        )  mpc(10u"kg"; base=u"kg")
end


sigmoid(x::Real) = 1 / (1 + exp(-x))
relu(x::Real) = max(x, 0.0)
softplus(x::Real) = log(1 + exp(x))
elu(x::Real) = max(x, 0.0) + (exp(x) - 1)*(x <= 0.0)

function Base.push!{T, A<:Activation}(net::ForwardNet{T}, ::Type{A},
    name::Symbol,
    parent::NameOrIndex,
    )

    input = ForwardNets.output(net[parent])
    output = deepcopy(input)
    node = A(name, input, output)
    push!(net, node, parent)
end
Base.push!{T, A<:Activation}(net::ForwardNet{T}, ::Type{A}, parent::NameOrIndex) =
    push!(net, A, create_next_name!(net, string(A)), parent)

type TanH{T} <: Activation{T}
    name::Symbol
    input::Array{T}
    output::Array{T}
end
name(a::TanH) = a.name
output(a::TanH) = a.output
function forward!(a::TanH)
    a.output .= tanh.(a.input)
    a
end

type ReLU{T} <: Activation{T}
    name::Symbol
    input::Array{T}
    output::Array{T}
end
name(a::ReLU) = a.name
output(a::ReLU) = a.output
function forward!{T}(a::ReLU{T})
    a.output .= max.(a.input, zero(T))
    a
end

type ELU{T} <: Activation{T}
    name::Symbol
    input::Array{T}
    output::Array{T}
end
name(a::ELU) = a.name
output(a::ELU) = a.output
function forward!{T}(a::ELU{T})
    a.output .= elu.(a.input)
    a
end

type SoftPlus{T} <: Activation{T}
    name::Symbol
    input::Array{T}
    output::Array{T}
end
name(a::SoftPlus) = a.name
output(a::SoftPlus) = a.output
function forward!(a::SoftPlus)
    a.output .= softplus.(a.input)
    a
end


function leapfrog_step!(r::AbstractVecOrMat{V3{T}}, v::AbstractVecOrMat{V3{T}},
         m::AbstractVector; G, dt) where T
    @assert length(r) == length(m) == length(v)
    a = zeros(V3{T}, length(m))
    @inbounds for j=1:length(m)
        r[j] = dt/2*v[j] + r[j]
    end
    update_acceleration!(a, r, m, G)
    @inbounds for j=1:length(m)
        v[j] = dt*a[j] + v[j]
        r[j] = dt/2*v[j] + r[j]
    end
    return r, v
end

function leapfrog!(r::AbstractVecOrMat{V3{T}}, v::AbstractVecOrMat{V3{T}},
         m::AbstractVector; G=G_year_AU, n, dt) where T
    leapfrog_looper!(LeapFrog, r, v, m; G=G, n=n, dt=dt)
end

function leapfrog(planets::AbstractVector{Body{T}}; G=G_year_AU, n, dt, keep_history=false) where {T}
    leapfrog(LeapFrog, planets; G=G, n=n, dt=dt, keep_history=keep_history)
end

steper(::Type{LeapFrog}) = leapfrog_step!


function cosineknn_test()
    A = load_matrix_network("bfs_example")
    S = cosineknn(A,2)

    A = speye(Int64,4)
    A[4,1] = 1
    CKNN = cosineknn(A,2)
    if ((CKNN[4,1] *2)^2 - 2) > 1e-7
        error("cosine knn failed")
    end
    return true
end


#core.jl

function operators(n)
    rowb, colb, base, vacio = integer_digits(n)
    l = 2^n
    lb = n*2^(n-1)
    row = spzeros(lb+1)
    col = spzeros(lb+1)
    data = spzeros(lb+1)
    row[lb+1] = l
    col[lb+1] = 1
    data[lb+1] = 0
    for i in 1:lb
        j = floor(Int, rowb[i])-2^(n-floor(Int, colb[i]))
        sign = (-1)^(sum(base[floor(Int, rowb[i]),1:floor(Int, colb[i])])+1)
        row[i] = j
        col[i] = floor(Int, rowb[i])+(floor(Int, colb[i])-1)*l
        data[i] = sign
    end
    cm_tot = sparse(row, col, data)
    cd_tot = sparse(cm_tot')
    return cm_tot, cd_tot, l, base
end

#This function outputs elements for building
#the sparse matrix of basis
function integer_digits(n)
    rowb = spzeros(n*2^(n-1))
    colb = spzeros(n*2^(n-1))
    data = spzeros(n*2^(n-1))
    counter = 1
    for i in 0:(2^n-1)
        binary_base = bitstring(i)[65-n:64]
        bin_vector = splitter(binary_base)
        for j in 1:n
            if bin_vector[j] == 1
                rowb[counter] = (i+1)::Int
                colb[counter] = j::Int
                data[counter] = 1
                counter = counter + 1
            end
        end
    end
    base = sparse(rowb, colb, data)
    vacio = spzeros(2^n)
    vacio[1] = 1
    return rowb, colb, base, vacio
end

# The following function outputs the basis with fixed_state
# particle number together with the indices
function basis_m(n::Int64,m::Int64)
    len = binomial(n,m)
    rowb = spzeros(m*len)
    colb = spzeros(m*len)
    data = spzeros(m*len)
    #binarios will be the index for the basis element in
    #the complete basis
    binarios = spzeros(binomial(n,m))
    counter = 1
    counter2 = 1
    base = spzeros(len,n)
    for i in 0:(2^n-1)
        binary_base = bitstring(i)[65-n:64]
        bin_vector = splitter(binary_base)
        if sum(bin_vector) == m
            for j in 1:n
                if bin_vector[j] == 1
                    rowb[counter] = counter2::Int
                    colb[counter] = j::Int
                    data[counter] = 1
                    counter = counter + 1
                end
             end
            binarios[counter2] = i
            counter2 = counter2+1
        end
    end
    base = sparse(rowb, colb, data)
    # It could also output rowb and colb if necessary
    binarios = binarios .+1
    return base, binarios
end

function splitter(x)
    vectorized = []
    for i in x
        str_to_int = parse(Int, i)
        append!(vectorized, str_to_int)
    end
    return vectorized
end


### A Pluto.jl notebook ###
# v0.9.2

using Markdown

# ╔═╡ 5b2ee40e-a2b8-11ea-0fef-c35fe6918860
md"""
# The tower of Hanoi

The tower of hanoi is a famous puzzle.

![setup of the tower of a hanoi](https://upload.wikimedia.org/wikipedia/commons/0/07/Tower_of_Hanoi.jpeg)

The game consists of three rods with disks stacked on top of them. The puzzle will start with all the disk in a stack on one of the rods (like in the picture). The goal is to move all the discs to a single stack on  different rod.

To move the disks, you have to follow the following rules:

* You can move only one disk at a time.
* For each move, you have to take the upper disk from one of the stacks, and place it on top of another stack or empty rod.
* You cannot place a larger disk on top of a smaller disk.

This notebook will define a Julia implementation of the puzzle. It's up to you to write an algorithm that solves it.
"""

# ╔═╡ 95fbd0d2-a2b9-11ea-0682-fdf783251797
md"""
## Setting up the game pieces

What does a Julia implementation look like? We're not really interested in writing code that will manipulate physical disks. Our final goal is a function that will give us a _recipe_ to solve the tower of hanoi, which is just a list of moves to make. Because of that, we can use a lot of abstraction in our implementation, and keep the data structures as simple as possible.

To start, we have to define some representation of the disks and the stacks. The disks have one important property, which is that they are ordered. We can use integers to represent them.
"""

# ╔═╡ 620d6834-a2ba-11ea-150a-2132bb54e4b3
num_disks = 8

# ╔═╡ 35ada214-a32c-11ea-0da3-d5d494b28467
md"""(Side note: the number of disks is arbitrary. When testing your function, you may want to set it to 1 or 2 to start.)"""

# ╔═╡ 7243cc8e-a2ba-11ea-3f29-99356f0cdcf4
all_disks = 1:num_disks

# ╔═╡ 7e1ba2ac-a2ba-11ea-0134-2f61ed75be18
md"""
A single stack can be represented as an array with all the disks in it. We will list them from top to bottom.
"""

# ╔═╡ 43781a52-a339-11ea-3803-e56e2d08aa83
first_stack = collect(all_disks)

# ╔═╡ b648ab70-a2ba-11ea-2dcc-55b630e44325
md"""
Now we have to make three of those.
"""

# ╔═╡ 32f26f80-a2bb-11ea-0f2a-3fc631ada63d
starting_stacks = [first_stack, [], []]

# ╔═╡ e347f1de-a2bb-11ea-06e7-87cca6f2a240
md"""
## Defining the rules

Now that we have our "game board", we can implement the rules.

To start, we make two functions for states. A state of the game is just an array of stacks.

We will define a function that checks if a state is okay according to the rules. To be legal, all the stacks should be in the correct order, so no larger disks on top of smaller disks.

Another good thing to check: no disks should have appeared or disappeared since we started!
"""

# ╔═╡ 512fa6d2-a2bd-11ea-3dbe-b935b536967b
function islegal(stacks)
	#check order
	order_correct_per_stack = map(stacks) do stack
		stack == sort(stack)
	end
	order_correct = sum(order_correct_per_stack) == length(stacks)

	#check if we use the same disk set that we started with
	disks_in_state = sort([disk for stack in stacks for disk in stack])
	disks_complete = disks_in_state == all_disks

	order_correct && disks_complete
end

# ╔═╡ c56a5858-a2bd-11ea-1d96-77eaf5e74925
md"""
Another function for states: check if we are done! We can assume that we already checked if the state was legal. So we know that all the disks are there and they are ordered correctly.  To check if we are finished, we just need to check if the last stack contains all the disks.
"""

# ╔═╡ d5cc41e8-a2bd-11ea-39c7-b7df8de6ae3e
function iscomplete(stacks)
	last(stacks) == all_disks
end

# ╔═╡ 53374f0e-a2c0-11ea-0c91-97474780721e
md"""
Now the only rules left to implement are the rules for moving disks.

We could implement this as another check on states, but it's easier to just write a legal `move` function. Your solution will specify moves for the `move` function, so this will be the only way that the stacks are actually manipulated. That we, we are sure that nothing fishy is happening.

We will make our `move` function so that its input consists of a state of the game, and instructions for what to do. Its output will be the new state of the game.

So what should those instructions look like? It may seem intuitive to give a _disk_ that should be moved, but that's more than we need. After all, we are only allowed to take the top disk from one stack, and move it to the top of another. So we only have to say which _stacks_ we are moving between.

(Note that the `move` function is okay with moving a larger disk on top of a smaller disk. We already implemented that restriction in `islegal`.)
"""

# ╔═╡ e915394e-a2c0-11ea-0cd9-1df6fd3c7adf
function move(stacks, source::Int, target::Int)
	#check if the from stack if not empty
	if length(stacks[source]) == 0
		throw(ErrorException("Error: attempted to move disk from empty stack"))
	end

	#make a copy of the stacks
	new_stacks = [stack for stack in stacks]

	disk = first(new_stacks[source]) #take disk
	new_stacks[source] = new_stacks[source][2:end] #remove from old stack
	new_stacks[target] = vcat([disk], new_stacks[target]) #put on new stack

	new_stacks
end

# ╔═╡ 87b2d164-a2c4-11ea-3095-916628943879
md"""
## Solving the problem

We have implemented the game pieces and the rules, so you can start working on your solution.

To do this, you can fill in the `solve()` function. This function should give a solution for the `starting_stacks`, by moving all the disks from stack 1 to stack 3.

As output, `solve` should give a recipe, that tells us what to do. This recipe should be an array of moves. Each moves is a `(source, target)` tuple, specifying from which stack to which stack you should move.

For example, it might look like this:
"""

# ╔═╡ 29b410cc-a329-11ea-202a-795b31ce5ad5
wrong_solution = [(1,2), (2,3), (2,1), (1,3)]

# ╔═╡ ea24e778-a32e-11ea-3f11-dbe9d36b1011
md"""
Now you can work on building an actual solution. Some tips:
* `solve()` can keep track of the board if you want, but it doesn't have to.
* The section below will actually run your moves, which is very useful for checking them.
* If you want, you can change `num_disks` to 1 or 2. That can be a good starting point.
"""

# ╔═╡ 010dbdbc-a2c5-11ea-34c3-837eae17416f
function solve() ::Array{Tuple{Int, Int}}
	[]
end

# ╔═╡ 3eb3c4c0-a2c5-11ea-0bcc-c9b52094f660
md"""
## Checking solutions

This is where we can check a solution. We start with a function that takes our recipe and runs it.
"""

# ╔═╡ 4709db36-a327-11ea-13a3-bbfb18da84ce
function run_solution(moves, start = starting_stacks)
	all_states = map(x -> start, 1:length(moves) + 1)

	for i = 1:length(moves)
		try
			source, target = moves[i]
			all_states[i + 1] = move(all_states[i],source, target)
		catch

			all_states[i + 1] = []
		end
	end

	all_states
end

# ╔═╡ 372824b4-a330-11ea-2f26-7b9a1ad018f1
md"""
You can use this function to see what your solution does.

If `run_solution` encounters an error, it will give empty arrays from that point onwards. Look at what happens in the `wrong_solution` version and compare it to the moves in `wrong_solution`.
"""

# ╔═╡ 9173b174-a327-11ea-3a69-9f7525f2e7b4
run_solution(solve())

# ╔═╡ d2227b40-a329-11ea-105c-b585d5fcf970
run_solution(wrong_solution)

# ╔═╡ bb5088ec-a330-11ea-2c41-6b8b92724b3b
md"""
Now that we have way to run a recipe, we can check if its output is correct. We will check if all the intermediate states are legal and the final state is the finished puzzle.
"""

# ╔═╡ 10fb1c56-a2c5-11ea-2a06-0d8c36bfa138
function check_solution(moves, start = starting_stacks)
	try
		#run the solution
		all_states = run_solution(moves, start)

		#check if each state is legal
		legal_moves = map(islegal, all_states)
		all_legal = sum(legal_moves) == length(all_states)

		#check if the final state is is the completed puzzle
		complete = (iscomplete ∘ last)(all_states)

		all_legal && complete
	catch
		#return false if we encountered an error
		return false
	end
end

# ╔═╡ 8ea7f944-a329-11ea-22cc-4dbd11ec0610
check_solution(solve())

# ╔═╡ e54add0a-a330-11ea-2eeb-1d42f552ba38
if check_solution(solve())
	if num_disks >= 8
		md"""
		Congratulations, your solution works!
		"""
	else
		md"""
		Your solution works for $(num_disks) disks. Change `num_disks` to see if it works for 8 or more.
		"""
	end
else
	md"""
	The `solve()` function doesn't work yet. Keep working on it!
	"""
end

# ╔═╡ Cell order:
# ╟─5b2ee40e-a2b8-11ea-0fef-c35fe6918860
# ╟─95fbd0d2-a2b9-11ea-0682-fdf783251797
# ╠═620d6834-a2ba-11ea-150a-2132bb54e4b3
# ╟─35ada214-a32c-11ea-0da3-d5d494b28467
# ╠═7243cc8e-a2ba-11ea-3f29-99356f0cdcf4
# ╟─7e1ba2ac-a2ba-11ea-0134-2f61ed75be18
# ╠═43781a52-a339-11ea-3803-e56e2d08aa83
# ╟─b648ab70-a2ba-11ea-2dcc-55b630e44325
# ╠═32f26f80-a2bb-11ea-0f2a-3fc631ada63d
# ╟─e347f1de-a2bb-11ea-06e7-87cca6f2a240
# ╠═512fa6d2-a2bd-11ea-3dbe-b935b536967b
# ╟─c56a5858-a2bd-11ea-1d96-77eaf5e74925
# ╠═d5cc41e8-a2bd-11ea-39c7-b7df8de6ae3e
# ╟─53374f0e-a2c0-11ea-0c91-97474780721e
# ╠═e915394e-a2c0-11ea-0cd9-1df6fd3c7adf
# ╟─87b2d164-a2c4-11ea-3095-916628943879
# ╠═29b410cc-a329-11ea-202a-795b31ce5ad5
# ╟─ea24e778-a32e-11ea-3f11-dbe9d36b1011
# ╠═010dbdbc-a2c5-11ea-34c3-837eae17416f
# ╟─3eb3c4c0-a2c5-11ea-0bcc-c9b52094f660
# ╠═4709db36-a327-11ea-13a3-bbfb18da84ce
# ╟─372824b4-a330-11ea-2f26-7b9a1ad018f1
# ╠═9173b174-a327-11ea-3a69-9f7525f2e7b4
# ╠═d2227b40-a329-11ea-105c-b585d5fcf970
# ╟─bb5088ec-a330-11ea-2c41-6b8b92724b3b
# ╠═10fb1c56-a2c5-11ea-2a06-0d8c36bfa138
# ╠═8ea7f944-a329-11ea-22cc-4dbd11ec0610
# ╟─e54add0a-a330-11ea-2eeb-1d42f552ba38


@testset "Elastic net ($T)" for T in [Float32, Float64, ComplexF32, ComplexF64]
    using ProximalOperators
    using ProximalAlgorithms
    using LinearAlgebra

    A = T[
        1.0 -2.0 3.0 -4.0 5.0
        2.0 -1.0 0.0 -1.0 3.0
        -1.0 0.0 4.0 -3.0 2.0
        -1.0 -1.0 -1.0 1.0 3.0
    ]
    b = T[1.0, 2.0, 3.0, 4.0]

    m, n = size(A)

    R = real(T)

    reg = ElasticNet(R(1), R(1))
    reg1 = NormL1(R(1))
    reg2 = SqrNormL2(R(1))
    loss = Translate(SqrNormL2(R(1)), -b)
    cost = LeastSquares(A, b)

    L = opnorm(A)^2

    x_star = T[-0.6004983388704322, 0.0, 0.0, 0.195182724252491, 0.764119601328903]

    @testset "DavisYin" begin

        # with known initial iterate

        x0 = zeros(T, n)
        x0_backup = copy(x0)
        solver = ProximalAlgorithms.DavisYin(tol = R(1e-6))
        xf_dys, xg_dys, it_dys = solver(x0, f = reg1, g = reg2, h = cost, Lh = L)
        @test eltype(xf_dys) == T
        @test eltype(xg_dys) == T
        @test norm(xf_dys - x_star, Inf) <= 1e-3
        @test norm(xg_dys - x_star, Inf) <= 1e-3
        @test it_dys <= 140
        @test x0 == x0_backup

        # with random initial iterate

        x0 = randn(T, n)
        x0_backup = copy(x0)
        solver = ProximalAlgorithms.DavisYin(tol = R(1e-6))
        xf_dys, xg_dys, it_dys = solver(x0, f = reg1, g = reg2, h = cost, Lh = L)
        @test eltype(xf_dys) == T
        @test eltype(xg_dys) == T
        @test norm(xf_dys - x_star, Inf) <= 1e-3
        @test norm(xg_dys - x_star, Inf) <= 1e-3
        @test x0 == x0_backup

    end

    afba_test_params = [
        (R(2), R(0), 130),
        (R(1), R(1), 1890),
        (R(0), R(1), 320),
        (R(0), R(0), 194),
        (R(1), R(0), 130),
    ]

    @testset "AFBA" for (theta, mu, maxit) in afba_test_params

        # with known initial iterates

        x0 = zeros(T, n)
        x0_backup = copy(x0)
        y0 = zeros(T, m)
        y0_backup = copy(y0)

        solver = ProximalAlgorithms.AFBA(theta = theta, mu = mu, tol = R(1e-6))
        x_afba, y_afba, it_afba =
            solver(x0, y0, f = reg2, g = reg1, h = loss, L = A, beta_f = R(1))
        @test eltype(x_afba) == T
        @test eltype(y_afba) == T
        @test norm(x_afba - x_star, Inf) <= 1e-4
        @test it_afba <= maxit
        @test x0 == x0_backup
        @test y0 == y0_backup

        # with random initial iterates

        x0 = randn(T, n)
        x0_backup = copy(x0)
        y0 = randn(T, m)
        y0_backup = copy(y0)

        solver = ProximalAlgorithms.AFBA(theta = theta, mu = mu, tol = R(1e-6))
        x_afba, y_afba, it_afba =
            solver(x0, y0, f = reg2, g = reg1, h = loss, L = A, beta_f = R(1))
        @test eltype(x_afba) == T
        @test eltype(y_afba) == T
        @test norm(x_afba - x_star, Inf) <= 1e-4
        @test x0 == x0_backup
        @test y0 == y0_backup

    end
end


@testset "linear system fancy" begin
# Only on one processor...

# Initialize PETSc. Command line arguments passed to Julia are parsed by PETSc. Alternatively, you can
# also provide "command line arguments by defining them in a string, for instance
# `PetscInitialize("-ksp_monitor_short -ksp_gmres_cgs_refinement_type refine_always")` or by providing each argument in
# separate strings : `PetscInitialize(["-ksp_monitor_short", "-ksp_gmres_cgs_refinement_type", "refine_always")`
PetscInitialize()

# Number of mesh points and mesh step
n = 11
Δx = 1. / (n - 1)

# Create a matrix of size `(n,n)` and a vector of size `(n)`
A = create_matrix(n, n)
b = create_vector(n)

# We can then use command line options to set our matrix/vectors.
set_from_options!(A)
set_from_options!(b)

# Finish the set up
set_up!(A)
set_up!(b)

# Let's build the right hand side vector. We first get the range of rows of `b` handled by the local processor.
# The `rstart, rend = get_range(*)` methods differ a little bit from PETSc API since the two integers it
# returns are the effective Julia range of rows handled by the local processor. If `n` is the total
# number of rows, then `rstart ∈ [1,n]` and `rend` is the last row handled by the local processor.
b_start, b_end = get_range(b)

# Now let's build the right hand side vector. Their are various ways to do this, this is just one.
n_loc = length(b) ## Note that n_loc = b_end - b_start + 1...
b[b_start:b_end] = 2 * ones(n_loc)

# And here is the differentiation matrix. Rembember that PETSc.MatSetValues simply ignores negatives rows indices.
A_start, A_end = get_range(A)
for i in A_start:A_end
    A[i, i-1:i] = [-1. 1.] / Δx
end

# Set boundary condition (only the proc handling index `1` is acting)
(b_start == 1) && (b[1] = 0.)

# Assemble matrice and vector
assemble!(A)
assemble!(b)

# Set up the linear solver
ksp = create_ksp(A)
set_from_options!(ksp)
set_up!(ksp)

# Solve the system
x = solve(ksp, b)

# Print the solution
@show x

# Convert to Julia array
array = vec2array(x)
@test isapprox(array, range(0., 2.; length = n))

# Free memory
destroy!(A)
destroy!(b)
destroy!(x)

# Note that it's also possible to build a matrix using the COO format as in `SparseArrays`:
M = create_matrix(3,3; auto_setup = true)
M_start, M_end = get_range(M)
I = [1, 1, 1, 2, 3]
J = [1, 3, 1, 3, 2]
V = [1, 2, 3, 4, 5]
k = findall(x -> M_start <= x <= M_end, I) # just a stupid trick to allow this example to run in parallel
set_values!(M, I[k], J[k], V[k], ADD_VALUES)
assemble!(M)
@show M
destroy!(M)
# This is very convenient in sequential since you can fill the three vectors I, J, V in your code and decide only
# at the last moment if you'd like to use `SparseArrays` or `PetscMat`.

# Finalize Petsc
PetscFinalize()

# Reach this point?
@test true

end


using Base.Test
using Cubature
include("../model/AccelerationControlledDifferentialDrive.jl")

let state = initial_state(0, 0, 0, 0, 0)
    control = [10, 10]
    elapsed_time = 2
    (x0, y0, theta0, wl0, wr0) = state
    (al, ar) = control
    r = 0.033
    L = 0.13
    ε_integration = 0.000001
    expected_position_x = x0 + hquadrature(t -> r/2 * (wl0 + al*t + wr0 + ar*t) * cos(theta0 + r/L*(wr0-wl0)*t + r/L*(ar-al)*t^2/2), 0, elapsed_time; abstol=ε_integration)[1] # TODO cubature doesn't seem to care about abstol
    expected_position_y = y0 + hquadrature(t -> r/2 * (wl0 + al*t + wr0 + ar*t) * sin(theta0 + r/L*(wr0-wl0)*t + r/L*(ar-al)*t^2/2), 0, elapsed_time; abstol=ε_integration)[1]
    expected_orientation = theta0 + r/L*(wr0-wl0)*elapsed_time + r/L*(ar-al)*elapsed_time^2/2
    ε_position = 0.001
	ε_orientation = 0.01

    state = new_state(state, control, elapsed_time)

    println()
	println("Constant acceleration linear move results")
    println("State:      ", state)
	println("Expected:   ", [expected_position_x expected_position_y expected_orientation])
	println("Error[mm]:  ", sqrt((expected_position_x - state[1])^2 + (expected_position_y - state[2])^2) * 1000)
	@test state[1] ≈ expected_position_x atol=ε_position
	@test state[2] ≈ expected_position_y atol=ε_position
	@test state[3] ≈ expected_orientation atol=ε_orientation
    println("Constant acceleration linear move pass")
end

let state = initial_state(1, 1.5, π/2, 5, 5)
    control = [-5, 0]
    elapsed_time = 0.1
    (x0, y0, theta0, wl0, wr0) = state
    (al, ar) = control
    r = 0.033
    L = 0.13
    ε_integration = 0.000001
    expected_position_x = x0 + hquadrature(t -> r/2 * (wl0 + al*t + wr0 + ar*t) * cos(theta0 + r/L*(wr0-wl0)*t + r/L*(ar-al)*t^2/2), 0, elapsed_time; abstol=ε_integration)[1]
    expected_position_y = y0 + hquadrature(t -> r/2 * (wl0 + al*t + wr0 + ar*t) * sin(theta0 + r/L*(wr0-wl0)*t + r/L*(ar-al)*t^2/2), 0, elapsed_time; abstol=ε_integration)[1]
    expected_orientation = theta0 + r/L*(wr0-wl0)*elapsed_time + r/L*(ar-al)*elapsed_time^2/2
    ε_position = 0.001
	ε_orientation = 0.01

    state = new_state(state, control, elapsed_time)

    println()
	println("Turn left from constant linear move results")
    println("State:      ", state)
	println("Expected:   ", [expected_position_x expected_position_y expected_orientation])
	println("Error[mm]:  ", sqrt((expected_position_x - state[1])^2 + (expected_position_y - state[2])^2) * 1000)
	@test state[1] ≈ expected_position_x atol=ε_position
	@test state[2] ≈ expected_position_y atol=ε_position
	@test state[3] ≈ expected_orientation atol=ε_orientation
    println("Turn left from constant linear move pass")
end

let state = initial_state(1, 1.5, π/2, 5, 5)
    control = [-5, 0]
    elapsed_time = 1.0
    cycle_time = 0.1
    (x0, y0, theta0, wl0, wr0) = state
    (al, ar) = control
    r = 0.033
    L = 0.13
    ε_integration = 0.000001
    expected_position_x = x0 + hquadrature(t -> r/2 * (wl0 + al*t + wr0 + ar*t) * cos(theta0 + r/L*(wr0-wl0)*t + r/L*(ar-al)*t^2/2), 0, elapsed_time; abstol=ε_integration)[1]
    expected_position_y = y0 + hquadrature(t -> r/2 * (wl0 + al*t + wr0 + ar*t) * sin(theta0 + r/L*(wr0-wl0)*t + r/L*(ar-al)*t^2/2), 0, elapsed_time; abstol=ε_integration)[1]
    expected_orientation = theta0 + r/L*(wr0-wl0)*elapsed_time + r/L*(ar-al)*elapsed_time^2/2
    ε_position = 0.001
	ε_orientation = 0.01

    for i in 1:10
        state = new_state(state, control, cycle_time)
    end

    println()
	println("Turn left from constant linear move multiple update results")
    println("State:      ", state)
	println("Expected:   ", [expected_position_x expected_position_y expected_orientation])
	println("Error[mm]:  ", sqrt((expected_position_x - state[1])^2 + (expected_position_y - state[2])^2) * 1000)
	@test state[1] ≈ expected_position_x atol=ε_position
	@test state[2] ≈ expected_position_y atol=ε_position
	@test state[3] ≈ expected_orientation atol=ε_orientation
    println("Turn left from constant linear move multiple update pass")
end

let (x0, y0, theta0, wl0, wr0) = initial_state(0, 0, 0, 0, 0)
    (al, ar) = [10, 10]
    elapsed_time = 2
    r = 0.033
    L = 0.13
    expected_x = 0.66
    expected_y = 0
    expected_theta = 0
    ε_integration = 0.000001
    ε_position = 0.001
	ε_orientation = 0.01

    calculated_theta = theta0 + r/L*(wr0-wl0)*elapsed_time + r/L*(ar-al)*elapsed_time^2/2
    calculated_x = x0 + hquadrature(t -> r/2 * (wl0 + al*t + wr0 + ar*t) * cos(theta0 + r/L*(wr0-wl0)*t + r/L*(ar-al)*t^2/2), 0, elapsed_time; abstol=ε_integration)[1]
    calculated_y = y0 + hquadrature(t -> r/2 * (wl0 + al*t + wr0 + ar*t) * sin(theta0 + r/L*(wr0-wl0)*t + r/L*(ar-al)*t^2/2), 0, elapsed_time; abstol=ε_integration)[1]

    println()
    println("Cubature test suit test constant acceleration linear move result")
    println("Calculated: ", [calculated_x calculated_y calculated_theta])
	println("Expected:   ", [expected_x expected_y expected_theta])
	println("Error[mm]:  ", sqrt((expected_x - calculated_x)^2 + (expected_y - calculated_y)^2) * 1000)
    @test calculated_x ≈ expected_x atol = ε_position
	@test calculated_y ≈ expected_y atol = ε_position
	@test calculated_theta ≈ expected_theta atol = ε_orientation
    println("Cubature test suit test constant acceleration linear move pass")
end


### Training utilities

export training!

function training!(
	T::InvertibleNetwork,
	fval::Array{Float32, 1},
	loss::Function, Ztrain::Array{Float32, 4},
	nepochs::Int64, batchsize::Int64,
	opt::Optimiser;
	verbose::Bool = true, verbose_b::Bool = false,
	randinput::Bool = false,
	gradclip::Bool = false)

	trainsize = size(Ztrain, 4)
	nbatches = Int64(round(trainsize/batchsize))

	# Loop over iterations
	for n = 1:nepochs

	    # Shuffling data
	    idx_train_perm = randperm(trainsize)

	    # Loop over batches
	    for idx_batch = 1:nbatches

			# Selecting batch
	        idx = idx_train_perm[(idx_batch-1)*batchsize+1:idx_batch*batchsize]
	        Z = Ztrain[:, :, :, idx]
			randinput && (Z += 1f-2*randn(Float32, size(Z)))

	        # Evaluate objective and backpropagating
	        l = loss(Z)
	        append!(fval, l)

	        # Update network
			for p in get_params(T)
				gradclip && (p.grad ./= (norm(p.grad)+1f-10))
				update!(opt, p.data, p.grad)
			end

	        # Clear gradients
	        clear_grad!(T)

		    # Print msg
		    verbose_b && println("         Batch: [", idx_batch, "/", nbatches, "] --- fval = ", fval[end])

	    end

	    # Print msg
	    verbose && println("Epoch: [", n, "/", nepochs, "] --- fval = ", fval[end])

	end

	return fval

end


module Test_klfapps

using Random
using LinearAlgebra
using MatrixPencils
using Test

@testset "Matrix Pencils Applications" begin

Random.seed!(2351);

@testset "Matrix Pencils Structure Applications" begin

fast = true
for fast in (false,true)

M = zeros(0,0); N = zeros(0,0);
@time info  = pkstruct(M, N, fast = fast)
@test info.rki == [] && info.lki == [] && info.id == [] && info.nf == 0 && info.nrank == 0

M = zeros(3,0); N = zeros(3,0);
@time info = pkstruct(M, N, fast = fast)
@test info.rki == [] && info.lki == [0, 0, 0] && info.id == [] && info.nf == 0 && info.nrank == 0

M = zeros(0,3); N = zeros(0,3);
@time info = pkstruct(M, N, fast = fast)
@test info.rki == [0, 0, 0] && info.lki == [] && info.id == [] && info.nf == 0 && info.nrank == 0

M = zeros(1,1); N = ones(1,1);
@time info = pkstruct(M, N, fast = fast)
@test info.rki == [] && info.lki == [] && info.id == [] && info.nf == 1 && info.nrank == 1

M = ones(1,1); N = zeros(1,1);
@time info = pkstruct(M, N, fast = fast)
@test info.rki == [] && info.lki == [] && info.id == [1] && info.nf == 0 && info.nrank == 1

M = ones(1,1); N = nothing;
@time info = pkstruct(M, N, fast = fast)
@test info.rki == [] && info.lki == [] && info.id == [] && info.nf == 0 && info.nrank == 1

M = [1 0;0 1]; N = [0 1; 0 0];
@time info = pkstruct(M, N)
@test info.rki == [] && info.lki == [] && info.id == [2] && info.nf == 0 && info.nrank == 2

M = [  22  34  31   31  17
        45  45  42   19  29
        39  47  49   26  34
        27  31  26   21  15
        38  44  44   24  30 ];

N = [   13  26  25  17  24
        31  46  40  26  37
        26  40  19  25  25
        16  25  27  14  23
        24  35  18  21  22  ];

@time info = pkstruct(M, N)
rki, lki, id, nf, nrank = info
@test rki == [0] && lki == [0] && id == [1] && nf == 3 && nrank == 4

# Test Suite 2 (Example 2.2.1, Beelen)
M = zeros(14,16);
i1 = [1 2 3 6 7 8 9 10 11 12 13 13 14];
j1 = [4 6 7 8 9 10 11 12 13 14 15 16 16];
aval = [1 1 1 1 1 1 1 1 1 2 3 1 3];
for k = 1:length(i1)
    M[i1[k],j1[k]] = aval[k]
end
N = zeros(14,16);
i1 = [1 2 3 5 6 7 10 12 13 14]; j1 = [3 5 6 8 9 10 13 14 15 16];
for k = 1:length(i1)
    N[i1[k],j1[k]] = 1
end

Q = qr(rand(14,14)).Q; Z = qr(rand(16,16)).Q;
M2 = Q*M*Z; N2 = Q*N*Z;

M = copy(M2); N = copy(N2);
@time info = pkstruct(M, N, fast = true, atol1 = 1.e-7, atol2 = 1.e-7)
@test info.rki == [0, 0, 1, 2] && info.lki == [0, 3] && info.id == [1,2] && info.nf == 3 && info.nrank == 12

#for Ty in (Float64, Float32, Complex{Float64},  Complex{Float32})
Ty = Float64
Ty = Complex{Float64}
for Ty in (Float64, Complex{Float64})

#Ty = Complex{Float64}
abstol = sqrt(eps(one(real(Ty))))

# given structure
mr = 2; nr = 3; ni = 4; nf = 10; ml = 5; nl = 4;
mM = mr+ni+nf+ml;
nM = nr+ni+nf+nl;
M = [rand(Ty,mr,nM);
     zeros(Ty,ni,nr) Matrix{Ty}(I,ni,ni) rand(Ty,ni,nf+nl);
     zeros(Ty,nf,nr+ni) rand(Ty,nf,nf+nl);
     zeros(ml,nr+ni+nf) rand(Ty,ml,nl)]
N = [rand(Ty,mr,nM);
     zeros(Ty,ni,nr) diagm(1 => ones(Ty,ni-1)) rand(Ty,ni,nf+nl);
     zeros(Ty,nf,nr+ni) rand(Ty,nf,nf+nl);
     zeros(ml,nr+ni+nf) rand(Ty,ml,nl)]
Q = qr(rand(Ty,mM,mM)).Q;
Z = qr(rand(Ty,nM,nM)).Q;
M = Q*M*Z;
N = Q*N*Z;

atol1 = 1.e-7; atol2 = 1.e-7;
@time info = pkstruct(M, N, fast = fast, atol1 = atol1, atol2 = atol2)
@test info.rki == [mr] && info.lki == [nl] && info.id == [ni] && info.nf == nf && info.nrank == mr+nl+ni+nf

end
end
end

Random.seed!(2351);

@testset "Matrix Pencils Eigenvalues Applications" begin

fast = true
for fast in (false,true)

M = zeros(0,0); N = zeros(0,0);
@time val, kinfo  = peigvals(M, N, fast = fast)
@test val == [] && (kinfo.rki,kinfo.lki,kinfo.id,kinfo.nf,kinfo.nrank) == (Int64[], Int64[], Int64[], 0, 0)

M = zeros(3,0); N = zeros(3,0);
@time val, kinfo = peigvals(M, N, fast = fast)
@test val == [] && (kinfo.rki,kinfo.lki,kinfo.id,kinfo.nf,kinfo.nrank) == (Int64[], [0, 0, 0], Int64[], 0, 0)

M = zeros(0,3); N = zeros(0,3);
@time val, kinfo = peigvals(M, N, fast = fast)
@test val == [] && (kinfo.rki,kinfo.lki,kinfo.id,kinfo.nf,kinfo.nrank) == ([0, 0, 0], Int64[], Int64[], 0, 0)

M = zeros(1,1); N = ones(1,1);
@time val, kinfo = peigvals(M, N, fast = fast)
@test val == [0] && (kinfo.rki,kinfo.lki,kinfo.id,kinfo.nf,kinfo.nrank) == (Int64[], Int64[], Int64[], 1, 1)

N = zeros(1,1); M = ones(1,1);
@time val, kinfo = peigvals(M, N, fast = fast)
@test val == [Inf] && (kinfo.rki,kinfo.lki,kinfo.id,kinfo.nf,kinfo.nrank) == (Int64[], Int64[], [1], 0, 1)

M = ones(1,1); N = nothing;
@time val, info = peigvals(M, N, fast = fast)
@test val == Float64[] && info.rki == [] && info.lki == [] && info.id == [] && info.nf == 0 && info.nrank == 1

M = [1 0;0 1]; N = [0 1; 0 0];

@time val, kinfo = peigvals(M, N)
@test val == [Inf;Inf] && (kinfo.rki,kinfo.lki,kinfo.id,kinfo.nf,kinfo.nrank) == (Int64[], Int64[], [2], 0, 2)

M = [  22  34  31   31  17
        45  45  42   19  29
        39  47  49   26  34
        27  31  26   21  15
        38  44  44   24  30 ];

N = [   13  26  25  17  24
        31  46  40  26  37
        26  40  19  25  25
        16  25  27  14  23
        24  35  18  21  22  ];

@time val, kinfo = peigvals(M, N)
@test length(val) == 4 &&
      length(filter(y-> y == true,isinf.(val))) == 1 &&
      length(filter(y-> y == true,isfinite.(val))) == 3 &&
      (kinfo.rki,kinfo.lki,kinfo.id,kinfo.nf,kinfo.nrank) == ([0], [0], [1], 3, 4)

# Test Suite 2 (Example 2.2.1, Beelen)
M = zeros(14,16);
i1 = [1 2 3 6 7 8 9 10 11 12 13 13 14];
j1 = [4 6 7 8 9 10 11 12 13 14 15 16 16];
aval = [1 1 1 1 1 1 1 1 1 2 3 1 3];
for k = 1:length(i1)
    M[i1[k],j1[k]] = aval[k]
end
N = zeros(14,16);
i1 = [1 2 3 5 6 7 10 12 13 14]; j1 = [3 5 6 8 9 10 13 14 15 16];
for k = 1:length(i1)
    N[i1[k],j1[k]] = 1
end

Q = qr(rand(14,14)).Q; Z = qr(rand(16,16)).Q;
M2 = Q*M*Z; N2 = Q*N*Z;

M = copy(M2); N = copy(N2);
@time val, kinfo = peigvals(M, N, fast = true, atol1 = 1.e-7, atol2 = 1.e-7)
@test length(val) == 6 &&
      length(filter(y-> y == true,isinf.(val))) == 3 &&
      length(filter(y-> y == true,isfinite.(val))) == 3 &&
      (kinfo.rki,kinfo.lki,kinfo.id,kinfo.nf,kinfo.nrank) == ([0, 0, 1, 2], [0, 3], [1, 2], 3, 12)

#for Ty in (Float64, Float32, Complex{Float64},  Complex{Float32})
#Ty = Float64; fast = true;
for Ty in (Float64, Complex{Float64})

# given structure
mr = 2; nr = 3; ni = 4; nf = 10; ml = 5; nl = 4;
mM = mr+ni+nf+ml;
nM = nr+ni+nf+nl;
M = [rand(Ty,mr,nM);
     zeros(Ty,ni,nr) Matrix{Ty}(I,ni,ni) rand(Ty,ni,nf+nl);
     zeros(Ty,nf,nr+ni) rand(Ty,nf,nf+nl);
     zeros(ml,nr+ni+nf) rand(Ty,ml,nl)]
N = [rand(Ty,mr,nM);
     zeros(Ty,ni,nr) diagm(1 => ones(Ty,ni-1)) rand(Ty,ni,nf+nl);
     zeros(Ty,nf,nr+ni) rand(Ty,nf,nf+nl);
     zeros(ml,nr+ni+nf) rand(Ty,ml,nl)]
Q = qr(rand(Ty,mM,mM)).Q;
Z = qr(rand(Ty,nM,nM)).Q;
M = Q*M*Z;
N = Q*N*Z;

@time val, kinfo = peigvals(M, N, fast = fast, atol1 = 1.e-5, atol2 = 1.e-7)
@test length(val) == ni+nf &&
      length(filter(y-> y == true,isinf.(val))) == ni &&
      length(filter(y-> y == true,isfinite.(val))) == nf &&
      (kinfo.rki,kinfo.lki,kinfo.id,kinfo.nf,kinfo.nrank) == ([mr], [nl], [ni], nf, mr+nl+ni+nf)

end
end
end

Random.seed!(2151);

@testset "Matrix Pencils Zeros Applications" begin

fast = true
for fast in (false,true)

M = zeros(0,0); N = zeros(0,0);
@time val, iz, kinfo  = pzeros(M, N, fast = fast)
@test val == [] && iz == [] && (kinfo.rki,kinfo.lki,kinfo.id,kinfo.nf,kinfo.nrank) == (Int64[], Int64[], Int64[], 0, 0)

M = zeros(3,0); N = zeros(3,0);
@time val, iz, kinfo  = pzeros(M, N, fast = fast)
@test val == [] && iz == [] && (kinfo.rki,kinfo.lki,kinfo.id,kinfo.nf,kinfo.nrank) == (Int64[], [0, 0, 0], Int64[], 0, 0)

M = zeros(0,3); N = zeros(0,3);
@time val, iz, kinfo  = pzeros(M, N, fast = fast)
@test val == [] && iz == [] && (kinfo.rki,kinfo.lki,kinfo.id,kinfo.nf,kinfo.nrank) == ([0, 0, 0], Int64[], Int64[], 0, 0)

M = zeros(1,1); N = ones(1,1);
@time val, iz, kinfo  = pzeros(M, N, fast = fast)
@test val == [0] && iz == [] && (kinfo.rki,kinfo.lki,kinfo.id,kinfo.nf,kinfo.nrank) == (Int64[], Int64[], Int64[], 1, 1)

N = zeros(1,1); M = ones(1,1);
@time val, iz, kinfo  = pzeros(M, N, fast = fast)
@test val == [] && iz == [] && (kinfo.rki,kinfo.lki,kinfo.id,kinfo.nf,kinfo.nrank) == (Int64[], Int64[], [1], 0, 1)

M = ones(1,1); N = nothing;
@time val, iz, info = pzeros(M, N, fast = fast)
@test val == Float64[] && iz == [] && info.rki == [] && info.lki == [] && info.id == [] && info.nf == 0  && info.nrank == 1

M = [1 0;0 1]; N = [0 1; 0 0];
@time val, iz, kinfo  = pzeros(M, N, fast = fast)
@test val == [Inf] && iz == [1] && (kinfo.rki,kinfo.lki,kinfo.id,kinfo.nf,kinfo.nrank) == (Int64[], Int64[], [2], 0, 2)

M = [  22  34  31   31  17
        45  45  42   19  29
        39  47  49   26  34
        27  31  26   21  15
        38  44  44   24  30 ];

N = [   13  26  25  17  24
        31  46  40  26  37
        26  40  19  25  25
        16  25  27  14  23
        24  35  18  21  22  ];

@time val, iz, kinfo   = pzeros(M, N, fast = fast)
@test length(val) == 3 &&
      length(filter(y-> y == true,isinf.(val))) == 0 &&
      length(filter(y-> y == true,isfinite.(val))) == 3 &&
      iz == [] && (kinfo.rki,kinfo.lki,kinfo.id,kinfo.nf,kinfo.nrank) == ([0], [0], [1], 3, 4)

# Test Suite 2 (Example 2.2.1, Beelen)
M = zeros(14,16);
i1 = [1 2 3 6 7 8 9 10 11 12 13 13 14];
j1 = [4 6 7 8 9 10 11 12 13 14 15 16 16];
aval = [1 1 1 1 1 1 1 1 1 2 3 1 3];
for k = 1:length(i1)
    M[i1[k],j1[k]] = aval[k]
end
N = zeros(14,16);
i1 = [1 2 3 5 6 7 10 12 13 14]; j1 = [3 5 6 8 9 10 13 14 15 16];
for k = 1:length(i1)
    N[i1[k],j1[k]] = 1
end

Q = qr(rand(14,14)).Q; Z = qr(rand(16,16)).Q;
M2 = Q*M*Z; N2 = Q*N*Z;

M = copy(M2); N = copy(N2);
@time val, iz, kinfo   = pzeros(M, N, fast = fast, atol1 = 1.e-7, atol2 = 1.e-7)
@test length(val) == 4 &&
      length(filter(y-> y == true,isinf.(val))) == 1 &&
      length(filter(y-> y == true,isfinite.(val))) == 3 &&
      iz == [1] && (kinfo.rki,kinfo.lki,kinfo.id,kinfo.nf,kinfo.nrank) == ([0, 0, 1, 2], [0, 3], [1, 2], 3, 12)

#Ty = Float64; fast = true;
for Ty in (Float64, Complex{Float64})

abstol = sqrt(eps(one(real(Ty))))

# given structure
mr = 2; nr = mr+1; ni = 4; nf = 8; nl = 4; ml = nl+1;
mM = mr+ni+nf+ml;
nM = nr+ni+nf+nl;
M2 = [rand(Ty,mr,nM);
     zeros(Ty,ni,nr) Matrix{Ty}(I,ni,ni) rand(Ty,ni,nf+nl);
     zeros(Ty,nf,nr+ni) rand(Ty,nf,nf+nl);
     zeros(ml,nr+ni+nf) rand(Ty,ml,nl)]
N2 = [rand(Ty,mr,nM);
     zeros(Ty,ni,nr) diagm(1 => ones(Ty,ni-1)) rand(Ty,ni,nf+nl);
     zeros(Ty,nf,nr+ni) rand(Ty,nf,nf+nl);
     zeros(ml,nr+ni+nf) rand(Ty,ml,nl)]
Q = qr(rand(Ty,mM,mM)).Q;
Z = qr(rand(Ty,nM,nM)).Q;
M2 = Q*M2*Z;
N2 = Q*N2*Z;

M = copy(M2); N = copy(N2);
atol1 = 1.e-7; atol2 = 1.e-7;
@time val, iz, kinfo  = pzeros(M, N, fast = fast, atol1 = atol1, atol2 = atol2)
@test length(val) == ni+nf-1 &&
      length(val[isinf.(val)]) == ni-1 &&
      length(val[isfinite.(val)]) == nf && iz == (ni > 1 ? [ni-1] : [ ]) &&
      (kinfo.rki,kinfo.lki,kinfo.id,kinfo.nf,kinfo.nrank) == ([mr], [nl], ni > 0 ? [ni] : [], nf, mr+nl+ni+nf)

end
end
end # pzeros

@testset "Matrix Pencils Rank Applications" begin

fast = true
for fast in (false,true)

M = zeros(0,0); N = zeros(0,0);
@test prank(M, N, fastrank = fast) == 0

M = zeros(3,0); N = zeros(3,0);
@test prank(M, N, fastrank = fast) == 0

M = zeros(0,3); N = zeros(0,3);
@test prank(M, N, fastrank = fast) == 0

M = zeros(1,1); N = ones(1,1);
@test prank(M, N, fastrank = fast) == 1

@test prank(N, M, fastrank = fast) == 1

M = ones(1,1); N = nothing;
@test prank(M, N, fastrank = fast) == 1

M = [  22  34  31   31  17
        45  45  42   19  29
        39  47  49   26  34
        27  31  26   21  15
        38  44  44   24  30 ];

N = [   13  26  25  17  24
        31  46  40  26  37
        26  40  19  25  25
        16  25  27  14  23
        24  35  18  21  22  ];
@test prank(M, N, fastrank = fast) == 4

# Test Suite 2 (Example 2.2.1, Beelen)
M = zeros(14,16);
i1 = [1 2 3 6 7 8 9 10 11 12 13 13 14];
j1 = [4 6 7 8 9 10 11 12 13 14 15 16 16];
aval = [1 1 1 1 1 1 1 1 1 2 3 1 3];
for k = 1:length(i1)
    M[i1[k],j1[k]] = aval[k]
end
N = zeros(14,16);
i1 = [1 2 3 5 6 7 10 12 13 14]; j1 = [3 5 6 8 9 10 13 14 15 16];
for k = 1:length(i1)
    N[i1[k],j1[k]] = 1
end

Q = qr(rand(14,14)).Q; Z = qr(rand(16,16)).Q;
M2 = Q*M*Z; N2 = Q*N*Z;

M = copy(M2); N = copy(N2);

@test prank(M, N, fastrank = fast) == 12

Ty = Float64
Ty = Complex{Float64}
#for Ty in (Float64, Float32, Complex{Float64},  Complex{Float32})
for Ty in (Float64, Complex{Float64})

abstol = sqrt(eps(one(real(Ty))))

# full row rank
r = 5; n = 20;
M = rand(Ty,r,n)
N = rand(Ty,r,n)

@time prnk = prank(M, N, fastrank = fast)
@test prnk == r

# full column rank
r = 5; m = 20;
M = rand(Ty,m,r)
N = rand(Ty,m,r)

@time prnk = prank(M, N, fastrank = fast)
@test prnk == r

# given rank
r = 5; m = 20; n = 30;
Q = rand(Ty,m,m);
Z = rand(Ty,n,n);
M = Q*[ rand(Ty,r,r) zeros(Ty,r,n-r); zeros(Ty,m-r,n) ]*Z;
N = Q*[ rand(Ty,r,r) zeros(Ty,r,n-r); zeros(Ty,m-r,n) ]*Z;

@time prnk = prank(M, N, atol1 = 100*eps(opnorm(M,1)), atol2 = 100*eps(opnorm(N,1)), fastrank = fast)
@test prnk == r

end
end
end  # prank

end  # applications testset

end  # module


using Random

include("../src/sorting.jl")

function testsort(sortfunc, testpairs)
    for (arr, ans) in testpairs
        temparr = copy(arr)
        sortfunc(temparr)
        @test temparr == ans
    end
end

function testrandom(sortfunc, seednum)
        Random.seed!(seednum)
        testdata = randn(100)

        d1 = copy(testdata)
        sort!(d1)
        d2 = copy(testdata)
        sortfunc(d2)

        @test d1 == d2
end

@testset "Sort" begin

    @testset "Basic cases" begin
        testpairs = [
                ([], []),
                ([3, 1, 2], [1, 2, 3]),
                ([4, 2, 3, 1], [1, 2, 3, 4]),
                ([4, 3, 3, 1, -1], [-1, 1, 3, 3, 4]),
                ([1.28, -2.591, -2.2266], [-2.591, -2.2266, 1.28]),
                ([1], [1]),
                (['b', 'c', 'a'], ['a', 'b', 'c']),
                (['E', 'A', 'S', 'Y', 'Q', 'U',
                  'E', 'S', 'T', 'I', 'O', 'N'],
                ['A', 'E', 'E', 'I', 'N', 'O',
                 'Q', 'S', 'S', 'T', 'U', 'Y'])
            ]

        testsort(insertsort!, testpairs)
        testsort(mergesort!, testpairs)
        testsort(quicksort!, testpairs)
    end

    @testset "Compare with Base.sort" begin
        for seed = [20190128, 20200828, 20211228]
            testrandom(insertsort!, seed)
            testrandom(mergesort!, seed)
            testrandom(quicksort!, seed)
        end
    end

end


# ---
# title: 315. Count of Smaller Numbers After Self
# id: problem315
# author: zhwang
# date: 2022-03-26
# difficulty: Hard
# categories: Binary Search, Divide and Conquer, Sort, Binary Indexed Tree, Segment Tree
# link: <https://leetcode.com/problems/count-of-smaller-numbers-after-self/description/>
# hidden: true
# ---
#
# You are given an integer array _nums_ and you have to return a new _counts_
# array. The _counts_ array has the property where `counts[i]` is the number of
# smaller elements to the right of `nums[i]`.
#
#
#
# **Example 1:**
#
#
#
#     Input: nums = [5,2,6,1]
#     Output: [2,1,1,0]
#     Explanation:
#     To the right of 5 there are **2** smaller elements (2 and 1).
#     To the right of 2 there is only **1** smaller element (1).
#     To the right of 6 there is **1** smaller element (1).
#     To the right of 1 there is **0** smaller element.
#
#
#
#
# **Constraints:**
#
#   * `0 <= nums.length <= 10^5`
#   * `-10^4 <= nums[i] <= 10^4`
#
#
## @lc code=start
using LeetCode

## method 1: using merge sort
function count_smaller_method1(arr::Vector{Int})::Vector{Int}
    res = fill(0, length(arr))
    function merge_sort(arr::AbstractVector)::AbstractVector
        ## arr = [(idx1, num1), ...]
        (n = length(arr)) == 1 && return arr
        mid = n >> 1
        lpart = merge_sort(@view(arr[1:mid]))
        rpart = merge_sort(@view(arr[(mid + 1):end]))
        return merge_sorted(lpart, rpart)
    end
    function merge_sorted(
        lpart::AbstractVector, rpart::AbstractVector
    )::Vector{Tuple{Int,Int}}
        l1, l2 = length(lpart), length(rpart)
        combined = Vector{Tuple{Int,Int}}(undef, l1 + l2)
        p1 = p2 = 1
        for pos in eachindex(combined)
            if p2 > l2 || p1 <= l1 && lpart[p1][2] <= rpart[p2][2]
                combined[pos] = lpart[p1]
                res[lpart[p1][1]] += (p2 - 1)
                p1 += 1
            else
                combined[pos] = rpart[p2]
                p2 += 1
            end
        end
        return combined
    end

    merge_sort(collect(enumerate(arr)))
    return res
end

## method 2: using binary search
## require function "search_left_border" from solution 33
function count_smaller_method2(arr::Vector{Int})::Vector{Int}
    queue, res = Int[], Array{Int}(undef, length(reverse!(arr)))
    for (i, num) in enumerate(arr)
        loc = search_left_border(1, length(queue), x -> queue[x] >= num)
        res[i] = loc - 1
        insert!(queue, loc, num)
    end
    reverse!(arr)
    return reverse!(res)
end
## @lc code=end


using StatsBase
using ProgressMeter

###############################################################################
###############################################################################
### jackknife error analysis
### based on the paper 'delete-m jackknife for unequal m' by Busing et al.
global skip_jackknife = false

"""
    jackknife(f, data[, naive=missing])

returns the naive estimate f(data) (which is evaluated by the function if not
provided), the jackknife estimate, and the jackknife error.

The jackknife estimate and error are calculated over samples of data that omit
single elements. If blocking is intended, then data should be a vector of
blocks of data. In this case all blocks have to have the same length! (see
jackknife_mj otherwise). This function works for functions `f` with scalar or
vector output.
"""
function jackknife(f, data; naive=missing)
    if ismissing(naive)
        naive = f(data)
    end
    if skip_jackknife
        return naive, fill(NaN, size(naive)), fill(NaN, size(naive))
    end
    g = length(data)
    est = Vector{typeof(naive)}(undef, g)
    p = Progress(g, 2, "Jackknife: ", offset=0)
    for j in 1:g
        est[j] = f(data[1:end .!= j])
        next!(p)
    end
    mean_est = mean(est)
    est_J = g*naive .- (g-1)*mean_est

    argument(est) = (est-mean_est).^2
    var_J = sum(argument, est).*((g-1)/g)
    return naive, est_J, sqrt.(var_J)
end

# delete mj jackknife
"""
    jackknife_mj(f, data[, naive=missing])

returns the naive estimate f(data) (which is evaluated by the function if not
provided), the jackknife estimate, and the jackknife error.

Same as `jackknife` but data can be vector of blocks of different length.
However, none of the blocks may have length zero, so these have to be filtered
out beforehand.
"""
function jackknife_mj(f, data; naive=missing)
    if ismissing(naive)
        naive = f(data)
    end
    if skip_jackknife
        return naive, fill(NaN, size(naive)), fill(NaN, size(naive))
    end
    g = length(data)
    m = length.(data)
    @assert sum(m.==0)==0
    h = sum(m)./m
    est_tilde = Vector{typeof(naive)}(undef, g)
    p = Progress(g, 2, "Jackknife: ", offset=0)
    for j in 1:g
        est_tilde[j] = h[j]*naive .- (h[j]-1)*f(data[1:end .!= j])
        next!(p)
    end
    est_J = sum(est_tilde./h)

    # define summation argument in place to work also for vector quantities
    argument((est_tilde_j, h_j)) = (est_tilde_j .- est_J).^2 / (h_j-1)
    var_J = sum( x->argument(x), zip(est_tilde, h) ) /g

    return naive, est_J, sqrt.(var_J)
end

###############################################################################
# tests

function test_jackknife()
    passed = true
    data=[[1,2,3], [1,1,1], [2,2,2], [1,2,4]]

    # scalar
    result_jackknife = jackknife(x->mean(sum.(x)), data)
    result_jackknife_mj = jackknife_mj(x->mean(sum.(x)), data)
    println(result_jackknife, " ", result_jackknife_mj)
    passed &= sum(result_jackknife .- result_jackknife_mj) < 1e-5
    println(passed)

    # vector
    result_jackknife = jackknife(x->mean(diff.(x)), data)
    result_jackknife_mj = jackknife_mj(x->mean(diff.(x)), data)
    println(result_jackknife, " ", result_jackknife_mj)
    passed &= sum(sum.(result_jackknife .- result_jackknife_mj)) < 1e-5

    # reshaping arrays
    num_blocks = 30
    max_len = 40
    rng = MersenneTwister(1000)
    random_data = [randn(rng,Float64, rand(rng,2:max_len)) for i in 1:num_blocks]
    flat = vcat(random_data...)
    block_len = Int(floor(length(flat)/num_blocks))+1
    equal_blocks = [ flat[i*block_len+1:min((i+1)*block_len,length(flat))] for i in 0:Int(floor(length(flat)/block_len)) ]
    result_jackknife = jackknife(x->sum(sum.(x))/sum(length.(x)),equal_blocks)
    result_jackknife_mj = jackknife_mj(x->sum(sum.(x))/sum(length.(x)),random_data)
    println(result_jackknife, " ", result_jackknife_mj)
    passed &= sum(sum.(result_jackknife .- result_jackknife_mj)) < 1e-3

    return passed
end


#=

Utility functions used throughout the library.

=#

function pmute(a::AbstractVector)
    # Return all unique permutations of the vector a, which must be a 1d
    # numerical array.

    sort!(a)  # Sort so we can deal with repeated elements

    # just initializing here so these are available in all `while` scopes
    i = 0
    first = 1
    alen = length(a)

    while true
        i = alen
        while true
            i -= 1

            if a[i] < a[i + 1]
                j = alen

                while a[i] >= a[j]
                    j -= 1  # j--
                end

                a[i], a[j] = a[j], a[i]  # swap(a[j], a[i])
                t = a[(i + 1):end]
                reverse!(t)
                a[(i + 1):end] = t

                # Output current.
                produce(copy(a))

                break  # next.
            end

            if i == first
                reverse!(a)
                produce(copy(a))
                return
            end
        end
    end
end

function cartprod(arrs, out=Array(eltype(arrs[1]),
                                  prod([length(a) for a in arrs]),
                                  length(arrs)))
    sz = Int[length(a) for a in arrs]
    k = 1
    for v in product(arrs...)
        i = 1
        for el in v
            out[k, i] = el
            i += 1
        end
        k += 1
    end

    return out
end

# Note the routines below are taken from Combinatorics.jl
# Because that package does not support Julia 0.4 I have
# included them here.  When 0.5 is released these will be
# removed in favor of the package.
immutable WithReplacementCombinations{T}
    a::T
    t::Int
end

Base.eltype{T}(::Type{WithReplacementCombinations{T}}) = Vector{eltype(T)}

Base.length(c::WithReplacementCombinations) = binomial(length(c.a)+c.t-1, c.t)

"generate all combinations with replacement of size t from an array a."
with_replacement_combinations(a, t::Integer) = WithReplacementCombinations(a, t)

Base.start(c::WithReplacementCombinations) = [1 for i in 1:c.t]
function Base.next(c::WithReplacementCombinations, s)
    n = length(c.a)
    t = c.t
    comb = [c.a[si] for si in s]
    if t > 0
        s = copy(s)
        changed = false
        for i in t:-1:1
            if s[i] < n
                s[i] += 1
                for j in (i+1):t
                    s[j] = s[i]
                end
                changed = true
                break
            end
        end
        !changed && (s[1] = n+1)
    else
        s = [n+1]
    end
    (comb, s)
end
Base.done(c::WithReplacementCombinations, s) =
    !isempty(s) && s[1] > length(c.a) || c.t < 0


# Before running this, please make sure to activate and instantiate the environment
# corresponding to [this `Project.toml`](https://raw.githubusercontent.com/alan-turing-institute/MLJTutorials/master/Project.toml) and [this `Manifest.toml`](https://raw.githubusercontent.com/alan-turing-institute/MLJTutorials/master/Manifest.toml)
# so that you get an environment which matches the one used to generate the tutorials:
#
# ```julia
# cd("MLJTutorials") # cd to folder with the *.toml
# using Pkg; Pkg.activate("."); Pkg.instantiate()
# ```

using MLJ, DataFrames, Random

@load RidgeRegressor pkg=MultivariateStats

Random.seed!(5) # for reproducibility
x1 = rand(300)
x2 = rand(300)
x3 = rand(300)
y = exp.(x1 - x2 -2x3 + 0.1*rand(300))
X = DataFrame(x1=x1, x2=x2, x3=x3)
first(X, 3) |> pretty

test, train = partition(eachindex(y), 0.8);

Xs = source(X)
ys = source(y, kind=:target)

stand = machine(Standardizer(), Xs)
W = transform(stand, Xs)

fit!(W, rows=train);

W()             # transforms all data
W(rows=test, )  # transforms only test data
W(X[3:4, :])    # transforms specific data

box_model = UnivariateBoxCoxTransformer()
box = machine(box_model, ys)
z = transform(box, ys)

ridge_model = RidgeRegressor(lambda=0.1)
ridge = machine(ridge_model, W, z)
ẑ = predict(ridge, W)

ŷ = inverse_transform(box, ẑ)

fit!(ŷ, rows=train);

rms(y[test], ŷ(rows=test))

ridge_model.lambda = 5.0;

fit!(ŷ, rows=train)
rms(y[test], ŷ(rows=test))

W = X |> Standardizer()
z = y |> UnivariateBoxCoxTransformer()

ẑ = (W, z) |> RidgeRegressor(lambda=0.1);

ŷ = ẑ |> inverse_transform(z);

fit!(ŷ, rows=train)
rms(y[test], ŷ(rows=test))

ẑ[:lambda] = 5.0;

ẑ.machine.model.lambda = 5.0;

fit!(ŷ, rows=train)
rms(y[test], ŷ(rows=test))

# This file was generated using Literate.jl, https://github.com/fredrikekre/Literate.jl


using SparseArrays
using LinearAlgebra

#
import Base.exp;
import Base.sin;
import Base.cos;
function exp(A::Matrix{Complex{BigFloat}})
    Z=zero(A);
    n=size(A,1);
    Ak=Matrix{eltype(A)}(I,n,n)
    for k=0:100
        Z+= Ak/(factorial(big(k)));
        Ak[:] =Ak*A;
    end

    return Z
end
function sin(A::Matrix{BigFloat})
    return (exp(1im*A)-exp(-1im*A))/2im
end

function cos(A::Matrix{BigFloat})
    return (exp(1im*A)+exp(-1im*A))/2
end

struct HelmholtzProblem
    sigma
    Aders
    Uv
    V
    Afun
    b
    #
    dx
    xv
    s
end

function compute_Mlincomb(prob::HelmholtzProblem,sigma,Y)
    if (prob.sigma != sigma)
        @show sigma
        @show prob.sigma
        error("Wrong sigma");
    end
    z=zeros(eltype(Y),size(Y,1));
    for i=1:size(Y,2)
        z += prob.Aders[i]*Y[:,i];
    end
    return z;
end
function compute_Mder(prob::HelmholtzProblem,mu)
    return prob.Afun(mu);
end

"""

    prob=create_helmholtz(n,a,b,c,N,k_eval,beta_eval,h_eval)

Creates an object representin the discretization of

```u''(x)+(1+mu*k(x))^2*u+beta(x)*u=h(x)```

by discretizing the interval [a,b] with n discretization
points. In the domain [b,c] we have

* `beta(x)=0`
* `k(x)=k0=k(c)`.

"""
function create_helmholtz(n,a,b,c,N,k_eval,beta_eval,h_eval)

    k0=k_eval(c);

    s=2;

    # Discretize interior interval [a,b]
    xv=Vector(range(a,b,length=n+1))[2:end];
    dx=xv[2]-xv[1];

    # Second derivative
    DD=spdiagm(0 => -2*ones(n-1),
               1 => ones(n-1),
               -1=> ones(n-2))/(dx^2)

    # Create auxiliary diagonal matrices
    K_eval=spdiagm(0=>[k_eval.(xv[1:end-1]);0]);
    L=spdiagm(0=>[beta_eval.(xv[1:end-1]);0]);
    II= spdiagm(0=>[ones(n-1);0]);
    K=mu -> spdiagm(0=>[(1 .+ mu*k_eval.(xv[1:end-1])).^2;0]);

    # Boundary condition matrices
    Xn=spzeros(n,2); Xn[n,1]=1; Xn[n,2]=1;
    Yn=spzeros(n,2);
    Yn[n,1]=1;
    Yn[n,2]=3/(2*dx);
    Yn[n-1,2]=-2/dx;
    Yn[n-2,2]=1/(2*dx);

    g=mu->cos((c-b)*(I+mu*k0))
    f=mu->(I+mu*k0)\sin((c-b)*(I+mu*k0));
    F=mu->spdiagm(0=>[g(mu);f(mu)]);

    # Create the A-function

    Afun1=mu->DD+K(mu)+L;
    Afun2=mu->Xn*F(mu)*Yn';
    Afun=mu->Afun1(mu)+Afun2(mu);

    # Compute derivatives of A
    ders=Vector{typeof(DD)}(undef,N);

    ders[1]=DD+II+L;
    ders[2]=2*K_eval;
    ders[3]=2*K_eval*K_eval;

    # Compute derivatives of boundary conditions
    J=big.(diagm(0=>zeros(N), 1=>ones(N-1)));
    fders=Float64.(f(J)[1,:].*factorial.(big.(0:N-1)))
    gders=Float64.(g(J)[1,:].*factorial.(big.(0:N-1)))
    for i=1:N
        Q=Xn*spdiagm(0=>[gders[i];fders[i]])*Yn';
        if (isassigned(ders,i))
            ders[i] += Q;
        else
            ders[i]=Q;
        end
    end

    rhs=[h_eval.(xv[1:end-1]);0] # right-hand side

    #
    Uv_type=Matrix{eltype(DD)}
    Uv=Vector{Uv_type}(undef,N);

    for i=(s+2):N
        Uv[i]=Xn*spdiagm(0=>[gders[i];fders[i]]);
    end
    V=Yn;

    myproblem=HelmholtzProblem(0,ders,Uv,V,Afun,rhs,dx,xv,s);

end


using SV, Statistics, Calculus, LinearAlgebra, DelimitedFiles

function main()
# setup
θtrue = [exp(-0.736/2.0), 0.9, 0.363] # true param values, on param space
lb = [0.0, 0.0, 0.0]
ub = [3.0, 0.99, 3.0]
n = 1000
burnin = 100
# load the example data
y = readdlm("svdata.txt")
m0 =  sqrt(n)*aux_stat(y) # statistic
# or use a new draw
#m0 = SVmodel(θtrue, n, randn(n+100), randn(n+100), true)
# Estimation by indirect inference
S = 100 # number of simulation reps
shocks_u = randn(n+burnin,S)
shocks_e = randn(n+burnin,S)
ms = θ -> SVmoments(m0, n, θ, shocks_u, shocks_e)
# the estimation of the covariance is unstable when volatility is
# high, so first, identity weight, then efficient, with tighter bounds
weight = θ -> 1.0
m = θ -> vec(mean(ms(θ),dims=1)) # 1Xg
obj = θ -> m(θ)'*weight(θ)*m(θ)
θinit = (lb + ub)./2.0
θhat, objvalue, converged, details = samin(obj, θinit, lb, ub; ns = 5, verbosity = 3, rt = 0.5)
lb2 = 0.9.*θhat
ub2 = 1.1.*θhat
lb = max.(lb,lb2)
ub = min.(ub, ub2)
weight = θ -> inv(cov(ms(θ)))
θhat, objvalue, converged, details = samin(obj, θhat, lb, ub; ns = 5, verbosity = 3, rt = 0.5)
# compute the estimated standard errors and CIs
D = (Calculus.jacobian(m, vec(θhat), :central))
W = weight(θhat)
V = inv(D'*W*D)
se = sqrt.(diag(V))
println("true values, estimates, st. error, and limits of 95% CI")
prettyprint([θtrue θhat se θhat-1.96*se θhat+1.96*se],["true value", "estimate", "std. err.", "CI lower", "CI upper"])
end
main()


using BasisFunctions, DomainSets, Test
@testset "Spaces " begin
s = GenericFunctionSpace{Float64,ComplexF64}()
@test typeof(zero(s)(rand()))==ComplexF64
@test typeof(one(s)(rand()))==ComplexF64
@test zero(s)(rand())==0
@test one(s)(rand())==1

s = MeasureSpace(FourierWeight())
@test measure(s) ==FourierWeight()
@test domain(s) == UnitInterval()
@test space(FourierWeight()) == s
@test FourierSpace() == s
@test ChebyshevTSpace() == MeasureSpace(ChebyshevTWeight{Float64}())
@test Inf ∈ domain(L2())

s1 = GenericFunctionSpace{Float64,ComplexF64}()
s2 = MeasureSpace(FourierWeight())
@test components(tensorproduct(s1,s2)) == (s1,s2)
end


using Powseeker
using Plots

plot(PowseekerMDP())
plot!(SkierState(1, [0.0, 0.0], deg2rad(15)))
gui()


using Test
using LogicCircuits
using ProbabilisticCircuits
using DataFrames: DataFrame
using Suppressor

include("../helper/gpu.jl")

@testset "Likelihood" begin
    # Uses a PC with 4 variables, and tests 3 of the configurations to
    # match with python. Also tests all probabilities sum up to 1.

    prob_circuit = zoo_psdd("little_4var.psdd");
    @test prob_circuit isa ProbCircuit;

    # Step 1. Check Probabilities for 3 samples
    data = DataFrame(BitArray([0 0 0 0; 0 1 1 0; 0 0 1 1]));
    true_prob = [0.07; 0.03; 0.13999999999999999]

    calc_prob = EVI(prob_circuit, data)
    calc_prob = exp.(calc_prob)

    @test true_prob ≈ calc_prob atol=1e-7;

    # Step 2. Add up all probabilities and see if they add up to one
    N = 4;
    data_all = generate_data_all(N)

    calc_prob_all = EVI(prob_circuit, data_all)
    calc_prob_all = exp.(calc_prob_all)
    sum_prob_all = sum(calc_prob_all)

    @test 1 ≈ sum_prob_all atol = 1e-7;

    cpu_gpu_agree_approx(data_all) do d
        EVI(prob_circuit, d)
    end

    # make sure log-likelihoods are -Inf when the input is not satisfied
    alltrue = multiply(pos_literals(ProbCircuit,4))
    @test all(EVI(alltrue, data) .== -Inf)

    cpu_gpu_agree(data) do d
        EVI(alltrue, d)
    end

    # Test Sturdel EVI
    samples, _ = sample(prob_circuit, 100000)
    mix, weights, _ = learn_strudel(DataFrame(convert(BitArray, samples)); num_mix = 10,
                                    init_maxiter = 20, em_maxiter = 100, verbose = false)
    mix_calc_prob = exp.(EVI(mix, data, weights))

    @test true_prob ≈ mix_calc_prob atol = 0.1
    mix_calc_prob_all = exp.(EVI(mix, data_all))
    @test 1 ≈ sum(mix_calc_prob_all) atol = 0.1

    cpu_gpu_agree_approx(data_all) do d
        EVI(mix, d, weights)
    end
end

@testset "Bagging models' likelihood" begin
    dfb = DataFrame(BitMatrix([true true; true true; true true; true true]))
    r = fully_factorized_circuit(ProbCircuit,num_features(dfb))
    # bag_dfb = bagging_dataset(dfb; num_bags = 2, frac_examples = 1.0)
    bag_dfb = Vector{DataFrame}(undef, 2)
    bag_dfb[1] = dfb[[2, 1, 3, 4], :]
    bag_dfb[2] = dfb[[4, 3, 2, 1], :]

    r = compile(SharedProbCircuit, r, 2)

    estimate_parameters(r, bag_dfb; pseudocount = 1.0)

    ll = log_likelihood_per_instance(r, dfb)
    @test ll[1] ≈ -0.2107210 atol = 1e-6
    @test ll[2] ≈ -0.2107210 atol = 1e-6
    @test ll[3] ≈ -0.2107210 atol = 1e-6
    @test ll[4] ≈ -0.2107210 atol = 1e-6

    @test log_likelihood(r, dfb) ≈ -0.8428841 atol = 1e-6

    @test log_likelihood_avg(r, dfb) ≈ -0.2107210 atol = 1e-6
end